From b3e6129fd4e9feddba97cd2b6a0e66c03f6fdd91 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 20 Nov 2015 19:34:30 +1100 Subject: [PATCH] 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 | 214 +++++++++++++++++++++++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 26 ++-- 2 files changed, 231 insertions(+), 9 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 8c9a6490c3..aba5b4dff2 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/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; istate; + 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 } } diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 6708452b94..2c80b90ea9 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -25,6 +25,10 @@ #include #include "AP_AHRS.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 #include #include @@ -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