Line data    Source code

       1             : #include "RSC_Autorotation.h"
       2             : #include <GCS_MAVLink/GCS.h>
       3             : #include <AP_HAL/AP_HAL.h>
       4             : 
       5             : #define RSC_AROT_RAMP_TIME_DEFAULT 2 // time in seconds to ramp motors when bailing out of autorotation
       6             : 
       7             : extern const AP_HAL::HAL& hal;
       8             : 
       9             : // RSC autorotation state specific parameters
      10             : const AP_Param::GroupInfo RSC_Autorotation::var_info[] = {
      11             : 
      12             :     // @Param: ENBL
      13             :     // @DisplayName: Enable autorotation handling in RSC
      14             :     // @Description: Allows you to enable (1) or disable (0) the autorotation functionality within the Rotor Speed Controller.
      15             :     // @Values: 0:Disabled,1:Enabled
      16             :     // @User: Standard
      17             :     AP_GROUPINFO_FLAGS("ENBL", 1, RSC_Autorotation, enable, 0, AP_PARAM_FLAG_ENABLE),
      18             : 
      19             :     // @Param: RAMP
      20             :     // @DisplayName: Time for in-flight power re-engagement when exiting autorotations
      21             :     // @Description: When exiting an autorotation in a bailout manoeuvre, this is the time in seconds for the throttle output (HeliRSC servo) to ramp from idle (H_RSC_AROT_IDLE) to flight throttle setting when motor interlock is re-enabled. When using an ESC with an autorotation bailout function, this parameter should be set to 0.1 (minimum value).
      22             :     // @Range: 0.1 10
      23             :     // @Units: s
      24             :     // @Increment: 0.1
      25             :     // @User: Standard
      26             :     AP_GROUPINFO("RAMP", 2, RSC_Autorotation, bailout_throttle_time, RSC_AROT_RAMP_TIME_DEFAULT),
      27             : 
      28             :     // @Param: IDLE
      29             :     // @DisplayName: Idle throttle percentage during autorotation
      30             :     // @Description: Idle throttle used for during autotoration. For external governors, this would be set to a value that is within the autorotation window of the governer/ESC to enable fast spool-up, when bailing out of an autorotation.  Set 0 to disable.
      31             :     // @Range: 0 40
      32             :     // @Units: %
      33             :     // @Increment: 1
      34             :     // @User: Standard
      35             :     AP_GROUPINFO("IDLE", 3, RSC_Autorotation, idle_output, 0.0),
      36             : 
      37             :     // @Param: RUNUP
      38             :     // @DisplayName: Time allowed for in-flight power re-engagement
      39             :     // @Description: When exiting an autorotation in a bailout manoeuvre, this is the expected time in seconds for the main rotor to reach full speed after motor interlock is enabled. Must be at least one second longer than the H_RSC_AROT_RAMP time that is set. This timer should be set for at least the amount of time it takes to get your helicopter to full flight power. Failure to heed this warning could result in early entry into autonomously controlled collective modes (e.g. alt hold, loiter, etc), whereby the collective could be raised before the engine has reached full power, with a subsequently dangerous slowing of head speed.
      40             :     // @Range: 1 10
      41             :     // @Units: s
      42             :     // @Increment: 0.1
      43             :     // @User: Standard
      44             :     AP_GROUPINFO("RUNUP", 4, RSC_Autorotation, bailout_runup_time, RSC_AROT_RAMP_TIME_DEFAULT+1),
      45             : 
      46             :     AP_GROUPEND
      47             : };
      48             : 
      49         163 : RSC_Autorotation::RSC_Autorotation(void)
      50             : {
      51         163 :     AP_Param::setup_object_defaults(this, var_info);
      52         163 : }
      53             : 
      54             : // set the desired autorotation state
      55             : // this state machine handles the transition from active to deactivated via the bailout logic
      56             : // to force the state to be immediately deactivated, then the force_state bool is used
      57     4197976 : void RSC_Autorotation::set_active(bool active, bool force_state)
      58             : {
      59     4197976 :     if (!enabled()) {
      60     3922508 :         return;
      61             :     }
      62             : 
      63             :     // set the desired state based on the bool. We only set either ACTIVE or DEACTIVATED
      64             :     // here and let the autorotation state machine and RSC runup code handle the bail out case
      65      275468 :     RSC_Autorotation::State desired_state = active ? RSC_Autorotation::State::ACTIVE : RSC_Autorotation::State::DEACTIVATED;
      66             : 
      67             :     // don't do anything if desired state is already set
      68      275468 :     if (desired_state == state) {
      69      265689 :         return;
      70             :     }
      71             : 
      72             :     // Handle the transition from the ACTIVE to DEACTIVATED states via the BAILING_OUT case
      73             :     // set the bailout case if deactivated has just been requested
      74        9779 :     if ((state == State::ACTIVE) && (desired_state == State::DEACTIVATED) && !force_state) {
      75           5 :         desired_state = State::BAILING_OUT;
      76           5 :         bail_out_started_ms = AP_HAL::millis();
      77             :     }
      78             : 
      79             :     // Wait for allocated autorotation run up time before we allow progression of state to deactivated
      80        9760 :     if ((state == State::BAILING_OUT) && 
      81        9759 :         (desired_state == State::DEACTIVATED) &&
      82       29298 :         (bail_out_started_ms > 0) && 
      83        9759 :         (AP_HAL::millis() - bail_out_started_ms < uint32_t(get_runup_time()*1000)))
      84             :     {
      85        9755 :         return;
      86             :     }
      87             : 
      88             :     // handle GCS messages
      89          24 :     switch (desired_state)
      90             :     {
      91           9 :     case State::DEACTIVATED:
      92           9 :         GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RSC: Autorotation Stopped");
      93           9 :         break;
      94             : 
      95           5 :     case State::BAILING_OUT:
      96           5 :         GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RSC: Bailing Out");
      97           5 :         break;
      98             : 
      99          10 :     case State::ACTIVE:
     100          10 :         GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RSC: In Autorotation");
     101          10 :         break;
     102             : 
     103           0 :     default:
     104             :         // do nothing
     105           0 :         break;
     106             :     }
     107             : 
     108             :     // Actually set the state
     109          24 :     state = desired_state;
     110             : }
     111             : 
     112      254377 : bool RSC_Autorotation::get_idle_throttle(float& idle_throttle)
     113             : {
     114      254377 :     if (state != State::ACTIVE) {
     115             :         // We do not want to use autorotation idle throttle
     116      226954 :         return false;
     117             :     }
     118             : 
     119       27423 :     if (idle_output.get() <= 0) {
     120             :         // If autorotation idle is not set, do not modify idle throttle as we just use H_RSC_IDLE
     121             :         // Heli with an ICE engine is an example of this type of config
     122       16244 :         return true;
     123             :     }
     124             : 
     125             :     // if we are autorotating and the autorotation idle throttle param is set we want to
     126             :     // to output this as the idle throttle for ESCs with an autorotation window
     127       11179 :     idle_throttle = constrain_float(idle_output.get()*0.01, 0.0, 0.4);
     128             : 
     129       11179 :     return true;
     130             : }
     131             : 
     132        9760 : float RSC_Autorotation::get_bailout_ramp(void) const
     133             : {
     134             :     // Allow ramp times as quick as 0.1 of a second for ESCs with autorotation windows
     135        9760 :     return MAX(float(bailout_throttle_time.get()), 0.1);
     136             : }
     137             : 
     138       46942 : float RSC_Autorotation::get_runup_time(void) const
     139             : {
     140             :     // If we are in the autorotation state we want the rotor speed model to ramp down rapidly to zero, ensuring we get past
     141             :     // the critical rotor speed, and therefore triggering a proper bailout should we re-engage the interlock at any point
     142       46942 :     if (state == State::ACTIVE) {
     143       27423 :         return 0.1;
     144             :     }
     145             : 
     146             :     // Never let the runup timer be less than the throttle ramp time
     147       19519 :     return (float) MAX(bailout_throttle_time.get() + 1, bailout_runup_time.get());
     148             : }
     149             : 
     150             : // sanity check of parameters, should be called only whilst disarmed
     151        2339 : bool RSC_Autorotation::arming_checks(size_t buflen, char *buffer) const
     152             : {
     153             :     // throttle runup must be larger than ramp, keep the params up to date to not confuse users
     154        2339 :     if (bailout_throttle_time.get() + 1 > bailout_runup_time.get()) {
     155           0 :         hal.util->snprintf(buffer, buflen, "H_RSC_AROT_RUNUP must be > H_RSC_AROT_RAMP");
     156           0 :         return false;
     157             :     }
     158        2339 :     return true;
     159             : }