Skip to content

Commit

Permalink
AP_MotorsHeli: Fixup
Browse files Browse the repository at this point in the history
  • Loading branch information
@MattKear
MattKear committed Sep 21, 2024
1 parent df11880 commit 3cb4d51
Show file tree
Hide file tree
Showing 5 changed files with 22 additions and 176 deletions.
5 changes: 5 additions & 0 deletions libraries/AP_Motors/AP_MotorsHeli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include "AP_MotorsHeli.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AC_Autorotation/RSC_Autorotation.h>

extern const AP_HAL::HAL& hal;

Expand Down Expand Up @@ -147,6 +148,10 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL_LAND_MIN", 32, AP_MotorsHeli, _collective_land_min_deg, AP_MOTORS_HELI_COLLECTIVE_LAND_MIN),

// @Group: RSC_AROT_
// @Path: ../AP_Motors/AP_MotorsHeli_RSC.cpp
AP_SUBGROUPINFO(_main_rotor.autorotation, "RSC_AROT_", 33, AP_MotorsHeli, RSC_Autorotation),

AP_GROUPEND
};

Expand Down
13 changes: 5 additions & 8 deletions libraries/AP_Motors/AP_MotorsHeli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,20 +118,17 @@ class AP_MotorsHeli : public AP_Motors {
// support passing init_targets_on_arming flag to greater code
bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; }

// arot_man_enabled - gets contents of manual_autorotation_enabled parameter
bool arot_man_enabled() const { return _main_rotor.autorotation_enabled(); }

// helper for vehicle code to request autorotation states in the RSC.
void set_autorotation_active(bool tf) { _main_rotor.set_autorotation_active(tf); }
void set_autorotation_active(bool tf) { _main_rotor.autorotation.set_active(tf, false); }

// helper to force the RSC autorotation state to deactivated
void force_deactivate_autorotation(void) { _main_rotor.set_autorotation_active(false, true); }
void force_deactivate_autorotation(void) { _main_rotor.autorotation.set_active(false, true); }

// helper for vehicle code to check autorotation state
// true if RSC is actively autorotating or bailing out
bool in_autorotation(void) const { return _main_rotor.in_autorotation(); }

// helper for vehicle code to check autorotation state
bool autorotation_bailout(void) const { return _main_rotor.in_autorotation(); }
// true if bailing out autorotation
bool autorotation_bailout(void) const { return _main_rotor.autorotation.bailing_out(); }

// set land complete flag
void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
Expand Down
136 changes: 7 additions & 129 deletions libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,6 @@
// default main rotor ramp up time in seconds
#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to setpoint
#define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed
#define AP_MOTORS_HELI_RSC_AROT_BAILOUT_TIME 1 // time in seconds to ramp motors when bailing out of autorotation
#define AP_MOTORS_HELI_RSC_AROT_IDLE 0
#define AP_MOTORS_HELI_RSC_AROT_ENABLE 0

// Throttle Curve Defaults
#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 25
Expand Down Expand Up @@ -221,39 +218,11 @@ const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = {
// @User: Standard
AP_GROUPINFO("GOV_TORQUE", 24, AP_MotorsHeli_RSC, _governor_torque, 30),

// @Param: AROT_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.
// @Range: 1 10
// @Units: s
// @Increment: 0.5
// @User: Standard
AP_GROUPINFO("AROT_RAMP", 25, AP_MotorsHeli_RSC, autorotation.bailout_throttle_time, AP_MOTORS_HELI_RSC_AROT_BAILOUT_TIME),

// @Param: AROT_ENBL
// @DisplayName: Enable Manual Autorotations
// @Description: Allows you to enable (1) or disable (0) the manual autorotation capability.
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("AROT_ENBL", 26, AP_MotorsHeli_RSC, autorotation.enable, AP_MOTORS_HELI_RSC_AROT_ENABLE),
// 25 was AROT_ENG_T, has been moved to AROT_RAMP in RSC autorotation sub group

// @Param: AROT_IDLE
// @DisplayName: Idle Throttle Percentage during Autorotation
// @Description: Idle throttle used for all RSC modes. For external governors, this would be set to signal it to enable fast spool-up, when bailing out of an autorotation. Set 0 to disable. If also using a tail rotor of type DDVP with external governor then this value must lie within the autorotation window of both governors.
// @Range: 0 40
// @Units: %
// @Increment: 1
// @User: Standard
AP_GROUPINFO("AROT_IDLE", 27, AP_MotorsHeli_RSC, autorotation.idle_output, AP_MOTORS_HELI_RSC_AROT_IDLE),
// 26 was AROT_MN_EN, moved to H_RSC_AROT_ENBL in RSC autorotation sub group

// @Param: AROT_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 (throttle hold off). 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("AROT_RUNUP", 28, AP_MotorsHeli_RSC, autorotation.bailout_runup_time, AP_MOTORS_HELI_RSC_AROT_BAILOUT_TIME+1),
// 27 was AROT_IDLE, moved to RSC autorotation sub group

AP_GROUPEND
};
Expand Down Expand Up @@ -329,7 +298,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
_starting = true;

// ensure we always deactivate the autorotation state if we disarm
autorotation.set_state(Autorotation::State::DEACTIVATED, true);
autorotation.set_active(false, true);

// ensure _idle_throttle not set to invalid value
_idle_throttle = get_idle_output();
Expand Down Expand Up @@ -463,7 +432,7 @@ void AP_MotorsHeli_RSC::update_rotor_runup(float dt)
}
// if in autorotation, don't let rotor_runup_output go less than critical speed to keep
// runup complete flag from being set to false
if (autorotation_active() && !rotor_speed_above_critical()) {
if (in_autorotation() && !rotor_speed_above_critical()) {
_rotor_runup_output = get_critical_speed();
}

Expand Down Expand Up @@ -646,100 +615,9 @@ void AP_MotorsHeli_RSC::write_log(void) const
}
#endif

// helper for external sources to request changes in autorotation state
// to force the state to be immediatly deactivated, then the force_state bool is used
void AP_MotorsHeli_RSC::set_autorotation_active(bool tf, bool force_state)
{
// 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
const Autorotation::State desired_state = tf ? Autorotation::State::ACTIVE : Autorotation::State::DEACTIVATED;
autorotation.set_state(desired_state, force_state);
}

// considered to be "in an autorotation" if active or bailing out
bool AP_MotorsHeli_RSC::in_autorotation(void) const {
return autorotation_active() || autorotation_bailout();
}

// 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 immediatly deactivated, then the force_state bool is used
void AP_MotorsHeli_RSC::Autorotation::set_state(State desired_state, bool force_state)
{
// don't do anything if desired state is already set
if (desired_state == state || enable.get() == 0) {
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 AP_MotorsHeli_RSC::Autorotation::get_idle_throttle(float& idle_throttle)
bool AP_MotorsHeli_RSC::in_autorotation(void) const
{
if (state != Autorotation::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 AP_MotorsHeli_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 AP_MotorsHeli_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 == Autorotation::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());
return autorotation.active() || autorotation.bailing_out();
}
41 changes: 4 additions & 37 deletions libraries/AP_Motors/AP_MotorsHeli_RSC.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_Logger/AP_Logger_config.h>
#include <AC_Autorotation/RSC_Autorotation.h>

// rotor control modes
enum RotorControlMode {
Expand Down Expand Up @@ -78,14 +79,8 @@ class AP_MotorsHeli_RSC {
// set_collective. collective for throttle curve calculation
void set_collective(float collective) { _collective_in = collective; }

// helper for external sources to request changes in autorotation state
void set_autorotation_active(bool tf, bool force = false);

// helpers to check if we are considered to be autorotating
// true if we are considered to be autorotating or bailing out of an autorotation
bool in_autorotation(void) const;
bool autorotation_active(void) const { return autorotation.get_state() == Autorotation::State::ACTIVE; }
bool autorotation_bailout(void) const { return autorotation.get_state() == Autorotation::State::BAILING_OUT; }
bool autorotation_enabled(void) const { return autorotation.enable.get() == 1; }

// turbine start initialize sequence
void set_turbine_start(bool turbine_start) {_turbine_start = turbine_start; }
Expand All @@ -104,6 +99,8 @@ class AP_MotorsHeli_RSC {
void write_log(void) const;
#endif

RSC_Autorotation autorotation;

// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

Expand Down Expand Up @@ -176,34 +173,4 @@ class AP_MotorsHeli_RSC {
float get_governor_torque() const { return _governor_torque * 0.01; }
float get_governor_compensator() const { return _governor_compensator * 0.000001; }

// helper class to manage autorotation state and variables within RSC
class Autorotation {
public:

bool get_idle_throttle(float& idle_throttle);
float get_bailout_ramp(void) const;
float get_runup_time(void) const;
bool bailing_out(void) { return state == Autorotation::State::BAILING_OUT; }

enum class State {
DEACTIVATED,
BAILING_OUT,
ACTIVE,
};

void set_state(State desired_state, bool force_state);
State get_state(void) const { return state; };

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

private:

State state;
uint32_t bail_out_started_ms; // (milliseconds) time that bailout started, used to time transition from "bailing out" to "autorotation stopped"

} autorotation;

};
3 changes: 1 addition & 2 deletions libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -391,7 +391,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
collective_out = _collective_land_min_pct;
limit.throttle_lower = true;

}

// updates below land min collective flag
Expand Down Expand Up @@ -447,7 +446,7 @@ float AP_MotorsHeli_Single::get_yaw_offset(float collective)
return 0.0;
}

if (_main_rotor.autorotation_active() || (_main_rotor.get_control_output() <= _main_rotor.get_idle_output())) {
if (_main_rotor.autorotation.active() || (_main_rotor.get_control_output() <= _main_rotor.get_idle_output())) {
// Motor is stopped or at idle, and thus not creating torque
return 0.0;
}
Expand Down

0 comments on commit 3cb4d51

Please sign in to comment.