diff --git a/msg/magnetometer_bias_estimate.msg b/msg/magnetometer_bias_estimate.msg index 8ceccd8335..3c0c1136cf 100644 --- a/msg/magnetometer_bias_estimate.msg +++ b/msg/magnetometer_bias_estimate.msg @@ -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 diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index b7d7855bc9..878ff04059 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -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), diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp index f8a52ea659..a313fbacc6 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -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); } diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp index d4efeb5946..3d0723ac0d 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp @@ -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")}; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index e76db0f226..2e9cbea69b 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -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); } diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index 5e32caa323..62f4dbfa9a 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -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_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] {};