Browse Source

AP_NavEKF3: Fix bug preventing copter wind estimation at low speed

Also re-tunes process noise default for smoother wind velocity state estimates.
gps-1.3.1
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
commit
8fd1e98701
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 2
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -39,7 +39,7 @@
#define FLOW_I_GATE_DEFAULT 300 #define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100 #define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_DEFAULT 1 #define FLOW_USE_DEFAULT 1
#define WIND_P_NSE_DEFAULT 1.0 #define WIND_P_NSE_DEFAULT 0.5
#elif APM_BUILD_TYPE(APM_BUILD_Rover) #elif APM_BUILD_TYPE(APM_BUILD_Rover)
// rover defaults // rover defaults

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -66,7 +66,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
inhibitWindStates = true; inhibitWindStates = true;
updateStateIndexLim(); updateStateIndexLim();
} else if (inhibitWindStates && canEstimateWind && } else if (inhibitWindStates && canEstimateWind &&
sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f)) { (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) {
inhibitWindStates = false; inhibitWindStates = false;
updateStateIndexLim(); updateStateIndexLim();
// set states and variances // set states and variances

Loading…
Cancel
Save