|
|
|
@ -16,25 +16,25 @@ extern const AP_HAL::HAL& hal;
@@ -16,25 +16,25 @@ extern const AP_HAL::HAL& hal;
|
|
|
|
|
NavEKF3_core::NavEKF3_core(void) : |
|
|
|
|
stateStruct(*reinterpret_cast<struct state_elements *>(&statesArray)), |
|
|
|
|
|
|
|
|
|
_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_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_UpdateFilter")), |
|
|
|
|
_perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_CovariancePrediction")), |
|
|
|
|
_perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseVelPosNED")), |
|
|
|
|
_perf_FuseMagnetometer(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseMagnetometer")), |
|
|
|
|
_perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseAirspeed")), |
|
|
|
|
_perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseSideslip")), |
|
|
|
|
_perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_TerrainOffset")), |
|
|
|
|
_perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")) |
|
|
|
|
{ |
|
|
|
|
_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"); |
|
|
|
|
_perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test0"); |
|
|
|
|
_perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test1"); |
|
|
|
|
_perf_test[2] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test2"); |
|
|
|
|
_perf_test[3] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test3"); |
|
|
|
|
_perf_test[4] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test4"); |
|
|
|
|
_perf_test[5] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test5"); |
|
|
|
|
_perf_test[6] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test6"); |
|
|
|
|
_perf_test[7] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test7"); |
|
|
|
|
_perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test8"); |
|
|
|
|
_perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test9"); |
|
|
|
|
firstInitTime_ms = 0; |
|
|
|
|
lastInitFailReport_ms = 0; |
|
|
|
|
} |
|
|
|
@ -508,7 +508,7 @@ void NavEKF3_core::UpdateFilter(bool predict)
@@ -508,7 +508,7 @@ void NavEKF3_core::UpdateFilter(bool predict)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// start the timer used for load measurement
|
|
|
|
|
#if EK2_DISABLE_INTERRUPTS |
|
|
|
|
#if EK3_DISABLE_INTERRUPTS |
|
|
|
|
irqstate_t istate = irqsave(); |
|
|
|
|
#endif |
|
|
|
|
hal.util->perf_begin(_perf_UpdateFilter); |
|
|
|
@ -556,7 +556,7 @@ void NavEKF3_core::UpdateFilter(bool predict)
@@ -556,7 +556,7 @@ void NavEKF3_core::UpdateFilter(bool predict)
|
|
|
|
|
|
|
|
|
|
// stop the timer used for load measurement
|
|
|
|
|
hal.util->perf_end(_perf_UpdateFilter); |
|
|
|
|
#if EK2_DISABLE_INTERRUPTS |
|
|
|
|
#if EK3_DISABLE_INTERRUPTS |
|
|
|
|
irqrestore(istate); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|