Browse Source

AP_NavEKF3: Remove EKF2 names

master
priseborough 8 years ago committed by Randy Mackay
parent
commit
59ee074560
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 4
      libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
  3. 40
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
  4. 2
      libraries/AP_NavEKF3/AP_NavEKF3_core.h

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -360,7 +360,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { @@ -360,7 +360,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m/s/s/s
AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
// 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK2_MAGE_P_NSE and EK2_MAGB_P_NSE
// 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK3_MAGE_P_NSE and EK3_MAGB_P_NSE
// @Param: WIND_P_NSE
// @DisplayName: Wind velocity process noise (m/s^2)

4
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@ -84,11 +84,11 @@ bool NavEKF3_core::calcGpsGoodToAlign(void) @@ -84,11 +84,11 @@ bool NavEKF3_core::calcGpsGoodToAlign(void)
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and
// EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not
// EK3_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 1");
}
} else {
gpsVertVelFail = false;

40
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@ -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
}

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h

@ -21,7 +21,7 @@ @@ -21,7 +21,7 @@
#pragma GCC optimize("O3")
#define EK2_DISABLE_INTERRUPTS 0
#define EK3_DISABLE_INTERRUPTS 0
#include <AP_Math/AP_Math.h>

Loading…
Cancel
Save