From ec178c87457de1443673d5082d0c12d6b7d5d932 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Oct 2021 14:17:40 +0200 Subject: [PATCH] create new mag_bias_estimator module Co-authored-by: Daniel Agar --- msg/CMakeLists.txt | 1 + msg/magnetometer_bias_estimate.msg | 7 + src/lib/CMakeLists.txt | 2 +- .../CMakeLists.txt | 0 .../FieldSensorBiasEstimator.hpp | 7 +- .../FieldSensorBiasEstimatorTest.cpp | 8 +- .../MagnetometerBiasEstimator.cpp | 104 ------- .../MagnetometerBiasEstimator.hpp | 86 ------ src/modules/logger/logged_topics.cpp | 1 + .../mag_bias_estimator}/CMakeLists.txt | 20 +- .../mag_bias_estimator/MagBiasEstimator.cpp | 275 ++++++++++++++++++ .../mag_bias_estimator/MagBiasEstimator.hpp | 115 ++++++++ src/modules/mag_bias_estimator/params.c | 58 ++++ 13 files changed, 478 insertions(+), 206 deletions(-) create mode 100644 msg/magnetometer_bias_estimate.msg rename src/lib/{magnetometer_bias_estimator/FieldSensorBiasEstimator => field_sensor_bias_estimator}/CMakeLists.txt (100%) rename src/lib/{magnetometer_bias_estimator/FieldSensorBiasEstimator => field_sensor_bias_estimator}/FieldSensorBiasEstimator.hpp (95%) rename src/lib/{magnetometer_bias_estimator/FieldSensorBiasEstimator => field_sensor_bias_estimator}/FieldSensorBiasEstimatorTest.cpp (92%) delete mode 100644 src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp delete mode 100644 src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp rename src/{lib/magnetometer_bias_estimator => modules/mag_bias_estimator}/CMakeLists.txt (85%) create mode 100644 src/modules/mag_bias_estimator/MagBiasEstimator.cpp create mode 100644 src/modules/mag_bias_estimator/MagBiasEstimator.hpp create mode 100644 src/modules/mag_bias_estimator/params.c diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f2ee2e7b9e..168c25176a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -98,6 +98,7 @@ set(msg_files log_message.msg logger_status.msg mag_worker_data.msg + magnetometer_bias_estimate.msg manual_control_setpoint.msg manual_control_switches.msg mavlink_log.msg diff --git a/msg/magnetometer_bias_estimate.msg b/msg/magnetometer_bias_estimate.msg new file mode 100644 index 0000000000..8ceccd8335 --- /dev/null +++ b/msg/magnetometer_bias_estimate.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32[4] bias_x # estimated X-bias of all the sensors +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 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 38bf63ee1c..5cbf524583 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -45,12 +45,12 @@ add_subdirectory(controllib) add_subdirectory(conversion) add_subdirectory(crypto) add_subdirectory(drivers) +add_subdirectory(field_sensor_bias_estimator) add_subdirectory(geo) add_subdirectory(hysteresis) add_subdirectory(l1) add_subdirectory(landing_slope) add_subdirectory(led) -add_subdirectory(magnetometer_bias_estimator) add_subdirectory(mathlib) add_subdirectory(mixer) add_subdirectory(mixer_module) diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/CMakeLists.txt b/src/lib/field_sensor_bias_estimator/CMakeLists.txt similarity index 100% rename from src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/CMakeLists.txt rename to src/lib/field_sensor_bias_estimator/CMakeLists.txt diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp similarity index 95% rename from src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp rename to src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp index fb75453a7f..c7e23cae89 100644 --- a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp +++ b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp @@ -59,6 +59,7 @@ public: // Set initial states void setField(const matrix::Vector3f &field) { _field_prev = field; } void setBias(const matrix::Vector3f &bias) { _state_bias = bias; } + void setLearningGain(float learning_gain) { _learning_gain = learning_gain; } /** * Update the estimator and extract updated biases. @@ -70,7 +71,7 @@ public: { const matrix::Vector3f field_pred = _field_prev + (-gyro % (_field_prev - _state_bias)) * dt; const matrix::Vector3f field_innov = field - field_pred; - _state_bias += GAIN_BIAS * (-gyro % field_innov); + _state_bias += _learning_gain * (-gyro % field_innov); _field_prev = field; } @@ -78,10 +79,8 @@ public: const matrix::Vector3f &getBias() { return _state_bias; } private: - // gains - static constexpr float GAIN_BIAS = 100.f; - // states matrix::Vector3f _field_prev{}; matrix::Vector3f _state_bias{}; + float _learning_gain{20.f}; }; diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp similarity index 92% rename from src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp rename to src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp index b1ed252558..d53ebdbf70 100644 --- a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp +++ b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp @@ -31,14 +31,20 @@ * ****************************************************************************/ +/** + * Test code for the Field Sensor Bias Estimator + * Run this test only using make tests TESTFILTER=FieldSensorBiasEstimator + */ + #include #include using namespace matrix; -TEST(MagnetometerBiasEstimatorTest, AllZeroCase) +TEST(MagnetometerBiasEstimatorTest, constantZRotation) { FieldSensorBiasEstimator field_sensor_bias_estimator; + field_sensor_bias_estimator.setLearningGain(100.f); const Vector3f virtual_gyro = Vector3f(0.f, 0.f, 0.1f); Vector3f virtual_unbiased_mag = Vector3f(0.9f, 0.f, 1.79f); // taken from SITL jmavsim initialization const Vector3f virtual_bias(0.2f, -0.4f, 0.5f); diff --git a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp deleted file mode 100644 index c0754ddc34..0000000000 --- a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file MagnetometerBiasEstimator.cpp - */ - -#include "MagnetometerBiasEstimator.hpp" - -#include -#include -#include - -using namespace matrix; -using namespace time_literals; - -MagnetometerBiasEstimator::MagnetometerBiasEstimator(const matrix::Dcmf &board_rotation) : - ModuleParams(nullptr), - _board_rotation(board_rotation) -{} - -void MagnetometerBiasEstimator::extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw) -{ - // fill in vectors - Vector3f gyro(gyro_raw.gyro_rad); - Vector3f mag(mag_raw.magnetometer_ga); - - // prepare delta time in seconds - hrt_abstime time_stamp_current = hrt_absolute_time(); - float dt = math::min((time_stamp_current - _time_stamp_last_loop), hrt_abstime(500_ms)) / 1e6f; - _time_stamp_last_loop = time_stamp_current; - - _field_sensor_bias_estimator.updateEstimate(gyro, mag, dt); - const Vector3f &bias = _field_sensor_bias_estimator.getBias(); - - // save the bias to the parameters to apply it given the right circumstances - const bool longer_than_10_sec = (time_stamp_current - _time_stamp_last_save) > hrt_abstime(10_s); - const bool bias_significant = bias.norm_squared() > (0.01f * 0.01f); - - _actuator_armed_sub.update(); - - if (!_actuator_armed_sub.get().armed && bias_significant && longer_than_10_sec) { - saveBias(bias); - _time_stamp_last_save = time_stamp_current; - } - - (mag - bias).copyTo(mag_raw.magnetometer_ga); -} - -void MagnetometerBiasEstimator::saveBias(const matrix::Vector3f &bias) -{ - // estimated bias is in body frame, but the driver needs sensor frame - const Vector3f transformed_bias = _board_rotation.transpose() * bias; - - // get current calibration - updateParams(); - Vector3f calibration_bias(_param_cal_mag0_xoff.get(), - _param_cal_mag0_yoff.get(), - _param_cal_mag0_zoff.get()); - - // estimated bias comes on top of existing calibration - calibration_bias += transformed_bias; - - // save new calibration - _param_cal_mag0_xoff.set(calibration_bias(0)); - _param_cal_mag0_yoff.set(calibration_bias(1)); - _param_cal_mag0_zoff.set(calibration_bias(2)); - _param_cal_mag0_xoff.commit(); - _param_cal_mag0_yoff.commit(); - _param_cal_mag0_zoff.commit(); - - // reset internal bias to zero because from now the driver will correct - _field_sensor_bias_estimator.setBias(Vector3f()); -} diff --git a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp deleted file mode 100644 index 4fb0867463..0000000000 --- a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp +++ /dev/null @@ -1,86 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file MagnetometerBiasEstimator.hpp - * - * Runs the FieldSensorBiasEstimator with PX4 magnetometer and gyroscope data and saves the result to the calibration parameters. - * - * @author Matthias Grob - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -class MagnetometerBiasEstimator : public ModuleParams -{ -public: - MagnetometerBiasEstimator(const matrix::Dcmf &board_rotation); - ~MagnetometerBiasEstimator() = default; - - /** - * Update the estimator and extract updated magnetometer biases. - * @param mag_raw struct containing the magnetometer data to operate on (gets adjusted with current estimate) - * @param raw struct containing the gyroscope data to use - */ - void extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw); - -private: - void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &mag, const float dt); - void saveBias(const matrix::Vector3f &bias); - - FieldSensorBiasEstimator _field_sensor_bias_estimator{}; - const matrix::Dcmf &_board_rotation; - - hrt_abstime _time_stamp_last_loop{0}; - hrt_abstime _time_stamp_last_save{0}; - - uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_cal_mag0_xoff, - (ParamFloat) _param_cal_mag0_yoff, - (ParamFloat) _param_cal_mag0_zoff - ) -}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5a7af2de20..89ed72b35c 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -69,6 +69,7 @@ void LoggedTopics::add_default_topics() add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); add_topic("internal_combustion_engine_status", 10); + add_topic("magnetometer_bias_estimate", 200); add_topic("manual_control_setpoint", 200); add_topic("manual_control_switches"); add_topic("mission_result"); diff --git a/src/lib/magnetometer_bias_estimator/CMakeLists.txt b/src/modules/mag_bias_estimator/CMakeLists.txt similarity index 85% rename from src/lib/magnetometer_bias_estimator/CMakeLists.txt rename to src/modules/mag_bias_estimator/CMakeLists.txt index 4d2594ee40..d52a439d3c 100644 --- a/src/lib/magnetometer_bias_estimator/CMakeLists.txt +++ b/src/modules/mag_bias_estimator/CMakeLists.txt @@ -30,14 +30,14 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -add_subdirectory(FieldSensorBiasEstimator) - -px4_add_library(MagnetometerBiasEstimator - MagnetometerBiasEstimator.cpp +px4_add_module( + MODULE modules__mag_bias_estimator + MAIN mag_bias_estimator + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + MagBiasEstimator.cpp + MagBiasEstimator.hpp + DEPENDS + px4_work_queue ) -target_include_directories(MagnetometerBiasEstimator - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) -target_link_libraries(MagnetometerBiasEstimator PUBLIC FieldSensorBiasEstimator) diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp new file mode 100644 index 0000000000..15d05ba483 --- /dev/null +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -0,0 +1,275 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MagBiasEstimator.hpp" + +using namespace time_literals; +using matrix::Vector3f; + +namespace mag_bias_estimator +{ + +MagBiasEstimator::MagBiasEstimator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +MagBiasEstimator::~MagBiasEstimator() +{ + perf_free(_cycle_perf); +} + +int MagBiasEstimator::task_spawn(int argc, char *argv[]) +{ + MagBiasEstimator *obj = new MagBiasEstimator(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + + return 0; +} + +void MagBiasEstimator::start() +{ + ScheduleOnInterval(20_ms); // 50 Hz +} + +void MagBiasEstimator::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + 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; + + // reset on any arming state change + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + _reset_field_estimator[mag_index] = true; + } + + _arming_state = vehicle_status.arming_state; + } + } + } + + // only run when disarmed + if (_armed) { + return; + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + const auto calibration_count = _calibration[mag_index].calibration_count(); + _calibration[mag_index].ParametersUpdate(); + + if (calibration_count != _calibration[mag_index].calibration_count()) { + _reset_field_estimator[mag_index] = true; + } + + _bias_estimator[mag_index].setLearningGain(_param_mbe_learn_gain.get()); + } + } + + if (_vehicle_status_flags_sub.updated()) { + vehicle_status_flags_s vehicle_status_flags; + + if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) { + + // 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_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; + } + } + } + } + } + + publishMagBiasEstimate(); + + perf_end(_cycle_perf); +} + +void MagBiasEstimator::publishMagBiasEstimate() +{ + magnetometer_bias_estimate_s mag_bias_est{}; + + 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]; + } + + + _magnetometer_bias_estimate_pub.publish(mag_bias_est); +} + +int MagBiasEstimator::print_status() +{ + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].device_id() != 0) { + + _calibration[mag_index].PrintStatus(); + + const Vector3f &bias = _bias_estimator[mag_index].getBias(); + + PX4_INFO("%d (%" PRIu32 ") bias: [% 05.3f % 05.3f % 05.3f]", + mag_index, _calibration[mag_index].device_id(), + (double)bias(0), + (double)bias(1), + (double)bias(2)); + } + } + + return 0; +} + +int MagBiasEstimator::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Online magnetometer bias estimator. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mag_bias_estimator", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int mag_bias_estimator_main(int argc, char *argv[]) +{ + return MagBiasEstimator::main(argc, argv); +} + +} // namespace load_mon diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp new file mode 100644 index 0000000000..d4efeb5946 --- /dev/null +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mag_bias_estimator +{ + +class MagBiasEstimator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + MagBiasEstimator(); + ~MagBiasEstimator() override; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void start(); + +private: + void Run() override; + void publishMagBiasEstimate(); + + static constexpr int MAX_SENSOR_COUNT = 4; + + FieldSensorBiasEstimator _bias_estimator[MAX_SENSOR_COUNT]; + hrt_abstime _timestamp_last_update[MAX_SENSOR_COUNT] {}; + + uORB::SubscriptionMultiArray _sensor_mag_subs{ORB_ID::sensor_mag}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + + uORB::Publication _magnetometer_bias_estimate_pub{ORB_ID(magnetometer_bias_estimate)}; + + calibration::Magnetometer _calibration[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}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mbe_learn_gain + ) +}; + +} // namespace mag_bias_estimator diff --git a/src/modules/mag_bias_estimator/params.c b/src/modules/mag_bias_estimator/params.c new file mode 100644 index 0000000000..9df4e565a3 --- /dev/null +++ b/src/modules/mag_bias_estimator/params.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Enable online mag bias calibration + * + * This enables continuous calibration of the magnetometers + * before takeoff using gyro data. + * + * @boolean + * @reboot_required true + * @group Magnetometer Bias Estimator + */ +PARAM_DEFINE_INT32(MBE_ENABLE, 1); + +/** + * Mag bias estimator learning gain + * + * Increase to make the estimator more responsive + * Decrease to make the estimator more robust to noise + * + * @min 0.1 + * @max 100 + * @increment 0.1 + * @decimal 1 + * @group Magnetometer Bias Estimator + */ +PARAM_DEFINE_FLOAT(MBE_LEARN_GAIN, 0.4f);