AC_Autorotation: Add RSC_Autorotation class

libraries/AC_Autorotation/RSC_Autorotation.cpp

@@ -0,0 +1,159 @@
+#include "RSC_Autorotation.h"
+#include <GCS_MAVLink/GCS.h>
+#include <AP_HAL/AP_HAL.h>
+
+#define RSC_AROT_RAMP_TIME_DEFAULT 2 // time in seconds to ramp motors when bailing out of autorotation
+
+extern const AP_HAL::HAL& hal;
+
+// RSC autorotation state specific parameters
+const AP_Param::GroupInfo RSC_Autorotation::var_info[] = {
+
+    // @Param: ENBL
+    // @DisplayName: Enable autorotation handling in RSC
+    // @Description: Allows you to enable (1) or disable (0) the autorotation functionality within the Rotor Speed Controller.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    AP_GROUPINFO_FLAGS("ENBL", 1, RSC_Autorotation, enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: RAMP
+    // @DisplayName: Time for in-flight power re-engagement when exiting autorotations
+    // @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).
+    // @Range: 0.1 10
+    // @Units: s
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("RAMP", 2, RSC_Autorotation, bailout_throttle_time, RSC_AROT_RAMP_TIME_DEFAULT),
+
+    // @Param: IDLE
+    // @DisplayName: Idle throttle percentage during autorotation
+    // @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.
+    // @Range: 0 40
+    // @Units: %
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("IDLE", 3, RSC_Autorotation, idle_output, 0.0),
+
+    // @Param: RUNUP
+    // @DisplayName: Time allowed for in-flight power re-engagement
+    // @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.
+    // @Range: 1 10
+    // @Units: s
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("RUNUP", 4, RSC_Autorotation, bailout_runup_time, RSC_AROT_RAMP_TIME_DEFAULT+1),
+
+    AP_GROUPEND
+};
+
+RSC_Autorotation::RSC_Autorotation(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// set the desired autorotation state
+// this state machine handles the transition from active to deactivated via the bailout logic
+// to force the state to be immediately deactivated, then the force_state bool is used
+void RSC_Autorotation::set_active(bool active, bool force_state)
+{
+    if (enable.get() != 1) {
+        return;
+    }
+
+    // set the desired state based on the bool. We only set either ACTIVE or DEACTIVATED
+    // here and let the autorotation state machine and RSC runup code handle the bail out case
+    RSC_Autorotation::State desired_state = active ? RSC_Autorotation::State::ACTIVE : RSC_Autorotation::State::DEACTIVATED;
+
+    // don't do anything if desired state is already set
+    if (desired_state == state) {
+        return;
+    }
+
+    // Handle the transition from the ACTIVE to DEACTIVATED states via the BAILING_OUT case
+    // set the bailout case if deactivated has just been requested
+    if ((state == State::ACTIVE) && (desired_state == State::DEACTIVATED) && !force_state) {
+        desired_state = State::BAILING_OUT;
+        bail_out_started_ms = AP_HAL::millis();
+    }
+
+    // Wait for allocated autorotation run up time before we allow progression of state to deactivated
+    if ((state == State::BAILING_OUT) && 
+        (desired_state == State::DEACTIVATED) &&
+        (bail_out_started_ms > 0) && 
+        (AP_HAL::millis() - bail_out_started_ms < uint32_t(get_runup_time()*1000)))
+    {
+        return;
+    }
+
+    // handle GCS messages
+    switch (desired_state)
+    {
+    case State::DEACTIVATED:
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RSC: Autorotation Stopped");
+        break;
+
+    case State::BAILING_OUT:
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RSC: Bailing Out");
+        break;
+
+    case State::ACTIVE:
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RSC: In Autorotation");
+        break;
+
+    default:
+        // do nothing
+        break;
+    }
+
+    // Actually set the state
+    state = desired_state;
+}
+
+bool RSC_Autorotation::get_idle_throttle(float& idle_throttle)
+{
+    if (state != State::ACTIVE) {
+        // We do not want to use autorotation idle throttle
+        return false;
+    }
+
+    if (idle_output.get() <= 0) {
+        // If autorotation idle is not set, do not modify idle throttle as we just use H_RSC_IDLE
+        // Heli with an ICE engine is an example of this type of config
+        return true;
+    }
+
+    // if we are autorotating and the autorotation idle throttle param is set we want to
+    // to output this as the idle throttle for ESCs with an autorotation window
+    idle_throttle = constrain_float(idle_output.get()*0.01, 0.0, 0.4);
+
+    return true;
+}
+
+float RSC_Autorotation::get_bailout_ramp(void) const
+{
+    // Allow ramp times as quick as 0.1 of a second for ESCs with autorotation windows
+    return MAX(float(bailout_throttle_time.get()), 0.1);
+}
+
+float RSC_Autorotation::get_runup_time(void) const
+{
+    // If we are in the autorotation state we want the rotor speed model to ramp down rapidly to zero, ensuring we get past
+    // the critical rotor speed, and therefore triggering a proper bailout should we re-engage the interlock at any point
+    if (state == State::ACTIVE) {
+        return 0.1;
+    }
+
+    // Never let the runup timer be less than the throttle ramp time
+    return (float) MAX(bailout_throttle_time.get() + 1, bailout_runup_time.get());
+}
+
+// sanity check of parameters, should be called only whilst disarmed
+bool RSC_Autorotation::arming_checks(size_t buflen, char *buffer) const
+{
+    // throttle runup must be larger than ramp, keep the params up to date to not confuse users
+    if (bailout_throttle_time.get() + 1 > bailout_runup_time.get()) {
+        hal.util->snprintf(buffer, buflen, "H_RSC_AROT_RUNUP must be > H_RSC_AROT_RAMP");
+        return false;
+    }
+    return true;
+}

libraries/AC_Autorotation/RSC_Autorotation.h

@@ -0,0 +1,52 @@
+// Class supporting autorotation state within the heli rotor speed controller
+
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+
+// helper class to manage autorotation state and variables within RSC
+class RSC_Autorotation
+{
+public:
+
+    RSC_Autorotation(void);
+
+    enum class State {
+        DEACTIVATED,
+        BAILING_OUT,
+        ACTIVE,
+    };
+
+    // state accessors
+    bool active(void) const { return state == State::ACTIVE; }
+    bool bailing_out(void) const { return state == State::BAILING_OUT; }
+
+    // update idle throttle when in autorotation
+    bool get_idle_throttle(float& idle_throttle);
+
+    // get the throttle ramp rate needed when bailing out of autorotation
+    float get_bailout_ramp(void) const;
+
+    // get the allowed run-up time that we expect the rotor to need to complete a bailout
+    float get_runup_time(void) const;
+
+    // request changes in autorotation state
+    void set_active(bool active, bool force_state);
+
+    // sanity check of parameters, should be called only whilst disarmed
+    bool arming_checks(size_t buflen, char *buffer) const;
+
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    AP_Int8  idle_output;             // (percent) rsc output used when in autorotation, used for setting autorotation window on ESCs
+    AP_Int8  bailout_throttle_time;   // (seconds) time for in-flight power re-engagement when bailing-out of an autorotation
+    AP_Int8  bailout_runup_time;      // (seconds) expected time for the motor to fully engage and for the rotor to regain safe head speed if necessary
+    AP_Int8  enable;                  // enables autorotation state within the RSC
+
+    State state;
+    uint32_t bail_out_started_ms;     // (milliseconds) time that bailout started, used to time transition from "bailing out" to "autorotation stopped"
+
+};