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

AP_NavEKF3: move initialisation of rngBcn into BeaconFusion method

This commit is contained in:
Peter Barker 2024-02-26 14:10:35 +11:00 committed by Andrew Tridgell
parent 744df1ceba
commit 2df3cb98c6
3 changed files with 46 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -3,6 +3,49 @@
#if EK3_FEATURE_BEACON_FUSION
// initialise state:
void NavEKF3_core::BeaconFusion::InitialiseVariables()
{
memset((void *)&dataDelayed, 0, sizeof(dataDelayed));
lastPassTime_ms = 0;
testRatio = 0.0f;
health = false;
varInnov = 0.0f;
innov = 0.0f;
memset(&lastTime_ms, 0, sizeof(lastTime_ms));
dataToFuse = false;
vehiclePosNED.zero();
vehiclePosErr = 1.0f;
last3DmeasTime_ms = 0;
goodToAlign = false;
lastChecked = 0;
receiverPos.zero();
memset(&receiverPosCov, 0, sizeof(receiverPosCov));
alignmentStarted = false;
alignmentCompleted = false;
lastIndex = 0;
posSum.zero();
numMeas = 0;
sum = 0.0f;
N = 0;
maxPosD = 0.0f;
minPosD = 0.0f;
posDownOffsetMax = 0.0f;
posOffsetMaxVar = 0.0f;
maxOffsetStateChangeFilt = 0.0f;
posDownOffsetMin = 0.0f;
posOffsetMinVar = 0.0f;
minOffsetStateChangeFilt = 0.0f;
fuseDataReportIndex = 0;
if (AP::dal().beacon()) {
if (fusionReport == nullptr) {
fusionReport = new BeaconFusion::FusionReport[AP::dal().beacon()->count()];
}
}
posOffsetNED.zero();
originEstInit = false;
}
/********************************************************
* FUSE MEASURED_DATA *
********************************************************/

39
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -345,44 +345,7 @@ void NavEKF3_core::InitialiseVariables()
// range beacon fusion variables
#if EK3_FEATURE_BEACON_FUSION
memset((void *)&rngBcn.dataDelayed, 0, sizeof(rngBcn.dataDelayed));
rngBcn.lastPassTime_ms = 0;
rngBcn.testRatio = 0.0f;
rngBcn.health = false;
rngBcn.varInnov = 0.0f;
rngBcn.innov = 0.0f;
memset(&rngBcn.lastTime_ms, 0, sizeof(rngBcn.lastTime_ms));
rngBcn.dataToFuse = false;
rngBcn.vehiclePosNED.zero();
rngBcn.vehiclePosErr = 1.0f;
rngBcn.last3DmeasTime_ms = 0;
rngBcn.goodToAlign = false;
rngBcn.lastChecked = 0;
rngBcn.receiverPos.zero();
memset(&rngBcn.receiverPosCov, 0, sizeof(rngBcn.receiverPosCov));
rngBcn.alignmentStarted = false;
rngBcn.alignmentCompleted = false;
rngBcn.lastIndex = 0;
rngBcn.posSum.zero();
rngBcn.numMeas = 0;
rngBcn.sum = 0.0f;
rngBcn.N = 0;
rngBcn.maxPosD = 0.0f;
rngBcn.minPosD = 0.0f;
rngBcn.posDownOffsetMax = 0.0f;
rngBcn.posOffsetMaxVar = 0.0f;
rngBcn.maxOffsetStateChangeFilt = 0.0f;
rngBcn.posDownOffsetMin = 0.0f;
rngBcn.posOffsetMinVar = 0.0f;
rngBcn.minOffsetStateChangeFilt = 0.0f;
rngBcn.fuseDataReportIndex = 0;
if (dal.beacon()) {
if (rngBcn.fusionReport == nullptr) {
rngBcn.fusionReport = new BeaconFusion::FusionReport[dal.beacon()->count()];
}
}
rngBcn.posOffsetNED.zero();
rngBcn.originEstInit = false;
rngBcn.InitialiseVariables();
#endif // EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_BODY_ODOM

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1323,6 +1323,8 @@ private:
#if EK3_FEATURE_BEACON_FUSION
class BeaconFusion {
public:
void InitialiseVariables();
EKF_obs_buffer_t<rng_bcn_elements> storedRange; // Beacon range buffer
rng_bcn_elements dataDelayed; // Range beacon data at the fusion time horizon
uint32_t lastPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)