fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 00:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor_calibration: refactor and centralize calibration slot logic

Browse Source 
- centralize logic for selecting a preferred calibration slot
   - automatically use existing calibration slot if it exists, otherwise
find first available slot, with a preference for a requested index
 - existing commander calibration methods rewrite all calibration slots
to match current sensor ordering
This commit is contained in:
Daniel Agar
2022-01-12 16:16:26 -05:00
parent d1304e1ceb
commit bb1177d504
18 changed files with 202 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+5 -80
View File
@@ -35,6 +35,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/events.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <float.h>
@@ -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();
}
}
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+8 -43
View File
@@ -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;
}
}
}
src/modules/sensors/voted_sensors_update.cpp
+2 -2
View File
@@ -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