From c9a83c8127e17fbecd6a84552bd77f37cfe667e6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Jan 2021 16:40:00 +1100 Subject: [PATCH] AP_InertialSensor: added INS_TCAL_OPTIONS make persistent params optional, and allow save of accelcal --- .../AP_InertialSensor/AP_InertialSensor.cpp | 8 +++ .../AP_InertialSensor/AP_InertialSensor.h | 6 +++ .../AP_InertialSensor_tempcal.cpp | 50 +++++++++++++++---- 3 files changed, 54 insertions(+), 10 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index d29ae9c948..fae39e45cf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -621,6 +621,14 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Calibration: 1 AP_GROUPINFO("GYR_CALTEMP3", 51, AP_InertialSensor, caltemp_gyro[2], -100), #endif + + // @Param: TCAL_OPTIONS + // @DisplayName: Options for temperature calibration + // @Description: This enables optional temperature calibration features. Setting PersistParams will save the accelerometer and temperature calibration parameters in the bootloader sector on the next update of the bootloader. + // @Bitmask: 0:PersistParams + // @User: Advanced + AP_GROUPINFO("TCAL_OPTIONS", 52, AP_InertialSensor, tcal_options, 0), + #endif // HAL_INS_TEMPERATURE_CAL_ENABLE /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index d8d4aa9ae4..9305a45492 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -771,9 +771,15 @@ public: private: TCal tcal[INS_MAX_INSTANCES]; + enum class TCalOptions : uint8_t { + PERSIST_TEMP_CAL = (1U<<0), + PERSIST_ACCEL_CAL = (1U<<1), + }; + // temperature that last calibration was run at AP_Float caltemp_accel[INS_MAX_INSTANCES]; AP_Float caltemp_gyro[INS_MAX_INSTANCES]; + AP_Int32 tcal_options; bool tcal_learning; int8_t tcal_old_brd_target; #endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp index 72d219b87c..b3cab406f3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp @@ -432,17 +432,17 @@ void AP_InertialSensor::TCal::get_persistent_params(ExpandingString &str) const } const uint8_t imu = instance()+1; str.printf("INS_TCAL%u_ENABLE=1\n", imu); - str.printf("INS_TCAL%u_TMIN=%.1f\n", imu, temp_min.get()); - str.printf("INS_TCAL%u_TMAX=%.1f\n", imu, temp_max.get()); + str.printf("INS_TCAL%u_TMIN=%.2f\n", imu, temp_min.get()); + str.printf("INS_TCAL%u_TMAX=%.2f\n", imu, temp_max.get()); for (uint8_t k=0; k<3; k++) { const Vector3f &acc = accel_coeff[k].get(); const Vector3f &gyr = gyro_coeff[k].get(); - str.printf("INS_TCAL%u_ACC%u_X=%.1f\n", imu, k+1, acc.x); - str.printf("INS_TCAL%u_ACC%u_Y=%.1f\n", imu, k+1, acc.y); - str.printf("INS_TCAL%u_ACC%u_Z=%.1f\n", imu, k+1, acc.z); - str.printf("INS_TCAL%u_GYR%u_X=%.1f\n", imu, k+1, gyr.x); - str.printf("INS_TCAL%u_GYR%u_Y=%.1f\n", imu, k+1, gyr.y); - str.printf("INS_TCAL%u_GYR%u_Z=%.1f\n", imu, k+1, gyr.z); + str.printf("INS_TCAL%u_ACC%u_X=%f\n", imu, k+1, acc.x); + str.printf("INS_TCAL%u_ACC%u_Y=%f\n", imu, k+1, acc.y); + str.printf("INS_TCAL%u_ACC%u_Z=%f\n", imu, k+1, acc.z); + str.printf("INS_TCAL%u_GYR%u_X=%f\n", imu, k+1, gyr.x); + str.printf("INS_TCAL%u_GYR%u_Y=%f\n", imu, k+1, gyr.y); + str.printf("INS_TCAL%u_GYR%u_Z=%f\n", imu, k+1, gyr.z); } } @@ -452,8 +452,38 @@ void AP_InertialSensor::TCal::get_persistent_params(ExpandingString &str) const */ void AP_InertialSensor::get_persistent_params(ExpandingString &str) const { - for (auto &tc : tcal) { - tc.get_persistent_params(str); + bool save_options = false; + if (uint32_t(tcal_options.get()) & uint32_t(TCalOptions::PERSIST_ACCEL_CAL)) { + save_options = true; + for (uint8_t i=0; i 0) { + id[0] = '1'+i; + } + str.printf("INS_ACC%sOFFS_X=%f\n", id, aoff.x); + str.printf("INS_ACC%sOFFS_Y=%f\n", id, aoff.y); + str.printf("INS_ACC%sOFFS_Z=%f\n", id, aoff.z); + str.printf("INS_ACC%sSCAL_X=%f\n", id, ascl.x); + str.printf("INS_ACC%sSCAL_Y=%f\n", id, ascl.y); + str.printf("INS_ACC%sSCAL_Z=%f\n", id, ascl.z); + str.printf("INS_ACC_CALTEMP%u=%.2f\n", imu, caltemp_accel[i].get()); + } + } + if (uint32_t(tcal_options.get()) & uint32_t(TCalOptions::PERSIST_TEMP_CAL)) { + for (auto &tc : tcal) { + tc.get_persistent_params(str); + } + save_options = true; + } + if (save_options) { + /* + we also have to save the TCAL_OPTIONS parameter so that + fuuture flashing of the bootloader doesn't cause an erase + */ + str.printf("INS_TCAL_OPTIONS=%u\n", unsigned(tcal_options.get())); } }