2019-01-15 13:46:13 -04:00
|
|
|
#include "mode.h"
|
|
|
|
#include "Plane.h"
|
|
|
|
|
|
|
|
bool ModeAuto::_enter()
|
|
|
|
{
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2022-05-16 05:31:15 -03:00
|
|
|
// check if we should refuse auto mode due to a missing takeoff in
|
|
|
|
// guided_wait_takeoff state
|
|
|
|
if (plane.previous_mode == &plane.mode_guided &&
|
|
|
|
quadplane.guided_wait_takeoff_on_mode_enter) {
|
|
|
|
if (!plane.mission.starts_with_takeoff_cmd()) {
|
|
|
|
gcs().send_text(MAV_SEVERITY_ERROR,"Takeoff waypoint required");
|
2022-05-18 08:50:17 -03:00
|
|
|
quadplane.guided_wait_takeoff = true;
|
2022-05-16 05:31:15 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.quadplane.available() && plane.quadplane.enable == 2) {
|
|
|
|
plane.auto_state.vtol_mode = true;
|
|
|
|
} else {
|
|
|
|
plane.auto_state.vtol_mode = false;
|
|
|
|
}
|
2021-09-10 03:28:21 -03:00
|
|
|
#else
|
|
|
|
plane.auto_state.vtol_mode = false;
|
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
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();
|
|
|
|
|
2019-05-09 06:23:56 -03:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
plane.g2.soaring_controller.init_cruising();
|
2019-04-07 18:56:08 -03:00
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void ModeAuto::_exit()
|
|
|
|
{
|
|
|
|
if (plane.mission.state() == AP_Mission::MISSION_RUNNING) {
|
|
|
|
plane.mission.stop();
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
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) {
|
2019-01-15 13:46:13 -04:00
|
|
|
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
|
2019-10-17 00:49:32 -03:00
|
|
|
plane.set_mode(plane.mode_rtl, ModeReason::MISSION_END);
|
2019-01-15 13:46:13 -04:00
|
|
|
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;
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.quadplane.in_vtol_auto()) {
|
2019-10-01 11:11:48 -03:00
|
|
|
plane.quadplane.control_auto();
|
2021-09-10 03:28:21 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
|
2022-09-29 20:10:41 -03:00
|
|
|
(nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
|
2019-01-15 13:46:13 -04:00
|
|
|
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
|
2021-09-18 15:02:12 -03:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
|
2019-01-15 13:46:13 -04:00
|
|
|
} else {
|
|
|
|
plane.calc_throttle();
|
|
|
|
}
|
2021-11-15 01:08:34 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2021-10-25 00:59:23 -03:00
|
|
|
} else if (nav_cmd_id == MAV_CMD_NAV_SCRIPT_TIME) {
|
|
|
|
// NAV_SCRIPTING has a desired roll and pitch rate and desired throttle
|
2023-02-12 09:33:27 -04:00
|
|
|
plane.nav_roll_cd = ahrs.roll_sensor;
|
|
|
|
plane.nav_pitch_cd = ahrs.pitch_sensor;
|
2021-10-25 00:59:23 -03:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
|
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
} 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();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void ModeAuto::navigate()
|
2020-07-29 12:01:34 -03:00
|
|
|
{
|
|
|
|
if (AP::ahrs().home_is_set()) {
|
|
|
|
plane.mission.update();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-01-30 21:26:51 -04:00
|
|
|
|
|
|
|
bool ModeAuto::does_auto_navigation() const
|
|
|
|
{
|
|
|
|
#if AP_SCRIPTING_ENABLED
|
|
|
|
return (!plane.nav_scripting_active());
|
|
|
|
#endif
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool ModeAuto::does_auto_throttle() const
|
|
|
|
{
|
|
|
|
#if AP_SCRIPTING_ENABLED
|
|
|
|
return (!plane.nav_scripting_active());
|
|
|
|
#endif
|
|
|
|
return true;
|
|
|
|
}
|
2023-02-14 08:18:09 -04:00
|
|
|
|
|
|
|
// returns true if the vehicle can be armed in this mode
|
|
|
|
bool ModeAuto::_pre_arm_checks(size_t buflen, char *buffer) const
|
|
|
|
{
|
|
|
|
#if HAL_QUADPLANE_ENABLED
|
|
|
|
if (plane.quadplane.enabled()) {
|
|
|
|
if (plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO) &&
|
|
|
|
!plane.quadplane.is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
|
|
|
|
hal.util->snprintf(buffer, buflen, "not in VTOL takeoff");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (!plane.mission.starts_with_takeoff_cmd()) {
|
|
|
|
hal.util->snprintf(buffer, buflen, "missing takeoff waypoint");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
// Note that this bypasses the base class checks
|
|
|
|
return true;
|
|
|
|
}
|
2023-06-17 14:34:08 -03:00
|
|
|
|
|
|
|
bool ModeAuto::is_landing() const
|
|
|
|
{
|
2023-06-21 16:30:05 -03:00
|
|
|
return (plane.flight_stage == AP_FixedWing::FlightStage::LAND);
|
2023-06-17 14:34:08 -03:00
|
|
|
}
|