forked from Archive/PX4-Autopilot
355 lines
11 KiB
C++
355 lines
11 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2019 Estimation and Control Library (ECL). 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 ECL 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 mag_control.cpp
|
|
* Control functions for ekf magnetic field fusion
|
|
*/
|
|
|
|
#include "ekf.h"
|
|
#include <mathlib/mathlib.h>
|
|
|
|
void Ekf::controlMagFusion()
|
|
{
|
|
updateMagFilter();
|
|
checkMagFieldStrength();
|
|
|
|
// If we are on ground, store the local position and time to use as a reference
|
|
// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
|
|
if (!_control_status.flags.in_air) {
|
|
_last_on_ground_posD = _state.pos(2);
|
|
_control_status.flags.mag_aligned_in_flight = false;
|
|
_num_bad_flight_yaw_events = 0;
|
|
}
|
|
|
|
if ((_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE)
|
|
|| _control_status.flags.mag_fault
|
|
|| !_control_status.flags.yaw_align) {
|
|
stopMagFusion();
|
|
return;
|
|
}
|
|
|
|
if (canRunMagFusion()) {
|
|
if (_control_status.flags.in_air) {
|
|
checkHaglYawResetReq();
|
|
runInAirYawReset();
|
|
runVelPosReset();
|
|
|
|
} else {
|
|
runOnGroundYawReset();
|
|
}
|
|
|
|
// Determine if we should use simple magnetic heading fusion which works better when
|
|
// there are large external disturbances or the more accurate 3-axis fusion
|
|
switch (_params.mag_fusion_type) {
|
|
case MAG_FUSE_TYPE_AUTO:
|
|
selectMagAuto();
|
|
break;
|
|
|
|
case MAG_FUSE_TYPE_INDOOR:
|
|
/* fallthrough */
|
|
case MAG_FUSE_TYPE_HEADING:
|
|
startMagHdgFusion();
|
|
break;
|
|
|
|
case MAG_FUSE_TYPE_3D:
|
|
startMag3DFusion();
|
|
break;
|
|
|
|
default:
|
|
selectMagAuto();
|
|
break;
|
|
}
|
|
|
|
checkMagDeclRequired();
|
|
checkMagInhibition();
|
|
|
|
runMagAndMagDeclFusions();
|
|
}
|
|
}
|
|
|
|
void Ekf::updateMagFilter()
|
|
{
|
|
if (_mag_data_ready) {
|
|
_mag_lpf.update(_mag_sample_delayed.mag);
|
|
}
|
|
}
|
|
|
|
bool Ekf::canRunMagFusion() const
|
|
{
|
|
// check for new magnetometer data that has fallen behind the fusion time horizon
|
|
// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used
|
|
return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw && _mag_data_ready;
|
|
}
|
|
|
|
void Ekf::checkHaglYawResetReq()
|
|
{
|
|
// We need to reset the yaw angle after climbing away from the ground to enable
|
|
// recovery from ground level magnetic interference.
|
|
if (!_control_status.flags.mag_aligned_in_flight) {
|
|
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
|
// and request a yaw reset if not already requested.
|
|
static constexpr float mag_anomalies_max_hagl = 1.5f;
|
|
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
|
_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
|
|
}
|
|
}
|
|
|
|
float Ekf::getTerrainVPos() const
|
|
{
|
|
return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
|
|
}
|
|
|
|
void Ekf::runOnGroundYawReset()
|
|
{
|
|
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
|
|
const bool has_realigned_yaw = canResetMagHeading()
|
|
? resetMagHeading(_mag_lpf.getState())
|
|
: false;
|
|
|
|
_mag_yaw_reset_req = !has_realigned_yaw;
|
|
}
|
|
}
|
|
|
|
bool Ekf::isYawResetAuthorized() const
|
|
{
|
|
return !_mag_use_inhibit;
|
|
}
|
|
|
|
bool Ekf::canResetMagHeading() const
|
|
{
|
|
return !isStrongMagneticDisturbance();
|
|
}
|
|
|
|
void Ekf::runInAirYawReset()
|
|
{
|
|
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
|
|
bool has_realigned_yaw = false;
|
|
|
|
if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
|
|
else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }
|
|
|
|
_mag_yaw_reset_req = !has_realigned_yaw;
|
|
_control_status.flags.mag_aligned_in_flight = has_realigned_yaw;
|
|
}
|
|
}
|
|
|
|
bool Ekf::canRealignYawUsingGps() const
|
|
{
|
|
return _control_status.flags.fixed_wing;
|
|
}
|
|
|
|
void Ekf::runVelPosReset()
|
|
{
|
|
if (_velpos_reset_request) {
|
|
resetVelocity();
|
|
resetPosition();
|
|
_velpos_reset_request = false;
|
|
}
|
|
}
|
|
|
|
void Ekf::selectMagAuto()
|
|
{
|
|
check3DMagFusionSuitability();
|
|
canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion();
|
|
}
|
|
|
|
void Ekf::check3DMagFusionSuitability()
|
|
{
|
|
checkYawAngleObservability();
|
|
checkMagBiasObservability();
|
|
|
|
if (isMagBiasObservable() || isYawAngleObservable()) {
|
|
_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
|
|
}
|
|
}
|
|
|
|
void Ekf::checkYawAngleObservability()
|
|
{
|
|
// Check if there has been enough change in horizontal velocity to make yaw observable
|
|
// Apply hysteresis to check to avoid rapid toggling
|
|
_yaw_angle_observable = _yaw_angle_observable
|
|
? _accel_lpf_NE.norm() > _params.mag_acc_gate
|
|
: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
|
|
|
|
_yaw_angle_observable = _yaw_angle_observable
|
|
&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
|
|
}
|
|
|
|
void Ekf::checkMagBiasObservability()
|
|
{
|
|
// check if there is enough yaw rotation to make the mag bias states observable
|
|
if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
|
|
// initial yaw motion is detected
|
|
_mag_bias_observable = true;
|
|
|
|
} else if (_mag_bias_observable) {
|
|
// require sustained yaw motion of 50% the initial yaw rate threshold
|
|
const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started);
|
|
const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt;
|
|
_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
|
|
}
|
|
|
|
_yaw_delta_ef = 0.0f;
|
|
_time_yaw_started = _imu_sample_delayed.time_us;
|
|
}
|
|
|
|
bool Ekf::isYawAngleObservable() const
|
|
{
|
|
return _yaw_angle_observable;
|
|
}
|
|
|
|
bool Ekf::isMagBiasObservable() const
|
|
{
|
|
return _mag_bias_observable;
|
|
}
|
|
|
|
bool Ekf::canUse3DMagFusion() const
|
|
{
|
|
// Use of 3D fusion requires an in-air heading alignment but it should not
|
|
// be used when the heading and mag biases are not observable for more than 2 seconds
|
|
return _control_status.flags.mag_aligned_in_flight
|
|
&& ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
|
|
}
|
|
|
|
void Ekf::checkMagDeclRequired()
|
|
{
|
|
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
|
|
// then the declination must be fused as an observation to prevent long term heading drift
|
|
// fusing declination when gps aiding is available is optional, but recommended to prevent
|
|
// problem if the vehicle is static for extended periods of time
|
|
const bool user_selected = (_params.mag_declination_source & MASK_FUSE_DECL);
|
|
const bool not_using_ne_aiding = !_control_status.flags.gps;
|
|
_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
|
|
}
|
|
|
|
void Ekf::checkMagInhibition()
|
|
{
|
|
_mag_use_inhibit = shouldInhibitMag();
|
|
if (!_mag_use_inhibit) {
|
|
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
|
|
}
|
|
|
|
// If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading
|
|
if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
|
|
_mag_inhibit_yaw_reset_req = true;
|
|
}
|
|
}
|
|
|
|
bool Ekf::shouldInhibitMag() const
|
|
{
|
|
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
|
|
// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
|
|
// is available, assume that we are operating indoors and the magnetometer should not be used.
|
|
// Also inhibit mag fusion when a strong magnetic field interference is detected
|
|
const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
|
|
|
|
const bool heading_not_required_for_navigation = !_control_status.flags.gps
|
|
&& !_control_status.flags.ev_pos
|
|
&& !_control_status.flags.ev_vel;
|
|
|
|
return (user_selected && heading_not_required_for_navigation)
|
|
|| isStrongMagneticDisturbance();
|
|
}
|
|
|
|
void Ekf::checkMagFieldStrength()
|
|
{
|
|
if (_params.check_mag_strength) {
|
|
_control_status.flags.mag_field_disturbed = _NED_origin_initialised
|
|
? !isMeasuredMatchingGpsMagStrength()
|
|
: !isMeasuredMatchingAverageMagStrength();
|
|
|
|
} else {
|
|
_control_status.flags.mag_field_disturbed = false;
|
|
}
|
|
}
|
|
|
|
bool Ekf::isStrongMagneticDisturbance() const
|
|
{
|
|
return _control_status.flags.mag_field_disturbed;
|
|
}
|
|
|
|
bool Ekf::isMeasuredMatchingGpsMagStrength() const
|
|
{
|
|
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
|
|
return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size);
|
|
}
|
|
|
|
bool Ekf::isMeasuredMatchingAverageMagStrength() const
|
|
{
|
|
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
|
|
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
|
|
return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(),
|
|
average_earth_mag_field_strength,
|
|
average_earth_mag_gate_size);
|
|
}
|
|
|
|
bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
|
|
{
|
|
return (measured >= expected - gate)
|
|
&& (measured <= expected + gate);
|
|
}
|
|
|
|
void Ekf::runMagAndMagDeclFusions()
|
|
{
|
|
if (_control_status.flags.mag_3D) {
|
|
run3DMagAndDeclFusions();
|
|
|
|
} else if (_control_status.flags.mag_hdg) {
|
|
fuseHeading();
|
|
}
|
|
}
|
|
|
|
void Ekf::run3DMagAndDeclFusions()
|
|
{
|
|
if (!_mag_decl_cov_reset) {
|
|
// After any magnetic field covariance reset event the earth field state
|
|
// covariances need to be corrected to incorporate knowledge of the declination
|
|
// before fusing magnetomer data to prevent rapid rotation of the earth field
|
|
// states for the first few observations.
|
|
fuseDeclination(0.02f);
|
|
_mag_decl_cov_reset = true;
|
|
fuseMag();
|
|
|
|
} else {
|
|
// The normal sequence is to fuse the magnetometer data first before fusing
|
|
// declination angle at a higher uncertainty to allow some learning of
|
|
// declination angle over time.
|
|
fuseMag();
|
|
if (_control_status.flags.mag_dec) {
|
|
fuseDeclination(0.5f);
|
|
}
|
|
}
|
|
}
|
|
|