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; }