From bbbb0dbc324dd443672a6235f80635317c0cb5eb Mon Sep 17 00:00:00 2001 From: VladSheleverGyrotech Date: Tue, 6 Feb 2024 19:47:58 +0200 Subject: [PATCH 1/9] ExternalAHRS: fix InertialLabs External AHRS driver --- .../AP_ExternalAHRS_InertialLabs.cpp | 94 ++++++++++++++----- .../AP_ExternalAHRS_InertialLabs.h | 26 ++++- 2 files changed, 97 insertions(+), 23 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index d10ca6997c7cd..32009bbbc9dfd 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -96,6 +96,7 @@ AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *_fro // don't offer IMU by default, at 200Hz it is too slow for many aircraft set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | + uint16_t(AP_ExternalAHRS::AvailableSensor::IMU) | uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); @@ -267,7 +268,7 @@ 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; + ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*9.8106f*1.0e-6; break; } case MessageType::GYRO_DATA_HR: { @@ -349,10 +350,8 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } 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_north = cosF(radians(u.gnss_vel_track.track_over_ground*0.01))*u.gnss_vel_track.hor_speed*0.01; + gps_data.ned_vel_east = sinF(radians(u.gnss_vel_track.track_over_ground*0.01))*u.gnss_vel_track.hor_speed*0.01; gps_data.ned_vel_down = -u.gnss_vel_track.ver_speed*0.01; break; } @@ -378,7 +377,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } 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-2; // Pa break; } case MessageType::TRUE_AIRSPEED: { @@ -413,6 +412,31 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() state2.unit_status2 = u.unit_status2; break; } + case MessageType::GNSS_ANGLES: { + CHECK_SIZE(u.gnss_angles); + state2.gnss_heading = u.gnss_angles.gnss_heading * 1.0e-2; + state2.gnss_pitch = u.gnss_angles.gnss_pitch * 1.0e-2; + break; + } + case MessageType::GNSS_ANGLE_POS_TYPE: { + CHECK_SIZE(u.gnss_angle_pos_type); + state2.gnss_angle_pos_type = u.gnss_angle_pos_type; + break; + } + case MessageType::GNSS_HEADING_TIMESTAMP: { + CHECK_SIZE(u.gnss_heading_timestamp); + state2.gnss_heading_timestamp = u.gnss_heading_timestamp; + break; + } + case MessageType::GNSS_DOP: { + CHECK_SIZE(u.gnss_dop); + state2.gnss_gdop = u.gnss_dop.gnss_gdop * 1.0e-1; + state2.gnss_pdop = u.gnss_dop.gnss_pdop * 1.0e-1; + gps_data.hdop = u.gnss_dop.gnss_hdop * 1.0e-1; + gps_data.vdop = u.gnss_dop.gnss_vdop * 1.0e-1; + state2.gnss_tdop = u.gnss_dop.gnss_tdop * 1.0e-1; + break; + } } if (msg_len == 0) { @@ -472,7 +496,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() AP::baro().handle_external(baro_data); } #endif - #if AP_COMPASS_EXTERNALAHRS_ENABLED +#if AP_COMPASS_EXTERNALAHRS_ENABLED if (GOT_MSG(MAG_DATA)) { AP::compass().handle_external(mag_data); } @@ -489,7 +513,6 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() #endif // AP_AIRSPEED_EXTERNAL_ENABLED buffer_ofs = 0; -#if HAL_LOGGING_ENABLED if (GOT_MSG(POSITION) && GOT_MSG(ORIENTATION_ANGLES) && GOT_MSG(VELOCITIES)) { @@ -510,15 +533,18 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @Field: Lat: latitude // @Field: Lon: longitude // @Field: Alt: altitude AMSL + // @Field: TimeINS: GPS INS time (round) + // @Field: INSvolt: Supply voltage - AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt", - "sdddnnnDUm", - "F000000GG0", - "QffffffLLf", + AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,TimeINS,INSvolt", + "sdddnnnDUmsv", + "F000000GG000", + "QffffffLLfIf", now_us, degrees(roll), degrees(pitch), degrees(yaw), state.velocity.x, state.velocity.y, state.velocity.z, - state.location.lat, state.location.lng, state.location.alt*0.01); + state.location.lat, state.location.lng, state.location.alt*0.01, + state2.gnss_ins_time_ms, state2.supply_voltage); // @LoggerMessage: ILB2 // @Description: InertialLabs AHRS data2 @@ -535,8 +561,8 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() "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); + 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); // @LoggerMessage: ILB3 // @Description: InertialLabs AHRS data3 @@ -552,21 +578,45 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @Field: WVN: Wind velocity north // @Field: WVE: Wind velocity east // @Field: WVD: Wind velocity down + // @Field: ADU: Air Data Unit status - AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD", - "s-----------", - "F-----------", - "QHHBBBBBffff", + AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD,ADU", + "s------------", + "F------------", + "QHHBBBBBffffH", 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); - } -#endif // HAL_LOGGING_ENABLED + state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z, + state2.air_data_status); + // @LoggerMessage: ILB4 + // @Description: InertialLabs AHRS data4 + // @Field: TimeUS: Time since system startup + // @Field: GpsYaw: GNSS Heading + // @Field: GpsPitch: GNSS Pitch + // @Field: GpsHTS: GNSS Heading timestamp + // @Field: GpsAType: GNSS Angles position type + // @Field: GDOP: GNSS GDOP + // @Field: PDOP: GNSS PDOP + // @Field: HDOP: GNSS HDOP + // @Field: VDOP: GNSS VDOP + // @Field: TDOP: GNSS TDOP + + AP::logger().WriteStreaming("ILB4", "TimeUS,GpsYaw,GpsPitch,GpsHTS,GpsAType,GDOP,PDOP,HDOP,VDOP,TDOP", + "sdds------", + "F00I------", + "QffIBfffff", + now_us, + state2.gnss_heading, state2.gnss_pitch, + state2.gnss_heading_timestamp, state2.gnss_angle_pos_type, + state2.gnss_gdop * 1.0e-2, state2.gnss_pdop * 1.0e-2, gps_data.hdop * 1.0e-2, + gps_data.vdop * 1.0e-2, state2.gnss_tdop * 1.0e-2); + } + return true; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index bee7d5c28e0cc..3c99ba0e17c4f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -77,6 +77,10 @@ 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, }; /* @@ -139,7 +143,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? @@ -170,6 +174,19 @@ 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 gnss_heading; // deg*100 + int16_t gnss_pitch; // deg*100 + } gnss_angles; + uint8_t gnss_angle_pos_type; + uint32_t gnss_heading_timestamp; // ms + struct PACKED { + uint16_t gnss_gdop; + uint16_t gnss_pdop; + uint16_t gnss_hdop; + uint16_t gnss_vdop; + uint16_t gnss_tdop; + } gnss_dop; // *10e3 }; AP_ExternalAHRS::gps_data_message_t gps_data; @@ -214,6 +231,13 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { Vector3f wind_speed; uint16_t air_data_status; float supply_voltage; + float gnss_heading; + float gnss_pitch; + uint8_t gnss_angle_pos_type; + uint32_t gnss_heading_timestamp; + float gnss_gdop; + float gnss_pdop; + float gnss_tdop; } state2; uint32_t last_att_ms; From a209c503652b7985473791cc67256d181eb57987 Mon Sep 17 00:00:00 2001 From: VSheleverIL Date: Mon, 26 Feb 2024 15:37:11 +0200 Subject: [PATCH 2/9] ExternalAHRS: EKF_FailSafe trigger "fixed" --- .../AP_ExternalAHRS_InertialLabs.cpp | 43 +++++++++---------- .../AP_ExternalAHRS_InertialLabs.h | 5 +-- 2 files changed, 22 insertions(+), 26 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index 32009bbbc9dfd..ba1d8b6679ad8 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -99,7 +99,7 @@ AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *_fro uint16_t(AP_ExternalAHRS::AvailableSensor::IMU) | uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); - + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_InertialLabs::update_thread, void), "ILabs", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { AP_HAL::panic("InertialLabs Failed to start ExternalAHRS update thread"); } @@ -268,7 +268,7 @@ 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()*9.8106f*1.0e-6; + ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*9.8106f*1.0e-6; break; } case MessageType::GYRO_DATA_HR: { @@ -377,7 +377,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::DIFFERENTIAL_PRESSURE: { CHECK_SIZE(u.differential_pressure); - airspeed_data.differential_pressure = u.differential_pressure*1.0e-2; // Pa + airspeed_data.differential_pressure = u.differential_pressure*1.0e-4*1.0e2; // 1.0e2: mbar to Pa break; } case MessageType::TRUE_AIRSPEED: { @@ -430,11 +430,11 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::GNSS_DOP: { CHECK_SIZE(u.gnss_dop); - state2.gnss_gdop = u.gnss_dop.gnss_gdop * 1.0e-1; - state2.gnss_pdop = u.gnss_dop.gnss_pdop * 1.0e-1; - gps_data.hdop = u.gnss_dop.gnss_hdop * 1.0e-1; - gps_data.vdop = u.gnss_dop.gnss_vdop * 1.0e-1; - state2.gnss_tdop = u.gnss_dop.gnss_tdop * 1.0e-1; + state2.gnss_gdop = u.gnss_dop.gnss_gdop * 1.0e-1; // in cm + state2.gnss_pdop = u.gnss_dop.gnss_pdop * 1.0e-1; // in cm + gps_data.hdop = u.gnss_dop.gnss_hdop * 1.0e-1; // in cm + gps_data.vdop = u.gnss_dop.gnss_vdop * 1.0e-1; // in cm + state2.gnss_tdop = u.gnss_dop.gnss_tdop * 1.0e-1; // in cm break; } } @@ -490,17 +490,13 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() last_gps_ms = now_ms; } } -#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 (GOT_MSG(MAG_DATA)) { AP::compass().handle_external(mag_data); } -#endif #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) && @@ -598,7 +594,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @Field: TimeUS: Time since system startup // @Field: GpsYaw: GNSS Heading // @Field: GpsPitch: GNSS Pitch - // @Field: GpsHTS: GNSS Heading timestamp + // @Field: GpsHTS: GNSS Heading timestamp // @Field: GpsAType: GNSS Angles position type // @Field: GDOP: GNSS GDOP // @Field: PDOP: GNSS PDOP @@ -616,7 +612,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() state2.gnss_gdop * 1.0e-2, state2.gnss_pdop * 1.0e-2, gps_data.hdop * 1.0e-2, gps_data.vdop * 1.0e-2, state2.gnss_tdop * 1.0e-2); } - + return true; } @@ -753,16 +749,17 @@ void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const } // send message - const float vel_gate = 5; - const float pos_gate = 5; - const float hgt_gate = 5; - const float mag_var = 0; + // const float vel_gate = 5; + // const float pos_gate = 5; + // const float hgt_gate = 5; + // const float mag_var = 0; + // mavlink_msg_ekf_status_report_send(link.get_chan(), flags, + // state2.kf_vel_covariance.length()/vel_gate, + // state2.kf_pos_covariance.xy().length()/pos_gate, + // state2.kf_pos_covariance.z/hgt_gate, + // mag_var, 0, 0); mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - state2.kf_vel_covariance.length()/vel_gate, - state2.kf_pos_covariance.xy().length()/pos_gate, - state2.kf_pos_covariance.z/hgt_gate, - mag_var, 0, 0); + 0, 0, 0, 0, 0, 0); } #endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED - diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index 3c99ba0e17c4f..6325cc3a267f1 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -131,7 +131,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { uint8_t info1; uint8_t info2; }; - + union PACKED ILabsData { uint32_t gps_time_ms; // ms since start of GPS week uint16_t gps_week; @@ -186,7 +186,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { uint16_t gnss_hdop; uint16_t gnss_vdop; uint16_t gnss_tdop; - } gnss_dop; // *10e3 + } gnss_dop; // 10e3 }; AP_ExternalAHRS::gps_data_message_t gps_data; @@ -247,4 +247,3 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { }; #endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED - From b73ab693c62d2842b02824b849a4fbfb164f5ab6 Mon Sep 17 00:00:00 2001 From: VSheleverIL Date: Thu, 29 Feb 2024 17:20:05 +0200 Subject: [PATCH 3/9] ExternalAHRS: New GNSS data for ArduPilot --- .../AP_ExternalAHRS_InertialLabs.cpp | 271 +++++++++++------- .../AP_ExternalAHRS_InertialLabs.h | 30 +- 2 files changed, 186 insertions(+), 115 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index ba1d8b6679ad8..6b28bb4b915b0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -258,12 +258,12 @@ 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_ins_data.ms_tow = u.gps_time_ms; break; } case MessageType::GPS_WEEK: { CHECK_SIZE(u.gps_week); - gps_data.gps_week = u.gps_week; + gps_ins_data.gps_week = u.gps_week; break; } case MessageType::ACCEL_DATA_HR: { @@ -301,6 +301,9 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() case MessageType::VELOCITIES: { CHECK_SIZE(u.velocity); state.velocity = u.velocity.tofloat().rfu_to_frd()*0.01; + gps_ins_data.ned_vel_north = state.velocity.x; + gps_ins_data.ned_vel_east = state.velocity.y; + gps_ins_data.ned_vel_down = state.velocity.z; state.have_velocity = true; last_vel_ms = now_ms; break; @@ -310,6 +313,9 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() state.location.lat = u.position.lat; state.location.lng = u.position.lon; state.location.alt = u.position.alt; + gps_ins_data.latitude = u.position.lat; + gps_ins_data.longitude = u.position.lon; + gps_ins_data.msl_altitude = u.position.alt; state.have_location = true; state.last_location_update_us = AP_HAL::micros(); last_pos_ms = now_ms; @@ -317,12 +323,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()*0.001; 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()*0.001; break; } case MessageType::UNIT_STATUS: { @@ -332,20 +338,20 @@ 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; + gps_ins_data.fix_type = u.gnss_extended_info.fix_type+1; + gps_data.gnss_extended_info = u.gnss_extended_info; break; } case MessageType::NUM_SATS: { CHECK_SIZE(u.num_sats); - gps_data.satellites_in_view = u.num_sats; + gps_ins_data.satellites_in_view = u.num_sats; 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.gps_position); + gps_data.gps_lat = u.gps_position.gps_lat*1.0e-7; + gps_data.gps_lon = u.gps_position.gps_lon*1.0e-7; + gps_data.gps_alt = u.gps_position.gps_alt*1.0e-2; break; } case MessageType::GNSS_VEL_TRACK: { @@ -357,22 +363,22 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::GNSS_POS_TIMESTAMP: { CHECK_SIZE(u.gnss_pos_timestamp); - gps_data.ms_tow = u.gnss_pos_timestamp; + gps_data.gnss_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; + gps_data.gnss_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; + gps_data.gnss_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; + gps_data.gnss_jam_status = u.gnss_jam_status; break; } case MessageType::DIFFERENTIAL_PRESSURE: { @@ -414,27 +420,27 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::GNSS_ANGLES: { CHECK_SIZE(u.gnss_angles); - state2.gnss_heading = u.gnss_angles.gnss_heading * 1.0e-2; - state2.gnss_pitch = u.gnss_angles.gnss_pitch * 1.0e-2; + gps_data.gnss_heading = u.gnss_angles.gnss_heading*1.0e-2; + gps_data.gnss_pitch = u.gnss_angles.gnss_pitch*1.0e-2; break; } case MessageType::GNSS_ANGLE_POS_TYPE: { CHECK_SIZE(u.gnss_angle_pos_type); - state2.gnss_angle_pos_type = u.gnss_angle_pos_type; + gps_data.gnss_angle_pos_type = u.gnss_angle_pos_type; break; } case MessageType::GNSS_HEADING_TIMESTAMP: { CHECK_SIZE(u.gnss_heading_timestamp); - state2.gnss_heading_timestamp = u.gnss_heading_timestamp; + gps_data.gnss_heading_timestamp = u.gnss_heading_timestamp; break; } case MessageType::GNSS_DOP: { CHECK_SIZE(u.gnss_dop); - state2.gnss_gdop = u.gnss_dop.gnss_gdop * 1.0e-1; // in cm - state2.gnss_pdop = u.gnss_dop.gnss_pdop * 1.0e-1; // in cm - gps_data.hdop = u.gnss_dop.gnss_hdop * 1.0e-1; // in cm - gps_data.vdop = u.gnss_dop.gnss_vdop * 1.0e-1; // in cm - state2.gnss_tdop = u.gnss_dop.gnss_tdop * 1.0e-1; // in cm + gps_data.gnss_gdop = u.gnss_dop.gnss_gdop*1.0e-1; + gps_data.gnss_pdop = u.gnss_dop.gnss_pdop*1.0e-1; + gps_ins_data.hdop = u.gnss_dop.gnss_hdop*1.0e-1; + gps_ins_data.vdop = u.gnss_dop.gnss_vdop*1.0e-1; + gps_data.gnss_tdop = u.gnss_dop.gnss_tdop*1.0e-1; break; } } @@ -470,25 +476,91 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() GOT_MSG(GNSS_POSITION) && GOT_MSG(GNSS_NEW_DATA) && GOT_MSG(GNSS_EXTENDED_INFO) && - state2.gnss_new_data != 0) { + gps_data.gnss_new_data !=0) { uint8_t instance; if (AP::gps().get_first_external_instance(instance)) { - AP::gps().handle_external(gps_data, instance); + AP::gps().handle_external(gps_ins_data, instance); } - if (gps_data.satellites_in_view > 3) { + if (gps_ins_data.satellites_in_view > 3) { if (last_gps_ms == 0) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got GPS lock"); if (!state.have_origin) { state.origin = Location{ - gps_data.latitude, - gps_data.longitude, - gps_data.msl_altitude, + gps_ins_data.latitude, + gps_ins_data.longitude, + gps_ins_data.msl_altitude, Location::AltFrame::ABSOLUTE}; state.have_origin = true; } } last_gps_ms = now_ms; } + + uint64_t now_us_gps = AP_HAL::micros64(); + + // @LoggerMessage: ILB3 + // @Description: InertialLabs AHRS data3 + // @Field: TimeUS: Time since system startup + // @Field: TimeGPS: GNSS Position timestamp + // @Field: Lat: GNSS Latitude + // @Field: Lon: GNSS Longitude + // @Field: Alt: GNSS Altitude + // @Field: VN: GNSS velocity north + // @Field: VE: GNSS velocity east + // @Field: VD: GNSS velocity down + + AP::logger().WriteStreaming("ILB3", "TimeUS,TimeGPS,Lat,Lon,Alt,VN,VE,VD", + "s-DUmnnn", + "F0GG----", + "QIffffff", + now_us_gps, gps_data.gnss_pos_timestamp, + gps_data.gps_lat, gps_data.gps_lon, gps_data.gps_alt, + gps_data.ned_vel_north, gps_data.ned_vel_east, gps_data.ned_vel_down); + + // @LoggerMessage: ILB4 + // @Description: InertialLabs AHRS data4 + // @Field: TimeUS: Time since system startup + // @Field: TimeGPS: GNSS Position timestamp + // @Filed: NSat: Number of satellites + // @Field: FType: fix type + // @Field: SpStat: spoofing status + // @Field: GI1: GNSS Info1 + // @Field: GI2: GNSS Info2 + // @Field: GJS: GNSS jamming status + // @Field: GAPS: GNSS Angles position type + + AP::logger().WriteStreaming("ILB4", "TimeUS,TimeGPS,NSat,FType,SpStat,GI1,GI2,GJS,GAPS", + "s--------", + "F--------", + "QIBBBBBBB", + now_us_gps, gps_data.gnss_pos_timestamp, + gps_ins_data.satellites_in_view, + gps_data.gnss_extended_info.fix_type, gps_data.gnss_extended_info.spoofing_status, + gps_data.gnss_info_short.info1, gps_data.gnss_info_short.info2, + gps_data.gnss_jam_status, gps_data.gnss_angle_pos_type); + + // @LoggerMessage: ILB5 + // @Description: InertialLabs AHRS data5 + // @Field: TimeUS: Time since system startup + // @Field: TimeGPS: 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("ILB5", "TimeUS,TimeGPS,GpsHTS,GpsYaw,GpsPitch,GDOP,PDOP,HDOP,VDOP,TDOP", + "s--dd-----", + "F---------", + "QIIfffffff", + now_us_gps, gps_data.gnss_pos_timestamp, gps_data.gnss_heading_timestamp, + gps_data.gnss_heading, gps_data.gnss_pitch, + gps_data.gnss_gdop, gps_data.gnss_pdop, gps_ins_data.hdop, + gps_ins_data.vdop, gps_data.gnss_tdop); + } if (GOT_MSG(BARO_DATA) && GOT_MSG(TEMPERATURE)) { @@ -520,6 +592,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @LoggerMessage: ILB1 // @Description: InertialLabs AHRS data1 // @Field: TimeUS: Time since system startup + // @Field: TimeINS: GPS INS time (round) // @Field: Roll: euler roll // @Field: Pitch: euler pitch // @Field: Yaw: euler yaw @@ -528,89 +601,84 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @Field: VD: velocity down // @Field: Lat: latitude // @Field: Lon: longitude - // @Field: Alt: altitude AMSL - // @Field: TimeINS: GPS INS time (round) - // @Field: INSvolt: Supply voltage - - AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,TimeINS,INSvolt", - "sdddnnnDUmsv", - "F000000GG000", - "QffffffLLfIf", - now_us, + // @Field: Alt: altitude MSL + // @Field: INSdc: Supply voltage + + AP::logger().WriteStreaming("ILB1", "TimeUS,TimeINS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,INSdc", + "ssdddnnnDUmv", + "F0000000GG00", + "QIffffffLLff", + now_us, gps_ins_data.ms_tow, degrees(roll), degrees(pitch), degrees(yaw), state.velocity.x, state.velocity.y, state.velocity.z, state.location.lat, state.location.lng, state.location.alt*0.01, - state2.gnss_ins_time_ms, state2.supply_voltage); + state2.supply_voltage); // @LoggerMessage: ILB2 // @Description: InertialLabs AHRS data2 // @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("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD", - "smmmnnn", - "F000000", - "Qffffff", - now_us, + // @Field: TimeINS: GPS INS time (round) + // @Field: Stat1: USW1 + // @Field: Stat2: USW2 + // @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("ILB2", "TimeUS,TimeINS,Stat1,Stat2,PVN,PVE,PVD,VVN,VVE,VVD", + "ss--mmmnnn", + "F0--------", + "QIHHffffff", + now_us, gps_ins_data.ms_tow, + state2.unit_status, state2.unit_status2, 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); - // @LoggerMessage: ILB3 - // @Description: InertialLabs AHRS data3 + // @LoggerMessage: ILB6 + // @Description: InertialLabs AHRS data6 // @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: GX: Gyro X + // @Field: GY: Gyro Y + // @Field: GZ: Gyro z + // @Field: AX: Accelerometer X + // @Field: AY: Accelerometer Y + // @Field: AZ: Accelerometer Z + // @Field: MX: Magnetometer X + // @Field: MY: Magnetometer Y + // @Field: MZ: Magnetometer Z + + AP::logger().WriteStreaming("ILB6", "TimeUS,TimeINS,GX,GY,GZ,AX,AY,AZ,MX,MY,MZ", + "sskkkooo---", + "F----------", + "QIfffffffff", + now_us, gps_ins_data.ms_tow, + ins_data.gyro.y, ins_data.gyro.x,-ins_data.gyro.z, + ins_data.accel.y, ins_data.accel.x, -ins_data.accel.z, + mag_data.field.y, mag_data.field.x, -mag_data.field.z); + + // @LoggerMessage: ILB7 + // @Description: InertialLabs AHRS data7 + // @Field: TimeUS: Time since system startup + // @Field: TimeINS: GPS INS time (round) + // @Field: Press: Static pressure + // @Field: Diff: Differential pressure + // @Field: Temp: Temperature // @Field: TAS: true airspeed // @Field: WVN: Wind velocity north // @Field: WVE: Wind velocity east // @Field: WVD: Wind velocity down // @Field: ADU: Air Data Unit status - AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD,ADU", - "s------------", - "F------------", - "QHHBBBBBffffH", - 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, + AP::logger().WriteStreaming("ILB7", "TimeUS,TimeINS,Press,Diff,Temp,TAS,WVN,WVE,WVD,ADU", + "ssPPOnnnn-", + "F--B------", + "QIfffffffH", + now_us, gps_ins_data.ms_tow, + baro_data.pressure_pa, airspeed_data.differential_pressure, baro_data.temperature, + state2.true_airspeed, state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z, state2.air_data_status); - - // @LoggerMessage: ILB4 - // @Description: InertialLabs AHRS data4 - // @Field: TimeUS: Time since system startup - // @Field: GpsYaw: GNSS Heading - // @Field: GpsPitch: GNSS Pitch - // @Field: GpsHTS: GNSS Heading timestamp - // @Field: GpsAType: GNSS Angles position type - // @Field: GDOP: GNSS GDOP - // @Field: PDOP: GNSS PDOP - // @Field: HDOP: GNSS HDOP - // @Field: VDOP: GNSS VDOP - // @Field: TDOP: GNSS TDOP - - AP::logger().WriteStreaming("ILB4", "TimeUS,GpsYaw,GpsPitch,GpsHTS,GpsAType,GDOP,PDOP,HDOP,VDOP,TDOP", - "sdds------", - "F00I------", - "QffIBfffff", - now_us, - state2.gnss_heading, state2.gnss_pitch, - state2.gnss_heading_timestamp, state2.gnss_angle_pos_type, - state2.gnss_gdop * 1.0e-2, state2.gnss_pdop * 1.0e-2, gps_data.hdop * 1.0e-2, - gps_data.vdop * 1.0e-2, state2.gnss_tdop * 1.0e-2); } return true; @@ -748,18 +816,7 @@ void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const flags |= EKF_UNINITIALIZED; } - // send message - // const float vel_gate = 5; - // const float pos_gate = 5; - // const float hgt_gate = 5; - // const float mag_var = 0; - // mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - // state2.kf_vel_covariance.length()/vel_gate, - // state2.kf_pos_covariance.xy().length()/pos_gate, - // state2.kf_pos_covariance.z/hgt_gate, - // mag_var, 0, 0); - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - 0, 0, 0, 0, 0, 0); + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0); } #endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index 6325cc3a267f1..ac8c660e96dfa 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -154,10 +154,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 gps_lat; // deg*1e7 + int32_t gps_lon; // deg*1e7 + int32_t gps_alt; // m*100, AMSL + } gps_position; struct PACKED { int32_t hor_speed; // m/s*100 uint16_t track_over_ground; // deg*100 @@ -189,7 +194,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { } gnss_dop; // 10e3 }; - AP_ExternalAHRS::gps_data_message_t gps_data; + AP_ExternalAHRS::gps_data_message_t gps_ins_data; AP_ExternalAHRS::mag_data_message_t mag_data; AP_ExternalAHRS::baro_data_message_t baro_data; AP_ExternalAHRS::ins_data_message_t ins_data; @@ -219,18 +224,27 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { struct { 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; + } state2; + + struct { + float gps_lat; + float gps_lon; + float gps_alt; + float ned_vel_north; + float ned_vel_east; + float ned_vel_down; + uint32_t gnss_pos_timestamp; + 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 gnss_heading; float gnss_pitch; uint8_t gnss_angle_pos_type; @@ -238,7 +252,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { float gnss_gdop; float gnss_pdop; float gnss_tdop; - } state2; + } gps_data; uint32_t last_att_ms; uint32_t last_vel_ms; From 8c44974b56fa3f983ce1dd8f680c6217ad277b81 Mon Sep 17 00:00:00 2001 From: VSheleverIL Date: Mon, 4 Mar 2024 14:56:36 +0200 Subject: [PATCH 4/9] BoardConfig: new value for BRD_BOOT_DELAY parameter --- libraries/AP_BoardConfig/AP_BoardConfig.cpp | 2 +- libraries/AP_BoardConfig/AP_BoardConfig.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index 7367d34306e6b..725e336c3b3c5 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -385,7 +385,7 @@ void AP_BoardConfig::init() #endif if (_boot_delay_ms > 0) { - uint16_t delay_ms = uint16_t(_boot_delay_ms.get()); + uint32_t delay_ms = uint32_t(_boot_delay_ms.get()); if (hal.util->was_watchdog_armed() && delay_ms > 200) { // don't delay a long time on watchdog reset, the pilot // may be able to save the vehicle diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 47bf754da4653..ba30ffda9d254 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -310,7 +310,7 @@ class AP_BoardConfig { AP_Int8 _sdcard_slowdown; #endif - AP_Int16 _boot_delay_ms; + AP_Int32 _boot_delay_ms; AP_Int32 _options; From 7ee39af79ec6e12045d5b52d49efc5070ec496e2 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 28 Feb 2024 16:25:44 +0700 Subject: [PATCH 5/9] GCSMavlink: Cosmetic changes --- libraries/GCS_MAVLink/GCS_Common.cpp | 30 ++++++++++++++-------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ec4ff7eae0d76..34696fc07eb8c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -172,7 +172,7 @@ bool GCS_MAVLINK::init(uint8_t instance) } // since tcdrain() and TCSADRAIN may not be implemented... hal.scheduler->delay(1); - + _port->set_flow_control(old_flow_control); // now change back to desired baudrate @@ -328,7 +328,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const float current, consumed_mah, consumed_wh; const int8_t percentage = battery_remaining_pct(instance); - + if (battery.current_amps(current, instance)) { current = constrain_float(current * 100,-INT16_MAX,INT16_MAX); } else { @@ -1787,7 +1787,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) { const uint8_t c = (uint8_t)_port->read(); const uint32_t protocol_timeout = 4000; - + if (alternative.handler && now_ms - alternative.last_mavlink_ms > protocol_timeout) { /* @@ -1799,7 +1799,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) alternative.last_alternate_ms = now_ms; gcs_alternative_active[chan] = true; } - + /* we may also try parsing as MAVLink if we haven't had a successful parse on the alternative protocol for 4s @@ -3150,7 +3150,7 @@ float GCS_MAVLINK::vfr_hud_climbrate() const { #if AP_AHRS_ENABLED Vector3f velned; - if (AP::ahrs().get_velocity_NED(velned) || + if (AP::ahrs().get_velocity_NED(velned) || AP::ahrs().get_vert_pos_rate_D(velned.z)) { return -velned.z; } @@ -3183,7 +3183,7 @@ void GCS_MAVLINK::send_vfr_hud() } /* - handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command + handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command Optionally disable PX4IO overrides. This is done for quadplanes to prevent the mixer running while rebooting which can start the VTOL @@ -3731,7 +3731,7 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg) // correct offboard timestamp to be in local ms since boot uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ATT_POS_MOCAP)); - + AP_VisualOdom *visual_odom = AP::visualodom(); if (visual_odom == nullptr) { return; @@ -4125,7 +4125,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) #endif case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - // only pass if override is not selected + // only pass if override is not selected if (!(_port->get_options() & _port->OPTION_NOSTREAMOVERRIDE)) { handle_request_data_stream(msg); } @@ -4133,7 +4133,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_DATA96: handle_data_packet(msg); - break; + break; #if HAL_VISUALODOM_ENABLED case MAVLINK_MSG_ID_VISION_POSITION_DELTA: @@ -4519,7 +4519,7 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm AP::compass().force_save_calibration(); ret = MAV_RESULT_ACCEPTED; } - + return ret; } @@ -5652,7 +5652,7 @@ void GCS_MAVLINK::send_water_depth() const if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){ return; - } + } // get position const AP_AHRS &ahrs = AP::ahrs(); @@ -5661,7 +5661,7 @@ void GCS_MAVLINK::send_water_depth() const for (uint8_t i=0; inum_sensors(); i++) { const AP_RangeFinder_Backend *s = rangefinder->get_backend(i); - + if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) { continue; } @@ -6716,7 +6716,7 @@ uint64_t GCS_MAVLINK::capabilities() const if (!AP_BoardConfig::ftp_disabled()){ //if ftp disable board option is not set ret |= MAV_PROTOCOL_CAPABILITY_FTP; } - + return ret; } @@ -6836,7 +6836,7 @@ void GCS_MAVLINK::send_high_latency2() const //send_text(MAV_SEVERITY_INFO, "Yaw: %u", (((uint16_t)ahrs.yaw_sensor / 100) % 360)); - mavlink_msg_high_latency2_send(chan, + mavlink_msg_high_latency2_send(chan, AP_HAL::millis(), //[ms] Timestamp (milliseconds since boot or Unix epoch) gcs().frame_type(), // Type of the MAV (quadrotor, helicopter, etc.) MAV_AUTOPILOT_ARDUPILOTMEGA, // Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers. @@ -6886,7 +6886,7 @@ int8_t GCS_MAVLINK::high_latency_air_temperature() const } /* - handle a MAV_CMD_CONTROL_HIGH_LATENCY command + handle a MAV_CMD_CONTROL_HIGH_LATENCY command Enable or disable any marked (via SERIALn_PROTOCOL) high latency connections */ From 7aee05f8ac84141c1567e3f7e10f5c3a8ff23eb1 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 28 Feb 2024 16:26:06 +0700 Subject: [PATCH 6/9] ExternalAHRS: Cosmetic changes --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 3 +-- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 3 +-- libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp | 1 - libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 3 +-- 4 files changed, 3 insertions(+), 7 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 9b15e6135bfe2..c7d4acf4cd1b8 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -80,7 +80,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Bitmask: 0:GPS,1:IMU,2:Baro,3:Compass // @User: Advanced AP_GROUPINFO("_SENSORS", 4, AP_ExternalAHRS, sensors, 0xF), - + AP_GROUPEND }; @@ -302,4 +302,3 @@ AP_ExternalAHRS &externalAHRS() }; #endif // HAL_EXTERNAL_AHRS_ENABLED - diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index c904c68072d19..ef8cd1d06f602 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -163,7 +163,7 @@ class AP_ExternalAHRS { float differential_pressure; // Pa float temperature; // degC } airspeed_data_message_t; - + protected: enum class OPTIONS { @@ -197,4 +197,3 @@ namespace AP { }; #endif // HAL_EXTERNAL_AHRS_ENABLED - diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp index eaff493f5af3f..e2079f51e04c0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp @@ -38,4 +38,3 @@ bool AP_ExternalAHRS_backend::option_is_set(AP_ExternalAHRS::OPTIONS option) con } #endif // HAL_EXTERNAL_AHRS_ENABLED - diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index 02eb1b5319b7d..eb35c218e24c0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -57,10 +57,9 @@ class AP_ExternalAHRS_backend { void set_default_sensors(uint16_t sensors) { frontend.set_default_sensors(sensors); } - + private: AP_ExternalAHRS &frontend; }; #endif // HAL_EXTERNAL_AHRS_ENABLED - From d44b6b710c25640062443089f2fd2c482f84fc9b Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 28 Feb 2024 16:29:31 +0700 Subject: [PATCH 7/9] Mavlink: Add inertialLabs commands to mavlink --- libraries/GCS_MAVLink/GCS.h | 19 +++++++++--------- libraries/GCS_MAVLink/GCS_Common.cpp | 30 +++++++++++++++++++++++++++- modules/mavlink | 2 +- 3 files changed, 40 insertions(+), 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 87a76c3823f3f..300df6ece13cc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -408,7 +408,7 @@ class GCS_MAVLINK } // return a bitmap of active channels. Used by libraries to loop - // over active channels to send to all active channels + // over active channels to send to all active channels static uint8_t active_channel_mask(void) { return mavlink_active; } // return a bitmap of streaming channels @@ -425,7 +425,7 @@ class GCS_MAVLINK static bool is_private(mavlink_channel_t _chan) { return (mavlink_private & (1U<<(unsigned)_chan)) != 0; } - + // return true if channel is private bool is_private(void) const { return is_private(chan); } @@ -444,7 +444,7 @@ class GCS_MAVLINK allow forwarding of packets / heartbeats to be blocked as required by some components to reduce traffic */ static void disable_channel_routing(mavlink_channel_t chan) { routing.no_route_mask |= (1U<<(chan-MAVLINK_COMM_0)); } - + /* search for a component in the routing table with given mav_type and retrieve it's sysid, compid and channel returns if a matching component is found @@ -598,6 +598,8 @@ class GCS_MAVLINK bool get_ap_message_interval(ap_message id, uint16_t &interval_ms) const; MAV_RESULT handle_command_request_message(const mavlink_command_int_t &packet); + MAV_RESULT handle_inertiallabs_message(const mavlink_command_int_t &packet); + MAV_RESULT handle_START_RX_PAIR(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet); @@ -718,7 +720,7 @@ class GCS_MAVLINK MAV_RESULT handle_control_high_latency(const mavlink_command_int_t &packet); #endif // HAL_HIGH_LATENCY2_ENABLED - + static constexpr const float magic_force_arm_value = 2989.0f; static constexpr const float magic_force_disarm_value = 21196.0f; @@ -896,7 +898,7 @@ class GCS_MAVLINK // time when we missed sending a parameter for GCS static uint32_t reserve_param_space_start_ms; - + // bitmask of what mavlink channels are active static uint8_t mavlink_active; @@ -916,7 +918,7 @@ class GCS_MAVLINK }; struct pending_param_reply { - mavlink_channel_t chan; + mavlink_channel_t chan; float value; enum ap_var_type p_type; int16_t param_index; @@ -973,7 +975,7 @@ class GCS_MAVLINK struct pending_ftp { uint32_t offset; - mavlink_channel_t chan; + mavlink_channel_t chan; uint16_t seq_number; FTP_OP opcode; FTP_OP req_opcode; @@ -1066,7 +1068,7 @@ class GCS_MAVLINK } alternative; JitterCorrection lag_correction; - + // we cache the current location and send it even if the AHRS has // no idea where we are: Location global_position_current_loc; @@ -1366,4 +1368,3 @@ void can_printf(const char *fmt, ...); #define AP_HAVE_GCS_SEND_TEXT 0 #endif // HAL_GCS_ENABLED - diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 34696fc07eb8c..badde300cc390 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2989,6 +2989,29 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int return MAV_RESULT_ACCEPTED; } +MAV_RESULT GCS_MAVLINK::handle_inertiallabs_message(const mavlink_command_int_t &packet) +{ + switch ((uint32_t)packet.param1) { + case INERTIALLABS_AHRS_COMMAND_TYPE::DISABLE_GNSS: + send_text(MAV_SEVERITY_INFO, "DISABLE_GNSS command sent to the Inertial Labs AHRS"); + return MAV_RESULT_ACCEPTED; + + case INERTIALLABS_AHRS_COMMAND_TYPE::ENABLE_GNSS: + send_text(MAV_SEVERITY_INFO, "ENABLE_GNSS command sent to the Inertial Labs AHRS"); + return MAV_RESULT_ACCEPTED; + + case INERTIALLABS_AHRS_COMMAND_TYPE::START_VG3D_CALIBRATION_IN_FLIGHT: + send_text(MAV_SEVERITY_INFO, "START_VG3DCLB_FLIGHT command sent to the Inertial Labs AHRS"); + return MAV_RESULT_ACCEPTED; + + case INERTIALLABS_AHRS_COMMAND_TYPE::STOP_VG3D_CALIBRATION_IN_FLIGHT: + send_text(MAV_SEVERITY_INFO, "STOP_VG3DCLB_FLIGHT command sent to the Inertial Labs AHRS"); + return MAV_RESULT_ACCEPTED; + default: + return MAV_RESULT_FAILED; + } +} + bool GCS_MAVLINK::get_ap_message_interval(ap_message id, uint16_t &interval_ms) const { // check if it's a specially-handled message: @@ -4261,7 +4284,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; } #endif - } + } // switch (msg.msgid) } @@ -5317,6 +5340,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_REQUEST_MESSAGE: return handle_command_request_message(packet); +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + case MAV_CMD_INERTIALLABS_AHRS_SEND: + return handle_inertiallabs_message(packet); +#endif + } return MAV_RESULT_UNSUPPORTED; diff --git a/modules/mavlink b/modules/mavlink index 130a836efbfef..8c187b19264d9 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 130a836efbfef0eb3287a92cd5e187de7facdce2 +Subproject commit 8c187b19264d917757972c738992a24e01e0f2e5 From 9c7139a8510d0418a1af209d559cb039267458c9 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Thu, 21 Mar 2024 00:56:26 +0700 Subject: [PATCH 8/9] ExternalAHRS: Add command sending to AHRS --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 7 ++++ libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 1 + .../AP_ExternalAHRS_InertialLabs.cpp | 5 +++ .../AP_ExternalAHRS_InertialLabs.h | 1 + .../AP_ExternalAHRS_InertialLabs_command.h | 34 +++++++++++++++++++ .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 1 + 6 files changed, 49 insertions(+) create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs_command.h diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index c7d4acf4cd1b8..a9c63ae1f7ece 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -262,6 +262,13 @@ void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const } } +void AP_ExternalAHRS::write_bytes(const char *bytes, uint8_t len) +{ + if (backend) { + backend->write_bytes(bytes, len); + } +} + void AP_ExternalAHRS::update(void) { if (backend) { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index ef8cd1d06f602..78077d3a27fa2 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -118,6 +118,7 @@ class AP_ExternalAHRS { Vector3f get_gyro(void); Vector3f get_accel(void); void send_status_report(class GCS_MAVLINK &link) const; + void write_bytes(const char *bytes, uint8_t len); // update backend void update(); diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index 6b28bb4b915b0..013c9f4a9946d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -819,4 +819,9 @@ void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0); } +void AP_ExternalAHRS_InertialLabs::write_bytes(const char *bytes, uint8_t len) +{ + uart->write(reinterpret_cast(bytes), len); +} + #endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index ac8c660e96dfa..e458800f4411d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -38,6 +38,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; void get_filter_status(nav_filter_status &status) const override; void send_status_report(class GCS_MAVLINK &link) const override; + void write_bytes(const char *bytes, uint8_t len) override; // check for new data void update() override { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs_command.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs_command.h new file mode 100644 index 0000000000000..53f5cca6ae37a --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs_command.h @@ -0,0 +1,34 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + support for serial connected InertialLabs INS system + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + +namespace InertialLabs::Command { + +const char ENABLE_GNSS[] = "\xAA\x55\x00\x00\x07\x00\x71\x78\x00"; +const char DISABLE_GNSS[] = "\xAA\x55\x00\x00\x07\x00\x72\x79\x00"; +const char START_VG3DCLB_FLIGHT[] = "\xAA\x55\x00\x00\x07\x00\x26\x2D\x00"; +const char STOP_VG3DCLB_FLIGHT[] = "\xAA\x55\x00\x00\x07\x00\x27\x2E\x00"; + +} // namespace Inertiallabs::Command + +#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index eb35c218e24c0..fedb2938f299b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -42,6 +42,7 @@ class AP_ExternalAHRS_backend { virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0; virtual void get_filter_status(nav_filter_status &status) const {} virtual void send_status_report(class GCS_MAVLINK &link) const {} + virtual void write_bytes(const char *bytes, uint8_t len) {}; // Check for new data. // This is used when there's not a separate thread for EAHRS. From 2ef0119ae676cd27e6956d44300bbf545b3fe3d1 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Thu, 21 Mar 2024 00:57:11 +0700 Subject: [PATCH 9/9] Mavlink: Add command sending to External AHRS --- libraries/GCS_MAVLink/GCS_Common.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index badde300cc390..f8f0d5d152745 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -74,6 +74,11 @@ #include #include +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED +#include +#include +#endif + #include #if HAL_RCINPUT_WITH_AP_RADIO @@ -2994,18 +2999,26 @@ MAV_RESULT GCS_MAVLINK::handle_inertiallabs_message(const mavlink_command_int_t switch ((uint32_t)packet.param1) { case INERTIALLABS_AHRS_COMMAND_TYPE::DISABLE_GNSS: send_text(MAV_SEVERITY_INFO, "DISABLE_GNSS command sent to the Inertial Labs AHRS"); + AP::externalAHRS().write_bytes(InertialLabs::Command::DISABLE_GNSS, + sizeof(InertialLabs::Command::DISABLE_GNSS) - 1); return MAV_RESULT_ACCEPTED; case INERTIALLABS_AHRS_COMMAND_TYPE::ENABLE_GNSS: send_text(MAV_SEVERITY_INFO, "ENABLE_GNSS command sent to the Inertial Labs AHRS"); + AP::externalAHRS().write_bytes(InertialLabs::Command::ENABLE_GNSS, + sizeof(InertialLabs::Command::ENABLE_GNSS) - 1); return MAV_RESULT_ACCEPTED; case INERTIALLABS_AHRS_COMMAND_TYPE::START_VG3D_CALIBRATION_IN_FLIGHT: send_text(MAV_SEVERITY_INFO, "START_VG3DCLB_FLIGHT command sent to the Inertial Labs AHRS"); + AP::externalAHRS().write_bytes(InertialLabs::Command::START_VG3DCLB_FLIGHT, + sizeof(InertialLabs::Command::START_VG3DCLB_FLIGHT) - 1); return MAV_RESULT_ACCEPTED; case INERTIALLABS_AHRS_COMMAND_TYPE::STOP_VG3D_CALIBRATION_IN_FLIGHT: send_text(MAV_SEVERITY_INFO, "STOP_VG3DCLB_FLIGHT command sent to the Inertial Labs AHRS"); + AP::externalAHRS().write_bytes(InertialLabs::Command::STOP_VG3DCLB_FLIGHT, + sizeof(InertialLabs::Command::STOP_VG3DCLB_FLIGHT) - 1); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED;