Browse Source

AP_NavEKF2: Only allow rangefinder height option during optical flow nav

mission-4.1.18
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
commit
b8427e5d95
  1. 2
      libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

2
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@ -544,7 +544,7 @@ void NavEKF2_core::selectHeightForFusion() @@ -544,7 +544,7 @@ void NavEKF2_core::selectHeightForFusion()
baroDataToFuse = RecallBaro();
// determine if we should be using a height source other than baro
bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500);
bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500 && frontend->_fusionModeGPS == 3);
bool usingGpsForHgt = (frontend->_altSource == 2 && imuSampleTime_ms - lastTimeGpsReceived_ms < 500);
// if there is new baro data to fuse, calculate filterred baro data required by other processes

Loading…
Cancel
Save