AP_NavEKF_Source: enables dynamically changing sensor sources

5 years ago · 9b84abecaa
2 changed files with 582 additions and 0 deletions
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
@ -0,0 +1,467 @@
				@@ -0,0 +1,467 @@
+/*
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_NavEKF_Source.h"
+#include <AP_Math/AP_Math.h>
+#include <AP_DAL/AP_DAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
+
+    // @Param: 1_POSXY
+    // @DisplayName: Position Horizontal Source (Primary)
+    // @Description: Position Horizontal Source (Primary)
+    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("1_POSXY", 1, AP_NavEKF_Source, _source_set[0].posxy, (int8_t)AP_NavEKF_Source::SourceXY::GPS),
+
+    // @Param: 1_VELXY
+    // @DisplayName: Velocity Horizontal Source
+    // @Description: Velocity Horizontal Source
+    // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
+    // @User: Advanced
+    AP_GROUPINFO("1_VELXY", 2, AP_NavEKF_Source, _source_set[0].velxy, (int8_t)AP_NavEKF_Source::SourceXY::GPS),
+
+    // @Param: 1_POSZ
+    // @DisplayName: Position Vertical Source
+    // @Description: Position Vertical Source
+    // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("1_POSZ", 3, AP_NavEKF_Source, _source_set[0].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),
+
+    // @Param: 1_VELZ
+    // @DisplayName: Velocity Vertical Source
+    // @Description: Velocity Vertical Source
+    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("1_VELZ", 4, AP_NavEKF_Source, _source_set[0].velz, (int8_t)AP_NavEKF_Source::SourceZ::GPS),
+
+    // @Param: 1_YAW
+    // @DisplayName: Yaw Source
+    // @Description: Yaw Source
+    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
+    // @User: Advanced
+    AP_GROUPINFO("1_YAW", 5, AP_NavEKF_Source, _source_set[0].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::COMPASS),
+
+#if AP_NAKEKF_SOURCE_SET_MAX >= 2
+    // @Param: 2_POSXY
+    // @DisplayName: Position Horizontal Source (Secondary)
+    // @Description: Position Horizontal Source (Secondary)
+    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("2_POSXY", 6, AP_NavEKF_Source, _source_set[1].posxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
+
+    // @Param: 2_VELXY
+    // @DisplayName: Velocity Horizontal Source (Secondary)
+    // @Description: Velocity Horizontal Source (Secondary)
+    // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
+    // @User: Advanced
+    AP_GROUPINFO("2_VELXY", 7, AP_NavEKF_Source, _source_set[1].velxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
+
+    // @Param: 2_POSZ
+    // @DisplayName: Position Vertical Source (Secondary)
+    // @Description: Position Vertical Source (Secondary)
+    // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("2_POSZ", 8, AP_NavEKF_Source, _source_set[1].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),
+
+    // @Param: 2_VELZ
+    // @DisplayName: Velocity Vertical Source (Secondary)
+    // @Description: Velocity Vertical Source (Secondary)
+    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("2_VELZ", 9, AP_NavEKF_Source, _source_set[1].velz, (int8_t)AP_NavEKF_Source::SourceZ::NONE),
+
+    // @Param: 2_YAW
+    // @DisplayName: Yaw Source (Secondary)
+    // @Description: Yaw Source (Secondary)
+    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
+    // @User: Advanced
+    AP_GROUPINFO("2_YAW", 10, AP_NavEKF_Source, _source_set[1].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::COMPASS),
+#endif
+
+#if AP_NAKEKF_SOURCE_SET_MAX >= 3
+    // @Param: 3_POSXY
+    // @DisplayName: Position Horizontal Source (Tertiary)
+    // @Description: Position Horizontal Source (Tertiary)
+    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("3_POSXY", 11, AP_NavEKF_Source, _source_set[2].posxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
+
+    // @Param: 3_VELXY
+    // @DisplayName: Velocity Horizontal Source (Tertiary)
+    // @Description: Velocity Horizontal Source (Tertiary)
+    // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
+    // @User: Advanced
+    AP_GROUPINFO("3_VELXY", 12, AP_NavEKF_Source, _source_set[2].velxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
+
+    // @Param: 3_POSZ
+    // @DisplayName: Position Vertical Source (Tertiary)
+    // @Description: Position Vertical Source (Tertiary)
+    // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("3_POSZ", 13, AP_NavEKF_Source, _source_set[2].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),
+
+    // @Param: 3_VELZ
+    // @DisplayName: Velocity Vertical Source (Tertiary)
+    // @Description: Velocity Vertical Source (Tertiary)
+    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
+    // @User: Advanced
+    AP_GROUPINFO("3_VELZ", 14, AP_NavEKF_Source, _source_set[2].velz, (int8_t)AP_NavEKF_Source::SourceZ::NONE),
+
+    // @Param: 3_YAW
+    // @DisplayName: Yaw Source (Tertiary)
+    // @Description: Yaw Source (Tertiary)
+    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
+    // @User: Advanced
+    AP_GROUPINFO("3_YAW", 15, AP_NavEKF_Source, _source_set[2].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::COMPASS),
+#endif
+
+    // @Param: _OPTIONS
+    // @DisplayName: EKF Source Options
+    // @Description: EKF Source Options
+    // @Bitmask: 0:FuseAllVelocities
+    // @User: Advanced
+    AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, 0),
+
+    AP_GROUPEND
+};
+
+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 = (SourceXY)_source_set[0].posxy.get();
+    _active_source_set.velxy = (SourceXY)_source_set[0].velxy.get();
+    _active_source_set.posz = (SourceZ)_source_set[0].posz.get();
+    _active_source_set.velz = (SourceZ)_source_set[0].velz.get();
+    _active_source_set.yaw = (SourceYaw)_source_set[0].yaw.get();
+
+    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;
+    }
+
+    _active_source_set.posxy = (SourceXY)_source_set[source_set_idx].posxy.get();
+    _active_source_set.velxy = (SourceXY)_source_set[source_set_idx].velxy.get();
+    _active_source_set.posz = (SourceZ)_source_set[source_set_idx].posz.get();
+    _active_source_set.velz = (SourceZ)_source_set[source_set_idx].velz.get();
+    _active_source_set.yaw = (SourceYaw)_source_set[source_set_idx].yaw.get();
+}
+
+// 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) {
+        return true;
+    }
+
+    // check for fuse all velocities
+    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
+        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+            if ((SourceXY)_source_set[i].velxy.get() == velxy_source) {
+                return true;
+            }
+        }
+    }
+
+    // if we got this far source should not be used
+    return false;
+}
+
+bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
+{
+    if (velz_source == _active_source_set.velz) {
+        return true;
+    }
+
+    // check for fuse all velocities
+    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
+        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+            if ((SourceZ)_source_set[i].velz.get() == velz_source) {
+                return true;
+            }
+        }
+    }
+
+    // if we got this far source should not be used
+    return false;
+}
+
+// true if a velocity source is configured
+bool AP_NavEKF_Source::haveVelZSource() const
+{
+    if (_active_source_set.velz != SourceZ::NONE) {
+        return true;
+    }
+
+    // check for fuse all velocities
+    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
+        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+            if ((SourceZ)_source_set[i].velz.get() != SourceZ::NONE) {
+                return true;
+            }
+        }
+    }
+
+    // if we got this far no velocity z source has been configured
+    return false;
+}
+
+// align position of inactive sources to ahrs
+void AP_NavEKF_Source::align_inactive_sources()
+{
+    // align visual odometry
+#if HAL_VISUALODOM_ENABLED
+
+    auto *visual_odom = AP::dal().visualodom();
+    if (!visual_odom || !visual_odom->enabled()) {
+        return;
+    }
+
+    // consider aligning XY position:
+    bool align_posxy = false;
+    if ((getPosXYSource() == SourceXY::GPS) ||
+        (getPosXYSource() == SourceXY::BEACON)) {
+        // only align position if active source is GPS or Beacon
+        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+            if ((SourceXY)_source_set[i].posxy.get() == SourceXY::EXTNAV) {
+                // ExtNav could potentially be used, so align it
+                align_posxy = true;
+                break;
+            }
+        }
+    }
+
+    // consider aligning Z position:
+    bool align_posz = false;
+    if ((getPosZSource() == SourceZ::BARO) ||
+        (getPosZSource() == SourceZ::RANGEFINDER) ||
+        (getPosZSource() == SourceZ::GPS) ||
+        (getPosZSource() == SourceZ::BEACON)) {
+        // ExtNav is not the active source; we do not want to align active source!
+        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+            if ((SourceZ)_source_set[i].posz.get() == SourceZ::EXTNAV) {
+                // ExtNav could potentially be used, so align it
+                align_posz = true;
+                break;
+            }
+        }
+    }
+    visual_odom->align_position_to_ahrs(align_posxy, align_posz);
+#endif
+}
+
+// sensor specific helper functions
+bool AP_NavEKF_Source::usingGPS() const
+{
+    return getPosXYSource() == SourceXY::GPS ||
+           getPosZSource() == SourceZ::GPS ||
+           getVelXYSource() == SourceXY::GPS ||
+           getVelZSource() == SourceZ::GPS;
+}
+
+// true if some parameters have been configured (used during parameter conversion)
+bool AP_NavEKF_Source::any_params_configured_in_storage() const
+{
+    return _source_set[0].posxy.configured_in_storage() ||
+           _source_set[0].velxy.configured_in_storage() ||
+           _source_set[0].posz.configured_in_storage() ||
+           _source_set[0].velz.configured_in_storage() ||
+           _source_set[0].yaw.configured_in_storage();
+}
+
+// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
+bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
+{
+    auto &dal = AP::dal();
+    bool baro_required = false;
+    bool beacon_required = false;
+    bool compass_required = false;
+    bool gps_required = false;
+    bool rangefinder_required = false;
+    bool visualodom_required = false;
+    bool optflow_required = false;
+
+    // check source params are valid
+    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+
+        // check posxy
+        switch ((SourceXY)_source_set[i].posxy.get()) {
+        case SourceXY::NONE:
+            break;
+        case SourceXY::GPS:
+            gps_required = true;
+            break;
+        case SourceXY::BEACON:
+            beacon_required = true;
+            break;
+        case SourceXY::EXTNAV:
+            visualodom_required = true;
+            break;
+        case SourceXY::OPTFLOW:
+        case SourceXY::WHEEL_ENCODER:
+        default:
+            // invalid posxy value
+            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i+1);
+            return false;
+        }
+
+        // check velxy
+        switch ((SourceXY)_source_set[i].velxy.get()) {
+        case SourceXY::NONE:
+            break;
+        case SourceXY::GPS:
+            gps_required = true;
+            break;
+        case SourceXY::OPTFLOW:
+            optflow_required = true;
+            break;
+        case SourceXY::EXTNAV:
+            visualodom_required = true;
+            break;
+        case SourceXY::WHEEL_ENCODER:
+            // ToDo: add wheelencoder_required and test below
+            break;
+        case SourceXY::BEACON:
+        default:
+            // invalid velxy value
+            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i+1);
+            return false;
+        }
+
+        // check posz
+        switch ((SourceZ)_source_set[i].posz.get()) {
+        case SourceZ::BARO:
+            baro_required = true;
+            break;
+        case SourceZ::RANGEFINDER:
+            rangefinder_required = true;
+            break;
+        case SourceZ::GPS:
+            gps_required = true;
+            break;
+        case SourceZ::BEACON:
+            beacon_required = true;
+            break;
+        case SourceZ::EXTNAV:
+            visualodom_required = true;
+            break;
+        case SourceZ::NONE:
+        default:
+            // invalid posz value
+            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i+1);
+            return false;
+        }
+
+        // check velz
+        switch ((SourceZ)_source_set[i].velz.get()) {
+        case SourceZ::NONE:
+            break;
+        case SourceZ::GPS:
+            gps_required = true;
+            break;
+        case SourceZ::EXTNAV:
+            visualodom_required = true;
+            break;
+        case SourceZ::BARO:
+        case SourceZ::RANGEFINDER:
+        case SourceZ::BEACON:
+        default:
+            // invalid velz value
+            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i+1);
+            return false;
+        }
+
+        // check yaw
+        switch ((SourceYaw)_source_set[i].yaw.get()) {
+        case SourceYaw::NONE:
+        case SourceYaw::COMPASS:
+        case SourceYaw::EXTERNAL:
+        case SourceYaw::EXTERNAL_COMPASS_FALLBACK:
+            // valid yaw value
+            break;
+        default:
+            // invalid yaw value
+            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i+1);
+            return false;
+        }
+    }
+
+    // check all required sensors are available
+    const char* ekf_requires_msg = "EK3 sources require %s";
+    if (baro_required && (dal.baro().num_instances() == 0)) {
+        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Baro");
+        return false;
+    }
+
+    if (beacon_required && (dal.beacon() == nullptr || !dal.beacon()->enabled())) {
+        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Beacon");
+        return false;
+    }
+
+    if (compass_required && dal.compass().get_num_enabled() == 0) {
+        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Compass");
+        return false;
+    }
+
+    if (gps_required && (dal.gps().num_sensors() == 0)) {
+        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "GPS");
+        return false;
+    }
+
+    if (optflow_required && !dal.opticalflow_enabled()) {
+        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "OpticalFlow");
+        return false;
+    }
+
+    if (rangefinder_required && (dal.rangefinder() == nullptr || !dal.rangefinder()->has_orientation(ROTATION_PITCH_270))) {
+        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "RangeFinder");
+        return false;
+    }
+
+    if (visualodom_required) {
+        bool visualodom_available = false;
+#if HAL_VISUALODOM_ENABLED
+        auto *vo = AP::dal().visualodom();
+        visualodom_available = vo && vo->enabled();
+#endif
+        if (!visualodom_available) {
+            hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "VisualOdom");
+            return false;
+        }
+    }
+
+    return true;
+}
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.h
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.h
@ -0,0 +1,115 @@
				@@ -0,0 +1,115 @@
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+
+#define AP_NAKEKF_SOURCE_SET_MAX 3  // three sets of sources
+
+class AP_NavEKF_Source
+{
+
+public:
+    // Constructor
+    AP_NavEKF_Source();
+
+    /* Do not allow copies */
+    AP_NavEKF_Source(const AP_NavEKF_Source &other) = delete;
+    AP_NavEKF_Source &operator=(const AP_NavEKF_Source&) = delete;
+
+    enum class SourceXY {
+        NONE = 0,
+        // BARO = 1 (not applicable)
+        // RANGEFINDER = 2 (not applicable)
+        GPS = 3,
+        BEACON = 4,
+        OPTFLOW = 5,
+        EXTNAV = 6,
+        WHEEL_ENCODER = 7
+    };
+
+    enum class SourceZ {
+        NONE = 0,
+        BARO = 1,
+        RANGEFINDER = 2,
+        GPS = 3,
+        BEACON = 4,
+        // OPTFLOW = 5 (not applicable, optical flow can be used for terrain alt but not relative or absolute alt)
+        EXTNAV = 6
+        // WHEEL_ENCODER = 7 (not applicable)
+    };
+
+    enum class SourceYaw {
+        NONE = 0,
+        COMPASS = 1,
+        EXTERNAL = 2,
+        EXTERNAL_COMPASS_FALLBACK = 3
+    };
+
+    // enum for OPTIONS parameter
+    enum class SourceOptions {
+        FUSE_ALL_VELOCITIES = (1 << 0)  // fuse all velocities configured in source sets
+    };
+
+    // initialisation
+    void init();
+
+    // get current position source
+    SourceXY getPosXYSource() const { return _active_source_set.posxy; }
+    SourceZ getPosZSource() const { return _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; }
+    void setVelZSource(SourceZ source) { _active_source_set.velz = source; }
+
+    // true/false of whether velocity source should be used
+    bool useVelXYSource(SourceXY velxy_source) const;
+    bool useVelZSource(SourceZ velz_source) const;
+
+    // true if a velocity source is configured
+    bool haveVelZSource() const;
+
+    // get yaw source
+    SourceYaw getYawSource() const { return _active_source_set.yaw; }
+
+    // align position of inactive sources to ahrs
+    void align_inactive_sources();
+
+    // sensor-specific helper functions
+
+    // true if any source is GPS
+    bool usingGPS() const;
+
+    // true if any primary source parameters have been configured (used for parameter conversion)
+    bool any_params_configured_in_storage() const;
+
+    // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
+    bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    // Parameters
+    struct {
+        AP_Int8 posxy;  // xy position source
+        AP_Int8 velxy;  // xy velocity source
+        AP_Int8 posz;   // position z (aka altitude or height) source
+        AP_Int8 velz;   // velocity z source
+        AP_Int8 yaw;    // yaw source
+    } _source_set[AP_NAKEKF_SOURCE_SET_MAX];
+
+    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
+};