// exit_mission_callback - callback function called from ap-mission when the mission has completed // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode void Plane::exit_mission_callback() { if (control_mode == AUTO) { gcs_send_text_fmt(PSTR("Returning to Home")); memset(&auto_rtl_command, 0, sizeof(auto_rtl_command)); auto_rtl_command.content.location = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM; setup_terrain_target_alt(auto_rtl_command.content.location); update_flight_stage(); setup_glide_slope(); setup_turn_angle(); start_command(auto_rtl_command); } }
void Plane::do_RTL(void) { auto_state.next_wp_no_crosstrack = true; auto_state.no_crosstrack = true; prev_WP_loc = current_loc; next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); if (g.loiter_radius < 0) { loiter.direction = -1; } else { loiter.direction = 1; } update_flight_stage(); setup_glide_slope(); setup_turn_angle(); 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 != nullptr && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) && (control_mode == GUIDED || control_mode == AVOID_ADSB) && 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(), MODE_REASON_GCS_COMMAND); } return; } /* allocate the geo-fence state if need be */ if (geofence_state == nullptr || !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 (landing.is_expecting_impact()) { 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(MAV_SEVERITY_INFO,"Geofence 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 || control_mode == AVOID_ADSB || control_mode == RTL || 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(MAV_SEVERITY_NOTICE,"Geofence 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: case FENCE_ACTION_RTL: // 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); if (g.fence_action == FENCE_ACTION_RTL) { set_mode(RTL, MODE_REASON_FENCE_BREACH); } else { set_mode(GUIDED, MODE_REASON_FENCE_BREACH); } g.auto_trim.set(saved_auto_trim); if (g.fence_ret_rally != 0 || g.fence_action == FENCE_ACTION_RTL) { //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; if (g.fence_action != FENCE_ACTION_RTL) { //not needed for RTL mode setup_terrain_target_alt(guided_WP_loc); set_guided_WP(); } if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) { guided_throttle_passthru = true; } break; } }
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) { 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; // reset external attitude guidance guided_state.last_forced_rpy_ms.zero(); guided_state.last_forced_throttle_ms = 0; // set mode previous_mode = control_mode; control_mode = mode; previous_mode_reason = control_mode_reason; control_mode_reason = reason; #if FRSKY_TELEM_ENABLED == ENABLED frsky_telemetry.update_control_mode(control_mode); #endif 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; auto_navigation_mode = false; break; case MANUAL: case STABILIZE: case TRAINING: case FLY_BY_WIRE_A: auto_throttle_mode = false; auto_navigation_mode = false; break; case AUTOTUNE: auto_throttle_mode = false; auto_navigation_mode = false; autotune_start(); break; case ACRO: auto_throttle_mode = false; auto_navigation_mode = false; acro_state.locked_roll = false; acro_state.locked_pitch = false; break; case CRUISE: auto_throttle_mode = true; auto_navigation_mode = false; 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; auto_navigation_mode = false; set_target_altitude_current(); break; case CIRCLE: // the altitude to circle at is taken from the current altitude auto_throttle_mode = true; auto_navigation_mode = true; next_WP_loc.alt = current_loc.alt; break; case AUTO: auto_throttle_mode = true; auto_navigation_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; auto_navigation_mode = true; prev_WP_loc = current_loc; do_RTL(get_RTL_altitude()); break; case LOITER: auto_throttle_mode = true; auto_navigation_mode = true; do_loiter_at_location(); break; case AVOID_ADSB: case GUIDED: auto_throttle_mode = true; auto_navigation_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: auto_navigation_mode = false; 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; adsb.set_is_auto_mode(auto_navigation_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(); }