Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : #include "RC_Channel_Plane.h"
       4             : #include "qautotune.h"
       5             : 
       6             : // defining these two macros and including the RC_Channels_VarInfo
       7             : // header defines the parameter information common to all vehicle
       8             : // types
       9             : #define RC_CHANNELS_SUBCLASS RC_Channels_Plane
      10             : #define RC_CHANNEL_SUBCLASS RC_Channel_Plane
      11             : 
      12             : #include <RC_Channel/RC_Channels_VarInfo.h>
      13             : 
      14             : // note that this callback is not presently used on Plane:
      15      392073 : int8_t RC_Channels_Plane::flight_mode_channel_number() const
      16             : {
      17      392073 :     return plane.g.flight_mode_channel.get();
      18             : }
      19             : 
      20    32281030 : bool RC_Channels_Plane::in_rc_failsafe() const
      21             : {
      22    32281030 :     return (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe);
      23             : }
      24             : 
      25    30412576 : bool RC_Channels_Plane::has_valid_input() const
      26             : {
      27    30412576 :     if (in_rc_failsafe()) {
      28       45006 :         return false;
      29             :     }
      30    30367570 :     if (plane.failsafe.throttle_counter != 0) {
      31        1379 :         return false;
      32             :     }
      33    30366191 :     return RC_Channels::has_valid_input();
      34             : }
      35             : 
      36     2764019 : RC_Channel * RC_Channels_Plane::get_arming_channel(void) const
      37             : {
      38     2764019 :     return plane.channel_rudder;
      39             : }
      40             : 
      41           9 : void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
      42             :                                                    const AuxSwitchPos ch_flag)
      43             : {
      44           9 :     switch(ch_flag) {
      45           4 :     case AuxSwitchPos::HIGH: {
      46             :         // engage mode (if not possible we remain in current flight mode)
      47           4 :         plane.set_mode_by_number(number, ModeReason::AUX_FUNCTION);
      48           4 :         break;
      49             :     }
      50           5 :     default:
      51             :         // return to flight mode switch's flight mode if we are currently
      52             :         // in this mode
      53           5 :         if (plane.control_mode->mode_number() == number) {
      54           2 :             rc().reset_mode_switch();
      55             :         }
      56             :     }
      57           9 : }
      58             : 
      59             : #if HAL_QUADPLANE_ENABLED
      60           0 : void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
      61             : {
      62           0 :     switch(ch_flag) {
      63           0 :         case AuxSwitchPos::HIGH:
      64           0 :             gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Force enabled");
      65           0 :             plane.quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
      66           0 :             break;
      67             : 
      68           0 :         case AuxSwitchPos::MIDDLE:
      69           0 :             gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled");
      70           0 :             plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_ENABLED);
      71           0 :             break;
      72             : 
      73           0 :         case AuxSwitchPos::LOW:
      74           0 :             gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Disabled");
      75           0 :             plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_DISABLED);
      76           0 :             break;
      77             :     }
      78           0 : }
      79             : #endif  // HAL_QUADPLANE_ENABLED
      80             : 
      81           0 : void RC_Channel_Plane::do_aux_function_crow_mode(AuxSwitchPos ch_flag)
      82             : {
      83           0 :         switch(ch_flag) {
      84           0 :         case AuxSwitchPos::HIGH:
      85           0 :             plane.crow_mode = Plane::CrowMode::CROW_DISABLED;
      86           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Crow Flaps Disabled");
      87           0 :             break;
      88           0 :         case AuxSwitchPos::MIDDLE:
      89           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Progressive Crow Flaps"); 
      90           0 :             plane.crow_mode = Plane::CrowMode::PROGRESSIVE;   
      91           0 :             break;
      92           0 :         case AuxSwitchPos::LOW:
      93           0 :             plane.crow_mode = Plane::CrowMode::NORMAL;
      94           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Normal Crow Flaps");
      95           0 :             break;
      96             :         }    
      97           0 : }
      98             : 
      99             : #if HAL_SOARING_ENABLED
     100          17 : void RC_Channel_Plane::do_aux_function_soaring_3pos(AuxSwitchPos ch_flag)
     101             : {
     102          17 :     SoaringController::ActiveStatus desired_state = SoaringController::ActiveStatus::SOARING_DISABLED;
     103             : 
     104          17 :     switch (ch_flag) {
     105           8 :         case AuxSwitchPos::LOW:
     106           8 :             desired_state = SoaringController::ActiveStatus::SOARING_DISABLED;
     107           8 :             break;
     108           8 :         case AuxSwitchPos::MIDDLE:
     109           8 :             desired_state = SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;
     110           8 :             break;
     111           1 :         case AuxSwitchPos::HIGH:
     112           1 :             desired_state = SoaringController::ActiveStatus::AUTO_MODE_CHANGE;
     113           1 :             break;
     114             :         }
     115             : 
     116          17 :     plane.g2.soaring_controller.set_pilot_desired_state(desired_state);
     117          17 : }
     118             : #endif
     119             : 
     120           0 : void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag)
     121             : {
     122           0 :         switch(ch_flag) {
     123           0 :         case AuxSwitchPos::HIGH:
     124           0 :             plane.flare_mode = Plane::FlareMode::ENABLED_PITCH_TARGET;
     125           0 :             break;
     126           0 :         case AuxSwitchPos::MIDDLE:
     127           0 :             plane.flare_mode = Plane::FlareMode::ENABLED_NO_PITCH_TARGET;
     128           0 :             break;
     129           0 :         case AuxSwitchPos::LOW:
     130           0 :             plane.flare_mode = Plane::FlareMode::FLARE_DISABLED;
     131           0 :             break;
     132             :         }    
     133           0 : }
     134             : 
     135             : 
     136       11072 : void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
     137             :                                          const RC_Channel::AuxSwitchPos ch_flag)
     138             : {
     139       11072 :     switch(ch_option) {
     140             :     // the following functions do not need to be initialised:
     141           2 :     case AUX_FUNC::AUTO:
     142             :     case AUX_FUNC::CIRCLE:
     143             :     case AUX_FUNC::ACRO:
     144             :     case AUX_FUNC::TRAINING:
     145             :     case AUX_FUNC::FLAP:
     146             :     case AUX_FUNC::GUIDED:
     147             :     case AUX_FUNC::INVERTED:
     148             :     case AUX_FUNC::LOITER:
     149             :     case AUX_FUNC::MANUAL:
     150             :     case AUX_FUNC::RTL:
     151             :     case AUX_FUNC::TAKEOFF:
     152             :     case AUX_FUNC::FBWA:
     153             :     case AUX_FUNC::AIRBRAKE:
     154             : #if HAL_QUADPLANE_ENABLED
     155             :     case AUX_FUNC::QRTL:
     156             :     case AUX_FUNC::QSTABILIZE:
     157             : #endif
     158             :     case AUX_FUNC::FBWA_TAILDRAGGER:
     159             :     case AUX_FUNC::FWD_THR:
     160             :     case AUX_FUNC::LANDING_FLARE:
     161             : #if HAL_PARACHUTE_ENABLED
     162             :     case AUX_FUNC::PARACHUTE_RELEASE:
     163             : #endif
     164             :     case AUX_FUNC::MODE_SWITCH_RESET:
     165             :     case AUX_FUNC::CRUISE:
     166             : #if HAL_QUADPLANE_ENABLED
     167             :     case AUX_FUNC::ARMDISARM_AIRMODE:
     168             : #endif
     169             :     case AUX_FUNC::PLANE_AUTO_LANDING_ABORT:
     170             :     case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC:
     171             :     case AUX_FUNC::EMERGENCY_LANDING_EN:
     172             :     case AUX_FUNC::FW_AUTOTUNE:
     173             :     case AUX_FUNC::VFWD_THR_OVERRIDE:
     174             :     case AUX_FUNC::PRECISION_LOITER:
     175             : #if QAUTOTUNE_ENABLED
     176             :     case AUX_FUNC::AUTOTUNE_TEST_GAINS:
     177             : #endif
     178             : #if AP_QUICKTUNE_ENABLED
     179             :     case AUX_FUNC::QUICKTUNE:
     180             : #endif
     181             : #if MODE_AUTOLAND_ENABLED
     182             :     case AUX_FUNC::AUTOLAND:
     183             : #endif
     184             : #if AP_PLANE_SYSTEMID_ENABLED
     185             :     case AUX_FUNC::SYSTEMID:
     186             : #endif
     187           2 :         break;
     188          14 :     case AUX_FUNC::SOARING:
     189             : #if HAL_QUADPLANE_ENABLED
     190             :     case AUX_FUNC::Q_ASSIST:
     191             :     case AUX_FUNC::AIRMODE:
     192             :     case AUX_FUNC::WEATHER_VANE_ENABLE:
     193             : #endif
     194             : #if AP_AIRSPEED_AUTOCAL_ENABLE
     195             :     case AUX_FUNC::ARSPD_CALIBRATE:
     196             : #endif
     197             :     case AUX_FUNC::TER_DISABLE:
     198             :     case AUX_FUNC::CROW_SELECT:
     199             : #if AP_ICENGINE_ENABLED
     200             :     case AUX_FUNC::ICE_START_STOP:
     201             : #endif
     202          14 :         run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT, ch_in);
     203          14 :         break;
     204             : 
     205           0 :     case AUX_FUNC::REVERSE_THROTTLE:
     206           0 :         plane.have_reverse_throttle_rc_option = true;
     207             :         // setup input throttle as a range. This is needed as init_aux_function is called
     208             :         // after set_control_channels()
     209           0 :         if (plane.channel_throttle) {
     210           0 :             plane.channel_throttle->set_range(100);
     211             :         }
     212             :         // note that we don't call do_aux_function() here as we don't
     213             :         // want to startup with reverse thrust
     214           0 :         break;
     215             : 
     216       11056 :     default:
     217             :         // handle in parent class
     218       11056 :         RC_Channel::init_aux_function(ch_option, ch_flag);
     219       11056 :         break;
     220             :     }
     221       11072 : }
     222             : 
     223             : // do_aux_function - implement the function invoked by auxiliary switches
     224         207 : bool RC_Channel_Plane::do_aux_function(const AuxFuncTrigger &trigger)
     225             : {
     226         207 :     const AUX_FUNC &ch_option = trigger.func;
     227         207 :     const AuxSwitchPos &ch_flag = trigger.pos;
     228             : 
     229         207 :     switch(ch_option) {
     230           2 :     case AUX_FUNC::INVERTED:
     231           2 :         plane.inverted_flight = (ch_flag == AuxSwitchPos::HIGH);
     232           2 :         break;
     233             : 
     234           4 :     case AUX_FUNC::REVERSE_THROTTLE:
     235           4 :         plane.reversed_throttle = (ch_flag == AuxSwitchPos::HIGH);
     236           4 :         gcs().send_text(MAV_SEVERITY_INFO, "RevThrottle: %s", plane.reversed_throttle?"ENABLE":"DISABLE");
     237           4 :         break;
     238             : 
     239           0 :     case AUX_FUNC::AUTO:
     240           0 :         do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
     241           0 :         break;
     242             : 
     243           0 :     case AUX_FUNC::CIRCLE:
     244           0 :         do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
     245           0 :         break;
     246             : 
     247           0 :     case AUX_FUNC::ACRO:
     248           0 :         do_aux_function_change_mode(Mode::Number::ACRO, ch_flag);
     249           0 :         break;
     250             : 
     251           0 :     case AUX_FUNC::TRAINING:
     252           0 :         do_aux_function_change_mode(Mode::Number::TRAINING, ch_flag);
     253           0 :         break;
     254             :         
     255           0 :     case AUX_FUNC::LOITER:
     256           0 :         do_aux_function_change_mode(Mode::Number::LOITER, ch_flag);
     257           0 :         break;        
     258             : 
     259           5 :     case AUX_FUNC::GUIDED:
     260           5 :         do_aux_function_change_mode(Mode::Number::GUIDED, ch_flag);
     261           5 :         break;
     262             : 
     263           0 :     case AUX_FUNC::MANUAL:
     264           0 :         do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag);
     265           0 :         break;
     266             : 
     267           4 :     case AUX_FUNC::RTL:
     268           4 :         do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
     269           4 :         break;
     270             : 
     271           0 :     case AUX_FUNC::TAKEOFF:
     272           0 :         do_aux_function_change_mode(Mode::Number::TAKEOFF, ch_flag);
     273           0 :         break;
     274             : 
     275           0 :     case AUX_FUNC::FBWA:
     276           0 :         do_aux_function_change_mode(Mode::Number::FLY_BY_WIRE_A, ch_flag);
     277           0 :         break;
     278             : 
     279             : #if HAL_QUADPLANE_ENABLED
     280           0 :     case AUX_FUNC::QRTL:
     281           0 :         do_aux_function_change_mode(Mode::Number::QRTL, ch_flag);
     282           0 :         break;
     283             : 
     284           0 :     case AUX_FUNC::QSTABILIZE:
     285           0 :         do_aux_function_change_mode(Mode::Number::QSTABILIZE, ch_flag);
     286           0 :         break;
     287             : 
     288           0 :     case AUX_FUNC::VFWD_THR_OVERRIDE: {
     289           0 :         const bool enable = (ch_flag == AuxSwitchPos::HIGH);
     290           0 :         if (enable != plane.quadplane.vfwd_enable_active) {
     291           0 :             plane.quadplane.vfwd_enable_active = enable;
     292           0 :             gcs().send_text(MAV_SEVERITY_INFO, "QFwdThr: %s", enable?"ON":"OFF");
     293             :         }
     294           0 :         break;
     295             :     }
     296             : #endif
     297             : 
     298             : #if HAL_SOARING_ENABLED
     299          17 :     case AUX_FUNC::SOARING:
     300          17 :         do_aux_function_soaring_3pos(ch_flag);
     301          17 :         break;
     302             : #endif
     303             : 
     304           5 :     case AUX_FUNC::FLAP:
     305             :     case AUX_FUNC::FBWA_TAILDRAGGER:
     306             :     case AUX_FUNC::AIRBRAKE:
     307           5 :         break; // input labels, nothing to do
     308             : 
     309             : #if HAL_QUADPLANE_ENABLED
     310           0 :     case AUX_FUNC::Q_ASSIST:
     311           0 :         do_aux_function_q_assist_state(ch_flag);
     312           0 :         break;
     313             : #endif
     314             : 
     315           0 :     case AUX_FUNC::FWD_THR:
     316           0 :         break; // VTOL forward throttle input label, nothing to do
     317             : 
     318           0 :     case AUX_FUNC::TER_DISABLE:
     319           0 :             switch (ch_flag) {
     320           0 :             case AuxSwitchPos::HIGH:
     321           0 :                 plane.non_auto_terrain_disable = true;
     322           0 :                 if (plane.control_mode->allows_terrain_disable()) {
     323           0 :                     plane.set_target_altitude_current();
     324             :                 }
     325           0 :                 break;
     326           0 :             case AuxSwitchPos::MIDDLE:
     327           0 :                 break;
     328           0 :             case AuxSwitchPos::LOW:
     329           0 :                 plane.non_auto_terrain_disable = false;
     330           0 :                 if (plane.control_mode->allows_terrain_disable()) {
     331           0 :                     plane.set_target_altitude_current();
     332             :                 }
     333           0 :                 break;
     334             :             }
     335           0 :             gcs().send_text(MAV_SEVERITY_INFO, "NON AUTO TERRN: %s", plane.non_auto_terrain_disable?"OFF":"ON");
     336           0 :         break;
     337             : 
     338           0 :     case AUX_FUNC::CROW_SELECT:
     339           0 :         do_aux_function_crow_mode(ch_flag);
     340           0 :         break;
     341             : 
     342             : #if HAL_QUADPLANE_ENABLED
     343           6 :     case AUX_FUNC::AIRMODE:
     344           6 :         switch (ch_flag) {
     345           3 :         case AuxSwitchPos::HIGH:
     346           3 :             plane.quadplane.air_mode = AirMode::ON;
     347           3 :             plane.quadplane.throttle_wait = false;
     348           3 :             break;
     349           1 :         case AuxSwitchPos::MIDDLE:
     350           1 :             break;
     351           2 :         case AuxSwitchPos::LOW:
     352           2 :             plane.quadplane.air_mode = AirMode::OFF;
     353           2 :             break;
     354             :         }
     355           6 :         break;
     356             : #endif
     357             : 
     358             : #if AP_AIRSPEED_AUTOCAL_ENABLE
     359           0 :     case AUX_FUNC::ARSPD_CALIBRATE:
     360           0 :         switch (ch_flag) {
     361           0 :         case AuxSwitchPos::HIGH:
     362           0 :             plane.airspeed.set_calibration_enabled(true);
     363           0 :             break;
     364           0 :         case AuxSwitchPos::MIDDLE:
     365           0 :             break;
     366           0 :         case AuxSwitchPos::LOW:
     367           0 :             plane.airspeed.set_calibration_enabled(false);
     368           0 :             break;
     369             :         }
     370           0 :         break;
     371             : #endif
     372             : 
     373           0 :     case AUX_FUNC::LANDING_FLARE:
     374           0 :         do_aux_function_flare(ch_flag);
     375           0 :         break;
     376             : 
     377             : #if HAL_PARACHUTE_ENABLED
     378           0 :     case AUX_FUNC::PARACHUTE_RELEASE:
     379           0 :         if (ch_flag == AuxSwitchPos::HIGH) {
     380           0 :             plane.parachute_manual_release();
     381             :         }
     382           0 :         break;
     383             : #endif
     384             : 
     385           2 :     case AUX_FUNC::MODE_SWITCH_RESET:
     386           2 :         rc().reset_mode_switch();
     387           2 :         break;
     388             : 
     389           0 :     case AUX_FUNC::CRUISE:
     390           0 :         do_aux_function_change_mode(Mode::Number::CRUISE, ch_flag);
     391           0 :         break;
     392             : 
     393             : #if HAL_QUADPLANE_ENABLED
     394          32 :     case AUX_FUNC::ARMDISARM_AIRMODE:
     395          32 :         RC_Channel::do_aux_function_armdisarm(ch_flag);
     396          32 :         if (plane.arming.is_armed()) {
     397          16 :             plane.quadplane.air_mode = AirMode::ON;
     398          16 :             plane.quadplane.throttle_wait = false;
     399             :         }
     400          32 :         break;
     401             : 
     402           0 :     case AUX_FUNC::WEATHER_VANE_ENABLE: {
     403           0 :         if (plane.quadplane.weathervane != nullptr) {
     404           0 :             switch (ch_flag) {
     405           0 :                 case AuxSwitchPos::HIGH:
     406           0 :                     plane.quadplane.weathervane->allow_weathervaning(true);
     407           0 :                     break;
     408           0 :                 case AuxSwitchPos::MIDDLE:
     409             :                     // nothing
     410           0 :                     break;
     411           0 :                 case AuxSwitchPos::LOW:
     412           0 :                     plane.quadplane.weathervane->allow_weathervaning(false);
     413           0 :                     break;
     414             :             }
     415             :         }
     416           0 :         break;
     417             :     }
     418             : #endif
     419             : 
     420           0 :     case AUX_FUNC::PLANE_AUTO_LANDING_ABORT:
     421           0 :         switch(ch_flag) {
     422           0 :         case AuxSwitchPos::HIGH:
     423           0 :             IGNORE_RETURN(plane.trigger_land_abort(0));
     424           0 :             break;
     425           0 :         case AuxSwitchPos::MIDDLE:
     426             :         case AuxSwitchPos::LOW:
     427           0 :             break;
     428             :         }
     429           0 :         break;
     430             : 
     431           0 :     case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC:
     432           0 :         if (ch_flag == AuxSwitchPos::HIGH) {
     433           0 :             plane.trim_radio();
     434             :         }
     435           0 :         break;
     436             : 
     437           0 :     case AUX_FUNC::EMERGENCY_LANDING_EN:
     438           0 :         switch (ch_flag) {
     439           0 :         case AuxSwitchPos::HIGH:
     440           0 :             plane.emergency_landing = true;
     441           0 :             break;
     442           0 :         case AuxSwitchPos::MIDDLE:
     443           0 :             break;
     444           0 :         case AuxSwitchPos::LOW:
     445           0 :             plane.emergency_landing = false;
     446           0 :             break;
     447             :         }
     448           0 :         break;
     449             : 
     450           0 :     case AUX_FUNC::FW_AUTOTUNE:
     451           0 :         if (ch_flag == AuxSwitchPos::HIGH && plane.control_mode->mode_allows_autotuning()) {
     452           0 :            plane.autotune_enable(true);
     453           0 :         } else if (ch_flag == AuxSwitchPos::HIGH) {
     454           0 :            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Autotuning not allowed in this mode!");
     455             :         } else {
     456           0 :            plane.autotune_enable(false); 
     457             :         }
     458           0 :         break;
     459             : 
     460           0 :     case AUX_FUNC::PRECISION_LOITER:
     461             :         // handled by lua scripting, just ignore here
     462           0 :         break;
     463             : 
     464             : #if QAUTOTUNE_ENABLED
     465           0 :     case AUX_FUNC::AUTOTUNE_TEST_GAINS:
     466           0 :         plane.quadplane.qautotune.do_aux_function(ch_flag);
     467           0 :         break;
     468             : #endif
     469             : 
     470             : #if AP_QUICKTUNE_ENABLED
     471           5 :     case AUX_FUNC::QUICKTUNE:
     472           5 :         plane.quicktune.update_switch_pos(ch_flag);
     473           5 :         break;
     474             : #endif
     475             : 
     476             : #if AP_PLANE_SYSTEMID_ENABLED
     477           0 :     case AUX_FUNC::SYSTEMID:
     478           0 :         if (ch_flag == AuxSwitchPos::HIGH) {
     479           0 :             plane.g2.systemid.start();
     480           0 :         } else if (ch_flag == AuxSwitchPos::LOW) {
     481           0 :             plane.g2.systemid.stop();
     482             :         }
     483           0 :         break;
     484             : #endif
     485             : 
     486             : #if AP_ICENGINE_ENABLED
     487          28 :     case AUX_FUNC::ICE_START_STOP:
     488          28 :         plane.g2.ice_control.do_aux_function(trigger);
     489          28 :         break;
     490             : #endif
     491             : 
     492             : #if MODE_AUTOLAND_ENABLED
     493           0 :     case AUX_FUNC::AUTOLAND:
     494           0 :         do_aux_function_change_mode(Mode::Number::AUTOLAND, ch_flag);
     495           0 :         break;
     496             : 
     497             : #endif
     498             : 
     499          97 :     default:
     500          97 :         return RC_Channel::do_aux_function(trigger);
     501             :     }
     502             : 
     503         110 :     return true;
     504             : }