Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

NavEKF2: use new perf counter API

This commit is contained in:
Andrew Tridgell 2015-10-20 16:42:50 +11:00
parent f66966ccae
commit cb0f7cb370
6 changed files with 67 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -29,7 +29,7 @@ extern const AP_HAL::HAL& hal;
void NavEKF2_core::FuseAirspeed()
{
// start performance timer
perf_begin(_perf_FuseAirspeed);
hal.util->perf_begin(_perf_FuseAirspeed);
// declarations
float vn;
@ -187,7 +187,7 @@ void NavEKF2_core::FuseAirspeed()
ConstrainVariances();
// stop performance timer
perf_end(_perf_FuseAirspeed);
hal.util->perf_end(_perf_FuseAirspeed);
}
// select fusion of true airspeed measurements
@ -292,7 +292,7 @@ void NavEKF2_core::SelectBetaFusion()
void NavEKF2_core::FuseSideslip()
{
// start performance timer
perf_begin(_perf_FuseSideslip);
hal.util->perf_begin(_perf_FuseSideslip);
// declarations
float q0;
@ -471,7 +471,7 @@ void NavEKF2_core::FuseSideslip()
ConstrainVariances();
// stop the performance timer
perf_end(_perf_FuseSideslip);
hal.util->perf_end(_perf_FuseSideslip);
}
/********************************************************

50
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -107,7 +107,7 @@ void NavEKF2_core::alignYawGPS()
void NavEKF2_core::SelectMagFusion()
{
// start performance timer
perf_begin(_perf_FuseMagnetometer);
hal.util->perf_begin(_perf_FuseMagnetometer);
// clear the flag that lets other processes know that the expensive magnetometer fusion operation has been perfomred on that time step
// used for load levelling
@ -145,15 +145,15 @@ void NavEKF2_core::SelectMagFusion()
}
// fuse the three magnetometer componenents sequentially
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) {
perf_begin(_perf_test[0]);
hal.util->perf_begin(_perf_test[0]);
FuseMagnetometer();
perf_end(_perf_test[0]);
hal.util->perf_end(_perf_test[0]);
}
}
}
// stop performance timer
perf_end(_perf_FuseMagnetometer);
hal.util->perf_end(_perf_FuseMagnetometer);
}
/*
@ -163,7 +163,7 @@ void NavEKF2_core::SelectMagFusion()
*/
void NavEKF2_core::FuseMagnetometer()
{
perf_begin(_perf_test[1]);
hal.util->perf_begin(_perf_test[1]);
// declarations
ftype &q0 = mag_state.q0;
@ -186,7 +186,7 @@ void NavEKF2_core::FuseMagnetometer()
Vector6 SK_MY;
Vector6 SK_MZ;
perf_end(_perf_test[1]);
hal.util->perf_end(_perf_test[1]);
// perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are
@ -197,7 +197,7 @@ void NavEKF2_core::FuseMagnetometer()
// calculate observation jacobians and Kalman gains
if (obsIndex == 0)
{
perf_begin(_perf_test[2]);
hal.util->perf_begin(_perf_test[2]);
// copy required states to local variable names
q0 = stateStruct.quat[0];
q1 = stateStruct.quat[1];
@ -257,7 +257,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit();
obsIndex = 1;
faultStatus.bad_xmag = true;
perf_end(_perf_test[2]);
hal.util->perf_end(_perf_test[2]);
return;
}
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
@ -310,11 +310,11 @@ void NavEKF2_core::FuseMagnetometer()
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
magFuseRequired = true;
perf_end(_perf_test[2]);
hal.util->perf_end(_perf_test[2]);
}
else if (obsIndex == 1) // we are now fusing the Y measurement
{
perf_begin(_perf_test[3]);
hal.util->perf_begin(_perf_test[3]);
// calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
@ -335,7 +335,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit();
obsIndex = 2;
faultStatus.bad_ymag = true;
perf_end(_perf_test[3]);
hal.util->perf_end(_perf_test[3]);
return;
}
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
@ -383,11 +383,11 @@ void NavEKF2_core::FuseMagnetometer()
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
magFuseRequired = true;
perf_end(_perf_test[3]);
hal.util->perf_end(_perf_test[3]);
}
else if (obsIndex == 2) // we are now fusing the Z measurement
{
perf_begin(_perf_test[4]);
hal.util->perf_begin(_perf_test[4]);
// calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
@ -408,7 +408,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit();
obsIndex = 3;
faultStatus.bad_zmag = true;
perf_end(_perf_test[4]);
hal.util->perf_end(_perf_test[4]);
return;
}
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
@ -456,10 +456,10 @@ void NavEKF2_core::FuseMagnetometer()
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
magFuseRequired = false;
perf_end(_perf_test[4]);
hal.util->perf_end(_perf_test[4]);
}
perf_begin(_perf_test[5]);
hal.util->perf_begin(_perf_test[5]);
// calculate the measurement innovation
innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
// calculate the innovation test ratio
@ -470,7 +470,7 @@ void NavEKF2_core::FuseMagnetometer()
// In this case we might as well try using the magnetometer, but with a reduced weighting
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) {
perf_begin(_perf_test[8]);
hal.util->perf_begin(_perf_test[8]);
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();
@ -510,8 +510,8 @@ void NavEKF2_core::FuseMagnetometer()
KH[i][j] = 0.0f;
}
}
perf_end(_perf_test[8]);
perf_begin(_perf_test[9]);
hal.util->perf_end(_perf_test[8]);
hal.util->perf_begin(_perf_test[9]);
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
ftype res = 0;
@ -532,18 +532,18 @@ void NavEKF2_core::FuseMagnetometer()
P[i][j] = P[i][j] - KHP[i][j];
}
}
perf_end(_perf_test[9]);
hal.util->perf_end(_perf_test[9]);
}
perf_end(_perf_test[5]);
hal.util->perf_end(_perf_test[5]);
// force the covariance matrix to be symmetrical and limit the variances to prevent
// ill-condiioning.
perf_begin(_perf_test[6]);
hal.util->perf_begin(_perf_test[6]);
ForceSymmetry();
perf_end(_perf_test[6]);
perf_begin(_perf_test[7]);
hal.util->perf_end(_perf_test[6]);
hal.util->perf_begin(_perf_test[7]);
ConstrainVariances();
perf_end(_perf_test[7]);
hal.util->perf_end(_perf_test[7]);
}

8
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -31,7 +31,7 @@ void NavEKF2_core::SelectFlowFusion()
}
// start performance timer
perf_begin(_perf_FuseOptFlow);
hal.util->perf_begin(_perf_FuseOptFlow);
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
@ -89,7 +89,7 @@ void NavEKF2_core::SelectFlowFusion()
}
// stop the performance timer
perf_end(_perf_FuseOptFlow);
hal.util->perf_end(_perf_FuseOptFlow);
}
/*
@ -99,7 +99,7 @@ The filter can fuse motion compensated optiocal flow rates and range finder meas
void NavEKF2_core::EstimateTerrainOffset()
{
// start performance timer
perf_begin(_perf_TerrainOffset);
hal.util->perf_begin(_perf_TerrainOffset);
// constrain height above ground to be above range measured on ground
float heightAboveGndEst = max((terrainState - stateStruct.position.z), rngOnGnd);
@ -272,7 +272,7 @@ void NavEKF2_core::EstimateTerrainOffset()
}
// stop the performance timer
perf_end(_perf_TerrainOffset);
hal.util->perf_end(_perf_TerrainOffset);
}
/*

4
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -159,7 +159,7 @@ void NavEKF2_core::SelectVelPosFusion()
void NavEKF2_core::FuseVelPosNED()
{
// start performance timer
perf_begin(_perf_FuseVelPosNED);
hal.util->perf_begin(_perf_FuseVelPosNED);
// health is set bad until test passed
velHealth = false;
@ -486,7 +486,7 @@ void NavEKF2_core::FuseVelPosNED()
ConstrainVariances();
// stop performance timer
perf_end(_perf_FuseVelPosNED);
hal.util->perf_end(_perf_FuseVelPosNED);
}
/********************************************************

48
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -35,29 +35,27 @@ NavEKF2_core::NavEKF2_core(NavEKF2 &_frontend, const AP_AHRS *ahrs, AP_Baro &bar
//variables
lastRngMeasTime_ms(0), // time in msec that the last range measurement was taken
rngMeasIndex(0) // index into ringbuffer of current range measurement
rngMeasIndex(0), // index into ringbuffer of current range measurement
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EK2_UpdateFilter")),
_perf_CovariancePrediction(perf_alloc(PC_ELAPSED, "EK2_CovariancePrediction")),
_perf_FuseVelPosNED(perf_alloc(PC_ELAPSED, "EK2_FuseVelPosNED")),
_perf_FuseMagnetometer(perf_alloc(PC_ELAPSED, "EK2_FuseMagnetometer")),
_perf_FuseAirspeed(perf_alloc(PC_ELAPSED, "EK2_FuseAirspeed")),
_perf_FuseSideslip(perf_alloc(PC_ELAPSED, "EK2_FuseSideslip")),
_perf_TerrainOffset(perf_alloc(PC_ELAPSED, "EK2_TerrainOffset")),
_perf_FuseOptFlow(perf_alloc(PC_ELAPSED, "EK2_FuseOptFlow"))
#endif
_perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")),
_perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_CovariancePrediction")),
_perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseVelPosNED")),
_perf_FuseMagnetometer(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseMagnetometer")),
_perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseAirspeed")),
_perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseSideslip")),
_perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_TerrainOffset")),
_perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseOptFlow"))
{
_perf_test[0] = perf_alloc(PC_ELAPSED, "EK2_Test0");
_perf_test[1] = perf_alloc(PC_ELAPSED, "EK2_Test1");
_perf_test[2] = perf_alloc(PC_ELAPSED, "EK2_Test2");
_perf_test[3] = perf_alloc(PC_ELAPSED, "EK2_Test3");
_perf_test[4] = perf_alloc(PC_ELAPSED, "EK2_Test4");
_perf_test[5] = perf_alloc(PC_ELAPSED, "EK2_Test5");
_perf_test[6] = perf_alloc(PC_ELAPSED, "EK2_Test6");
_perf_test[7] = perf_alloc(PC_ELAPSED, "EK2_Test7");
_perf_test[8] = perf_alloc(PC_ELAPSED, "EK2_Test8");
_perf_test[9] = perf_alloc(PC_ELAPSED, "EK2_Test9");
_perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test0");
_perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test1");
_perf_test[2] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test2");
_perf_test[3] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test3");
_perf_test[4] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test4");
_perf_test[5] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test5");
_perf_test[6] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test6");
_perf_test[7] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test7");
_perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test8");
_perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test9");
}
/********************************************************
@ -349,7 +347,7 @@ void NavEKF2_core::UpdateFilter()
#if EK2_DISABLE_INTERRUPTS
irqstate_t istate = irqsave();
#endif
perf_begin(_perf_UpdateFilter);
hal.util->perf_begin(_perf_UpdateFilter);
// TODO - in-flight restart method
@ -401,7 +399,7 @@ void NavEKF2_core::UpdateFilter()
calcOutputStatesFast();
// stop the timer used for load measurement
perf_end(_perf_UpdateFilter);
hal.util->perf_end(_perf_UpdateFilter);
#if EK2_DISABLE_INTERRUPTS
irqrestore(istate);
#endif
@ -653,7 +651,7 @@ void NavEKF2_core::calcOutputStatesFast() {
*/
void NavEKF2_core::CovariancePrediction()
{
perf_begin(_perf_CovariancePrediction);
hal.util->perf_begin(_perf_CovariancePrediction);
float windVelSigma; // wind velocity 1-sigma process noise - m/s
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
@ -1167,7 +1165,7 @@ void NavEKF2_core::CovariancePrediction()
summedDelVel.zero();
dt = 0.0f;
perf_end(_perf_CovariancePrediction);
hal.util->perf_end(_perf_CovariancePrediction);
}

29
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -32,10 +32,6 @@
#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
#define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_HDOP (1<<1)
@ -915,18 +911,16 @@ private:
// string representing last reason for prearm failure
char prearm_fail_string[40];
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// performance counters
perf_counter_t _perf_UpdateFilter;
perf_counter_t _perf_CovariancePrediction;
perf_counter_t _perf_FuseVelPosNED;
perf_counter_t _perf_FuseMagnetometer;
perf_counter_t _perf_FuseAirspeed;
perf_counter_t _perf_FuseSideslip;
perf_counter_t _perf_TerrainOffset;
perf_counter_t _perf_FuseOptFlow;
perf_counter_t _perf_test[10];
#endif
AP_HAL::Util::perf_counter_t _perf_UpdateFilter;
AP_HAL::Util::perf_counter_t _perf_CovariancePrediction;
AP_HAL::Util::perf_counter_t _perf_FuseVelPosNED;
AP_HAL::Util::perf_counter_t _perf_FuseMagnetometer;
AP_HAL::Util::perf_counter_t _perf_FuseAirspeed;
AP_HAL::Util::perf_counter_t _perf_FuseSideslip;
AP_HAL::Util::perf_counter_t _perf_TerrainOffset;
AP_HAL::Util::perf_counter_t _perf_FuseOptFlow;
AP_HAL::Util::perf_counter_t _perf_test[10];
// should we assume zero sideslip?
bool assume_zero_sideslip(void) const;
@ -935,9 +929,4 @@ private:
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_NavEKF2_core