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

NavEKF: use new perf counter API

Browse Source
This commit is contained in:
Andrew Tridgell 2015-10-20 16:42:40 +11:00
parent 8dbb51c64e
commit f66966ccae
2 changed files with 35 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -443,19 +443,8 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
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 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 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")),
_perf_CovariancePrediction(perf_alloc(PC_ELAPSED, "EKF_CovariancePrediction")),
_perf_FuseVelPosNED(perf_alloc(PC_ELAPSED, "EKF_FuseVelPosNED")),
_perf_FuseMagnetometer(perf_alloc(PC_ELAPSED, "EKF_FuseMagnetometer")),
_perf_FuseAirspeed(perf_alloc(PC_ELAPSED, "EKF_FuseAirspeed")),
_perf_FuseSideslip(perf_alloc(PC_ELAPSED, "EKF_FuseSideslip")),
_perf_OpticalFlowEKF(perf_alloc(PC_ELAPSED, "EKF_FuseOptFlow"))
#endif
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
// Check basic filter health metrics and return a consolidated health status // Check basic filter health metrics and return a consolidated health status
@ -728,7 +717,7 @@ void NavEKF::UpdateFilter()
#if EKF_DISABLE_INTERRUPTS #if EKF_DISABLE_INTERRUPTS
irqstate_t istate = irqsave(); irqstate_t istate = irqsave();
#endif #endif
perf_begin(_perf_UpdateFilter); hal.util->perf_begin(_perf_UpdateFilter);
//get starting time for update step //get starting time for update step
imuSampleTime_ms = hal.scheduler->millis(); imuSampleTime_ms = hal.scheduler->millis();
@ -798,7 +787,7 @@ void NavEKF::UpdateFilter()
end: end:
// stop the timer used for load measurement // stop the timer used for load measurement
perf_end(_perf_UpdateFilter); hal.util->perf_end(_perf_UpdateFilter);
#if EKF_DISABLE_INTERRUPTS #if EKF_DISABLE_INTERRUPTS
irqrestore(istate); irqrestore(istate);
#endif #endif
@ -985,7 +974,7 @@ void NavEKF::SelectVelPosFusion()
void NavEKF::SelectMagFusion() void NavEKF::SelectMagFusion()
{ {
// start performance timer // start performance timer
perf_begin(_perf_FuseMagnetometer); hal.util->perf_begin(_perf_FuseMagnetometer);
// check for and read new magnetometer measurements // check for and read new magnetometer measurements
readMagData(); readMagData();
@ -1019,14 +1008,14 @@ void NavEKF::SelectMagFusion()
} }
// stop performance timer // stop performance timer
perf_end(_perf_FuseMagnetometer); hal.util->perf_end(_perf_FuseMagnetometer);
} }
// select fusion of optical flow measurements // select fusion of optical flow measurements
void NavEKF::SelectFlowFusion() void NavEKF::SelectFlowFusion()
{ {
// start performance timer // start performance timer
perf_begin(_perf_FuseOptFlow); hal.util->perf_begin(_perf_FuseOptFlow);
// Perform Data Checks // Perform Data Checks
// Check if the optical flow data is still valid // Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
@ -1107,7 +1096,7 @@ void NavEKF::SelectFlowFusion()
} }
} }
// stop the performance timer // stop the performance timer
perf_end(_perf_FuseOptFlow); hal.util->perf_end(_perf_FuseOptFlow);
} }
// select fusion of true airspeed measurements // select fusion of true airspeed measurements
@ -1265,7 +1254,7 @@ void NavEKF::UpdateStrapdownEquationsNED()
// calculate the predicted state covariance matrix // calculate the predicted state covariance matrix
void NavEKF::CovariancePrediction() void NavEKF::CovariancePrediction()
{ {
perf_begin(_perf_CovariancePrediction); hal.util->perf_begin(_perf_CovariancePrediction);
float windVelSigma; // wind velocity 1-sigma process noise - m/s float windVelSigma; // wind velocity 1-sigma process noise - m/s
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
@ -1934,14 +1923,14 @@ void NavEKF::CovariancePrediction()
summedDelVel.zero(); summedDelVel.zero();
dt = 0.0f; dt = 0.0f;
perf_end(_perf_CovariancePrediction); hal.util->perf_end(_perf_CovariancePrediction);
} }
// fuse selected position, velocity and height measurements // fuse selected position, velocity and height measurements
void NavEKF::FuseVelPosNED() void NavEKF::FuseVelPosNED()
{ {
// start performance timer // start performance timer
perf_begin(_perf_FuseVelPosNED); hal.util->perf_begin(_perf_FuseVelPosNED);
// health is set bad until test passed // health is set bad until test passed
velHealth = false; velHealth = false;
@ -2375,7 +2364,7 @@ void NavEKF::FuseVelPosNED()
ConstrainVariances(); ConstrainVariances();
// stop performance timer // stop performance timer
perf_end(_perf_FuseVelPosNED); hal.util->perf_end(_perf_FuseVelPosNED);
} }
// fuse magnetometer measurements and apply innovation consistency checks // fuse magnetometer measurements and apply innovation consistency checks
@ -2770,7 +2759,7 @@ The filter can fuse motion compensated optiocal flow rates and range finder meas
void NavEKF::EstimateTerrainOffset() void NavEKF::EstimateTerrainOffset()
{ {
// start performance timer // start performance timer
perf_begin(_perf_OpticalFlowEKF); hal.util->perf_begin(_perf_OpticalFlowEKF);
// constrain height above ground to be above range measured on ground // constrain height above ground to be above range measured on ground
float heightAboveGndEst = max((terrainState - state.position.z), rngOnGnd); float heightAboveGndEst = max((terrainState - state.position.z), rngOnGnd);
@ -2943,7 +2932,7 @@ void NavEKF::EstimateTerrainOffset()
} }
// stop the performance timer // stop the performance timer
perf_end(_perf_OpticalFlowEKF); hal.util->perf_end(_perf_OpticalFlowEKF);
} }
void NavEKF::FuseOptFlow() void NavEKF::FuseOptFlow()
@ -3229,7 +3218,7 @@ void NavEKF::FuseOptFlow()
void NavEKF::FuseAirspeed() void NavEKF::FuseAirspeed()
{ {
// start performance timer // start performance timer
perf_begin(_perf_FuseAirspeed); hal.util->perf_begin(_perf_FuseAirspeed);
// declarations // declarations
float vn; float vn;
@ -3388,14 +3377,14 @@ void NavEKF::FuseAirspeed()
ConstrainVariances(); ConstrainVariances();
// stop performance timer // stop performance timer
perf_end(_perf_FuseAirspeed); hal.util->perf_end(_perf_FuseAirspeed);
} }
// fuse sythetic sideslip measurement of zero // fuse sythetic sideslip measurement of zero
void NavEKF::FuseSideslip() void NavEKF::FuseSideslip()
{ {
// start performance timer // start performance timer
perf_begin(_perf_FuseSideslip); hal.util->perf_begin(_perf_FuseSideslip);
// declarations // declarations
float q0; float q0;
@ -3578,7 +3567,7 @@ void NavEKF::FuseSideslip()
ConstrainVariances(); ConstrainVariances();
// stop the performance timer // stop the performance timer
perf_end(_perf_FuseSideslip); hal.util->perf_end(_perf_FuseSideslip);
} }
// zero specified range of rows in the state covariance matrix // zero specified range of rows in the state covariance matrix
@ -4736,6 +4725,17 @@ void NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f
// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary. // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
void NavEKF::InitialiseVariables() void NavEKF::InitialiseVariables()
{ {
if (_perf_UpdateFilter == nullptr) {
// only allocate perf variables once
_perf_UpdateFilter = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EKF_UpdateFilter");
_perf_CovariancePrediction = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EKF_CovariancePrediction");
_perf_FuseVelPosNED = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EKF_FuseVelPosNED");
_perf_FuseMagnetometer = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EKF_FuseMagnetometer");
_perf_FuseAirspeed = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EKF_FuseAirspeed");
_perf_FuseSideslip = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EKF_FuseSideslip");
_perf_OpticalFlowEKF = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EKF_FuseOptFlow");
}
// initialise time stamps // initialise time stamps
imuSampleTime_ms = hal.scheduler->millis(); imuSampleTime_ms = hal.scheduler->millis();
lastHealthyMagTime_ms = imuSampleTime_ms; lastHealthyMagTime_ms = imuSampleTime_ms;

27
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -35,10 +35,6 @@
#include <AP_Math/vectorN.h> #include <AP_Math/vectorN.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <systemlib/perf_counter.h>
#endif
// GPS pre-flight check bit locations // GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0) #define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_HDOP (1<<1) #define MASK_GPS_HDOP (1<<1)
@ -887,17 +883,15 @@ private:
// string representing last reason for prearm failure // string representing last reason for prearm failure
char prearm_fail_string[40]; char prearm_fail_string[40];
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// performance counters // performance counters
perf_counter_t _perf_UpdateFilter; AP_HAL::Util::perf_counter_t _perf_UpdateFilter;
perf_counter_t _perf_CovariancePrediction; AP_HAL::Util::perf_counter_t _perf_CovariancePrediction;
perf_counter_t _perf_FuseVelPosNED; AP_HAL::Util::perf_counter_t _perf_FuseVelPosNED;
perf_counter_t _perf_FuseMagnetometer; AP_HAL::Util::perf_counter_t _perf_FuseMagnetometer;
perf_counter_t _perf_FuseAirspeed; AP_HAL::Util::perf_counter_t _perf_FuseAirspeed;
perf_counter_t _perf_FuseSideslip; AP_HAL::Util::perf_counter_t _perf_FuseSideslip;
perf_counter_t _perf_OpticalFlowEKF; AP_HAL::Util::perf_counter_t _perf_OpticalFlowEKF;
perf_counter_t _perf_FuseOptFlow; AP_HAL::Util::perf_counter_t _perf_FuseOptFlow;
#endif
// should we assume zero sideslip? // should we assume zero sideslip?
bool assume_zero_sideslip(void) const; bool assume_zero_sideslip(void) const;
@ -906,9 +900,4 @@ private:
float InitialGyroBiasUncertainty(void) const; float InitialGyroBiasUncertainty(void) const;
}; };
#if CONFIG_HAL_BOARD != HAL_BOARD_PX4 && CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
#define perf_begin(x)
#define perf_end(x)
#endif
#endif // AP_NavEKF #endif // AP_NavEKF