AP_AHRS: add AP_AHRS_NavEKF3 shim

libraries/AP_AHRS/AP_AHRS.h

@@ -27,7 +27,7 @@
 
 #include "AP_AHRS_Backend.h"
 #include <AP_NavEKF2/AP_NavEKF2.h>
-#include <AP_NavEKF3/AP_NavEKF3.h>
+#include "AP_AHRS_NavEKF3.h"
 #include <AP_NavEKF/AP_Nav_Common.h>              // definitions shared by inertial and ekf nav filters
 
 #include "AP_AHRS_DCM.h"
@@ -74,6 +74,10 @@ public:
         return _singleton;
     }
 
+#if AP_AHRS_NAVEKF3_ENABLED
+    AP_AHRS_NavEKF3 ekf3;
+#endif
+
     // periodically checks to see if we should update the AHRS
     // orientation (e.g. based on the AHRS_ORIENTATION parameter)
     // allow for runtime change of orientation
@@ -473,7 +477,7 @@ public:
 #if AP_AHRS_DCM_ENABLED
         DCM = 0,
 #endif
-#if HAL_NAVEKF3_AVAILABLE
+#if AP_AHRS_NAVEKF3_ENABLED
         THREE = 3,
 #endif
 #if HAL_NAVEKF2_AVAILABLE
@@ -500,9 +504,6 @@ public:
 #if HAL_NAVEKF2_AVAILABLE
     NavEKF2 EKF2;
 #endif
-#if HAL_NAVEKF3_AVAILABLE
-    NavEKF3 EKF3;
-#endif
 
     // for holding parameters
     static const struct AP_Param::GroupInfo var_info[];
@@ -818,7 +819,6 @@ private:
     bool _ekf2_started;
 #endif
 #if HAL_NAVEKF3_AVAILABLE
-    bool _ekf3_started;
     void update_EKF3(void);
 #endif
 
@@ -1060,9 +1060,12 @@ private:
     AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats};
     struct AP_AHRS_Backend::Estimates dcm_estimates;
 #endif
+#if AP_AHRS_NAVEKF3_ENABLED
+    struct AP_AHRS_Backend::Estimates ekf3_estimates;
+#endif
 #if AP_AHRS_SIM_ENABLED
 #if HAL_NAVEKF3_AVAILABLE
-    AP_AHRS_SIM sim{EKF3};
+    AP_AHRS_SIM sim{ekf3.EKF3};
 #else
     AP_AHRS_SIM sim;
 #endif

libraries/AP_AHRS/AP_AHRS_NavEKF3.cpp

@@ -0,0 +1,83 @@
+#include "AP_AHRS_config.h"
+
+#if AP_AHRS_NAVEKF3_ENABLED
+
+#include "AP_AHRS_NavEKF3.h"
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+void AP_AHRS_NavEKF3::get_results(AP_AHRS_Backend::Estimates &results)
+{
+    /*
+     * attitude estimates:
+     */
+    EKF3.getRotationBodyToNED(results.dcm_matrix);
+
+    Vector3f eulers;
+    EKF3.getEulerAngles(eulers);
+    results.roll_rad  = eulers.x;
+    results.pitch_rad = eulers.y;
+    results.yaw_rad   = eulers.z;
+
+    EKF3.getQuaternion(results.quaternion);
+    results.quaternion.rotate(-AP::ahrs().get_trim());
+
+    results.attitude_valid = started;
+
+    /*
+     * rotational rate estimates:
+     */
+    const AP_InertialSensor &_ins = AP::ins();
+
+    // Use the primary EKF to select the primary gyro
+    const int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex();
+    const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_first_usable_gyro();
+    const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_first_usable_accel();
+
+    // get gyro bias for primary EKF and change sign to give gyro drift
+    // Note sign convention used by EKF is bias = measurement - truth
+    Vector3f drift;
+    EKF3.getGyroBias(-1, drift);
+    results.gyro_drift = -drift;
+
+    // use the same IMU as the primary EKF and correct for gyro drift
+    results.gyro_estimate = _ins.get_gyro(primary_gyro) + results.gyro_drift;
+
+    /*
+     * acceleration estimates
+     */
+    // get 3-axis accel bias estimates for active EKF (this is usually
+    // for the primary IMU)
+    EKF3.getAccelBias(-1, results.accel_bias);
+
+    // use the primary IMU for accel earth frame
+    Vector3f accel = _ins.get_accel(primary_accel);
+    accel -= results.accel_bias;
+    results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
+
+    /*
+     * velocity estimates
+     */
+    EKF3.getVelNED(results.velocity_NED);
+    results.velocity_NED_valid = true;
+    results.vert_pos_rate_D = EKF3.getPosDownDerivative();
+    results.vert_pos_rate_D_valid = true;
+
+    /*
+     * position estimates
+     */
+    results.location_valid = EKF3.getLLH(results.location);
+}
+
+bool AP_AHRS_NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
+{
+    if (!started) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 not started");
+        return false;
+    }
+    return EKF3.pre_arm_check(requires_position, failure_msg, failure_msg_len);
+}
+
+#endif  // AP_AHRS_NAVEKF3_ENABLED

libraries/AP_AHRS/AP_AHRS_NavEKF3.h

@@ -0,0 +1,153 @@
+#pragma once
+
+/*
+   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/>.
+ */
+
+/*
+ *  shim AP_NavEKF3 into AP_AHRS_Backend
+ */
+
+#include "AP_AHRS_config.h"
+
+#if AP_AHRS_NAVEKF3_ENABLED
+
+#include <AP_NavEKF3/AP_NavEKF3.h>
+#include "AP_AHRS_Backend.h"
+
+class AP_AHRS_NavEKF3 : public AP_AHRS_Backend {
+public:
+
+    /* Do not allow copies */
+    CLASS_NO_COPY(AP_AHRS_NavEKF3);
+    AP_AHRS_NavEKF3() {}
+
+    bool healthy(void) const override {
+        if (!started) {
+            return false;
+        }
+        if (!EKF3.healthy()) {
+            return false;
+        }
+        return true;
+    }
+
+    void reset_gyro_drift() override { EKF3.resetGyroBias(); }
+
+    void update() override { EKF3.UpdateFilter(); }
+
+    void get_results(Estimates &results) override;
+    void reset() override {
+        if (!started) {
+            return;
+        }
+        started = EKF3.InitialiseFilter();
+    }
+
+    bool get_origin(Location &ret) const override {
+        return EKF3.getOriginLLH(ret);
+    }
+    bool set_origin(const Location &loc) override {
+        return EKF3.setOriginLLH(loc);
+    }
+
+    // return a wind estimation vector, in m/s
+    bool wind_estimate(Vector3f &wind) const override {
+        return EKF3.getWind(wind);
+    }
+
+    Vector2f groundspeed_vector(void) override {
+        Vector3f vec;
+        EKF3.getVelNED(vec);
+        return vec.xy();
+    }
+
+    bool            use_compass() override {
+        return EKF3.use_compass();
+    }
+
+    // Set to true if the terrain underneath is stable enough to be used as a height reference
+    // this is not related to terrain following
+    void set_terrain_hgt_stable(bool stable) override {
+        EKF3.setTerrainHgtStable(stable);
+    }
+
+    uint32_t getLastYawResetAngle(float &yawAng) override {
+        return EKF3.getLastYawResetAngle(yawAng);
+    };
+    uint32_t getLastPosNorthEastReset(Vector2f &pos) override WARN_IF_UNUSED {
+        return EKF3.getLastPosNorthEastReset(pos);
+    };
+    uint32_t getLastVelNorthEastReset(Vector2f &vel) const override WARN_IF_UNUSED {
+        return EKF3.getLastVelNorthEastReset(vel);
+    };
+    uint32_t getLastPosDownReset(float &posDelta) override WARN_IF_UNUSED {
+        return EKF3.getLastPosDownReset(posDelta);
+    };
+    void resetHeightDatum(void) override {
+        EKF3.resetHeightDatum();
+    }
+    void request_yaw_reset() override {
+        EKF3.requestYawReset();
+    }
+    // get latest altitude estimate above ground level in meters and validity flag
+    bool get_hagl(float &hagl) const override WARN_IF_UNUSED {
+        return EKF3.getHAGL(hagl);
+    }
+
+    bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override;
+
+    void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override {
+        return EKF3.getEkfControlLimits(ekfGndSpdLimit, controlScaleXY);
+    }
+    bool get_hgt_ctrl_limit(float &limit) const override WARN_IF_UNUSED {
+        return EKF3.getHeightControlLimit(limit);
+    }
+
+    void send_ekf_status_report(class GCS_MAVLINK &link) const override {
+        EKF3.send_status_report(link);
+    }
+
+    // get_filter_status - returns filter status as a series of flags
+    bool get_filter_status(nav_filter_status &status) const override {
+        EKF3.getFilterStatus(status);
+        return true;
+    }
+
+    // return the innovations for the specified instance
+    // An out of range instance (eg -1) returns data for the primary instance
+    bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override {
+        return EKF3.getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
+    }
+
+    bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override {
+        Vector2f offset;
+        return EKF3.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
+    }
+
+    bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const override WARN_IF_UNUSED {
+        return EKF3.getVelInnovationsAndVariancesForSource((AP_NavEKF_Source::SourceXY)source, innovations, variances);
+    }
+
+    void request_yaw_reset(void) override {
+        EKF3.requestYawReset();
+    }
+
+    // this is out here so parameters can be poked into it
+    NavEKF3 EKF3;
+
+    bool started;
+};
+
+#endif  // AP_AHRS_NAVEKF3_ENABLED

libraries/AP_AHRS/AP_AHRS_config.h

@@ -32,6 +32,11 @@
 #ifndef HAL_NAVEKF3_AVAILABLE
 #define HAL_NAVEKF3_AVAILABLE AP_AHRS_BACKEND_DEFAULT_ENABLED && AP_INERTIALSENSOR_ENABLED
 #endif
+#endif
+
+#ifndef AP_AHRS_NAVEKF3_ENABLED
+#define AP_AHRS_NAVEKF3_ENABLED HAL_NAVEKF3_AVAILABLE
+#endif
 
 #ifndef AP_AHRS_SIM_ENABLED
 #define AP_AHRS_SIM_ENABLED AP_AHRS_BACKEND_DEFAULT_ENABLED && AP_SIM_ENABLED && AP_INERTIALSENSOR_ENABLED