AP_AHRS: added EKF type 10 for SITL

this bypasses all attitude and position estimators and uses the SITL
state directly. It can be used for when the SITL backend cannot
provide perfect sensor data

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -28,6 +28,16 @@
 
 extern const AP_HAL::HAL& hal;
 
+// constructor
+AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng,
+                               NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags) :
+    AP_AHRS_DCM(ins, baro, gps),
+    EKF1(_EKF1),
+    EKF2(_EKF2),
+    _flags(flags)
+{
+}
+
 // return the smoothed gyro vector corrected for drift
 const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const
 {
@@ -70,6 +80,9 @@ void AP_AHRS_NavEKF::update(void)
     update_DCM();
     update_EKF1();
     update_EKF2();
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    update_SITL();
+#endif
 }
 
 void AP_AHRS_NavEKF::update_DCM(void)
@@ -220,6 +233,40 @@ void AP_AHRS_NavEKF::update_EKF2(void)
     }
 }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+void AP_AHRS_NavEKF::update_SITL(void)
+{
+    if (_sitl == nullptr) {
+        _sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
+    }
+    if (_sitl && active_EKF_type() == EKF_TYPE_SITL) {
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+
+        roll  = radians(fdm.rollDeg);
+        pitch = radians(fdm.pitchDeg);
+        yaw   = radians(fdm.yawDeg);
+
+        _dcm_matrix.from_euler(roll, pitch, yaw);
+
+        update_cd_values();
+        update_trig();
+
+        _gyro_bias.zero();
+
+        _gyro_estimate = Vector3f(radians(fdm.rollRate),
+                                  radians(fdm.pitchRate),
+                                  radians(fdm.yawRate));
+
+        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            _accel_ef_ekf[i] = Vector3f(fdm.xAccel,
+                                        fdm.yAccel,
+                                        fdm.zAccel);
+        }
+        _accel_ef_ekf_blended = _accel_ef_ekf[0];
+    }
+}
+#endif // CONFIG_HAL_BOARD
+
 // accelerometer values in the earth frame in m/s/s
 const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const
 {
@@ -285,6 +332,17 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
         }
         break;
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL: {
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+        memset(&loc, 0, sizeof(loc));
+        loc.lat = fdm.latitude * 1e7;
+        loc.lng = fdm.longitude * 1e7;
+        loc.alt = fdm.altitude*100;
+        return true;
+    }
+#endif
+        
     default:
         break;
     }
@@ -318,6 +376,12 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void)
     case EKF_TYPE2:
         EKF2.getWind(-1,wind);
         break;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        wind.zero();
+        break;
+#endif
     }
     return wind;
 }
@@ -339,6 +403,10 @@ bool AP_AHRS_NavEKF::use_compass(void)
         return EKF1.use_compass();
     case EKF_TYPE2:
         return EKF2.use_compass();
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return true;
+#endif
     }
     return AP_AHRS_DCM::use_compass();
 }
@@ -397,6 +465,13 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
     case EKF_TYPE2:
         EKF2.getVelNED(-1,vec);
         return Vector2f(vec.x, vec.y);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL: {
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+        return Vector2f(fdm.speedN, fdm.speedE);
+    }
+#endif
     }
 }
 
@@ -427,6 +502,14 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
     case EKF_TYPE2:
         EKF2.getVelNED(-1,vec);
         return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL: {
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+        vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD);
+        return true;
+    }
+#endif
     }
 }
 
@@ -446,6 +529,14 @@ bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity)
     case EKF_TYPE2:
         velocity = EKF2.getPosDownDerivative(-1);
         return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL: {
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+        velocity = fdm.speedD;
+        return true;
+    }
+#endif
     }
 }
 
@@ -462,6 +553,14 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const
 
     case EKF_TYPE2:
         return EKF2.getHAGL(height);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL: {
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+        height = fdm.altitude - get_home().alt*0.01f;
+        return true;
+    }
+#endif
     }
 }
 
@@ -479,6 +578,18 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
 
     case EKF_TYPE2:
         return EKF2.getPosNED(-1,vec);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL: {
+        Location loc;
+        get_position(loc);
+        Vector2f diff2d = location_diff(get_home(), loc);
+        const struct SITL::sitl_fdm &fdm = _sitl->state;
+        vec = Vector3f(diff2d.x, diff2d.y,
+                       -(fdm.altitude - get_home().alt*0.01f));
+        return true;
+    }
+#endif
     }
 }
 
@@ -493,9 +604,15 @@ uint8_t AP_AHRS_NavEKF::ekf_type(void) const
     }
 
     // check for invalid type
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (type > 2 && type != EKF_TYPE_SITL) {
+        type = 1;
+    }
+#else
     if (type > 2) {
         type = 1;
     }
+#endif
     return type;
 }
 
@@ -540,6 +657,12 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
         }
         break;
     }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        ret = EKF_TYPE_SITL;
+        break;
+#endif
     }
 
     if (ret != EKF_TYPE_NONE &&
@@ -548,6 +671,10 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
         nav_filter_status filt_state;
         if (ret == EKF_TYPE1) {
             EKF1.getFilterStatus(filt_state);
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        } else if (ret == EKF_TYPE_SITL) {
+            get_filter_status(filt_state);
+#endif
         } else {
             EKF2.getFilterStatus(-1,filt_state);
         }
@@ -613,6 +740,11 @@ bool AP_AHRS_NavEKF::healthy(void) const
         }
         return true;
     }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return true;
+#endif
     }
 
     return AP_AHRS_DCM::healthy();
@@ -638,6 +770,11 @@ bool AP_AHRS_NavEKF::initialised(void) const
     case 2:
         // initialisation complete 10sec after ekf has started
         return (ekf2_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS));
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return true;
+#endif
     }
 };
 
@@ -656,6 +793,21 @@ bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const
     case EKF_TYPE2:
         EKF2.getFilterStatus(-1,status);
         return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        memset(&status, 0, sizeof(status));
+        status.flags.attitude = 1;
+        status.flags.horiz_vel = 1;
+        status.flags.vert_vel = 1;
+        status.flags.horiz_pos_rel = 1;
+        status.flags.horiz_pos_abs = 1;
+        status.flags.vert_pos = 1;
+        status.flags.pred_horiz_pos_rel = 1;
+        status.flags.pred_horiz_pos_abs = 1;
+        status.flags.using_gps = 1;
+        return true;
+#endif
     }
 
 }
@@ -678,6 +830,11 @@ uint8_t AP_AHRS_NavEKF::setInhibitGPS(void)
 
     case 2:
         return EKF2.setInhibitGPS();
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return false;
+#endif
     }
 }
 
@@ -694,6 +851,14 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel
     case 2:
         EKF2.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler);
         break;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        // same as EKF1 for no optical flow
+        ekfGndSpdLimit = 400.0f;
+        ekfNavVelGainScaler = 1.0f;
+        break;
+#endif
     }
 }
 
@@ -709,6 +874,12 @@ bool AP_AHRS_NavEKF::getMagOffsets(Vector3f &magOffsets)
 
     case 2:
         return EKF2.getMagOffsets(magOffsets);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        magOffsets.zero();
+        return true;
+#endif
     }
 }
 
@@ -735,6 +906,10 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) const
         return EKF1.getLastYawResetAngle(yawAng);
     case 2:
         return EKF2.getLastYawResetAngle(yawAng);
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return 0;
+#endif
     }
     return 0;
 }
@@ -748,6 +923,10 @@ uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos) const
         return EKF1.getLastPosNorthEastReset(pos);
     case 2:
         return EKF2.getLastPosNorthEastReset(pos);
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return 0;
+#endif
     }
     return 0;
 }
@@ -761,6 +940,10 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
         return EKF1.getLastVelNorthEastReset(vel);
     case 2:
         return EKF2.getLastVelNorthEastReset(vel);
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return 0;
+#endif
     }
     return 0;
 }
@@ -779,6 +962,10 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void)
     case 2:
         EKF1.resetHeightDatum();
         return EKF2.resetHeightDatum();
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return false;
+#endif
     }
     return false;
 }
@@ -817,6 +1004,12 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const
             return false;
         }
         return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        ret = get_home();
+        return ret.lat != 0 || ret.lng != 0;
+#endif
     }
 }
 
@@ -838,6 +1031,11 @@ bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const
     case EKF_TYPE2:
         return EKF2.getHeightControlLimit(limit);
         return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return false;
+#endif
     }
 }
 
@@ -856,6 +1054,11 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
 
     case EKF_TYPE2:
         return EKF2.getLLH(loc);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        return get_position(loc);
+#endif
     }
 }
 
@@ -880,6 +1083,17 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
         // use EKF to get variance
         EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset);
         return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        velVar = 0;
+        posVar = 0;
+        hgtVar = 0;
+        magVar.zero();
+        tasVar = 0;
+        offset.zero();
+        return true;
+#endif
     }
 }
 

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -25,6 +25,10 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AP_AHRS.h"
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <SITL/SITL.h>
+#endif
+
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
 #include <AP_NavEKF/AP_NavEKF.h>
 #include <AP_NavEKF2/AP_NavEKF2.h>
@@ -43,14 +47,7 @@ public:
 
     // Constructor
     AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng,
-                   NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE
-        ) :
-        AP_AHRS_DCM(ins, baro, gps),
-        EKF1(_EKF1),
-        EKF2(_EKF2),
-        _flags(flags)
-    {
-    }
+                   NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE);
 
     // return the smoothed gyro vector corrected for drift
     const Vector3f &get_gyro(void) const;
@@ -201,7 +198,13 @@ public:
     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
 
 private:
-    enum EKF_TYPE {EKF_TYPE_NONE, EKF_TYPE1, EKF_TYPE2};
+    enum EKF_TYPE {EKF_TYPE_NONE=0,
+                   EKF_TYPE1=1,
+                   EKF_TYPE2=2
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+                   ,EKF_TYPE_SITL=10
+#endif
+    };
     EKF_TYPE active_EKF_type(void) const;
 
     bool always_use_EKF() const {
@@ -226,6 +229,11 @@ private:
     void update_DCM(void);
     void update_EKF1(void);
     void update_EKF2(void);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    SITL::SITL *_sitl;
+    void update_SITL(void);
+#endif    
 };
 #endif