forked from Archive/PX4-Autopilot
magnetometer allow setting initial calibration from bias if available and stable
This commit is contained in:
Daniel Agar
parent
b7efd4f947
commit
8fbf79527f
|
|
@ -5,3 +5,4 @@ float32[4] bias_y # estimated Y-bias of all the sensors
|
|||
float32[4] bias_z # estimated Z-bias of all the sensors
|
||||
|
||||
bool[4] valid # true if the estimator has converged
|
||||
bool[4] stable
|
||||
|
|
|
|||
|
|
@ -60,10 +60,6 @@ void Magnetometer::set_device_id(uint32_t device_id, bool external)
|
|||
set_external(external);
|
||||
_device_id = device_id;
|
||||
|
||||
if (_device_id != 0) {
|
||||
_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
|
||||
}
|
||||
|
||||
ParametersUpdate();
|
||||
}
|
||||
}
|
||||
|
|
@ -151,6 +147,10 @@ void Magnetometer::set_rotation(Rotation rotation)
|
|||
|
||||
void Magnetometer::ParametersUpdate()
|
||||
{
|
||||
if (_device_id != 0) {
|
||||
_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
|
||||
}
|
||||
|
||||
if (_calibration_index >= 0) {
|
||||
|
||||
// CAL_MAGx_ROT
|
||||
|
|
@ -287,7 +287,7 @@ void Magnetometer::PrintStatus()
|
|||
{
|
||||
if (external()) {
|
||||
PX4_INFO("%s %" PRIu32
|
||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, External ROT: %d",
|
||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d",
|
||||
SensorString(), device_id(), enabled(),
|
||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
||||
(double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
|
||||
|
|
|
|||
|
|
@ -86,20 +86,26 @@ void MagBiasEstimator::Run()
|
|||
|
||||
if (_vehicle_status_sub.copy(&vehicle_status)) {
|
||||
if (_arming_state != vehicle_status.arming_state) {
|
||||
_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
|
||||
_arming_state = vehicle_status.arming_state;
|
||||
|
||||
// reset on any arming state change
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
_reset_field_estimator[mag_index] = true;
|
||||
for (auto &reset : _reset_field_estimator) {
|
||||
reset = true;
|
||||
}
|
||||
|
||||
_arming_state = vehicle_status.arming_state;
|
||||
if (_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
ScheduleOnInterval(1_s);
|
||||
|
||||
} else {
|
||||
// restore 50 Hz scheduling
|
||||
ScheduleOnInterval(20_ms);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// only run when disarmed
|
||||
if (_armed) {
|
||||
if (_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -128,86 +134,104 @@ void MagBiasEstimator::Run()
|
|||
vehicle_status_flags_s vehicle_status_flags;
|
||||
|
||||
if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) {
|
||||
bool system_calibrating = vehicle_status_flags.condition_calibration_enabled;
|
||||
|
||||
// do nothing during regular sensor calibration
|
||||
_system_calibrating = vehicle_status_flags.condition_calibration_enabled;
|
||||
_system_sensors_initialized = vehicle_status_flags.condition_system_sensors_initialized
|
||||
&& vehicle_status_flags.condition_system_hotplug_timeout;
|
||||
}
|
||||
}
|
||||
if (system_calibrating != _system_calibrating) {
|
||||
_system_calibrating = system_calibrating;
|
||||
|
||||
if (_system_calibrating || !_system_sensors_initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
Vector3f angular_velocity{};
|
||||
|
||||
{
|
||||
// Assume a constant angular velocity during two mag samples
|
||||
vehicle_angular_velocity_s vehicle_angular_velocity;
|
||||
|
||||
if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
|
||||
angular_velocity = Vector3f{vehicle_angular_velocity.xyz};
|
||||
}
|
||||
}
|
||||
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
sensor_mag_s sensor_mag;
|
||||
|
||||
while (_sensor_mag_subs[mag_index].update(&sensor_mag)) {
|
||||
|
||||
// apply existing mag calibration
|
||||
_calibration[mag_index].set_device_id(sensor_mag.device_id, sensor_mag.is_external);
|
||||
const Vector3f raw_mag{sensor_mag.x, sensor_mag.y, sensor_mag.z};
|
||||
const Vector3f mag_calibrated = _calibration[mag_index].Correct(raw_mag);
|
||||
|
||||
float dt = (sensor_mag.timestamp_sample - _timestamp_last_update[mag_index]) * 1e-6f;
|
||||
_timestamp_last_update[mag_index] = sensor_mag.timestamp_sample;
|
||||
|
||||
if (dt < 0.001f || dt > 0.2f) {
|
||||
_reset_field_estimator[mag_index] = true;
|
||||
}
|
||||
|
||||
if (_reset_field_estimator[mag_index]) {
|
||||
// reset
|
||||
_bias_estimator[mag_index].setBias(Vector3f{});
|
||||
_bias_estimator[mag_index].setField(mag_calibrated);
|
||||
|
||||
_reset_field_estimator[mag_index] = false;
|
||||
_valid[mag_index] = false;
|
||||
|
||||
} else {
|
||||
const Vector3f bias_prev = _bias_estimator[mag_index].getBias();
|
||||
|
||||
_bias_estimator[mag_index].updateEstimate(angular_velocity, mag_calibrated, dt);
|
||||
|
||||
const Vector3f &bias = _bias_estimator[mag_index].getBias();
|
||||
const Vector3f bias_rate = (bias - bias_prev) / dt;
|
||||
|
||||
Vector3f fitness;
|
||||
fitness(0) = fabsf(angular_velocity(0)) / fmaxf(fabsf(bias_rate(1)) + fabsf(bias_rate(2)), 0.02f);
|
||||
fitness(1) = fabsf(angular_velocity(1)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(2)), 0.02f);
|
||||
fitness(2) = fabsf(angular_velocity(2)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(1)), 0.02f);
|
||||
|
||||
if (!PX4_ISFINITE(bias(0)) || !PX4_ISFINITE(bias(1)) || !PX4_ISFINITE(bias(2)) || bias.longerThan(5.f)) {
|
||||
_reset_field_estimator[mag_index] = true;
|
||||
_valid[mag_index] = false;
|
||||
|
||||
} else {
|
||||
const bool bias_significant = bias.longerThan(0.04f);
|
||||
const bool has_converged = fitness(0) > 20.f || fitness(1) > 20.f || fitness(2) > 20.f;
|
||||
|
||||
if (bias_significant && has_converged) {
|
||||
_valid[mag_index] = true;
|
||||
}
|
||||
for (auto &reset : _reset_field_estimator) {
|
||||
reset = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
publishMagBiasEstimate();
|
||||
// do nothing during regular sensor calibration
|
||||
if (_system_calibrating) {
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
// Assume a constant angular velocity during two mag samples
|
||||
vehicle_angular_velocity_s vehicle_angular_velocity;
|
||||
|
||||
if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
|
||||
|
||||
const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
|
||||
|
||||
bool updated = false;
|
||||
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
sensor_mag_s sensor_mag;
|
||||
|
||||
while (_sensor_mag_subs[mag_index].update(&sensor_mag)) {
|
||||
|
||||
updated = true;
|
||||
|
||||
// apply existing mag calibration
|
||||
_calibration[mag_index].set_device_id(sensor_mag.device_id, sensor_mag.is_external);
|
||||
|
||||
const Vector3f mag_calibrated = _calibration[mag_index].Correct(Vector3f{sensor_mag.x, sensor_mag.y, sensor_mag.z});
|
||||
|
||||
float dt = (sensor_mag.timestamp_sample - _timestamp_last_update[mag_index]) * 1e-6f;
|
||||
_timestamp_last_update[mag_index] = sensor_mag.timestamp_sample;
|
||||
|
||||
if (dt < 0.001f || dt > 0.2f) {
|
||||
_reset_field_estimator[mag_index] = true;
|
||||
}
|
||||
|
||||
if (_reset_field_estimator[mag_index]) {
|
||||
// reset
|
||||
_bias_estimator[mag_index].setBias(Vector3f{});
|
||||
_bias_estimator[mag_index].setField(mag_calibrated);
|
||||
|
||||
_reset_field_estimator[mag_index] = false;
|
||||
_valid[mag_index] = false;
|
||||
_time_valid[mag_index] = 0;
|
||||
|
||||
} else {
|
||||
updated = true;
|
||||
|
||||
const Vector3f bias_prev = _bias_estimator[mag_index].getBias();
|
||||
|
||||
_bias_estimator[mag_index].updateEstimate(angular_velocity, mag_calibrated, dt);
|
||||
|
||||
const Vector3f &bias = _bias_estimator[mag_index].getBias();
|
||||
const Vector3f bias_rate = (bias - bias_prev) / dt;
|
||||
|
||||
if (!PX4_ISFINITE(bias(0)) || !PX4_ISFINITE(bias(1)) || !PX4_ISFINITE(bias(2)) || bias.longerThan(5.f)) {
|
||||
_reset_field_estimator[mag_index] = true;
|
||||
_valid[mag_index] = false;
|
||||
_time_valid[mag_index] = 0;
|
||||
|
||||
} else {
|
||||
|
||||
Vector3f fitness{
|
||||
fabsf(angular_velocity(0)) / fmaxf(fabsf(bias_rate(1)) + fabsf(bias_rate(2)), 0.02f),
|
||||
fabsf(angular_velocity(1)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(2)), 0.02f),
|
||||
fabsf(angular_velocity(2)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(1)), 0.02f)
|
||||
};
|
||||
|
||||
const bool bias_significant = bias.longerThan(0.04f);
|
||||
const bool has_converged = fitness(0) > 20.f || fitness(1) > 20.f || fitness(2) > 20.f;
|
||||
|
||||
if (bias_significant && has_converged) {
|
||||
if (!_valid[mag_index]) {
|
||||
_time_valid[mag_index] = hrt_absolute_time();
|
||||
}
|
||||
|
||||
_valid[mag_index] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (updated) {
|
||||
publishMagBiasEstimate();
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
|
@ -218,15 +242,19 @@ void MagBiasEstimator::publishMagBiasEstimate()
|
|||
|
||||
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
|
||||
const Vector3f &bias = _bias_estimator[mag_index].getBias();
|
||||
|
||||
mag_bias_est.timestamp = hrt_absolute_time();
|
||||
mag_bias_est.bias_x[mag_index] = bias(0);
|
||||
mag_bias_est.bias_y[mag_index] = bias(1);
|
||||
mag_bias_est.bias_z[mag_index] = bias(2);
|
||||
|
||||
mag_bias_est.valid[mag_index] = _valid[mag_index];
|
||||
|
||||
if (_valid[mag_index]) {
|
||||
mag_bias_est.valid[mag_index] = true;
|
||||
mag_bias_est.stable[mag_index] = (hrt_elapsed_time(&_time_valid[mag_index]) > 30_s);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
mag_bias_est.timestamp = hrt_absolute_time();
|
||||
_magnetometer_bias_estimate_pub.publish(mag_bias_est);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -97,13 +97,13 @@ private:
|
|||
|
||||
calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
|
||||
|
||||
hrt_abstime _time_valid[MAX_SENSOR_COUNT] {};
|
||||
|
||||
bool _reset_field_estimator[MAX_SENSOR_COUNT] {};
|
||||
bool _valid[MAX_SENSOR_COUNT] {};
|
||||
|
||||
bool _armed{false};
|
||||
bool _system_calibrating{false};
|
||||
bool _system_sensors_initialized{false};
|
||||
uint8_t _arming_state{0};
|
||||
bool _system_calibrating{false};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -87,7 +87,10 @@ private:
|
|||
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
|
||||
*/
|
||||
void calcMagInconsistency();
|
||||
void MagCalibrationUpdate();
|
||||
|
||||
void UpdateMagBiasEstimate();
|
||||
void UpdateMagCalibration();
|
||||
void UpdatePowerCompensation();
|
||||
|
||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||
|
||||
|
|
@ -111,13 +114,12 @@ private:
|
|||
uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
|
||||
|
||||
bool _in_flight_mag_cal_available{false}; ///< from navigation filter
|
||||
bool _on_ground_mag_bias_estimate_available{false}; ///< from pre-takeoff mag_bias_estimator
|
||||
bool _should_save_on_disarm{false};
|
||||
|
||||
struct MagCal {
|
||||
uint32_t device_id{0};
|
||||
matrix::Vector3f mag_offset{};
|
||||
matrix::Vector3f mag_bias_variance{};
|
||||
matrix::Vector3f offset{};
|
||||
matrix::Vector3f variance{};
|
||||
float temperature{NAN};
|
||||
} _mag_cal[ORB_MULTI_MAX_INSTANCES] {};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
|
||||
|
|
@ -127,6 +129,8 @@ private:
|
|||
{this, ORB_ID(sensor_mag), 3}
|
||||
};
|
||||
|
||||
hrt_abstime _last_calibration_update{0};
|
||||
|
||||
matrix::Vector3f _calibration_estimator_bias[MAX_SENSOR_COUNT] {};
|
||||
|
||||
calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
|
||||
|
|
@ -148,7 +152,7 @@ private:
|
|||
DataValidatorGroup _voter{1};
|
||||
unsigned _last_failover_count{0};
|
||||
|
||||
uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
|
||||
uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {};
|
||||
matrix::Vector3f _mag_sum[MAX_SENSOR_COUNT] {};
|
||||
int _mag_sum_count[MAX_SENSOR_COUNT] {};
|
||||
hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
|
||||
|
|
|
|||
Loading…
Reference in New Issue