bool Mode::enter()
{
const bool ignore_checks = !hal.util->get_soft_armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
if (!ignore_checks) {
// get EKF filter status
nav_filter_status filt_status;
rover.ahrs.get_filter_status(filt_status);
// check position estimate. requires origin and at least one horizontal position flag to be true
Location origin;
const bool position_ok = ahrs.get_origin(origin) &&
(filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs ||
filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel);
if (requires_position() && !position_ok) {
return false;
}
// check velocity estimate (if we have position estimate, we must have velocity estimate)
if (requires_velocity() && !position_ok && !filt_status.flags.horiz_vel) {
return false;
}
}
return _enter();
}
bool Mode::enter()
{
const bool ignore_checks = !hal.util->get_soft_armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
if (!ignore_checks) {
// get EKF filter status
nav_filter_status filt_status;
rover.ahrs.get_filter_status(filt_status);
// check position estimate. requires origin and at least one horizontal position flag to be true
const bool position_ok = rover.ekf_position_ok() && !rover.failsafe.ekf;
if (requires_position() && !position_ok) {
return false;
}
// check velocity estimate (if we have position estimate, we must have velocity estimate)
if (requires_velocity() && !position_ok && !filt_status.flags.horiz_vel) {
return false;
}
}
bool ret = _enter();
// initialisation common to all modes
if (ret) {
set_reversed(false);
// clear sailboat tacking flags
rover.sailboat_clear_tack();
}
return ret;
}
