Skip to content

Commit

Permalink
AC_Autorotation: Add RSC_Autorotation class
Browse files Browse the repository at this point in the history
  • Loading branch information
MattKear committed Sep 23, 2024
1 parent b3dc3da commit e5a8278
Show file tree
Hide file tree
Showing 2 changed files with 205 additions and 0 deletions.
153 changes: 153 additions & 0 deletions libraries/AC_Autorotation/RSC_Autorotation.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,153 @@
#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 || idle_output.get() <= 0) {
// we do not want to use autorotation idle throttle
return false;
}

// 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;
}
52 changes: 52 additions & 0 deletions libraries/AC_Autorotation/RSC_Autorotation.h
Original file line number Diff line number Diff line change
@@ -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"

};

0 comments on commit e5a8278

Please sign in to comment.