Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : #include "quadplane.h"
       4             : #include "qautotune.h"
       5             : 
       6        2550 : Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
       7             : {
       8        2550 :     Mode *ret = nullptr;
       9        2550 :     switch (num) {
      10        1533 :     case Mode::Number::MANUAL:
      11        1533 :         ret = &mode_manual;
      12        1533 :         break;
      13          11 :     case Mode::Number::CIRCLE:
      14          11 :         ret = &mode_circle;
      15          11 :         break;
      16           3 :     case Mode::Number::STABILIZE:
      17           3 :         ret = &mode_stabilize;
      18           3 :         break;
      19           1 :     case Mode::Number::TRAINING:
      20           1 :         ret = &mode_training;
      21           1 :         break;
      22           4 :     case Mode::Number::ACRO:
      23           4 :         ret = &mode_acro;
      24           4 :         break;
      25         417 :     case Mode::Number::FLY_BY_WIRE_A:
      26         417 :         ret = &mode_fbwa;
      27         417 :         break;
      28           6 :     case Mode::Number::FLY_BY_WIRE_B:
      29           6 :         ret = &mode_fbwb;
      30           6 :         break;
      31          68 :     case Mode::Number::CRUISE:
      32          68 :         ret = &mode_cruise;
      33          68 :         break;
      34           3 :     case Mode::Number::AUTOTUNE:
      35           3 :         ret = &mode_autotune;
      36           3 :         break;
      37         197 :     case Mode::Number::AUTO:
      38         197 :         ret = &mode_auto;
      39         197 :         break;
      40          21 :     case Mode::Number::RTL:
      41          21 :         ret = &mode_rtl;
      42          21 :         break;
      43          35 :     case Mode::Number::LOITER:
      44          35 :         ret = &mode_loiter;
      45          35 :         break;
      46           1 :     case Mode::Number::AVOID_ADSB:
      47             : #if HAL_ADSB_ENABLED
      48           1 :         ret = &mode_avoidADSB;
      49           1 :         break;
      50             : #endif
      51             :     // if ADSB is not compiled in then fallthrough to guided
      52          49 :     case Mode::Number::GUIDED:
      53          49 :         ret = &mode_guided;
      54          49 :         break;
      55           0 :     case Mode::Number::INITIALISING:
      56           0 :         ret = &mode_initializing;
      57           0 :         break;
      58             : #if HAL_QUADPLANE_ENABLED
      59           4 :     case Mode::Number::QSTABILIZE:
      60           4 :         ret = &mode_qstabilize;
      61           4 :         break;
      62          26 :     case Mode::Number::QHOVER:
      63          26 :         ret = &mode_qhover;
      64          26 :         break;
      65          78 :     case Mode::Number::QLOITER:
      66          78 :         ret = &mode_qloiter;
      67          78 :         break;
      68          15 :     case Mode::Number::QLAND:
      69          15 :         ret = &mode_qland;
      70          15 :         break;
      71          17 :     case Mode::Number::QRTL:
      72          17 :         ret = &mode_qrtl;
      73          17 :         break;
      74           3 :     case Mode::Number::QACRO:
      75           3 :         ret = &mode_qacro;
      76           3 :         break;
      77             : #if QAUTOTUNE_ENABLED
      78           1 :     case Mode::Number::QAUTOTUNE:
      79           1 :         ret = &mode_qautotune;
      80           1 :         break;
      81             : #endif
      82             : #endif  // HAL_QUADPLANE_ENABLED
      83          53 :     case Mode::Number::TAKEOFF:
      84          53 :         ret = &mode_takeoff;
      85          53 :         break;
      86             : #if MODE_AUTOLAND_ENABLED
      87           3 :     case Mode::Number::AUTOLAND:
      88           3 :         ret = &mode_autoland;
      89           3 :         break;
      90             : #endif //MODE_AUTOLAND_ENABLED
      91           1 :     case Mode::Number::THERMAL:
      92             : #if HAL_SOARING_ENABLED
      93           1 :         ret = &mode_thermal;
      94             : #endif
      95           1 :         break;
      96             : #if HAL_QUADPLANE_ENABLED
      97           0 :     case Mode::Number::LOITER_ALT_QLAND:
      98           0 :         ret = &mode_loiter_qland;
      99           0 :         break;
     100             : #endif  // HAL_QUADPLANE_ENABLED
     101             : 
     102             :     }
     103        2550 :     return ret;
     104             : }
     105             : 
     106      390652 : void RC_Channels_Plane::read_mode_switch()
     107             : {
     108      390652 :     if (millis() - plane.failsafe.last_valid_rc_ms > 100) {
     109             :         // only use signals that are less than 0.1s old.
     110         289 :         return;
     111             :     }
     112      390363 :     RC_Channels::read_mode_switch();
     113             : }
     114             : 
     115         700 : void RC_Channel_Plane::mode_switch_changed(modeswitch_pos_t new_pos)
     116             : {
     117         700 :     if (new_pos < 0 || (uint8_t)new_pos >= ARRAY_SIZE(plane.g.flight_modes)) {
     118             :         // should not have been called
     119           0 :         return;
     120             :     }
     121             : 
     122         700 :     plane.set_mode_by_number((Mode::Number)plane.g.flight_modes[new_pos].get(), ModeReason::RC_COMMAND);
     123             : }
     124             : 
     125             : /*
     126             :   called when entering autotune
     127             :  */
     128           4 : void Plane::autotune_start(void)
     129             : {
     130           4 :     const bool tune_roll = g2.axis_bitmask.get() & int8_t(AutoTuneAxis::ROLL);
     131           4 :     const bool tune_pitch = g2.axis_bitmask.get() & int8_t(AutoTuneAxis::PITCH);
     132           4 :     const bool tune_yaw = g2.axis_bitmask.get() & int8_t(AutoTuneAxis::YAW);
     133           4 :     if (tune_roll || tune_pitch || tune_yaw) {
     134           4 :         gcs().send_text(MAV_SEVERITY_INFO, "Started autotune");
     135           4 :         if (tune_roll) { 
     136           4 :             rollController.autotune_start();
     137             :         }
     138           4 :         if (tune_pitch) { 
     139           4 :             pitchController.autotune_start();
     140             :         }
     141           4 :         if (tune_yaw) { 
     142           4 :             yawController.autotune_start();
     143             :         }
     144           4 :         autotuning = true;
     145           4 :         gcs().send_text(MAV_SEVERITY_INFO, "Autotuning %s%s%s", tune_roll?"roll ":"", tune_pitch?"pitch ":"", tune_yaw?"yaw":"");
     146             :     } else {
     147           0 :         gcs().send_text(MAV_SEVERITY_INFO, "No axis selected for tuning!");
     148             :     }        
     149           4 : }
     150             : 
     151             : /*
     152             :   called when exiting autotune
     153             :  */
     154        1924 : void Plane::autotune_restore(void)
     155             : {
     156        1924 :     rollController.autotune_restore();
     157        1924 :     pitchController.autotune_restore();
     158        1924 :     yawController.autotune_restore();
     159        1924 :     if (autotuning) {
     160           4 :         autotuning = false;
     161           4 :         gcs().send_text(MAV_SEVERITY_INFO, "Stopped autotune");
     162             :     }
     163        1924 : }
     164             : 
     165             : /*
     166             :   enable/disable autotune for AUTO modes
     167             :  */
     168           2 : void Plane::autotune_enable(bool enable)
     169             : {
     170           2 :     if (enable) {
     171           1 :         autotune_start();
     172             :     } else {
     173           1 :         autotune_restore();
     174             :     }
     175           2 : }
     176             : 
     177             : /*
     178             :   are we flying inverted?
     179             :  */
     180    20050885 : bool Plane::fly_inverted(void)
     181             : {
     182    20050885 :     if (control_mode == &plane.mode_manual) {
     183          17 :         return false;
     184             :     }
     185    20050868 :     if (inverted_flight) {
     186             :         // controlled with aux switch
     187       12781 :         return true;
     188             :     }
     189    20038087 :     if (control_mode == &mode_auto && auto_state.inverted_flight) {
     190        2625 :         return true;
     191             :     }
     192    20035462 :     return false;
     193             : }