From 9931b46516d9c0ec68cda5b2f8908b4884301982 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 31 Jul 2024 00:07:02 +0400 Subject: [PATCH 1/3] AP_ExternalAHRS: actualize ILabs EAHRS data processing --- .../AP_ExternalAHRS_InertialLabs.cpp | 373 +++++++++++++----- .../AP_ExternalAHRS_InertialLabs.h | 64 ++- 2 files changed, 340 insertions(+), 97 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index 129c11616a151..845196fdf7297 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -62,11 +62,11 @@ extern const AP_HAL::HAL &hal; #define ILABS_UNIT_STATUS2_BARO_FAIL 0x0008 #define ILABS_UNIT_STATUS2_DIFF_PRESS_FAIL 0x0010 #define ILABS_UNIT_STATUS2_MAGCAL_2D_ACT 0x0020 -#define ILABS_UNIT_STATUS2_MAGCAL_3D_ACT 0x0020 -#define ILABS_UNIT_STATUS2_GNSS_FUSION_OFF 0x0040 -#define ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF 0x0080 -#define ILABS_UNIT_STATUS2_MAG_FUSION_OFF 0x0100 -#define ILABS_UNIT_STATUS2_GNSS_POS_VALID 0x0200 +#define ILABS_UNIT_STATUS2_MAGCAL_3D_ACT 0x0040 +#define ILABS_UNIT_STATUS2_GNSS_FUSION_OFF 0x0080 +#define ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF 0x0100 +#define ILABS_UNIT_STATUS2_MAG_FUSION_OFF 0x0200 +#define ILABS_UNIT_STATUS2_GNSS_POS_VALID 0x0400 // air data status bits #define ILABS_AIRDATA_INIT_FAIL 0x0001 @@ -75,9 +75,9 @@ extern const AP_HAL::HAL &hal; #define ILABS_AIRDATA_DIFF_PRESS_FAIL 0x0008 #define ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR 0x0010 #define ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR 0x0020 -#define ILABS_AIRDATA_PRESS_ALT_FAIL 0x0040 -#define ILABS_AIRDATA_AIRSPEED_FAIL 0x0080 -#define ILABS_AIRDATA_BELOW_THRESHOLD 0x0100 +#define ILABS_AIRDATA_PRESS_ALT_FAIL 0x0100 +#define ILABS_AIRDATA_AIRSPEED_FAIL 0x0200 +#define ILABS_AIRDATA_BELOW_THRESHOLD 0x0400 // constructor @@ -257,7 +257,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() case MessageType::GPS_INS_TIME_MS: { // this is the GPS tow timestamp in ms for when the IMU data was sampled CHECK_SIZE(u.gps_time_ms); - state2.gnss_ins_time_ms = u.gps_time_ms; + gps_data.ms_tow = u.gps_time_ms; break; } case MessageType::GPS_WEEK: { @@ -267,29 +267,31 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::ACCEL_DATA_HR: { CHECK_SIZE(u.accel_data_hr); - ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*GRAVITY_MSS*1.0e-6; + // should use 9.8106 instead of GRAVITY_MSS-constant in accordance with the device-documentation + ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*9.8106f*1.0e-6; // m/s^2 break; } case MessageType::GYRO_DATA_HR: { CHECK_SIZE(u.gyro_data_hr); - ins_data.gyro = u.gyro_data_hr.tofloat().rfu_to_frd()*DEG_TO_RAD*1.0e-5; + ins_data.gyro = u.gyro_data_hr.tofloat().rfu_to_frd()*DEG_TO_RAD*1.0e-5; // rad/s break; } case MessageType::BARO_DATA: { CHECK_SIZE(u.baro_data); - baro_data.pressure_pa = u.baro_data.pressure_pa2*2; + baro_data.pressure_pa = u.baro_data.pressure_pa2*2; // Pa + state2.baro_alt = u.baro_data.baro_alt*0.01; // m break; } case MessageType::MAG_DATA: { CHECK_SIZE(u.mag_data); - mag_data.field = u.mag_data.tofloat().rfu_to_frd()*(10*NTESLA_TO_MGAUSS); + mag_data.field = u.mag_data.tofloat().rfu_to_frd()*(10*NTESLA_TO_MGAUSS); // milligauss break; } case MessageType::ORIENTATION_ANGLES: { CHECK_SIZE(u.orientation_angles); state.quat.from_euler(radians(u.orientation_angles.roll*0.01), - radians(u.orientation_angles.pitch*0.01), - radians(u.orientation_angles.yaw*0.01)); + radians(u.orientation_angles.pitch*0.01), + radians(u.orientation_angles.yaw*0.01)); state.have_quaternion = true; if (last_att_ms == 0) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got link"); @@ -300,15 +302,23 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() case MessageType::VELOCITIES: { CHECK_SIZE(u.velocity); state.velocity = u.velocity.tofloat().rfu_to_frd()*0.01; + gps_data.ned_vel_north = state.velocity.x; // m/s + gps_data.ned_vel_east = state.velocity.y; // m/s + gps_data.ned_vel_down = state.velocity.z; // m/s state.have_velocity = true; last_vel_ms = now_ms; break; } case MessageType::POSITION: { CHECK_SIZE(u.position); - state.location.lat = u.position.lat; - state.location.lng = u.position.lon; - state.location.alt = u.position.alt; + state.location.lat = u.position.lat; // deg*1.0e7 + state.location.lng = u.position.lon; // deg*1.0e7 + state.location.alt = u.position.alt; // m*100 + + gps_data.latitude = u.position.lat; // deg*1.0e7 + gps_data.longitude = u.position.lon; // deg*1.0e7 + gps_data.msl_altitude = u.position.alt; // m*100 + state.have_location = true; state.last_location_update_us = AP_HAL::micros(); last_pos_ms = now_ms; @@ -316,12 +326,12 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::KF_VEL_COVARIANCE: { CHECK_SIZE(u.kf_vel_covariance); - state2.kf_vel_covariance = u.kf_vel_covariance.tofloat() * 0.001; + state2.kf_vel_covariance = u.kf_vel_covariance.tofloat().rfu_to_frd(); // mm/s break; } case MessageType::KF_POS_COVARIANCE: { CHECK_SIZE(u.kf_pos_covariance); - state2.kf_pos_covariance = u.kf_pos_covariance.tofloat() * 0.001; + state2.kf_pos_covariance = u.kf_pos_covariance.tofloat(); // mm break; } case MessageType::UNIT_STATUS: { @@ -332,7 +342,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() case MessageType::GNSS_EXTENDED_INFO: { CHECK_SIZE(u.gnss_extended_info); gps_data.fix_type = u.gnss_extended_info.fix_type+1; - state2.gnss_extended_info = u.gnss_extended_info; + gnss_data.spoof_status = u.gnss_extended_info.spoofing_status; break; } case MessageType::NUM_SATS: { @@ -341,54 +351,52 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() break; } case MessageType::GNSS_POSITION: { - CHECK_SIZE(u.position); - gps_data.latitude = u.position.lat; - gps_data.longitude = u.position.lon; - gps_data.msl_altitude = u.position.alt; + CHECK_SIZE(u.gnss_position); + gnss_data.lat = u.gnss_position.lat; // deg*1.0e7 + gnss_data.lng = u.gnss_position.lon; // deg*1.0e7 + gnss_data.alt = u.gnss_position.alt; // mm break; } case MessageType::GNSS_VEL_TRACK: { CHECK_SIZE(u.gnss_vel_track); - Vector2f velxy; - velxy.offset_bearing(u.gnss_vel_track.track_over_ground*0.01, u.gnss_vel_track.hor_speed*0.01); - gps_data.ned_vel_north = velxy.x; - gps_data.ned_vel_east = velxy.y; - gps_data.ned_vel_down = -u.gnss_vel_track.ver_speed*0.01; + gnss_data.hor_speed = u.gnss_vel_track.hor_speed*0.01; // m/s + gnss_data.ver_speed = u.gnss_vel_track.ver_speed*0.01; // m/s + gnss_data.track_over_ground = u.gnss_vel_track.track_over_ground*0.01; // deg break; } case MessageType::GNSS_POS_TIMESTAMP: { CHECK_SIZE(u.gnss_pos_timestamp); - gps_data.ms_tow = u.gnss_pos_timestamp; + gnss_data.pos_timestamp = u.gnss_pos_timestamp; break; } case MessageType::GNSS_INFO_SHORT: { CHECK_SIZE(u.gnss_info_short); - state2.gnss_info_short = u.gnss_info_short; + gnss_data.info_short = u.gnss_info_short; break; } case MessageType::GNSS_NEW_DATA: { CHECK_SIZE(u.gnss_new_data); - state2.gnss_new_data = u.gnss_new_data; + gnss_data.new_data = u.gnss_new_data; break; } case MessageType::GNSS_JAM_STATUS: { CHECK_SIZE(u.gnss_jam_status); - state2.gnss_jam_status = u.gnss_jam_status; + gnss_data.jam_status = u.gnss_jam_status; break; } case MessageType::DIFFERENTIAL_PRESSURE: { CHECK_SIZE(u.differential_pressure); - airspeed_data.differential_pressure = u.differential_pressure*1.0e-4; + airspeed_data.differential_pressure = u.differential_pressure*1.0e-4*100; // 100: mbar to Pa break; } case MessageType::TRUE_AIRSPEED: { CHECK_SIZE(u.true_airspeed); - state2.true_airspeed = u.true_airspeed*0.01; + state2.true_airspeed = u.true_airspeed*0.01; // m/s break; } case MessageType::WIND_SPEED: { CHECK_SIZE(u.wind_speed); - state2.wind_speed = u.wind_speed.tofloat().rfu_to_frd()*0.01; + state2.wind_speed = u.wind_speed.tofloat().rfu_to_frd()*0.01; // m/s break; } case MessageType::AIR_DATA_STATUS: { @@ -398,14 +406,15 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::SUPPLY_VOLTAGE: { CHECK_SIZE(u.supply_voltage); - state2.supply_voltage = u.supply_voltage*0.01; + state2.supply_voltage = u.supply_voltage*0.01; // V break; } case MessageType::TEMPERATURE: { CHECK_SIZE(u.temperature); // assume same temperature for baro and airspeed - baro_data.temperature = u.temperature*0.1; - airspeed_data.temperature = u.temperature*0.1; + baro_data.temperature = u.temperature*0.1; // degC + airspeed_data.temperature = u.temperature*0.1; // degC + ins_data.temperature = u.temperature*0.1; break; } case MessageType::UNIT_STATUS2: { @@ -413,6 +422,37 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() state2.unit_status2 = u.unit_status2; break; } + case MessageType::GNSS_ANGLES: { + CHECK_SIZE(u.gnss_angles); + gnss_data.heading = u.gnss_angles.heading*0.01; // deg + gnss_data.pitch = u.gnss_angles.pitch*0.01; // deg + break; + } + case MessageType::GNSS_ANGLE_POS_TYPE: { + CHECK_SIZE(u.gnss_angle_pos_type); + gnss_data.angle_pos_type = u.gnss_angle_pos_type; + break; + } + case MessageType::GNSS_HEADING_TIMESTAMP: { + CHECK_SIZE(u.gnss_heading_timestamp); + gnss_data.heading_timestamp = u.gnss_heading_timestamp; + break; + } + case MessageType::GNSS_DOP: { + CHECK_SIZE(u.gnss_dop); + gnss_data.gdop = u.gnss_dop.gdop*0.1; + gnss_data.pdop = u.gnss_dop.pdop*0.1; + gnss_data.tdop = u.gnss_dop.tdop*0.1; + + gps_data.hdop = u.gnss_dop.hdop*0.1; + gps_data.vdop = u.gnss_dop.vdop*0.1; + break; + } + case MessageType::INS_SOLUTION_STATUS: { + CHECK_SIZE(u.ins_sol_status); + state2.ins_sol_status = u.ins_sol_status; + break; + } } if (msg_len == 0) { @@ -440,13 +480,37 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() AP::ins().handle_external(ins_data); state.accel = ins_data.accel; state.gyro = ins_data.gyro; + +#if HAL_LOGGING_ENABLED + uint64_t now_us = AP_HAL::micros64(); + + // @LoggerMessage: ILB1 + // @Description: InertialLabs AHRS data1 + // @Field: TimeUS: Time since system startup + // @Field: GMS: GPS INS time (round) + // @Field: GyrX: Gyro X + // @Field: GyrY: Gyro Y + // @Field: GyrZ: Gyro z + // @Field: AccX: Accelerometer X + // @Field: AccY: Accelerometer Y + // @Field: AccZ: Accelerometer Z + + AP::logger().WriteStreaming("ILB1", "TimeUS,GMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ", + "s-kkkooo", + "F-------", + "QIffffff", + now_us, gps_data.ms_tow, + ins_data.gyro.x, ins_data.gyro.y, ins_data.gyro.z, + ins_data.accel.x, ins_data.accel.y, ins_data.accel.z); +#endif // HAL_LOGGING_ENABLED } + if (GOT_MSG(GPS_INS_TIME_MS) && GOT_MSG(NUM_SATS) && GOT_MSG(GNSS_POSITION) && GOT_MSG(GNSS_NEW_DATA) && GOT_MSG(GNSS_EXTENDED_INFO) && - state2.gnss_new_data != 0) { + gnss_data.new_data != 0) { uint8_t instance; if (AP::gps().get_first_external_instance(instance)) { AP::gps().handle_external(gps_data, instance); @@ -465,18 +529,137 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } last_gps_ms = now_ms; } + +#if HAL_LOGGING_ENABLED + uint64_t now_us = AP_HAL::micros64(); + + // @LoggerMessage: ILB4 + // @Description: InertialLabs AHRS data4 + // @Field: TimeUS: Time since system startup + // @Field: GMS: GNSS Position timestamp + // @Field: GWk: GPS Week + // @Field: NSat: Number of satellites + // @Field: NewGPS: Indicator of new update of GPS data + // @Field: Lat: GNSS Latitude + // @Field: Lng: GNSS Longitude + // @Field: Alt: GNSS Altitude + // @Field: GCrs: GNSS Track over ground + // @Field: Spd: GNSS Horizontal speed + // @Field: VZ: GNSS Vertical speed + + AP::logger().WriteStreaming("ILB4", "TimeUS,GMS,GWk,NSat,NewGPS,Lat,Lng,Alt,GCrs,Spd,VZ", + "s----DUmhnn", + "F----------", + "QIHBBffffff", + now_us, gnss_data.pos_timestamp, gps_data.gps_week, + gps_data.satellites_in_view, gnss_data.new_data, + gnss_data.lat*1.0e-7, gnss_data.lng*1.0e-7, gnss_data.alt*0.01, + gnss_data.track_over_ground, gnss_data.hor_speed, gnss_data.ver_speed + ); + + // @LoggerMessage: ILB5 + // @Description: InertialLabs AHRS data5 + // @Field: TimeUS: Time since system startup + // @Field: GMS: GNSS Position timestamp + // @Field: FType: fix type + // @Field: GSS: GNSS spoofing status + // @Field: GJS: GNSS jamming status + // @Field: GI1: GNSS Info1 + // @Field: GI2: GNSS Info2 + // @Field: GAPS: GNSS Angles position type + + AP::logger().WriteStreaming("ILB5", "TimeUS,GMS,FType,GSS,GJS,GI1,GI2,GAPS", + "s-------", + "F-------", + "QIBBBBBB", + now_us, gnss_data.pos_timestamp, gps_data.fix_type, + gnss_data.spoof_status, gnss_data.jam_status, + gnss_data.info_short.info1, gnss_data.info_short.info2, + gnss_data.angle_pos_type); + + // @LoggerMessage: ILB6 + // @Description: InertialLabs AHRS data6 + // @Field: TimeUS: Time since system startup + // @Field: GMS: GNSS Position timestamp + // @Field: GpsHTS: GNSS Heading timestamp + // @Field: GpsYaw: GNSS Heading + // @Field: GpsPitch: GNSS Pitch + // @Field: GDOP: GNSS GDOP + // @Field: PDOP: GNSS PDOP + // @Field: HDOP: GNSS HDOP + // @Field: VDOP: GNSS VDOP + // @Field: TDOP: GNSS TDOP + + AP::logger().WriteStreaming("ILB6", "TimeUS,GMS,GpsHTS,GpsYaw,GpsPitch,GDOP,PDOP,HDOP,VDOP,TDOP", + "s--hd-----", + "F---------", + "QIIfffffff", + now_us, gnss_data.pos_timestamp, gnss_data.heading_timestamp, + gnss_data.heading, gnss_data.pitch, gnss_data.gdop, gnss_data.pdop, + gps_data.hdop, gps_data.vdop, gnss_data.tdop); +#endif // HAL_LOGGING_ENABLED } + #if AP_BARO_EXTERNALAHRS_ENABLED if (GOT_MSG(BARO_DATA) && GOT_MSG(TEMPERATURE)) { AP::baro().handle_external(baro_data); - } -#endif - #if AP_COMPASS_EXTERNALAHRS_ENABLED + +#if HAL_LOGGING_ENABLED + uint64_t now_us = AP_HAL::micros64(); + + // @LoggerMessage: ILB3 + // @Description: InertialLabs AHRS data3 + // @Field: TimeUS: Time since system startup + // @Field: GMS: GPS INS time (round) + // @Field: Press: Static pressure + // @Field: Diff: Differential pressure + // @Field: Temp: Temperature + // @Field: Alt: Baro altitude + // @Field: TAS: true airspeed + // @Field: VWN: Wind velocity north + // @Field: VWE: Wind velocity east + // @Field: VWD: Wind velocity down + // @Field: ADU: Air Data Unit status + + AP::logger().WriteStreaming("ILB3", "TimeUS,GMS,Press,Diff,Temp,Alt,TAS,VWN,VWE,VWD,ADU", + "s-PPOmnnnn-", + "F----------", + "QIffffffffH", + now_us, gps_data.ms_tow, + baro_data.pressure_pa, airspeed_data.differential_pressure, baro_data.temperature, + state2.baro_alt, state2.true_airspeed, + state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z, + state2.air_data_status); +#endif // HAL_LOGGING_ENABLED + } +#endif // AP_BARO_EXTERNALAHRS_ENABLED + +#if AP_COMPASS_EXTERNALAHRS_ENABLED if (GOT_MSG(MAG_DATA)) { AP::compass().handle_external(mag_data); + +#if HAL_LOGGING_ENABLED + uint64_t now_us = AP_HAL::micros64(); + + // @LoggerMessage: ILB2 + // @Description: InertialLabs AHRS data2 + // @Field: TimeUS: Time since system startup + // @Field: GMS: GPS INS time (round) + // @Field: MagX: Magnetometer X + // @Field: MagY: Magnetometer Y + // @Field: MagZ: Magnetometer Z + + AP::logger().WriteStreaming("ILB2", "TimeUS,GMS,MagX,MagY,MagZ", + "s----", + "F----", + "QIfff", + now_us, gps_data.ms_tow, + mag_data.field.x, mag_data.field.y, mag_data.field.z); +#endif // HAL_LOGGING_ENABLED } -#endif +#endif // AP_COMPASS_EXTERNALAHRS_ENABLED + #if AP_AIRSPEED_EXTERNAL_ENABLED && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)) // only on plane and copter as others do not link AP_Airspeed if (GOT_MSG(DIFFERENTIAL_PRESSURE) && @@ -486,64 +669,74 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() arsp->handle_external(airspeed_data); } } + #endif // AP_AIRSPEED_EXTERNAL_ENABLED + buffer_ofs = 0; -#if HAL_LOGGING_ENABLED if (GOT_MSG(POSITION) && GOT_MSG(ORIENTATION_ANGLES) && GOT_MSG(VELOCITIES)) { - float roll, pitch, yaw; - state.quat.to_euler(roll, pitch, yaw); + float roll, pitch, yaw_deg; + state.quat.to_euler(roll, pitch, yaw_deg); + + yaw_deg = fmodf(degrees(yaw_deg), 360.0f); + if (yaw_deg < 0.0f) { + yaw_deg += 360.0f; + } + +#if HAL_LOGGING_ENABLED uint64_t now_us = AP_HAL::micros64(); - // @LoggerMessage: ILB1 - // @Description: InertialLabs AHRS data1 + // @LoggerMessage: ILB7 + // @Description: InertialLabs AHRS data7 // @Field: TimeUS: Time since system startup - // @Field: PosVarN: position variance north - // @Field: PosVarE: position variance east - // @Field: PosVarD: position variance down - // @Field: VelVarN: velocity variance north - // @Field: VelVarE: velocity variance east - // @Field: VelVarD: velocity variance down - - AP::logger().WriteStreaming("ILB1", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD", - "smmmnnn", - "F000000", - "Qffffff", - now_us, - state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z, - state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z); + // @Field: GMS: GPS INS time (round) + // @Field: Roll: euler roll + // @Field: Pitch: euler pitch + // @Field: Yaw: euler yaw + // @Field: VN: velocity north + // @Field: VE: velocity east + // @Field: VD: velocity down + // @Field: Lat: latitude + // @Field: Lng: longitude + // @Field: Alt: altitude MSL + // @Field: USW: USW1 + // @Field: USW2: USW2 + // @Field: Vdc: Supply voltage + + AP::logger().WriteStreaming("ILB7", "TimeUS,GMS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lng,Alt,USW,USW2,Vdc", + "s-dddnnnDUm--v", + "F-------------", + "QIfffffffffHHf", + now_us, gps_data.ms_tow, + degrees(roll), degrees(pitch), yaw_deg, + state.velocity.x, state.velocity.y, state.velocity.z, + state.location.lat*1.0e-7, state.location.lng*1.0e-7, state.location.alt*0.01, + state2.unit_status, state2.unit_status2, + state2.supply_voltage); - // @LoggerMessage: ILB2 - // @Description: InertialLabs AHRS data3 + // @LoggerMessage: ILB8 + // @Description: InertialLabs AHRS data8 // @Field: TimeUS: Time since system startup - // @Field: Stat1: unit status1 - // @Field: Stat2: unit status2 - // @Field: FType: fix type - // @Field: SpStat: spoofing status - // @Field: GI1: GNSS Info1 - // @Field: GI2: GNSS Info2 - // @Field: GJS: GNSS jamming status - // @Field: TAS: true airspeed - // @Field: WVN: Wind velocity north - // @Field: WVE: Wind velocity east - // @Field: WVD: Wind velocity down - - AP::logger().WriteStreaming("ILB2", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD", - "s-----------", - "F-----------", - "QHHBBBBBffff", - now_us, - state2.unit_status, state2.unit_status2, - state2.gnss_extended_info.fix_type, state2.gnss_extended_info.spoofing_status, - state2.gnss_info_short.info1, state2.gnss_info_short.info2, - state2.gnss_jam_status, - state2.true_airspeed, - state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z); - } + // @Field: GMS: GPS INS time (round) + // @Field: PVN: position variance north + // @Field: PVE: position variance east + // @Field: PVD: position variance down + // @Field: VVN: velocity variance north + // @Field: VVE: velocity variance east + // @Field: VVD: velocity variance down + + AP::logger().WriteStreaming("ILB8", "TimeUS,GMS,PVN,PVE,PVD,VVN,VVE,VVD", + "s-mmmnnn", + "F-------", + "QIffffff", + now_us, gps_data.ms_tow, + state2.kf_pos_covariance.x, state2.kf_pos_covariance.y, state2.kf_pos_covariance.z, + state2.kf_vel_covariance.x, state2.kf_vel_covariance.y, state2.kf_vel_covariance.z); #endif // HAL_LOGGING_ENABLED + } return true; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index 639d39e2d5323..f89bb95d754d8 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -77,6 +77,11 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { SUPPLY_VOLTAGE = 0x50, TEMPERATURE = 0x52, UNIT_STATUS2 = 0x5A, + GNSS_ANGLES = 0x33, + GNSS_ANGLE_POS_TYPE = 0x3A, + GNSS_HEADING_TIMESTAMP = 0x40, + GNSS_DOP = 0x42, + INS_SOLUTION_STATUS = 0x54, }; /* @@ -139,7 +144,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { } 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? @@ -150,10 +155,15 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { 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; // set ILABS_UNIT_STATUS_* gnss_extended_info_t gnss_extended_info; uint8_t num_sats; + struct PACKED { + int32_t lat; // deg*1e7 + int32_t lon; // deg*1e7 + int32_t alt; // m*100 + } gnss_position; struct PACKED { int32_t hor_speed; // m/s*100 uint16_t track_over_ground; // deg*100 @@ -170,6 +180,20 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { 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; }; AP_ExternalAHRS::gps_data_message_t gps_data; @@ -206,22 +230,48 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { } message_lengths[]; struct { + float baro_alt; Vector3f kf_vel_covariance; Vector3f kf_pos_covariance; - uint32_t gnss_ins_time_ms; uint16_t unit_status; uint16_t unit_status2; - gnss_extended_info_t gnss_extended_info; - gnss_info_short_t gnss_info_short; - uint8_t gnss_new_data; - uint8_t gnss_jam_status; float differential_pressure; float true_airspeed; Vector3f wind_speed; uint16_t air_data_status; float supply_voltage; + uint8_t ins_sol_status; } state2; + struct { + float lat; + float lng; + float alt; + float hor_speed; + float ver_speed; + float track_over_ground; + uint8_t new_data; + uint32_t pos_timestamp; + uint32_t heading_timestamp; + uint8_t spoof_status; + uint8_t jam_status; + uint8_t angle_pos_type; + gnss_info_short_t info_short; + float heading; + float pitch; + float gdop; + float pdop; + float tdop; + } gnss_data; + + uint16_t last_unit_status; + uint16_t last_unit_status2; + uint16_t last_air_data_status; + uint8_t last_spoof_status; + uint8_t last_jam_status; + + uint32_t last_critical_msg_ms; + uint32_t last_att_ms; uint32_t last_vel_ms; uint32_t last_pos_ms; From f8f70ace796b3c13e4c581d30da1d91afcdd1d33 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 31 Jul 2024 00:56:51 +0400 Subject: [PATCH 2/3] AP_ExternalAHRS: add GCS mesages sending for the ILabs EAHRS --- .../AP_ExternalAHRS_InertialLabs.cpp | 278 ++++++++++++++++++ 1 file changed, 278 insertions(+) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index 845196fdf7297..751449a8beec7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -738,6 +738,284 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() #endif // HAL_LOGGING_ENABLED } + const uint32_t dt_critical_usw = 10000; + uint32_t now_usw = AP_HAL::millis(); + + // InertialLabs critical messages to GCS (sending messages once every 10 seconds) + if ((last_unit_status != state2.unit_status) || + (last_unit_status2 != state2.unit_status2) || + (last_air_data_status != state2.air_data_status) || + (now_usw - last_critical_msg_ms > dt_critical_usw)) { + + // Critical USW message + if (state2.unit_status & ILABS_UNIT_STATUS_ALIGNMENT_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Unsuccessful initial alignment"); + } + if (state2.unit_status & ILABS_UNIT_STATUS_OPERATION_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: IMU data are incorrect"); + } + + if (state2.unit_status & ILABS_UNIT_STATUS_GYRO_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Gyros failure"); + } + + if (state2.unit_status & ILABS_UNIT_STATUS_ACCEL_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Accelerometers failure"); + } + + if (state2.unit_status & ILABS_UNIT_STATUS_MAG_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Magnetometers failure"); + } + + if (state2.unit_status & ILABS_UNIT_STATUS_ELECTRONICS_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Electronics failure"); + } + + if (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: GNSS receiver failure"); + } + + // Critical USW2 message + if (state2.unit_status2 & ILABS_UNIT_STATUS2_BARO_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Baro altimeter failure"); + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Diff. pressure sensor failure"); + } + + // Critical ADU message + if (state2.air_data_status & ILABS_AIRDATA_INIT_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Static pressure sensor unsuccessful initialization"); + } + + if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_INIT_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Diff. pressure sensor unsuccessful initialization"); + } + + if (state2.air_data_status & ILABS_AIRDATA_STATIC_PRESS_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ILAB: Static pressure sensor failure is detect"); + } + + if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ILAB: Diff. pressure sensor failure is detect"); + } + + last_critical_msg_ms = AP_HAL::millis(); + } + + if (last_unit_status != state2.unit_status) { + if (state2.unit_status & ILABS_UNIT_STATUS_RUNTIME_CAL) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: On-the-fly calibration is in progress"); + } + + if (state2.unit_status & ILABS_UNIT_STATUS_VOLTAGE_LOW) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Low input voltage"); + } else { + if (last_unit_status & ILABS_UNIT_STATUS_VOLTAGE_LOW) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Input voltage is in range"); + } + } + + if (state2.unit_status & ILABS_UNIT_STATUS_VOLTAGE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: High input voltage"); + } else { + if (last_unit_status & ILABS_UNIT_STATUS_VOLTAGE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Input voltage is in range"); + } + } + + if (state2.unit_status & ILABS_UNIT_STATUS_X_RATE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Y-axis angular rate is exceeded"); + } else { + if (last_unit_status & ILABS_UNIT_STATUS_X_RATE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Y-axis angular rate is in range"); + } + } + + if (state2.unit_status & ILABS_UNIT_STATUS_Y_RATE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: X-axis angular rate is exceeded"); + } else { + if (last_unit_status & ILABS_UNIT_STATUS_Y_RATE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: X-axis angular rate is in range"); + } + } + + if (state2.unit_status & ILABS_UNIT_STATUS_Z_RATE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Z-axis angular rate is exceeded"); + } else { + if (last_unit_status & ILABS_UNIT_STATUS_Z_RATE_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Z-axis angular rate is in range"); + } + } + + if (state2.unit_status & ILABS_UNIT_STATUS_MAG_FIELD_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Large total magnetic field"); + } else { + if (last_unit_status & ILABS_UNIT_STATUS_MAG_FIELD_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Total magnetic field is in range"); + } + } + + if (state2.unit_status & ILABS_UNIT_STATUS_TEMP_RANGE_ERR) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Temperature is out of range"); + } else { + if (last_unit_status & ILABS_UNIT_STATUS_TEMP_RANGE_ERR) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Temperature is in range"); + } + } + + if (state2.unit_status & ILABS_UNIT_STATUS_RUNTIME_CAL2) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: On-the-fly calibration successful"); + } + + last_unit_status = state2.unit_status; + } + + // InertialLabs INS Unit Status Word 2 (USW2) messages to GCS + if (last_unit_status2 != state2.unit_status2) { + if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_X_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Y-acceleration is out of range"); + } else { + if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_X_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Y-acceleration is in range"); + } + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Y_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: X-acceleration is out of range"); + } else { + if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Y_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: X-acceleration is in range"); + } + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Z_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Z-acceleration is out of range"); + } else { + if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Z_HIGH) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Z-acceleration is in range"); + } + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAGCAL_2D_ACT) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Automatic 2D calibration is in progress"); + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAGCAL_3D_ACT) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Automatic 3D calibration is in progress"); + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS input switched off"); + } else { + if (last_unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS input switched on"); + } + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure input switched off"); + } else { + if (last_unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure input switched on"); + } + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAG_FUSION_OFF) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Magnetometer input switched off"); + } else { + if (last_unit_status2 & ILABS_UNIT_STATUS2_MAG_FUSION_OFF) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Magnetometer input switched on"); + } + } + + if (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Incorrect GNSS position"); + } else { + if (last_unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS position is correct"); + } + } + + last_unit_status2 = state2.unit_status2; + } + + // InertialLabs INS Air Data Unit (ADU) status messages to GCS + if (last_air_data_status != state2.air_data_status) { + if (state2.air_data_status & ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Static pressure is out of range"); + } else { + if (last_air_data_status & ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Static pressure is in range"); + } + } + + if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Diff. pressure is out of range"); + } else { + if (last_air_data_status & ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure is in range"); + } + } + + if (state2.air_data_status & ILABS_AIRDATA_PRESS_ALT_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Pressure altitude is incorrect"); + } else { + if (last_air_data_status & ILABS_AIRDATA_PRESS_ALT_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Pressure altitude is correct"); + } + } + + if (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Air speed is incorrect"); + } else { + if (last_air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Air speed is correct"); + } + } + + if (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Air speed is below the threshold"); + } else { + if (last_air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Air speed is above the threshold"); + } + } + + last_air_data_status = state2.air_data_status; + } + + // InertialLabs INS spoofing detection messages to GCS + if (last_spoof_status != gnss_data.spoof_status) { + if ((last_spoof_status == 2 || last_spoof_status == 3) && (gnss_data.spoof_status == 1)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS no spoofing"); + } + + if (last_spoof_status == 2) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS spoofing indicated"); + } + + if (last_spoof_status == 3) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS multiple spoofing indicated"); + } + + last_spoof_status = gnss_data.spoof_status; + } + + // InertialLabs INS jamming detection messages to GCS + if (last_jam_status != gnss_data.jam_status) { + if ((last_jam_status == 2 || last_jam_status == 3) && (gnss_data.jam_status == 1)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS no jamming"); + } + + if (gnss_data.jam_status == 3) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS jamming indicated and no fix"); + } + + last_jam_status = gnss_data.jam_status; + } + return true; } From 837fe98addaff0f8281822e1667f5906aea4b6ac Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 7 Aug 2024 04:11:05 +0400 Subject: [PATCH 3/3] 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 aa191f0e9ef93..9f4fd26fe3ae7 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 5a49dea18712b..462793360cbf0 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;