Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: removed inhibitGps and inhibitGpsVertVel options

Browse Source 
these were unused
This commit is contained in:
Andrew Tridgell 2020-11-21 06:38:49 +11:00
parent 281e2cb693
commit 7313e9eb0f
7 changed files with 6 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1023,34 +1023,6 @@ bool NavEKF2::resetHeightDatum(void)
return status;
}
// Commands the EKF to not use GPS.
// This command must be sent prior to arming as it will only be actioned when the filter is in static mode
// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
// Returns 0 if command rejected
// Returns 1 if attitude, vertical velocity and vertical position will be provided
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t NavEKF2::setInhibitGPS(void)
{
AP::dal().log_event2(AP_DAL::Event::setInhibitGPS);
if (!core) {
return 0;
}
return core[primary].setInhibitGPS();
}
// Set the argument to true to prevent the EKF using the GPS vertical velocity
// This can be used for situations where GPS velocity errors are causing problems with height accuracy
void NavEKF2::setInhibitGpsVertVelUse(const bool varIn) {
if (varIn) {
AP::dal().log_event2(AP_DAL::Event::setInhibitGpsVertVelUse);
} else {
AP::dal().log_event2(AP_DAL::Event::unsetInhibitGpsVertVelUse);
}
inhibitGpsVertVelUse = varIn;
};
// return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const

14
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -114,18 +114,6 @@ public:
// If using a range finder for height no reset is performed and it returns false
bool resetHeightDatum(void);
// Commands the EKF to not use GPS.
// This command must be sent prior to arming as it will only be actioned when the filter is in static mode
// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
// Returns 0 if command rejected
// Returns 1 if attitude, vertical velocity and vertical position will be provided
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t setInhibitGPS(void);
// Set the argument to true to prevent the EKF using the GPS vertical velocity
// This can be used for situations where GPS velocity errors are causing problems with height accuracy
void setInhibitGpsVertVelUse(const bool varIn);
// return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
@ -504,8 +492,6 @@ private:
bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
bool inhibitGpsVertVelUse; // true when GPS vertical velocity use is prohibited
// time of last lane switch
uint32_t lastLaneSwitch_ms;

23
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -162,7 +162,7 @@ void NavEKF2_core::setAidingMode()
bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
// GPS aiding is the preferred option unless excluded by the user
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable);
bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
bool canUseExtNav = readyToUseExtNav();
if(canUseGPS || canUseRangeBeacon || canUseExtNav) {
@ -180,7 +180,7 @@ void NavEKF2_core::setAidingMode()
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS is available
// If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowSensorTimeout || flowFusionTimeout) {
PV_AidingMode = AID_NONE;
@ -299,7 +299,7 @@ void NavEKF2_core::setAidingMode()
break;
case AID_ABSOLUTE: {
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS());
bool canUseRangeBeacon = readyToUseRangeBeacon();
bool canUseExtNav = readyToUseExtNav();
// We have commenced aiding and GPS usage is allowed
@ -484,23 +484,6 @@ bool NavEKF2_core::checkGyroCalStatus(void)
return delAngBiasLearned;
}
// Commands the EKF to not use GPS.
// This command must be sent prior to arming
// This command is forgotten by the EKF each time the vehicle disarms
// Returns 0 if command rejected
// Returns 1 if attitude, vertical velocity and vertical position will be provided
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t NavEKF2_core::setInhibitGPS(void)
{
if((PV_AidingMode == AID_ABSOLUTE) && motorsArmed) {
return 0;
} else {
gpsInhibit = true;
return 1;
}
// option 2 is not yet implemented as it requires a deeper integration of optical flow and GPS operation
}
// Update the filter status
void NavEKF2_core::updateFilterStatus(void)
{

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -569,7 +569,7 @@ void NavEKF2_core::readGpsData()
}
// Check if GPS can output vertical velocity, if it is allowed to be used, and set GPS fusion mode accordingly
if (gps.have_vertical_velocity() && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
if (gps.have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;

4
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -220,7 +220,7 @@ void NavEKF2_core::ResetHeight(void)
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
// Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 &&
dal.gps().have_vertical_velocity() && !frontend->inhibitGpsVertVelUse) {
dal.gps().have_vertical_velocity()) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel) {
stateStruct.velocity.z = extNavVelNew.vel.z;
@ -707,7 +707,7 @@ void NavEKF2_core::FuseVelPosNED()
uint8_t imax = 2;
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
if (!useExtNavVel && (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE ||
frontend->inhibitGpsVertVelUse || !dal.gps().have_vertical_velocity())) {
!dal.gps().have_vertical_velocity())) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations

1
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -246,7 +246,6 @@ void NavEKF2_core::InitialiseVariables()
quatAtLastMagReset = stateStruct.quat;
delAngBiasLearned = false;
memset(&filterStatus, 0, sizeof(filterStatus));
gpsInhibit = false;
activeHgtSource = 0;
memset(&rngMeasIndex, 0, sizeof(rngMeasIndex));
memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms));

9
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -129,14 +129,6 @@ public:
// If using a range finder for height no reset is performed and it returns false
bool resetHeightDatum(void);
// Commands the EKF to not use GPS.
// This command must be sent prior to arming as it will only be actioned when the filter is in static mode
// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
// Returns 0 if command rejected
// Returns 1 if attitude, vertical velocity and vertical position will be provided
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t setInhibitGPS(void);
// return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
@ -1043,7 +1035,6 @@ private:
};
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions
bool gpsInhibit; // externally set flag informing the EKF not to use the GPS
bool gndOffsetValid; // true when the ground offset state can still be considered valid
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
float delTimeOF; // time that delAngBodyOF is summed across