mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
lib/magnetometer_bias_estimator: Add library for online magnetometer calibration on the ground
Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Matthias Grob
GitHub
parent
36d15ada1c
commit
d9954ecaf0
@ -50,6 +50,7 @@ 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)
|
||||
|
||||
43
src/lib/magnetometer_bias_estimator/CMakeLists.txt
Normal file
43
src/lib/magnetometer_bias_estimator/CMakeLists.txt
Normal file
@ -0,0 +1,43 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(FieldSensorBiasEstimator)
|
||||
|
||||
px4_add_library(MagnetometerBiasEstimator
|
||||
MagnetometerBiasEstimator.cpp
|
||||
)
|
||||
target_include_directories(MagnetometerBiasEstimator
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
target_link_libraries(MagnetometerBiasEstimator PUBLIC FieldSensorBiasEstimator)
|
||||
@ -0,0 +1,37 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_library(FieldSensorBiasEstimator INTERFACE)
|
||||
target_include_directories(FieldSensorBiasEstimator INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
px4_add_unit_gtest(SRC FieldSensorBiasEstimatorTest.cpp LINKLIBS FieldSensorBiasEstimator)
|
||||
@ -0,0 +1,87 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 DAfieldES (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 DAfieldE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FieldSensorBiasEstimator.hpp
|
||||
*
|
||||
* Estimator for the magnetometer/accelerometer bias calibration parameters to run online with the help of Gyroscope data.
|
||||
*
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
|
||||
*
|
||||
* Publication documenting the algorithm:
|
||||
* Adaptive Estimation of Measurement Bias in Three-Dimensional Field Sensors with Angular-Rate Sensors: Theory and Comparative Experimental Evaluation
|
||||
* Giancarlo Troni and Louis L. Whitcomb, Department of Mechanical Engineering, Johns Hopkins University, Baltimore, Maryland 21218, USA
|
||||
*
|
||||
* http://www.roboticsproceedings.org/rss09/p50.pdf
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
class FieldSensorBiasEstimator
|
||||
{
|
||||
public:
|
||||
FieldSensorBiasEstimator() = default;
|
||||
~FieldSensorBiasEstimator() = default;
|
||||
|
||||
// Set initial states
|
||||
void setField(const matrix::Vector3f &field) { _field_prev = field; }
|
||||
void setBias(const matrix::Vector3f &bias) { _state_bias = bias; }
|
||||
|
||||
/**
|
||||
* Update the estimator and extract updated biases.
|
||||
* @param gyro bias corrected gyroscope data in the same coordinate frame as the field sensor data
|
||||
* @param field biased field sensor data
|
||||
* @param dt time in seconds since the last update
|
||||
*/
|
||||
void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &field, const float dt)
|
||||
{
|
||||
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);
|
||||
_field_prev = field;
|
||||
}
|
||||
|
||||
const matrix::Vector3f &getField() { return _field_prev; }
|
||||
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{};
|
||||
};
|
||||
@ -0,0 +1,69 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <FieldSensorBiasEstimator.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(MagnetometerBiasEstimatorTest, AllZeroCase)
|
||||
{
|
||||
FieldSensorBiasEstimator field_sensor_bias_estimator;
|
||||
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);
|
||||
Vector3f virtual_mag = virtual_unbiased_mag + virtual_bias;
|
||||
|
||||
// Initialize with the current measurement
|
||||
field_sensor_bias_estimator.setField(virtual_mag);
|
||||
|
||||
for (int i = 0; i <= 1000; i++) {
|
||||
float dt = .01f;
|
||||
|
||||
// turn the mag values according to the gyro
|
||||
|
||||
virtual_mag = virtual_unbiased_mag + virtual_bias;
|
||||
// printf("---- %i\n", i);
|
||||
// printf("virtual_gyro\n"); virtual_gyro.print();
|
||||
// printf("virtual_mag\n"); virtual_mag.print();
|
||||
|
||||
field_sensor_bias_estimator.updateEstimate(virtual_gyro, virtual_mag, dt);
|
||||
virtual_unbiased_mag = Dcmf(AxisAnglef(-virtual_gyro * dt)) * virtual_unbiased_mag;
|
||||
}
|
||||
|
||||
const Vector3f bias_est = field_sensor_bias_estimator.getBias();
|
||||
EXPECT_NEAR(bias_est(0), virtual_bias(0), 1e-3f) << "Estimated X bias " << bias_est(0);
|
||||
EXPECT_NEAR(bias_est(1), virtual_bias(1), 1e-3f) << "Estimated Y bias " << bias_est(1);
|
||||
// The Z bias is not observable due to pure yaw rotation
|
||||
EXPECT_NEAR(bias_est(2), 0.f, 1e-3f) << "Estimated Z bias " << bias_est(2);
|
||||
}
|
||||
@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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());
|
||||
}
|
||||
@ -0,0 +1,86 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
)
|
||||
};
|
||||
Loading…
x
Reference in New Issue
Block a user