/*
canonicalise _ekf_type, forcing it to be 0, 1 or 2
*/
uint8_t AP_AHRS_NavEKF::ekf_type(void) const
{
uint8_t type = _ekf_type;
if (always_use_EKF() && type == 0) {
type = 1;
}
#if !AP_AHRS_WITH_EKF1
if (type == 1) {
type = 2;
}
#endif
// check for invalid type
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (type > 2 && type != EKF_TYPE_SITL) {
type = 2;
}
#else
if (type > 2) {
type = 2;
}
#endif
return type;
}
/*
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
{
uint8_t type = _ekf_type;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (type == EKF_TYPE_SITL) {
return type;
}
#endif
if ((always_use_EKF() && (type == 0)) || (type == 1) || (type > 3)) {
type = 2;
}
return type;
}
/*
canonicalise _ekf_type, forcing it to be 0, 1 or 2
*/
uint8_t AP_AHRS_NavEKF::ekf_type(void) const
{
uint8_t type = _ekf_type;
if (always_use_EKF() && type == 0) {
type = 1;
}
// check for invalid type
if (type > 2) {
type = 1;
}
return type;
}
AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
{
EKF_TYPE ret = EKF_TYPE_NONE;
switch (ekf_type()) {
case 0:
return EKF_TYPE_NONE;
#if AP_AHRS_WITH_EKF1
case 1: {
// do we have an EKF yet?
if (!ekf1_started) {
return EKF_TYPE_NONE;
}
if (always_use_EKF()) {
uint8_t ekf_faults;
EKF1.getFilterFaults(ekf_faults);
if (ekf_faults == 0) {
ret = EKF_TYPE1;
}
} else if (EKF1.healthy()) {
ret = EKF_TYPE1;
}
break;
}
#endif
case 2: {
// do we have an EKF2 yet?
if (!ekf2_started) {
return EKF_TYPE_NONE;
}
if (always_use_EKF()) {
uint8_t ekf2_faults;
EKF2.getFilterFaults(-1,ekf2_faults);
if (ekf2_faults == 0) {
ret = EKF_TYPE2;
}
} else if (EKF2.healthy()) {
ret = EKF_TYPE2;
}
break;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
ret = EKF_TYPE_SITL;
break;
#endif
}
/*
fixed wing and rover when in fly_forward mode will fall back to
DCM if the EKF doesn't have GPS. This is the safest option as
DCM is very robust
*/
if (ret != EKF_TYPE_NONE &&
(_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) &&
_flags.fly_forward) {
nav_filter_status filt_state;
if (ret == EKF_TYPE2) {
EKF2.getFilterStatus(-1,filt_state);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
} else if (ret == EKF_TYPE_SITL) {
get_filter_status(filt_state);
#endif
#if AP_AHRS_WITH_EKF1
} else {
EKF1.getFilterStatus(filt_state);
#endif
}
if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _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
// DCM. This is a safety net while some issues with the EKF
// get sorted out
return EKF_TYPE_NONE;
}
if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {
return EKF_TYPE_NONE;
}
if (!filt_state.flags.attitude ||
!filt_state.flags.horiz_vel ||
!filt_state.flags.vert_vel ||
!filt_state.flags.horiz_pos_abs ||
!filt_state.flags.vert_pos) {
return EKF_TYPE_NONE;
}
}
return ret;
}
AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
{
EKF_TYPE ret = EKF_TYPE_NONE;
switch (ekf_type()) {
case 0:
return EKF_TYPE_NONE;
#if AP_AHRS_WITH_EKF1
case 1: {
// do we have an EKF yet?
if (!ekf1_started) {
return EKF_TYPE_NONE;
}
if (always_use_EKF()) {
uint16_t ekf_faults;
EKF1.getFilterFaults(ekf_faults);
if (ekf_faults == 0) {
ret = EKF_TYPE1;
}
} else if (EKF1.healthy()) {
ret = EKF_TYPE1;
}
break;
}
#endif
case 2: {
// do we have an EKF2 yet?
if (!ekf2_started) {
return EKF_TYPE_NONE;
}
if (always_use_EKF()) {
uint16_t ekf2_faults;
EKF2.getFilterFaults(-1,ekf2_faults);
if (ekf2_faults == 0) {
ret = EKF_TYPE2;
}
} else if (EKF2.healthy()) {
ret = EKF_TYPE2;
}
break;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
ret = EKF_TYPE_SITL;
break;
#endif
}
/*
fixed wing and rover when in fly_forward mode will fall back to
DCM if the EKF doesn't have GPS. This is the safest option as
DCM is very robust. Note that we also check the filter status
when fly_forward is false and we are disarmed. This is to ensure
that the arming checks do wait for good GPS position on fixed
wing and rover
*/
if (ret != EKF_TYPE_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) {
EKF2.getFilterStatus(-1,filt_state);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
} else if (ret == EKF_TYPE_SITL) {
get_filter_status(filt_state);
#endif
#if AP_AHRS_WITH_EKF1
} else {
EKF1.getFilterStatus(filt_state);
#endif
}
if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _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
// DCM. This is a safety net while some issues with the EKF
// get sorted out
return EKF_TYPE_NONE;
}
if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {
return EKF_TYPE_NONE;
}
if (!filt_state.flags.attitude ||
!filt_state.flags.vert_vel ||
!filt_state.flags.vert_pos) {
return EKF_TYPE_NONE;
}
if (!filt_state.flags.horiz_vel ||
!filt_state.flags.horiz_pos_abs) {
if ((!_compass || !_compass->use_for_yaw()) &&
_gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
_gps.ground_speed() < 2) {
/*
special handling for non-compass mode when sitting
still. The EKF may not yet have aligned its yaw. We
accept EKF as healthy to allow arming. Once we reach
speed the EKF should get yaw alignment
*/
if (filt_state.flags.pred_horiz_pos_abs &&
filt_state.flags.pred_horiz_pos_rel) {
return ret;
}
}
return EKF_TYPE_NONE;
}
}
return ret;
}
AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
{
EKF_TYPE ret = EKF_TYPE_NONE;
switch (ekf_type()) {
case 0:
return EKF_TYPE_NONE;
case 1: {
// do we have an EKF yet?
if (!ekf1_started) {
return EKF_TYPE_NONE;
}
if (always_use_EKF()) {
uint8_t ekf_faults;
EKF1.getFilterFaults(ekf_faults);
if (ekf_faults == 0) {
ret = EKF_TYPE1;
}
} else if (EKF1.healthy()) {
ret = EKF_TYPE1;
}
break;
}
case 2: {
// do we have an EKF2 yet?
if (!ekf2_started) {
return EKF_TYPE_NONE;
}
if (always_use_EKF()) {
uint8_t ekf2_faults;
EKF2.getFilterFaults(-1,ekf2_faults);
if (ekf2_faults == 0) {
ret = EKF_TYPE2;
}
} else if (EKF2.healthy()) {
ret = EKF_TYPE2;
}
break;
}
}
if (ret != EKF_TYPE_NONE &&
(_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND)) {
nav_filter_status filt_state;
if (ret == EKF_TYPE1) {
EKF1.getFilterStatus(filt_state);
} else {
EKF2.getFilterStatus(-1,filt_state);
}
if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _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
// DCM. This is a safety net while some issues with the EKF
// get sorted out
return EKF_TYPE_NONE;
}
if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {
return EKF_TYPE_NONE;
}
if (!filt_state.flags.attitude ||
!filt_state.flags.horiz_vel ||
!filt_state.flags.vert_vel ||
!filt_state.flags.horiz_pos_abs ||
!filt_state.flags.vert_pos) {
return EKF_TYPE_NONE;
}
}
return ret;
}
