Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Allow operation with EK3_SRCx_POSZ = 0 (NONE)

This commit is contained in:
Paul Riseborough 2023-08-20 17:30:27 +10:00 committed by Andrew Tridgell
parent 2572c9be48
commit 5aa7bd0b7a
2 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -207,8 +207,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: ALT_M_NSE // @Param: ALT_M_NSE
// @DisplayName: Altitude measurement noise (m) // @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. // @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 10.0 // @Range: 0.1 100.0
// @Increment: 0.1 // @Increment: 0.1
// @User: Advanced // @User: Advanced
// @Units: m // @Units: m

17
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1151,7 +1151,10 @@ void NavEKF3_core::selectHeightForFusion()
const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500); const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500);
#endif #endif
// select height source // 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 // user has specified the range finder as a primary height source
activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; 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) { } 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; posDownObsNoise *= frontend->gndEffectBaroScaler;
} }
velPosObs[5] = -hgtMea; 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 { } else {
fuseHgtData = false; fuseHgtData = false;
} }