aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: added INS_TCAL_OPTIONS

Browse Source 
make persistent params optional, and allow save of accelcal
This commit is contained in:
Andrew Tridgell 2021-01-16 16:40:00 +11:00 committed by Peter Barker
parent 5c823debc0
commit c9a83c8127
3 changed files with 54 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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
/*

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -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

50
libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp
View File

@ -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<INS_MAX_INSTANCES; i++) {
const uint8_t imu = i+1;
const Vector3f &aoff = _accel_offset[i].get();
const Vector3f &ascl = _accel_scale[i].get();
char id[2] = "";
if (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()));
}
}