mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-30 09:23:33 +08:00
magnetometer allow setting initial calibration from bias if available and stable
This commit is contained in:
Daniel Agar
@@ -119,27 +119,134 @@ void VehicleMagnetometer::ParametersUpdate(bool force)
|
||||
|
||||
_mag_comp_type = mag_comp_typ;
|
||||
|
||||
// update mag priority (CAL_MAGx_PRIO)
|
||||
for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
|
||||
const int32_t priority_old = _calibration[mag].priority();
|
||||
_calibration[mag].ParametersUpdate();
|
||||
const int32_t priority_new = _calibration[mag].priority();
|
||||
|
||||
if (priority_old != priority_new) {
|
||||
if (_priority[mag] == priority_old) {
|
||||
_priority[mag] = priority_new;
|
||||
if (!_armed) {
|
||||
bool calibration_updated = false;
|
||||
|
||||
} else {
|
||||
// change relative priority to incorporate any sensor faults
|
||||
int priority_change = priority_new - priority_old;
|
||||
_priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100);
|
||||
// update mag priority (CAL_MAGx_PRIO)
|
||||
for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
|
||||
const auto calibration_count = _calibration[mag].calibration_count();
|
||||
const int32_t priority_old = _calibration[mag].priority();
|
||||
_calibration[mag].ParametersUpdate();
|
||||
const int32_t priority_new = _calibration[mag].priority();
|
||||
|
||||
if (priority_old != priority_new) {
|
||||
if (_priority[mag] == priority_old) {
|
||||
_priority[mag] = priority_new;
|
||||
|
||||
} else {
|
||||
// change relative priority to incorporate any sensor faults
|
||||
int priority_change = priority_new - priority_old;
|
||||
_priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100);
|
||||
}
|
||||
}
|
||||
|
||||
if (calibration_count != _calibration[mag].calibration_count()) {
|
||||
calibration_updated = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (calibration_updated) {
|
||||
// clear all
|
||||
for (auto &bias : _calibration_estimator_bias) {
|
||||
bias.zero();
|
||||
}
|
||||
|
||||
for (auto &cal : _mag_cal) {
|
||||
cal = {};
|
||||
}
|
||||
|
||||
_in_flight_mag_cal_available = false;
|
||||
_last_calibration_update = hrt_absolute_time();
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleMagnetometer::UpdateMagBiasEstimate()
|
||||
{
|
||||
if (_magnetometer_bias_estimate_sub.updated()) {
|
||||
// Continuous mag calibration is running when not armed
|
||||
magnetometer_bias_estimate_s mag_bias_est;
|
||||
|
||||
if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
|
||||
bool parameters_notify = false;
|
||||
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) {
|
||||
|
||||
const Vector3f bias{mag_bias_est.bias_x[mag_index],
|
||||
mag_bias_est.bias_y[mag_index],
|
||||
mag_bias_est.bias_z[mag_index]};
|
||||
|
||||
_calibration_estimator_bias[mag_index] = bias;
|
||||
|
||||
// set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available
|
||||
if (!_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;
|
||||
|
||||
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 (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] == _calibration[mag_index].device_id()) {
|
||||
cal_slot_match = true;
|
||||
_calibration[mag_index].set_calibration_index(cal_index);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (parameters_notify) {
|
||||
param_notify_changes();
|
||||
_last_calibration_update = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleMagnetometer::MagCalibrationUpdate()
|
||||
void VehicleMagnetometer::UpdateMagCalibration()
|
||||
{
|
||||
// State variance assumed for magnetometer bias storage.
|
||||
// This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value.
|
||||
@@ -170,21 +277,16 @@ void VehicleMagnetometer::MagCalibrationUpdate()
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) {
|
||||
|
||||
const auto old_offset = _mag_cal[i].mag_offset;
|
||||
|
||||
_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;
|
||||
_mag_cal[i].mag_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias);
|
||||
_mag_cal[i].mag_bias_variance = bias_variance;
|
||||
|
||||
// readd estimated bias that was removed before publishing vehicle_magnetometer
|
||||
_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) +
|
||||
_calibration_estimator_bias[mag_index];
|
||||
|
||||
_mag_cal[i].variance = bias_variance;
|
||||
_mag_cal[i].temperature = _last_data[mag_index].temperature;
|
||||
|
||||
_in_flight_mag_cal_available = true;
|
||||
|
||||
if ((old_offset - _mag_cal[i].mag_offset).longerThan(0.01f)) {
|
||||
PX4_DEBUG("Mag %d (%d) est. offset saved: [% 05.3f % 05.3f % 05.3f] (bias [% 05.3f % 05.3f % 05.3f])",
|
||||
mag_index, _mag_cal[i].device_id,
|
||||
(double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2),
|
||||
(double)bias(0), (double)bias(1), (double)bias(2));
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -192,115 +294,68 @@ void VehicleMagnetometer::MagCalibrationUpdate()
|
||||
}
|
||||
}
|
||||
|
||||
if (_in_flight_mag_cal_available || _on_ground_mag_bias_estimate_available) {
|
||||
_should_save_on_disarm = true;
|
||||
}
|
||||
|
||||
} else if (_should_save_on_disarm) {
|
||||
} else if (_in_flight_mag_cal_available) {
|
||||
// not armed and mag cal available
|
||||
bool calibration_param_save_needed = false;
|
||||
// iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme
|
||||
Vector3f state_variance{magb_vref, magb_vref, magb_vref};
|
||||
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
// first, apply the calibration from the mag bias estimator (not EKF2)
|
||||
if (_on_ground_mag_bias_estimate_available) {
|
||||
const Vector3f new_offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
|
||||
|
||||
if (_calibration[mag_index].set_offset(new_offset)) {
|
||||
calibration_param_save_needed = true;
|
||||
_calibration_estimator_bias[mag_index].zero();
|
||||
}
|
||||
}
|
||||
|
||||
// apply all valid saved offsets
|
||||
if (_in_flight_mag_cal_available) {
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) {
|
||||
const Vector3f mag_cal_orig{_calibration[mag_index].offset()};
|
||||
Vector3f mag_cal_offset{_calibration[mag_index].offset()};
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) {
|
||||
const Vector3f mag_cal_orig{_calibration[mag_index].offset()};
|
||||
|
||||
// calculate weighting using ratio of variances and update stored bias values
|
||||
const Vector3f &observation = _mag_cal[i].mag_offset;
|
||||
const Vector3f &obs_variance = _mag_cal[i].mag_bias_variance;
|
||||
// calculate weighting using ratio of variances and update stored bias values
|
||||
const Vector3f &observation{_mag_cal[i].offset};
|
||||
const Vector3f &obs_variance{_mag_cal[i].variance};
|
||||
|
||||
for (int axis_index = 0; axis_index < 3; axis_index++) {
|
||||
const float innovation_variance = state_variance(axis_index) + obs_variance(axis_index);
|
||||
const float innovation = mag_cal_offset(axis_index) - observation(axis_index);
|
||||
const float kalman_gain = state_variance(axis_index) / innovation_variance;
|
||||
mag_cal_offset(axis_index) -= innovation * kalman_gain;
|
||||
state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f);
|
||||
}
|
||||
const Vector3f innovation{mag_cal_orig - observation};
|
||||
const Vector3f innovation_variance{state_variance + obs_variance};
|
||||
const Vector3f kalman_gain{state_variance.edivide(innovation_variance)};
|
||||
|
||||
if (_calibration[mag_index].set_offset(mag_cal_offset)) {
|
||||
// new offset
|
||||
const Vector3f mag_cal_offset{mag_cal_orig - innovation.emult(kalman_gain)};
|
||||
|
||||
PX4_INFO("%d (%" PRIu32 ") EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
|
||||
mag_index, _calibration[mag_index].device_id(), i,
|
||||
(double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
|
||||
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
|
||||
(double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));
|
||||
|
||||
calibration_param_save_needed = true;
|
||||
}
|
||||
|
||||
// clear
|
||||
_mag_cal[i].device_id = 0;
|
||||
_mag_cal[i].mag_offset.zero();
|
||||
_mag_cal[i].mag_bias_variance.zero();
|
||||
for (int axis_index = 0; axis_index < 3; axis_index++) {
|
||||
state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f);
|
||||
}
|
||||
|
||||
if (_calibration[mag_index].set_offset(mag_cal_offset)) {
|
||||
|
||||
PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",
|
||||
mag_index, _calibration[mag_index].device_id(), i,
|
||||
(double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
|
||||
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
|
||||
(double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2));
|
||||
|
||||
_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
|
||||
|
||||
_calibration[mag_index].ParametersSave();
|
||||
|
||||
calibration_param_save_needed = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// clear
|
||||
_mag_cal[i] = {};
|
||||
}
|
||||
|
||||
_calibration_estimator_bias[mag_index].zero();
|
||||
}
|
||||
|
||||
if (calibration_param_save_needed) {
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
if (_calibration[mag_index].device_id() != 0) {
|
||||
_calibration[mag_index].ParametersSave();
|
||||
}
|
||||
}
|
||||
|
||||
param_notify_changes();
|
||||
_last_calibration_update = hrt_absolute_time();
|
||||
}
|
||||
|
||||
_should_save_on_disarm = false;
|
||||
_on_ground_mag_bias_estimate_available = false;
|
||||
_in_flight_mag_cal_available = false;
|
||||
|
||||
} else {
|
||||
// Continuous mag calibration is running when not armed
|
||||
if (_magnetometer_bias_estimate_sub.updated()) {
|
||||
magnetometer_bias_estimate_s mag_bias_est;
|
||||
|
||||
if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
if (mag_bias_est.valid[mag_index]) {
|
||||
const Vector3f bias{mag_bias_est.bias_x[mag_index],
|
||||
mag_bias_est.bias_y[mag_index],
|
||||
mag_bias_est.bias_z[mag_index]};
|
||||
_calibration_estimator_bias[mag_index] = bias;
|
||||
_on_ground_mag_bias_estimate_available = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleMagnetometer::Run()
|
||||
void VehicleMagnetometer::UpdatePowerCompensation()
|
||||
{
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
ParametersUpdate();
|
||||
|
||||
// check vehicle status for changes to armed state
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
vehicle_control_mode_s vehicle_control_mode;
|
||||
|
||||
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
||||
_armed = vehicle_control_mode.flag_armed;
|
||||
}
|
||||
}
|
||||
|
||||
if (_mag_comp_type != MagCompensationType::Disabled) {
|
||||
// update power signal for mag compensation
|
||||
if (_armed && (_mag_comp_type == MagCompensationType::Throttle)) {
|
||||
@@ -332,6 +387,27 @@ void VehicleMagnetometer::Run()
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void VehicleMagnetometer::Run()
|
||||
{
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
ParametersUpdate();
|
||||
|
||||
// check vehicle status for changes to armed state
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
vehicle_control_mode_s vehicle_control_mode;
|
||||
|
||||
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
||||
_armed = vehicle_control_mode.flag_armed;
|
||||
}
|
||||
}
|
||||
|
||||
UpdatePowerCompensation();
|
||||
|
||||
UpdateMagBiasEstimate();
|
||||
|
||||
bool updated[MAX_SENSOR_COUNT] {};
|
||||
|
||||
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
|
||||
@@ -391,8 +467,7 @@ void VehicleMagnetometer::Run()
|
||||
_mag_sum[uorb_index] += vect;
|
||||
_mag_sum_count[uorb_index]++;
|
||||
|
||||
_last_data[uorb_index].timestamp_sample = report.timestamp_sample;
|
||||
_last_data[uorb_index].device_id = report.device_id;
|
||||
_last_data[uorb_index] = report;
|
||||
_last_data[uorb_index].x = vect(0);
|
||||
_last_data[uorb_index].y = vect(1);
|
||||
_last_data[uorb_index].z = vect(2);
|
||||
@@ -502,10 +577,10 @@ void VehicleMagnetometer::Run()
|
||||
calcMagInconsistency();
|
||||
}
|
||||
|
||||
MagCalibrationUpdate();
|
||||
UpdateMagCalibration();
|
||||
|
||||
// reschedule timeout
|
||||
ScheduleDelayed(20_ms);
|
||||
ScheduleDelayed(40_ms);
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user