From 583b25e6aaacc582d6963efbd853a9fa0b4f5118 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 12 Dec 2019 18:31:43 +1100 Subject: [PATCH] AP_AHRS: allow NavEKFs to be compiled out --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 338 +++++++++++++++++++++++---- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 28 ++- 2 files changed, 325 insertions(+), 41 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 2d88f1270b..a67a3bcd4f 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -82,8 +82,12 @@ void AP_AHRS_NavEKF::reset_gyro_drift(void) AP_AHRS_DCM::reset_gyro_drift(); // reset the EKF gyro bias states +#if HAL_NAVEKF2_AVAILABLE EKF2.resetGyroBias(); +#endif +#if HAL_NAVEKF3_AVAILABLE EKF3.resetGyroBias(); +#endif } void AP_AHRS_NavEKF::update(bool skip_ins_update) @@ -109,12 +113,20 @@ void AP_AHRS_NavEKF::update(bool skip_ins_update) if (_ekf_type == 2) { // if EK2 is primary then run EKF2 first to give it CPU // priority +#if HAL_NAVEKF2_AVAILABLE update_EKF2(); +#endif +#if HAL_NAVEKF3_AVAILABLE update_EKF3(); +#endif } else { // otherwise run EKF3 first +#if HAL_NAVEKF3_AVAILABLE update_EKF3(); +#endif +#if HAL_NAVEKF2_AVAILABLE update_EKF2(); +#endif } #if AP_MODULE_SUPPORTED @@ -155,6 +167,7 @@ void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update) _dcm_attitude(roll, pitch, yaw); } +#if HAL_NAVEKF2_AVAILABLE void AP_AHRS_NavEKF::update_EKF2(void) { if (!_ekf2_started) { @@ -226,8 +239,9 @@ void AP_AHRS_NavEKF::update_EKF2(void) } } } +#endif - +#if HAL_NAVEKF3_AVAILABLE void AP_AHRS_NavEKF::update_EKF3(void) { if (!_ekf3_started) { @@ -301,6 +315,7 @@ void AP_AHRS_NavEKF::update_EKF3(void) } } } +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL void AP_AHRS_NavEKF::update_SITL(void) @@ -385,12 +400,16 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers) AP_AHRS_DCM::reset(recover_eulers); _dcm_attitude(roll, pitch, yaw); +#if HAL_NAVEKF2_AVAILABLE if (_ekf2_started) { _ekf2_started = EKF2.InitialiseFilter(); } +#endif +#if HAL_NAVEKF3_AVAILABLE if (_ekf3_started) { _ekf3_started = EKF3.InitialiseFilter(); } +#endif } // reset the current attitude, used on new IMU calibration @@ -401,12 +420,16 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw); _dcm_attitude(roll, pitch, yaw); +#if HAL_NAVEKF2_AVAILABLE if (_ekf2_started) { _ekf2_started = EKF2.InitialiseFilter(); } +#endif +#if HAL_NAVEKF3_AVAILABLE if (_ekf3_started) { _ekf3_started = EKF3.InitialiseFilter(); } +#endif } // dead-reckoning support @@ -416,18 +439,22 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const case EKFType::NONE: return AP_AHRS_DCM::get_position(loc); +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: if (EKF2.getLLH(loc)) { return true; } break; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: if (EKF3.getLLH(loc)) { return true; } break; - +#endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: { if (_sitl) { @@ -471,13 +498,17 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) const break; #endif +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.getWind(-1,wind); break; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.getWind(-1,wind); break; +#endif } return wind; @@ -496,11 +527,15 @@ bool AP_AHRS_NavEKF::use_compass(void) switch (active_EKF_type()) { case EKFType::NONE: break; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.use_compass(); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.use_compass(); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -517,12 +552,20 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const switch (active_EKF_type()) { case EKFType::NONE: // EKF is secondary +#if HAL_NAVEKF2_AVAILABLE EKF2.getEulerAngles(-1, eulers); return _ekf2_started; +#else + return false; +#endif +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -542,12 +585,20 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const switch (active_EKF_type()) { case EKFType::NONE: // EKF is secondary +#if HAL_NAVEKF2_AVAILABLE EKF2.getQuaternion(-1, quat); return _ekf2_started; +#else + return false; +#endif +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -566,12 +617,20 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const switch (active_EKF_type()) { case EKFType::NONE: // EKF is secondary +#if HAL_NAVEKF2_AVAILABLE EKF2.getLLH(loc); return _ekf2_started; +#else + return false; +#endif +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -593,13 +652,17 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void) case EKFType::NONE: break; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.getVelNED(-1,vec); return Vector2f(vec.x, vec.y); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.getVelNED(-1,vec); return Vector2f(vec.x, vec.y); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: { @@ -619,19 +682,27 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void) // from which to decide the origin on its own bool AP_AHRS_NavEKF::set_origin(const Location &loc) { +#if HAL_NAVEKF2_AVAILABLE const bool ret2 = EKF2.setOriginLLH(loc); +#endif +#if HAL_NAVEKF3_AVAILABLE const bool ret3 = EKF3.setOriginLLH(loc); +#endif // return success if active EKF's origin was set switch (active_EKF_type()) { case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return ret2; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return ret3; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -662,13 +733,17 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const case EKFType::NONE: break; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.getVelNED(-1,vec); return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.getVelNED(-1,vec); return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -690,13 +765,17 @@ bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.getMagNED(-1,vec); return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.getMagNED(-1,vec); return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -713,13 +792,17 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.getMagXYZ(-1,vec); return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.getMagXYZ(-1,vec); return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -738,13 +821,17 @@ bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: velocity = EKF2.getPosDownDerivative(-1); return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: velocity = EKF3.getPosDownDerivative(-1); return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -768,11 +855,15 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getHAGL(height); - +#endif + +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getHAGL(height); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: { @@ -797,6 +888,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { Vector2f posNE; float posD; @@ -809,7 +901,9 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const } return false; } +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { Vector2f posNE; float posD; @@ -822,6 +916,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const } return false; } +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: { @@ -869,15 +964,19 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { bool position_is_valid = EKF2.getPosNE(-1,posNE); return position_is_valid; } +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { bool position_is_valid = EKF3.getPosNE(-1,posNE); return position_is_valid; } +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: { @@ -921,15 +1020,19 @@ bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { bool position_is_valid = EKF2.getPosD(-1,posD); return position_is_valid; } +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { bool position_is_valid = EKF3.getPosD(-1,posD); return position_is_valid; } +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: { @@ -971,19 +1074,36 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::ekf_type(void) const switch (type) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: + return type; #endif +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: + return type; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return type; +#endif case EKFType::NONE: if (always_use_EKF()) { +#if HAL_NAVEKF2_AVAILABLE return EKFType::TWO; +#elif HAL_NAVEKF3_AVAILABLE + return EKFType::THREE; +#endif } return EKFType::NONE; } // we can get to here if the user has mis-set AHRS_EKF_TYPE - any - // value above 3 will get to here. + // value above 3 will get to here. TWO is returned here for no + // better reason than "tradition". +#if HAL_NAVEKF2_AVAILABLE return EKFType::TWO; +#elif HAL_NAVEKF3_AVAILABLE + return EKFType::THREE; +#else + return EKFType::NONE; +#endif } AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const @@ -994,6 +1114,7 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const case EKFType::NONE: return EKFType::NONE; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { // do we have an EKF2 yet? if (!_ekf2_started) { @@ -1010,7 +1131,9 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const } break; } +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { // do we have an EKF3 yet? if (!_ekf3_started) { @@ -1027,7 +1150,8 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const } break; } - +#endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: ret = EKFType::SITL; @@ -1048,15 +1172,21 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const _vehicle_class == AHRS_VEHICLE_GROUND) && (_flags.fly_forward || !hal.util->get_soft_armed())) { nav_filter_status filt_state; +#if HAL_NAVEKF2_AVAILABLE if (ret == EKFType::TWO) { EKF2.getFilterStatus(-1,filt_state); - } else if (ret == EKFType::THREE) { - EKF3.getFilterStatus(-1,filt_state); -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - } else if (ret == EKFType::SITL) { - get_filter_status(filt_state); -#endif } +#endif +#if HAL_NAVEKF3_AVAILABLE + if (ret == EKFType::THREE) { + EKF3.getFilterStatus(-1,filt_state); + } +#endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (ret == EKFType::SITL) { + get_filter_status(filt_state); + } +#endif if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { // if the EKF is not fusing GPS and we have a 3D lock, then // plane and rover would prefer to use the GPS position from @@ -1105,6 +1235,7 @@ bool AP_AHRS_NavEKF::healthy(void) const case EKFType::NONE: return AP_AHRS_DCM::healthy(); +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { bool ret = _ekf2_started && EKF2.healthy(); if (!ret) { @@ -1119,7 +1250,9 @@ bool AP_AHRS_NavEKF::healthy(void) const } return true; } +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { bool ret = _ekf3_started && EKF3.healthy(); if (!ret) { @@ -1134,7 +1267,8 @@ bool AP_AHRS_NavEKF::healthy(void) const } return true; } - +#endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: return true; @@ -1155,17 +1289,21 @@ bool AP_AHRS_NavEKF::prearm_healthy(void) const prearm_health = true; break; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: if (_ekf2_started && EKF2.all_cores_healthy()) { prearm_health = true; } break; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: if (_ekf3_started && EKF3.all_cores_healthy()) { prearm_health = true; } break; +#endif } return prearm_health && healthy(); } @@ -1182,13 +1320,17 @@ bool AP_AHRS_NavEKF::initialised(void) const case EKFType::NONE: return true; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: // initialisation complete 10sec after ekf has started return (_ekf2_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // initialisation complete 10sec after ekf has started return (_ekf3_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1205,13 +1347,17 @@ bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.getFilterStatus(-1,status); return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.getFilterStatus(-1,status); return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1235,20 +1381,28 @@ bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const // write optical flow data to EKF void AP_AHRS_NavEKF::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset) { +#if HAL_NAVEKF2_AVAILABLE EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset); +#endif +#if HAL_NAVEKF3_AVAILABLE EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset); +#endif } // write body frame odometry measurements to the EKF void AP_AHRS_NavEKF::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset) { +#if HAL_NAVEKF3_AVAILABLE EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset); +#endif } // Write position and quaternion data from an external navigation system void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) { +#if HAL_NAVEKF2_AVAILABLE EKF2.writeExtNavData(sensOffset, pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms); +#endif } @@ -1259,11 +1413,15 @@ uint8_t AP_AHRS_NavEKF::setInhibitGPS(void) case EKFType::NONE: return 0; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.setInhibitGPS(); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.setInhibitGPS(); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1281,13 +1439,17 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel case EKFType::NONE: break; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler); break; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler); break; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1307,11 +1469,15 @@ bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getMagOffsets(mag_idx, magOffsets); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getMagOffsets(mag_idx, magOffsets); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1326,33 +1492,39 @@ bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const // Retrieves the NED delta velocity corrected void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { - const EKFType type = active_EKF_type(); - if (type == EKFType::TWO || type == EKFType::THREE) { - int8_t imu_idx = 0; - Vector3f accel_bias; - if (type == EKFType::TWO) { - accel_bias.zero(); - imu_idx = EKF2.getPrimaryCoreIMUIndex(); - EKF2.getAccelZBias(-1,accel_bias.z); - } else if (type == EKFType::THREE) { - imu_idx = EKF3.getPrimaryCoreIMUIndex(); - EKF3.getAccelBias(-1,accel_bias); - } - if (imu_idx == -1) { - // should never happen, call parent implementation in this scenario - AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt); - return; - } - ret.zero(); - const AP_InertialSensor &_ins = AP::ins(); - _ins.get_delta_velocity((uint8_t)imu_idx, ret); - dt = _ins.get_delta_velocity_dt((uint8_t)imu_idx); - ret -= accel_bias*dt; - ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret; - ret.z += GRAVITY_MSS*dt; - } else { - AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt); + int8_t imu_idx = -1; + Vector3f accel_bias; + switch (active_EKF_type()) { + case EKFType::NONE: + break; +#if HAL_NAVEKF2_AVAILABLE + case EKFType::TWO: + imu_idx = EKF2.getPrimaryCoreIMUIndex(); + EKF2.getAccelZBias(-1,accel_bias.z); + break; +#endif +#if HAL_NAVEKF3_AVAILABLE + case EKFType::THREE: + imu_idx = EKF3.getPrimaryCoreIMUIndex(); + EKF3.getAccelBias(-1,accel_bias); + break; +#endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: + break; +#endif } + if (imu_idx == -1) { + AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt); + return; + } + ret.zero(); + const AP_InertialSensor &_ins = AP::ins(); + _ins.get_delta_velocity((uint8_t)imu_idx, ret); + dt = _ins.get_delta_velocity_dt((uint8_t)imu_idx); + ret -= accel_bias*dt; + ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret; + ret.z += GRAVITY_MSS*dt; } // report any reason for why the backend is refusing to initialise @@ -1362,11 +1534,15 @@ const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const case EKFType::NONE: return nullptr; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.prearm_failure_reason(); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.prearm_failure_reason(); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1386,6 +1562,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu // only check yaw if compasses are being used bool check_yaw = _compass && _compass->use_for_yaw(); +#if HAL_NAVEKF2_AVAILABLE // check primary vs ekf2 for (uint8_t i = 0; i < EKF2.activeCores(); i++) { Quaternion ekf2_quat; @@ -1403,7 +1580,9 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu return false; } } +#endif +#if HAL_NAVEKF3_AVAILABLE // check primary vs ekf3 for (uint8_t i = 0; i < EKF3.activeCores(); i++) { Quaternion ekf3_quat; @@ -1421,6 +1600,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu return false; } } +#endif // check primary vs dcm Quaternion dcm_quat; @@ -1450,11 +1630,15 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) case EKFType::NONE: return 0; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getLastYawResetAngle(yawAng); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getLastYawResetAngle(yawAng); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1473,11 +1657,15 @@ uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos) case EKFType::NONE: return 0; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getLastPosNorthEastReset(pos); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getLastPosNorthEastReset(pos); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1496,11 +1684,15 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const case EKFType::NONE: return 0; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getLastVelNorthEastReset(vel); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getLastVelNorthEastReset(vel); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1519,11 +1711,15 @@ uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) case EKFType::NONE: return 0; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getLastPosDownReset(posDelta); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getLastPosDownReset(posDelta); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1546,17 +1742,29 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void) switch (ekf_type()) { case EKFType::NONE: +#if HAL_NAVEKF3_AVAILABLE EKF3.resetHeightDatum(); +#endif +#if HAL_NAVEKF2_AVAILABLE EKF2.resetHeightDatum(); +#endif return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: +#if HAL_NAVEKF3_AVAILABLE EKF3.resetHeightDatum(); +#endif return EKF2.resetHeightDatum(); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: +#if HAL_NAVEKF2_AVAILABLE EKF2.resetHeightDatum(); +#endif return EKF3.resetHeightDatum(); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1594,12 +1802,16 @@ void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const } break; #endif - + +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.send_status_report(chan); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.send_status_report(chan); +#endif } } @@ -1613,17 +1825,21 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: if (!EKF2.getOriginLLH(-1,ret)) { return false; } return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: if (!EKF3.getOriginLLH(-1,ret)) { return false; } return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1649,11 +1865,15 @@ bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const // We are not using an EKF so no limiting applies return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getHeightControlLimit(limit); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getHeightControlLimit(limit); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1673,11 +1893,15 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const // We are not using an EKF so no data return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.getLLH(loc); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.getLLH(loc); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1697,15 +1921,19 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec // We are not using an EKF so no data return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: // use EKF to get innovations EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // use EKF to get innovations EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1732,15 +1960,19 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, // We are not using an EKF so no data return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: // use EKF to get variance EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); return true; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // use EKF to get variance EKF3.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); return true; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1759,14 +1991,22 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, void AP_AHRS_NavEKF::setTakeoffExpected(bool val) { +#if HAL_NAVEKF2_AVAILABLE EKF2.setTakeoffExpected(val); +#endif +#if HAL_NAVEKF3_AVAILABLE EKF3.setTakeoffExpected(val); +#endif } void AP_AHRS_NavEKF::setTouchdownExpected(bool val) { +#if HAL_NAVEKF2_AVAILABLE EKF2.setTouchdownExpected(val); +#endif +#if HAL_NAVEKF3_AVAILABLE EKF3.setTouchdownExpected(val); +#endif } bool AP_AHRS_NavEKF::getGpsGlitchStatus() const @@ -1785,11 +2025,15 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const case EKFType::NONE: return false; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.have_ekf_logging(); +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: return EKF3.have_ekf_logging(); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: @@ -1807,14 +2051,18 @@ uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const switch (ekf_type()) { case EKFType::NONE: break; +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: // let EKF2 choose primary IMU imu = EKF2.getPrimaryCoreIMUIndex(); break; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // let EKF2 choose primary IMU imu = EKF3.getPrimaryCoreIMUIndex(); break; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: break; @@ -1863,20 +2111,28 @@ void AP_AHRS_NavEKF::check_lane_switch(void) break; #endif +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: EKF2.checkLaneSwitch(); break; +#endif +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: EKF3.checkLaneSwitch(); break; +#endif } } void AP_AHRS_NavEKF::Log_Write() { +#if HAL_NAVEKF2_AVAILABLE get_NavEKF2().Log_Write(); +#endif +#if HAL_NAVEKF3_AVAILABLE get_NavEKF3().Log_Write(); +#endif } AP_AHRS_NavEKF &AP::ahrs_navekf() @@ -1888,10 +2144,14 @@ AP_AHRS_NavEKF &AP::ahrs_navekf() bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const { switch (active_EKF_type()) { +#if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: return EKF2.isExtNavUsedForYaw(); +#endif case EKFType::NONE: +#if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: #endif diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index c14eec896c..197f1eb8bc 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -23,6 +23,14 @@ #include +#ifndef HAL_NAVEKF2_AVAILABLE +#define HAL_NAVEKF2_AVAILABLE 1 +#endif + +#ifndef HAL_NAVEKF3_AVAILABLE +#define HAL_NAVEKF3_AVAILABLE 1 +#endif + #define AP_AHRS_NAVEKF_AVAILABLE 1 #include "AP_AHRS.h" @@ -89,19 +97,23 @@ public: bool use_compass() override; // we will need to remove these to fully hide which EKF we are using +#if HAL_NAVEKF2_AVAILABLE NavEKF2 &get_NavEKF2(void) { return EKF2; } const NavEKF2 &get_NavEKF2_const(void) const { return EKF2; } +#endif +#if HAL_NAVEKF3_AVAILABLE NavEKF3 &get_NavEKF3(void) { return EKF3; } const NavEKF3 &get_NavEKF3_const(void) const { return EKF3; } +#endif // return secondary attitude solution if available, as eulers in radians bool get_secondary_attitude(Vector3f &eulers) const override; @@ -272,14 +284,22 @@ public: bool is_ext_nav_used_for_yaw(void) const; // these are only out here so vehicles can reference them for parameters +#if HAL_NAVEKF2_AVAILABLE NavEKF2 EKF2; +#endif +#if HAL_NAVEKF3_AVAILABLE NavEKF3 EKF3; +#endif private: enum class EKFType { NONE = 0, +#if HAL_NAVEKF3_AVAILABLE THREE = 3, +#endif +#if HAL_NAVEKF2_AVAILABLE TWO = 2 +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL ,SITL = 10 #endif @@ -290,8 +310,14 @@ private: return _ekf_flags & FLAG_ALWAYS_USE_EKF; } +#if HAL_NAVEKF2_AVAILABLE + void update_EKF2(void); bool _ekf2_started; +#endif +#if HAL_NAVEKF3_AVAILABLE bool _ekf3_started; + void update_EKF3(void); +#endif bool _force_ekf; // rotation from vehicle body to NED frame @@ -308,8 +334,6 @@ private: EKFType ekf_type(void) const; void update_DCM(bool skip_ins_update); - void update_EKF2(void); - void update_EKF3(void); // get the index of the current primary IMU uint8_t get_primary_IMU_index(void) const;