Browse Source

AP_NavEKF2: Allow immediate use of GPS in-flight for plane without compass

Planes operating without a compass must align the yaw to the GPS heading immediately after launch or takeoff and cannot wait for GPS accuracy checks.
master
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
commit
cf8175a073
  1. 6
      libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

6
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

@ -24,6 +24,12 @@ extern const AP_HAL::HAL& hal; @@ -24,6 +24,12 @@ extern const AP_HAL::HAL& hal;
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
if (inFlight && assume_zero_sideslip() && !use_compass()) {
// this is a special case where a plane has launched without magnetometer
// is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
return true;
}
// User defined multiplier to be applied to check thresholds
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;

Loading…
Cancel
Save