void Rover::set_mode(enum mode mode) { if (control_mode == mode){ // don't switch modes if we are already in the correct mode. return; } // If we are changing out of AUTO mode reset the loiter timer if (control_mode == AUTO) { loiter_time = 0; } control_mode = mode; throttle_last = 0; throttle = 500; g.pidSpeedThrottle.reset_I(); #if FRSKY_TELEM_ENABLED == ENABLED frsky_telemetry.update_control_mode(control_mode); #endif if (control_mode != AUTO) { auto_triggered = false; } switch(control_mode) { case MANUAL: case HOLD: case LEARNING: case STEERING: auto_throttle_mode = false; break; case AUTO: auto_throttle_mode = true; rtl_complete = false; restart_nav(); break; case RTL: auto_throttle_mode = true; do_RTL(); break; case GUIDED: auto_throttle_mode = true; rtl_complete = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code. */ guided_WP = current_loc; set_guided_WP(); break; default: auto_throttle_mode = true; do_RTL(); break; } if (should_log(MASK_LOG_MODE)) { DataFlash.Log_Write_Mode(control_mode); } }
/* * check if we have breached the geo-fence */ void Plane::geofence_check(bool altitude_check_only) { if (!geofence_enabled()) { // switch back to the chosen control mode if still in // GUIDED to the return point if (geofence_state != NULL && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) && control_mode == GUIDED && geofence_present() && geofence_state->boundary_uptodate && geofence_state->old_switch_position == oldSwitchPosition && guided_WP_loc.lat == geofence_state->guided_lat && guided_WP_loc.lng == geofence_state->guided_lng) { geofence_state->old_switch_position = 254; set_mode(get_previous_mode()); } return; } /* allocate the geo-fence state if need be */ if (geofence_state == NULL || !geofence_state->boundary_uptodate) { geofence_load(); if (!geofence_enabled()) { // may have been disabled by load return; } } bool outside = false; uint8_t breach_type = FENCE_BREACH_NONE; struct Location loc; // Never trigger a fence breach in the final stage of landing if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { return; } if (geofence_state->floor_enabled && geofence_check_minalt()) { outside = true; breach_type = FENCE_BREACH_MINALT; } else if (geofence_check_maxalt()) { outside = true; breach_type = FENCE_BREACH_MAXALT; } else if (!altitude_check_only && ahrs.get_position(loc)) { Vector2l location; location.x = loc.lat; location.y = loc.lng; outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1); if (outside) { breach_type = FENCE_BREACH_BOUNDARY; } } if (!outside) { if (geofence_state->fence_triggered && !altitude_check_only) { // we have moved back inside the fence geofence_state->fence_triggered = false; gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence OK")); #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 0); #endif gcs_send_message(MSG_FENCE_STATUS); } // we're inside, all is good with the world return; } // we are outside the fence if (geofence_state->fence_triggered && (control_mode == GUIDED || g.fence_action == FENCE_ACTION_REPORT)) { // we have already triggered, don't trigger again until the // user disables/re-enables using the fence channel switch return; } // we are outside, and have not previously triggered. geofence_state->fence_triggered = true; geofence_state->breach_count++; geofence_state->breach_time = millis(); geofence_state->breach_type = breach_type; #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 1); #endif gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered")); gcs_send_message(MSG_FENCE_STATUS); // see what action the user wants switch (g.fence_action) { case FENCE_ACTION_REPORT: break; case FENCE_ACTION_GUIDED: case FENCE_ACTION_GUIDED_THR_PASS: if (g.fence_ret_rally != 0) { //return to a rally point guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); } else { //return to fence return point, not a rally point if (g.fence_retalt > 0) { //fly to the return point using fence_retalt guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt; } else if (g.fence_minalt >= g.fence_maxalt) { // invalid min/max, use RTL_altitude guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; } else { // fly to the return point, with an altitude half way between // min and max guided_WP_loc.alt = home.alt + 100.0f*(g.fence_minalt + g.fence_maxalt)/2; } guided_WP_loc.options = 0; guided_WP_loc.lat = geofence_state->boundary[0].x; guided_WP_loc.lng = geofence_state->boundary[0].y; } geofence_state->guided_lat = guided_WP_loc.lat; geofence_state->guided_lng = guided_WP_loc.lng; geofence_state->old_switch_position = oldSwitchPosition; setup_terrain_target_alt(guided_WP_loc); set_guided_WP(); // make sure we don't auto trim the surfaces on this mode change int8_t saved_auto_trim = g.auto_trim; g.auto_trim.set(0); set_mode(GUIDED); g.auto_trim.set(saved_auto_trim); if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) { guided_throttle_passthru = true; } break; } }
void Plane::set_mode(enum FlightMode mode) { if(control_mode == mode) { // don't switch modes if we are already in the correct mode. return; } if(g.auto_trim > 0 && control_mode == MANUAL) trim_control_surfaces(); // perform any cleanup required for prev flight mode exit_mode(control_mode); // cancel inverted flight auto_state.inverted_flight = false; // don't cross-track when starting a mission auto_state.next_wp_no_crosstrack = true; // reset landing check auto_state.checked_for_autoland = false; // reset go around command auto_state.commanded_go_around = false; // zero locked course steer_state.locked_course_err = 0; // reset crash detection crash_state.is_crashed = false; // set mode previous_mode = control_mode; control_mode = mode; if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { // restore last gains autotune_restore(); } // zero initial pitch and highest airspeed on mode change auto_state.highest_airspeed = 0; auto_state.initial_pitch_cd = ahrs.pitch_sensor; // disable taildrag takeoff on mode change auto_state.fbwa_tdrag_takeoff_mode = false; // start with previous WP at current location prev_WP_loc = current_loc; switch(control_mode) { case INITIALISING: auto_throttle_mode = true; break; case MANUAL: case STABILIZE: case TRAINING: case FLY_BY_WIRE_A: auto_throttle_mode = false; break; case AUTOTUNE: auto_throttle_mode = false; autotune_start(); break; case ACRO: auto_throttle_mode = false; acro_state.locked_roll = false; acro_state.locked_pitch = false; break; case CRUISE: auto_throttle_mode = true; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; set_target_altitude_current(); break; case FLY_BY_WIRE_B: auto_throttle_mode = true; set_target_altitude_current(); break; case CIRCLE: // the altitude to circle at is taken from the current altitude auto_throttle_mode = true; next_WP_loc.alt = current_loc.alt; break; case AUTO: auto_throttle_mode = true; next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); break; case RTL: auto_throttle_mode = true; prev_WP_loc = current_loc; do_RTL(); break; case LOITER: auto_throttle_mode = true; do_loiter_at_location(); break; case GUIDED: auto_throttle_mode = true; guided_throttle_passthru = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code */ guided_WP_loc = current_loc; set_guided_WP(); break; } // start with throttle suppressed in auto_throttle modes throttle_suppressed = auto_throttle_mode; if (should_log(MASK_LOG_MODE)) DataFlash.Log_Write_Mode(control_mode); // reset attitude integrators on mode change rollController.reset_I(); pitchController.reset_I(); yawController.reset_I(); steerController.reset_I(); }
void Plane::set_mode(enum FlightMode mode) { if(control_mode == mode) { // don't switch modes if we are already in the correct mode. return; } if(g.auto_trim > 0 && control_mode == MANUAL) trim_control_surfaces(); // perform any cleanup required for prev flight mode exit_mode(control_mode); // cancel inverted flight auto_state.inverted_flight = false; // don't cross-track when starting a mission auto_state.next_wp_no_crosstrack = true; // reset landing check auto_state.checked_for_autoland = false; // reset go around command auto_state.commanded_go_around = false; // not in pre-flare auto_state.land_pre_flare = false; // zero locked course steer_state.locked_course_err = 0; // reset crash detection crash_state.is_crashed = false; crash_state.impact_detected = false; // always reset this because we don't know who called set_mode. In evasion // behavior you should set this flag after set_mode so you know the evasion // logic is controlling the mode. This allows manual override of the mode // to exit evasion behavior automatically but if the mode is manually switched // then we won't resume AUTO after an evasion adsb_state.is_evading = false; // set mode previous_mode = control_mode; control_mode = mode; if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { // restore last gains autotune_restore(); } // zero initial pitch and highest airspeed on mode change auto_state.highest_airspeed = 0; auto_state.initial_pitch_cd = ahrs.pitch_sensor; // disable taildrag takeoff on mode change auto_state.fbwa_tdrag_takeoff_mode = false; // start with previous WP at current location prev_WP_loc = current_loc; // new mode means new loiter loiter.start_time_ms = 0; // assume non-VTOL mode auto_state.vtol_mode = false; auto_state.vtol_loiter = false; switch(control_mode) { case INITIALISING: auto_throttle_mode = true; break; case MANUAL: case STABILIZE: case TRAINING: case FLY_BY_WIRE_A: auto_throttle_mode = false; break; case AUTOTUNE: auto_throttle_mode = false; autotune_start(); break; case ACRO: auto_throttle_mode = false; acro_state.locked_roll = false; acro_state.locked_pitch = false; break; case CRUISE: auto_throttle_mode = true; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; set_target_altitude_current(); break; case FLY_BY_WIRE_B: auto_throttle_mode = true; set_target_altitude_current(); break; case CIRCLE: // the altitude to circle at is taken from the current altitude auto_throttle_mode = true; next_WP_loc.alt = current_loc.alt; break; case AUTO: auto_throttle_mode = true; if (quadplane.available() && quadplane.enable == 2) { auto_state.vtol_mode = true; } else { auto_state.vtol_mode = false; } next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); break; case RTL: auto_throttle_mode = true; prev_WP_loc = current_loc; do_RTL(get_RTL_altitude()); break; case LOITER: auto_throttle_mode = true; do_loiter_at_location(); break; case GUIDED: auto_throttle_mode = true; guided_throttle_passthru = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code */ guided_WP_loc = current_loc; set_guided_WP(); break; case QSTABILIZE: case QHOVER: case QLOITER: case QLAND: case QRTL: if (!quadplane.init_mode()) { control_mode = previous_mode; } else { auto_throttle_mode = false; auto_state.vtol_mode = true; } break; } // start with throttle suppressed in auto_throttle modes throttle_suppressed = auto_throttle_mode; if (should_log(MASK_LOG_MODE)) DataFlash.Log_Write_Mode(control_mode); // reset attitude integrators on mode change rollController.reset_I(); pitchController.reset_I(); yawController.reset_I(); steerController.reset_I(); }
// sets ahrs home to specified location // returns true if home location set successfully bool Rover::set_home(const Location& loc, bool lock) { // check location is valid if (loc.lat == 0 && loc.lng == 0 && loc.alt == 0) { return false; } if (!check_latlng(loc)) { return false; } // set EKF origin to home if it hasn't been set yet Location ekf_origin; if (!ahrs.get_origin(ekf_origin)) { ahrs.set_origin(loc); } // set ahrs home ahrs.set_home(loc); // init compass declination if (home_is_set == HOME_UNSET) { // record home is set home_is_set = HOME_SET_NOT_LOCKED; // log new home position which mission library will pull from ahrs if (should_log(MASK_LOG_CMD)) { AP_Mission::Mission_Command temp_cmd; if (mission.read_cmd_from_storage(0, temp_cmd)) { DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd); } } // initialise navigation to home next_WP = prev_WP = home; // Load home for a default guided_WP set_guided_WP(home); } // lock home position if (lock) { home_is_set = HOME_SET_AND_LOCKED; } // Save Home to EEPROM mission.write_home_to_storage(); // log ahrs home and ekf origin dataflash Log_Write_Home_And_Origin(); // send new home location to GCS gcs().send_home(loc); // send text of home position to ground stations gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm", static_cast<double>(loc.lat * 1.0e-7f), static_cast<double>(loc.lng * 1.0e-7f), static_cast<double>(loc.alt * 0.01f)); // return success return true; }