ardupilot/ArduPlane/mode_auto.cpp

108 lines
3.3 KiB
C++

#include "mode.h"
#include "Plane.h"
bool ModeAuto::_enter()
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.available() && plane.quadplane.enable == 2) {
plane.auto_state.vtol_mode = true;
} else {
plane.auto_state.vtol_mode = false;
}
#else
plane.auto_state.vtol_mode = false;
#endif
plane.next_WP_loc = plane.prev_WP_loc = plane.current_loc;
// start or resume the mission, based on MIS_AUTORESET
plane.mission.start_or_resume();
if (hal.util->was_watchdog_armed()) {
if (hal.util->persistent_data.waypoint_num != 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Watchdog: resume WP %u", hal.util->persistent_data.waypoint_num);
plane.mission.set_current_cmd(hal.util->persistent_data.waypoint_num);
hal.util->persistent_data.waypoint_num = 0;
}
}
#if HAL_SOARING_ENABLED
plane.g2.soaring_controller.init_cruising();
#endif
return true;
}
void ModeAuto::_exit()
{
if (plane.mission.state() == AP_Mission::MISSION_RUNNING) {
plane.mission.stop();
bool restart = plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND;
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id)) {
restart = false;
}
#endif
if (restart) {
plane.landing.restart_landing_sequence();
}
}
plane.auto_state.started_flying_in_auto_ms = 0;
}
void ModeAuto::update()
{
if (plane.mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
plane.set_mode(plane.mode_rtl, ModeReason::MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
uint16_t nav_cmd_id = plane.mission.get_current_nav_cmd().id;
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_auto()) {
plane.quadplane.control_auto();
return;
}
#endif
if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
plane.takeoff_calc_roll();
plane.takeoff_calc_pitch();
plane.calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
plane.calc_nav_roll();
plane.calc_nav_pitch();
// allow landing to restrict the roll limits
plane.nav_roll_cd = plane.landing.constrain_roll(plane.nav_roll_cd, plane.g.level_roll_limit*100UL);
if (plane.landing.is_throttle_suppressed()) {
// if landing is considered complete throttle is never allowed, regardless of landing type
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else {
plane.calc_throttle();
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
plane.steer_state.hold_course_cd = -1;
}
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
}
}
void ModeAuto::navigate()
{
if (AP::ahrs().home_is_set()) {
plane.mission.update();
}
}