From 9997cbf203f75c0b902a37fbb8e0f87b1a9a9ecc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2019 09:45:45 +1100 Subject: [PATCH] AP_AHRS: make ekf_type return from enum class Eliminate default cases and rely on compiler to enforce all cases handled in switch statement. This will be important when we make EKF2 optional. --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 487 +++++++++++++++------------ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 13 +- 2 files changed, 270 insertions(+), 230 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index f7553dd2f7..3b068602da 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -53,7 +53,7 @@ AP_AHRS_NavEKF::AP_AHRS_NavEKF(NavEKF2 &_EKF2, // return the smoothed gyro vector corrected for drift const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const { - if (!active_EKF_type()) { + if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_gyro(); } return _gyro_estimate; @@ -61,7 +61,7 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const const Matrix3f &AP_AHRS_NavEKF::get_rotation_body_to_ned(void) const { - if (!active_EKF_type()) { + if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_rotation_body_to_ned(); } return _dcm_matrix; @@ -69,7 +69,7 @@ const Matrix3f &AP_AHRS_NavEKF::get_rotation_body_to_ned(void) const const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const { - if (!active_EKF_type()) { + if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_gyro_drift(); } return _gyro_drift; @@ -175,7 +175,7 @@ void AP_AHRS_NavEKF::update_EKF2(void) } if (_ekf2_started) { EKF2.UpdateFilter(); - if (active_EKF_type() == EKF_TYPE2) { + if (active_EKF_type() == EKFType::TWO) { Vector3f eulers; EKF2.getRotationBodyToNED(_dcm_matrix); EKF2.getEulerAngles(-1,eulers); @@ -248,7 +248,7 @@ void AP_AHRS_NavEKF::update_EKF3(void) } if (_ekf3_started) { EKF3.UpdateFilter(); - if (active_EKF_type() == EKF_TYPE3) { + if (active_EKF_type() == EKFType::THREE) { Vector3f eulers; EKF3.getRotationBodyToNED(_dcm_matrix); EKF3.getEulerAngles(-1,eulers); @@ -319,7 +319,7 @@ void AP_AHRS_NavEKF::update_SITL(void) const struct SITL::sitl_fdm &fdm = _sitl->state; const AP_InertialSensor &_ins = AP::ins(); - if (active_EKF_type() == EKF_TYPE_SITL) { + if (active_EKF_type() == EKFType::SITL) { fdm.quaternion.rotation_matrix(_dcm_matrix); _dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body(); @@ -367,7 +367,7 @@ void AP_AHRS_NavEKF::update_SITL(void) // accelerometer values in the earth frame in m/s/s const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const { - if (active_EKF_type() == EKF_TYPE_NONE) { + if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_accel_ef(i); } return _accel_ef_ekf[i]; @@ -376,7 +376,7 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const // blended accelerometer values in the earth frame in m/s/s const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const { - if (active_EKF_type() == EKF_TYPE_NONE) { + if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_accel_ef_blended(); } return _accel_ef_ekf_blended; @@ -417,23 +417,23 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con bool AP_AHRS_NavEKF::get_position(struct Location &loc) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return AP_AHRS_DCM::get_position(loc); - case EKF_TYPE2: + case EKFType::TWO: if (EKF2.getLLH(loc)) { return true; } break; - case EKF_TYPE3: + case EKFType::THREE: if (EKF3.getLLH(loc)) { return true; } break; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: { + case EKFType::SITL: { if (_sitl) { const struct SITL::sitl_fdm &fdm = _sitl->state; loc = {}; @@ -445,9 +445,6 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const break; } #endif - - default: - break; } return AP_AHRS_DCM::get_position(loc); } @@ -468,21 +465,21 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) const { Vector3f wind; switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: wind = AP_AHRS_DCM::wind_estimate(); break; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: wind.zero(); break; #endif - case EKF_TYPE2: + case EKFType::TWO: EKF2.getWind(-1,wind); break; - case EKF_TYPE3: + case EKFType::THREE: EKF3.getWind(-1,wind); break; @@ -501,16 +498,16 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float *airspeed_ret) const bool AP_AHRS_NavEKF::use_compass(void) { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: break; - case EKF_TYPE2: + case EKFType::TWO: return EKF2.use_compass(); - case EKF_TYPE3: + case EKFType::THREE: return EKF3.use_compass(); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return true; #endif } @@ -522,17 +519,17 @@ bool AP_AHRS_NavEKF::use_compass(void) bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // EKF is secondary EKF2.getEulerAngles(-1, eulers); return _ekf2_started; - case EKF_TYPE2: + case EKFType::TWO: - case EKF_TYPE3: + case EKFType::THREE: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: #endif // DCM is secondary eulers = _dcm_attitude; @@ -547,17 +544,17 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // EKF is secondary EKF2.getQuaternion(-1, quat); return _ekf2_started; - case EKF_TYPE2: + case EKFType::TWO: - case EKF_TYPE3: + case EKFType::THREE: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: #endif // DCM is secondary quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned()); @@ -571,17 +568,17 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // EKF is secondary EKF2.getLLH(loc); return _ekf2_started; - case EKF_TYPE2: + case EKFType::TWO: - case EKF_TYPE3: + case EKFType::THREE: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: #endif // return DCM position AP_AHRS_DCM::get_position(loc); @@ -597,29 +594,28 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void) Vector3f vec; switch (active_EKF_type()) { - case EKF_TYPE_NONE: - return AP_AHRS_DCM::groundspeed_vector(); + case EKFType::NONE: + break; - case EKF_TYPE2: - default: + case EKFType::TWO: EKF2.getVelNED(-1,vec); return Vector2f(vec.x, vec.y); - case EKF_TYPE3: + case EKFType::THREE: EKF3.getVelNED(-1,vec); return Vector2f(vec.x, vec.y); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: { + case EKFType::SITL: { if (_sitl) { const struct SITL::sitl_fdm &fdm = _sitl->state; return Vector2f(fdm.speedN, fdm.speedE); - } else { - return AP_AHRS_DCM::groundspeed_vector(); } + break; } #endif } + return AP_AHRS_DCM::groundspeed_vector(); } // set the EKF's origin location in 10e7 degrees. This should only @@ -632,17 +628,17 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc) // return success if active EKF's origin was set switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: + case EKFType::TWO: return ret2; - case EKF_TYPE3: + case EKFType::THREE: return ret3; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: if (_sitl) { struct SITL::sitl_fdm &fdm = _sitl->state; fdm.home = loc; @@ -659,7 +655,7 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc) // return true if inertial navigation is active bool AP_AHRS_NavEKF::have_inertial_nav(void) const { - return active_EKF_type() != EKF_TYPE_NONE; + return active_EKF_type() != EKFType::NONE; } // return a ground velocity in meters/second, North/East/Down @@ -667,20 +663,19 @@ bool AP_AHRS_NavEKF::have_inertial_nav(void) const bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: - return AP_AHRS_DCM::get_velocity_NED(vec); + case EKFType::NONE: + break; - case EKF_TYPE2: - default: + case EKFType::TWO: EKF2.getVelNED(-1,vec); return true; - case EKF_TYPE3: + case EKFType::THREE: EKF3.getVelNED(-1,vec); return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: if (!_sitl) { return false; } @@ -689,48 +684,49 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const return true; #endif } + return AP_AHRS_DCM::get_velocity_NED(vec); } // returns the expected NED magnetic field bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: - default: + case EKFType::TWO: EKF2.getMagNED(-1,vec); return true; - case EKF_TYPE3: + case EKFType::THREE: EKF3.getMagNED(-1,vec); return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return false; #endif } + return false; } // returns the estimated magnetic field offsets in body frame bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: + case EKFType::TWO: EKF2.getMagXYZ(-1,vec); return true; - case EKF_TYPE3: + case EKFType::THREE: EKF3.getMagXYZ(-1,vec); return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return false; #endif } @@ -743,19 +739,19 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: + case EKFType::TWO: velocity = EKF2.getPosDownDerivative(-1); return true; - case EKF_TYPE3: + case EKFType::THREE: velocity = EKF3.getPosDownDerivative(-1); return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: if (_sitl) { const struct SITL::sitl_fdm &fdm = _sitl->state; velocity = fdm.speedD; @@ -773,17 +769,17 @@ bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const bool AP_AHRS_NavEKF::get_hagl(float &height) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: + case EKFType::TWO: return EKF2.getHAGL(height); - case EKF_TYPE3: + case EKFType::THREE: return EKF3.getHAGL(height); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: { + case EKFType::SITL: { if (!_sitl) { return false; } @@ -802,10 +798,10 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: { + case EKFType::TWO: { Vector2f posNE; float posD; if (EKF2.getPosNE(-1,posNE) && EKF2.getPosD(-1,posD)) { @@ -818,7 +814,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const return false; } - case EKF_TYPE3: { + case EKFType::THREE: { Vector2f posNE; float posD; if (EKF3.getPosNE(-1,posNE) && EKF3.getPosD(-1,posD)) { @@ -832,7 +828,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: { + case EKFType::SITL: { if (!_sitl) { return false; } @@ -874,21 +870,21 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_home(Vector3f &vec) const bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: { + case EKFType::TWO: { bool position_is_valid = EKF2.getPosNE(-1,posNE); return position_is_valid; } - case EKF_TYPE3: { + case EKFType::THREE: { bool position_is_valid = EKF3.getPosNE(-1,posNE); return position_is_valid; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: { + case EKFType::SITL: { Location loc; get_position(loc); posNE = get_home().get_distance_NE(loc); @@ -926,21 +922,21 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_home(Vector2f &posNE) const bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: { + case EKFType::TWO: { bool position_is_valid = EKF2.getPosD(-1,posD); return position_is_valid; } - case EKF_TYPE3: { + case EKFType::THREE: { bool position_is_valid = EKF3.getPosD(-1,posD); return position_is_valid; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: { + case EKFType::SITL: { if (!_sitl) { return false; } @@ -973,65 +969,72 @@ void AP_AHRS_NavEKF::get_relative_position_D_home(float &posD) const canonicalise _ekf_type, forcing it to be 0, 2 or 3 type 1 has been deprecated */ -uint8_t AP_AHRS_NavEKF::ekf_type(void) const +AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::ekf_type(void) const { - uint8_t type = _ekf_type; + EKFType type = (EKFType)_ekf_type.get(); + switch (type) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - if (type == EKF_TYPE_SITL) { - return type; - } + case EKFType::SITL: #endif - if ((always_use_EKF() && (type == 0)) || (type == 1) || (type > 3)) { - type = 2; + case EKFType::TWO: + case EKFType::THREE: + return type; + case EKFType::NONE: + if (always_use_EKF()) { + return EKFType::TWO; + } + return EKFType::NONE; } - return type; + // we can get to here if the user has mis-set AHRS_EKF_TYPE - any + // value above 3 will get to here. + return EKFType::TWO; } -AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const +AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const { - EKF_TYPE ret = EKF_TYPE_NONE; + EKFType ret = EKFType::NONE; switch (ekf_type()) { - case 0: - return EKF_TYPE_NONE; + case EKFType::NONE: + return EKFType::NONE; - case 2: { + case EKFType::TWO: { // do we have an EKF2 yet? if (!_ekf2_started) { - return EKF_TYPE_NONE; + return EKFType::NONE; } if (always_use_EKF()) { uint16_t ekf2_faults; EKF2.getFilterFaults(-1,ekf2_faults); if (ekf2_faults == 0) { - ret = EKF_TYPE2; + ret = EKFType::TWO; } } else if (EKF2.healthy()) { - ret = EKF_TYPE2; + ret = EKFType::TWO; } break; } - case 3: { + case EKFType::THREE: { // do we have an EKF3 yet? if (!_ekf3_started) { - return EKF_TYPE_NONE; + return EKFType::NONE; } if (always_use_EKF()) { uint16_t ekf3_faults; EKF3.getFilterFaults(-1,ekf3_faults); if (ekf3_faults == 0) { - ret = EKF_TYPE3; + ret = EKFType::THREE; } } else if (EKF3.healthy()) { - ret = EKF_TYPE3; + ret = EKFType::THREE; } break; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: - ret = EKF_TYPE_SITL; + case EKFType::SITL: + ret = EKFType::SITL; break; #endif } @@ -1044,17 +1047,17 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const that the arming checks do wait for good GPS position on fixed wing and rover */ - if (ret != EKF_TYPE_NONE && + if (ret != EKFType::NONE && (_vehicle_class == AHRS_VEHICLE_FIXED_WING || _vehicle_class == AHRS_VEHICLE_GROUND) && (_flags.fly_forward || !hal.util->get_soft_armed())) { nav_filter_status filt_state; - if (ret == EKF_TYPE2) { + if (ret == EKFType::TWO) { EKF2.getFilterStatus(-1,filt_state); - } else if (ret == EKF_TYPE3) { + } else if (ret == EKFType::THREE) { EKF3.getFilterStatus(-1,filt_state); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - } else if (ret == EKF_TYPE_SITL) { + } else if (ret == EKFType::SITL) { get_filter_status(filt_state); #endif } @@ -1063,15 +1066,15 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const // plane and rover would prefer to use the GPS position from // DCM. This is a safety net while some issues with the EKF // get sorted out - return EKF_TYPE_NONE; + return EKFType::NONE; } if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) { - return EKF_TYPE_NONE; + return EKFType::NONE; } if (!filt_state.flags.attitude || !filt_state.flags.vert_vel || !filt_state.flags.vert_pos) { - return EKF_TYPE_NONE; + return EKFType::NONE; } if (!filt_state.flags.horiz_vel || (!filt_state.flags.horiz_pos_abs && !filt_state.flags.horiz_pos_rel)) { @@ -1088,7 +1091,7 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const return ret; } } - return EKF_TYPE_NONE; + return EKFType::NONE; } } return ret; @@ -1103,17 +1106,17 @@ bool AP_AHRS_NavEKF::healthy(void) const // sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters // an internal processing error, but not for bad sensor data. switch (ekf_type()) { - case 0: + case EKFType::NONE: return AP_AHRS_DCM::healthy(); - case 2: { + case EKFType::TWO: { bool ret = _ekf2_started && EKF2.healthy(); if (!ret) { return false; } if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING || _vehicle_class == AHRS_VEHICLE_GROUND) && - active_EKF_type() != EKF_TYPE2) { + active_EKF_type() != EKFType::TWO) { // on fixed wing we want to be using EKF to be considered // healthy if EKF is enabled return false; @@ -1121,14 +1124,14 @@ bool AP_AHRS_NavEKF::healthy(void) const return true; } - case 3: { + case EKFType::THREE: { bool ret = _ekf3_started && EKF3.healthy(); if (!ret) { return false; } if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING || _vehicle_class == AHRS_VEHICLE_GROUND) && - active_EKF_type() != EKF_TYPE3) { + active_EKF_type() != EKFType::THREE) { // on fixed wing we want to be using EKF to be considered // healthy if EKF is enabled return false; @@ -1137,7 +1140,7 @@ bool AP_AHRS_NavEKF::healthy(void) const } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return true; #endif } @@ -1150,20 +1153,19 @@ bool AP_AHRS_NavEKF::prearm_healthy(void) const bool prearm_health = false; switch (ekf_type()) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: #endif - case EKF_TYPE_NONE: + case EKFType::NONE: prearm_health = true; break; - case EKF_TYPE2: - default: + case EKFType::TWO: if (_ekf2_started && EKF2.all_cores_healthy()) { prearm_health = true; } break; - case EKF_TYPE3: + case EKFType::THREE: if (_ekf3_started && EKF3.all_cores_healthy()) { prearm_health = true; } @@ -1181,43 +1183,42 @@ void AP_AHRS_NavEKF::set_ekf_use(bool setting) bool AP_AHRS_NavEKF::initialised(void) const { switch (ekf_type()) { - case 0: + case EKFType::NONE: return true; - case 2: - default: + 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)); - case 3: + 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)); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return true; #endif } + return false; }; // get_filter_status : returns filter status as a series of flags bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const { switch (ekf_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: - default: + case EKFType::TWO: EKF2.getFilterStatus(-1,status); return true; - case EKF_TYPE3: + case EKFType::THREE: EKF3.getFilterStatus(-1,status); return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: memset(&status, 0, sizeof(status)); status.flags.attitude = 1; status.flags.horiz_vel = 1; @@ -1232,6 +1233,7 @@ bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const #endif } + return false; } // write optical flow data to EKF @@ -1258,38 +1260,41 @@ void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &sensOffset, const Vector3f uint8_t AP_AHRS_NavEKF::setInhibitGPS(void) { switch (ekf_type()) { - case 0: + case EKFType::NONE: + return 0; - case 2: - default: + case EKFType::TWO: return EKF2.setInhibitGPS(); - case 3: + case EKFType::THREE: return EKF3.setInhibitGPS(); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: - return false; + case EKFType::SITL: + return 0; #endif } + // since there is no default case above, this is unreachable + return 0; } // get speed limit void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const { switch (ekf_type()) { - case 0: + case EKFType::NONE: + break; - case 2: + case EKFType::TWO: EKF2.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler); break; - case 3: + case EKFType::THREE: EKF3.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler); break; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: // same as EKF2 for no optical flow ekfGndSpdLimit = 400.0f; ekfNavVelGainScaler = 1.0f; @@ -1303,35 +1308,37 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const { switch (ekf_type()) { - case 0: + case EKFType::NONE: + return false; - case 2: - default: + case EKFType::TWO: return EKF2.getMagOffsets(mag_idx, magOffsets); - case 3: + case EKFType::THREE: return EKF3.getMagOffsets(mag_idx, magOffsets); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: magOffsets.zero(); return true; #endif } + // since there is no default case above, this is unreachable + return false; } // Retrieves the NED delta velocity corrected void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { - const EKF_TYPE type = active_EKF_type(); - if (type == EKF_TYPE2 || type == EKF_TYPE3) { + const EKFType type = active_EKF_type(); + if (type == EKFType::TWO || type == EKFType::THREE) { int8_t imu_idx = 0; Vector3f accel_bias; - if (type == EKF_TYPE2) { + if (type == EKFType::TWO) { accel_bias.zero(); imu_idx = EKF2.getPrimaryCoreIMUIndex(); EKF2.getAccelZBias(-1,accel_bias.z); - } else if (type == EKF_TYPE3) { + } else if (type == EKFType::THREE) { imu_idx = EKF3.getPrimaryCoreIMUIndex(); EKF3.getAccelBias(-1,accel_bias); } @@ -1356,17 +1363,21 @@ void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) cons const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const { switch (ekf_type()) { - case 0: + case EKFType::NONE: return nullptr; - case 2: - default: + case EKFType::TWO: return EKF2.prearm_failure_reason(); - case 3: + case EKFType::THREE: return EKF3.prearm_failure_reason(); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: + return nullptr; +#endif } + return nullptr; } @@ -1440,15 +1451,17 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) const { switch (ekf_type()) { - case 2: - default: + case EKFType::NONE: + return 0; + + case EKFType::TWO: return EKF2.getLastYawResetAngle(yawAng); - case 3: + case EKFType::THREE: return EKF3.getLastYawResetAngle(yawAng); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return 0; #endif } @@ -1461,15 +1474,17 @@ uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos) const { switch (ekf_type()) { - case 2: - default: + case EKFType::NONE: + return 0; + + case EKFType::TWO: return EKF2.getLastPosNorthEastReset(pos); - case 3: + case EKFType::THREE: return EKF3.getLastPosNorthEastReset(pos); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return 0; #endif } @@ -1482,15 +1497,17 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const { switch (ekf_type()) { - case 2: - default: + case EKFType::NONE: + return 0; + + case EKFType::TWO: return EKF2.getLastVelNorthEastReset(vel); - case 3: + case EKFType::THREE: return EKF3.getLastVelNorthEastReset(vel); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return 0; #endif } @@ -1503,14 +1520,17 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) const { switch (ekf_type()) { - case EKF_TYPE2: + case EKFType::NONE: + return 0; + + case EKFType::TWO: return EKF2.getLastPosDownReset(posDelta); - case EKF_TYPE3: + case EKFType::THREE: return EKF3.getLastPosDownReset(posDelta); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return 0; #endif } @@ -1529,19 +1549,21 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void) switch (ekf_type()) { - case 2: - default: { + case EKFType::NONE: + EKF3.resetHeightDatum(); + EKF2.resetHeightDatum(); + return false; + + case EKFType::TWO: EKF3.resetHeightDatum(); return EKF2.resetHeightDatum(); - } - case 3: { + case EKFType::THREE: EKF2.resetHeightDatum(); return EKF3.resetHeightDatum(); - } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return false; #endif } @@ -1552,13 +1574,13 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void) void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const { switch (ekf_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // send zero status report mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0, 0); break; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: { // send status report with everything looking good const uint16_t flags = @@ -1577,10 +1599,10 @@ void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const break; #endif - case EKF_TYPE2: + case EKFType::TWO: return EKF2.send_status_report(chan); - case EKF_TYPE3: + case EKFType::THREE: return EKF3.send_status_report(chan); } @@ -1592,24 +1614,23 @@ void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const bool AP_AHRS_NavEKF::get_origin(Location &ret) const { switch (ekf_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: return false; - case EKF_TYPE2: - default: + case EKFType::TWO: if (!EKF2.getOriginLLH(-1,ret)) { return false; } return true; - case EKF_TYPE3: + case EKFType::THREE: if (!EKF3.getOriginLLH(-1,ret)) { return false; } return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: if (!_sitl) { return false; } @@ -1618,6 +1639,8 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const return true; #endif } + + return false; } // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag @@ -1626,22 +1649,23 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const { switch (ekf_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // We are not using an EKF so no limiting applies return false; - case EKF_TYPE2: - default: + case EKFType::TWO: return EKF2.getHeightControlLimit(limit); - case EKF_TYPE3: + case EKFType::THREE: return EKF3.getHeightControlLimit(limit); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return false; #endif } + + return false; } // get_location - updates the provided location with the latest calculated location @@ -1649,22 +1673,23 @@ bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const bool AP_AHRS_NavEKF::get_location(struct Location &loc) const { switch (ekf_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // We are not using an EKF so no data return false; - case EKF_TYPE2: - default: + case EKFType::TWO: return EKF2.getLLH(loc); - case EKF_TYPE3: + case EKFType::THREE: return EKF3.getLLH(loc); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: return get_position(loc); #endif } + + return false; } // return the innovations for the primariy EKF @@ -1672,23 +1697,22 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const { switch (ekf_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // We are not using an EKF so no data return false; - case EKF_TYPE2: - default: + case EKFType::TWO: // use EKF to get innovations EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); return true; - case EKF_TYPE3: + case EKFType::THREE: // use EKF to get innovations EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: velInnov.zero(); posInnov.zero(); magInnov.zero(); @@ -1697,6 +1721,8 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec return true; #endif } + + return false; } // get_variances - provides the innovations normalised using the innovation variance where a value of 0 @@ -1706,23 +1732,22 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { switch (ekf_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: // We are not using an EKF so no data return false; - case EKF_TYPE2: - default: + case EKFType::TWO: // use EKF to get variance EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); return true; - case EKF_TYPE3: + case EKFType::THREE: // use EKF to get variance EKF3.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); return true; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: velVar = 0; posVar = 0; hgtVar = 0; @@ -1732,6 +1757,8 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, return true; #endif } + + return false; } void AP_AHRS_NavEKF::setTakeoffExpected(bool val) @@ -1759,15 +1786,21 @@ bool AP_AHRS_NavEKF::getGpsGlitchStatus() const bool AP_AHRS_NavEKF::have_ekf_logging(void) const { switch (ekf_type()) { - case 2: + case EKFType::NONE: + return false; + + case EKFType::TWO: return EKF2.have_ekf_logging(); - case 3: + case EKFType::THREE: return EKF3.have_ekf_logging(); - default: - break; +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: + return false; +#endif } + // since there is no default case above, this is unreachable return false; } @@ -1776,16 +1809,20 @@ uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const { int8_t imu = -1; switch (ekf_type()) { - case 2: + case EKFType::NONE: + break; + case EKFType::TWO: // let EKF2 choose primary IMU imu = EKF2.getPrimaryCoreIMUIndex(); break; - case 3: + case EKFType::THREE: // let EKF2 choose primary IMU imu = EKF3.getPrimaryCoreIMUIndex(); break; - default: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: break; +#endif } if (imu == -1) { imu = AP::ins().get_primary_accel(); @@ -1803,7 +1840,7 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const // get the index of the current primary accelerometer sensor uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const { - if (ekf_type() != 0) { + if (ekf_type() != EKFType::NONE) { return get_primary_IMU_index(); } return AP::ins().get_primary_accel(); @@ -1812,7 +1849,7 @@ uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const // get the index of the current primary gyro sensor uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const { - if (ekf_type() != 0) { + if (ekf_type() != EKFType::NONE) { return get_primary_IMU_index(); } return AP::ins().get_primary_gyro(); @@ -1822,19 +1859,19 @@ uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const void AP_AHRS_NavEKF::check_lane_switch(void) { switch (active_EKF_type()) { - case EKF_TYPE_NONE: + case EKFType::NONE: break; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKF_TYPE_SITL: + case EKFType::SITL: break; #endif - case EKF_TYPE2: + case EKFType::TWO: EKF2.checkLaneSwitch(); break; - case EKF_TYPE3: + case EKFType::THREE: EKF3.checkLaneSwitch(); break; } @@ -1855,11 +1892,13 @@ AP_AHRS_NavEKF &AP::ahrs_navekf() bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const { switch (active_EKF_type()) { - case EKF_TYPE2: + case EKFType::TWO: return EKF2.isExtNavUsedForYaw(); - case EKF_TYPE_NONE: - case EKF_TYPE3: - case EKF_TYPE_SITL: + case EKFType::NONE: + case EKFType::THREE: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: +#endif return false; } // since there is no default case above, this is unreachable diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 6b0b3dedaa..9bc276dee8 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -272,14 +272,15 @@ public: bool is_ext_nav_used_for_yaw(void) const; private: - enum EKF_TYPE {EKF_TYPE_NONE=0, - EKF_TYPE3=3, - EKF_TYPE2=2 + enum class EKFType { + NONE = 0, + THREE = 3, + TWO = 2 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - ,EKF_TYPE_SITL=10 + ,SITL = 10 #endif }; - EKF_TYPE active_EKF_type(void) const; + EKFType active_EKF_type(void) const; bool always_use_EKF() const { return _ekf_flags & FLAG_ALWAYS_USE_EKF; @@ -303,7 +304,7 @@ private: uint32_t start_time_ms = 0; uint8_t _ekf_flags; // bitmask from Flags enumeration - uint8_t ekf_type(void) const; + EKFType ekf_type(void) const; void update_DCM(bool skip_ins_update); void update_EKF2(void); void update_EKF3(void);