ArduCopter/mode_autorotate.cpp

   1 /* -----------------------------------------------------------------------------------------
   2     This is currently a SITL only function until the project is complete.
   3     To trial this in SITL you will need to use Real Flight 8.
   4     Instructions for how to set this up in SITL can be found here:
   5     https://discuss.ardupilot.org/t/autonomous-autorotation-gsoc-project-blog/42139
   6  -----------------------------------------------------------------------------------------*/
   7
   8 #include "Copter.h"
   9 #include <AC_Autorotation/AC_Autorotation.h>
  10 #include "mode.h"
  11
  12 #if MODE_AUTOROTATE_ENABLED
  13
  14 bool ModeAutorotate::init(bool ignore_checks)
  15 {
  16 #if FRAME_CONFIG != HELI_FRAME
  17     // Only allow trad heli to use autorotation mode
  18     return false;
  19 #endif
  20
  21     // Check that mode is enabled, make sure this is the first check as this is the most
  22     // important thing for users to fix if they are planning to use autorotation mode
  23     if (!g2.arot.enabled()) {
  24         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: Mode not enabled");
  25         return false;
  26     }
  27
  28     // Must be armed to use mode, prevent triggering state machine on the ground
  29     if (!motors->armed() || copter.ap.land_complete || copter.ap.land_complete_maybe) {
  30         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: Must be armed and flying");
  31         return false;
  32     }
  33
  34     // Initialise controller
  35     g2.arot.init();
  36
  37     // Setting default starting state
  38     current_phase = Phase::ENTRY_INIT;
  39
  40     // Set entry timer
  41     _entry_time_start_ms = millis();
  42
  43     // reset logging timer
  44     _last_logged_ms = 0;
  45
  46     return true;
  47 }
  48
  49 void ModeAutorotate::run()
  50 {
  51     // Current time
  52     const uint32_t now_ms = millis();
  53
  54     // Set dt in library
  55     float const last_loop_time_s = AP::scheduler().get_last_loop_time_s();
  56     g2.arot.set_dt(last_loop_time_s);
  57
  58     //----------------------------------------------------------------
  59     //                  State machine logic
  60     //----------------------------------------------------------------
  61     // State machine progresses through the autorotation phases as you read down through the if statements.
  62     // More urgent phases (the ones closer to the ground) take precedence later in the if statements.
  63
  64     if (current_phase < Phase::GLIDE_INIT && ((now_ms - _entry_time_start_ms) > g2.arot.entry_time_ms)) {
  65         // Flight phase can be progressed to steady state glide
  66         current_phase = Phase::GLIDE_INIT;
  67     }
  68
  69     // Check if we believe we have landed. We need the landed state to zero all
  70     // controls and make sure that the copter landing detector will trip
  71     if (current_phase < Phase::LANDED && g2.arot.check_landed()) {
  72         current_phase = Phase::LANDED_INIT;
  73     }
  74
  75     // Check if we are bailing out and need to re-set the spool state
  76     if (motors->autorotation_bailout()) {
  77         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
  78     }
  79
  80     // Get norm input from yaw channel
  81     const float pilot_norm_input = channel_yaw->norm_input_dz();
  82
  83     //----------------------------------------------------------------
  84     //                  State machine actions
  85     //----------------------------------------------------------------
  86     switch (current_phase) {
  87
  88         case Phase::ENTRY_INIT:
  89             // Entry phase functions to be run only once
  90             g2.arot.init_entry();
  91             current_phase = Phase::ENTRY;
  92             FALLTHROUGH;
  93
  94         case Phase::ENTRY:
  95             // Smoothly transition the collective to enter autorotation regime and begin forward speed control
  96             g2.arot.run_entry(pilot_norm_input);
  97             break;
  98
  99         case Phase::GLIDE_INIT:
 100             // Glide phase functions to be run only once
 101             g2.arot.init_glide();
 102             current_phase = Phase::GLIDE;
 103             FALLTHROUGH;
 104
 105         case Phase::GLIDE:
 106             // Maintain head speed and forward speed as we glide to the ground
 107             g2.arot.run_glide(pilot_norm_input);
 108             break;
 109
 110         case Phase::FLARE_INIT:
 111         case Phase::FLARE:
 112         case Phase::TOUCH_DOWN_INIT:
 113         case Phase::TOUCH_DOWN:
 114             break;
 115
 116         case Phase::LANDED_INIT:
 117             // Landed phase functions to be run only once
 118             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AROT: Landed");
 119             current_phase = Phase::LANDED;
 120             FALLTHROUGH;
 121
 122         case Phase::LANDED:
 123             // Don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly
 124             g2.arot.run_landed();
 125             break;
 126     }
 127
 128     // Slow rate (25 Hz) logging for the mode
 129     if (now_ms - _last_logged_ms > 40U) {
 130         g2.arot.log_write_autorotation();
 131         _last_logged_ms = now_ms;
 132     }
 133
 134 } // End function run()
 135
 136 #endif