diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index 91b4739f96..4a0443875e 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -144,15 +144,31 @@ void Accelerometer::set_rotation(Rotation rotation) _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); } +bool Accelerometer::set_calibration_index(int calibration_index) +{ + if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { + _calibration_index = calibration_index; + return true; + } + + return false; +} + void Accelerometer::ParametersUpdate() { if (_device_id == 0) { return; } - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); - ParametersLoad(); + if (_calibration_index == -1) { + // no saved calibration available + Reset(); + + } else { + ParametersLoad(); + } } bool Accelerometer::ParametersLoad() @@ -236,9 +252,25 @@ void Accelerometer::Reset() _calibration_count = 0; } -bool Accelerometer::ParametersSave() +bool Accelerometer::ParametersSave(int desired_calibration_index, bool force) { - if (_calibration_index >= 0) { + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { + _calibration_index = desired_calibration_index; + + } else if (!force || (_calibration_index < 0) + || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) { + + // ensure we have a valid calibration slot (matching existing or first available slot) + int8_t calibration_index_prev = _calibration_index; + _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); + + if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + calibration_index_prev, _calibration_index); + } + } + + if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { // save calibration bool success = true; success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index 495bcab96a..138b4d036e 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -59,7 +59,7 @@ public: void PrintStatus(); - void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } + bool set_calibration_index(int calibration_index); void set_device_id(uint32_t device_id); bool set_offset(const matrix::Vector3f &offset); bool set_scale(const matrix::Vector3f &scale); @@ -92,7 +92,7 @@ public: } bool ParametersLoad(); - bool ParametersSave(); + bool ParametersSave(int desired_calibration_index = -1, bool force = false); void ParametersUpdate(); void Reset(); diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index 39e135f069..f6888083bd 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -129,15 +129,31 @@ void Gyroscope::set_rotation(Rotation rotation) _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); } +bool Gyroscope::set_calibration_index(int calibration_index) +{ + if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { + _calibration_index = calibration_index; + return true; + } + + return false; +} + void Gyroscope::ParametersUpdate() { if (_device_id == 0) { return; } - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); - ParametersLoad(); + if (_calibration_index == -1) { + // no saved calibration available + Reset(); + + } else { + ParametersLoad(); + } } bool Gyroscope::ParametersLoad() @@ -217,9 +233,25 @@ void Gyroscope::Reset() _calibration_count = 0; } -bool Gyroscope::ParametersSave() +bool Gyroscope::ParametersSave(int desired_calibration_index, bool force) { - if (_calibration_index >= 0) { + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { + _calibration_index = desired_calibration_index; + + } else if (!force || (_calibration_index < 0) + || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) { + + // ensure we have a valid calibration slot (matching existing or first available slot) + int8_t calibration_index_prev = _calibration_index; + _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); + + if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + calibration_index_prev, _calibration_index); + } + } + + if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { // save calibration bool success = true; success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index e9be3f9c35..a54c33f712 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -59,7 +59,7 @@ public: void PrintStatus(); - void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } + bool set_calibration_index(int calibration_index); void set_device_id(uint32_t device_id); bool set_offset(const matrix::Vector3f &offset); void set_rotation(Rotation rotation); @@ -96,7 +96,7 @@ public: } bool ParametersLoad(); - bool ParametersSave(); + bool ParametersSave(int desired_calibration_index = -1, bool force = false); void ParametersUpdate(); void Reset(); diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 24eb308fff..e4eda34ae3 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -129,15 +129,31 @@ void Magnetometer::set_rotation(Rotation rotation) _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); } +bool Magnetometer::set_calibration_index(int calibration_index) +{ + if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { + _calibration_index = calibration_index; + return true; + } + + return false; +} + void Magnetometer::ParametersUpdate() { if (_device_id == 0) { return; } - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); - ParametersLoad(); + if (_calibration_index == -1) { + // no saved calibration available + Reset(); + + } else { + ParametersLoad(); + } } bool Magnetometer::ParametersLoad() @@ -229,9 +245,25 @@ void Magnetometer::Reset() _calibration_count = 0; } -bool Magnetometer::ParametersSave() +bool Magnetometer::ParametersSave(int desired_calibration_index, bool force) { - if (_calibration_index >= 0) { + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { + _calibration_index = desired_calibration_index; + + } else if (!force || (_calibration_index < 0) + || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) { + + // ensure we have a valid calibration slot (matching existing or first available slot) + int8_t calibration_index_prev = _calibration_index; + _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); + + if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + calibration_index_prev, _calibration_index); + } + } + + if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { // save calibration bool success = true; success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 4f14e5a3ec..57a2170639 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -60,7 +60,7 @@ public: void PrintStatus(); - void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } + bool set_calibration_index(int calibration_index); void set_device_id(uint32_t device_id); bool set_offset(const matrix::Vector3f &offset); bool set_scale(const matrix::Vector3f &scale); @@ -94,7 +94,7 @@ public: } bool ParametersLoad(); - bool ParametersSave(); + bool ParametersSave(int desired_calibration_index = -1, bool force = false); void ParametersUpdate(); void Reset(); diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index e9269d1ad0..4ddf7bd82d 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -54,13 +54,15 @@ using matrix::Vector3f; namespace calibration { -int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id) +static constexpr int MAX_SENSOR_COUNT = 4; // TODO: per sensor? + +int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id) { if (device_id == 0) { return -1; } - for (unsigned i = 0; i < 4; ++i) { + for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { char str[20] {}; sprintf(str, "CAL_%s%u_ID", sensor_type, i); @@ -85,6 +87,53 @@ int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id) return -1; } +int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index) +{ + // if this device is already using a calibration slot then keep it + int calibration_index = FindCurrentCalibrationIndex(sensor_type, device_id); + + if (calibration_index >= 0) { + return calibration_index; + } + + + // device isn't currently using a calibration slot, select user preference (preferred_index) + // if available, otherwise use the first available slot + uint32_t cal_device_ids[MAX_SENSOR_COUNT] {}; + + for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { + char str[20] {}; + sprintf(str, "CAL_%s%u_ID", sensor_type, i); + int32_t device_id_val = 0; + + if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) { + cal_device_ids[i] = device_id_val; + } + } + + // use preferred_index if it's available + if ((preferred_index >= 0) && (preferred_index < MAX_SENSOR_COUNT) + && (cal_device_ids[preferred_index] == 0)) { + + calibration_index = preferred_index; + + } else { + // otherwise select first available slot + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (cal_device_ids[i] == 0) { + calibration_index = i; + break; + } + } + } + + if (calibration_index == -1) { + PX4_ERR("no %s calibration slots available", sensor_type); + } + + return calibration_index; +} + int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance) { // eg CAL_MAGn_ID/CAL_MAGn_ROT diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp index 9e29eaa574..4696f01f36 100644 --- a/src/lib/sensor_calibration/Utilities.hpp +++ b/src/lib/sensor_calibration/Utilities.hpp @@ -50,7 +50,17 @@ namespace calibration * @param device_id * @return int8_t Valid calibration index on success, -1 otherwise */ -int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id); +int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id); + +/** + * @brief Find sensor's calibration index if it exists, otherwise select an available slot. + * + * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param device_id + * @param preferred_index preferred index (optional) + * @return int8_t Valid calibration index on success, -1 otherwise + */ +int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index = -1); /** * @brief Get sensor calibration parameter value. diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index cf1eeef756..e7b1f57b99 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -69,7 +69,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s calibration_valid = true; } else { - calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0); + calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", device_id) >= 0); } if (!calibration_valid) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index ca09bd526e..8587f39b50 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -68,7 +68,7 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & calibration_valid = true; } else { - calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0); + calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", device_id) >= 0); } if (!calibration_valid) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index 713405e867..b0b1d80861 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -70,7 +70,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st calibration_valid = true; } else { - calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0); + calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", device_id) >= 0); } if (!calibration_valid) { diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5907496d3f..ffa24a1016 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -350,9 +350,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) } else { calibrations[cur_accel].Reset(); } - - // reset calibration index to match uORB numbering - calibrations[cur_accel].set_calibration_index(cur_accel); } if (active_sensors == 0) { @@ -435,7 +432,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) calibrations[i].PrintStatus(); - if (calibrations[i].ParametersSave()) { + if (calibrations[i].ParametersSave(i, true)) { param_save = true; failed = false; @@ -587,9 +584,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) calibration::Accelerometer calibration{arp.device_id}; - // reset cal index to uORB - calibration.set_calibration_index(accel_index); - if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) || !PX4_ISFINITE(offset(0)) || !PX4_ISFINITE(offset(1)) @@ -601,7 +595,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) calibration.set_offset(offset); calibration.set_temperature(temperature_avg); - if (calibration.ParametersSave()) { + if (calibration.ParametersSave(accel_index)) { calibration.PrintStatus(); param_save = true; failed = false; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 282bfe695b..69f077dd9b 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -202,9 +202,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) { worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id); } - - // reset calibration index to match uORB numbering - worker_data.calibrations[cur_gyro].set_calibration_index(cur_gyro); } unsigned try_count = 0; @@ -274,11 +271,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) calibration.set_offset(worker_data.offset[uorb_index]); calibration.set_temperature(worker_data.temperature[uorb_index]); - calibration.set_calibration_index(uorb_index); - calibration.PrintStatus(); - if (calibration.ParametersSave()) { + if (calibration.ParametersSave(uorb_index, true)) { param_save = true; failed = false; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 08ca4d697f..7a31a9a491 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -914,11 +914,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma current_cal.set_temperature(worker_data.temperature[cur_mag]); - current_cal.set_calibration_index(cur_mag); - current_cal.PrintStatus(); - if (current_cal.ParametersSave()) { + if (current_cal.ParametersSave(cur_mag, true)) { param_save = true; failed = false; @@ -1018,16 +1016,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian calibration::Magnetometer cal{mag.device_id}; - // force calibration index to uORB index - cal.set_calibration_index(cur_mag); - // use any existing scale and store the offset to the expected earth field const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field); cal.set_offset(offset); cal.set_temperature(mag.temperature); // save new calibration - if (cal.ParametersSave()) { + if (cal.ParametersSave(cur_mag)) { cal.PrintStatus(); param_save = true; failed = false; diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp index a1d74d1f3d..a865cc86f5 100644 --- a/src/modules/gyro_calibration/GyroCalibration.cpp +++ b/src/modules/gyro_calibration/GyroCalibration.cpp @@ -243,8 +243,6 @@ void GyroCalibration::Run() return; } - _gyro_calibration[gyro].set_calibration_index(gyro); - const Vector3f old_offset{_gyro_calibration[gyro].offset()}; if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) { @@ -252,7 +250,7 @@ void GyroCalibration::Run() calibration_updated = true; - PX4_INFO("gyro %d (%" PRIu32 ") updating calibration, [%.4f, %.4f, %.4f] -> [%.4f, %.4f, %.4f] %.1f°C", + PX4_INFO("gyro %d (%" PRIu32 ") updating offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] %.1f degC", gyro, _gyro_calibration[gyro].device_id(), (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), @@ -269,7 +267,7 @@ void GyroCalibration::Run() for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { if (_gyro_calibration[gyro].device_id() != 0) { - if (_gyro_calibration[gyro].ParametersSave()) { + if (_gyro_calibration[gyro].ParametersSave(gyro)) { param_save = true; } } @@ -312,7 +310,7 @@ int GyroCalibration::print_status() { for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { if (_gyro_calibration[gyro].device_id() != 0) { - PX4_INFO_RAW("gyro %d (%" PRIu32 "), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f°C (count %d)\n", + PX4_INFO_RAW("gyro %d (%" PRIu32 "), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f degC (count %d)\n", gyro, _gyro_calibration[gyro].device_id(), (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2), diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 389f1fdd5d..5af1496a43 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include @@ -792,46 +793,8 @@ void VehicleIMU::SensorCalibrationSaveAccel() (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - // find appropriate calibration slot if not already set - if (_accel_calibration.calibration_index() < 0) { - uint32_t cal_device_ids[calibration::Accelerometer::MAX_SENSOR_COUNT] {}; - bool cal_slot_match = false; - - for (unsigned cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) { - char str[20] {}; - sprintf(str, "CAL_%s%u_ID", "ACC", cal_index); - int32_t device_id_val = 0; - - if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) { - cal_device_ids[cal_index] = device_id_val; - - if (cal_device_ids[cal_index] == _accel_calibration.device_id()) { - cal_slot_match = true; - _accel_calibration.set_calibration_index(cal_index); - break; - } - } - } - - if (!cal_slot_match) { - // prefer slot that matches sensor instance - int accel_index = _sensor_accel_sub.get_instance(); - - if (cal_device_ids[accel_index] == 0) { - _accel_calibration.set_calibration_index(accel_index); - - } else { - for (int cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) { - if (cal_device_ids[accel_index] == 0) { - _accel_calibration.set_calibration_index(cal_index); - break; - } - } - } - } - } - - if (_accel_calibration.ParametersSave()) { + // save parameters with preferred calibration slot to current sensor index + if (_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance())) { param_notify_changes(); } } @@ -880,46 +843,8 @@ void VehicleIMU::SensorCalibrationSaveGyro() (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - // find appropriate calibration slot if not already set - if (_gyro_calibration.calibration_index() < 0) { - uint32_t cal_device_ids[calibration::Gyroscope::MAX_SENSOR_COUNT] {}; - bool cal_slot_match = false; - - for (unsigned cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) { - char str[20] {}; - sprintf(str, "CAL_%s%u_ID", "GYRO", cal_index); - int32_t device_id_val = 0; - - if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) { - cal_device_ids[cal_index] = device_id_val; - - if (cal_device_ids[cal_index] == _gyro_calibration.device_id()) { - cal_slot_match = true; - _gyro_calibration.set_calibration_index(cal_index); - break; - } - } - } - - if (!cal_slot_match) { - // prefer slot that matches sensor instance - int gyro_index = _sensor_gyro_sub.get_instance(); - - if (cal_device_ids[gyro_index] == 0) { - _gyro_calibration.set_calibration_index(gyro_index); - - } else { - for (int cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) { - if (cal_device_ids[gyro_index] == 0) { - _gyro_calibration.set_calibration_index(cal_index); - break; - } - } - } - } - } - - if (_gyro_calibration.ParametersSave()) { + // save parameters with preferred calibration slot to current sensor index + if (_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance())) { param_notify_changes(); } } diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 56a1722300..94d3f5ad5d 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -186,53 +186,18 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index] && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) { - if (_calibration[mag_index].calibration_index() < 0) { - // find available slots - uint32_t cal_device_ids[MAX_SENSOR_COUNT] {}; - bool cal_slot_match = false; + // set initial mag calibration + const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); - for (unsigned cal_index = 0; cal_index < MAX_SENSOR_COUNT; cal_index++) { - char str[20] {}; - sprintf(str, "CAL_%s%u_ID", "MAG", cal_index); - int32_t device_id_val = 0; + if (_calibration[mag_index].set_offset(offset)) { + _calibration[mag_index].set_temperature(_last_data[mag_index].temperature); - if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) { - cal_device_ids[cal_index] = device_id_val; + // save parameters with preferred calibration slot to current sensor index + _calibration[mag_index].ParametersSave(mag_index); - if (cal_device_ids[cal_index] == _calibration[mag_index].device_id()) { - cal_slot_match = true; - _calibration[mag_index].set_calibration_index(cal_index); - } - } - } + _calibration_estimator_bias[mag_index].zero(); - if (!cal_slot_match) { - // prefer slot that matches sensor instance - if (cal_device_ids[mag_index] == 0) { - _calibration[mag_index].set_calibration_index(mag_index); - - } else { - for (int cal_index = 0; cal_index < MAX_SENSOR_COUNT; cal_index++) { - if (cal_device_ids[mag_index] == 0) { - _calibration[mag_index].set_calibration_index(cal_index); - break; - } - } - } - } - } - - // set initial mag calibration if calibration index is set successfully - if (_calibration[mag_index].calibration_index() >= 0) { - const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); - - if (_calibration[mag_index].set_offset(offset)) { - _calibration[mag_index].set_temperature(_last_data[mag_index].temperature); - _calibration[mag_index].ParametersSave(); - _calibration_estimator_bias[mag_index].zero(); - - parameters_notify = true; - } + parameters_notify = true; } } } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 90fc583e92..e9c8a7783e 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -90,7 +90,7 @@ void VotedSensorsUpdate::parametersUpdate() if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) { // find corresponding configured accel priority - int8_t accel_cal_index = calibration::FindCalibrationIndex("ACC", imu.get().accel_device_id); + int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id); if (accel_cal_index >= 0) { // found matching CAL_ACCx_PRIO @@ -113,7 +113,7 @@ void VotedSensorsUpdate::parametersUpdate() } // find corresponding configured gyro priority - int8_t gyro_cal_index = calibration::FindCalibrationIndex("GYRO", imu.get().gyro_device_id); + int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id); if (gyro_cal_index >= 0) { // found matching CAL_GYROx_PRIO