From 84693ea3c4f9f74d522cce804eb796d35e91a091 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 17 Jul 2021 18:31:56 +1000 Subject: [PATCH] AP_NavEKF3: Revert EK3_BETA_MASK parameter extension These are not required due to use of bit 7 in FLIGHT_OPTIONS to achieve the same function. --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 8 ++++---- libraries/AP_NavEKF3/AP_NavEKF3.h | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 17f496d8c3..d3a05338d9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -416,13 +416,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Units: m AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF3, _noaidHorizNoise, 10.0f), - // @Param: ADATA_MASK + // @Param: BETA_MASK // @DisplayName: Bitmask controlling sidelip angle fusion - // @Description: 1 byte bitmap controlling use of sideslip angle and estimated airspeed during operation of 'fly forward' vehicle types such as fixed wing planes. By assuming that the angle of sideslip is small, the wind velocity state estimates are corrected whenever the EKF is not dead reckoning (e.g. has an independent velocity or position sensor such as GPS). This behaviour is on by default and cannot be disabled. When the EKF is dead reckoning, the wind states are used as a reference, enabling use of the small angle of sideslip assumption to correct non wind velocity states (eg attitude, velocity, position, etc) and improve navigation accuracy. This behaviour is on by default and cannot be disabled. The behaviour controlled by this parameter is the use of the small angle of sideslip assumption and the use of the estimated airspeed (average of ARSPD_FBW_MIN and ARSPD_FBW_MAX) to correct non wind velocity states when the EKF is NOT dead reckoning. This is primarily of benefit to reduce the buildup of yaw angle errors during straight and level flight without a yaw sensor (e.g. magnetometer or dual antenna GPS yaw) provided aerobatic flight maneuvers with large sideslip angles are not performed. The 'SideslipAlways' option might be used where the yaw sensor is intentionally not fitted or disabled. The 'SideslipWhenFailedYawSensor' option might be used if a yaw sensor is fitted, but protection against in-flight failure and continual rejection by the EKF is desired. For vehicles operated within visual range of the operator performing frequent turning maneuvers, setting this parameter is unnecessary. The AirspeedAlways option might be used when operating without an airspeed sensor and when the TRIM_THROTTLE has been adjusted to fly close to the average of ARSPD_FBW_MIN and ARSPD_FBW_MAX. - // @Bitmask: 0:SideslipAlways,1:SideslipWhenFailedYawSensor,2:AirspeedAlways + // @Description: 1 byte bitmap controlling use of sideslip angle fusion for estimation of non wind states during operation of 'fly forward' vehicle types such as fixed wing planes. By assuming that the angle of sideslip is small, the wind velocity state estimates are corrected whenever the EKF is not dead reckoning (e.g. has an independent velocity or position sensor such as GPS). This behaviour is on by default and cannot be disabled. When the EKF is dead reckoning, the wind states are used as a reference, enabling use of the small angle of sideslip assumption to correct non wind velocity states (eg attitude, velocity, position, etc) and improve navigation accuracy. This behaviour is on by default and cannot be disabled. The behaviour controlled by this parameter is the use of the small angle of sideslip assumption to correct non wind velocity states when the EKF is NOT dead reckoning. This is primarily of benefit to reduce the buildup of yaw angle errors during straight and level flight without a yaw sensor (e.g. magnetometer or dual antenna GPS yaw) provided aerobatic flight maneuvers with large sideslip angles are not performed. The 'always' option might be used where the yaw sensor is intentionally not fitted or disabled. The 'WhenNoYawSensor' option might be used if a yaw sensor is fitted, but protection against in-flight failure and continual rejection by the EKF is desired. For vehicles operated within visual range of the operator performing frequent turning maneuvers, setting this parameter is unnecessary. + // @Bitmask: 0:Always,1:WhenNoYawSensor // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("ADATA_MASK", 36, NavEKF3, _airDataMask, 0), + AP_GROUPINFO("BETA_MASK", 36, NavEKF3, _betaMask, 0), // control of magnetic yaw angle fusion diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 508ce7262d..c35e9b7daa 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -425,7 +425,7 @@ private: AP_Float _ballisticCoef_x; // ballistic coefficient measured for flow in X body frame directions AP_Float _ballisticCoef_y; // ballistic coefficient measured for flow in Y body frame directions AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient - AP_Int8 _airDataMask; // Bitmask controlling when sideslip angle and default airspeed fusion is used to estimate non wind states + AP_Int8 _betaMask; // Bitmask controlling when sideslip angle fusion is used to estimate non wind states AP_Float _ognmTestScaleFactor; // Scale factor applied to the thresholds used by the on ground not moving test AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 6952165fa5..5aabf6a3f1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -237,7 +237,7 @@ void NavEKF3_core::SelectBetaDragFusion() // use of air data to constrain drift is necessary if we have limited sensor data or are doing inertial dead reckoning bool is_dead_reckoning = ((imuSampleTime_ms - lastPosPassTime_ms) > frontend->deadReckonDeclare_ms) && ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms); const bool noYawSensor = !use_compass() && !using_external_yaw(); - const bool f_required = (noYawSensor && (frontend->_airDataMask & (1<<1))) || is_dead_reckoning; + const bool f_required = (noYawSensor && (frontend->_betaMask & (1<<1))) || is_dead_reckoning; // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states) const bool f_beta_feasible = (assume_zero_sideslip() && !inhibitWindStates); @@ -245,7 +245,7 @@ void NavEKF3_core::SelectBetaDragFusion() // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion if (f_beta_feasible && f_timeTrigger) { // unless air data is required to constrain drift, it is only used to update wind state estimates - if (f_required || (frontend->_airDataMask & (1<<0))) { + if (f_required || (frontend->_betaMask & (1<<0))) { // we are required to correct all states airDataFusionWindOnly = false; } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 690c691791..91375edc54 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -77,7 +77,7 @@ void NavEKF3_core::setWindMagStateLearningMode() Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); ftype trueAirspeedVariance; - const bool haveAirspeedMeasurement = usingDefaultAirspeed || imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500; + const bool haveAirspeedMeasurement = usingDefaultAirspeed || (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500 && useAirspeed()); if (haveAirspeedMeasurement) { trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX); const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index dd0b1b4f3e..c5261b30f2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -862,7 +862,7 @@ void NavEKF3_core::readAirSpdData() tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar)); tasDataDelayed.time_ms = 0; - usingDefaultAirspeed = frontend->_airDataMask & (1<<2); + usingDefaultAirspeed = true; } else { usingDefaultAirspeed = false; }