Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3       17992 : Mode::Mode() :
       4       17992 :     unused_integer{17},
       5             : #if HAL_QUADPLANE_ENABLED
       6       17992 :     pos_control(plane.quadplane.pos_control),
       7       17992 :     attitude_control(plane.quadplane.attitude_control),
       8       17992 :     loiter_nav(plane.quadplane.loiter_nav),
       9       17992 :     quadplane(plane.quadplane),
      10       17992 :     poscontrol(plane.quadplane.poscontrol),
      11             : #endif
      12       17992 :     ahrs(plane.ahrs)
      13             : {
      14       17992 : }
      15             : 
      16        1926 : void Mode::exit()
      17             : {
      18             :     // call sub-classes exit
      19        1926 :     _exit();
      20             :     // stop autotuning if not AUTOTUNE mode
      21        1926 :     if (plane.control_mode != &plane.mode_autotune){
      22        1923 :         plane.autotune_restore();
      23             :     }
      24             : 
      25        1926 : }
      26             : 
      27        1930 : bool Mode::enter()
      28             : {
      29             : #if AP_SCRIPTING_ENABLED
      30             :     // reset nav_scripting.enabled
      31        1930 :     plane.nav_scripting.enabled = false;
      32             : #endif
      33             : 
      34             :     // cancel inverted flight
      35        1930 :     plane.auto_state.inverted_flight = false;
      36             :     
      37             :     // cancel waiting for rudder neutral
      38        1930 :     plane.takeoff_state.waiting_for_rudder_neutral = false;
      39             : 
      40             :     // don't cross-track when starting a mission
      41        1930 :     plane.auto_state.next_wp_crosstrack = false;
      42             : 
      43             :     // reset landing check
      44        1930 :     plane.auto_state.checked_for_autoland = false;
      45             : 
      46             :     // zero locked course
      47        1930 :     plane.steer_state.locked_course_err = 0;
      48        1930 :     plane.steer_state.locked_course = false;
      49             : 
      50             :     // reset crash detection
      51        1930 :     plane.crash_state.is_crashed = false;
      52        1930 :     plane.crash_state.impact_detected = false;
      53             : 
      54             :     // reset external attitude guidance
      55        1930 :     plane.guided_state.last_forced_rpy_ms.zero();
      56        1930 :     plane.guided_state.last_forced_throttle_ms = 0;
      57             : 
      58             : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
      59        1930 :     plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
      60        1930 :     plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
      61        1930 :     plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
      62        1930 :     plane.guided_state.target_alt_time_ms = 0;
      63        1930 :     plane.guided_state.target_location.set_alt_cm(-1, Location::AltFrame::ABSOLUTE); 
      64             : #endif
      65             : 
      66             : #if AP_CAMERA_ENABLED
      67        1930 :     plane.camera.set_is_auto_mode(this == &plane.mode_auto);
      68             : #endif
      69             : 
      70             :     // zero initial pitch and highest airspeed on mode change
      71        1930 :     plane.auto_state.highest_airspeed = 0;
      72        1930 :     plane.auto_state.initial_pitch_cd = ahrs.pitch_sensor;
      73             : 
      74             :     // disable taildrag takeoff on mode change
      75        1930 :     plane.auto_state.fbwa_tdrag_takeoff_mode = false;
      76             : 
      77             :     // wipe the takeoff rotation complete state
      78        1930 :     plane.auto_state.rotation_complete = false;
      79             : 
      80             :     // start with previous WP at current location
      81        1930 :     plane.prev_WP_loc = plane.current_loc;
      82             : 
      83             :     // new mode means new loiter
      84        1930 :     plane.loiter.start_time_ms = 0;
      85             : 
      86             :     // record time of mode change
      87        1930 :     plane.last_mode_change_ms = AP_HAL::millis();
      88             : 
      89             :     // set VTOL auto state
      90        1930 :     plane.auto_state.vtol_mode = is_vtol_mode();
      91        1930 :     plane.auto_state.vtol_loiter = false;
      92             : 
      93             :     // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
      94        1930 :     plane.new_airspeed_cm = -1;
      95             :     
      96             :     // clear postponed long failsafe if mode change (from GCS) occurs before recall of long failsafe
      97        1930 :     plane.long_failsafe_pending = false;
      98             : 
      99             : #if HAL_QUADPLANE_ENABLED
     100        1930 :     quadplane.mode_enter();
     101             : #endif
     102             : 
     103             : #if AP_TERRAIN_AVAILABLE
     104        1930 :     plane.target_altitude.terrain_following_pending = false;
     105             : #endif
     106             : 
     107             : #if AP_PLANE_SYSTEMID_ENABLED
     108        1930 :     plane.g2.systemid.stop();
     109             : #endif
     110             : 
     111             :     // disable auto mode servo idle during altitude wait command
     112        1930 :     plane.auto_state.idle_mode = false;
     113             : 
     114        1930 :     bool enter_result = _enter();
     115             : 
     116        1930 :     if (enter_result) {
     117             :         // -------------------
     118             :         // these must be done AFTER _enter() because they use the results to set more flags
     119             : 
     120             :         // start with throttle suppressed in auto_throttle modes
     121        1929 :         plane.throttle_suppressed = does_auto_throttle();
     122             : #if HAL_ADSB_ENABLED
     123        1929 :         plane.adsb.set_is_auto_mode(does_auto_navigation());
     124             : #endif
     125             : 
     126             :         // set the nav controller stale AFTER _enter() so that we can check if we're currently in a loiter during the mode change
     127        1929 :         plane.nav_controller->set_data_is_stale();
     128             : 
     129             :         // reset steering integrator on mode change
     130        1929 :         plane.steerController.reset_I();
     131             : 
     132             :         // update RC failsafe, as mode change may have necessitated changing the failsafe throttle
     133        1929 :         plane.control_failsafe();
     134             : 
     135             : #if AP_FENCE_ENABLED
     136             :         // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
     137             :         // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
     138             :         // but it should be harmless to disable the fence temporarily in these situations as well
     139        1929 :         plane.fence.manual_recovery_start();
     140             : #endif
     141             :         //reset mission if in landing sequence, disarmed, not flying, and have changed to a non-autothrottle mode to clear prearm
     142        1929 :         if (plane.mission.get_in_landing_sequence_flag() &&
     143        1932 :             !plane.is_flying() && !plane.arming.is_armed_and_safety_off() &&
     144           3 :             !plane.control_mode->does_auto_navigation()) {
     145           3 :            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "In landing sequence: mission reset");
     146           3 :            plane.mission.reset();
     147             :         }
     148             : 
     149             :         // Make sure the flight stage is correct for the new mode
     150        1929 :         plane.update_flight_stage();
     151             :         
     152             :         // reset landing state
     153        1929 :         plane.landing.reset();
     154             : 
     155             : 
     156             : #if HAL_QUADPLANE_ENABLED
     157        1929 :         if (quadplane.enabled()) {
     158             :             float aspeed;
     159         929 :             bool have_airspeed = quadplane.ahrs.airspeed_EAS(aspeed);
     160         929 :             quadplane.assisted_flight = quadplane.assist.should_assist(aspeed, have_airspeed);
     161             :         }
     162             : 
     163        1929 :         if (is_vtol_mode() && !quadplane.tailsitter.enabled()) {
     164             :             // if flying inverted and entering a VTOL mode cancel
     165             :             // inverted flight
     166         143 :             plane.inverted_flight = false;
     167             :         }
     168             : #endif
     169             :     }
     170             : 
     171        1930 :     return enter_result;
     172             : }
     173             : 
     174     3715674 : bool Mode::is_vtol_man_throttle() const
     175             : {
     176             : #if HAL_QUADPLANE_ENABLED
     177     3720387 :     if (plane.quadplane.tailsitter.is_in_fw_flight() &&
     178        4713 :         plane.quadplane.assisted_flight) {
     179             :         // We are a tailsitter that has fully transitioned to Q-assisted forward flight.
     180             :         // In this case the forward throttle directly drives the vertical throttle so
     181             :         // set vertical throttle state to match the forward throttle state. Confusingly the booleans are inverted,
     182             :         // forward throttle uses 'does_auto_throttle' whereas vertical uses 'is_vtol_man_throttle'.
     183        4713 :         return !does_auto_throttle();
     184             :     }
     185             : #endif
     186     3710961 :     return false;
     187             : }
     188             : 
     189      527309 : void Mode::update_target_altitude()
     190             : {
     191      527309 :     Location target_location;
     192             : 
     193      527309 :     if (plane.landing.is_flaring()) {
     194             :         // during a landing flare, use TECS_LAND_SINK as a target sink
     195             :         // rate, and ignores the target altitude
     196       23257 :         plane.set_target_altitude_location(plane.next_WP_loc);
     197      504052 :     } else if (plane.landing.is_on_approach()) {
     198        6253 :         plane.landing.setup_landing_glide_slope(plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.target_altitude.offset_cm);
     199             : #if AP_RANGEFINDER_ENABLED
     200        6253 :         plane.landing.adjust_landing_slope_for_rangefinder_bump(plane.rangefinder_state, plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.auto_state.wp_distance, plane.target_altitude.offset_cm);
     201             : #endif
     202      497799 :     } else if (plane.landing.get_target_altitude_location(target_location)) {
     203         739 :         plane.set_target_altitude_location(target_location);
     204             : #if HAL_SOARING_ENABLED
     205      497060 :     } else if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.get_throttle_suppressed()) {
     206             :         // Reset target alt to current alt, to prevent large altitude errors when gliding.
     207        8191 :         plane.set_target_altitude_location(plane.current_loc);
     208        8191 :         plane.reset_offset_altitude();
     209             : #endif
     210      488869 :     } else if (plane.reached_loiter_target()) {
     211             :         // once we reach a loiter target then lock to the final
     212             :         // altitude target
     213      102007 :         plane.set_target_altitude_location(plane.next_WP_loc);
     214             : #if AP_TERRAIN_AVAILABLE
     215      445357 :     } else if (plane.next_WP_loc.terrain_alt &&
     216       58495 :                plane.set_target_altitude_proportion_terrain()) {
     217             :         // special case for target as terrain relative handled inside
     218             :         // set_target_altitude_proportion_terrain
     219             : #endif
     220      405823 :     } else if (plane.target_altitude.offset_cm != 0 &&
     221       77456 :                !plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc)) {
     222             :         // control climb/descent rate
     223       69082 :         plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion);
     224             : 
     225             :         // stay within the range of the start and end locations in altitude
     226       69082 :         plane.constrain_target_altitude_location(plane.next_WP_loc, plane.prev_WP_loc);
     227             :     } else {
     228      259285 :         plane.set_target_altitude_location(plane.next_WP_loc);
     229             :     }
     230      527309 : }
     231             : 
     232             : // returns true if the vehicle can be armed in this mode
     233       26163 : bool Mode::pre_arm_checks(size_t buflen, char *buffer) const
     234             : {
     235       26163 :     if (!_pre_arm_checks(buflen, buffer)) {
     236         466 :         if (strlen(buffer) == 0) {
     237             :             // If no message is provided add a generic one
     238         388 :             hal.util->snprintf(buffer, buflen, "mode not armable");
     239             :         }
     240         466 :         return false;
     241             :     }
     242             : 
     243       25697 :     return true;
     244             : }
     245             : 
     246             : // Auto and Guided do not call this to bypass the q-mode check.
     247       23885 : bool Mode::_pre_arm_checks(size_t buflen, char *buffer) const
     248             : {
     249             : #if HAL_QUADPLANE_ENABLED
     250       30569 :     if (plane.quadplane.enabled() && !is_vtol_mode() &&
     251        6684 :             plane.quadplane.option_is_set(QuadPlane::Option::ONLY_ARM_IN_QMODE_OR_AUTO)) {
     252           0 :         hal.util->snprintf(buffer, buflen, "not Q mode");
     253           0 :         return false;
     254             :     }
     255             : #endif
     256       23885 :     return true;
     257             : }
     258             : 
     259     5922671 : void Mode::run()
     260             : {
     261             :     // Direct stick mixing functionality has been removed, so as not to remove all stick mixing from the user completely
     262             :     // the old direct option is now used to enable fbw mixing, this is easier than doing a param conversion.
     263     5922671 :     switch ((StickMixing)plane.g.stick_mixing) {
     264     5922671 :         case StickMixing::FBW:
     265             :         case StickMixing::FBW_NO_PITCH:
     266             :         case StickMixing::DIRECT_REMOVED:
     267     5922671 :             plane.stabilize_stick_mixing_fbw();
     268     5922671 :             break;
     269           0 :         case StickMixing::NONE:
     270             :         case StickMixing::VTOL_YAW:
     271           0 :             break;
     272             :     }
     273     5922671 :     plane.stabilize_roll();
     274     5922671 :     plane.stabilize_pitch();
     275     5922671 :     plane.stabilize_yaw();
     276     5922671 : }
     277             : 
     278             : // Reset rate and steering controllers
     279      813554 : void Mode::reset_controllers()
     280             : {
     281             :     // reset integrators
     282      813554 :     plane.rollController.reset();
     283      813554 :     plane.pitchController.reset();
     284      813554 :     plane.yawController.reset_I();
     285             : 
     286             :     // reset steering controls
     287      813554 :     plane.steer_state.locked_course = false;
     288      813554 :     plane.steer_state.locked_course_err = 0;
     289             : 
     290             :     // reset TECS
     291      813554 :     plane.TECS_controller.reset();
     292      813554 : }
     293             : 
     294        6734 : bool Mode::is_taking_off() const
     295             : {
     296        6734 :     return (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF);
     297             : }
     298             : 
     299             : // Helper to output to both k_rudder and k_steering servo functions
     300     1766932 : void Mode::output_rudder_and_steering(float val)
     301             : {
     302     1766932 :     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val);
     303     1766932 :     SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val);
     304     1766932 : }
     305             : 
     306             : // Output pilot throttle, this is used in stabilized modes without auto throttle control
     307             : // Direct mapping if THR_PASS_STAB is set
     308             : // Otherwise apply curve for trim correction if configured
     309     1131208 : void Mode::output_pilot_throttle()
     310             : {
     311     1131208 :     if (plane.g.throttle_passthru_stabilize) {
     312             :         // THR_PASS_STAB set, direct mapping
     313           0 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
     314           0 :         return;
     315             :     }
     316             : 
     317             :     // get throttle, but adjust center to output TRIM_THROTTLE if flight option set
     318     1131208 :     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true));
     319             : }
     320             : 
     321             : // true if throttle min/max limits should be applied
     322     7763246 : bool Mode::use_throttle_limits() const
     323             : {
     324             : #if AP_SCRIPTING_ENABLED
     325     7763246 :     if (plane.nav_scripting_active()) {
     326        6657 :         return false;
     327             :     }
     328             : #endif
     329             : 
     330     7756589 :     if (this == &plane.mode_stabilize ||
     331     7753820 :         this == &plane.mode_training ||
     332     7751512 :         this == &plane.mode_acro ||
     333     6656889 :         this == &plane.mode_fbwa ||
     334             :         this == &plane.mode_autotune) {
     335             :         // a manual throttle mode
     336     1119445 :         return !plane.g.throttle_passthru_stabilize;
     337             :     }
     338             : 
     339     6637144 :     if (is_guided_mode() && plane.guided_throttle_passthru) {
     340             :         // manual pass through of throttle while in GUIDED
     341          16 :         return false;
     342             :     }
     343             : 
     344             : #if HAL_QUADPLANE_ENABLED
     345     6637128 :     if (quadplane.in_vtol_mode()) {
     346     2741745 :         return quadplane.allow_forward_throttle_in_vtol_mode();
     347             :     }
     348             : #endif
     349             : 
     350     3895383 :     return true;
     351             : }
     352             : 
     353             : // true if voltage correction should be applied to throttle
     354     7763246 : bool Mode::use_battery_compensation() const
     355             : {
     356             : #if AP_SCRIPTING_ENABLED
     357     7763246 :     if (plane.nav_scripting_active()) {
     358        6657 :         return false;
     359             :     }
     360             : #endif
     361             : 
     362     7756589 :     if (this == &plane.mode_stabilize ||
     363     7753820 :         this == &plane.mode_training ||
     364     7751512 :         this == &plane.mode_acro ||
     365     6656889 :         this == &plane.mode_fbwa ||
     366             :         this == &plane.mode_autotune) {
     367             :         // a manual throttle mode
     368     1119445 :         return false;
     369             :     }
     370             : 
     371     6637144 :     if (is_guided_mode() && plane.guided_throttle_passthru) {
     372             :         // manual pass through of throttle while in GUIDED
     373          16 :         return false;
     374             :     }
     375             : 
     376             : #if HAL_QUADPLANE_ENABLED
     377     6637128 :     if (quadplane.in_vtol_mode()) {
     378     2741745 :         return false;
     379             :     }
     380             : #endif
     381             : 
     382     3895383 :     return true;
     383             : }
     384             : 
     385             : #if AP_PLANE_SYSTEMID_ENABLED
     386             : // Return true if fixed wing system ID should be allowed
     387           0 : bool Mode::allow_fw_systemid() const {
     388             : 
     389           0 :     if (!supports_fw_systemid()) {
     390             :         // Mode does not support fw system ID
     391           0 :         return false;
     392             :     }
     393             : 
     394           0 :     if (is_taking_off() || is_landing()) {
     395             :         // Taking off or landing
     396           0 :         return false;
     397             :     }
     398             : 
     399             : #if HAL_QUADPLANE_ENABLED
     400           0 :     if (quadplane.available()) {
     401           0 :         if (quadplane.in_assisted_flight()) {
     402             :             // VTOL motors assisting
     403           0 :             return false;
     404             :         }
     405           0 :         if (!quadplane.transition->complete()) {
     406             :             // Still in transition
     407           0 :             return false;
     408             :         }
     409             :     }
     410             : #endif // HAL_QUADPLANE_ENABLED
     411             : 
     412           0 :     return true;
     413             : }
     414             : #endif // AP_PLANE_SYSTEMID_ENABLED