From 55e3838eb2a79088a20cc4a776406bd3b6ffae21 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 7 Aug 2024 04:11:05 +0400 Subject: [PATCH] SITL: Actualize InertialLabs sim --- libraries/SITL/SIM_InertialLabs.cpp | 162 ++++++++++++++++++++-------- libraries/SITL/SIM_InertialLabs.h | 30 ++++-- 2 files changed, 140 insertions(+), 52 deletions(-) diff --git a/libraries/SITL/SIM_InertialLabs.cpp b/libraries/SITL/SIM_InertialLabs.cpp index aa191f0e9ef93a..9f4fd26fe3ae73 100644 --- a/libraries/SITL/SIM_InertialLabs.cpp +++ b/libraries/SITL/SIM_InertialLabs.cpp @@ -31,72 +31,146 @@ void InertialLabs::send_packet(void) pkt.msg_len = sizeof(pkt)-2; - pkt.accel_data_hr.x = (fdm.yAccel * 1.0e6)/GRAVITY_MSS; - pkt.accel_data_hr.y = (fdm.xAccel * 1.0e6)/GRAVITY_MSS; - pkt.accel_data_hr.z = (-fdm.zAccel * 1.0e6)/GRAVITY_MSS; + const auto gps_tow = GPS_Backend::gps_time(); + + // 0x01 GPS INS Time (round) + pkt.gps_ins_time_ms = gps_tow.ms; + + // 0x23 Accelerometer data HR + pkt.accel_data_hr.x = (fdm.yAccel / GRAVITY_MSS) * 1.0e6; // g*1.0e6 + pkt.accel_data_hr.y = (fdm.xAccel / GRAVITY_MSS) * 1.0e6; // g*1.0e6 + pkt.accel_data_hr.z = (-fdm.zAccel / GRAVITY_MSS) * 1.0e6; // g*1.0e6 - pkt.gyro_data_hr.y = fdm.rollRate*1.0e5; - pkt.gyro_data_hr.x = fdm.pitchRate*1.0e5; - pkt.gyro_data_hr.z = -fdm.yawRate*1.0e5; + // 0x21 Gyro data HR + pkt.gyro_data_hr.y = fdm.rollRate * 1.0e5; // deg/s*1.0e5 + pkt.gyro_data_hr.x = fdm.pitchRate * 1.0e5; // deg/s*1.0e5 + pkt.gyro_data_hr.z = -fdm.yawRate * 1.0e5; // deg/s*1.0e5 + // 0x25 Barometer data float p, t_K; AP_Baro::get_pressure_temperature_for_alt_amsl(fdm.altitude+rand_float()*0.25, p, t_K); - pkt.baro_data.pressure_pa2 = p; - pkt.baro_data.baro_alt = fdm.altitude; - pkt.temperature = KELVIN_TO_C(t_K); + pkt.baro_data.pressure_pa2 = p * 0.5; // Pa/2 + pkt.baro_data.baro_alt = fdm.altitude * 100; // m - pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS)*0.1; - pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS)*0.1; - pkt.mag_data.z = (-fdm.bodyMagField.z / NTESLA_TO_MGAUSS)*0.1; + // 0x24 Magnetometer data + pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS) * 0.1; // nT/10 + pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS) * 0.1; // nT/10 + pkt.mag_data.z = (-fdm.bodyMagField.z / NTESLA_TO_MGAUSS) * 0.1; // nT/10 - pkt.orientation_angles.roll = fdm.rollDeg*100; - pkt.orientation_angles.pitch = fdm.pitchDeg*100; - pkt.orientation_angles.yaw = fdm.yawDeg*100; + // 0x07 Orientation angles + float roll_rad, pitch_rad, yaw_rad, heading_deg; + fdm.quaternion.to_euler(roll_rad, pitch_rad, yaw_rad); + heading_deg = fmodf(degrees(yaw_rad), 360.0f); + if (heading_deg < 0.0f) { + heading_deg += 360.0f; + } + pkt.orientation_angles.roll = roll_rad * RAD_TO_DEG * 100; // deg*100 + pkt.orientation_angles.pitch = pitch_rad * RAD_TO_DEG * 100; // deg*100 + pkt.orientation_angles.yaw = heading_deg * 100; // deg*100 - pkt.velocity.x = fdm.speedE*100; - pkt.velocity.y = fdm.speedN*100; - pkt.velocity.z = -fdm.speedD*100; + // 0x12 Velocities + pkt.velocity.x = fdm.speedE * 100; // m/s*100 + pkt.velocity.y = fdm.speedN * 100; // m/s*100 + pkt.velocity.z = -fdm.speedD * 100; // m/s*100 - pkt.position.lat = fdm.latitude*1e7; - pkt.position.lon = fdm.longitude*1e7; - pkt.position.alt = fdm.altitude*1e2; + // 0x10 Position + pkt.position.lat = fdm.latitude * 1e7; // deg*1.0e7 + pkt.position.lon = fdm.longitude * 1e7; // deg*1.0e7 + pkt.position.alt = fdm.altitude * 1e2; // m*100 - pkt.kf_vel_covariance.x = 10; - pkt.kf_vel_covariance.z = 10; - pkt.kf_vel_covariance.z = 10; + // 0x58 KF velocity covariance + pkt.kf_vel_covariance.x = 10; // mm/s + pkt.kf_vel_covariance.y = 10; // mm/s + pkt.kf_vel_covariance.z = 10; // mm/s - pkt.kf_pos_covariance.x = 20; - pkt.kf_pos_covariance.z = 20; - pkt.kf_pos_covariance.z = 20; + // 0x57 KF position covariance HR + pkt.kf_pos_covariance.x = 20; // mm + pkt.kf_pos_covariance.y = 20; // mm + pkt.kf_pos_covariance.z = 20; // mm - const auto gps_tow = GPS_Backend::gps_time(); + // 0x53 Unit status word (USW) + pkt.unit_status = 0; // INS data is valid - pkt.gps_ins_time_ms = gps_tow.ms; + // 0x28 Differential pressure + pkt.differential_pressure = fdm.airspeed_raw_pressure[0] * 0.01 * 1.0e4; // mbar*1.0e4 (0.01 - Pa to mbar) - pkt.gnss_new_data = 0; + // 0x86 True airspeed (TAS) + pkt.true_airspeed = fdm.airspeed * 100; // m/s*100 + + // 0x8A Wind speed + pkt.wind_speed.x = fdm.wind_ef.y * 100; + pkt.wind_speed.y = fdm.wind_ef.x * 100; + pkt.wind_speed.z = 0; + + // 0x8D ADU status + pkt.air_data_status = 0; // ADU data is valid + + // 0x50 Supply voltage + pkt.supply_voltage = 12.3 * 100; // VDC*100 + + // 0x52 Temperature + pkt.temperature = KELVIN_TO_C(t_K)*10; // degC + + // 0x5A Unit status word (USW2) + pkt.unit_status2 = 0; // INS data is valid + + // 0x54 INS (Navigation) solution status + pkt.ins_sol_status = 0; // INS solution is good + pkt.gnss_new_data = 0; if (packets_sent % gps_frequency == 0) { - // update GPS data at 5Hz + // 0x3C GPS week pkt.gps_week = gps_tow.week; - pkt.gnss_pos_timestamp = gps_tow.ms; - pkt.gnss_new_data = 3; - pkt.gps_position.lat = pkt.position.lat; - pkt.gps_position.lon = pkt.position.lon; - pkt.gps_position.alt = pkt.position.alt; + // 0x4A GNSS extended info + pkt.gnss_extended_info.fix_type = 2; // 3D fix + pkt.gnss_extended_info.spoofing_status = 1; // no spoofing indicated + + // 0x3B Number of satellites used in solution pkt.num_sats = 32; - pkt.gnss_vel_track.hor_speed = norm(fdm.speedN,fdm.speedE)*100; + + // 0x30 GNSS Position + pkt.gps_position.lat = fdm.latitude * 1e7; // deg*1.0e7 + pkt.gps_position.lon = fdm.longitude * 1e7; // deg*1.0e7 + pkt.gps_position.alt = fdm.altitude * 1e2; // m*100 + + // 0x32 GNSS Velocity, Track over ground Vector2d track{fdm.speedN,fdm.speedE}; - pkt.gnss_vel_track.track_over_ground = wrap_360(degrees(track.angle()))*100; - pkt.gnss_vel_track.ver_speed = -fdm.speedD*100; + pkt.gnss_vel_track.hor_speed = norm(fdm.speedN,fdm.speedE) * 100; // m/s*100 + pkt.gnss_vel_track.track_over_ground = wrap_360(degrees(track.angle())) * 100; // deg*100 + pkt.gnss_vel_track.ver_speed = -fdm.speedD * 100; // m/s*100 - pkt.gnss_extended_info.fix_type = 2; - } + // 0x3E GNSS Position timestamp + pkt.gnss_pos_timestamp = gps_tow.ms; - pkt.differential_pressure = 0.5*sq(fdm.airspeed+fabsF(rand_float()*0.25))*1.0e4; - pkt.supply_voltage = 12.3*100; - pkt.temperature = 23.4*10; + // 0x36 GNSS info short + pkt.gnss_info_short.info1 = 0; // 0 – Single point position + pkt.gnss_info_short.info2 = 12; // bit 2 and 3 are set (Time is fine set and is being steered) + + // 0x41 New GPS + pkt.gnss_new_data = 3; // GNSS position and velocity data update + + // 0xС0 u-blox jamming status + pkt.gnss_jam_status = 1; // ok (no significant jamming) + + // 0x33 GNSS Heading, GNSS Pitch + pkt.gnss_angles.heading = 0; // only for dual-antenna GNSS receiver + pkt.gnss_angles.pitch = 0; // only for dual-antenna GNSS receiver + + // 0x3A GNSS Angles position type + pkt.gnss_angle_pos_type = 0; // only for dual-antenna GNSS receiver + + // 0x40 GNSS Heading timestamp + pkt.gnss_heading_timestamp = 0; // only for dual-antenna GNSS receiver + + // 0x42 Dilution of precision + pkt.gnss_dop.gdop = 1000; // *1.0e3 + pkt.gnss_dop.pdop = 1000; // *1.0e3 + pkt.gnss_dop.hdop = 1000; // *1.0e3 + pkt.gnss_dop.vdop = 1000; // *1.0e3 + pkt.gnss_dop.tdop = 1000; // *1.0e3 + } const uint8_t *buffer = (const uint8_t *)&pkt; pkt.crc = crc_sum_of_bytes_16(&buffer[2], sizeof(pkt)-4); diff --git a/libraries/SITL/SIM_InertialLabs.h b/libraries/SITL/SIM_InertialLabs.h index 5a49dea18712b5..462793360cbf0b 100644 --- a/libraries/SITL/SIM_InertialLabs.h +++ b/libraries/SITL/SIM_InertialLabs.h @@ -52,16 +52,16 @@ class InertialLabs : public SerialDevice struct PACKED ILabsPacket { uint16_t magic = 0x55AA; - uint8_t msg_type = 1; + uint8_t msg_type = 0x01; uint8_t msg_id = 0x95; uint16_t msg_len; // total packet length-2 - // send Table4, 27 messages - uint8_t num_messages = 27; - uint8_t messages[27] = { + // send Table4, 32 messages + uint8_t num_messages = 32; + uint8_t messages[32] = { 0x01, 0x3C, 0x23, 0x21, 0x25, 0x24, 0x07, 0x12, 0x10, 0x58, 0x57, 0x53, 0x4a, - 0x3b, 0x30, 0x32, 0x3e, 0x36, 0x41, 0xc0, 0x28, 0x86, 0x8a, 0x8d, 0x50, - 0x52, 0x5a + 0x3b, 0x30, 0x32, 0x3e, 0x36, 0x41, 0xc0, 0x28, 0x86, 0x8a, 0x8d, 0x50, 0x52, + 0x5a, 0x33, 0x3a, 0x40, 0x42, 0x54 }; uint32_t gps_ins_time_ms; // ms since start of GPS week for IMU data uint16_t gps_week; @@ -73,7 +73,7 @@ class InertialLabs : public SerialDevice } baro_data; vec3_16_t mag_data; // nT/10 struct PACKED { - int16_t yaw; // deg*100 + uint16_t yaw; // deg*100 int16_t pitch; // deg*100 int16_t roll; // deg*100 } orientation_angles; // 321 euler order @@ -84,7 +84,7 @@ class InertialLabs : public SerialDevice int32_t alt; // m*100, AMSL } position; vec3_u8_t kf_vel_covariance; // mm/s - vec3_u16_t kf_pos_covariance; + vec3_u16_t kf_pos_covariance; // mm uint16_t unit_status; gnss_extended_info_t gnss_extended_info; uint8_t num_sats; @@ -109,6 +109,20 @@ class InertialLabs : public SerialDevice uint16_t supply_voltage; // V*100 int16_t temperature; // degC*10 uint16_t unit_status2; + struct PACKED { + uint16_t heading; // deg*100 + int16_t pitch; // deg*100 + } gnss_angles; + uint8_t gnss_angle_pos_type; + uint32_t gnss_heading_timestamp; // ms + struct PACKED { + uint16_t gdop; + uint16_t pdop; + uint16_t hdop; + uint16_t vdop; + uint16_t tdop; + } gnss_dop; // 10e3 + uint8_t ins_sol_status; uint16_t crc; } pkt;