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() void NavEKF2_core::FuseAirspeed()
{ {
// start performance timer // start performance timer
perf_begin(_perf_FuseAirspeed); hal.util->perf_begin(_perf_FuseAirspeed);
// declarations // declarations
float vn; float vn;
@ -187,7 +187,7 @@ void NavEKF2_core::FuseAirspeed()
ConstrainVariances(); ConstrainVariances();
// stop performance timer // stop performance timer
perf_end(_perf_FuseAirspeed); hal.util->perf_end(_perf_FuseAirspeed);
} }
// select fusion of true airspeed measurements // select fusion of true airspeed measurements
@ -292,7 +292,7 @@ void NavEKF2_core::SelectBetaFusion()
void NavEKF2_core::FuseSideslip() void NavEKF2_core::FuseSideslip()
{ {
// start performance timer // start performance timer
perf_begin(_perf_FuseSideslip); hal.util->perf_begin(_perf_FuseSideslip);
// declarations // declarations
float q0; float q0;
@ -471,7 +471,7 @@ void NavEKF2_core::FuseSideslip()
ConstrainVariances(); ConstrainVariances();
// stop the performance timer // 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() void NavEKF2_core::SelectMagFusion()
{ {
// start performance timer // 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 // 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 // used for load levelling
@ -145,15 +145,15 @@ void NavEKF2_core::SelectMagFusion()
} }
// fuse the three magnetometer componenents sequentially // fuse the three magnetometer componenents sequentially
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) { 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(); FuseMagnetometer();
perf_end(_perf_test[0]); hal.util->perf_end(_perf_test[0]);
} }
} }
} }
// stop performance timer // 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() void NavEKF2_core::FuseMagnetometer()
{ {
perf_begin(_perf_test[1]); hal.util->perf_begin(_perf_test[1]);
// declarations // declarations
ftype &q0 = mag_state.q0; ftype &q0 = mag_state.q0;
@ -186,7 +186,7 @@ void NavEKF2_core::FuseMagnetometer()
Vector6 SK_MY; Vector6 SK_MY;
Vector6 SK_MZ; Vector6 SK_MZ;
perf_end(_perf_test[1]); hal.util->perf_end(_perf_test[1]);
// perform sequential fusion of magnetometer measurements. // perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are // this assumes that the errors in the different components are
@ -197,7 +197,7 @@ void NavEKF2_core::FuseMagnetometer()
// calculate observation jacobians and Kalman gains // calculate observation jacobians and Kalman gains
if (obsIndex == 0) if (obsIndex == 0)
{ {
perf_begin(_perf_test[2]); hal.util->perf_begin(_perf_test[2]);
// copy required states to local variable names // copy required states to local variable names
q0 = stateStruct.quat[0]; q0 = stateStruct.quat[0];
q1 = stateStruct.quat[1]; q1 = stateStruct.quat[1];
@ -257,7 +257,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit(); CovarianceInit();
obsIndex = 1; obsIndex = 1;
faultStatus.bad_xmag = true; faultStatus.bad_xmag = true;
perf_end(_perf_test[2]); hal.util->perf_end(_perf_test[2]);
return; return;
} }
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2); 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 // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = 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 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 // calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f; 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]; H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
@ -335,7 +335,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit(); CovarianceInit();
obsIndex = 2; obsIndex = 2;
faultStatus.bad_ymag = true; faultStatus.bad_ymag = true;
perf_end(_perf_test[3]); hal.util->perf_end(_perf_test[3]);
return; return;
} }
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; 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 // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = 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 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 // calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f; 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]; 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(); CovarianceInit();
obsIndex = 3; obsIndex = 3;
faultStatus.bad_zmag = true; faultStatus.bad_zmag = true;
perf_end(_perf_test[4]); hal.util->perf_end(_perf_test[4]);
return; return;
} }
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2); 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 // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = false; 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 // calculate the measurement innovation
innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex]; innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
// calculate the innovation test ratio // 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 // 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)) { 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 // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero(); stateStruct.angErr.zero();
@ -510,8 +510,8 @@ void NavEKF2_core::FuseMagnetometer()
KH[i][j] = 0.0f; KH[i][j] = 0.0f;
} }
} }
perf_end(_perf_test[8]); hal.util->perf_end(_perf_test[8]);
perf_begin(_perf_test[9]); hal.util->perf_begin(_perf_test[9]);
for (unsigned j = 0; j<=stateIndexLim; j++) { for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) { for (unsigned i = 0; i<=stateIndexLim; i++) {
ftype res = 0; ftype res = 0;
@ -532,18 +532,18 @@ void NavEKF2_core::FuseMagnetometer()
P[i][j] = P[i][j] - KHP[i][j]; 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 // force the covariance matrix to be symmetrical and limit the variances to prevent
// ill-condiioning. // ill-condiioning.
perf_begin(_perf_test[6]); hal.util->perf_begin(_perf_test[6]);
ForceSymmetry(); ForceSymmetry();
perf_end(_perf_test[6]); hal.util->perf_end(_perf_test[6]);
perf_begin(_perf_test[7]); hal.util->perf_begin(_perf_test[7]);
ConstrainVariances(); 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 // 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);
@ -89,7 +89,7 @@ void NavEKF2_core::SelectFlowFusion()
} }
// stop the performance timer // 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() void NavEKF2_core::EstimateTerrainOffset()
{ {
// start performance timer // start performance timer
perf_begin(_perf_TerrainOffset); hal.util->perf_begin(_perf_TerrainOffset);
// 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 - stateStruct.position.z), rngOnGnd); float heightAboveGndEst = max((terrainState - stateStruct.position.z), rngOnGnd);
@ -272,7 +272,7 @@ void NavEKF2_core::EstimateTerrainOffset()
} }
// stop the performance timer // 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() void NavEKF2_core::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;
@ -486,7 +486,7 @@ void NavEKF2_core::FuseVelPosNED()
ConstrainVariances(); ConstrainVariances();
// stop performance timer // 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 //variables
lastRngMeasTime_ms(0), // time in msec that the last range measurement was taken 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(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")),
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EK2_UpdateFilter")), _perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_CovariancePrediction")),
_perf_CovariancePrediction(perf_alloc(PC_ELAPSED, "EK2_CovariancePrediction")), _perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseVelPosNED")),
_perf_FuseVelPosNED(perf_alloc(PC_ELAPSED, "EK2_FuseVelPosNED")), _perf_FuseMagnetometer(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseMagnetometer")),
_perf_FuseMagnetometer(perf_alloc(PC_ELAPSED, "EK2_FuseMagnetometer")), _perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseAirspeed")),
_perf_FuseAirspeed(perf_alloc(PC_ELAPSED, "EK2_FuseAirspeed")), _perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseSideslip")),
_perf_FuseSideslip(perf_alloc(PC_ELAPSED, "EK2_FuseSideslip")), _perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_TerrainOffset")),
_perf_TerrainOffset(perf_alloc(PC_ELAPSED, "EK2_TerrainOffset")), _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseOptFlow"))
_perf_FuseOptFlow(perf_alloc(PC_ELAPSED, "EK2_FuseOptFlow"))
#endif
{ {
_perf_test[0] = perf_alloc(PC_ELAPSED, "EK2_Test0"); _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test0");
_perf_test[1] = perf_alloc(PC_ELAPSED, "EK2_Test1"); _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test1");
_perf_test[2] = perf_alloc(PC_ELAPSED, "EK2_Test2"); _perf_test[2] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test2");
_perf_test[3] = perf_alloc(PC_ELAPSED, "EK2_Test3"); _perf_test[3] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test3");
_perf_test[4] = perf_alloc(PC_ELAPSED, "EK2_Test4"); _perf_test[4] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test4");
_perf_test[5] = perf_alloc(PC_ELAPSED, "EK2_Test5"); _perf_test[5] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test5");
_perf_test[6] = perf_alloc(PC_ELAPSED, "EK2_Test6"); _perf_test[6] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test6");
_perf_test[7] = perf_alloc(PC_ELAPSED, "EK2_Test7"); _perf_test[7] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test7");
_perf_test[8] = perf_alloc(PC_ELAPSED, "EK2_Test8"); _perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test8");
_perf_test[9] = perf_alloc(PC_ELAPSED, "EK2_Test9"); _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 #if EK2_DISABLE_INTERRUPTS
irqstate_t istate = irqsave(); irqstate_t istate = irqsave();
#endif #endif
perf_begin(_perf_UpdateFilter); hal.util->perf_begin(_perf_UpdateFilter);
// TODO - in-flight restart method // TODO - in-flight restart method
@ -401,7 +399,7 @@ void NavEKF2_core::UpdateFilter()
calcOutputStatesFast(); calcOutputStatesFast();
// 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 EK2_DISABLE_INTERRUPTS #if EK2_DISABLE_INTERRUPTS
irqrestore(istate); irqrestore(istate);
#endif #endif
@ -653,7 +651,7 @@ void NavEKF2_core::calcOutputStatesFast() {
*/ */
void NavEKF2_core::CovariancePrediction() 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 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
@ -1167,7 +1165,7 @@ void NavEKF2_core::CovariancePrediction()
summedDelVel.zero(); summedDelVel.zero();
dt = 0.0f; 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> #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)
@ -915,18 +911,16 @@ 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_TerrainOffset; AP_HAL::Util::perf_counter_t _perf_TerrainOffset;
perf_counter_t _perf_FuseOptFlow; AP_HAL::Util::perf_counter_t _perf_FuseOptFlow;
perf_counter_t _perf_test[10]; AP_HAL::Util::perf_counter_t _perf_test[10];
#endif
// should we assume zero sideslip? // should we assume zero sideslip?
bool assume_zero_sideslip(void) const; bool assume_zero_sideslip(void) const;
@ -935,9 +929,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_NavEKF2_core #endif // AP_NavEKF2_core