AP_NavEKF3: remove HAL_CPU_CLASS_150 check, 150 is already a minimum requirement
This commit is contained in:
parent
b32e8a4424
commit
55377b234f
@ -1,5 +1,4 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_Vehicle/AP_Vehicle.h>
|
#include <AP_Vehicle/AP_Vehicle.h>
|
||||||
@ -1559,4 +1558,3 @@ void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) co
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif //HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -468,5 +466,3 @@ void NavEKF3_core::FuseSideslip()
|
|||||||
* MISC FUNCTIONS *
|
* MISC FUNCTIONS *
|
||||||
********************************************************/
|
********************************************************/
|
||||||
|
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -562,4 +560,3 @@ void NavEKF3_core::updateFilterStatus(void)
|
|||||||
filterStatus.flags.gps_quality_good = gpsGoodToAlign;
|
filterStatus.flags.gps_quality_good = gpsGoodToAlign;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -31,5 +29,3 @@ float NavEKF3_core::InitialGyroBiasUncertainty(void) const
|
|||||||
return 2.5f;
|
return 2.5f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -1195,5 +1193,3 @@ void NavEKF3_core::recordMagReset()
|
|||||||
yawInnovAtLastMagReset = innovYaw;
|
yawInnovAtLastMagReset = innovYaw;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,6 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -869,4 +868,3 @@ void NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing)
|
|||||||
memset(&timing, 0, sizeof(timing));
|
memset(&timing, 0, sizeof(timing));
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -746,4 +744,3 @@ void NavEKF3_core::FuseOptFlow()
|
|||||||
* MISC FUNCTIONS *
|
* MISC FUNCTIONS *
|
||||||
********************************************************/
|
********************************************************/
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -629,4 +627,3 @@ void NavEKF3_core::getOutputTrackingError(Vector3f &error) const
|
|||||||
error = outputTrackError;
|
error = outputTrackError;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -1609,4 +1607,3 @@ void NavEKF3_core::SelectBodyOdomFusion()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -641,4 +639,3 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|||||||
rngBcnDataDelayed.beacon_posNED.z += bcnPosOffsetNED.z;
|
rngBcnDataDelayed.beacon_posNED.z += bcnPosOffsetNED.z;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -469,4 +467,3 @@ void NavEKF3_core::detectOptFlowTakeoff(void)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
@ -1,7 +1,5 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
||||||
|
|
||||||
#include "AP_NavEKF3.h"
|
#include "AP_NavEKF3.h"
|
||||||
#include "AP_NavEKF3_core.h"
|
#include "AP_NavEKF3_core.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
@ -691,7 +689,7 @@ void NavEKF3_core::UpdateStrapdownEquationsNED()
|
|||||||
* The inspiration for using a complementary filter to correct for time delays in the EKF
|
* The inspiration for using a complementary filter to correct for time delays in the EKF
|
||||||
* is based on the work by A Khosravian.
|
* 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
|
* A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
|
||||||
*/
|
*/
|
||||||
void NavEKF3_core::calcOutputStates()
|
void NavEKF3_core::calcOutputStates()
|
||||||
@ -1779,4 +1777,3 @@ void NavEKF3_core::initialiseQuatCovariances(const Vector3f &rotVarVec)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_CPU_CLASS
|
|
||||||
|
Loading…
Reference in New Issue
Block a user