AP_AHRS: added AHRS_EKF_TYPE=11 for external AHRS

2020-12-29 15:02:43 +11:00 · 2020-12-29 15:02:43 +11:00 · e284c5694d
parent b74e5c0e2f
commit e284c5694d
2 changed files with 283 additions and 3 deletions
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@ -130,6 +130,10 @@ void AP_AHRS_NavEKF::update(bool skip_ins_update)
    update_SITL();
 #endif

+#if HAL_EXTERNAL_AHRS_ENABLED
+    update_external();
+#endif
+    
    if (_ekf_type == 2) {
        // if EK2 is primary then run EKF2 first to give it CPU
        // priority
@ -410,6 +414,34 @@ void AP_AHRS_NavEKF::update_SITL(void)
 }
 #endif // CONFIG_HAL_BOARD

+#if HAL_EXTERNAL_AHRS_ENABLED
+void AP_AHRS_NavEKF::update_external(void)
+{
+    if (active_EKF_type() == EKFType::EXTERNAL) {
+        Quaternion quat;
+        if (!AP::externalAHRS().get_quaternion(quat)) {
+            return;
+        }
+        quat.rotation_matrix(_dcm_matrix);
+        _dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body();
+        _dcm_matrix.to_euler(&roll, &pitch, &yaw);
+
+        update_cd_values();
+        update_trig();
+
+        _gyro_drift.zero();
+
+        _gyro_estimate = AP::externalAHRS().get_gyro();
+
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            Vector3f accel = AP::externalAHRS().get_accel();
+            _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
+        }
+        _accel_ef_ekf_blended = _accel_ef_ekf[0];
+    }
+}
+#endif // HAL_EXTERNAL_AHRS_ENABLED
+
 // accelerometer values in the earth frame in m/s/s
 const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const
 {
@ -503,6 +535,12 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
        break;
    }
 #endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL: {
+        return AP::externalAHRS().get_location(loc);
+    }
+#endif
    }

    // fall back to position from DCM
@ -550,6 +588,12 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) const
        break;
 #endif

+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        wind.zero();
+        break;
+#endif
+        
    }
    return wind;
 }
@ -582,6 +626,11 @@ bool AP_AHRS_NavEKF::airspeed_vector_true(Vector3f &vec) const
    case EKFType::SITL:
        break;
 #endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        break;
+#endif
    }
    return false;
 }
@ -606,6 +655,12 @@ bool AP_AHRS_NavEKF::use_compass(void)
    case EKFType::SITL:
        return true;
 #endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        // fall through
+        break;
+#endif
    }
    return AP_AHRS_DCM::use_compass();
 }
@ -635,6 +690,10 @@ bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const
        } else {
            return false;
        }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return AP::externalAHRS().get_quaternion(quat);
 #endif
    }
    // since there is no default case above, this is unreachable
@ -657,6 +716,16 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
        case EKFType::TWO:
            EKF2.getEulerAngles(-1, eulers);
            return _ekf2_started;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+        case EKFType::EXTERNAL: {
+            Quaternion quat;
+            if (!AP::externalAHRS().get_quaternion(quat)) {
+                return false;
+            }
+            quat.to_euler(eulers.x, eulers.y, eulers.z);
+            return true;
+        }
 #endif
        default:
            return false;
@ -671,6 +740,9 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const

 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
 #endif
        // DCM is secondary
        eulers = _dcm_attitude;
@ -690,6 +762,8 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const
 #if HAL_NAVEKF2_AVAILABLE
        EKF2.getQuaternion(-1, quat);
        return _ekf2_started;
+#elif HAL_EXTERNAL_AHRS_ENABLED
+        return AP::externalAHRS().get_quaternion(quat);
 #else
        return false;
 #endif
@ -704,6 +778,9 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const

 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
 #endif
        // DCM is secondary
        quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
@ -722,6 +799,8 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const
 #if HAL_NAVEKF2_AVAILABLE
        EKF2.getLLH(loc);
        return _ekf2_started;
+#elif HAL_EXTERNAL_AHRS_ENABLED
+        return AP::externalAHRS().get_location(loc);
 #else
        return false;
 #endif
@ -736,6 +815,9 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const

 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
 #endif
        // return DCM position
        AP_AHRS_DCM::get_position(loc);
@ -774,6 +856,11 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
        }
        break;
    }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL: {
+        return AP::externalAHRS().get_groundspeed_vector();
+    }
 #endif
    }
    return AP_AHRS_DCM::groundspeed_vector();
@ -811,6 +898,11 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc)
        // never allow origin set in SITL. The origin is set by the
        // simulation backend
        return false;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        // don't allow origin set with external AHRS
+        return false;
 #endif
    }
    // since there is no default case above, this is unreachable
@ -844,13 +936,18 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
 #endif

 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    case EKFType::SITL:
+    case EKFType::SITL: {
        if (!_sitl) {
            return false;
        }
        const struct SITL::sitl_fdm &fdm = _sitl->state;
        vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD);
        return true;
+    }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return AP::externalAHRS().get_velocity_NED(vec);
 #endif
    }
    return AP_AHRS_DCM::get_velocity_NED(vec);
@ -878,6 +975,10 @@ bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return false;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
 #endif
    }
    return false;
@ -905,6 +1006,10 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return false;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
 #endif
    }
    // since there is no default case above, this is unreachable
@ -940,6 +1045,10 @@ bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const
        } else {
            return false;
        }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return AP::externalAHRS().get_speed_down(velocity);
 #endif
    }
    // since there is no default case above, this is unreachable
@ -972,6 +1081,11 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const
        height = fdm.altitude - get_home().alt*0.01f;
        return true;
    }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL: {
+        return false;
+    }
 #endif
    }
    // since there is no default case above, this is unreachable
@ -1032,6 +1146,20 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const
                       -(fdm.altitude - orgn.alt*0.01f));
        return true;
    }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL: {
+        auto &serahrs = AP::externalAHRS();
+        Location loc, orgn;
+        if (serahrs.get_origin(orgn) &&
+            serahrs.get_location(loc)) {
+            const Vector2f diff2d = orgn.get_distance_NE(loc);
+            vec = Vector3f(diff2d.x, diff2d.y,
+                           -(loc.alt - orgn.alt)*0.01);
+            return true;
+        }
+        return false;
+    }
 #endif
    }
    // since there is no default case above, this is unreachable
@ -1089,6 +1217,17 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const
        posNE = orgn.get_distance_NE(loc);
        return true;
    }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL: {
+        Location loc, orgn;
+        if (!get_position(loc) ||
+            !get_origin(orgn)) {
+            return false;
+        }
+        posNE = orgn.get_distance_NE(loc);
+        return true;
+    }
 #endif
    }
    // since there is no default case above, this is unreachable
@ -1151,6 +1290,17 @@ bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const
        posD = -(fdm.altitude - orgn.alt*0.01f);
        return true;
    }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL: {
+        Location orgn, loc;
+        if (!get_origin(orgn) ||
+            !get_position(loc)) {
+            return false;
+        }
+        posD = -(loc.alt - orgn.alt)*0.01;
+        return true;
+    }
 #endif
    }
    // since there is no default case above, this is unreachable
@ -1184,6 +1334,10 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::ekf_type(void) const
    case EKFType::SITL:
        return type;
 #endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return type;
+#endif
 #if HAL_NAVEKF2_AVAILABLE
    case EKFType::TWO:
        return type;
@ -1264,6 +1418,11 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
    case EKFType::SITL:
        ret = EKFType::SITL;
        break;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        ret = EKFType::EXTERNAL;
+        break;
 #endif
    }

@ -1297,6 +1456,12 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
        if (ret == EKFType::SITL) {
            get_filter_status(filt_state);
        }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+        if (ret == EKFType::EXTERNAL) {
+            get_filter_status(filt_state);
+            should_use_gps = true;
+        }
 #endif
        if (hal.util->get_soft_armed() &&
            (!filt_state.flags.using_gps ||
@ -1388,6 +1553,10 @@ bool AP_AHRS_NavEKF::healthy(void) const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return true;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return AP::externalAHRS().healthy();
 #endif
    }

@ -1412,6 +1581,11 @@ bool AP_AHRS_NavEKF::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) c
    case EKFType::NONE:
        return ret;

+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return AP::externalAHRS().pre_arm_check(failure_msg, failure_msg_len);
+#endif
+        
 #if HAL_NAVEKF2_AVAILABLE
    case EKFType::TWO:
        if (!_ekf2_started) {
@ -1463,6 +1637,10 @@ bool AP_AHRS_NavEKF::initialised(void) const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return true;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return AP::externalAHRS().initialised();
 #endif
    }
    return false;
@ -1500,6 +1678,11 @@ bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const
        status.flags.pred_horiz_pos_abs = 1;
        status.flags.using_gps = 1;
        return true;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        AP::externalAHRS().get_filter_status(status);
+        return true;
 #endif
    }

@ -1583,6 +1766,11 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel
        ekfGndSpdLimit = 400.0f;
        ekfNavVelGainScaler = 1.0f;
        break;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        // no limits
+        break;
 #endif
    }
 }
@ -1609,6 +1797,10 @@ bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
    case EKFType::SITL:
        magOffsets.zero();
        return true;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
 #endif
    }
    // since there is no default case above, this is unreachable
@ -1638,6 +1830,10 @@ void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) cons
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        break;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        break;
 #endif
    }
    if (imu_idx == -1) {
@ -1757,6 +1953,10 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return 0;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return 0;
 #endif
    }
    return 0;
@ -1784,6 +1984,10 @@ uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos)
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return 0;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return 0;
 #endif
    }
    return 0;
@ -1811,6 +2015,10 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return 0;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return 0;
 #endif
    }
    return 0;
@ -1838,6 +2046,10 @@ uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta)
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return 0;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return 0;
 #endif
    }
    return 0;
@ -1883,6 +2095,10 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void)
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return false;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
 #endif
    }
    return false;
@ -1917,6 +2133,13 @@ void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const
        break;
 #endif

+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        // send zero status report
+        mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0, 0);
+        break;
+#endif
+
 #if HAL_NAVEKF2_AVAILABLE
    case EKFType::TWO:
        return EKF2.send_status_report(chan);
@ -1956,13 +2179,18 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const
 #endif

 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    case EKFType::SITL:
+    case EKFType::SITL: {
        if (!_sitl) {
            return false;
        }
        const struct SITL::sitl_fdm &fdm = _sitl->state;
        ret = fdm.home;
        return true;
+    }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return AP::externalAHRS().get_origin(ret);
 #endif
    }

@ -1992,6 +2220,10 @@ bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return false;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
 #endif
    }

@ -2050,6 +2282,10 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        return get_position(loc);
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return get_position(loc);
 #endif
    }

@ -2088,6 +2324,11 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec
        yawInnov = 0.0f;
        return true;
 #endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
+#endif
    }

    return false;
@ -2131,6 +2372,11 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
        tasVar = 0;
        return true;
 #endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
+#endif
    }

    return false;
@ -2162,6 +2408,11 @@ bool AP_AHRS_NavEKF::get_vel_innovations_and_variances_for_source(uint8_t source
        // SITL does not support source level variances
        return false;
 #endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        return false;
+#endif
    }

    return false;
@ -2267,6 +2518,10 @@ uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
        break;
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        break;
 #endif
    }
    if (imu == -1) {
@ -2289,7 +2544,10 @@ int8_t AP_AHRS_NavEKF::get_primary_core_index() const
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
 #endif
-        // SITL and DCM have only one core
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+#endif
+        // we have only one core
        return 0;

 #if HAL_NAVEKF2_AVAILABLE
@ -2338,6 +2596,11 @@ void AP_AHRS_NavEKF::check_lane_switch(void)
        break;
 #endif

+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        break;
+#endif
+        
 #if HAL_NAVEKF2_AVAILABLE
    case EKFType::TWO:
        EKF2.checkLaneSwitch();
@ -2364,6 +2627,11 @@ void AP_AHRS_NavEKF::request_yaw_reset(void)
        break;
 #endif

+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        break;
+#endif
+        
 #if HAL_NAVEKF2_AVAILABLE
    case EKFType::TWO:
        EKF2.requestYawReset();
@ -2416,6 +2684,9 @@ bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const
 #endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKFType::SITL:
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
 #endif
        return false; 
    }
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@ -39,6 +39,8 @@
 #include <SITL/SITL.h>
 #endif

+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+
 #include <AP_NavEKF2/AP_NavEKF2.h>
 #include <AP_NavEKF3/AP_NavEKF3.h>
 #include <AP_NavEKF/AP_Nav_Common.h>              // definitions shared by inertial and ekf nav filters
@ -334,6 +336,9 @@ private:
 #endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        ,SITL = 10
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+        ,EXTERNAL = 11
 #endif
    };
    EKFType active_EKF_type(void) const;
@ -384,4 +389,8 @@ private:
    uint32_t _last_body_odm_update_ms;
    void update_SITL(void);
 #endif    
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    void update_external(void);
+#endif    
 };