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

AP_NavEKF: add takeoffExpected and touchdownExpected

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-20 15:41:42 -07:00 committed by Randy Mackay
parent b6b55bf6f2
commit 8c92524b8a
2 changed files with 62 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -413,10 +413,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
fScaleFactorPnoise(1e-10f), // Process noise added to focal length scale factor state variance at each time step
flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec)
gndEffectTO_ms(30000), // time in msec that baro ground effect compensation will timeout after initiation
gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect
gndEffectBaroTO_ms(5000), // time in msec that the baro measurement will be rejected if the gndEffectBaroVarLim has failed it
flowIntervalMax_ms(100) // maximum allowable time between flow fusion events
flowIntervalMax_ms(100), // maximum allowable time between flow fusion events
gndEffectTimeout_ms(1000), // time in msec that baro ground effect compensation will timeout after initiation
gndEffectBaroScaler(4.0f) // scaler applied to the barometer observation variance when operating in ground effect
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@ -1965,7 +1964,7 @@ void NavEKF::FuseVelPosNED()
R_OBS[5] = sq(constrain_float(_baroAltNoise, 0.1f, 10.0f));
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
if (getGndEffectMode() && vehicleArmed) {
if ((getTakeoffExpected() || getTouchdownExpected()) && vehicleArmed) {
R_OBS[5] *= gndEffectBaroScaler;
}
@ -4569,7 +4568,10 @@ void NavEKF::InitialiseVariables()
gndOffsetValid = false;
flowXfailed = false;
validOrigin = false;
gndEffectMode = false;
takeoffExpectedSet_ms = 0;
takeoffExpected = false;
touchdownExpectedSet_ms = 0;
touchdownExpected = false;
gpsSpdAccuracy = 0.0f;
baroHgtOffset = 0.0f;
gpsAidingBad = false;
@ -4913,32 +4915,41 @@ bool NavEKF::setOriginLLH(struct Location &loc)
return true;
}
// determine if the baro ground effect compensation is active
bool NavEKF::getGndEffectMode(void)
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
bool NavEKF::getTakeoffExpected()
{
if (gndEffectMode && (imuSampleTime_ms - startTimeTO_ms < gndEffectTO_ms) && gndEffectTO_ms != 0) {
gndEffectMode = true;
} else {
gndEffectMode = false;
startTimeTO_ms = 0;
if (takeoffExpected && imuSampleTime_ms - takeoffExpectedSet_ms > gndEffectTimeout_ms) {
takeoffExpected = false;
}
return gndEffectMode;
return takeoffExpected;
}
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF::setTakeoffExpected(bool val)
{
takeoffExpectedSet_ms = imuSampleTime_ms;
takeoffExpected = val;
}
// Tell the EKF to de-weight the baro sensor to take account of ground effect on baro during takeoff of landing when set to true
// Should be set to off by sending a false when the ground effect height region is cleared or the landing completed
// If not reactivated, this condition times out after the number of msec set by the _gndEffectTO_ms parameter
// The amount of de-weighting is controlled by the gndEffectBaroScaler parameter
void NavEKF::setGndEffectMode(bool setMode)
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
bool NavEKF::getTouchdownExpected()
{
if (setMode) {
startTimeTO_ms = imuSampleTime_ms;
gndEffectMode = true;
} else {
gndEffectMode = true;
startTimeTO_ms = 0;
if (touchdownExpected && imuSampleTime_ms - touchdownExpectedSet_ms > gndEffectTimeout_ms) {
touchdownExpected = false;
}
return touchdownExpected;
}
// called by vehicle code to specify that a touchdown is expected to happen
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF::setTouchdownExpected(bool val)
{
touchdownExpectedSet_ms = imuSampleTime_ms;
touchdownExpected = val;
}
// Monitor GPS data to see if quality is good enough to initialise the EKF

40
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -200,11 +200,13 @@ public:
// return data for debugging optical flow fusion
void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
// Tells the EKF to de-weight the baro sensor to take account of ground effect on baro during takeoff of landing when set to true
// Should be set to off by sending a false when the ground effect height region is cleared or the landing completed
// If not reactivated, this condition times out after the number of msec set by the _gndEffectTO_ms parameter
// The amount of de-weighting is controlled by the gndEffectBaroScaler parameter
void setGndEffectMode(bool modeOn);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTakeoffExpected(bool val);
// called by vehicle code to specify that a touchdown is expected to happen
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTouchdownExpected(bool val);
/*
return the filter fault status as a bitmasked integer
@ -427,9 +429,11 @@ private:
// Set the NED origin to be used until the next filter reset
void setOrigin();
// get status of baro ground effect compensation mode
// returns true when baro ground effect compensation is required
bool getGndEffectMode(void);
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
bool getTakeoffExpected();
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
bool getTouchdownExpected();
// Assess GPS data quality and return true if good enough to align the EKF
bool calcGpsGoodToAlign(void);
@ -479,9 +483,6 @@ private:
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
const uint16_t gndEffectTO_ms; // time in msec that ground effect mode is active after being activated
const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active
const float gndEffectBaroTO_ms; // time in msec that the baro measurement will be rejected if the gndEffectBaroVarLim has failed it
const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
const float msecHgtDelay; // Height measurement delay (msec)
const uint16_t msecMagDelay; // Magnetometer measurement delay (msec)
@ -511,6 +512,12 @@ private:
const uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec)
const uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events
// ground effect tuning parameters
const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated
const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active
// Variables
bool statesInitialised; // boolean true when filter states have been initialised
bool velHealth; // boolean true if velocity measurements have passed innovation consistency check
@ -640,9 +647,6 @@ private:
bool prevVehicleArmed; // vehicleArmed from previous frame
struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset
bool validOrigin; // true when the EKF origin is valid
bool gndEffectMode; // modifiy baro processing to compensate for ground effect
uint32_t startTimeTO_ms; // time in msec that the takeoff condition started - used to compensate for ground effect on baro height
uint32_t startTimeLAND_ms; // time in msec that the landing condition started - used to compensate for ground effect on baro height
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
Vector3f lastMagOffsets; // magnetometer offsets returned by compass object from previous update
@ -725,9 +729,17 @@ private:
float rangeAtArming; // range finder measurement when armed
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
// IMU processing
bool haveDeltaAngles;
float dtDelVel1;
float dtDelVel2;
// baro ground effect
bool takeoffExpected; // external state from ArduCopter - takeoff expected
uint32_t takeoffExpectedSet_ms; // system time at which takeoffExpected was set
bool touchdownExpected; // external state from ArduCopter - touchdown expected
uint32_t touchdownExpectedSet_ms; // system time at which touchdownExpected was set
// states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps
// to level computational load as this can be an expensive operation