Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4         192 : bool ModeAuto::_enter()
       5             : {
       6             : #if HAL_QUADPLANE_ENABLED
       7             :     // check if we should refuse auto mode due to a missing takeoff in
       8             :     // guided_wait_takeoff state
       9         192 :     if (plane.previous_mode == &plane.mode_guided &&
      10          16 :         quadplane.guided_wait_takeoff_on_mode_enter) {
      11           0 :         if (!plane.mission.starts_with_takeoff_cmd()) {
      12           0 :             gcs().send_text(MAV_SEVERITY_ERROR,"Takeoff waypoint required");
      13           0 :             quadplane.guided_wait_takeoff = true;
      14           0 :             return false;
      15             :         }
      16             :     }
      17             :     
      18         192 :     if (plane.quadplane.available() && plane.quadplane.enable == 2) {
      19           1 :         plane.auto_state.vtol_mode = true;
      20             :     } else {
      21         191 :         plane.auto_state.vtol_mode = false;
      22             :     }
      23             : 
      24             :     // initialise waypoint and spline controller so that WP_SPD parameter
      25             :     // changes are picked up on mode entry, matching ArduCopter's mode_auto init
      26         192 :     if (plane.quadplane.available()) {
      27          55 :         plane.quadplane.wp_nav->wp_and_spline_init_m();
      28             :     }
      29             : #else
      30             :     plane.auto_state.vtol_mode = false;
      31             : #endif
      32         192 :     plane.next_WP_loc = plane.prev_WP_loc = plane.current_loc;
      33             :     // start or resume the mission, based on MIS_AUTORESET
      34         192 :     plane.mission.start_or_resume();
      35             : 
      36         192 :     if (hal.util->was_watchdog_armed()) {
      37           0 :         if (hal.util->persistent_data.waypoint_num != 0) {
      38           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Watchdog: resume WP %u", hal.util->persistent_data.waypoint_num);
      39           0 :             plane.mission.set_current_cmd(hal.util->persistent_data.waypoint_num);
      40           0 :             hal.util->persistent_data.waypoint_num = 0;
      41             :         }
      42             :     }
      43             : 
      44             : #if HAL_SOARING_ENABLED
      45         192 :     plane.g2.soaring_controller.init_cruising();
      46             : #endif
      47             : 
      48         192 :     return true;
      49             : }
      50             : 
      51         122 : void ModeAuto::_exit()
      52             : {
      53         122 :     if (plane.mission.state() == AP_Mission::MISSION_RUNNING) {
      54         117 :         plane.mission.stop();
      55             : 
      56         117 :         bool restart = plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND;
      57             : #if HAL_QUADPLANE_ENABLED
      58         117 :         if (plane.quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id)) {
      59          13 :             restart = false;
      60             :         }
      61             : #endif
      62         117 :         if (restart) {
      63          56 :             plane.landing.restart_landing_sequence();
      64             :         }
      65             :     }
      66         122 :     plane.auto_state.started_flying_in_auto_ms = 0;
      67         122 : }
      68             : 
      69     3603874 : void ModeAuto::update()
      70             : {
      71     3603874 :     if (plane.mission.state() != AP_Mission::MISSION_RUNNING) {
      72             :         // this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
      73             :         // restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
      74           0 :         plane.set_mode(plane.mode_rtl, ModeReason::MISSION_END);
      75           0 :         gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
      76           0 :         return;
      77             :     }
      78             : 
      79     3603874 :     uint16_t nav_cmd_id = plane.mission.get_current_nav_cmd().id;
      80             : 
      81             : #if HAL_QUADPLANE_ENABLED
      82     3603874 :     if (plane.quadplane.in_vtol_auto()) {
      83      958749 :         plane.quadplane.control_auto();
      84      958749 :         return;
      85             :     }
      86             : #endif
      87             : 
      88             : #if AP_PLANE_GLIDER_PULLUP_ENABLED
      89     2645125 :     if (pullup.in_pullup()) {
      90        1620 :         return;
      91             :     }
      92             : #endif
      93             : 
      94     2643505 :     if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
      95      208818 :         (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
      96       28022 :         plane.takeoff_calc_roll();
      97       28022 :         plane.takeoff_calc_pitch();
      98       28022 :         plane.takeoff_calc_throttle();
      99     2615483 :     } else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
     100      207583 :         plane.calc_nav_roll();
     101      207583 :         plane.calc_nav_pitch();
     102             : 
     103             :         // allow landing to restrict the roll limits
     104      207583 :         plane.nav_roll_cd = plane.landing.constrain_roll(plane.nav_roll_cd, plane.g.level_roll_limit*100UL);
     105             : 
     106      207583 :         if (plane.landing.is_throttle_suppressed()) {
     107             :             // if landing is considered complete throttle is never allowed, regardless of landing type
     108      121325 :             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
     109             :         } else {
     110       86258 :             plane.calc_throttle();
     111             :         }
     112             : #if AP_SCRIPTING_ENABLED
     113     2407900 :     } else if (nav_cmd_id == MAV_CMD_NAV_SCRIPT_TIME) {
     114             :         // NAV_SCRIPTING has a desired roll and pitch rate and desired throttle
     115           0 :         plane.nav_roll_cd = ahrs.roll_sensor;
     116           0 :         plane.nav_pitch_cd = ahrs.pitch_sensor;
     117             : #endif
     118             :     } else {
     119             :         // we are doing normal AUTO flight, the special cases
     120             :         // are for takeoff and landing
     121     2407900 :         if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
     122     2407900 :             plane.steer_state.hold_course_cd = -1;
     123             :         }
     124     2407900 :         plane.calc_nav_roll();
     125     2407900 :         plane.calc_nav_pitch();
     126     2407900 :         plane.calc_throttle();
     127             :     }
     128             : }
     129             : 
     130      245874 : void ModeAuto::navigate()
     131             : {
     132      245874 :     if (AP::ahrs().home_is_set()) {
     133      245826 :         plane.mission.update();
     134             :     }
     135      245874 : }
     136             : 
     137             : 
     138     5749115 : bool ModeAuto::does_auto_navigation() const
     139             : {
     140             : #if AP_SCRIPTING_ENABLED
     141     5749115 :    return (!plane.nav_scripting_active());
     142             : #endif
     143             :    return true;
     144             : }
     145             : 
     146    31022763 : bool ModeAuto::does_auto_throttle() const
     147             : {
     148             : #if AP_SCRIPTING_ENABLED
     149    31022763 :    return (!plane.nav_scripting_active());
     150             : #endif
     151             :    return true;
     152             : }
     153             : 
     154             : // returns true if the vehicle can be armed in this mode
     155        1717 : bool ModeAuto::_pre_arm_checks(size_t buflen, char *buffer) const
     156             : {
     157             : #if HAL_QUADPLANE_ENABLED
     158        1717 :     if (plane.quadplane.enabled()) {
     159         647 :         if (plane.quadplane.option_is_set(QuadPlane::Option::ONLY_ARM_IN_QMODE_OR_AUTO) &&
     160           0 :                 !plane.quadplane.is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
     161           0 :             hal.util->snprintf(buffer, buflen, "not in VTOL takeoff");
     162           0 :             return false;
     163             :         }
     164         647 :         if (!plane.mission.starts_with_takeoff_cmd()) {
     165          78 :             hal.util->snprintf(buffer, buflen, "missing takeoff waypoint");
     166          78 :             return false;
     167             :         }
     168             :     }
     169             : #endif
     170             :     // Note that this bypasses the base class checks
     171        1639 :     return true;
     172             : }
     173             : 
     174        3058 : bool ModeAuto::is_landing() const
     175             : {
     176        3058 :     return (plane.flight_stage == AP_FixedWing::FlightStage::LAND);
     177             : }
     178             : 
     179     3603874 : void ModeAuto::run()
     180             : {
     181             : #if AP_PLANE_GLIDER_PULLUP_ENABLED
     182     3603874 :     if (pullup.in_pullup()) {
     183        1620 :         pullup.stabilize_pullup();
     184        1620 :         return;
     185             :     }
     186             : #endif
     187             :     
     188     3602254 :     if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) {
     189             : 
     190      199220 :         wiggle_servos();
     191             : 
     192      199220 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
     193      199220 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
     194      199220 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
     195             : 
     196      199220 :         SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
     197      199220 :         SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleLeft);
     198      199220 :         SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleRight);
     199             : 
     200             :         // Relax attitude control
     201      199220 :         reset_controllers();
     202             : 
     203             :     } else {
     204             :         // Normal flight, run base class
     205     3403034 :         Mode::run();
     206             : 
     207             :     }
     208             : }