From 3cb4d51d5aab3eaafc9c869f6c70ef1834715731 Mon Sep 17 00:00:00 2001 From: MattKear Date: Sat, 21 Sep 2024 02:27:34 +0100 Subject: [PATCH] AP_MotorsHeli: Fixup --- libraries/AP_Motors/AP_MotorsHeli.cpp | 5 + libraries/AP_Motors/AP_MotorsHeli.h | 13 +- libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 136 +------------------ libraries/AP_Motors/AP_MotorsHeli_RSC.h | 41 +----- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 3 +- 5 files changed, 22 insertions(+), 176 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 458d639aa89d0..7336c2e592539 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -18,6 +18,7 @@ #include "AP_MotorsHeli.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -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 }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 8dc690611b95d..1201469bc098c 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -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; } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index d922976a44209..79418385f78ea 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -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 @@ -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 }; @@ -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(); @@ -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(); } @@ -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(); } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index a4cda5475e0f0..69836d5fb4a86 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -5,6 +5,7 @@ #include #include #include +#include // rotor control modes enum RotorControlMode { @@ -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; } @@ -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[]; @@ -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; - }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 121febf87c2fe..9f31a818ffdfa 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -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 @@ -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; }