Line data    Source code

       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.club/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           1 : 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           1 :     if (!g2.arot.enabled()) {
      24           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: Mode not enabled");
      25           0 :         return false;
      26             :     }
      27             : 
      28             :     // Must be armed to use mode, prevent triggering state machine on the ground
      29           1 :     if (!motors->armed() || copter.ap.land_complete || copter.ap.land_complete_maybe) {
      30           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AROT: Must be armed and flying");
      31           0 :         return false;
      32             :     }
      33             : 
      34             :     // Initialise controller
      35           1 :     g2.arot.init();
      36             : 
      37             :     // Setting default starting state
      38           1 :     current_phase = Phase::ENTRY_INIT;
      39             : 
      40             :     // Set entry timer
      41           1 :     _entry_time_start_ms = millis();
      42             : 
      43             :     // reset logging timer
      44           1 :     _last_logged_ms = 0;
      45             : 
      46           1 :     return true;
      47             : }
      48             : 
      49       14401 : void ModeAutorotate::run()
      50             : {
      51             :     // Current time
      52       14401 :     const uint32_t now_ms = millis();
      53             : 
      54             :     // Set dt in library
      55       14401 :     float const last_loop_time_s = AP::scheduler().get_last_loop_time_s();
      56       14401 :     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       14401 :     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           1 :         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       14401 :     if (current_phase < Phase::LANDED && g2.arot.check_landed()) {
      72           1 :         current_phase = Phase::LANDED_INIT;
      73             :     }
      74             : 
      75             :     // Check if we are bailing out and need to re-set the spool state
      76       14401 :     if (motors->autorotation_bailout()) {
      77         960 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      78             :     }
      79             : 
      80             :     // Get norm input from yaw channel
      81       14401 :     const float pilot_norm_input = channel_yaw->norm_input_dz();
      82             : 
      83             :     //----------------------------------------------------------------
      84             :     //                  State machine actions
      85             :     //----------------------------------------------------------------
      86       14401 :     switch (current_phase) {
      87             : 
      88           1 :         case Phase::ENTRY_INIT:
      89             :             // Entry phase functions to be run only once
      90           1 :             g2.arot.init_entry();
      91           1 :             current_phase = Phase::ENTRY;
      92             :             FALLTHROUGH;
      93             : 
      94         800 :         case Phase::ENTRY:
      95             :             // Smoothly transition the collective to enter autorotation regime and begin forward speed control
      96         800 :             g2.arot.run_entry(pilot_norm_input);
      97         800 :             break;
      98             : 
      99           1 :         case Phase::GLIDE_INIT:
     100             :             // Glide phase functions to be run only once
     101           1 :             g2.arot.init_glide();
     102           1 :             current_phase = Phase::GLIDE;
     103             :             FALLTHROUGH;
     104             : 
     105        5920 :         case Phase::GLIDE:
     106             :             // Maintain head speed and forward speed as we glide to the ground
     107        5920 :             g2.arot.run_glide(pilot_norm_input);
     108        5920 :             break;
     109             : 
     110           0 :         case Phase::FLARE_INIT:
     111             :         case Phase::FLARE:
     112             :         case Phase::TOUCH_DOWN_INIT:
     113             :         case Phase::TOUCH_DOWN:
     114           0 :             break;
     115             : 
     116           1 :         case Phase::LANDED_INIT:
     117             :             // Landed phase functions to be run only once
     118           1 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AROT: Landed");
     119           1 :             current_phase = Phase::LANDED;
     120             :             FALLTHROUGH;
     121             : 
     122        7681 :         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        7681 :             g2.arot.run_landed();
     125        7681 :             break;
     126             :     }
     127             : 
     128             :     // Slow rate (25 Hz) logging for the mode
     129       14401 :     if (now_ms - _last_logged_ms > 40U) {
     130         848 :         g2.arot.log_write_autorotation();
     131         848 :         _last_logged_ms = now_ms;
     132             :     }
     133             : 
     134       14401 : } // End function run()
     135             : 
     136             : #endif