mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 01:33:56 -04:00
AP_InertialSensor: add support for extra Aux IMUs
This commit is contained in:
bugobliterator
Andrew Tridgell
parent
087770f718
commit
10439cc42e
@ -131,7 +131,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Units: rad/s
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYROFFS", 3, AP_InertialSensor, _gyro_offset[0], 0),
|
||||
AP_GROUPINFO("GYROFFS", 3, AP_InertialSensor, _gyro_offset_old_param[0], 0),
|
||||
|
||||
// @Param: GYR2OFFS_X
|
||||
// @DisplayName: Gyro2 offsets of X axis
|
||||
@ -155,7 +155,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Calibration: 1
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
AP_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset[1], 0),
|
||||
AP_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset_old_param[1], 0),
|
||||
#endif
|
||||
|
||||
// @Param: GYR3OFFS_X
|
||||
@ -180,7 +180,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Calibration: 1
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
AP_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset[2], 0),
|
||||
AP_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset_old_param[2], 0),
|
||||
#endif
|
||||
|
||||
// @Param: ACCSCAL_X
|
||||
@ -203,7 +203,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Range: 0.8 1.2
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACCSCAL", 12, AP_InertialSensor, _accel_scale[0], 1.0),
|
||||
AP_GROUPINFO("ACCSCAL", 12, AP_InertialSensor, _accel_scale_old_param[0], 1.0),
|
||||
|
||||
// @Param: ACCOFFS_X
|
||||
// @DisplayName: Accelerometer offsets of X axis
|
||||
@ -228,7 +228,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Range: -3.5 3.5
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACCOFFS", 13, AP_InertialSensor, _accel_offset[0], 0),
|
||||
AP_GROUPINFO("ACCOFFS", 13, AP_InertialSensor, _accel_offset_old_param[0], 0),
|
||||
|
||||
// @Param: ACC2SCAL_X
|
||||
// @DisplayName: Accelerometer2 scaling of X axis
|
||||
@ -252,7 +252,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Calibration: 1
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
AP_GROUPINFO("ACC2SCAL", 14, AP_InertialSensor, _accel_scale[1], 1.0),
|
||||
AP_GROUPINFO("ACC2SCAL", 14, AP_InertialSensor, _accel_scale_old_param[1], 1.0),
|
||||
#endif
|
||||
|
||||
// @Param: ACC2OFFS_X
|
||||
@ -280,7 +280,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Calibration: 1
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
AP_GROUPINFO("ACC2OFFS", 15, AP_InertialSensor, _accel_offset[1], 0),
|
||||
AP_GROUPINFO("ACC2OFFS", 15, AP_InertialSensor, _accel_offset_old_param[1], 0),
|
||||
#endif
|
||||
|
||||
// @Param: ACC3SCAL_X
|
||||
@ -305,7 +305,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Calibration: 1
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
AP_GROUPINFO("ACC3SCAL", 16, AP_InertialSensor, _accel_scale[2], 1.0),
|
||||
AP_GROUPINFO("ACC3SCAL", 16, AP_InertialSensor, _accel_scale_old_param[2], 1.0),
|
||||
#endif
|
||||
|
||||
// @Param: ACC3OFFS_X
|
||||
@ -333,7 +333,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Calibration: 1
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
AP_GROUPINFO("ACC3OFFS", 17, AP_InertialSensor, _accel_offset[2], 0),
|
||||
AP_GROUPINFO("ACC3OFFS", 17, AP_InertialSensor, _accel_offset_old_param[2], 0),
|
||||
#endif
|
||||
|
||||
// @Param: GYRO_FILTER
|
||||
@ -357,7 +357,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Description: Use first IMU for attitude, velocity and position estimates
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("USE", 20, AP_InertialSensor, _use[0], 1),
|
||||
AP_GROUPINFO("USE", 20, AP_InertialSensor, _use_old_param[0], 1),
|
||||
|
||||
// @Param: USE2
|
||||
// @DisplayName: Use second IMU for attitude, velocity and position estimates
|
||||
@ -366,7 +366,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
AP_GROUPINFO("USE2", 21, AP_InertialSensor, _use[1], 1),
|
||||
AP_GROUPINFO("USE2", 21, AP_InertialSensor, _use_old_param[1], 1),
|
||||
#endif
|
||||
|
||||
// @Param: USE3
|
||||
@ -376,7 +376,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
AP_GROUPINFO("USE3", 22, AP_InertialSensor, _use[2], 1),
|
||||
AP_GROUPINFO("USE3", 22, AP_InertialSensor, _use_old_param[2], 1),
|
||||
#endif
|
||||
|
||||
// @Param: STILL_THRESH
|
||||
@ -430,7 +430,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("POS1", 27, AP_InertialSensor, _accel_pos[0], 0.0f),
|
||||
AP_GROUPINFO("POS1", 27, AP_InertialSensor, _accel_pos_old_param[0], 0.0f),
|
||||
|
||||
// @Param: POS2_X
|
||||
// @DisplayName: IMU accelerometer X position
|
||||
@ -457,7 +457,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
AP_GROUPINFO("POS2", 28, AP_InertialSensor, _accel_pos[1], 0.0f),
|
||||
AP_GROUPINFO("POS2", 28, AP_InertialSensor, _accel_pos_old_param[1], 0.0f),
|
||||
#endif
|
||||
|
||||
// @Param: POS3_X
|
||||
@ -484,7 +484,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
AP_GROUPINFO("POS3", 29, AP_InertialSensor, _accel_pos[2], 0.0f),
|
||||
AP_GROUPINFO("POS3", 29, AP_InertialSensor, _accel_pos_old_param[2], 0.0f),
|
||||
#endif
|
||||
|
||||
// @Param: GYR_ID
|
||||
@ -492,7 +492,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Description: Gyro sensor ID, taking into account its type, bus and instance
|
||||
// @ReadOnly: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GYR_ID", 30, AP_InertialSensor, _gyro_id[0], 0),
|
||||
AP_GROUPINFO("GYR_ID", 30, AP_InertialSensor, _gyro_id_old_param[0], 0),
|
||||
|
||||
// @Param: GYR2_ID
|
||||
// @DisplayName: Gyro2 ID
|
||||
@ -501,7 +501,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
AP_GROUPINFO("GYR2_ID", 31, AP_InertialSensor, _gyro_id[1], 0),
|
||||
AP_GROUPINFO("GYR2_ID", 31, AP_InertialSensor, _gyro_id_old_param[1], 0),
|
||||
#endif
|
||||
|
||||
// @Param: GYR3_ID
|
||||
@ -511,7 +511,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
AP_GROUPINFO("GYR3_ID", 32, AP_InertialSensor, _gyro_id[2], 0),
|
||||
AP_GROUPINFO("GYR3_ID", 32, AP_InertialSensor, _gyro_id_old_param[2], 0),
|
||||
#endif
|
||||
|
||||
// @Param: ACC_ID
|
||||
@ -519,7 +519,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @Description: Accelerometer sensor ID, taking into account its type, bus and instance
|
||||
// @ReadOnly: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ACC_ID", 33, AP_InertialSensor, _accel_id[0], 0),
|
||||
AP_GROUPINFO("ACC_ID", 33, AP_InertialSensor, _accel_id_old_param[0], 0),
|
||||
|
||||
// @Param: ACC2_ID
|
||||
// @DisplayName: Accelerometer2 ID
|
||||
@ -528,7 +528,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
AP_GROUPINFO("ACC2_ID", 34, AP_InertialSensor, _accel_id[1], 0),
|
||||
AP_GROUPINFO("ACC2_ID", 34, AP_InertialSensor, _accel_id_old_param[1], 0),
|
||||
#endif
|
||||
|
||||
// @Param: ACC3_ID
|
||||
@ -538,7 +538,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
AP_GROUPINFO("ACC3_ID", 35, AP_InertialSensor, _accel_id[2], 0),
|
||||
AP_GROUPINFO("ACC3_ID", 35, AP_InertialSensor, _accel_id_old_param[2], 0),
|
||||
#endif
|
||||
|
||||
// @Param: FAST_SAMPLE
|
||||
@ -585,18 +585,18 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// @Group: TCAL1_
|
||||
// @Path: AP_InertialSensor_tempcal.cpp
|
||||
AP_SUBGROUPINFO(tcal[0], "TCAL1_", 43, AP_InertialSensor, AP_InertialSensor::TCal),
|
||||
AP_SUBGROUPINFO(tcal_old_param[0], "TCAL1_", 43, AP_InertialSensor, AP_InertialSensor_TCal),
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
// @Group: TCAL2_
|
||||
// @Path: AP_InertialSensor_tempcal.cpp
|
||||
AP_SUBGROUPINFO(tcal[1], "TCAL2_", 44, AP_InertialSensor, AP_InertialSensor::TCal),
|
||||
AP_SUBGROUPINFO(tcal_old_param[1], "TCAL2_", 44, AP_InertialSensor, AP_InertialSensor_TCal),
|
||||
#endif
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
// @Group: TCAL3_
|
||||
// @Path: AP_InertialSensor_tempcal.cpp
|
||||
AP_SUBGROUPINFO(tcal[2], "TCAL3_", 45, AP_InertialSensor, AP_InertialSensor::TCal),
|
||||
AP_SUBGROUPINFO(tcal_old_param[2], "TCAL3_", 45, AP_InertialSensor, AP_InertialSensor_TCal),
|
||||
#endif
|
||||
|
||||
// @Param: ACC1_CALTEMP
|
||||
@ -605,7 +605,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACC1_CALTEMP", 46, AP_InertialSensor, caltemp_accel[0], -300),
|
||||
AP_GROUPINFO("ACC1_CALTEMP", 46, AP_InertialSensor, caltemp_accel_old_param[0], -300),
|
||||
|
||||
// @Param: GYR1_CALTEMP
|
||||
// @DisplayName: Calibration temperature for 1st gyroscope
|
||||
@ -613,7 +613,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYR1_CALTEMP", 47, AP_InertialSensor, caltemp_gyro[0], -300),
|
||||
AP_GROUPINFO("GYR1_CALTEMP", 47, AP_InertialSensor, caltemp_gyro_old_param[0], -300),
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
// @Param: ACC2_CALTEMP
|
||||
@ -622,7 +622,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACC2_CALTEMP", 48, AP_InertialSensor, caltemp_accel[1], -300),
|
||||
AP_GROUPINFO("ACC2_CALTEMP", 48, AP_InertialSensor, caltemp_accel_old_param[1], -300),
|
||||
|
||||
// @Param: GYR2_CALTEMP
|
||||
// @DisplayName: Calibration temperature for 2nd gyroscope
|
||||
@ -630,7 +630,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYR2_CALTEMP", 49, AP_InertialSensor, caltemp_gyro[1], -300),
|
||||
AP_GROUPINFO("GYR2_CALTEMP", 49, AP_InertialSensor, caltemp_gyro_old_param[1], -300),
|
||||
#endif
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
@ -640,7 +640,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACC3_CALTEMP", 50, AP_InertialSensor, caltemp_accel[2], -300),
|
||||
AP_GROUPINFO("ACC3_CALTEMP", 50, AP_InertialSensor, caltemp_accel_old_param[2], -300),
|
||||
|
||||
// @Param: GYR3_CALTEMP
|
||||
// @DisplayName: Calibration temperature for 3rd gyroscope
|
||||
@ -648,7 +648,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYR3_CALTEMP", 51, AP_InertialSensor, caltemp_gyro[2], -300),
|
||||
AP_GROUPINFO("GYR3_CALTEMP", 51, AP_InertialSensor, caltemp_gyro_old_param[2], -300),
|
||||
#endif
|
||||
|
||||
// @Param: TCAL_OPTIONS
|
||||
@ -660,6 +660,19 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
||||
|
||||
#endif // HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
|
||||
|
||||
#if INS_MAX_INSTANCES > 3
|
||||
// @Group: 4_
|
||||
// @Path: AP_InertialSensor_Params.cpp
|
||||
AP_SUBGROUPINFO(params[0], "4_", 53, AP_InertialSensor, AP_InertialSensor_Params),
|
||||
#endif
|
||||
|
||||
#if INS_MAX_INSTANCES > 4
|
||||
// @Group: 5_
|
||||
// @Path: AP_InertialSensor_Params.cpp
|
||||
AP_SUBGROUPINFO(params[1], "5_", 54, AP_InertialSensor, AP_InertialSensor_Params),
|
||||
#endif
|
||||
|
||||
/*
|
||||
NOTE: parameter indexes have gaps above. When adding new
|
||||
parameters check for conflicts carefully
|
||||
@ -717,20 +730,20 @@ bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rat
|
||||
_gyro_over_sampling[_gyro_count] = 1;
|
||||
_gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000);
|
||||
|
||||
bool saved = _gyro_id[_gyro_count].load();
|
||||
bool saved = _gyro_id(_gyro_count).load();
|
||||
|
||||
if (saved && (uint32_t)_gyro_id[_gyro_count] != id) {
|
||||
if (saved && (uint32_t)_gyro_id(_gyro_count) != id) {
|
||||
// inconsistent gyro id - mark it as needing calibration
|
||||
_gyro_cal_ok[_gyro_count] = false;
|
||||
}
|
||||
|
||||
_gyro_id[_gyro_count].set((int32_t) id);
|
||||
_gyro_id(_gyro_count).set((int32_t) id);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (!saved) {
|
||||
// assume this is the same sensor and save its ID to allow seamless
|
||||
// transition from when we didn't have the IDs.
|
||||
_gyro_id[_gyro_count].save();
|
||||
_gyro_id(_gyro_count).save();
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -753,25 +766,25 @@ bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_ra
|
||||
_accel_over_sampling[_accel_count] = 1;
|
||||
_accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS);
|
||||
|
||||
bool saved = _accel_id[_accel_count].load();
|
||||
bool saved = _accel_id(_accel_count).load();
|
||||
|
||||
if (!saved) {
|
||||
// inconsistent accel id
|
||||
_accel_id_ok[_accel_count] = false;
|
||||
} else if ((uint32_t)_accel_id[_accel_count] != id) {
|
||||
} else if ((uint32_t)_accel_id(_accel_count) != id) {
|
||||
// inconsistent accel id
|
||||
_accel_id_ok[_accel_count] = false;
|
||||
} else {
|
||||
_accel_id_ok[_accel_count] = true;
|
||||
}
|
||||
|
||||
_accel_id[_accel_count].set((int32_t) id);
|
||||
_accel_id(_accel_count).set((int32_t) id);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_SIM_ENABLED)
|
||||
// assume this is the same sensor and save its ID to allow seamless
|
||||
// transition from when we didn't have the IDs.
|
||||
_accel_id_ok[_accel_count] = true;
|
||||
_accel_id[_accel_count].save();
|
||||
_accel_id(_accel_count).save();
|
||||
#endif
|
||||
|
||||
instance = _accel_count++;
|
||||
@ -797,10 +810,10 @@ void AP_InertialSensor::_start_backends()
|
||||
|
||||
// clear IDs for unused sensor instances
|
||||
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
|
||||
_accel_id[i].set(0);
|
||||
_accel_id(i).set(0);
|
||||
}
|
||||
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
|
||||
_gyro_id[i].set(0);
|
||||
_gyro_id(i).set(0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -960,7 +973,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
|
||||
// count number of used sensors
|
||||
uint8_t sensors_used = 0;
|
||||
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
|
||||
sensors_used += _use[i];
|
||||
sensors_used += _use(i);
|
||||
}
|
||||
|
||||
uint8_t num_filters = 0;
|
||||
@ -983,7 +996,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
|
||||
// allocate notches
|
||||
for (uint8_t i=0; i<get_gyro_count(); i++) {
|
||||
// only allocate notches for IMUs in use
|
||||
if (_use[i]) {
|
||||
if (_use(i)) {
|
||||
for (auto ¬ch : harmonic_notches) {
|
||||
if (notch.params.enabled() || fft_enabled) {
|
||||
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
|
||||
@ -1042,8 +1055,8 @@ AP_InertialSensor::detect_backends(void)
|
||||
// boards. For users who really want limited IMUs they will need
|
||||
// to either use the INS_ENABLE_MASK or set INS_USE2=0 which will
|
||||
// enable the first IMU without triggering this check
|
||||
if (_use[0] == 1 && _use[1] == 1 && _use[2] == 0) {
|
||||
_use[2].set(1);
|
||||
if (_use(0) == 1 && _use(1) == 1 && _use(2) == 0) {
|
||||
_use(2).set(1);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -1352,7 +1365,7 @@ bool AP_InertialSensor::gyro_calibrated_ok_all() const
|
||||
}
|
||||
}
|
||||
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
|
||||
if (_gyro_id[i] != 0) {
|
||||
if (_gyro_id(i) != 0) {
|
||||
// missing gyro
|
||||
return false;
|
||||
}
|
||||
@ -1367,7 +1380,7 @@ bool AP_InertialSensor::use_gyro(uint8_t instance) const
|
||||
return false;
|
||||
}
|
||||
|
||||
return (get_gyro_health(instance) && _use[instance]);
|
||||
return (get_gyro_health(instance) && _use(instance));
|
||||
}
|
||||
|
||||
// get_accel_health_all - return true if all accels are healthy
|
||||
@ -1440,16 +1453,16 @@ bool AP_InertialSensor::accel_calibrated_ok_all() const
|
||||
return false;
|
||||
}
|
||||
// exactly 0.0 offset is extremely unlikely
|
||||
if (_accel_offset[i].get().is_zero()) {
|
||||
if (_accel_offset(i).get().is_zero()) {
|
||||
return false;
|
||||
}
|
||||
// zero scaling also indicates not calibrated
|
||||
if (_accel_scale[i].get().is_zero()) {
|
||||
if (_accel_scale(i).get().is_zero()) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
|
||||
if (_accel_id[i] != 0) {
|
||||
if (_accel_id(i) != 0) {
|
||||
// missing accel
|
||||
return false;
|
||||
}
|
||||
@ -1458,9 +1471,9 @@ bool AP_InertialSensor::accel_calibrated_ok_all() const
|
||||
// check calibrated accels matches number of accels (no unused accels should have offsets or scaling)
|
||||
if (get_accel_count() < INS_MAX_INSTANCES) {
|
||||
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
|
||||
const Vector3f &scaling = _accel_scale[i].get();
|
||||
const Vector3f &scaling = _accel_scale(i).get();
|
||||
bool have_scaling = (!is_zero(scaling.x) && !is_equal(scaling.x,1.0f)) || (!is_zero(scaling.y) && !is_equal(scaling.y,1.0f)) || (!is_zero(scaling.z) && !is_equal(scaling.z,1.0f));
|
||||
bool have_offsets = !_accel_offset[i].get().is_zero();
|
||||
bool have_offsets = !_accel_offset(i).get().is_zero();
|
||||
if (have_scaling || have_offsets) {
|
||||
return false;
|
||||
}
|
||||
@ -1478,7 +1491,7 @@ bool AP_InertialSensor::use_accel(uint8_t instance) const
|
||||
return false;
|
||||
}
|
||||
|
||||
return (get_accel_health(instance) && _use[instance]);
|
||||
return (get_accel_health(instance) && _use(instance));
|
||||
}
|
||||
|
||||
void
|
||||
@ -1516,7 +1529,7 @@ AP_InertialSensor::_init_gyro()
|
||||
|
||||
// remove existing gyro offsets
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
_gyro_offset[k].set(Vector3f());
|
||||
_gyro_offset(k).set(Vector3f());
|
||||
new_gyro_offset[k].zero();
|
||||
best_diff[k] = -1.f;
|
||||
last_average[k].zero();
|
||||
@ -1616,14 +1629,14 @@ AP_InertialSensor::_init_gyro()
|
||||
(unsigned)k,
|
||||
(double)ToDeg(best_diff[k]),
|
||||
(double)GYRO_INIT_MAX_DIFF_DPS);
|
||||
_gyro_offset[k].set(best_avg[k]);
|
||||
_gyro_offset(k).set(best_avg[k]);
|
||||
// flag calibration as failed for this gyro
|
||||
_gyro_cal_ok[k] = false;
|
||||
} else {
|
||||
_gyro_cal_ok[k] = true;
|
||||
_gyro_offset[k].set(new_gyro_offset[k]);
|
||||
_gyro_offset(k).set(new_gyro_offset[k]);
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_gyro[k].set(0.5 * (get_temperature(k) + start_temperature[k]));
|
||||
caltemp_gyro(k).set(0.5 * (get_temperature(k) + start_temperature[k]));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
@ -1642,17 +1655,17 @@ AP_InertialSensor::_init_gyro()
|
||||
void AP_InertialSensor::_save_gyro_calibration()
|
||||
{
|
||||
for (uint8_t i=0; i<_gyro_count; i++) {
|
||||
_gyro_offset[i].save();
|
||||
_gyro_id[i].save();
|
||||
_gyro_offset(i).save();
|
||||
_gyro_id(i).save();
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_gyro[i].save();
|
||||
caltemp_gyro(i).save();
|
||||
#endif
|
||||
}
|
||||
for (uint8_t i=_gyro_count; i<INS_MAX_INSTANCES; i++) {
|
||||
_gyro_offset[i].set_and_save(Vector3f());
|
||||
_gyro_id[i].set_and_save(0);
|
||||
_gyro_offset(i).set_and_save(Vector3f());
|
||||
_gyro_id(i).set_and_save(0);
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_gyro[i].set_and_save_ifchanged(-300);
|
||||
caltemp_gyro(i).set_and_save_ifchanged(-300);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
@ -1754,13 +1767,13 @@ void AP_InertialSensor::update(void)
|
||||
|
||||
// set primary to first healthy accel and gyro
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
if (_gyro_healthy[i] && _use[i]) {
|
||||
if (_gyro_healthy[i] && _use(i)) {
|
||||
_primary_gyro = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
if (_accel_healthy[i] && _use[i]) {
|
||||
if (_accel_healthy[i] && _use(i)) {
|
||||
_primary_accel = i;
|
||||
break;
|
||||
}
|
||||
@ -1858,7 +1871,7 @@ check_sample:
|
||||
if (_new_gyro_data[i]) {
|
||||
const uint8_t imask = (1U<<i);
|
||||
gyro_available_mask |= imask;
|
||||
if (_use[i]) {
|
||||
if (_use(i)) {
|
||||
_gyro_wait_mask |= imask;
|
||||
} else {
|
||||
_gyro_wait_mask &= ~imask;
|
||||
@ -1869,7 +1882,7 @@ check_sample:
|
||||
if (_new_accel_data[i]) {
|
||||
const uint8_t imask = (1U<<i);
|
||||
accel_available_mask |= imask;
|
||||
if (_use[i]) {
|
||||
if (_use(i)) {
|
||||
_accel_wait_mask |= imask;
|
||||
} else {
|
||||
_accel_wait_mask &= ~imask;
|
||||
@ -2074,7 +2087,7 @@ bool AP_InertialSensor::temperature_cal_running() const
|
||||
{
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
if (tcal[i].enable == TCal::Enable::LearnCalibration) {
|
||||
if (tcal(i).enable == AP_InertialSensor_TCal::Enable::LearnCalibration) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@ -2164,29 +2177,29 @@ void AP_InertialSensor::_acal_save_calibrations()
|
||||
for (uint8_t i=0; i<_accel_count; i++) {
|
||||
if (_accel_calibrator[i].get_status() == ACCEL_CAL_SUCCESS) {
|
||||
_accel_calibrator[i].get_calibration(bias, gain);
|
||||
_accel_offset[i].set_and_save(bias);
|
||||
_accel_scale[i].set_and_save(gain);
|
||||
_accel_id[i].save();
|
||||
_accel_offset(i).set_and_save(bias);
|
||||
_accel_scale(i).set_and_save(gain);
|
||||
_accel_id(i).save();
|
||||
_accel_id_ok[i] = true;
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[i].set_and_save(get_temperature(i));
|
||||
caltemp_accel(i).set_and_save(get_temperature(i));
|
||||
#endif
|
||||
} else {
|
||||
_accel_offset[i].set_and_save(Vector3f());
|
||||
_accel_scale[i].set_and_save(Vector3f());
|
||||
_accel_offset(i).set_and_save(Vector3f());
|
||||
_accel_scale(i).set_and_save(Vector3f());
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[i].set_and_save(-300);
|
||||
caltemp_accel(i).set_and_save(-300);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
// clear any unused accels
|
||||
for (uint8_t i=_accel_count; i<INS_MAX_INSTANCES; i++) {
|
||||
_accel_id[i].set_and_save(0);
|
||||
_accel_offset[i].set_and_save(Vector3f());
|
||||
_accel_scale[i].set_and_save(Vector3f());
|
||||
_accel_id(i).set_and_save(0);
|
||||
_accel_offset(i).set_and_save(Vector3f());
|
||||
_accel_scale(i).set_and_save(Vector3f());
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[i].set_and_save_ifchanged(-300);
|
||||
caltemp_accel(i).set_and_save_ifchanged(-300);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -2236,8 +2249,8 @@ void AP_InertialSensor::_acal_save_calibrations()
|
||||
void AP_InertialSensor::_acal_event_failure()
|
||||
{
|
||||
for (uint8_t i=0; i<_accel_count; i++) {
|
||||
_accel_offset[i].set_and_notify(Vector3f(0,0,0));
|
||||
_accel_scale[i].set_and_notify(Vector3f(1,1,1));
|
||||
_accel_offset(i).set_and_notify(Vector3f(0,0,0));
|
||||
_accel_scale(i).set_and_notify(Vector3f(1,1,1));
|
||||
}
|
||||
}
|
||||
|
||||
@ -2331,14 +2344,14 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
|
||||
|
||||
// save existing accel offsets
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
saved_offsets[k] = _accel_offset[k];
|
||||
saved_scaling[k] = _accel_scale[k];
|
||||
saved_offsets[k] = _accel_offset(k);
|
||||
saved_scaling[k] = _accel_scale(k);
|
||||
}
|
||||
|
||||
// remove existing accel offsets and scaling
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
_accel_offset[k].set(Vector3f());
|
||||
_accel_scale[k].set(Vector3f(1,1,1));
|
||||
_accel_offset(k).set(Vector3f());
|
||||
_accel_scale(k).set(Vector3f(1,1,1));
|
||||
new_accel_offset[k].zero();
|
||||
last_average[k].zero();
|
||||
converged[k] = false;
|
||||
@ -2416,12 +2429,12 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
// remove rotated gravity
|
||||
new_accel_offset[k] -= rotated_gravity;
|
||||
_accel_offset[k].set_and_save(new_accel_offset[k]);
|
||||
_accel_scale[k].save();
|
||||
_accel_id[k].save();
|
||||
_accel_offset(k).set_and_save(new_accel_offset[k]);
|
||||
_accel_scale(k).save();
|
||||
_accel_id(k).save();
|
||||
_accel_id_ok[k] = true;
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[k].set_and_save(get_temperature(k));
|
||||
caltemp_accel(k).set_and_save(get_temperature(k));
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -2431,8 +2444,8 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
|
||||
DEV_PRINTF("\nFAILED\n");
|
||||
// restore old values
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
_accel_offset[k].set(saved_offsets[k]);
|
||||
_accel_scale[k].set(saved_scaling[k]);
|
||||
_accel_offset(k).set(saved_offsets[k]);
|
||||
_accel_scale(k).set(saved_scaling[k]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -2507,11 +2520,11 @@ void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_
|
||||
void AP_InertialSensor::force_save_calibration(void)
|
||||
{
|
||||
for (uint8_t i=0; i<_accel_count; i++) {
|
||||
if (_accel_id[i] != 0) {
|
||||
_accel_id[i].save();
|
||||
if (_accel_id(i) != 0) {
|
||||
_accel_id(i).save();
|
||||
// we also save the scale as the default of 1.0 may be
|
||||
// over a stored value of 0.0
|
||||
_accel_scale[i].save();
|
||||
_accel_scale(i).save();
|
||||
_accel_id_ok[i] = true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -11,51 +11,16 @@
|
||||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
/**
|
||||
maximum number of INS instances available on this platform. If more
|
||||
than 1 then redundant sensors may be available
|
||||
*/
|
||||
#ifndef INS_MAX_INSTANCES
|
||||
#define INS_MAX_INSTANCES 3
|
||||
#endif
|
||||
#define INS_MAX_BACKENDS 2*INS_MAX_INSTANCES
|
||||
#define INS_MAX_NOTCHES 12
|
||||
#ifndef INS_VIBRATION_CHECK_INSTANCES
|
||||
#if HAL_MEM_CLASS >= HAL_MEM_CLASS_300
|
||||
#define INS_VIBRATION_CHECK_INSTANCES INS_MAX_INSTANCES
|
||||
#else
|
||||
#define INS_VIBRATION_CHECK_INSTANCES 1
|
||||
#endif
|
||||
#endif
|
||||
#define XYZ_AXIS_COUNT 3
|
||||
// The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400
|
||||
#define INS_MAX_GYRO_WINDOW_SAMPLES 8
|
||||
|
||||
#define DEFAULT_IMU_LOG_BAT_MASK 0
|
||||
|
||||
#ifndef HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
#define HAL_INS_TEMPERATURE_CAL_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
||||
#ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
|
||||
#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2
|
||||
#endif
|
||||
|
||||
// time for the estimated gyro rates to converge
|
||||
#ifndef HAL_INS_CONVERGANCE_MS
|
||||
#define HAL_INS_CONVERGANCE_MS 30000
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <AP_AccelCal/AP_AccelCal.h>
|
||||
#include <AP_HAL/utility/RingBuffer.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
#include <Filter/LowPassFilter.h>
|
||||
#include <Filter/HarmonicNotchFilter.h>
|
||||
#include <AP_Math/polyfit.h>
|
||||
#include "AP_InertialSensor_Params.h"
|
||||
#include "AP_InertialSensor_tempcal.h"
|
||||
|
||||
#ifndef AP_SIM_INS_ENABLED
|
||||
#define AP_SIM_INS_ENABLED AP_SIM_ENABLED
|
||||
@ -143,7 +108,7 @@ public:
|
||||
const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); }
|
||||
|
||||
// set gyro offsets in radians/sec
|
||||
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
|
||||
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset(i); }
|
||||
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
|
||||
|
||||
//get delta angle if available
|
||||
@ -197,19 +162,19 @@ public:
|
||||
#endif
|
||||
bool set_gyro_window_size(uint16_t size);
|
||||
// get accel offsets in m/s/s
|
||||
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
|
||||
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset(i); }
|
||||
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
|
||||
|
||||
// get accel scale
|
||||
const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
|
||||
const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale(i); }
|
||||
const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
|
||||
|
||||
// return a 3D vector defining the position offset of the IMU accelerometer in metres relative to the body frame origin
|
||||
const Vector3f &get_imu_pos_offset(uint8_t instance) const {
|
||||
return _accel_pos[instance];
|
||||
return _accel_pos(instance);
|
||||
}
|
||||
const Vector3f &get_imu_pos_offset(void) const {
|
||||
return _accel_pos[_primary_accel];
|
||||
return _accel_pos(_primary_accel);
|
||||
}
|
||||
|
||||
// return the temperature if supported. Zero is returned if no
|
||||
@ -233,6 +198,9 @@ public:
|
||||
|
||||
// class level parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
#if INS_AUX_INSTANCES
|
||||
AP_InertialSensor_Params params[INS_AUX_INSTANCES];
|
||||
#endif
|
||||
|
||||
// set overall board orientation
|
||||
void set_board_orientation(enum Rotation orientation) {
|
||||
@ -357,6 +325,7 @@ public:
|
||||
|
||||
// class level parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
||||
// Parameters
|
||||
AP_Int16 _required_count;
|
||||
@ -567,16 +536,47 @@ private:
|
||||
|
||||
// IDs to uniquely identify each sensor: shall remain
|
||||
// the same across reboots
|
||||
AP_Int32 _accel_id[INS_MAX_INSTANCES];
|
||||
AP_Int32 _gyro_id[INS_MAX_INSTANCES];
|
||||
AP_Int32 _accel_id_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
|
||||
AP_Int32 _gyro_id_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
|
||||
|
||||
// accelerometer scaling and offsets
|
||||
AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _accel_scale_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
|
||||
AP_Vector3f _accel_offset_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
|
||||
AP_Vector3f _gyro_offset_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
|
||||
|
||||
// accelerometer position offset in body frame
|
||||
AP_Vector3f _accel_pos[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _accel_pos_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
|
||||
|
||||
// Use Accessor methods to access above variables
|
||||
#if INS_AUX_INSTANCES
|
||||
#define INS_PARAM_WRAPPER(var) \
|
||||
inline decltype(var##_old_param[0])& var(uint8_t i) { \
|
||||
if (i<(INS_MAX_INSTANCES-INS_AUX_INSTANCES)) { \
|
||||
return var##_old_param[i]; \
|
||||
} else { \
|
||||
return params[i-(INS_MAX_INSTANCES-INS_AUX_INSTANCES)].var; \
|
||||
} \
|
||||
} \
|
||||
inline decltype(var##_old_param[0])& var(uint8_t i) const { \
|
||||
return const_cast<AP_InertialSensor*>(this)->var(i); \
|
||||
}
|
||||
#else
|
||||
#define INS_PARAM_WRAPPER(var) \
|
||||
inline decltype(var##_old_param[0])& var(uint8_t i) { \
|
||||
return var##_old_param[i]; \
|
||||
} \
|
||||
inline decltype(var##_old_param[0])& var(uint8_t i) const { \
|
||||
return const_cast<AP_InertialSensor*>(this)->var(i); \
|
||||
}
|
||||
#endif
|
||||
|
||||
// Accessor methods for old parameters
|
||||
INS_PARAM_WRAPPER(_accel_id);
|
||||
INS_PARAM_WRAPPER(_gyro_id);
|
||||
INS_PARAM_WRAPPER(_accel_scale);
|
||||
INS_PARAM_WRAPPER(_accel_offset);
|
||||
INS_PARAM_WRAPPER(_gyro_offset);
|
||||
INS_PARAM_WRAPPER(_accel_pos);
|
||||
|
||||
// accelerometer max absolute offsets to be used for calibration
|
||||
float _accel_max_abs_offsets[INS_MAX_INSTANCES];
|
||||
@ -609,7 +609,8 @@ private:
|
||||
AP_Int8 _gyro_cal_timing;
|
||||
|
||||
// use for attitude, velocity, position estimates
|
||||
AP_Int8 _use[INS_MAX_INSTANCES];
|
||||
AP_Int8 _use_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
|
||||
INS_PARAM_WRAPPER(_use);
|
||||
|
||||
// control enable of fast sampling
|
||||
AP_Int8 _fast_sampling_mask;
|
||||
@ -719,87 +720,28 @@ private:
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
public:
|
||||
// TCal class is public for use by SITL
|
||||
class TCal {
|
||||
public:
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
void correct_accel(float temperature, float cal_temp, Vector3f &accel) const;
|
||||
void correct_gyro(float temperature, float cal_temp, Vector3f &accel) const;
|
||||
void sitl_apply_accel(float temperature, Vector3f &accel) const;
|
||||
void sitl_apply_gyro(float temperature, Vector3f &accel) const;
|
||||
|
||||
void update_accel_learning(const Vector3f &accel);
|
||||
void update_gyro_learning(const Vector3f &accel);
|
||||
|
||||
enum class Enable : uint8_t {
|
||||
Disabled = 0,
|
||||
Enabled = 1,
|
||||
LearnCalibration = 2,
|
||||
};
|
||||
|
||||
// add samples for learning
|
||||
void update_accel_learning(const Vector3f &gyro, float temperature);
|
||||
void update_gyro_learning(const Vector3f &accel, float temperature);
|
||||
|
||||
// class for online learning of calibration
|
||||
class Learn {
|
||||
public:
|
||||
Learn(TCal &_tcal, float _start_temp);
|
||||
|
||||
// state for accel/gyro (accel first)
|
||||
struct LearnState {
|
||||
float last_temp;
|
||||
uint32_t last_sample_ms;
|
||||
Vector3f sum;
|
||||
uint32_t sum_count;
|
||||
LowPassFilter2p<float> temp_filter;
|
||||
// double precision is needed for good results when we
|
||||
// span a wide range of temperatures
|
||||
PolyFit<4, double, Vector3f> pfit;
|
||||
} state[2];
|
||||
|
||||
void add_sample(const Vector3f &sample, float temperature, LearnState &state);
|
||||
void finish_calibration(float temperature);
|
||||
bool save_calibration(float temperature);
|
||||
void reset(float temperature);
|
||||
float start_temp;
|
||||
float start_tmax;
|
||||
uint32_t last_save_ms;
|
||||
|
||||
TCal &tcal;
|
||||
uint8_t instance(void) const {
|
||||
return tcal.instance();
|
||||
}
|
||||
Vector3f accel_start;
|
||||
};
|
||||
|
||||
AP_Enum<Enable> enable;
|
||||
|
||||
// get persistent params for this instance
|
||||
void get_persistent_params(ExpandingString &str) const;
|
||||
|
||||
private:
|
||||
AP_Float temp_max;
|
||||
AP_Float temp_min;
|
||||
AP_Vector3f accel_coeff[3];
|
||||
AP_Vector3f gyro_coeff[3];
|
||||
Vector3f accel_tref;
|
||||
Vector3f gyro_tref;
|
||||
Learn *learn;
|
||||
|
||||
void correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const;
|
||||
Vector3f polynomial_eval(float temperature, const AP_Vector3f coeff[3]) const;
|
||||
|
||||
// get instance number
|
||||
uint8_t instance(void) const;
|
||||
};
|
||||
|
||||
// instance number for logging
|
||||
uint8_t tcal_instance(const TCal &tc) const {
|
||||
return &tc - &tcal[0];
|
||||
#if INS_AUX_INSTANCES
|
||||
uint8_t tcal_instance(const AP_InertialSensor_TCal &tc) const {
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
if (&tc == &tcal_old_param[i]) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
for (uint8_t i=0; i<INS_AUX_INSTANCES; i++) {
|
||||
if (&tc == ¶ms[i].tcal) {
|
||||
return i + INS_MAX_INSTANCES;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
#else
|
||||
uint8_t tcal_instance(const AP_InertialSensor_TCal &tc) const {
|
||||
return &tc - &tcal(0);
|
||||
}
|
||||
#endif
|
||||
private:
|
||||
TCal tcal[INS_MAX_INSTANCES];
|
||||
AP_InertialSensor_TCal tcal_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
|
||||
|
||||
enum class TCalOptions : uint8_t {
|
||||
PERSIST_TEMP_CAL = (1U<<0),
|
||||
@ -807,8 +749,13 @@ private:
|
||||
};
|
||||
|
||||
// temperature that last calibration was run at
|
||||
AP_Float caltemp_accel[INS_MAX_INSTANCES];
|
||||
AP_Float caltemp_gyro[INS_MAX_INSTANCES];
|
||||
AP_Float caltemp_accel_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
|
||||
AP_Float caltemp_gyro_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
|
||||
|
||||
INS_PARAM_WRAPPER(caltemp_accel);
|
||||
INS_PARAM_WRAPPER(caltemp_gyro);
|
||||
INS_PARAM_WRAPPER(tcal);
|
||||
|
||||
AP_Int32 tcal_options;
|
||||
bool tcal_learning;
|
||||
#endif
|
||||
|
||||
@ -103,7 +103,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
if (_imu.tcal_learning) {
|
||||
_imu.tcal[instance].update_accel_learning(accel, _imu.get_temperature(instance));
|
||||
_imu.tcal(instance).update_accel_learning(accel, _imu.get_temperature(instance));
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -115,15 +115,15 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// apply temperature corrections
|
||||
_imu.tcal[instance].correct_accel(_imu.get_temperature(instance), _imu.caltemp_accel[instance], accel);
|
||||
_imu.tcal(instance).correct_accel(_imu.get_temperature(instance), _imu.caltemp_accel(instance), accel);
|
||||
#endif
|
||||
|
||||
// apply offsets
|
||||
accel -= _imu._accel_offset[instance];
|
||||
accel -= _imu._accel_offset(instance);
|
||||
|
||||
|
||||
// apply scaling
|
||||
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
|
||||
const Vector3f &accel_scale = _imu._accel_scale(instance).get();
|
||||
accel.x *= accel_scale.x;
|
||||
accel.y *= accel_scale.y;
|
||||
accel.z *= accel_scale.z;
|
||||
@ -140,7 +140,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
if (_imu.tcal_learning) {
|
||||
_imu.tcal[instance].update_gyro_learning(gyro, _imu.get_temperature(instance));
|
||||
_imu.tcal(instance).update_gyro_learning(gyro, _imu.get_temperature(instance));
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -148,11 +148,11 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// apply temperature corrections
|
||||
_imu.tcal[instance].correct_gyro(_imu.get_temperature(instance), _imu.caltemp_gyro[instance], gyro);
|
||||
_imu.tcal(instance).correct_gyro(_imu.get_temperature(instance), _imu.caltemp_gyro(instance), gyro);
|
||||
#endif
|
||||
|
||||
// gyro calibration is always assumed to have been done in sensor frame
|
||||
gyro -= _imu._gyro_offset[instance];
|
||||
gyro -= _imu._gyro_offset(instance);
|
||||
}
|
||||
|
||||
gyro.rotate(_imu._board_orientation);
|
||||
|
||||
152
libraries/AP_InertialSensor/AP_InertialSensor_Params.cpp
Normal file
152
libraries/AP_InertialSensor/AP_InertialSensor_Params.cpp
Normal file
@ -0,0 +1,152 @@
|
||||
#include "AP_InertialSensor_Params.h"
|
||||
|
||||
const AP_Param::GroupInfo AP_InertialSensor_Params::var_info[] = {
|
||||
|
||||
// @Param: USE
|
||||
// @DisplayName: Use first IMU for attitude, velocity and position estimates
|
||||
// @Description: Use first IMU for attitude, velocity and position estimates
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("USE", 1, AP_InertialSensor_Params, _use, 1),
|
||||
|
||||
// @Param: ACC_ID
|
||||
// @DisplayName: Accelerometer ID
|
||||
// @Description: Accelerometer sensor ID, taking into account its type, bus and instance
|
||||
// @ReadOnly: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ACC_ID", 2, AP_InertialSensor_Params, _accel_id, 0),
|
||||
|
||||
|
||||
// @Param: ACCSCAL_X
|
||||
// @DisplayName: Accelerometer scaling of X axis
|
||||
// @Description: Accelerometer scaling of X axis. Calculated during acceleration calibration routine
|
||||
// @Range: 0.8 1.2
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACCSCAL_Y
|
||||
// @DisplayName: Accelerometer scaling of Y axis
|
||||
// @Description: Accelerometer scaling of Y axis Calculated during acceleration calibration routine
|
||||
// @Range: 0.8 1.2
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACCSCAL_Z
|
||||
// @DisplayName: Accelerometer scaling of Z axis
|
||||
// @Description: Accelerometer scaling of Z axis Calculated during acceleration calibration routine
|
||||
// @Range: 0.8 1.2
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACCSCAL", 3, AP_InertialSensor_Params, _accel_scale, 1.0),
|
||||
|
||||
|
||||
// @Param: ACCOFFS_X
|
||||
// @DisplayName: Accelerometer offsets of X axis
|
||||
// @Description: Accelerometer offsets of X axis. This is setup using the acceleration calibration or level operations
|
||||
// @Units: m/s/s
|
||||
// @Range: -3.5 3.5
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACCOFFS_Y
|
||||
// @DisplayName: Accelerometer offsets of Y axis
|
||||
// @Description: Accelerometer offsets of Y axis. This is setup using the acceleration calibration or level operations
|
||||
// @Units: m/s/s
|
||||
// @Range: -3.5 3.5
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACCOFFS_Z
|
||||
// @DisplayName: Accelerometer offsets of Z axis
|
||||
// @Description: Accelerometer offsets of Z axis. This is setup using the acceleration calibration or level operations
|
||||
// @Units: m/s/s
|
||||
// @Range: -3.5 3.5
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACCOFFS", 4, AP_InertialSensor_Params, _accel_offset, 0),
|
||||
|
||||
// @Param: POS_X
|
||||
// @DisplayName: IMU accelerometer X position
|
||||
// @Description: X position of the first IMU Accelerometer in body frame. Positive X is forward of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
|
||||
// @Units: m
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: POS_Y
|
||||
// @DisplayName: IMU accelerometer Y position
|
||||
// @Description: Y position of the first IMU accelerometer in body frame. Positive Y is to the right of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
|
||||
// @Units: m
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: POS_Z
|
||||
// @DisplayName: IMU accelerometer Z position
|
||||
// @Description: Z position of the first IMU accelerometer in body frame. Positive Z is down from the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
|
||||
// @Units: m
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("POS", 5, AP_InertialSensor_Params, _accel_pos, 0.0f),
|
||||
|
||||
// @Param: ACC_CALTEMP
|
||||
// @DisplayName: Calibration temperature for accelerometer
|
||||
// @Description: Temperature that the accelerometer was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACC_CALTEMP", 6, AP_InertialSensor_Params, caltemp_accel, -300),
|
||||
|
||||
// @Param: GYR_ID
|
||||
// @DisplayName: Gyro ID
|
||||
// @Description: Gyro sensor ID, taking into account its type, bus and instance
|
||||
// @ReadOnly: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GYR_ID", 7, AP_InertialSensor_Params, _gyro_id, 0),
|
||||
|
||||
|
||||
// @Param: GYROFFS_X
|
||||
// @DisplayName: Gyro offsets of X axis
|
||||
// @Description: Gyro sensor offsets of X axis. This is setup on each boot during gyro calibrations
|
||||
// @Units: rad/s
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYROFFS_Y
|
||||
// @DisplayName: Gyro offsets of Y axis
|
||||
// @Description: Gyro sensor offsets of Y axis. This is setup on each boot during gyro calibrations
|
||||
// @Units: rad/s
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYROFFS_Z
|
||||
// @DisplayName: Gyro offsets of Z axis
|
||||
// @Description: Gyro sensor offsets of Z axis. This is setup on each boot during gyro calibrations
|
||||
// @Units: rad/s
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYROFFS", 8, AP_InertialSensor_Params, _gyro_offset, 0),
|
||||
|
||||
|
||||
// @Param: GYR_CALTEMP
|
||||
// @DisplayName: Calibration temperature for gyroscope
|
||||
// @Description: Temperature that the gyroscope was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYR_CALTEMP", 9, AP_InertialSensor_Params, caltemp_gyro, -300),
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// @Group: TCAL_
|
||||
// @Path: AP_InertialSensor_tempcal.cpp
|
||||
AP_SUBGROUPINFO(tcal, "TCAL_", 10, AP_InertialSensor_Params, AP_InertialSensor_TCal),
|
||||
#endif
|
||||
AP_GROUPEND
|
||||
|
||||
};
|
||||
|
||||
|
||||
AP_InertialSensor_Params::AP_InertialSensor_Params(void) {
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
32
libraries/AP_InertialSensor/AP_InertialSensor_Params.h
Normal file
32
libraries/AP_InertialSensor/AP_InertialSensor_Params.h
Normal file
@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include "AP_InertialSensor_tempcal.h"
|
||||
#include "AP_InertialSensor_config.h"
|
||||
|
||||
class AP_InertialSensor_Params {
|
||||
public:
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
AP_InertialSensor_Params(void);
|
||||
|
||||
/* Do not allow copies */
|
||||
CLASS_NO_COPY(AP_InertialSensor_Params);
|
||||
|
||||
AP_Int32 _accel_id;
|
||||
AP_Vector3f _accel_scale;
|
||||
AP_Vector3f _accel_offset;
|
||||
AP_Vector3f _accel_pos;
|
||||
AP_Float caltemp_accel;
|
||||
|
||||
AP_Int32 _gyro_id;
|
||||
AP_Vector3f _gyro_offset;
|
||||
AP_Float caltemp_gyro;
|
||||
|
||||
AP_Int8 _use;
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
AP_InertialSensor_TCal tcal;
|
||||
#endif
|
||||
};
|
||||
@ -3,6 +3,55 @@
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
#include <AP_Logger/AP_Logger_config.h>
|
||||
|
||||
/**
|
||||
maximum number of INS instances available on this platform. If more
|
||||
than 1 then redundant sensors may be available
|
||||
*/
|
||||
|
||||
#ifndef INS_AUX_INSTANCES
|
||||
#define INS_AUX_INSTANCES 0
|
||||
#endif
|
||||
|
||||
#ifndef INS_MAX_INSTANCES
|
||||
#define INS_MAX_INSTANCES (3+INS_AUX_INSTANCES)
|
||||
#endif
|
||||
|
||||
#if INS_MAX_INSTANCES < 3 && INS_AUX_INSTANCES > 0
|
||||
#error "INS_AUX_INSTANCES must be zero if INS_MAX_INSTANCES is less than 3"
|
||||
#endif
|
||||
|
||||
#if INS_MAX_INSTANCES > 3 && INS_AUX_INSTANCES == 0
|
||||
#error "INS_AUX_INSTANCES must be non-zero if INS_MAX_INSTANCES is greater than 3"
|
||||
#endif
|
||||
|
||||
#define INS_MAX_BACKENDS 2*INS_MAX_INSTANCES
|
||||
#define INS_MAX_NOTCHES 12
|
||||
#ifndef INS_VIBRATION_CHECK_INSTANCES
|
||||
#if HAL_MEM_CLASS >= HAL_MEM_CLASS_300
|
||||
#define INS_VIBRATION_CHECK_INSTANCES INS_MAX_INSTANCES
|
||||
#else
|
||||
#define INS_VIBRATION_CHECK_INSTANCES 1
|
||||
#endif
|
||||
#endif
|
||||
#define XYZ_AXIS_COUNT 3
|
||||
// The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400
|
||||
#define INS_MAX_GYRO_WINDOW_SAMPLES 8
|
||||
|
||||
#define DEFAULT_IMU_LOG_BAT_MASK 0
|
||||
|
||||
#ifndef HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
#define HAL_INS_TEMPERATURE_CAL_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
||||
#ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
|
||||
#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2
|
||||
#endif
|
||||
|
||||
// time for the estimated gyro rates to converge
|
||||
#ifndef HAL_INS_CONVERGANCE_MS
|
||||
#define HAL_INS_CONVERGANCE_MS 30000
|
||||
#endif
|
||||
|
||||
#ifndef AP_INERTIALSENSOR_ENABLED
|
||||
#define AP_INERTIALSENSOR_ENABLED 1
|
||||
#endif
|
||||
|
||||
@ -18,7 +18,8 @@
|
||||
|
||||
#define AP_INLINE_VECTOR_OPS
|
||||
|
||||
#include "AP_InertialSensor.h"
|
||||
#include "AP_InertialSensor_tempcal.h"
|
||||
#include "AP_InertialSensor_config.h"
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
@ -44,7 +45,7 @@
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// temperature calibration parameters, per IMU
|
||||
const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
const AP_Param::GroupInfo AP_InertialSensor_TCal::var_info[] = {
|
||||
|
||||
// @Param: ENABLE
|
||||
// @DisplayName: Enable temperature calibration
|
||||
@ -52,7 +53,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @Values: 0:Disabled,1:Enabled,2:LearnCalibration
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_InertialSensor::TCal, enable, float(Enable::Disabled), AP_PARAM_FLAG_ENABLE),
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_InertialSensor_TCal, enable, float(Enable::Disabled), AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: TMIN
|
||||
// @DisplayName: Temperature calibration min
|
||||
@ -61,7 +62,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @Units: degC
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("TMIN", 2, AP_InertialSensor::TCal, temp_min, 0),
|
||||
AP_GROUPINFO("TMIN", 2, AP_InertialSensor_TCal, temp_min, 0),
|
||||
|
||||
// @Param: TMAX
|
||||
// @DisplayName: Temperature calibration max
|
||||
@ -70,7 +71,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @Units: degC
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("TMAX", 3, AP_InertialSensor::TCal, temp_max, 70),
|
||||
AP_GROUPINFO("TMAX", 3, AP_InertialSensor_TCal, temp_max, 70),
|
||||
|
||||
// @Param: ACC1_X
|
||||
// @DisplayName: Accelerometer 1st order temperature coefficient X axis
|
||||
@ -90,7 +91,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("ACC1", 4, AP_InertialSensor::TCal, accel_coeff[0], 0),
|
||||
AP_GROUPINFO("ACC1", 4, AP_InertialSensor_TCal, accel_coeff[0], 0),
|
||||
|
||||
// @Param: ACC2_X
|
||||
// @DisplayName: Accelerometer 2nd order temperature coefficient X axis
|
||||
@ -110,7 +111,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("ACC2", 5, AP_InertialSensor::TCal, accel_coeff[1], 0),
|
||||
AP_GROUPINFO("ACC2", 5, AP_InertialSensor_TCal, accel_coeff[1], 0),
|
||||
|
||||
// @Param: ACC3_X
|
||||
// @DisplayName: Accelerometer 3rd order temperature coefficient X axis
|
||||
@ -130,7 +131,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("ACC3", 6, AP_InertialSensor::TCal, accel_coeff[2], 0),
|
||||
AP_GROUPINFO("ACC3", 6, AP_InertialSensor_TCal, accel_coeff[2], 0),
|
||||
|
||||
// @Param: GYR1_X
|
||||
// @DisplayName: Gyroscope 1st order temperature coefficient X axis
|
||||
@ -150,7 +151,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("GYR1", 7, AP_InertialSensor::TCal, gyro_coeff[0], 0),
|
||||
AP_GROUPINFO("GYR1", 7, AP_InertialSensor_TCal, gyro_coeff[0], 0),
|
||||
|
||||
// @Param: GYR2_X
|
||||
// @DisplayName: Gyroscope 2nd order temperature coefficient X axis
|
||||
@ -170,7 +171,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("GYR2", 8, AP_InertialSensor::TCal, gyro_coeff[1], 0),
|
||||
AP_GROUPINFO("GYR2", 8, AP_InertialSensor_TCal, gyro_coeff[1], 0),
|
||||
|
||||
// @Param: GYR3_X
|
||||
// @DisplayName: Gyroscope 3rd order temperature coefficient X axis
|
||||
@ -190,7 +191,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("GYR3", 9, AP_InertialSensor::TCal, gyro_coeff[2], 0),
|
||||
AP_GROUPINFO("GYR3", 9, AP_InertialSensor_TCal, gyro_coeff[2], 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
@ -199,7 +200,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
/*
|
||||
evaluate a 3rd order polynomial (without the constant term) given a set of coefficients
|
||||
*/
|
||||
Vector3f AP_InertialSensor::TCal::polynomial_eval(float tdiff, const AP_Vector3f coeff[3]) const
|
||||
Vector3f AP_InertialSensor_TCal::polynomial_eval(float tdiff, const AP_Vector3f coeff[3]) const
|
||||
{
|
||||
// evaluate order 3 polynomial
|
||||
const Vector3f *c = (Vector3f *)&coeff[0];
|
||||
@ -209,7 +210,7 @@ Vector3f AP_InertialSensor::TCal::polynomial_eval(float tdiff, const AP_Vector3f
|
||||
/*
|
||||
correct a single sensor for the current temperature
|
||||
*/
|
||||
void AP_InertialSensor::TCal::correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const
|
||||
void AP_InertialSensor_TCal::correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const
|
||||
{
|
||||
if (enable != Enable::Enabled) {
|
||||
return;
|
||||
@ -229,12 +230,12 @@ void AP_InertialSensor::TCal::correct_sensor(float temperature, float cal_temp,
|
||||
v += polynomial_eval(cal_temp - TEMP_REFERENCE, coeff);
|
||||
}
|
||||
|
||||
void AP_InertialSensor::TCal::correct_accel(float temperature, float cal_temp, Vector3f &accel) const
|
||||
void AP_InertialSensor_TCal::correct_accel(float temperature, float cal_temp, Vector3f &accel) const
|
||||
{
|
||||
correct_sensor(temperature, cal_temp, accel_coeff, accel);
|
||||
}
|
||||
|
||||
void AP_InertialSensor::TCal::correct_gyro(float temperature, float cal_temp, Vector3f &gyro) const
|
||||
void AP_InertialSensor_TCal::correct_gyro(float temperature, float cal_temp, Vector3f &gyro) const
|
||||
{
|
||||
correct_sensor(temperature, cal_temp, gyro_coeff, gyro);
|
||||
}
|
||||
@ -244,19 +245,19 @@ void AP_InertialSensor::TCal::correct_gyro(float temperature, float cal_temp, Ve
|
||||
reference. This makes the SITL data independent of TEMP_REFERENCE
|
||||
and prevents an abrupt change at the endpoints
|
||||
*/
|
||||
void AP_InertialSensor::TCal::sitl_apply_accel(float temperature, Vector3f &accel) const
|
||||
void AP_InertialSensor_TCal::sitl_apply_accel(float temperature, Vector3f &accel) const
|
||||
{
|
||||
const float tmid = 0.5*(temp_max+temp_min);
|
||||
accel += polynomial_eval(temperature - tmid, accel_coeff);
|
||||
}
|
||||
|
||||
void AP_InertialSensor::TCal::sitl_apply_gyro(float temperature, Vector3f &gyro) const
|
||||
void AP_InertialSensor_TCal::sitl_apply_gyro(float temperature, Vector3f &gyro) const
|
||||
{
|
||||
const float tmid = 0.5*(temp_max+temp_min);
|
||||
gyro += polynomial_eval(temperature - tmid, gyro_coeff);
|
||||
}
|
||||
|
||||
AP_InertialSensor::TCal::Learn::Learn(TCal &_tcal, float _start_temp) :
|
||||
AP_InertialSensor_TCal::Learn::Learn(AP_InertialSensor_TCal &_tcal, float _start_temp) :
|
||||
start_temp(_start_temp),
|
||||
tcal(_tcal)
|
||||
{
|
||||
@ -266,7 +267,7 @@ AP_InertialSensor::TCal::Learn::Learn(TCal &_tcal, float _start_temp) :
|
||||
/*
|
||||
update polyfit with new sample
|
||||
*/
|
||||
void AP_InertialSensor::TCal::Learn::add_sample(const Vector3f &sample, float temperature, struct LearnState &st)
|
||||
void AP_InertialSensor_TCal::Learn::add_sample(const Vector3f &sample, float temperature, struct LearnState &st)
|
||||
{
|
||||
temperature = st.temp_filter.apply(temperature);
|
||||
|
||||
@ -341,7 +342,7 @@ void AP_InertialSensor::TCal::Learn::add_sample(const Vector3f &sample, float te
|
||||
/*
|
||||
update accel temperature compensation learning
|
||||
*/
|
||||
void AP_InertialSensor::TCal::update_accel_learning(const Vector3f &accel, float temperature)
|
||||
void AP_InertialSensor_TCal::update_accel_learning(const Vector3f &accel, float temperature)
|
||||
{
|
||||
if (enable != Enable::LearnCalibration) {
|
||||
return;
|
||||
@ -364,7 +365,7 @@ void AP_InertialSensor::TCal::update_accel_learning(const Vector3f &accel, float
|
||||
/*
|
||||
update gyro temperature compensation learning
|
||||
*/
|
||||
void AP_InertialSensor::TCal::update_gyro_learning(const Vector3f &gyro, float temperature)
|
||||
void AP_InertialSensor_TCal::update_gyro_learning(const Vector3f &gyro, float temperature)
|
||||
{
|
||||
if (enable != Enable::LearnCalibration) {
|
||||
return;
|
||||
@ -377,7 +378,7 @@ void AP_InertialSensor::TCal::update_gyro_learning(const Vector3f &gyro, float t
|
||||
/*
|
||||
reset calibration
|
||||
*/
|
||||
void AP_InertialSensor::TCal::Learn::reset(float temperature)
|
||||
void AP_InertialSensor_TCal::Learn::reset(float temperature)
|
||||
{
|
||||
memset((void*)&state[0], 0, sizeof(state));
|
||||
start_tmax = tcal.temp_max;
|
||||
@ -392,24 +393,24 @@ void AP_InertialSensor::TCal::Learn::reset(float temperature)
|
||||
/*
|
||||
finish and save calibration
|
||||
*/
|
||||
void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
|
||||
void AP_InertialSensor_TCal::Learn::finish_calibration(float temperature)
|
||||
{
|
||||
if (!save_calibration(temperature)) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed fit", instance()+1);
|
||||
AP_Notify::events.temp_cal_failed = 1;
|
||||
tcal.enable.set_and_save_ifchanged(int8_t(TCal::Enable::Disabled));
|
||||
tcal.enable.set_and_save_ifchanged(int8_t(AP_InertialSensor_TCal::Enable::Disabled));
|
||||
return;
|
||||
}
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: completed calibration tmin=%.1f tmax=%.1f",
|
||||
instance()+1,
|
||||
tcal.temp_min.get(), tcal.temp_max.get());
|
||||
tcal.enable.set_and_save_ifchanged(int8_t(TCal::Enable::Enabled));
|
||||
tcal.enable.set_and_save_ifchanged(int8_t(AP_InertialSensor_TCal::Enable::Enabled));
|
||||
}
|
||||
|
||||
/*
|
||||
save calibration state
|
||||
*/
|
||||
bool AP_InertialSensor::TCal::Learn::save_calibration(float temperature)
|
||||
bool AP_InertialSensor_TCal::Learn::save_calibration(float temperature)
|
||||
{
|
||||
Vector3f coefficients[3];
|
||||
|
||||
@ -440,7 +441,7 @@ bool AP_InertialSensor::TCal::Learn::save_calibration(float temperature)
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t AP_InertialSensor::TCal::instance(void) const
|
||||
uint8_t AP_InertialSensor_TCal::instance(void) const
|
||||
{
|
||||
return AP::ins().tcal_instance(*this);
|
||||
}
|
||||
@ -448,12 +449,30 @@ uint8_t AP_InertialSensor::TCal::instance(void) const
|
||||
/*
|
||||
get a string representation of parameters for this calibration set
|
||||
*/
|
||||
void AP_InertialSensor::TCal::get_persistent_params(ExpandingString &str) const
|
||||
void AP_InertialSensor_TCal::get_persistent_params(ExpandingString &str) const
|
||||
{
|
||||
if (enable != TCal::Enable::Enabled) {
|
||||
if (enable != AP_InertialSensor_TCal::Enable::Enabled) {
|
||||
return;
|
||||
}
|
||||
const uint8_t imu = instance()+1;
|
||||
#if INS_AUX_INSTANCES
|
||||
if (imu > 3) {
|
||||
str.printf("INS%u_TCAL_ENABLE=1\n", imu);
|
||||
str.printf("INS%u_TCAL_TMIN=%.2f\n", imu, temp_min.get());
|
||||
str.printf("INS%u_TCAL_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%u_TCAL_ACC%u_X=%f\n", imu, k+1, acc.x);
|
||||
str.printf("INS%u_TCAL_ACC%u_Y=%f\n", imu, k+1, acc.y);
|
||||
str.printf("INS%u_TCAL_ACC%u_Z=%f\n", imu, k+1, acc.z);
|
||||
str.printf("INS%u_TCAL_GYR%u_X=%f\n", imu, k+1, gyr.x);
|
||||
str.printf("INS%u_TCAL_GYR%u_Y=%f\n", imu, k+1, gyr.y);
|
||||
str.printf("INS%u_TCAL_GYR%u_Z=%f\n", imu, k+1, gyr.z);
|
||||
}
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
str.printf("INS_TCAL%u_ENABLE=1\n", imu);
|
||||
str.printf("INS_TCAL%u_TMIN=%.2f\n", imu, temp_min.get());
|
||||
str.printf("INS_TCAL%u_TMAX=%.2f\n", imu, temp_max.get());
|
||||
@ -478,28 +497,47 @@ void AP_InertialSensor::get_persistent_params(ExpandingString &str) const
|
||||
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++) {
|
||||
for (uint8_t i=0; i<(INS_MAX_INSTANCES-INS_AUX_INSTANCES); i++) {
|
||||
const uint8_t imu = i+1;
|
||||
const Vector3f &aoff = _accel_offset[i].get();
|
||||
const Vector3f &ascl = _accel_scale[i].get();
|
||||
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%s_ID=%u\n", id, unsigned(_accel_id[i].get()));
|
||||
str.printf("INS_ACC%s_ID=%u\n", id, unsigned(_accel_id(i).get()));
|
||||
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%u_CALTEMP=%.2f\n", imu, caltemp_accel[i].get());
|
||||
str.printf("INS_ACC%u_CALTEMP=%.2f\n", imu, caltemp_accel(i).get());
|
||||
}
|
||||
#if INS_AUX_INSTANCES
|
||||
for (uint8_t i=0; i<INS_AUX_INSTANCES; i++) {
|
||||
const uint8_t imu = i+(INS_MAX_INSTANCES-INS_AUX_INSTANCES);
|
||||
const Vector3f &aoff = params[i]._accel_offset.get();
|
||||
const Vector3f &ascl = params[i]._accel_scale.get();
|
||||
str.printf("INS%u_ACC_ID=%u\n", imu, unsigned(params[i]._accel_id.get()));
|
||||
str.printf("INS%u_ACCOFFS_X=%f\n", imu, aoff.x);
|
||||
str.printf("INS%u_ACCOFFS_Y=%f\n", imu, aoff.y);
|
||||
str.printf("INS%u_ACCOFFS_Z=%f\n", imu, aoff.z);
|
||||
str.printf("INS%u_ACCSCAL_X=%f\n", imu, ascl.x);
|
||||
str.printf("INS%u_ACCSCAL_Y=%f\n", imu, ascl.y);
|
||||
str.printf("INS%u_ACC_CALTEMP=%.2f\n", imu, params[i].caltemp_accel.get());
|
||||
}
|
||||
#endif
|
||||
}
|
||||
if (uint32_t(tcal_options.get()) & uint32_t(TCalOptions::PERSIST_TEMP_CAL)) {
|
||||
for (auto &tc : tcal) {
|
||||
for (auto &tc : tcal_old_param) {
|
||||
tc.get_persistent_params(str);
|
||||
}
|
||||
#if INS_AUX_INSTANCES
|
||||
for (uint8_t i=0; i<INS_AUX_INSTANCES; i++) {
|
||||
params[i].tcal.get_persistent_params(str);
|
||||
}
|
||||
#endif
|
||||
save_options = true;
|
||||
}
|
||||
if (save_options) {
|
||||
|
||||
81
libraries/AP_InertialSensor/AP_InertialSensor_tempcal.h
Normal file
81
libraries/AP_InertialSensor/AP_InertialSensor_tempcal.h
Normal file
@ -0,0 +1,81 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Math/polyfit.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
|
||||
// AP_InertialSensor_TCal class is public for use by SITL
|
||||
class AP_InertialSensor_TCal {
|
||||
public:
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
void correct_accel(float temperature, float cal_temp, Vector3f &accel) const;
|
||||
void correct_gyro(float temperature, float cal_temp, Vector3f &accel) const;
|
||||
void sitl_apply_accel(float temperature, Vector3f &accel) const;
|
||||
void sitl_apply_gyro(float temperature, Vector3f &accel) const;
|
||||
|
||||
void update_accel_learning(const Vector3f &accel);
|
||||
void update_gyro_learning(const Vector3f &accel);
|
||||
|
||||
enum class Enable : uint8_t {
|
||||
Disabled = 0,
|
||||
Enabled = 1,
|
||||
LearnCalibration = 2,
|
||||
};
|
||||
|
||||
// add samples for learning
|
||||
void update_accel_learning(const Vector3f &gyro, float temperature);
|
||||
void update_gyro_learning(const Vector3f &accel, float temperature);
|
||||
|
||||
// class for online learning of calibration
|
||||
class Learn {
|
||||
public:
|
||||
Learn(AP_InertialSensor_TCal &_tcal, float _start_temp);
|
||||
|
||||
// state for accel/gyro (accel first)
|
||||
struct LearnState {
|
||||
float last_temp;
|
||||
uint32_t last_sample_ms;
|
||||
Vector3f sum;
|
||||
uint32_t sum_count;
|
||||
LowPassFilter2p<float> temp_filter;
|
||||
// double precision is needed for good results when we
|
||||
// span a wide range of temperatures
|
||||
PolyFit<4, double, Vector3f> pfit;
|
||||
} state[2];
|
||||
|
||||
void add_sample(const Vector3f &sample, float temperature, LearnState &state);
|
||||
void finish_calibration(float temperature);
|
||||
bool save_calibration(float temperature);
|
||||
void reset(float temperature);
|
||||
float start_temp;
|
||||
float start_tmax;
|
||||
uint32_t last_save_ms;
|
||||
|
||||
AP_InertialSensor_TCal &tcal;
|
||||
uint8_t instance(void) const {
|
||||
return tcal.instance();
|
||||
}
|
||||
Vector3f accel_start;
|
||||
};
|
||||
|
||||
AP_Enum<Enable> enable;
|
||||
|
||||
// get persistent params for this instance
|
||||
void get_persistent_params(ExpandingString &str) const;
|
||||
|
||||
private:
|
||||
AP_Float temp_max;
|
||||
AP_Float temp_min;
|
||||
AP_Vector3f accel_coeff[3];
|
||||
AP_Vector3f gyro_coeff[3];
|
||||
Vector3f accel_tref;
|
||||
Vector3f gyro_tref;
|
||||
Learn *learn;
|
||||
|
||||
void correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const;
|
||||
Vector3f polynomial_eval(float temperature, const AP_Vector3f coeff[3]) const;
|
||||
|
||||
// get instance number
|
||||
uint8_t instance(void) const;
|
||||
};
|
||||
Loading…
Reference in New Issue
Block a user