Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: honour AP_BEACON_ENABLED 

if the DAL cuts its APIs out based on AP_BEACON_ENABLED we have to modify EKF2 to not use those APIs
This commit is contained in:
Peter Barker 2022-11-14 09:50:01 +11:00 committed by Peter Barker
parent 67986e8402
commit 81d39677ab
4 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -199,6 +199,7 @@ void NavEKF2_core::Log_Write_Quaternion(uint64_t time_us) const
AP::logger().WriteBlock(&pktq1, sizeof(pktq1)); AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
} }
#if AP_BEACON_ENABLED
void NavEKF2_core::Log_Write_Beacon(uint64_t time_us) void NavEKF2_core::Log_Write_Beacon(uint64_t time_us)
{ {
if (core_index != frontend->primary) { if (core_index != frontend->primary) {
@ -248,6 +249,7 @@ void NavEKF2_core::Log_Write_Beacon(uint64_t time_us)
AP::logger().WriteBlock(&pkt0, sizeof(pkt0)); AP::logger().WriteBlock(&pkt0, sizeof(pkt0));
rngBcnFuseDataReportIndex++; rngBcnFuseDataReportIndex++;
} }
#endif // AP_BEACON_ENABLED
void NavEKF2_core::Log_Write_Timing(uint64_t time_us) void NavEKF2_core::Log_Write_Timing(uint64_t time_us)
{ {
@ -314,8 +316,10 @@ void NavEKF2_core::Log_Write(uint64_t time_us)
Log_Write_Quaternion(time_us); Log_Write_Quaternion(time_us);
Log_Write_GSF(time_us); Log_Write_GSF(time_us);
#if AP_BEACON_ENABLED
// write range beacon fusion debug packet if the range value is non-zero // write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us); Log_Write_Beacon(time_us);
#endif
Log_Write_Timing(time_us); Log_Write_Timing(time_us);
} }

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -795,6 +795,7 @@ void NavEKF2_core::readAirSpdData()
* Range Beacon Measurements * * Range Beacon Measurements *
********************************************************/ ********************************************************/
#if AP_BEACON_ENABLED
// check for new range beacon data and push to data buffer if available // check for new range beacon data and push to data buffer if available
void NavEKF2_core::readRngBcnData() void NavEKF2_core::readRngBcnData()
{ {
@ -901,6 +902,7 @@ void NavEKF2_core::readRngBcnData()
rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms); rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
} }
#endif // AP_BEACON_ENABLED
/* /*
update timing statistics structure update timing statistics structure

2
libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
View File

@ -5,6 +5,7 @@
* FUSE MEASURED_DATA * * FUSE MEASURED_DATA *
********************************************************/ ********************************************************/
#if AP_BEACON_ENABLED
// select fusion of range beacon measurements // select fusion of range beacon measurements
void NavEKF2_core::SelectRngBcnFusion() void NavEKF2_core::SelectRngBcnFusion()
{ {
@ -570,3 +571,4 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
} }
#endif // AP_BEACON_ENABLED

2
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -573,8 +573,10 @@ void NavEKF2_core::UpdateFilter(bool predict)
// Muat be run after SelectVelPosFusion() so that fresh GPS data is available // Muat be run after SelectVelPosFusion() so that fresh GPS data is available
runYawEstimatorCorrection(); runYawEstimatorCorrection();
#if AP_BEACON_ENABLED
// Update states using range beacon data // Update states using range beacon data
SelectRngBcnFusion(); SelectRngBcnFusion();
#endif
// Update states using optical flow data // Update states using optical flow data
SelectFlowFusion(); SelectFlowFusion();