mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 00:04:02 -04:00
AP_AHRS_NavEKF: canonicalise use of get_active_NavEKF result
Some places had a default case, many didn't.
This commit is contained in:
parent
daf1b57736
commit
968d8cf7d9
@ -531,11 +531,15 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
|
|||||||
|
|
||||||
case EKF_TYPE3:
|
case EKF_TYPE3:
|
||||||
|
|
||||||
default:
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
case EKF_TYPE_SITL:
|
||||||
|
#endif
|
||||||
// DCM is secondary
|
// DCM is secondary
|
||||||
eulers = _dcm_attitude;
|
eulers = _dcm_attitude;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -552,11 +556,15 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const
|
|||||||
|
|
||||||
case EKF_TYPE3:
|
case EKF_TYPE3:
|
||||||
|
|
||||||
default:
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
case EKF_TYPE_SITL:
|
||||||
|
#endif
|
||||||
// DCM is secondary
|
// DCM is secondary
|
||||||
quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
|
quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return secondary position solution if available
|
// return secondary position solution if available
|
||||||
@ -572,11 +580,15 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const
|
|||||||
|
|
||||||
case EKF_TYPE3:
|
case EKF_TYPE3:
|
||||||
|
|
||||||
default:
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
case EKF_TYPE_SITL:
|
||||||
|
#endif
|
||||||
// return DCM position
|
// return DCM position
|
||||||
AP_AHRS_DCM::get_position(loc);
|
AP_AHRS_DCM::get_position(loc);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// EKF has a better ground speed vector estimate
|
// EKF has a better ground speed vector estimate
|
||||||
@ -620,6 +632,9 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc)
|
|||||||
|
|
||||||
// return success if active EKF's origin was set
|
// return success if active EKF's origin was set
|
||||||
switch (active_EKF_type()) {
|
switch (active_EKF_type()) {
|
||||||
|
case EKF_TYPE_NONE:
|
||||||
|
return false;
|
||||||
|
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2:
|
||||||
return ret2;
|
return ret2;
|
||||||
|
|
||||||
@ -636,10 +651,9 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc)
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
default:
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return true if inertial navigation is active
|
// return true if inertial navigation is active
|
||||||
@ -708,7 +722,6 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
|
|||||||
return false;
|
return false;
|
||||||
|
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2:
|
||||||
default:
|
|
||||||
EKF2.getMagXYZ(-1,vec);
|
EKF2.getMagXYZ(-1,vec);
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
@ -721,6 +734,8 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
|
|||||||
return false;
|
return false;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
|
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
|
||||||
@ -732,7 +747,6 @@ bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const
|
|||||||
return false;
|
return false;
|
||||||
|
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2:
|
||||||
default:
|
|
||||||
velocity = EKF2.getPosDownDerivative(-1);
|
velocity = EKF2.getPosDownDerivative(-1);
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
@ -751,6 +765,8 @@ bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// get latest height above ground level estimate in metres and a validity flag
|
// get latest height above ground level estimate in metres and a validity flag
|
||||||
@ -761,7 +777,6 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const
|
|||||||
return false;
|
return false;
|
||||||
|
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2:
|
||||||
default:
|
|
||||||
return EKF2.getHAGL(height);
|
return EKF2.getHAGL(height);
|
||||||
|
|
||||||
case EKF_TYPE3:
|
case EKF_TYPE3:
|
||||||
@ -778,6 +793,8 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return a relative ground position to the origin in meters
|
// return a relative ground position to the origin in meters
|
||||||
@ -788,8 +805,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const
|
|||||||
case EKF_TYPE_NONE:
|
case EKF_TYPE_NONE:
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2: {
|
||||||
default: {
|
|
||||||
Vector2f posNE;
|
Vector2f posNE;
|
||||||
float posD;
|
float posD;
|
||||||
if (EKF2.getPosNE(-1,posNE) && EKF2.getPosD(-1,posD)) {
|
if (EKF2.getPosNE(-1,posNE) && EKF2.getPosD(-1,posD)) {
|
||||||
@ -830,6 +846,8 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return a relative ground position to the home in meters
|
// return a relative ground position to the home in meters
|
||||||
@ -859,8 +877,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const
|
|||||||
case EKF_TYPE_NONE:
|
case EKF_TYPE_NONE:
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2: {
|
||||||
default: {
|
|
||||||
bool position_is_valid = EKF2.getPosNE(-1,posNE);
|
bool position_is_valid = EKF2.getPosNE(-1,posNE);
|
||||||
return position_is_valid;
|
return position_is_valid;
|
||||||
}
|
}
|
||||||
@ -879,6 +896,8 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return a relative ground position to the home in meters
|
// return a relative ground position to the home in meters
|
||||||
@ -910,8 +929,7 @@ bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const
|
|||||||
case EKF_TYPE_NONE:
|
case EKF_TYPE_NONE:
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2: {
|
||||||
default: {
|
|
||||||
bool position_is_valid = EKF2.getPosD(-1,posD);
|
bool position_is_valid = EKF2.getPosD(-1,posD);
|
||||||
return position_is_valid;
|
return position_is_valid;
|
||||||
}
|
}
|
||||||
@ -932,6 +950,8 @@ bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// write a relative ground position to home in meters, Down
|
// write a relative ground position to home in meters, Down
|
||||||
@ -1837,10 +1857,13 @@ bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const
|
|||||||
switch (active_EKF_type()) {
|
switch (active_EKF_type()) {
|
||||||
case EKF_TYPE2:
|
case EKF_TYPE2:
|
||||||
return EKF2.isExtNavUsedForYaw();
|
return EKF2.isExtNavUsedForYaw();
|
||||||
|
case EKF_TYPE_NONE:
|
||||||
default:
|
case EKF_TYPE3:
|
||||||
|
case EKF_TYPE_SITL:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
// since there is no default case above, this is unreachable
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // AP_AHRS_NAVEKF_AVAILABLE
|
#endif // AP_AHRS_NAVEKF_AVAILABLE
|
||||||
|
Loading…
Reference in New Issue
Block a user