From 5aa7bd0b7a045739686030c5ea9e60f595dffdfd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 20 Aug 2023 17:30:27 +1000 Subject: [PATCH] AP_NavEKF3: Allow operation with EK3_SRCx_POSZ = 0 (NONE) --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 4 ++-- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 17 ++++++++++++++++- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 0ca9aa2a6c..d7d4d3c61a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -207,8 +207,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: ALT_M_NSE // @DisplayName: Altitude measurement noise (m) - // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors. - // @Range: 0.1 10.0 + // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors. A larger value for EK3_ALT_M_NSE may be required when operating with EK3_SRCx_POSZ = 0. This parameter also sets the noise for the 'synthetic' zero height measurement that is used when EK3_SRCx_POSZ = 0. + // @Range: 0.1 100.0 // @Increment: 0.1 // @User: Advanced // @Units: m diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index af1746c4d8..ee735a27d4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1151,7 +1151,10 @@ void NavEKF3_core::selectHeightForFusion() const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500); #endif // select height source - if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { + if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::NONE)) { + // user has specified no height sensor + activeHgtSource = AP_NavEKF_Source::SourceZ::NONE; + } else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { // user has specified the range finder as a primary height source activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) { @@ -1305,6 +1308,18 @@ void NavEKF3_core::selectHeightForFusion() posDownObsNoise *= frontend->gndEffectBaroScaler; } velPosObs[5] = -hgtMea; + } else if ((activeHgtSource == AP_NavEKF_Source::SourceZ::NONE && imuSampleTime_ms - lastHgtPassTime_ms > 70)) { + // fuse a constant height of 0 at 14 Hz + hgtMea = 0.0f; + fuseHgtData = true; + velPosObs[5] = -hgtMea; + if (onGround) { + // use a typical vertical positoin observation noise when not flying for faster IMU delta velocity bias estimation + posDownObsNoise = sq(2.0f); + } else { + // alow a larger value when flying to accomodate vertical maneouvres + posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 2.0f, 100.0f)); + } } else { fuseHgtData = false; }