mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: make mag control responsible for WMM
- this further untangles mag control (which requires the WMM) from GPS
This commit is contained in:
Daniel Agar
Mathieu Bresciani
parent
be4d0d351c
commit
bbcf741e9e
@ -41,10 +41,6 @@
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
# include <lib/world_magnetic_model/geo_mag_declination.h>
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
// GPS pre-flight check bit locations
|
||||
@ -89,20 +85,20 @@ void Ekf::collect_gps(const gnssSample &gps)
|
||||
_gpos_origin_eph = gps.hacc;
|
||||
_gpos_origin_epv = gps.vacc;
|
||||
|
||||
_information_events.flags.gps_checks_passed = true;
|
||||
ECL_INFO("GPS checks passed");
|
||||
}
|
||||
_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(gps.lat)));
|
||||
|
||||
if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
|
||||
// a rough 2D fix is sufficient to lookup declination
|
||||
_information_events.flags.gps_checks_passed = true;
|
||||
|
||||
ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
|
||||
_pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
|
||||
|
||||
} else {
|
||||
// a rough 2D fix is sufficient to lookup earth spin rate
|
||||
const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
|
||||
|
||||
if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
|
||||
updateWmm(gps.lat, gps.lon);
|
||||
_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
|
||||
}
|
||||
|
||||
_wmm_gps_time_last_checked = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -39,6 +39,8 @@
|
||||
#include "ekf.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include <lib/world_magnetic_model/geo_mag_declination.h>
|
||||
|
||||
#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
|
||||
|
||||
void Ekf::controlMagFusion(const imuSample &imu_sample)
|
||||
@ -76,15 +78,43 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
|
||||
_mag_counter++;
|
||||
}
|
||||
|
||||
// check for WMM update periodically or if global origin has changed
|
||||
bool wmm_updated = false;
|
||||
|
||||
if (global_origin().isInitialized()) {
|
||||
|
||||
bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse);
|
||||
|
||||
if (global_origin_valid()
|
||||
&& (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
|
||||
) {
|
||||
// position of local NED origin in GPS / WGS84 frame
|
||||
double latitude_deg;
|
||||
double longitude_deg;
|
||||
global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);
|
||||
|
||||
if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
|
||||
wmm_updated = true;
|
||||
}
|
||||
|
||||
_wmm_mag_time_last_checked = _time_delayed_us;
|
||||
|
||||
} else if (origin_newer_than_last_mag) {
|
||||
// use global origin to update WMM
|
||||
if (updateWorldMagneticModel(global_origin().getProjectionReferenceLat(),
|
||||
global_origin().getProjectionReferenceLon())
|
||||
) {
|
||||
wmm_updated = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
|
||||
// this is useful if there is a lot of interference on the sensor measurement.
|
||||
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
|
||||
&& (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
|
||||
) {
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
|
||||
* Vector3f(_mag_strength_gps, 0, 0);
|
||||
|
||||
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
|
||||
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
|
||||
|
||||
_control_status.flags.synthetic_mag_z = true;
|
||||
|
||||
@ -140,8 +170,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
|
||||
|
||||
checkMagHeadingConsistency(mag_sample);
|
||||
|
||||
// WMM update can occur on the last epoch, just after mag fusion
|
||||
const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
|
||||
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
|
||||
|
||||
|
||||
@ -203,7 +231,21 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
|
||||
}
|
||||
|
||||
if (_control_status.flags.mag_dec) {
|
||||
fuseDeclination(0.5f);
|
||||
|
||||
if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_wmm_declination_rad)
|
||||
) {
|
||||
fuseDeclination(_wmm_declination_rad, 0.5f);
|
||||
|
||||
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
|
||||
) {
|
||||
|
||||
fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f);
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_dec = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -333,24 +375,21 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
|
||||
resetMagCov();
|
||||
|
||||
// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
|
||||
if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
|
||||
|
||||
if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) {
|
||||
// use expected earth field to reset states
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
|
||||
* Vector3f(_mag_strength_gps, 0, 0);
|
||||
|
||||
// mag_B: reset
|
||||
if (!reset_heading && _control_status.flags.yaw_align) {
|
||||
// mag_B: reset using WMM
|
||||
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
_state.mag_B = mag - (R_to_body * mag_earth_pred);
|
||||
_state.mag_B = mag - (R_to_body * _wmm_earth_field_gauss);
|
||||
|
||||
} else {
|
||||
_state.mag_B.zero();
|
||||
}
|
||||
|
||||
// mag_I: reset, skipped if no change in state and variance good
|
||||
_state.mag_I = mag_earth_pred;
|
||||
_state.mag_I = _wmm_earth_field_gauss;
|
||||
|
||||
if (reset_heading) {
|
||||
resetMagHeading(mag);
|
||||
@ -451,8 +490,8 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
|
||||
_mag_strength = mag_sample.length();
|
||||
|
||||
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
|
||||
if (PX4_ISFINITE(_mag_strength_gps)) {
|
||||
if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
|
||||
if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
|
||||
if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
|
||||
_control_status.flags.mag_field_disturbed = true;
|
||||
is_check_failing = true;
|
||||
}
|
||||
@ -475,9 +514,9 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
|
||||
_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
|
||||
|
||||
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
|
||||
if (PX4_ISFINITE(_mag_inclination_gps)) {
|
||||
if (PX4_ISFINITE(_wmm_inclination_rad)) {
|
||||
const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
|
||||
const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);
|
||||
const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
|
||||
|
||||
if (fabsf(inc_error_rad) > inc_tol_rad) {
|
||||
_control_status.flags.mag_field_disturbed = true;
|
||||
@ -549,13 +588,60 @@ float Ekf::getMagDeclination()
|
||||
// Use value consistent with earth field state
|
||||
return atan2f(_state.mag_I(1), _state.mag_I(0));
|
||||
|
||||
} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
|
||||
// use parameter value until GPS is available, then use value returned by geo library
|
||||
if (PX4_ISFINITE(_mag_declination_gps)) {
|
||||
return _mag_declination_gps;
|
||||
} else if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_wmm_declination_rad)
|
||||
) {
|
||||
// if available use value returned by geo library
|
||||
return _wmm_declination_rad;
|
||||
|
||||
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
|
||||
) {
|
||||
// using saved mag declination
|
||||
return math::radians(_params.mag_declination_deg);
|
||||
}
|
||||
|
||||
// otherwise unavailable
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longitude_deg)
|
||||
{
|
||||
// set the magnetic field data returned by the geo library using the current GPS position
|
||||
const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
|
||||
const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
|
||||
const float strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);
|
||||
|
||||
if (PX4_ISFINITE(declination_rad) && PX4_ISFINITE(inclination_rad) && PX4_ISFINITE(strength_gauss)) {
|
||||
|
||||
const bool declination_changed = (fabsf(declination_rad - _wmm_declination_rad) > math::radians(1.f));
|
||||
const bool inclination_changed = (fabsf(inclination_rad - _wmm_inclination_rad) > math::radians(1.f));
|
||||
const bool strength_changed = (fabsf(strength_gauss - _wmm_field_strength_gauss) > 0.01f);
|
||||
|
||||
if (!PX4_ISFINITE(_wmm_declination_rad)
|
||||
|| !PX4_ISFINITE(_wmm_inclination_rad)
|
||||
|| !PX4_ISFINITE(_wmm_field_strength_gauss)
|
||||
|| !_wmm_earth_field_gauss.longerThan(0.f)
|
||||
|| !_wmm_earth_field_gauss.isAllFinite()
|
||||
|| declination_changed
|
||||
|| inclination_changed
|
||||
|| strength_changed
|
||||
) {
|
||||
|
||||
ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)",
|
||||
(double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad),
|
||||
(double)latitude_deg, (double)longitude_deg
|
||||
);
|
||||
|
||||
_wmm_declination_rad = declination_rad;
|
||||
_wmm_inclination_rad = inclination_rad;
|
||||
_wmm_field_strength_gauss = strength_gauss;
|
||||
|
||||
_wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(strength_gauss, 0, 0);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// otherwise use the parameter value
|
||||
return math::radians(_params.mag_declination_deg);
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -150,51 +150,34 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Ekf::fuseDeclination(float decl_sigma)
|
||||
bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma)
|
||||
{
|
||||
float decl_measurement = NAN;
|
||||
// observation variance (rad**2)
|
||||
const float R_DECL = sq(decl_sigma);
|
||||
|
||||
if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_mag_declination_gps)
|
||||
) {
|
||||
decl_measurement = _mag_declination_gps;
|
||||
VectorState H;
|
||||
float decl_pred;
|
||||
float innovation_variance;
|
||||
|
||||
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
|
||||
) {
|
||||
decl_measurement = math::radians(_params.mag_declination_deg);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON,
|
||||
&decl_pred, &innovation_variance, &H);
|
||||
|
||||
const float innovation = wrap_pi(decl_pred - decl_measurement_rad);
|
||||
|
||||
if (innovation_variance < R_DECL) {
|
||||
// variance calculation is badly conditioned
|
||||
_fault_status.flags.bad_mag_decl = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(decl_measurement)) {
|
||||
// Calculate the Kalman gains
|
||||
VectorState Kfusion = P * H / innovation_variance;
|
||||
|
||||
// observation variance (rad**2)
|
||||
const float R_DECL = sq(decl_sigma);
|
||||
const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
|
||||
|
||||
VectorState H;
|
||||
float decl_pred;
|
||||
float innovation_variance;
|
||||
_fault_status.flags.bad_mag_decl = !is_fused;
|
||||
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance,
|
||||
&H);
|
||||
|
||||
const float innovation = wrap_pi(decl_pred - decl_measurement);
|
||||
|
||||
if (innovation_variance < R_DECL) {
|
||||
// variance calculation is badly conditioned
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calculate the Kalman gains
|
||||
VectorState Kfusion = P * H / innovation_variance;
|
||||
|
||||
const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
|
||||
|
||||
_fault_status.flags.bad_mag_decl = !is_fused;
|
||||
|
||||
return is_fused;
|
||||
}
|
||||
|
||||
return false;
|
||||
return is_fused;
|
||||
}
|
||||
|
||||
float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
|
||||
|
||||
@ -259,7 +259,6 @@ public:
|
||||
// return true if the origin is valid
|
||||
bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
|
||||
bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
|
||||
void updateWmm(double lat, double lon);
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
|
||||
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
|
||||
@ -466,6 +465,9 @@ public:
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// set the magnetic field data returned by the geo library using position
|
||||
bool updateWorldMagneticModel(const double latitude_deg, const double longitude_deg);
|
||||
|
||||
const auto &aid_src_mag() const { return _aid_src_mag; }
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
@ -770,8 +772,8 @@ private:
|
||||
bool update_all_states = false, bool update_tilt = false);
|
||||
|
||||
// fuse magnetometer declination measurement
|
||||
// argument passed in is the declination uncertainty in radians
|
||||
bool fuseDeclination(float decl_sigma);
|
||||
// declination uncertainty in radians
|
||||
bool fuseDeclination(float decl_measurement_rad, float decl_sigma);
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
|
||||
@ -96,13 +96,13 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
|
||||
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
|
||||
_gps_alt_ref = altitude;
|
||||
|
||||
updateWmm(current_lat, current_lon);
|
||||
|
||||
_gpos_origin_eph = eph;
|
||||
_gpos_origin_epv = epv;
|
||||
|
||||
_NED_origin_initialised = true;
|
||||
|
||||
_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(latitude)));
|
||||
|
||||
if (current_pos_available) {
|
||||
// reset horizontal position if we already have a global origin
|
||||
Vector2f position = _pos_ref.project(current_lat, current_lon);
|
||||
@ -131,39 +131,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::updateWmm(const double lat, const double lon)
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
// set the magnetic field data returned by the geo library using the current GPS position
|
||||
const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon));
|
||||
const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon));
|
||||
const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
|
||||
|
||||
if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
|
||||
|
||||
const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
|
||||
const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
|
||||
|
||||
if ((_wmm_gps_time_last_set == 0)
|
||||
|| !PX4_ISFINITE(_mag_declination_gps)
|
||||
|| !PX4_ISFINITE(_mag_inclination_gps)
|
||||
|| !PX4_ISFINITE(_mag_strength_gps)
|
||||
|| mag_declination_changed
|
||||
|| mag_inclination_changed
|
||||
) {
|
||||
_mag_declination_gps = mag_declination_gps;
|
||||
_mag_inclination_gps = mag_inclination_gps;
|
||||
_mag_strength_gps = mag_strength_gps;
|
||||
|
||||
_wmm_gps_time_last_set = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
}
|
||||
|
||||
|
||||
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
{
|
||||
float eph = INFINITY;
|
||||
|
||||
@ -250,7 +250,7 @@ public:
|
||||
bool get_mag_decl_deg(float &val) const
|
||||
{
|
||||
if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
|
||||
val = math::degrees(_mag_declination_gps);
|
||||
val = math::degrees(_wmm_declination_rad);
|
||||
return true;
|
||||
|
||||
} else {
|
||||
@ -261,7 +261,7 @@ public:
|
||||
bool get_mag_inc_deg(float &val) const
|
||||
{
|
||||
if (_NED_origin_initialised) {
|
||||
val = math::degrees(_mag_inclination_gps);
|
||||
val = math::degrees(_wmm_inclination_rad);
|
||||
return true;
|
||||
|
||||
} else {
|
||||
@ -272,9 +272,9 @@ public:
|
||||
void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const
|
||||
{
|
||||
inc_deg = math::degrees(_mag_inclination);
|
||||
inc_ref_deg = math::degrees(_mag_inclination_gps);
|
||||
inc_ref_deg = math::degrees(_wmm_inclination_rad);
|
||||
strength_gs = _mag_strength;
|
||||
strength_ref_gs = _mag_strength_gps;
|
||||
strength_ref_gs = _wmm_field_strength_gauss;
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
@ -451,13 +451,14 @@ protected:
|
||||
// allocate data buffers and initialize interface variables
|
||||
bool initialise_interface(uint64_t timestamp);
|
||||
|
||||
uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked
|
||||
uint64_t _wmm_gps_time_last_set{0}; // time WMM last set
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
float _mag_declination_gps {NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
|
||||
float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
|
||||
float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T)
|
||||
uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control
|
||||
|
||||
float _wmm_declination_rad{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
|
||||
float _wmm_inclination_rad{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
|
||||
float _wmm_field_strength_gauss{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (Gauss)
|
||||
|
||||
Vector3f _wmm_earth_field_gauss{}; // expected magnetic field vector from the last valid GPS position (Gauss)
|
||||
|
||||
float _mag_inclination{NAN};
|
||||
float _mag_strength{NAN};
|
||||
|
||||
@ -199,7 +199,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
|
||||
|
||||
EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
|
||||
const Vector3f vel = _ekf->getVelocity();
|
||||
EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-3f);
|
||||
EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-2f);
|
||||
const Vector2f vel_wind_earth = _ekf->getWindVelocity();
|
||||
EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f);
|
||||
EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);
|
||||
|
||||
@ -108,7 +108,6 @@ TEST_F(EkfBasicsTest, initialControlMode)
|
||||
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
|
||||
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
|
||||
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag);
|
||||
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec);
|
||||
EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air);
|
||||
EXPECT_EQ(0, (int) _ekf->control_status_flags().wind);
|
||||
EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user