From 55377b234f0f45046559b8c9ff5a6a72e66129d4 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Sun, 17 Feb 2019 10:51:52 -0800 Subject: [PATCH] AP_NavEKF3: remove HAL_CPU_CLASS_150 check, 150 is already a minimum requirement --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 -- libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 4 ---- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 3 --- libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp | 4 ---- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 4 ---- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 -- libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 3 --- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 3 --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 3 --- libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 3 --- libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 3 --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 5 +---- 12 files changed, 1 insertion(+), 38 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 8b324747a2..eb6f405a13 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1,5 +1,4 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 #include "AP_NavEKF3_core.h" #include @@ -1559,4 +1558,3 @@ void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) co } } -#endif //HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index ec403ea7ae..39aaf8b0ae 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -468,5 +466,3 @@ void NavEKF3_core::FuseSideslip() * MISC FUNCTIONS * ********************************************************/ - -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 64671cfc8e..490b1ad811 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -562,4 +560,3 @@ void NavEKF3_core::updateFilterStatus(void) filterStatus.flags.gps_quality_good = gpsGoodToAlign; } -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp index ef06360a84..05a9f7a4ac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -31,5 +29,3 @@ float NavEKF3_core::InitialGyroBiasUncertainty(void) const return 2.5f; } - -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 346fef0ad3..d7d8b3bba7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -1195,5 +1193,3 @@ void NavEKF3_core::recordMagReset() yawInnovAtLastMagReset = innovYaw; } - -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 75ec47a2f8..c88b6147fe 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -1,6 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -869,4 +868,3 @@ void NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing) memset(&timing, 0, sizeof(timing)); } -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index eb0f88fbd3..e7675db2d0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -746,4 +744,3 @@ void NavEKF3_core::FuseOptFlow() * MISC FUNCTIONS * ********************************************************/ -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 9db640ec57..8390fc4e12 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -629,4 +627,3 @@ void NavEKF3_core::getOutputTrackingError(Vector3f &error) const error = outputTrackError; } -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 57b471f107..f6b4691768 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -1609,4 +1607,3 @@ void NavEKF3_core::SelectBodyOdomFusion() } } -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 0823c7543d..399f5af674 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -641,4 +639,3 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP rngBcnDataDelayed.beacon_posNED.z += bcnPosOffsetNED.z; } -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 4a479a2803..7ea762ec49 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -469,4 +467,3 @@ void NavEKF3_core::detectOptFlowTakeoff(void) } } -#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8c9b86acb2..e444935df5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1,7 +1,5 @@ #include -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include @@ -691,7 +689,7 @@ void NavEKF3_core::UpdateStrapdownEquationsNED() * The inspiration for using a complementary filter to correct for time delays in the EKF * is based on the work by A Khosravian. * - * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements” + * "Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements" * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University */ void NavEKF3_core::calcOutputStates() @@ -1779,4 +1777,3 @@ void NavEKF3_core::initialiseQuatCovariances(const Vector3f &rotVarVec) } } -#endif // HAL_CPU_CLASS