zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: add takeoffExpected and touchdownExpected

master
Jonathan Challinger 10 years ago committed by Randy Mackay
parent
b6b55bf6f2
commit
8c92524b8a
2 changed files with 62 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 61
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 40
      libraries/AP_NavEKF/AP_NavEKF.h

61
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape View File

@ -413,10 +413,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) : @@ -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() @@ -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() @@ -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) @@ -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
Unescape View File

@ -200,11 +200,13 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save