Browse Source

AP_NavEKF3: don't use disabled gyros in opticalflow takeoff detection

master
Andrew Tridgell 6 years ago
parent
commit
2208689893
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@ -454,7 +454,7 @@ void NavEKF3_core::detectOptFlowTakeoff(void) @@ -454,7 +454,7 @@ void NavEKF3_core::detectOptFlowTakeoff(void)
Vector3f angRateVec;
Vector3f gyroBias;
getGyroBias(gyroBias);
bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1);
bool dual_ins = ins.use_gyro(0) && ins.use_gyro(1);
if (dual_ins) {
angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias;
} else {

Loading…
Cancel
Save