fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 05:53:34 +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/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
+1 -1
View File
@@ -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) {
src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
+1 -1
View File
@@ -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) {
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
+1 -1
View File
@@ -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) {
src/modules/commander/accelerometer_calibration.cpp
+2 -8
View File
@@ -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;
src/modules/commander/gyro_calibration.cpp
+1 -6
View File
@@ -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;
src/modules/commander/mag_calibration.cpp
+2 -7
View File
@@ -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;
src/modules/gyro_calibration/GyroCalibration.cpp
+3 -5
View File
@@ -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),
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