Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Paul Riseborough 2021-07-17 18:31:56 +10:00 committed by Randy Mackay
parent 645386cba6
commit 84693ea3c4
5 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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

2
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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)

4
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -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 {

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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;

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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;
}