aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: 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 7313e9eb0f
commit 80f7906744
7 changed files with 4 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1188,20 +1188,6 @@ bool NavEKF3::resetHeightDatum(void)
return status;
}
// Commands the EKF to not use GPS.
// This command must be sent prior to vehicle arming and EKF commencement of GPS usage
// Returns 0 if command rejected
// Returns 1 if command accepted
uint8_t NavEKF3::setInhibitGPS(void)
{
AP::dal().log_event3(AP_DAL::Event::setInhibitGPS);
if (!core) {
return 0;
}
return core[primary].setInhibitGPS();
}
// 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 NavEKF3::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const

12
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -113,16 +113,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 vehicle arming and EKF commencement of GPS usage
// Returns 0 if command rejected
// Returns 1 if command accepted
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) { 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 getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
@ -543,8 +533,6 @@ private:
float coreErrorScores[MAX_EKF_CORES]; // the instance error values used to update relative core error
uint64_t coreLastTimePrimary_us[MAX_EKF_CORES]; // last time we were using this core as primary
bool inhibitGpsVertVelUse; // true when GPS vertical velocity use is prohibited
// origin set by one of the cores
struct Location common_EKF_origin;
bool common_origin_valid;

16
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -485,7 +485,7 @@ bool NavEKF3_core::readyToUseGPS(void) const
return false;
}
return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && gpsDataToFuse && !gpsInhibit;
return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && gpsDataToFuse;
}
// return true if the filter to be ready to use the beacon range measurements
@ -609,20 +609,6 @@ void NavEKF3_core::checkGyroCalStatus(void)
}
}
// Commands the EKF to not use GPS.
// This command must be sent prior to vehicle arming and EKF commencement of GPS usage
// Returns 0 if command rejected
// Returns 1 if command accepted
uint8_t NavEKF3_core::setInhibitGPS(void)
{
if((PV_AidingMode == AID_ABSOLUTE) || motorsArmed) {
return 0;
} else {
gpsInhibit = true;
return 1;
}
}
// Update the filter status
void NavEKF3_core::updateFilterStatus(void)
{

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -625,7 +625,7 @@ void NavEKF3_core::readGpsData()
}
// Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
if (gps.have_vertical_velocity(selected_gps) && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
if (gps.have_vertical_velocity(selected_gps) && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS)) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;

4
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -246,7 +246,7 @@ void NavEKF3_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->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
!frontend->inhibitGpsVertVelUse && dal.gps().have_vertical_velocity(selected_gps)) {
dal.gps().have_vertical_velocity(selected_gps)) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
stateStruct.velocity.z = extNavVelDelayed.vel.z;
@ -722,7 +722,7 @@ void NavEKF3_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 ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE ||
frontend->inhibitGpsVertVelUse || !dal.gps().have_vertical_velocity(selected_gps)) && !useExtNavVel) {
!dal.gps().have_vertical_velocity(selected_gps)) && !useExtNavVel) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -316,7 +316,6 @@ void NavEKF3_core::InitialiseVariables()
gpsYawResetRequest = false;
delAngBiasLearned = false;
memset(&filterStatus, 0, sizeof(filterStatus));
gpsInhibit = false;
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
prevHgtSource = activeHgtSource;
memset(&rngMeasIndex, 0, sizeof(rngMeasIndex));

7
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -143,12 +143,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 vehicle arming and EKF commencement of GPS usage
// Returns 0 if command rejected
// Returns 1 if command accepted
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;
@ -1175,7 +1169,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