Browse Source

AP_NavEKF2: cope with INS_MAX_INSTANCES below 3

zr-v5.1
Andrew Tridgell 4 years ago
parent
commit
246a441883
  1. 5
      libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

5
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

@ -482,10 +482,13 @@ void NavEKF2_core::detectOptFlowTakeoff(void) @@ -482,10 +482,13 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
Vector3f angRateVec;
Vector3f gyroBias;
getGyroBias(gyroBias);
#if INS_MAX_INSTANCES > 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 {
} else
#endif
{
angRateVec = ins.get_gyro() - gyroBias;
}

Loading…
Cancel
Save