From 67932d9fd7f52e2b8dc5656809ef0df071e9aab0 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Mon, 29 Jul 2024 07:33:39 +0100 Subject: [PATCH 1/8] AP_MotorsHeli: Consolidate all autorotation state into its own class within RSC --- libraries/AP_Motors/AP_MotorsHeli.cpp | 27 ++- libraries/AP_Motors/AP_MotorsHeli.h | 22 +- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 8 - libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 8 - libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 193 +++++++++--------- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 56 +---- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 30 +-- .../AP_Motors_test/AP_Motors_test.cpp | 2 +- 8 files changed, 141 insertions(+), 205 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index d81da48b1a9222..506e4af32073bc 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: ../AC_Autorotation/RSC_Autorotation.cpp + AP_SUBGROUPINFO(_main_rotor.autorotation, "RSC_AROT_", 33, AP_MotorsHeli, RSC_Autorotation), + AP_GROUPEND }; @@ -554,6 +559,10 @@ bool AP_MotorsHeli::arming_checks(size_t buflen, char *buffer) const return false; } + if (!_main_rotor.autorotation.arming_checks(buflen, buffer)) { + return false; + } + return true; } @@ -606,7 +615,7 @@ void AP_MotorsHeli::set_rotor_runup_complete(bool new_value) #if HAL_LOGGING_ENABLED if (!_heliflags.rotor_runup_complete && new_value) { LOGGER_WRITE_EVENT(LogEvent::ROTOR_RUNUP_COMPLETE); - } else if (_heliflags.rotor_runup_complete && !new_value && !_heliflags.in_autorotation) { + } else if (_heliflags.rotor_runup_complete && !new_value && !_main_rotor.in_autorotation()) { LOGGER_WRITE_EVENT(LogEvent::ROTOR_SPEED_BELOW_CRITICAL); } #endif @@ -623,3 +632,19 @@ float AP_MotorsHeli::get_cyclic_angle_scaler(void) const { return ((float)(_collective_max-_collective_min))*1e-3 * (_collective_max_deg.get() - _collective_min_deg.get()) * 2.0; } #endif + +// Helper function for param conversions to be done in motors class +void AP_MotorsHeli::heli_motors_param_conversions(void) +{ + // PARAMETER_CONVERSION - Added: Sep-2024 + // move autorotation related parameters within the RSC into their own class + const AP_Param::ConversionInfo rsc_arot_conversion_info[] = { + { 90, 108096, AP_PARAM_INT8, "H_RSC_AROT_ENBL" }, + { 90, 104000, AP_PARAM_INT8, "H_RSC_AROT_RAMP" }, + { 90, 112192, AP_PARAM_INT16, "H_RSC_AROT_IDLE" }, + }; + uint8_t table_size = ARRAY_SIZE(rsc_arot_conversion_info); + for (uint8_t i=0; i #include +// default main rotor speed (ch8 out) as a number from 0 ~ 100 +#define AP_MOTORS_HELI_RSC_SETPOINT 70 + +// default main rotor critical speed +#define AP_MOTORS_HELI_RSC_CRITICAL 50 + +// RSC output defaults +#define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0 + +// 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 + +// Throttle Curve Defaults +#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 25 +#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 32 +#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 38 +#define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 50 +#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 100 + +// RSC governor defaults +#define AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT 100 + + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = { @@ -194,30 +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_ENG_T - // @DisplayName: Time for in-flight power re-engagement - // @Description: amount of seconds to move throttle output from idle to throttle curve position during manual autorotations - // @Range: 0 10 - // @Units: % - // @Increment: 0.5 - // @User: Standard - AP_GROUPINFO("AROT_ENG_T", 25, AP_MotorsHeli_RSC, _rsc_arot_engage_time, AP_MOTORS_HELI_RSC_AROT_ENGAGE_TIME), + // 25 was AROT_ENG_T, has been moved to AROT_RAMP in RSC autorotation sub group - // @Param: AROT_MN_EN - // @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_MN_EN", 26, AP_MotorsHeli_RSC, _rsc_arot_man_enable, 0), + // 26 was AROT_MN_EN, moved to H_RSC_AROT_ENBL 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, _arot_idle_output, AP_MOTORS_HELI_RSC_AROT_IDLE), + // 27 was AROT_IDLE, moved to RSC autorotation sub group AP_GROUPEND }; @@ -247,6 +252,8 @@ void AP_MotorsHeli_RSC::set_throttle_curve() // output - update value to send to ESC/Servo void AP_MotorsHeli_RSC::output(RotorControlState state) { + // Store rsc state for logging + _rsc_state = state; // _rotor_RPM available to the RSC output #if AP_RPM_ENABLED const AP_RPM *rpm = AP_RPM::get_singleton(); @@ -289,9 +296,9 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) _governor_fault = false; //turbine start flag on _starting = true; - _autorotating = false; - _bailing_out = false; - _gov_bailing_out = false; + + // ensure we always deactivate the autorotation state if we disarm + autorotation.set_active(false, true); // ensure _idle_throttle not set to invalid value _idle_throttle = get_idle_output(); @@ -309,44 +316,36 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) governor_reset(); _autothrottle = false; _governor_fault = false; - if (_in_autorotation) { - // if in autorotation, set the output to idle for autorotation. This will tell an external governor to use fast ramp for spool up. - // if autorotation idle is set to zero then default to the RSC idle value. - if (_arot_idle_output == 0) { - _idle_throttle = get_idle_output(); - } else { - _idle_throttle = constrain_float( get_arot_idle_output(), 0.0f, 0.4f); - } - if (!_autorotating) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Autorotation"); - _autorotating =true; - } - } else { - if (_autorotating) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Autorotation Stopped"); - _autorotating =false; - } - // set rotor control speed to idle speed parameter, this happens instantly and ignores ramping - if (_turbine_start && _starting == true ) { - _idle_throttle += 0.001f; - if (_control_output >= 1.0f) { - _idle_throttle = get_idle_output(); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Turbine startup"); - _starting = false; - } - } else { + + // turbine start sequence + if (_turbine_start && _starting == true ) { + _idle_throttle += 0.001f; + if (_control_output >= 1.0f) { _idle_throttle = get_idle_output(); - if (_fast_idle_timer > 0.0) { - // running at fast idle for engine cool down - _idle_throttle *= 1.5; - _fast_idle_timer -= dt; - } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Turbine startup"); + _starting = false; } - // this resets the bailout feature if the aircraft has landed. - _use_bailout_ramp = false; - _bailing_out = false; - _gov_bailing_out = false; + _control_output = _idle_throttle; + break; + } + + // all other idle throttle functions below this require idle throttle to be reset to H_RSC_IDLE on each call + _idle_throttle = get_idle_output(); + + // check if we need to use autorotation idle throttle + if (autorotation.get_idle_throttle(_idle_throttle)) { + // if we are here then we are autorotating + _control_output = _idle_throttle; + break; } + + // check if we need to use engine cooldown + if (_fast_idle_timer > 0.0) { + // running at fast idle for engine cool down + _idle_throttle *= 1.5; + _fast_idle_timer -= dt; + } + _control_output = _idle_throttle; break; @@ -365,7 +364,6 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) } // if turbine engine started without using start sequence, set starting flag just to be sure it can't be triggered when back in idle _starting = false; - _autorotating = false; if ((_control_mode == ROTOR_CONTROL_MODE_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SETPOINT)) { // set control rotor speed to ramp slewed value between idle and desired speed @@ -396,38 +394,20 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt) { - int8_t ramp_time; - int8_t bailout_time; - // sanity check ramp time and enable bailout if set - if (_ramp_time <= 0) { - ramp_time = 1; - } else { - ramp_time = _ramp_time; - } + float ramp_time = MAX(float(_ramp_time.get()), 1.0); - if (_rsc_arot_engage_time <= 0) { - bailout_time = 1; - } else { - bailout_time = _rsc_arot_engage_time; + // check if we need to use the bailout ramp up rate for the autorotation case + if (autorotation.bailing_out()) { + ramp_time = autorotation.get_bailout_ramp(); } // ramp output upwards towards target if (_rotor_ramp_output < rotor_ramp_input) { - if (_use_bailout_ramp) { - if (!_bailing_out) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "bailing_out"); - _bailing_out = true; - if (_control_mode == ROTOR_CONTROL_MODE_AUTOTHROTTLE) {_gov_bailing_out = true;} - } - _rotor_ramp_output += (dt / bailout_time); - } else { - _rotor_ramp_output += (dt / ramp_time); - } - if (_rotor_ramp_output > rotor_ramp_input) { - _rotor_ramp_output = rotor_ramp_input; - _bailing_out = false; - _use_bailout_ramp = false; - } + _rotor_ramp_output += (dt / ramp_time); + + // Do not allow output to exceed requested input + _rotor_ramp_output = MIN(_rotor_ramp_output, rotor_ramp_input); + } else { // ramping down happens instantly _rotor_ramp_output = rotor_ramp_input; @@ -437,14 +417,13 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt) // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut void AP_MotorsHeli_RSC::update_rotor_runup(float dt) { - int8_t runup_time = _runup_time; + float runup_time = _runup_time; // sanity check runup time runup_time = MAX(_ramp_time+1,runup_time); - // adjust rotor runup when bailing out - if (_use_bailout_ramp) { - // maintain same delta as set in parameters - runup_time = _runup_time-_ramp_time+1; + // adjust rotor runup when in autorotation or bailing out + if (in_autorotation()) { + runup_time = autorotation.get_runup_time(); } // protect against divide by zero @@ -465,7 +444,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 (_autorotating && !rotor_speed_above_critical()) { + if (in_autorotation() && !rotor_speed_above_critical()) { _rotor_runup_output = get_critical_speed(); } @@ -574,7 +553,7 @@ void AP_MotorsHeli_RSC::autothrottle_run() } else if (!_governor_engage && !_governor_fault) { // if governor is not engaged and rotor is overspeeding by more than governor range due to // misconfigured throttle curve or stuck throttle, set a fault and governor will not operate - if (_rotor_rpm > (_governor_rpm + _governor_range) && !_gov_bailing_out) { + if (_rotor_rpm > (_governor_rpm + _governor_range) && !autorotation.bailing_out()) { _governor_fault = true; governor_reset(); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed"); @@ -582,7 +561,7 @@ void AP_MotorsHeli_RSC::autothrottle_run() // when performing power recovery from autorotation, this waits for user to load rotor in order to // engage the governor - } else if (_rotor_rpm > _governor_rpm && _gov_bailing_out) { + } else if (_rotor_rpm > _governor_rpm && autorotation.bailing_out()) { _governor_output = 0.0f; // torque rise limiter accelerates rotor to the reference speed @@ -593,7 +572,6 @@ void AP_MotorsHeli_RSC::autothrottle_run() if (_rotor_rpm >= ((float)_governor_rpm - torque_ref_error_rpm)) { _governor_engage = true; _autothrottle = true; - _gov_bailing_out = false; GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Governor Engaged"); } } else { @@ -629,18 +607,29 @@ void AP_MotorsHeli_RSC::write_log(void) const // @Field: ERRPM: Estimated rotor speed // @Field: Gov: Governor Output // @Field: Throt: Throttle output + // @Field: Ramp: throttle ramp up + // @Field: Stat: RSC state // Write to data flash log AP::logger().WriteStreaming("HRSC", - "TimeUS,I,DRRPM,ERRPM,Gov,Throt", - "s#----", - "F-----", - "QBffff", + "TimeUS,I,DRRPM,ERRPM,Gov,Throt,Ramp,Stat", + "s#------", + "F-------", + "QBfffffB", AP_HAL::micros64(), _instance, get_desired_speed(), _rotor_runup_output, _governor_output, - get_control_output()); + get_control_output(), + _rotor_ramp_output, + uint8_t(_rsc_state)); } #endif + + +// considered to be "in an autorotation" if active or bailing out +bool AP_MotorsHeli_RSC::in_autorotation(void) const +{ + 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 353f167f9a07d8..47fa68f8ad180b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -5,31 +5,7 @@ #include #include #include - -// default main rotor speed (ch8 out) as a number from 0 ~ 100 -#define AP_MOTORS_HELI_RSC_SETPOINT 70 - -// default main rotor critical speed -#define AP_MOTORS_HELI_RSC_CRITICAL 50 - -// RSC output defaults -#define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0 - -// 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_ENGAGE_TIME 1 // time in seconds to ramp motors when bailing out of autorotation -#define AP_MOTORS_HELI_RSC_AROT_IDLE 0 - -// Throttle Curve Defaults -#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 25 -#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 32 -#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 38 -#define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 50 -#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 100 - -// RSC governor defaults -#define AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT 100 +#include // rotor control modes enum RotorControlMode { @@ -103,20 +79,8 @@ class AP_MotorsHeli_RSC { // set_collective. collective for throttle curve calculation void set_collective(float collective) { _collective_in = collective; } - // use bailout ramp time - void use_bailout_ramp_time(bool enable) { _use_bailout_ramp = enable; } - - // use external governor autorotation window - void set_autorotation_flag(bool flag) { _in_autorotation = flag; } - - // set the throttle percentage to be used during autorotation for this instance of Heli_RSC - void set_arot_idle_output(int16_t idle) { _arot_idle_output.set(idle); } - - // set the manual autorotation option for this instance of Heli_RSC - void set_rsc_arot_man_enable(int8_t enable) { _rsc_arot_man_enable.set(enable); } - - // set the autorotation power recovery time for this instance of Heli_RSC - void set_rsc_arot_engage_time(int8_t eng_time) { _rsc_arot_engage_time.set(eng_time); } + // true if we are considered to be autorotating or bailing out of an autorotation + bool in_autorotation(void) const; // turbine start initialize sequence void set_turbine_start(bool turbine_start) {_turbine_start = turbine_start; } @@ -135,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[]; @@ -145,9 +111,6 @@ class AP_MotorsHeli_RSC { AP_Int8 _runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time AP_Int16 _critical_speed; // Rotor speed below which flight is not possible AP_Int16 _idle_output; // Rotor control output while at idle - AP_Int16 _arot_idle_output; // Percent value used when in autorotation - AP_Int8 _rsc_arot_engage_time; // time in seconds for in-flight power re-engagement - AP_Int8 _rsc_arot_man_enable; // enables manual autorotation private: uint64_t _last_update_us; @@ -173,16 +136,13 @@ class AP_MotorsHeli_RSC { bool _governor_engage; // RSC governor status flag bool _autothrottle; // autothrottle status flag bool _governor_fault; // governor fault status flag - bool _use_bailout_ramp; // true if allowing RSC to quickly ramp up engine - bool _in_autorotation; // true if vehicle is currently in an autorotation bool _spooldown_complete; // flag for determining if spooldown is complete float _fast_idle_timer; // cooldown timer variable uint8_t _governor_fault_count; // variable for tracking governor speed sensor faults float _governor_torque_reference; // governor reference for load calculations - bool _autorotating; // flag that holds the status of autorotation - bool _bailing_out; // flag that holds the status of bail out(power engagement) float _idle_throttle; // current idle throttle setting - bool _gov_bailing_out; // flag that holds the status of governor bail out + + RotorControlState _rsc_state; // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void update_rotor_ramp(float rotor_ramp_input, float dt); @@ -212,5 +172,5 @@ class AP_MotorsHeli_RSC { float get_idle_output() const { return _idle_output * 0.01; } float get_governor_torque() const { return _governor_torque * 0.01; } float get_governor_compensator() const { return _governor_compensator * 0.000001; } - float get_arot_idle_output() const { return _arot_idle_output * 0.01; } + }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 24380cc3152fa7..6372363e37a43b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -270,22 +270,6 @@ void AP_MotorsHeli_Single::calculate_armed_scalars() _main_rotor._rsc_mode.save(); _heliflags.save_rsc_mode = false; } - - // allow use of external governor autorotation bailout - if (_heliflags.in_autorotation) { - _main_rotor.set_autorotation_flag(_heliflags.in_autorotation); - // set bailout ramp time - _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); - if (use_tail_RSC()) { - _tail_rotor.set_autorotation_flag(_heliflags.in_autorotation); - _tail_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); - } - } else { - _main_rotor.set_autorotation_flag(false); - if (use_tail_RSC()) { - _tail_rotor.set_autorotation_flag(false); - } - } } // calculate_scalars - recalculates various scalers used. @@ -326,18 +310,12 @@ void AP_MotorsHeli_Single::calculate_scalars() _tail_rotor.set_runup_time(_main_rotor._runup_time.get()); _tail_rotor.set_critical_speed(_main_rotor._critical_speed.get()); _tail_rotor.set_idle_output(_main_rotor._idle_output.get()); - _tail_rotor.set_arot_idle_output(_main_rotor._arot_idle_output.get()); - _tail_rotor.set_rsc_arot_man_enable(_main_rotor._rsc_arot_man_enable.get()); - _tail_rotor.set_rsc_arot_engage_time(_main_rotor._rsc_arot_engage_time.get()); } else { _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_DISABLED); _tail_rotor.set_ramp_time(0); _tail_rotor.set_runup_time(0); _tail_rotor.set_critical_speed(0); _tail_rotor.set_idle_output(0); - _tail_rotor.set_arot_idle_output(0); - _tail_rotor.set_rsc_arot_man_enable(0); - _tail_rotor.set_rsc_arot_engage_time(0); } } @@ -410,10 +388,9 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float } // ensure not below landed/landing collective - if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_heliflags.in_autorotation) { + 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 @@ -469,7 +446,7 @@ float AP_MotorsHeli_Single::get_yaw_offset(float collective) return 0.0; } - if (_heliflags.in_autorotation || (_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; } @@ -656,6 +633,9 @@ bool AP_MotorsHeli_Single::arming_checks(size_t buflen, char *buffer) const // Called from system.cpp void AP_MotorsHeli_Single::heli_motors_param_conversions(void) { + // Run common conversions from base class + AP_MotorsHeli::heli_motors_param_conversions(); + // PARAMETER_CONVERSION - Added: Nov-2023 // Convert trim for DDFP tails // Previous DDFP configs used servo trim for setting the yaw trim, which no longer works with thrust linearisation. Convert servo trim diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 7a3d0ee67d6321..f28db5c9243fbb 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -243,7 +243,7 @@ void setup() ::printf("autorotation only supported by heli frame types, got %i\n", frame_class); exit(1); } - motors_heli->set_in_autorotation(!is_zero(value)); + motors_heli->set_autorotation_active(!is_zero(value)); } else { ::printf("Expected \"frame_class\", \"yaw_headroom\" or \"throttle_avg_max\"\n"); From 6fcc6cd47dc456945ca65ecdb5f8570c97c40528 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sun, 4 Aug 2024 18:32:47 +0100 Subject: [PATCH 2/8] Copter: Heli: simplify autorotation mode change and support RSC autorotation state --- ArduCopter/Copter.h | 1 - ArduCopter/heli.cpp | 34 +++--- ArduCopter/mode.cpp | 20 +--- ArduCopter/mode.h | 10 +- ArduCopter/mode_autorotate.cpp | 111 +++++------------- .../AC_AttitudeControl_Heli.cpp | 2 +- 6 files changed, 49 insertions(+), 129 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5718fc11f4398c..9915885060a689 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -390,7 +390,6 @@ class Copter : public AP_Vehicle { // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, Mode *flightmode; - Mode::Number prev_control_mode; RCMapper rcmap; diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 7b4870de5a0cc7..bff2a3de170419 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -189,29 +189,25 @@ void Copter::heli_update_rotor_speed_targets() // to autorotation flight mode if manual collective is not being used. void Copter::heli_update_autorotation() { - // check if flying and interlock disengaged - if (!ap.land_complete && !motors->get_interlock()) { + bool in_autorotation_mode = false; #if MODE_AUTOROTATE_ENABLED - if (g2.arot.is_enable()) { - if (!flightmode->has_manual_throttle()) { - // set autonomous autorotation flight mode - set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START); - } - // set flag to facilitate both auto and manual autorotations - motors->set_in_autorotation(true); - motors->set_enable_bailout(true); - } + in_autorotation_mode = flightmode == &mode_autorotate; #endif - if (flightmode->has_manual_throttle() && motors->arot_man_enabled()) { - // set flag to facilitate both auto and manual autorotations - motors->set_in_autorotation(true); - motors->set_enable_bailout(true); - } - } else { - motors->set_in_autorotation(false); - motors->set_enable_bailout(false); + + // If we have landed then we do not want to be in autorotation and we do not want to via the bailout state + if (ap.land_complete || ap.land_complete_maybe) { + motors->force_deactivate_autorotation(); + return; } + // if we got this far we are flying, check for conditions to set autorotation state + if (!motors->get_interlock() && (flightmode->has_manual_throttle() || in_autorotation_mode)) { + // set state in motors to facilitate manual and assisted autorotations + motors->set_autorotation_active(true); + } else { + // deactivate the autorotation state via the bailout case + motors->set_autorotation_active(false); + } } // update collective low flag. Use a debounce time of 400 milliseconds. diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 74e3cbb3c043a5..5d8fc5a9e40bbb 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -292,20 +292,9 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) #if FRAME_CONFIG == HELI_FRAME // do not allow helis to enter a non-manual throttle mode if the // rotor runup is not complete - if (!ignore_checks && !new_flightmode->has_manual_throttle() && - (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) { - #if MODE_AUTOROTATE_ENABLED - //if the mode being exited is the autorotation mode allow mode change despite rotor not being at - //full speed. This will reduce altitude loss on bail-outs back to non-manual throttle modes - bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate); - #else - bool in_autorotation_check = false; - #endif - - if (!in_autorotation_check) { - mode_change_failed(new_flightmode, "runup not complete"); - return false; - } + if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()) { + mode_change_failed(new_flightmode, "runup not complete"); + return false; } #endif @@ -369,9 +358,6 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) // perform any cleanup required by previous flight mode exit_mode(flightmode, new_flightmode); - // store previous flight mode (only used by tradeheli's autorotation) - prev_control_mode = flightmode->mode_number(); - // update flight mode flightmode = new_flightmode; control_mode_reason = reason; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 98f01449c51f01..faf22b0180f45c 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -2015,18 +2015,14 @@ class ModeAutorotate : public Mode { int32_t _pitch_target; // Target pitch attitude to pass to attitude controller uint32_t _entry_time_start_ms; // Time remaining until entry phase moves on to glide phase float _hs_decay; // The head accerleration during the entry phase - float _bail_time; // Timer for exiting the bail out phase (s) - uint32_t _bail_time_start_ms; // Time at start of bail out - float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase - float _target_pitch_adjust; // Target pitch rate used during bail out phase enum class Autorotation_Phase { ENTRY, SS_GLIDE, FLARE, TOUCH_DOWN, - BAIL_OUT } phase_switch; - + LANDED } phase_switch; + enum class Navigation_Decision { USER_CONTROL_STABILISED, STRAIGHT_AHEAD, @@ -2039,10 +2035,10 @@ class ModeAutorotate : public Mode { bool ss_glide_initial : 1; bool flare_initial : 1; bool touch_down_initial : 1; + bool landed_initial : 1; bool straight_ahead_initial : 1; bool level_initial : 1; bool break_initial : 1; - bool bail_out_initial : 1; bool bad_rpm : 1; } _flags; diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index cc97d75f308f33..e13bc52516251e 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -14,8 +14,8 @@ #if MODE_AUTOROTATE_ENABLED #define AUTOROTATE_ENTRY_TIME 2.0f // (s) number of seconds that the entry phase operates for -#define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single #define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed (unit: -) +#define AUTOROTATION_MIN_MOVING_SPEED 100.0 // (cm/s) minimum speed used for "is moving" check bool ModeAutorotate::init(bool ignore_checks) { @@ -24,15 +24,16 @@ bool ModeAutorotate::init(bool ignore_checks) return false; #endif - // Check that mode is enabled + // Check that mode is enabled, make sure this is the first check as this is the most + // important thing for users to fix if they are planning to use autorotation mode if (!g2.arot.is_enable()) { - gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Not Enabled"); + gcs().send_text(MAV_SEVERITY_WARNING, "Autorot Mode Not Enabled"); return false; } - // Check that interlock is disengaged - if (motors->get_interlock()) { - gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Change Fail: Interlock Engaged"); + // Must be armed to use mode, prevent triggering state machine on the ground + if (!motors->armed() || copter.ap.land_complete || copter.ap.land_complete_maybe) { + gcs().send_text(MAV_SEVERITY_WARNING, "Autorot: Must be Armed and Flying"); return false; } @@ -52,10 +53,10 @@ bool ModeAutorotate::init(bool ignore_checks) _flags.ss_glide_initial = true; _flags.flare_initial = true; _flags.touch_down_initial = true; + _flags.landed_initial = true; _flags.level_initial = true; _flags.break_initial = true; _flags.straight_ahead_initial = true; - _flags.bail_out_initial = true; _msg_flags.bad_rpm = true; // Setting default starting switches @@ -74,20 +75,9 @@ bool ModeAutorotate::init(bool ignore_checks) void ModeAutorotate::run() { - // Check if interlock becomes engaged - if (motors->get_interlock() && !copter.ap.land_complete) { - phase_switch = Autorotation_Phase::BAIL_OUT; - } else if (motors->get_interlock() && copter.ap.land_complete) { - // Aircraft is landed and no need to bail out - set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT); - } - // Current time uint32_t now = millis(); //milliseconds - // Initialise internal variables - float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent - //---------------------------------------------------------------- // State machine logic //---------------------------------------------------------------- @@ -97,12 +87,22 @@ void ModeAutorotate::run() // Timer from entry phase to progress to glide phase if (phase_switch == Autorotation_Phase::ENTRY){ - if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME) { // Flight phase can be progressed to steady state glide phase_switch = Autorotation_Phase::SS_GLIDE; } + } + + // Check if we believe we have landed. We need the landed state to zero all controls and make sure that the copter landing detector will trip + bool speed_check = inertial_nav.get_velocity_z_up_cms() < AUTOROTATION_MIN_MOVING_SPEED && + inertial_nav.get_speed_xy_cms() < AUTOROTATION_MIN_MOVING_SPEED; + if (motors->get_below_land_min_coll() && AP::ins().is_still() && speed_check) { + phase_switch = Autorotation_Phase::LANDED; + } + // Check if we are bailing out and need to re-set the spool state + if (motors->autorotation_bailout()) { + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } @@ -199,79 +199,22 @@ void ModeAutorotate::run() { break; } - - case Autorotation_Phase::BAIL_OUT: + case Autorotation_Phase::LANDED: { - if (_flags.bail_out_initial == true) { - // Functions and settings to be done once are done here. + // Entry phase functions to be run only once + if (_flags.landed_initial == true) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); + gcs().send_text(MAV_SEVERITY_INFO, "Landed"); #endif - - // Set bail out timer remaining equal to the parameter value, bailout time - // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME. - _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f); - - // Set bail out start time - _bail_time_start_ms = now; - - // Set initial target vertical speed - _desired_v_z = curr_vel_z; - - // Initialise position and desired velocity - if (!pos_control->is_active_z()) { - pos_control->relax_z_controller(g2.arot.get_last_collective()); - } - - // Get pilot parameter limits - const float pilot_spd_dn = -get_pilot_speed_dn(); - const float pilot_spd_up = g.pilot_speed_up; - - float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up); - - // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick. - _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time - - // Calculate pitch target adjustment rate to return to level - _target_pitch_adjust = _pitch_target/_bail_time; - - // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); - pos_control->set_correction_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); - - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - _flags.bail_out_initial = false; + _flags.landed_initial = false; } - - if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) { - // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed - _desired_v_z -= _target_climb_rate_adjust*G_Dt; - _pitch_target -= _target_pitch_adjust*G_Dt; - } - // Set position controller - pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z); - - // Update controllers - pos_control->update_z_controller(); - - if ((now - _bail_time_start_ms)/1000.0f >= _bail_time) { - // Bail out timer complete. Change flight mode. Do not revert back to auto. Prevent aircraft - // from continuing mission and potentially flying further away after a power failure. - if (copter.prev_control_mode == Mode::Number::AUTO) { - set_mode(Mode::Number::ALT_HOLD, ModeReason::AUTOROTATION_BAILOUT); - } else { - set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT); - } - } - - break; + // don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly + _pitch_target *= 0.95; + break; } } - switch (nav_pos_switch) { case Navigation_Decision::USER_CONTROL_STABILISED: diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 3b5028ad4be52e..f44a7bca2d4d71 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -540,7 +540,7 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang update_althold_lean_angle_max(throttle_in); _motors.set_throttle_filter_cutoff(filter_cutoff); - if (apply_angle_boost && !((AP_MotorsHeli&)_motors).get_in_autorotation()) { + if (apply_angle_boost && !((AP_MotorsHeli&)_motors).in_autorotation()) { // Apply angle boost throttle_in = get_throttle_boosted(throttle_in); } else { From d07cbaf8835020ad00fd3ef9d15bbd7143d84361 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Aug 2024 16:43:40 +0100 Subject: [PATCH 3/8] AC_Autorotation: Remove bailout case --- libraries/AC_Autorotation/AC_Autorotation.cpp | 19 ++++--------------- libraries/AC_Autorotation/AC_Autorotation.h | 2 -- 2 files changed, 4 insertions(+), 17 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 627dd7894ec317..1dbcccd29423ad 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -3,9 +3,7 @@ #include #include -//Autorotation controller defaults -#define AROT_BAIL_OUT_TIME 2.0f // Default time for bail out controller to run (unit: s) - +// Autorotation controller defaults // Head Speed (HS) controller specific default definitions #define HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz) #define HS_CONTROLLER_HEADSPEED_P 0.7f // Default P gain for head speed controller (unit: -) @@ -81,15 +79,6 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @User: Advanced AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, FWD_SPD_CONTROLLER_MAX_ACCEL), - // @Param: BAIL_TIME - // @DisplayName: Bail Out Timer - // @Description: Time in seconds from bail out initiated to the exit of autorotation flight mode. - // @Units: s - // @Range: 0.5 4 - // @Increment: 0.1 - // @User: Advanced - AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, AROT_BAIL_OUT_TIME), - // @Param: HS_SENSOR // @DisplayName: Main Rotor RPM Sensor // @Description: Allocate the RPM sensor instance to use for measuring head speed. RPM1 = 0. RPM2 = 1. @@ -97,7 +86,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.5 3 // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("HS_SENSOR", 9, AC_Autorotation, _param_rpm_instance, 0), + AP_GROUPINFO("HS_SENSOR", 8, AC_Autorotation, _param_rpm_instance, 0), // @Param: FW_V_P // @DisplayName: Velocity (horizontal) P gain @@ -105,7 +94,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.1 6.0 // @Increment: 0.1 // @User: Advanced - AP_SUBGROUPINFO(_p_fw_vel, "FW_V_", 10, AC_Autorotation, AC_P), + AP_SUBGROUPINFO(_p_fw_vel, "FW_V_", 9, AC_Autorotation, AC_P), // @Param: FW_V_FF // @DisplayName: Velocity (horizontal) feed forward @@ -113,7 +102,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0 1 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), + AP_GROUPINFO("FW_V_FF", 10, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), AP_GROUPEND }; diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index d5cfa52097b3f1..612986662ecf47 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -28,7 +28,6 @@ class AC_Autorotation int16_t get_hs_set_point(void) { return _param_head_speed_set_point; } float get_col_entry_freq(void) { return _param_col_entry_cutoff_freq; } float get_col_glide_freq(void) { return _param_col_glide_cutoff_freq; } - float get_bail_time(void) { return _param_bail_time; } float get_last_collective() const { return _collective_out; } bool is_enable(void) { return _param_enable; } void Log_Write_Autorotation(void) const; @@ -81,7 +80,6 @@ class AC_Autorotation AP_Float _param_col_entry_cutoff_freq; AP_Float _param_col_glide_cutoff_freq; AP_Int16 _param_accel_max; - AP_Float _param_bail_time; AP_Int8 _param_rpm_instance; AP_Float _param_fwd_k_ff; From 91d2a47648be8f7a9dd1fe698e91f8372ddab639 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Aug 2024 20:57:48 +0100 Subject: [PATCH 4/8] Sub: correct comment on prev control mode --- ArduSub/mode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index d445dd5c8c16d5..cb3b10c53c1f88 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -112,7 +112,7 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason) // perform any cleanup required by previous flight mode exit_mode(flightmode, new_flightmode); - // store previous flight mode (only used by tradeheli's autorotation) + // store previous flight mode prev_control_mode = control_mode; // update flight mode From 833c4a56e512078564931e9c309712731c3242de Mon Sep 17 00:00:00 2001 From: MattKear Date: Mon, 7 Oct 2024 19:05:29 +0100 Subject: [PATCH 5/8] Autotest: Add method for check servo channel in range --- Tools/autotest/vehicle_test_suite.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 08b179380d4c2e..3cab908b5951cc 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7962,6 +7962,28 @@ def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operato if comparator(m_value, value): return m_value + def wait_servo_channel_in_range(self, channel, v_min, v_max, timeout=2): + """wait for channel value to be within acceptable range""" + channel_field = "servo%u_raw" % channel + tstart = self.get_sim_time() + while True: + remaining = timeout - (self.get_sim_time_cached() - tstart) + if remaining <= 0: + raise NotAchievedException("Channel value condition not met") + m = self.mav.recv_match(type='SERVO_OUTPUT_RAW', + blocking=True, + timeout=remaining) + if m is None: + continue + m_value = getattr(m, channel_field, None) + if m_value is None: + raise ValueError("message (%s) has no field %s" % + (str(m), channel_field)) + self.progress("want %u <= SERVO_OUTPUT_RAW.%s <= %u, got value = %u" % + (v_min, channel_field, v_max, m_value)) + if (v_min <= m_value) and (m_value <= v_max): + return m_value + def assert_servo_channel_value(self, channel, value, comparator=operator.eq): """assert channel value (default condition is equality)""" channel_field = "servo%u_raw" % channel From e25d78e242bfd06418add1bfa381cd57cffeab94 Mon Sep 17 00:00:00 2001 From: MattKear Date: Mon, 7 Oct 2024 19:10:03 +0100 Subject: [PATCH 6/8] Autotest: Heli: minor improvements to TurbineCoolDown --- Tools/autotest/helicopter.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 9f04327e4d1170..d06b97f6906604 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -700,25 +700,27 @@ def check_airspeeds(mav, m): def TurbineCoolDown(self, timeout=200): """Check Turbine Cool Down Feature""" + self.context_push() # set option for Turbine RAMP_TIME = 4 SETPOINT = 66 IDLE = 15 COOLDOWN_TIME = 5 - self.set_parameter("RC6_OPTION", 161) - self.set_parameter("H_RSC_RAMP_TIME", RAMP_TIME) - self.set_parameter("H_RSC_SETPOINT", SETPOINT) - self.set_parameter("H_RSC_IDLE", IDLE) - self.set_parameter("H_RSC_CLDWN_TIME", COOLDOWN_TIME) + self.set_parameters({"RC6_OPTION": 161, + "H_RSC_RAMP_TIME": RAMP_TIME, + "H_RSC_SETPOINT": SETPOINT, + "H_RSC_IDLE": IDLE, + "H_RSC_CLDWN_TIME": COOLDOWN_TIME}) self.set_rc(3, 1000) self.set_rc(8, 1000) self.progress("Starting turbine") self.wait_ready_to_arm() + self.context_collect("STATUSTEXT") self.arm_vehicle() self.set_rc(6, 2000) - self.wait_statustext('Turbine startup') + self.wait_statustext('Turbine startup', check_context=True) # Engage interlock to run up to head speed self.set_rc(8, 2000) @@ -743,6 +745,7 @@ def TurbineCoolDown(self, timeout=200): self.set_rc(6, 1000) self.wait_disarmed(timeout=20) + self.context_pop() def TurbineStart(self, timeout=200): """Check Turbine Start Feature""" From a18f3abd422e6217c3b7a2259b62c06d20ea0b07 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sat, 10 Aug 2024 17:22:40 +0100 Subject: [PATCH 7/8] Autotest: Update Autorotation tests for new mode change and bailout methods --- Tools/autotest/helicopter.py | 216 ++++++++++++++++++++++++----------- 1 file changed, 152 insertions(+), 64 deletions(-) diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index d06b97f6906604..9f3f57cc4ddaf0 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -302,11 +302,15 @@ def SplineWaypoint(self, timeout=600): self.progress("Lowering rotor speed") self.set_rc(8, 1000) - def AutoRotation(self, timeout=600): + def Autorotation(self, timeout=600): """Check engine-out behaviour""" - self.set_parameter("AROT_ENABLE", 1) + self.context_push() start_alt = 100 # metres - self.set_parameter("PILOT_TKOFF_ALT", start_alt * 100) + self.set_parameters({ + "AROT_ENABLE": 1, + "H_RSC_AROT_ENBL": 1, + }) + bail_out_time = self.get_parameter('H_RSC_AROT_RUNUP') self.change_mode('POSHOLD') self.set_rc(3, 1000) self.set_rc(8, 1000) @@ -322,85 +326,169 @@ def AutoRotation(self, timeout=600): relative=True, timeout=timeout) self.context_collect('STATUSTEXT') - self.progress("Triggering autorotate by raising interlock") - self.set_rc(3, 1000) + + # Reset collective to enter hover + self.set_rc(3, 1500) + + # Change to the autorotation flight mode + self.progress("Triggering autorotate mode") + self.change_mode('AUTOROTATE') + self.delay_sim_time(2) + + # Disengage the interlock to remove power self.set_rc(8, 1000) + # Ensure we have progressed through the mode's state machine self.wait_statustext("SS Glide Phase", check_context=True) - self.change_mode('STABILIZE') + self.progress("Testing bailout from autorotation") + self.set_rc(8, 2000) + # See if the output ramps to a value close to expected with the prescribed time + self.wait_servo_channel_value(8, 1659, timeout=bail_out_time+1, comparator=operator.ge) + + # Successfully bailed out, disengage the interlock and allow autorotation to progress + self.set_rc(8, 1000) self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s", check_context=True, regex=True) speed = float(self.re_match.group(1)) if speed > 30: raise NotAchievedException("Hit too hard") + + # Set throttle low to trip auto disarm + self.set_rc(3, 1000) + self.wait_disarmed() + self.context_pop() - def ManAutoRotation(self, timeout=600): + def ManAutorotation(self, timeout=600): """Check autorotation power recovery behaviour""" - RAMP_TIME = 4 - AROT_RAMP_TIME = 2 - start_alt = 100 # metres - self.set_parameters({ - "H_RSC_AROT_MN_EN": 1, - "H_RSC_AROT_ENG_T": AROT_RAMP_TIME, - "H_RSC_AROT_IDLE": 20, - "H_RSC_RAMP_TIME": RAMP_TIME, - "H_RSC_IDLE": 0, - "PILOT_TKOFF_ALT": start_alt * 100, - }) - self.change_mode('POSHOLD') - self.set_rc(3, 1000) - self.set_rc(8, 1000) - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_rc(8, 2000) - self.progress("wait for rotor runup to complete") - self.wait_servo_channel_value(8, 1659, timeout=10) - self.delay_sim_time(20) - self.set_rc(3, 2000) - self.wait_altitude(start_alt - 1, - (start_alt + 5), - relative=True, - timeout=timeout) - self.context_collect('STATUSTEXT') - self.change_mode('STABILIZE') - self.progress("Triggering manual autorotation by disabling interlock") - self.set_rc(3, 1000) - self.set_rc(8, 1000) - self.wait_servo_channel_value(8, 1199, timeout=3) - self.progress("channel 8 set to autorotation window") + RSC_CHAN = 8 + + def check_rsc_output(self, throttle, timeout): + # Check we get a sensible throttle output + expected_pwm = int(throttle * 0.01 * 1000 + 1000) + + # Help out the detection by accepting some margin + margin = 2 + + # See if the output ramps to a value close to expected with the prescribed time + self.wait_servo_channel_in_range(RSC_CHAN, expected_pwm-margin, expected_pwm+margin, timeout=timeout) + + def TestAutorotationConfig(self, rsc_idle, arot_ramp_time, arot_idle, cool_down): + RAMP_TIME = 10 + RUNUP_TIME = 15 + AROT_RUNUP_TIME = arot_ramp_time + 4 + RSC_SETPOINT = 66 + self.set_parameters({ + "H_RSC_AROT_ENBL": 1, + "H_RSC_AROT_RAMP": arot_ramp_time, + "H_RSC_AROT_RUNUP": AROT_RUNUP_TIME, + "H_RSC_AROT_IDLE": arot_idle, + "H_RSC_RAMP_TIME": RAMP_TIME, + "H_RSC_RUNUP_TIME": RUNUP_TIME, + "H_RSC_IDLE": rsc_idle, + "H_RSC_SETPOINT": RSC_SETPOINT, + "H_RSC_CLDWN_TIME": cool_down + }) - # wait to establish autorotation - self.delay_sim_time(2) + # Check the RSC config so we know what to expect on the throttle output + if self.get_parameter("H_RSC_MODE") != 2: + self.set_parameter("H_RSC_MODE", 2) + self.reboot_sitl() - self.set_rc(8, 2000) - self.wait_servo_channel_value(8, 1659, timeout=AROT_RAMP_TIME * 1.1) + self.change_mode('POSHOLD') + self.set_rc(3, 1000) + self.set_rc(8, 1000) + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_rc(8, 2000) + self.progress("wait for rotor runup to complete") + check_rsc_output(self, RSC_SETPOINT, RUNUP_TIME+1) + + self.delay_sim_time(20) + self.set_rc(3, 2000) + self.wait_altitude(100, + 105, + relative=True, + timeout=timeout) + self.context_collect('STATUSTEXT') + self.change_mode('STABILIZE') - # give time for engine to power up - self.set_rc(3, 1400) - self.delay_sim_time(2) + self.progress("Triggering manual autorotation by disabling interlock") + self.set_rc(3, 1000) + self.set_rc(8, 1000) - self.progress("in-flight power recovery") - self.set_rc(3, 1500) - self.delay_sim_time(5) + self.wait_statustext(r"RSC: In Autorotation", check_context=True) - # initiate autorotation again - self.set_rc(3, 1000) - self.set_rc(8, 1000) + # Check we are using the correct throttle output. This should happen instantly on ramp down. + idle_thr = rsc_idle + if (arot_idle > 0): + idle_thr = arot_idle - self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s", - check_context=True, - regex=True) - speed = float(self.re_match.group(1)) - if speed > 30: - raise NotAchievedException("Hit too hard") + check_rsc_output(self, idle_thr, 1) - self.set_rc(3, 1000) - # verify servo 8 resets to RSC_IDLE after land complete - self.wait_servo_channel_value(8, 1000, timeout=3) - self.wait_disarmed() + self.progress("RSC is outputting correct idle throttle") + + # Wait to establish autorotation. + self.delay_sim_time(2) + + # Re-engage interlock to start bailout sequence + self.set_rc(8, 2000) + + # Ensure we see the bailout state + self.wait_statustext("RSC: Bailing Out", check_context=True) + + # Check we are back up to flight throttle. Autorotation ramp up time should be used + check_rsc_output(self, RSC_SETPOINT, arot_ramp_time+1) + + # Give time for engine to power up + self.set_rc(3, 1400) + self.delay_sim_time(2) + + self.progress("in-flight power recovery") + self.set_rc(3, 1500) + self.delay_sim_time(5) + + # Initiate autorotation again + self.set_rc(3, 1000) + self.set_rc(8, 1000) + + self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s", + check_context=True, + regex=True) + speed = float(self.re_match.group(1)) + if speed > 30: + raise NotAchievedException("Hit too hard") + + # Check that cool down is still used correctly if set + # First wait until we are out of the autorotation state + self.wait_statustext("RSC: Autorotation Stopped") + if (cool_down > 0): + check_rsc_output(self, rsc_idle*1.5, cool_down) + + # Verify RSC output resets to RSC_IDLE after land complete + check_rsc_output(self, rsc_idle, 20) + self.wait_disarmed() + + # We test the bailout behavior of two different configs + # First we test config with a regular throttle curve + self.progress("testing autorotation with throttle curve config") + self.context_push() + TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=0, cool_down=0) + + # Now we test a config that would be used with an ESC with internal governor and an autorotation window + self.progress("testing autorotation with ESC autorotation window config") + TestAutorotationConfig(self, rsc_idle=0.0, arot_ramp_time=0.0, arot_idle=20.0, cool_down=0) + + # Check rsc output behavior when using the cool down feature + self.progress("testing autorotation with cool down enabled and zero autorotation idle") + TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=0, cool_down=5.0) + + self.progress("testing that H_RSC_AROT_IDLE is used over RSC_IDLE when cool down is enabled") + TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=10, cool_down=5.0) + + self.context_pop() def mission_item_home(self, target_system, target_component): '''returns a mission_item_int which can be used as home in a mission''' @@ -1024,8 +1112,8 @@ def tests(self): self.PosHoldTakeOff, self.StabilizeTakeOff, self.SplineWaypoint, - self.AutoRotation, - self.ManAutoRotation, + self.Autorotation, + self.ManAutorotation, self.governortest, self.FlyEachFrame, self.AirspeedDrivers, From 039ca25fbdf53a6cd4d3fc8879c9bbf5f29f3a69 Mon Sep 17 00:00:00 2001 From: MattKear Date: Sat, 21 Sep 2024 02:18:26 +0100 Subject: [PATCH 8/8] AC_Autorotation: Add RSC_Autorotation class --- .../AC_Autorotation/RSC_Autorotation.cpp | 159 ++++++++++++++++++ libraries/AC_Autorotation/RSC_Autorotation.h | 52 ++++++ 2 files changed, 211 insertions(+) create mode 100644 libraries/AC_Autorotation/RSC_Autorotation.cpp create mode 100644 libraries/AC_Autorotation/RSC_Autorotation.h diff --git a/libraries/AC_Autorotation/RSC_Autorotation.cpp b/libraries/AC_Autorotation/RSC_Autorotation.cpp new file mode 100644 index 00000000000000..53992de1a0467e --- /dev/null +++ b/libraries/AC_Autorotation/RSC_Autorotation.cpp @@ -0,0 +1,159 @@ +#include "RSC_Autorotation.h" +#include +#include + +#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; +} diff --git a/libraries/AC_Autorotation/RSC_Autorotation.h b/libraries/AC_Autorotation/RSC_Autorotation.h new file mode 100644 index 00000000000000..8c0bd230a7849f --- /dev/null +++ b/libraries/AC_Autorotation/RSC_Autorotation.h @@ -0,0 +1,52 @@ +// Class supporting autorotation state within the heli rotor speed controller + +#pragma once + +#include + +// 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" + +};