diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index d3da39a26ba59c..37ea34828fa1d0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -89,6 +89,13 @@ class AP_ExternalAHRS { // get serial port number, -1 for not enabled int8_t get_port(AvailableSensor sensor) const; + enum class TempCal { + DoesntProvideTemp, + IsNotTempCalibrated, + IsTempCalibrated + }; + + struct state_t { HAL_Semaphore sem; @@ -163,6 +170,7 @@ class AP_ExternalAHRS { Vector3f accel; Vector3f gyro; float temperature; + TempCal TempCalibration = TempCal::IsNotTempCalibrated; } ins_data_message_t; typedef struct { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index ae45f1fb9e6816..d4b6648c1debe7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -124,11 +124,10 @@ void AP_ExternalAHRS_MicroStrain5::post_imu() const } { - AP_ExternalAHRS::ins_data_message_t ins { - accel: imu_data.accel, - gyro: imu_data.gyro, - temperature: -300 - }; + AP_ExternalAHRS::ins_data_message_t ins; + ins.accel = imu_data.accel; + ins.gyro = imu_data.gyro; + ins.temperature= -300; AP::ins().handle_external(ins); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 4fb22c5a188af3..9bc13055b08734 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -152,13 +152,11 @@ void AP_ExternalAHRS_MicroStrain7::post_imu() const } { - // *INDENT-OFF* - AP_ExternalAHRS::ins_data_message_t ins { - accel: imu_data.accel, - gyro: imu_data.gyro, - temperature: -300 - }; - // *INDENT-ON* + AP_ExternalAHRS::ins_data_message_t ins; + ins.accel = imu_data.accel; + ins.gyro = imu_data.gyro; + ins.temperature= -300; + ins.TempCalibration = AP_ExternalAHRS::TempCal::IsTempCalibrated; AP::ins().handle_external(ins); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SBG.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SBG.cpp index 0ac5b940621aed..f46f9d0282dcdf 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SBG.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SBG.cpp @@ -631,6 +631,7 @@ void AP_ExternalAHRS_SBG::handle_msg(const sbgMessage &msg) if (updated_ins) { cached.sensors.ins_data.accel = state.accel; cached.sensors.ins_data.gyro = state.gyro; + cached.sensors.ins_data.TempCalibration = AP_ExternalAHRS::TempCal::IsTempCalibrated; cached.sensors.ins_ms = now_ms; AP::ins().handle_external(cached.sensors.ins_data); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 6c962e3b145c18..cd142a7ab86c33 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -2795,6 +2795,9 @@ void AP_InertialSensor::send_uart_data(void) void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) { for (uint8_t i = 0; i < _backend_count; i++) { + if(pkt.TempCalibration == AP_ExternalAHRS::TempCal::IsTempCalibrated){ + tcal(i).enable.set_and_save_ifchanged(int8_t(AP_InertialSensor_TCal::Enable::Disabled)); + } _backends[i]->handle_external(pkt); } }