forked from Archive/PX4-Autopilot
create new mag_bias_estimator module
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
bresch
Daniel Agar
parent
d858835fd0
commit
ec178c8745
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
};
|
||||
|
|
@ -31,14 +31,20 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Test code for the Field Sensor Bias Estimator
|
||||
* Run this test only using make tests TESTFILTER=FieldSensorBiasEstimator
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <FieldSensorBiasEstimator.hpp>
|
||||
|
||||
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);
|
||||
|
|
@ -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 <px4_posix.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
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());
|
||||
}
|
||||
|
|
@ -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 <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <px4_module_params.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/vehicle_magnetometer.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
|
||||
#include <FieldSensorBiasEstimator.hpp>
|
||||
|
||||
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_s> _actuator_armed_sub{ORB_ID(actuator_armed)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::CAL_MAG0_XOFF>) _param_cal_mag0_xoff,
|
||||
(ParamFloat<px4::params::CAL_MAG0_YOFF>) _param_cal_mag0_yoff,
|
||||
(ParamFloat<px4::params::CAL_MAG0_ZOFF>) _param_cal_mag0_zoff
|
||||
)
|
||||
};
|
||||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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 <drivers/drv_hrt.h>
|
||||
#include <lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/sensor_calibration/Magnetometer.hpp>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/magnetometer_bias_estimate.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_mag.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
namespace mag_bias_estimator
|
||||
{
|
||||
|
||||
class MagBiasEstimator : public ModuleBase<MagBiasEstimator>, 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_s, MAX_SENSOR_COUNT> _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_s> _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<px4::params::MBE_LEARN_GAIN>) _param_mbe_learn_gain
|
||||
)
|
||||
};
|
||||
|
||||
} // namespace mag_bias_estimator
|
||||
|
|
@ -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);
|
||||
Loading…
Reference in New Issue