AP_NavEKF: Source's active_source_set becomes index

libraries/AP_NavEKF/AP_NavEKF_Source.cpp

@@ -145,45 +145,19 @@ AP_NavEKF_Source::AP_NavEKF_Source()
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-void AP_NavEKF_Source::init()
-{
-    // ensure init is only run once
-    if (initialised) {
-        return;
-    }
-
-    // initialise active sources
-    _active_source_set.posxy = _source_set[0].posxy;
-    _active_source_set.velxy = _source_set[0].velxy;
-    _active_source_set.posz = _source_set[0].posz;
-    _active_source_set.velz = _source_set[0].velz;
-    _active_source_set.yaw = _source_set[0].yaw;
-
-    initialised = true;
-}
-
 // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
 void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx)
 {
-    // ensure init has been run
-    init();
-
     // sanity check source idx
-    if (source_set_idx >= AP_NAKEKF_SOURCE_SET_MAX) {
-        return;
+    if (source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) {
+        active_source_set = source_set_idx;
     }
-
-    _active_source_set.posxy = _source_set[source_set_idx].posxy;
-    _active_source_set.velxy = _source_set[source_set_idx].velxy;
-    _active_source_set.posz = _source_set[source_set_idx].posz;
-    _active_source_set.velz = _source_set[source_set_idx].velz;
-    _active_source_set.yaw = _source_set[source_set_idx].yaw;
 }
 
 // true/false of whether velocity source should be used
 bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
 {
-    if (velxy_source == _active_source_set.velxy) {
+    if (velxy_source == _source_set[active_source_set].velxy) {
         return true;
     }
 
@@ -202,7 +176,7 @@ bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
 
 bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
 {
-    if (velz_source == _active_source_set.velz) {
+    if (velz_source == _source_set[active_source_set].velz) {
         return true;
     }
 
@@ -222,7 +196,7 @@ bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
 // true if a velocity source is configured
 bool AP_NavEKF_Source::haveVelZSource() const
 {
-    if (_active_source_set.velz != SourceZ::NONE) {
+    if (_source_set[active_source_set].velz != SourceZ::NONE) {
         return true;
     }
 

libraries/AP_NavEKF/AP_NavEKF_Source.h

@@ -53,15 +53,15 @@ public:
     void init();
 
     // get current position source
-    SourceXY getPosXYSource() const { return _active_source_set.posxy; }
-    SourceZ getPosZSource() const { return _active_source_set.posz; }
+    SourceXY getPosXYSource() const { return _source_set[active_source_set].posxy; }
+    SourceZ getPosZSource() const { return _source_set[active_source_set].posz; }
 
     // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
     void setPosVelYawSourceSet(uint8_t source_set_idx);
 
     // get/set velocity source
-    SourceXY getVelXYSource() const { return _active_source_set.velxy; }
-    SourceZ getVelZSource() const { return _active_source_set.velz; }
+    SourceXY getVelXYSource() const { return _source_set[active_source_set].velxy; }
+    SourceZ getVelZSource() const { return _source_set[active_source_set].velz; }
 
     // true/false of whether velocity source should be used
     bool useVelXYSource(SourceXY velxy_source) const;
@@ -71,7 +71,7 @@ public:
     bool haveVelZSource() const;
 
     // get yaw source
-    SourceYaw getYawSource() const { return _active_source_set.yaw; }
+    SourceYaw getYawSource() const { return _source_set[active_source_set].yaw; }
 
     // align position of inactive sources to ahrs
     void align_inactive_sources();
@@ -114,14 +114,6 @@ private:
 
     AP_Int16 _options;      // source options bitmask
 
-    // active sources
-    struct {
-        SourceXY posxy;     // current xy position source
-        SourceZ posz;       // current z position source
-        SourceXY velxy;     // current xy velocity source
-        SourceZ velz;       // current z velocity source
-        SourceYaw yaw;      // current yaw source
-    } _active_source_set;
-    bool initialised;       // true once init has been run
+    uint8_t active_source_set; // index of active source set
     bool config_in_storage; // true once configured in storage has returned true
 };