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

AP_NavEKF3: add and use AP_BEACON_ENABLED

This commit is contained in:
Peter Barker 2022-01-28 11:09:44 +11:00 committed by Peter Barker
parent 7fa9364809
commit 5cb6906968
9 changed files with 67 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -302,8 +302,12 @@ void NavEKF3_core::setAidingMode()
// check if drag data is being used
bool dragUsed = (imuSampleTime_ms - lastDragPassTime_ms <= minTestTime_ms);
#if EK3_FEATURE_BEACON_FUSION
// Check if range beacon data is being used
bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
const bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
#else
const bool rngBcnUsed = false;
#endif
// Check if GPS or external nav is being used
bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
@ -323,7 +327,9 @@ void NavEKF3_core::setAidingMode()
if (!attAiding) {
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
#if EK3_FEATURE_BEACON_FUSION
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
#endif
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
}
@ -337,8 +343,11 @@ void NavEKF3_core::setAidingMode()
} else {
maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
}
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
posAidLossCritical =
#if EK3_FEATURE_BEACON_FUSION
(imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
#endif
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
}
if (attAidLossCritical) {
@ -411,12 +420,14 @@ void NavEKF3_core::setAidingMode()
posResetSource = resetDataSource::GPS;
velResetSource = resetDataSource::GPS;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
#if EK3_FEATURE_BEACON_FUSION
} else if (readyToUseRangeBeacon()) {
// We are commencing aiding using range beacons
posResetSource = resetDataSource::RNGBCN;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
#endif // EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_EXTERNAL_NAV
} else if (readyToUseExtNav()) {
// we are commencing aiding using external nav
@ -441,7 +452,9 @@ void NavEKF3_core::setAidingMode()
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
#if EK3_FEATURE_BEACON_FUSION
lastRngBcnPassTime_ms = imuSampleTime_ms;
#endif
break;
}
@ -541,11 +554,15 @@ bool NavEKF3_core::readyToUseGPS(void) const
// return true if the filter to be ready to use the beacon range measurements
bool NavEKF3_core::readyToUseRangeBeacon(void) const
{
#if EK3_FEATURE_BEACON_FUSION
if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::BEACON) {
return false;
}
return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse;
#else
return false;
#endif // EK3_FEATURE_BEACON_FUSION
}
// return true if the filter is ready to use external nav data

4
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -220,6 +220,7 @@ void NavEKF3_core::Log_Write_Quaternion(uint64_t time_us) const
AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
}
#if EK3_FEATURE_BEACON_FUSION
// logs beacon information, one beacon per call
void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
{
@ -266,6 +267,7 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
rngBcnFuseDataReportIndex++;
}
#endif // EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_BODY_ODOM
void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
@ -388,8 +390,10 @@ void NavEKF3_core::Log_Write(uint64_t time_us)
Log_Write_Quaternion(time_us);
#if EK3_FEATURE_BEACON_FUSION
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);
#endif
#if EK3_FEATURE_BODY_ODOM
// write debug data for body frame odometry fusion

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -863,6 +863,7 @@ void NavEKF3_core::readAirSpdData()
}
}
#if EK3_FEATURE_BEACON_FUSION
/********************************************************
* Range Beacon Measurements *
********************************************************/
@ -979,6 +980,7 @@ void NavEKF3_core::readRngBcnData()
}
}
#endif // EK3_FEATURE_BEACON_FUSION
/********************************************************
* Independant yaw sensor measurements *

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -233,11 +233,13 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
const struct Location &gpsloc = gps.location(selected_gps);
posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
return false;
#if EK3_FEATURE_BEACON_FUSION
} else if (rngBcnAlignmentStarted) {
// If we are attempting alignment using range beacon data, then report the position
posNE.x = receiverPos.x;
posNE.y = receiverPos.y;
return false;
#endif
} else {
// If no GPS fix is available, all we can do is provide the last known position
posNE = outputDataNew.position.xy().tofloat();

14
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -104,6 +104,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
stateStruct.position.xy() += gps_corrected.vel.xy()*0.001*tdiff;
// set the variances using the position measurement noise parameter
P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
#if EK3_FEATURE_BEACON_FUSION
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) {
// use the range beacon data as a second preference
stateStruct.position.x = receiverPos.x;
@ -111,6 +112,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
// set the variances from the beacon alignment filter
P[7][7] = receiverPosCov[0][0];
P[8][8] = receiverPosCov[1][1];
#endif
#if EK3_FEATURE_EXTERNAL_NAV
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) {
// use external nav data as the third preference
@ -1125,8 +1127,10 @@ void NavEKF3_core::selectHeightForFusion()
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
#if EK3_FEATURE_BEACON_FUSION
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON;
#endif
#if EK3_FEATURE_EXTERNAL_NAV
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) {
activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV;
@ -1136,8 +1140,16 @@ void NavEKF3_core::selectHeightForFusion()
// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh);
bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000 || !gpsAccuracyGoodForAltitude));
#if EK3_FEATURE_BEACON_FUSION
bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
bool fallback_to_baro = lostRngHgt || lostGpsHgt || lostRngBcnHgt;
#endif
bool fallback_to_baro =
lostRngHgt
|| lostGpsHgt
#if EK3_FEATURE_BEACON_FUSION
|| lostRngBcnHgt
#endif
;
#if EK3_FEATURE_EXTERNAL_NAV
bool lostExtNavHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) && !extNavDataIsFresh);
fallback_to_baro |= lostExtNavHgt;

3
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -1,6 +1,8 @@
#include "AP_NavEKF3.h"
#include "AP_NavEKF3_core.h"
#if EK3_FEATURE_BEACON_FUSION
/********************************************************
* FUSE MEASURED_DATA *
********************************************************/
@ -641,3 +643,4 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
rngBcnDataDelayed.beacon_posNED.z += bcnPosOffsetNED.z;
}
#endif // EK3_FEATURE_BEACON_FUSION

10
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -136,9 +136,11 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
return false;
}
// Note: range beacon data is read one beacon at a time and can arrive at a high rate
#if EK3_FEATURE_BEACON_FUSION
if(dal.beacon() && !storedRangeBeacon.init(imu_buffer_length+1)) {
return false;
}
#endif
#if EK3_FEATURE_EXTERNAL_NAV
if (frontend->sources.ext_nav_enabled() && !storedExtNav.init(extnav_buffer_length)) {
return false;
@ -341,6 +343,7 @@ void NavEKF3_core::InitialiseVariables()
ZERO_FARRAY(velPosObs);
// range beacon fusion variables
#if EK3_FEATURE_BEACON_FUSION
memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
lastRngBcnPassTime_ms = 0;
rngBcnTestRatio = 0.0f;
@ -379,6 +382,7 @@ void NavEKF3_core::InitialiseVariables()
}
bcnPosOffsetNED.zero();
bcnOriginEstInit = false;
#endif // EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_BODY_ODOM
// body frame displacement fusion
@ -419,7 +423,9 @@ void NavEKF3_core::InitialiseVariables()
storedTAS.reset();
storedRange.reset();
storedOutput.reset();
#if EK3_FEATURE_BEACON_FUSION
storedRangeBeacon.reset();
#endif
#if EK3_FEATURE_BODY_ODOM
storedBodyOdm.reset();
storedWheelOdm.reset();
@ -686,8 +692,10 @@ void NavEKF3_core::UpdateFilter(bool predict)
// Muat be run after SelectVelPosFusion() so that fresh GPS data is available
runYawEstimatorCorrection();
#if EK3_FEATURE_BEACON_FUSION
// Update states using range beacon data
SelectRngBcnFusion();
#endif
// Update states using optical flow data
SelectFlowFusion();
@ -812,10 +820,12 @@ void NavEKF3_core::UpdateStrapdownEquationsNED()
// limit states to protect against divergence
ConstrainStates();
#if EK3_FEATURE_BEACON_FUSION
// If main filter velocity states are valid, update the range beacon receiver position states
if (filterStatus.flags.horiz_vel) {
receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
}
#endif
}
/*

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -674,8 +674,10 @@ private:
// fuse body frame velocity measurements
void FuseBodyVel();
#if EK3_FEATURE_BEACON_FUSION
// fuse range beacon measurements
void FuseRngBcn();
#endif
// use range beacon measurements to calculate a static position
void FuseRngBcnStatic();
@ -745,14 +747,18 @@ private:
// check for new airspeed data and update stored measurements if available
void readAirSpdData();
#if EK3_FEATURE_BEACON_FUSION
// check for new range beacon data and update stored measurements if available
void readRngBcnData();
#endif
// determine when to perform fusion of GPS position and velocity measurements
void SelectVelPosFusion();
#if EK3_FEATURE_BEACON_FUSION
// determine when to perform fusion of range measurements take relative to a beacon at a known NED position
void SelectRngBcnFusion();
#endif
// determine when to perform fusion of magnetometer measurements
void SelectMagFusion();
@ -1273,6 +1279,7 @@ private:
yaw_elements yawAngDataStatic; // yaw angle (regardless of yaw source) when the vehicle was last on ground and not moving
// Range Beacon Sensor Fusion
#if EK3_FEATURE_BEACON_FUSION
EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
@ -1320,6 +1327,7 @@ private:
ftype testRatio; // innovation consistency test ratio
Vector3F beaconPosNED; // beacon NED position
} *rngBcnFusionReport;
#endif // if EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_DRAG_FUSION
// drag fusion for multicopter wind estimation

5
libraries/AP_NavEKF3/AP_NavEKF3_feature.h
View File

@ -6,6 +6,7 @@
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_Beacon/AP_Beacon_config.h>
// define for when to include all features
#define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay)
@ -25,3 +26,7 @@
#define EK3_FEATURE_DRAG_FUSION EK3_FEATURE_ALL || BOARD_FLASH_SIZE > 1024
#endif
// Beacon Fusion if beacon data available
#ifndef EK3_FEATURE_BEACON_FUSION
#define EK3_FEATURE_BEACON_FUSION AP_BEACON_ENABLED
#endif