AP_NavEKF3: move initialisation of rngBcn into BeaconFusion method

ArduPilot · Feb 26, 2024 · 9c0fdd9 · 9c0fdd9
1 parent 6d6c790
commit 9c0fdd9
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 38 deletions.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
@@ -3,6 +3,50 @@
 
 #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;
+    auto dal = AP::dal();
+    if (dal.beacon()) {
+        if (fusionReport == nullptr) {
+            fusionReport = new BeaconFusion::FusionReport[dal.beacon()->count()];
+        }
+    }
+    posOffsetNED.zero();
+    originEstInit = false;
+}
+
 /********************************************************
 *                   FUSE MEASURED_DATA                  *
 ********************************************************/

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -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

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1323,6 +1323,8 @@ class NavEKF3_core : public NavEKF_core_common
 #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)