mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: add kconfig for magnetometer support (enabled by default)
This commit is contained in:
Daniel Agar
parent
d1266c856f
commit
845b01a00d
@ -84,12 +84,9 @@ list(APPEND EKF_SRCS
|
||||
EKF/gravity_fusion.cpp
|
||||
EKF/height_control.cpp
|
||||
EKF/imu_down_sampler.cpp
|
||||
EKF/mag_3d_control.cpp
|
||||
EKF/mag_control.cpp
|
||||
EKF/mag_fusion.cpp
|
||||
EKF/mag_heading_control.cpp
|
||||
EKF/output_predictor.cpp
|
||||
EKF/vel_pos_fusion.cpp
|
||||
EKF/yaw_fusion.cpp
|
||||
EKF/zero_innovation_heading_update.cpp
|
||||
EKF/zero_gyro_update.cpp
|
||||
EKF/zero_velocity_update.cpp
|
||||
@ -121,6 +118,15 @@ if(CONFIG_EKF2_GNSS_YAW)
|
||||
list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_MAGNETOMETER)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/mag_3d_control.cpp
|
||||
EKF/mag_control.cpp
|
||||
EKF/mag_fusion.cpp
|
||||
EKF/mag_heading_control.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/optical_flow_control.cpp
|
||||
|
||||
@ -49,12 +49,9 @@ list(APPEND EKF_SRCS
|
||||
gravity_fusion.cpp
|
||||
height_control.cpp
|
||||
imu_down_sampler.cpp
|
||||
mag_3d_control.cpp
|
||||
mag_control.cpp
|
||||
mag_fusion.cpp
|
||||
mag_heading_control.cpp
|
||||
output_predictor.cpp
|
||||
vel_pos_fusion.cpp
|
||||
yaw_fusion.cpp
|
||||
zero_innovation_heading_update.cpp
|
||||
zero_gyro_update.cpp
|
||||
zero_velocity_update.cpp
|
||||
@ -86,6 +83,15 @@ if(CONFIG_EKF2_GNSS_YAW)
|
||||
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_MAGNETOMETER)
|
||||
list(APPEND EKF_SRCS
|
||||
mag_3d_control.cpp
|
||||
mag_control.cpp
|
||||
mag_fusion.cpp
|
||||
mag_heading_control.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
list(APPEND EKF_SRCS
|
||||
optical_flow_control.cpp
|
||||
|
||||
@ -102,8 +102,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// control use of observations for aiding
|
||||
controlMagFusion();
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
controlOpticalFlowFusion(imu_delayed);
|
||||
|
||||
@ -178,23 +178,21 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
}
|
||||
|
||||
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
|
||||
float mag_I_sig;
|
||||
float mag_I_sig = 0.0f;
|
||||
|
||||
if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
|
||||
|
||||
} else {
|
||||
mag_I_sig = 0.0f;
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
}
|
||||
|
||||
// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
|
||||
float mag_B_sig;
|
||||
float mag_B_sig = 0.0f;
|
||||
|
||||
if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
|
||||
|
||||
} else {
|
||||
mag_B_sig = 0.0f;
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
}
|
||||
|
||||
float wind_vel_nsd_scaled;
|
||||
@ -552,6 +550,7 @@ void Ekf::resetQuatCov(const float yaw_noise)
|
||||
|
||||
void Ekf::resetMagCov()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_mag_decl_cov_reset) {
|
||||
ECL_INFO("reset mag covariance");
|
||||
_mag_decl_cov_reset = false;
|
||||
@ -561,6 +560,11 @@ void Ekf::resetMagCov()
|
||||
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
|
||||
|
||||
saveMagCovData();
|
||||
#else
|
||||
P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
|
||||
P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void Ekf::resetGyroBiasZCov()
|
||||
|
||||
@ -110,7 +110,10 @@ void Ekf::reset()
|
||||
_gps_alt_ref = NAN;
|
||||
|
||||
_baro_counter = 0;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_mag_counter = 0;
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
_time_bad_vert_accel = 0;
|
||||
_time_good_vert_accel = 0;
|
||||
@ -142,8 +145,10 @@ void Ekf::reset()
|
||||
resetEstimatorAidStatus(_aid_src_gnss_yaw);
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
resetEstimatorAidStatus(_aid_src_mag_heading);
|
||||
resetEstimatorAidStatus(_aid_src_mag);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
resetEstimatorAidStatus(_aid_src_aux_vel);
|
||||
|
||||
@ -158,6 +158,7 @@ public:
|
||||
|
||||
void getHeadingInnov(float &heading_innov) const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
heading_innov = _aid_src_mag_heading.innovation;
|
||||
return;
|
||||
@ -166,6 +167,7 @@ public:
|
||||
heading_innov = Vector3f(_aid_src_mag.innovation).max();
|
||||
return;
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
@ -184,6 +186,7 @@ public:
|
||||
|
||||
void getHeadingInnovVar(float &heading_innov_var) const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
heading_innov_var = _aid_src_mag_heading.innovation_variance;
|
||||
return;
|
||||
@ -192,6 +195,7 @@ public:
|
||||
heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
|
||||
return;
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
@ -210,6 +214,7 @@ public:
|
||||
|
||||
void getHeadingInnovRatio(float &heading_innov_ratio) const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
heading_innov_ratio = _aid_src_mag_heading.test_ratio;
|
||||
return;
|
||||
@ -218,6 +223,7 @@ public:
|
||||
heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
|
||||
return;
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
@ -234,9 +240,11 @@ public:
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
|
||||
void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
|
||||
void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
|
||||
@ -351,12 +359,16 @@ public:
|
||||
|
||||
bool isYawFinalAlignComplete() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
|
||||
const bool is_mag_alignment_in_flight_complete = is_using_mag
|
||||
&& _control_status.flags.mag_aligned_in_flight
|
||||
&& ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6);
|
||||
return _control_status.flags.yaw_align
|
||||
&& (is_mag_alignment_in_flight_complete || !is_using_mag);
|
||||
#else
|
||||
return _control_status.flags.yaw_align;
|
||||
#endif
|
||||
}
|
||||
|
||||
// gyro bias (states 10, 11, 12)
|
||||
@ -369,6 +381,7 @@ public:
|
||||
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2
|
||||
float getAccelBiasLimit() const { return _params.acc_bias_lim; }
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
const Vector3f &getMagEarthField() const { return _state.mag_I; }
|
||||
|
||||
// mag bias (states 19, 20, 21)
|
||||
@ -382,6 +395,7 @@ public:
|
||||
return _saved_mag_bf_covmat.diag();
|
||||
}
|
||||
float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
|
||||
bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; }
|
||||
@ -497,8 +511,10 @@ public:
|
||||
const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
|
||||
const auto &aid_src_mag() const { return _aid_src_mag; }
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
const auto &aid_src_gravity() const { return _aid_src_gravity; }
|
||||
|
||||
@ -545,9 +561,6 @@ private:
|
||||
|
||||
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
|
||||
|
||||
float _mag_heading_prev{}; ///< previous value of mag heading (rad)
|
||||
float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad)
|
||||
|
||||
// booleans true when fresh sensor data is available at the fusion time horizon
|
||||
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
|
||||
|
||||
@ -574,19 +587,9 @@ private:
|
||||
Vector3f _zgup_delta_ang{};
|
||||
float _zgup_delta_ang_dt{0.f};
|
||||
|
||||
// used by magnetometer fusion mode selection
|
||||
Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
|
||||
float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad)
|
||||
float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec)
|
||||
bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
|
||||
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
|
||||
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
|
||||
AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
|
||||
float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
|
||||
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
|
||||
uint8_t _nb_mag_heading_reset_available{0};
|
||||
uint8_t _nb_mag_3d_reset_available{0};
|
||||
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
|
||||
|
||||
SquareMatrix24f P{}; ///< state covariance matrix
|
||||
|
||||
@ -668,9 +671,6 @@ private:
|
||||
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
estimator_aid_source1d_s _aid_src_mag_heading{};
|
||||
estimator_aid_source3d_s _aid_src_mag{};
|
||||
|
||||
estimator_aid_source3d_s _aid_src_gravity{};
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
@ -694,20 +694,40 @@ private:
|
||||
// Variables used by the initial filter alignment
|
||||
bool _is_first_imu_sample{true};
|
||||
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
|
||||
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
|
||||
AlphaFilter<Vector3f> _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s)
|
||||
AlphaFilter<Vector3f> _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
|
||||
|
||||
// Variables used to perform in flight resets and switch between height sources
|
||||
AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
|
||||
AlphaFilter<float> _baro_lpf{0.1f}; ///< filtered barometric height measurement (m)
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
float _mag_heading_prev{}; ///< previous value of mag heading (rad)
|
||||
float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad)
|
||||
|
||||
// used by magnetometer fusion mode selection
|
||||
bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
|
||||
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
|
||||
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
|
||||
AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
|
||||
float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
|
||||
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
|
||||
uint8_t _nb_mag_heading_reset_available{0};
|
||||
uint8_t _nb_mag_3d_reset_available{0};
|
||||
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
|
||||
|
||||
estimator_aid_source1d_s _aid_src_mag_heading{};
|
||||
estimator_aid_source3d_s _aid_src_mag{};
|
||||
|
||||
AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
|
||||
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
|
||||
|
||||
// Variables used to control activation of post takeoff functionality
|
||||
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
|
||||
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
|
||||
uint64_t _time_last_mag_check_failing{0};
|
||||
Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
|
||||
Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status{};
|
||||
|
||||
@ -744,9 +764,6 @@ private:
|
||||
// predict ekf covariance
|
||||
void predictCovariance(const imuSample &imu_delayed);
|
||||
|
||||
// ekf sequential fusion of magnetometer measurements
|
||||
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
|
||||
|
||||
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
|
||||
bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
|
||||
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
|
||||
@ -767,12 +784,17 @@ private:
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
void stopGpsYawFusion();
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// ekf sequential fusion of magnetometer measurements
|
||||
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
|
||||
|
||||
// fuse magnetometer declination measurement
|
||||
// argument passed in is the declination uncertainty in radians
|
||||
bool fuseDeclination(float decl_sigma);
|
||||
|
||||
// apply sensible limits to the declination and length of the NE mag field states estimates
|
||||
void limitDeclination();
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
// control fusion of air data observations
|
||||
@ -899,8 +921,10 @@ private:
|
||||
void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// Return the magnetic declination in radians to be used by the alignment and fusion processing
|
||||
float getMagDeclination();
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
void clearInhibitedStateKalmanGains(Vector24f &K) const
|
||||
{
|
||||
@ -1017,6 +1041,7 @@ private:
|
||||
bool shouldResetGpsFusion() const;
|
||||
bool isYawFailure() const;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// control fusion of magnetometer observations
|
||||
void controlMagFusion();
|
||||
void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src);
|
||||
@ -1035,6 +1060,19 @@ private:
|
||||
bool checkMagField(const Vector3f &mag);
|
||||
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
|
||||
|
||||
void stopMagHdgFusion();
|
||||
void stopMagFusion();
|
||||
|
||||
// load and save mag field state covariance data for re-use
|
||||
void loadMagCovData();
|
||||
void saveMagCovData();
|
||||
|
||||
// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
|
||||
// sensor measurement
|
||||
float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
// control fusion of fake position observations to constrain drift
|
||||
void controlFakePosFusion();
|
||||
|
||||
@ -1064,9 +1102,6 @@ private:
|
||||
void controlBaroHeightFusion();
|
||||
void controlGnssHeightFusion(const gpsSample &gps_sample);
|
||||
|
||||
void stopMagHdgFusion();
|
||||
void stopMagFusion();
|
||||
|
||||
void stopBaroHgtFusion();
|
||||
void stopGpsHgtFusion();
|
||||
|
||||
@ -1087,19 +1122,11 @@ private:
|
||||
// Argument is additional yaw variance in rad**2
|
||||
void increaseQuatYawErrVariance(float yaw_variance);
|
||||
|
||||
// load and save mag field state covariance data for re-use
|
||||
void loadMagCovData();
|
||||
void saveMagCovData();
|
||||
|
||||
void resetGyroBiasZCov();
|
||||
|
||||
// uncorrelate quaternion states from other states
|
||||
void uncorrelateQuatFromOtherStates();
|
||||
|
||||
// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
|
||||
// sensor measurement
|
||||
float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
|
||||
|
||||
bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
|
||||
{
|
||||
return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us);
|
||||
|
||||
@ -234,7 +234,10 @@ void Ekf::constrainStates()
|
||||
_state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit);
|
||||
|
||||
_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
|
||||
}
|
||||
|
||||
@ -671,6 +674,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
// return the largest magnetometer innovation test ratio
|
||||
mag = 0.f;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
|
||||
}
|
||||
@ -678,6 +682,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
if (_control_status.flags.mag_3D) {
|
||||
mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
@ -791,6 +796,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
|
||||
bool mag_innov_good = true;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
if (_aid_src_mag_heading.test_ratio < 1.f) {
|
||||
mag_innov_good = false;
|
||||
@ -801,6 +807,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
mag_innov_good = false;
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
|
||||
const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
|
||||
@ -945,26 +952,6 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
|
||||
P.slice<4, 4>(0, 0) += q_cov;
|
||||
}
|
||||
|
||||
void Ekf::saveMagCovData()
|
||||
{
|
||||
// save the NED axis covariance sub-matrix
|
||||
_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
|
||||
|
||||
// save the XYZ body covariance sub-matrix
|
||||
_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
|
||||
}
|
||||
|
||||
void Ekf::loadMagCovData()
|
||||
{
|
||||
// re-instate the NED axis covariance sub-matrix
|
||||
P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
|
||||
P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
|
||||
|
||||
// re-instate the XYZ body axis covariance sub-matrix
|
||||
P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
|
||||
P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
|
||||
}
|
||||
|
||||
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
|
||||
{
|
||||
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
||||
|
||||
@ -47,7 +47,9 @@
|
||||
EstimatorInterface::~EstimatorInterface()
|
||||
{
|
||||
delete _gps_buffer;
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
delete _mag_buffer;
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
delete _baro_buffer;
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
delete _range_buffer;
|
||||
@ -102,6 +104,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void EstimatorInterface::setMagData(const magSample &mag_sample)
|
||||
{
|
||||
if (!_initialised) {
|
||||
@ -137,6 +140,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
|
||||
ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
void EstimatorInterface::setGpsData(const gpsMessage &gps)
|
||||
{
|
||||
@ -707,9 +711,11 @@ void EstimatorInterface::print_status()
|
||||
printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_mag_buffer) {
|
||||
printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
if (_baro_buffer) {
|
||||
printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
|
||||
|
||||
@ -86,7 +86,9 @@ public:
|
||||
|
||||
void setIMUData(const imuSample &imu_sample);
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void setMagData(const magSample &mag_sample);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
void setGpsData(const gpsMessage &gps);
|
||||
|
||||
@ -231,6 +233,7 @@ public:
|
||||
Vector3f getPosition() const { return _output_predictor.getPosition(); }
|
||||
const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// Get the value of magnetic declination in degrees to be saved for use at the next startup
|
||||
// Returns true when the declination can be saved
|
||||
// At the next startup, set param.mag_declination_deg to the value saved
|
||||
@ -263,6 +266,7 @@ public:
|
||||
strength_gs = _mag_strength;
|
||||
strength_ref_gs = _mag_strength_gps;
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
// get EKF mode status
|
||||
const filter_control_status_u &control_status() const { return _control_status; }
|
||||
@ -412,7 +416,12 @@ protected:
|
||||
RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
|
||||
|
||||
RingBuffer<gpsSample> *_gps_buffer{nullptr};
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
RingBuffer<magSample> *_mag_buffer{nullptr};
|
||||
uint64_t _time_last_mag_buffer_push{0};
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
RingBuffer<baroSample> *_baro_buffer{nullptr};
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
@ -429,7 +438,6 @@ protected:
|
||||
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
|
||||
|
||||
uint64_t _time_last_gps_buffer_push{0};
|
||||
uint64_t _time_last_mag_buffer_push{0};
|
||||
uint64_t _time_last_baro_buffer_push{0};
|
||||
|
||||
uint64_t _time_last_gnd_effect_on{0};
|
||||
|
||||
@ -158,7 +158,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
|
||||
|
||||
} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
|
||||
// turn on fusion of external vision yaw measurements and disable all other heading fusion
|
||||
stopMagFusion();
|
||||
stopGpsYawFusion();
|
||||
stopGpsFusion();
|
||||
|
||||
|
||||
@ -217,3 +217,22 @@ void Ekf::stopMagFusion()
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::saveMagCovData()
|
||||
{
|
||||
// save the NED axis covariance sub-matrix
|
||||
_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
|
||||
|
||||
// save the XYZ body covariance sub-matrix
|
||||
_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
|
||||
}
|
||||
|
||||
void Ekf::loadMagCovData()
|
||||
{
|
||||
// re-instate the NED axis covariance sub-matrix
|
||||
P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
|
||||
P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
|
||||
|
||||
// re-instate the XYZ body axis covariance sub-matrix
|
||||
P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
|
||||
P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
|
||||
}
|
||||
|
||||
@ -254,114 +254,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
|
||||
return false;
|
||||
}
|
||||
|
||||
// update quaternion states and covariances using the yaw innovation and yaw observation variance
|
||||
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
|
||||
{
|
||||
Vector24f H_YAW;
|
||||
computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
|
||||
|
||||
return fuseYaw(aid_src_status, H_YAW);
|
||||
}
|
||||
|
||||
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
|
||||
{
|
||||
// define the innovation gate size
|
||||
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
|
||||
|
||||
// innovation test ratio
|
||||
setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
|
||||
|
||||
if (aid_src_status.fusion_enabled) {
|
||||
|
||||
// check if the innovation variance calculation is badly conditioned
|
||||
if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
_fault_status.flags.bad_hdg = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_hdg = true;
|
||||
|
||||
// we reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
ECL_ERR("yaw fusion numerical error - covariance reset");
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// calculate the Kalman gains
|
||||
// only calculate gains for states we are using
|
||||
Vector24f Kfusion;
|
||||
const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
|
||||
|
||||
for (uint8_t row = 0; row < _k_num_states; row++) {
|
||||
for (uint8_t col = 0; col <= 3; col++) {
|
||||
Kfusion(row) += P(row, col) * H_YAW(col);
|
||||
}
|
||||
|
||||
Kfusion(row) *= heading_innov_var_inv;
|
||||
}
|
||||
|
||||
// set the magnetometer unhealthy if the test fails
|
||||
if (aid_src_status.innovation_rejected) {
|
||||
_innov_check_fail_status.flags.reject_yaw = true;
|
||||
|
||||
// if we are in air we don't want to fuse the measurement
|
||||
// we allow to use it when on the ground because the large innovation could be caused
|
||||
// by interference or a large initial gyro bias
|
||||
if (!_control_status.flags.in_air
|
||||
&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
|
||||
&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
|
||||
) {
|
||||
// constrain the innovation to the maximum set by the gate
|
||||
// we need to delay this forced fusion to avoid starting it
|
||||
// immediately after touchdown, when the drone is still armed
|
||||
float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
|
||||
aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
|
||||
|
||||
// also reset the yaw gyro variance to converge faster and avoid
|
||||
// being stuck on a previous bad estimate
|
||||
resetGyroBiasZCov();
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_yaw = false;
|
||||
}
|
||||
|
||||
if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
|
||||
|
||||
_time_last_heading_fuse = _time_delayed_us;
|
||||
|
||||
aid_src_status.time_last_fuse = _time_delayed_us;
|
||||
aid_src_status.fused = true;
|
||||
|
||||
_fault_status.flags.bad_hdg = false;
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_hdg = true;
|
||||
}
|
||||
}
|
||||
|
||||
// otherwise
|
||||
aid_src_status.fused = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
|
||||
{
|
||||
if (shouldUse321RotationSequence(_R_to_earth)) {
|
||||
sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
|
||||
} else {
|
||||
sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::fuseDeclination(float decl_sigma)
|
||||
{
|
||||
if (!_control_status.flags.mag) {
|
||||
|
||||
147
src/modules/ekf2/EKF/yaw_fusion.cpp
Normal file
147
src/modules/ekf2/EKF/yaw_fusion.cpp
Normal file
@ -0,0 +1,147 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 "ekf.h"
|
||||
|
||||
#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
|
||||
#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
// update quaternion states and covariances using the yaw innovation and yaw observation variance
|
||||
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
|
||||
{
|
||||
Vector24f H_YAW;
|
||||
computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
|
||||
|
||||
return fuseYaw(aid_src_status, H_YAW);
|
||||
}
|
||||
|
||||
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
|
||||
{
|
||||
// define the innovation gate size
|
||||
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
|
||||
|
||||
// innovation test ratio
|
||||
setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
|
||||
|
||||
if (aid_src_status.fusion_enabled) {
|
||||
|
||||
// check if the innovation variance calculation is badly conditioned
|
||||
if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
_fault_status.flags.bad_hdg = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_hdg = true;
|
||||
|
||||
// we reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
ECL_ERR("yaw fusion numerical error - covariance reset");
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// calculate the Kalman gains
|
||||
// only calculate gains for states we are using
|
||||
Vector24f Kfusion;
|
||||
const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
|
||||
|
||||
for (uint8_t row = 0; row < _k_num_states; row++) {
|
||||
for (uint8_t col = 0; col <= 3; col++) {
|
||||
Kfusion(row) += P(row, col) * H_YAW(col);
|
||||
}
|
||||
|
||||
Kfusion(row) *= heading_innov_var_inv;
|
||||
}
|
||||
|
||||
// set the magnetometer unhealthy if the test fails
|
||||
if (aid_src_status.innovation_rejected) {
|
||||
_innov_check_fail_status.flags.reject_yaw = true;
|
||||
|
||||
// if we are in air we don't want to fuse the measurement
|
||||
// we allow to use it when on the ground because the large innovation could be caused
|
||||
// by interference or a large initial gyro bias
|
||||
if (!_control_status.flags.in_air
|
||||
&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
|
||||
&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
|
||||
) {
|
||||
// constrain the innovation to the maximum set by the gate
|
||||
// we need to delay this forced fusion to avoid starting it
|
||||
// immediately after touchdown, when the drone is still armed
|
||||
float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
|
||||
aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
|
||||
|
||||
// also reset the yaw gyro variance to converge faster and avoid
|
||||
// being stuck on a previous bad estimate
|
||||
resetGyroBiasZCov();
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_yaw = false;
|
||||
}
|
||||
|
||||
if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
|
||||
|
||||
_time_last_heading_fuse = _time_delayed_us;
|
||||
|
||||
aid_src_status.time_last_fuse = _time_delayed_us;
|
||||
aid_src_status.fused = true;
|
||||
|
||||
_fault_status.flags.bad_hdg = false;
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_hdg = true;
|
||||
}
|
||||
}
|
||||
|
||||
// otherwise
|
||||
aid_src_status.fused = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
|
||||
{
|
||||
if (shouldUse321RotationSequence(_R_to_earth)) {
|
||||
sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
|
||||
} else {
|
||||
sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
}
|
||||
}
|
||||
@ -60,7 +60,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_params(_ekf.getParamHandle()),
|
||||
_param_ekf2_predict_us(_params->filter_update_interval_us),
|
||||
_param_ekf2_imu_ctrl(_params->imu_ctrl),
|
||||
_param_ekf2_mag_delay(_params->mag_delay_ms),
|
||||
_param_ekf2_baro_delay(_params->baro_delay_ms),
|
||||
_param_ekf2_gps_delay(_params->gps_delay_ms),
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
@ -70,8 +69,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_acc_noise(_params->accel_noise),
|
||||
_param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
|
||||
_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
|
||||
_param_ekf2_mag_e_noise(_params->mage_p_noise),
|
||||
_param_ekf2_mag_b_noise(_params->magb_p_noise),
|
||||
_param_ekf2_wind_nsd(_params->wind_vel_nsd),
|
||||
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
|
||||
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
|
||||
@ -93,6 +90,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_beta_noise(_params->beta_noise),
|
||||
_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_param_ekf2_mag_delay(_params->mag_delay_ms),
|
||||
_param_ekf2_mag_e_noise(_params->mage_p_noise),
|
||||
_param_ekf2_mag_b_noise(_params->magb_p_noise),
|
||||
_param_ekf2_head_noise(_params->mag_heading_noise),
|
||||
_param_ekf2_mag_noise(_params->mag_noise),
|
||||
_param_ekf2_mag_decl(_params->mag_declination_deg),
|
||||
@ -102,6 +103,11 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_mag_type(_params->mag_fusion_type),
|
||||
_param_ekf2_mag_acclim(_params->mag_acc_gate),
|
||||
_param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate),
|
||||
_param_ekf2_mag_check(_params->mag_check),
|
||||
_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
|
||||
_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
|
||||
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
_param_ekf2_gps_check(_params->gps_check_mask),
|
||||
_param_ekf2_req_eph(_params->req_hacc),
|
||||
_param_ekf2_req_epv(_params->req_vacc),
|
||||
@ -189,10 +195,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
|
||||
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
|
||||
#endif // CONFIG_EKF2_BARO_COMPENSATION
|
||||
_param_ekf2_mag_check(_params->mag_check),
|
||||
_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
|
||||
_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
|
||||
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
|
||||
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
|
||||
{
|
||||
// advertise expected minimal topic set immediately to ensure logging
|
||||
@ -225,7 +227,9 @@ EKF2::~EKF2()
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
perf_free(_msg_missed_landing_target_pose_perf);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
perf_free(_msg_missed_magnetometer_perf);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
perf_free(_msg_missed_odometry_perf);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
perf_free(_msg_missed_optical_flow_perf);
|
||||
@ -312,6 +316,7 @@ bool EKF2::multi_init(int imu, int mag)
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
// mag advertise
|
||||
if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
|
||||
@ -319,13 +324,23 @@ bool EKF2::multi_init(int imu, int mag)
|
||||
_estimator_aid_src_mag_pub.advertise();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
_attitude_pub.advertise();
|
||||
_local_position_pub.advertise();
|
||||
_global_position_pub.advertise();
|
||||
_odometry_pub.advertise();
|
||||
_wind_pub.advertise();
|
||||
|
||||
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
|
||||
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu);
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
if (!_magnetometer_sub.ChangeInstance(mag)) {
|
||||
changed_instance = false;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
const int status_instance = _estimator_states_pub.get_instance();
|
||||
|
||||
@ -367,7 +382,9 @@ int EKF2::print_status()
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
perf_print_counter(_msg_missed_landing_target_pose_perf);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
perf_print_counter(_msg_missed_magnetometer_perf);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
perf_print_counter(_msg_missed_odometry_perf);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
perf_print_counter(_msg_missed_optical_flow_perf);
|
||||
@ -435,6 +452,8 @@ void EKF2::Run()
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
|
||||
if (_params->mag_fusion_type != MagFuseType::NONE) {
|
||||
float sens_mag_rate = 0.f;
|
||||
@ -450,6 +469,8 @@ void EKF2::Run()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
}
|
||||
|
||||
if (!_callback_registered) {
|
||||
@ -678,7 +699,9 @@ void EKF2::Run()
|
||||
UpdateFlowSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
UpdateGpsSample(ekf2_timestamps);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
UpdateMagSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
UpdateRangeSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
@ -719,7 +742,9 @@ void EKF2::Run()
|
||||
|
||||
UpdateAccelCalibration(now);
|
||||
UpdateGyroCalibration(now);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
UpdateMagCalibration(now);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
PublishSensorBias(now);
|
||||
|
||||
PublishAidSourceStatus(now);
|
||||
@ -918,6 +943,7 @@ void EKF2::VerifyParams()
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
// EKF2_MAG_TYPE obsolete options
|
||||
if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO)
|
||||
@ -935,6 +961,8 @@ void EKF2::VerifyParams()
|
||||
_param_ekf2_mag_type.set(0);
|
||||
_param_ekf2_mag_type.commit();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
}
|
||||
|
||||
void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||
@ -975,11 +1003,13 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||
PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// mag heading
|
||||
PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);
|
||||
|
||||
// mag 3d
|
||||
PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
// gravity
|
||||
PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub);
|
||||
@ -1291,7 +1321,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnov(innovations.heading);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_ekf.getMagInnov(innovations.mag_field);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
_ekf.getDragInnov(innovations.drag);
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
@ -1369,7 +1401,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovRatio(test_ratios.heading);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
_ekf.getDragInnovRatio(&test_ratios.drag[0]);
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
@ -1412,7 +1446,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovVar(variances.heading);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_ekf.getMagInnovVar(variances.mag_field);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
_ekf.getDragInnovVar(variances.drag);
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
@ -1583,12 +1619,17 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
||||
// estimator_sensor_bias
|
||||
const Vector3f gyro_bias{_ekf.getGyroBias()};
|
||||
const Vector3f accel_bias{_ekf.getAccelBias()};
|
||||
const Vector3f mag_bias{_ekf.getMagBias()};
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
const Vector3f mag_bias {_ekf.getMagBias()};
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
// publish at ~1 Hz, or sooner if there's a change
|
||||
if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
|
||||
|| (accel_bias - _last_accel_bias_published).longerThan(0.001f)
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|| (mag_bias - _last_mag_bias_published).longerThan(0.001f)
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|| (timestamp >= _last_sensor_bias_published + 1_s)) {
|
||||
|
||||
estimator_sensor_bias_s bias{};
|
||||
@ -1619,6 +1660,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
||||
_last_accel_bias_published = accel_bias;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
if (_device_id_mag != 0) {
|
||||
const Vector3f bias_var{_ekf.getMagBiasVariance()};
|
||||
|
||||
@ -1631,6 +1674,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
||||
_last_mag_bias_published = mag_bias;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_sensor_bias_pub.publish(bias);
|
||||
|
||||
@ -1695,10 +1740,13 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp)
|
||||
status.accel_device_id = _device_id_accel;
|
||||
status.baro_device_id = _device_id_baro;
|
||||
status.gyro_device_id = _device_id_gyro;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
status.mag_device_id = _device_id_mag;
|
||||
|
||||
_ekf.get_mag_checks(status.mag_inclination_deg, status.mag_inclination_ref_deg, status.mag_strength_gs,
|
||||
status.mag_strength_ref_gs);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_status_pub.publish(status);
|
||||
@ -2313,6 +2361,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
const unsigned last_generation = _magnetometer_sub.get_last_generation();
|
||||
@ -2356,6 +2405,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
(int64_t)ekf2_timestamps.timestamp / 100);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
@ -2556,6 +2606,7 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp)
|
||||
bias_valid, learning_valid);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp)
|
||||
{
|
||||
const Vector3f mag_bias = _ekf.getMagBias();
|
||||
@ -2585,6 +2636,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp)
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
int EKF2::custom_command(int argc, char *argv[])
|
||||
{
|
||||
@ -2624,6 +2676,7 @@ int EKF2::task_spawn(int argc, char *argv[])
|
||||
imu_instances = imu_instances_limited;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
int32_t sens_mag_mode = 1;
|
||||
const param_t param_sens_mag_mode = param_find("SENS_MAG_MODE");
|
||||
param_get(param_sens_mag_mode, &sens_mag_mode);
|
||||
@ -2654,6 +2707,8 @@ int EKF2::task_spawn(int argc, char *argv[])
|
||||
} else {
|
||||
mag_instances = 1;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
}
|
||||
|
||||
if (multi_mode && !replay_mode) {
|
||||
|
||||
@ -85,7 +85,6 @@
|
||||
#include <uORB/topics/vehicle_imu.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_magnetometer.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
@ -100,6 +99,10 @@
|
||||
# include <uORB/topics/landing_target_pose.h>
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
# include <uORB/topics/vehicle_magnetometer.h>
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
# include <uORB/topics/vehicle_optical_flow.h>
|
||||
# include <uORB/topics/vehicle_optical_flow_vel.h>
|
||||
@ -212,7 +215,9 @@ private:
|
||||
bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
@ -230,7 +235,9 @@ private:
|
||||
const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid);
|
||||
void UpdateAccelCalibration(const hrt_abstime ×tamp);
|
||||
void UpdateGyroCalibration(const hrt_abstime ×tamp);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void UpdateMagCalibration(const hrt_abstime ×tamp);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
// publish helper for estimator_aid_source topics
|
||||
template <typename T>
|
||||
@ -271,15 +278,10 @@ private:
|
||||
perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
|
||||
perf_counter_t _msg_missed_air_data_perf{nullptr};
|
||||
perf_counter_t _msg_missed_gps_perf{nullptr};
|
||||
perf_counter_t _msg_missed_magnetometer_perf{nullptr};
|
||||
perf_counter_t _msg_missed_odometry_perf{nullptr};
|
||||
|
||||
// Used to control saving of mag declination to be used on next startup
|
||||
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
|
||||
|
||||
InFlightCalibration _accel_cal{};
|
||||
InFlightCalibration _gyro_cal{};
|
||||
InFlightCalibration _mag_cal{};
|
||||
|
||||
uint64_t _gps_time_usec{0};
|
||||
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
|
||||
@ -289,16 +291,13 @@ private:
|
||||
uint8_t _accel_calibration_count{0};
|
||||
uint8_t _baro_calibration_count{0};
|
||||
uint8_t _gyro_calibration_count{0};
|
||||
uint8_t _mag_calibration_count{0};
|
||||
|
||||
uint32_t _device_id_accel{0};
|
||||
uint32_t _device_id_baro{0};
|
||||
uint32_t _device_id_gyro{0};
|
||||
uint32_t _device_id_mag{0};
|
||||
|
||||
Vector3f _last_accel_bias_published{};
|
||||
Vector3f _last_gyro_bias_published{};
|
||||
Vector3f _last_mag_bias_published{};
|
||||
|
||||
hrt_abstime _last_sensor_bias_published{0};
|
||||
hrt_abstime _last_gps_status_published{0};
|
||||
@ -309,6 +308,27 @@ private:
|
||||
hrt_abstime _status_fake_hgt_pub_last{0};
|
||||
hrt_abstime _status_fake_pos_pub_last{0};
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
uint32_t _device_id_mag {0};
|
||||
|
||||
perf_counter_t _msg_missed_magnetometer_perf {nullptr};
|
||||
|
||||
// Used to control saving of mag declination to be used on next startup
|
||||
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
|
||||
|
||||
InFlightCalibration _mag_cal{};
|
||||
uint8_t _mag_calibration_count{0};
|
||||
Vector3f _last_mag_bias_published{};
|
||||
|
||||
hrt_abstime _status_mag_pub_last{0};
|
||||
hrt_abstime _status_mag_heading_pub_last{0};
|
||||
|
||||
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
|
||||
@ -333,8 +353,6 @@ private:
|
||||
hrt_abstime _status_gnss_yaw_pub_last {0};
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
hrt_abstime _status_mag_pub_last{0};
|
||||
hrt_abstime _status_mag_heading_pub_last{0};
|
||||
|
||||
hrt_abstime _status_gravity_pub_last{0};
|
||||
|
||||
@ -386,7 +404,6 @@ private:
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
|
||||
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
|
||||
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
@ -448,9 +465,6 @@ private:
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
|
||||
|
||||
// publications with topic dependent on multi-mode
|
||||
@ -471,8 +485,6 @@ private:
|
||||
(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
|
||||
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
|
||||
_param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
|
||||
_param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
|
||||
@ -493,10 +505,6 @@ private:
|
||||
_param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
|
||||
(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
|
||||
_param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>)
|
||||
_param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec)
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
|
||||
_param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
|
||||
(ParamExtFloat<px4::params::EKF2_WIND_NSD>)
|
||||
_param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
|
||||
|
||||
@ -541,25 +549,24 @@ private:
|
||||
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
// control of magnetometer fusion
|
||||
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
|
||||
_param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad)
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>)
|
||||
_param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,///< magnetic declination (degrees)
|
||||
(ParamExtFloat<px4::params::EKF2_HDG_GATE>)
|
||||
_param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_GATE>)
|
||||
_param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD)
|
||||
(ParamExtInt<px4::params::EKF2_DECL_TYPE>)
|
||||
_param_ekf2_decl_type, ///< bitmask used to control the handling of declination data
|
||||
(ParamExtInt<px4::params::EKF2_MAG_TYPE>)
|
||||
_param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>)
|
||||
_param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>)
|
||||
_param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec)
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>) _param_ekf2_mag_delay,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>) _param_ekf2_mag_e_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>) _param_ekf2_mag_b_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>) _param_ekf2_head_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>) _param_ekf2_mag_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,
|
||||
(ParamExtFloat<px4::params::EKF2_HDG_GATE>) _param_ekf2_hdg_gate,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_GATE>) _param_ekf2_mag_gate,
|
||||
(ParamExtInt<px4::params::EKF2_DECL_TYPE>) _param_ekf2_decl_type,
|
||||
(ParamExtInt<px4::params::EKF2_MAG_TYPE>) _param_ekf2_mag_type,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>) _param_ekf2_mag_acclim,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>) _param_ekf2_mag_yawlim,
|
||||
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
|
||||
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synthetic_mag_z,
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
|
||||
_param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used
|
||||
@ -736,11 +743,6 @@ private:
|
||||
#endif // CONFIG_EKF2_BARO_COMPENSATION
|
||||
|
||||
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
|
||||
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
|
||||
(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
|
||||
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>)
|
||||
_param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone.
|
||||
|
||||
// Used by EKF-GSF experimental yaw estimator
|
||||
(ParamExtFloat<px4::params::EKF2_GSF_TAS>)
|
||||
|
||||
@ -56,6 +56,13 @@ depends on MODULES_EKF2
|
||||
---help---
|
||||
EKF2 GNSS yaw fusion support.
|
||||
|
||||
menuconfig EKF2_MAGNETOMETER
|
||||
depends on MODULES_EKF2
|
||||
bool "magnetometer support"
|
||||
default y
|
||||
---help---
|
||||
EKF2 magnetometer support.
|
||||
|
||||
menuconfig EKF2_OPTICAL_FLOW
|
||||
depends on MODULES_EKF2
|
||||
bool "optical flow fusion support"
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user