Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

ekf2: merge mag yaw angle observability into heading consistency

Browse Source 
- the additional hyteresis logic for "yaw angle observable" was
   needed when it controlled time dependent mag_3d
This commit is contained in:
Daniel Agar
2024-06-03 11:01:20 -04:00
parent bc51eb37eb
commit bfe3c86aeb
3 changed files with 4 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld
-1
View File
@@ -683,7 +683,6 @@
*(.text._ZN18MavlinkStreamDebug8get_sizeEv) *(.text._ZN18MavlinkStreamDebug8get_sizeEv)
*(.text._ZN12GPSDriverUBX7receiveEj) *(.text._ZN12GPSDriverUBX7receiveEj)
*(.text._ZN13BatteryStatus21parameter_update_pollEb) *(.text._ZN13BatteryStatus21parameter_update_pollEb)
*(.text._ZN3Ekf26checkYawAngleObservabilityEv)
*(.text._ZN3RTL18updateDatamanCacheEv) *(.text._ZN3RTL18updateDatamanCacheEv)
*(.text.__ieee754_sqrtf) *(.text.__ieee754_sqrtf)
*(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv) *(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv)
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+4 -20
View File
@@ -52,8 +52,6 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_aligned_in_flight = false; _control_status.flags.mag_aligned_in_flight = false;
} }
checkYawAngleObservability();
if (_params.mag_fusion_type == MagFuseType::NONE) { if (_params.mag_fusion_type == MagFuseType::NONE) {
stopMagFusion(); stopMagFusion();
return; return;
@@ -391,23 +389,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
} }
} }
void Ekf::checkYawAngleObservability()
{
if (_control_status.flags.gps) {
// Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling
if (_yaw_angle_observable) {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
} else {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
}
} else {
_yaw_angle_observable = false;
}
}
void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
{ {
// use mag bias if variance good // use mag bias if variance good
@@ -437,7 +418,10 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
} }
if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
if (_yaw_angle_observable) { // Check if there has been enough change in horizontal velocity to make yaw observable
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) {
// yaw angle must be observable to consider consistency // yaw angle must be observable to consider consistency
_control_status.flags.mag_heading_consistent = true; _control_status.flags.mag_heading_consistent = true;
} }
src/modules/ekf2/EKF/ekf.h
-2
View File
@@ -703,7 +703,6 @@ private:
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
// used by magnetometer fusion mode selection // used by magnetometer fusion mode selection
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
AlphaFilter<float> _mag_heading_innov_lpf{0.1f}; AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
@@ -1028,7 +1027,6 @@ private:
void resetMagStates(const Vector3f &mag, bool reset_heading = true); void resetMagStates(const Vector3f &mag, bool reset_heading = true);
bool haglYawResetReq(); bool haglYawResetReq();
void checkYawAngleObservability();
void checkMagHeadingConsistency(const magSample &mag_sample); void checkMagHeadingConsistency(const magSample &mag_sample);
bool checkMagField(const Vector3f &mag); bool checkMagField(const Vector3f &mag);