Skip to content

Commit

Permalink
AP_Motors: change gov cooldown to apply to all RSC modes
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Nov 17, 2021
1 parent 7600c2a commit fb94127
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 26 deletions.
56 changes: 33 additions & 23 deletions libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,14 +138,14 @@ const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = {
AP_GROUPINFO("AROT_PCT", 18, AP_MotorsHeli_RSC, _ext_gov_arot_pct, 0),
#endif

// @Param: GOV_CLDWN
// @DisplayName: AutoThrottle Cooldown Time
// @Description: Will provide a fast idle for engine cooldown by raising the Ground Idle speed setting by 50% for the number of seconds the timer is set for. A setting of zero disables the fast idle. This feature will only apply after the governor and autothrottle have been engaged (throttle switch on and rotor RPM at least 100% of normal speed). At any time during fast idle, disarming will shut the engine down.
// @Param: CLDWN_TIME
// @DisplayName: Cooldown Time
// @Description: Will provide a fast idle for engine cooldown by raising the Ground Idle speed setting by 50% for the number of seconds the timer is set for. A setting of zero disables the fast idle. This feature will only apply after the runup complete has been declared. At any time during fast idle, disarming will shut the engine down however the timer to declare below critical speed (which triggers ground idle state) will be extended by cooldown time to keep premature engine shutdown after autonomous landing.
// @Range: 0 120
// @Units: s
// @Increment: 1
// @User: Standard
AP_GROUPINFO("GOV_CLDWN", 19, AP_MotorsHeli_RSC, _autothrottle_cooldown, 0),
AP_GROUPINFO("CLDWN_TIME", 19, AP_MotorsHeli_RSC, _cooldown_time, 0),

// @Param: GOV_COMP
// @DisplayName: Governor Torque Compensator
Expand Down Expand Up @@ -225,7 +225,13 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
{
// _rotor_RPM available to the RSC output
const AP_RPM *rpm = AP_RPM::get_singleton();
if (rpm == nullptr || !rpm->get_rpm(0, _rotor_rpm)) {
if (rpm != nullptr) {
if (!rpm->get_rpm(0, _rotor_rpm)) {
// No valid RPM data
_rotor_rpm = -1;
}
} else {
// No RPM because pointer is null
_rotor_rpm = -1;
}

Expand Down Expand Up @@ -261,22 +267,22 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)

// set rotor control speed to engine idle and ensure governor is reset, if used
governor_reset();
_autothrottle = false;
_governor_fault = false;
if (!_autothrottle) {
if (_in_autorotation) {
// if in autorotation and using an external governor, set the output to tell the governor to use bailout ramp
_control_output = constrain_float( _rsc_arot_bailout_pct/100.0f , 0.0f, 0.4f);
if (_in_autorotation) {
// if in autorotation and using an external governor, set the output to tell the governor to use bailout ramp
_control_output = constrain_float( _rsc_arot_bailout_pct/100.0f , 0.0f, 0.4f);
} else {
// set rotor control speed to idle speed parameter, this happens instantly and ignores ramping
if (_cooldown_time > 0) {
_control_output = get_idle_output() * 1.5f;
_fast_idle_timer += dt;
if (_fast_idle_timer > (float)_cooldown_time) {
_fast_idle_timer = 0.0f;
}
} else {
// set rotor control speed to idle speed parameter, this happens instantly and ignores ramping
_control_output = get_idle_output();
}
} else {
_control_output = get_idle_output() * 1.5f;
_autothrottle_fast_idle += dt;
if (_autothrottle_fast_idle > (float)_autothrottle_cooldown) {
_autothrottle = false;
_autothrottle_fast_idle = 0.0f;
}
}
break;

Expand Down Expand Up @@ -429,11 +435,11 @@ void AP_MotorsHeli_RSC::autothrottle_run()
// autothrottle main power loop with governor
if (_governor_engage && !_governor_fault) {
// calculate droop - difference between actual and desired speed
float governor_droop = (_governor_rpm - _rotor_rpm) * get_governor_droop_response();
float governor_droop = ((float)_governor_rpm - _rotor_rpm) * get_governor_droop_response();
_governor_output = governor_droop + ((throttlecurve - _governor_torque_reference) * get_governor_ff());
if (_rotor_rpm < (_governor_rpm - 2.0f)) {
if (_rotor_rpm < ((float)_governor_rpm - 2.0f)) {
_governor_torque_reference += get_governor_compensator(); // torque compensator
} else if (_rotor_rpm > (_governor_rpm + 2.0f)) {
} else if (_rotor_rpm > ((float)_governor_rpm + 2.0f)) {
_governor_torque_reference -= get_governor_compensator();
}
// throttle output uses droop + torque compensation to maintain proper rotor speed
Expand All @@ -460,13 +466,17 @@ void AP_MotorsHeli_RSC::autothrottle_run()
// this limits the max torque rise the governor could call for from the main power loop
if (_rotor_rpm >= (_governor_rpm * 0.5f)) {
float torque_limit = (get_governor_torque() * get_governor_torque());
_governor_output = (_rotor_rpm / _governor_rpm) * torque_limit;
if (_governor_rpm > 0) {
_governor_output = (_rotor_rpm / (float)_governor_rpm) * torque_limit;
} else {
_governor_output = 0.0f;
}
_control_output = constrain_float(get_idle_output() + (_rotor_ramp_output * (throttlecurve + _governor_output - get_idle_output())), 0.0f, 1.0f);
_governor_torque_reference = _control_output; // increment torque setting to be passed to main power loop
if (_rotor_rpm >= (_governor_rpm - 2.0f)) {
if (_rotor_rpm >= ((float)_governor_rpm - 2.0f)) {
_governor_engage = true;
_autothrottle = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "Governor Engaged");
gcs().send_text(MAV_SEVERITY_NOTICE, "Governor Engaged");
}
} else {
// temporary use of throttle curve and ramp timer to accelerate rotor to governor min torque rise speed
Expand Down
6 changes: 3 additions & 3 deletions libraries/AP_Motors/AP_MotorsHeli_RSC.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ class AP_MotorsHeli_RSC {
bool _use_bailout_ramp; // true if allowing RSC to quickly ramp up engine
bool _in_autorotation; // true if vehicle is currently in an autorotation
int16_t _rsc_arot_bailout_pct; // the throttle percentage sent to the external governor to signal that autorotation bailout ramp should be used
float _autothrottle_fast_idle; // autothrottle variable for cooldown timer
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

Expand All @@ -165,12 +165,12 @@ class AP_MotorsHeli_RSC {
// parameters
AP_Int16 _power_slewrate; // throttle slew rate (percentage per second)
AP_Int16 _thrcrv[5]; // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective
AP_Float _governor_rpm; // governor reference for speed calculations
AP_Int16 _governor_rpm; // governor reference for speed calculations
AP_Float _governor_torque; // governor torque rise setting
AP_Float _governor_compensator; // governor torque compensator variable
AP_Float _governor_droop_response; // governor response to droop under load
AP_Float _governor_ff; // governor feedforward variable
AP_Int16 _autothrottle_cooldown; // autothrottle cooldown time to provide a fast idle
AP_Int16 _cooldown_time; // cooldown time to provide a fast idle

// parameter accessors to allow conversions
float get_critical_speed() const { return _critical_speed * 0.01; }
Expand Down

0 comments on commit fb94127

Please sign in to comment.