mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Motors:Heli_RSC Governor - add method of droop compensation
- improve handling of outliers from speed sensor - change governor_tc variable to governor_thrcurve - remove equals zero from variable declarations
This commit is contained in:
ChristopherOlson
Randy Mackay
parent
4120e29614
commit
b1046c7b80
@ -228,9 +228,10 @@ void AP_MotorsHeli_Dual::calculate_armed_scalars()
|
||||
} else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
|
||||
_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
|
||||
_rotor.set_governor_disengage(_rsc_gov.get_disengage()*0.01f);
|
||||
_rotor.set_governor_droop_setting(_rsc_gov.get_droop_setting()*0.01f);
|
||||
_rotor.set_governor_setpoint(_rsc_gov.get_setpoint());
|
||||
_rotor.set_governor_tc(_rsc_gov.get_tc()*0.01f);
|
||||
_rotor.set_governor_droop_response(_rsc_gov.get_droop_response()*0.01f);
|
||||
_rotor.set_governor_reference(_rsc_gov.get_reference());
|
||||
_rotor.set_governor_range(_rsc_gov.get_range());
|
||||
_rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -118,9 +118,10 @@ void AP_MotorsHeli_Quad::calculate_armed_scalars()
|
||||
} else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
|
||||
_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
|
||||
_rotor.set_governor_disengage(_rsc_gov.get_disengage()*0.01f);
|
||||
_rotor.set_governor_droop_setting(_rsc_gov.get_droop_setting()*0.01f);
|
||||
_rotor.set_governor_setpoint(_rsc_gov.get_setpoint());
|
||||
_rotor.set_governor_tc(_rsc_gov.get_tc()*0.01f);
|
||||
_rotor.set_governor_droop_response(_rsc_gov.get_droop_response()*0.01f);
|
||||
_rotor.set_governor_reference(_rsc_gov.get_reference());
|
||||
_rotor.set_governor_range(_rsc_gov.get_range());
|
||||
_rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -82,12 +82,12 @@ const AP_Param::GroupInfo RSCGovFloatParam::var_info[] = {
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, RSCGovFloatParam, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: SETPNT
|
||||
// @DisplayName: Governor RPM Setting
|
||||
// @DisplayName: Governor RPM Reference Setting
|
||||
// @Description: Main rotor rpm setting that governor maintains when engaged
|
||||
// @Range: 800 3500
|
||||
// @Increment: 10
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SETPNT", 2, RSCGovFloatParam, setpoint, AP_MOTORS_HELI_RSC_GOVERNOR_SET_DEFAULT),
|
||||
AP_GROUPINFO("SETPNT", 2, RSCGovFloatParam, reference, AP_MOTORS_HELI_RSC_GOVERNOR_SETPNT_DEFAULT),
|
||||
|
||||
// @Param: DISGAG
|
||||
// @DisplayName: Throttle Percentage for Governor Disengage
|
||||
@ -95,23 +95,31 @@ const AP_Param::GroupInfo RSCGovFloatParam::var_info[] = {
|
||||
// @Range: 0 50
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("DISGAG", 3, RSCGovFloatParam, disengage, AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE),
|
||||
AP_GROUPINFO("DISGAG", 3, RSCGovFloatParam, disengage, AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE_DEFAULT),
|
||||
|
||||
// @Param: DROOP
|
||||
// @DisplayName: Governor Droop Setting
|
||||
// @DisplayName: Governor Droop Response Setting
|
||||
// @Description: Governor droop response under load, 0-100%. Higher value is quicker response but may cause surging
|
||||
// @Range: 0 100
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("DROOP", 4, RSCGovFloatParam, droop_setting, AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT),
|
||||
AP_GROUPINFO("DROOP", 4, RSCGovFloatParam, droop_response, AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT),
|
||||
|
||||
// @Param: TC
|
||||
// @Param: THRCURVE
|
||||
// @DisplayName: Governor Throttle Curve Gain
|
||||
// @Description: Percentage of throttle curve gain in governor output
|
||||
// @Range: 50 100
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TC", 5, RSCGovFloatParam, tc, AP_MOTORS_HELI_RSC_GOVERNOR_TC),
|
||||
AP_GROUPINFO("THRCURVE", 5, RSCGovFloatParam, thrcurve, AP_MOTORS_HELI_RSC_GOVERNOR_THRCURVE_DEFAULT),
|
||||
|
||||
// @Param: RANGE
|
||||
// @DisplayName: Governor Operational Range
|
||||
// @Description: RPM range +/- governor rpm reference setting where governor is operational. If speed sensor fails or rpm falls outside of this range, the governor will disengage and return to throttle curve. Recommended range is 100
|
||||
// @Range: 50 200
|
||||
// @Increment: 10
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("RANGE", 6, RSCGovFloatParam, range, AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
@ -198,26 +206,30 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
|
||||
// governor provides two modes of throttle control - governor engaged
|
||||
// or throttle curve if governor is out of range or sensor failed
|
||||
float desired_throttle = calculate_desired_throttle(_collective_in);
|
||||
// governor is engaged if within -100/+50 rpm of setpoint
|
||||
if ((_rotor_rpm >= (_governor_setpoint - 100.0f)) && (_rotor_rpm <= (_governor_setpoint + 50.0f))) {
|
||||
float governor_droop = ((_rotor_rpm - _governor_setpoint) * desired_throttle) * -0.01f;
|
||||
// use 33% of governor output for soft-starting governor to rated speed
|
||||
// this will be over-ridden during autorotation bailout via the throttle curve
|
||||
if (_governor_engage && _rotor_rpm < (_governor_setpoint - 40.0f)) {
|
||||
_governor_output = (governor_droop * _governor_droop_setting) * 0.33f;
|
||||
} else {
|
||||
// governor has reached normal flight status, switch governor on and use full governor control
|
||||
// governor is active if within user-set range from reference speed
|
||||
if ((_rotor_rpm >= (_governor_reference - _governor_range)) && (_rotor_rpm <= (_governor_reference + _governor_range))) {
|
||||
float governor_droop = constrain_float(_governor_reference - _rotor_rpm,0.0f,_governor_range);
|
||||
// if rpm has not reached 40% of the operational range from reference speed, governor
|
||||
// remains in pre-engage status, no reference speed compensation due to droop
|
||||
// this provides a soft-start function that engages the governor less aggressively
|
||||
if (_governor_engage && _rotor_rpm < (_governor_reference - (_governor_range * 0.4f))) {
|
||||
_governor_output = ((_rotor_rpm - _governor_reference) * desired_throttle) * _governor_droop_response * -0.01f;
|
||||
} else {
|
||||
// normal flight status, governor fully engaged with reference speed compensation for droop
|
||||
_governor_engage = true;
|
||||
_governor_output = governor_droop * _governor_droop_setting;
|
||||
_governor_output = ((_rotor_rpm - (_governor_reference + governor_droop)) * desired_throttle) * _governor_droop_response * -0.01f;
|
||||
}
|
||||
// check for governor disengage for return to flight idle power
|
||||
if (desired_throttle <= _governor_disengage) {
|
||||
_governor_output = 0.0f;
|
||||
_governor_engage = false;
|
||||
}
|
||||
_control_output = constrain_float(_idle_output + (_rotor_ramp_output * (((desired_throttle * _governor_tc) + _governor_output) - _idle_output)), _idle_output, 1.0f);
|
||||
// throttle output with governor on is constrained from minimum called for from throttle curve
|
||||
// to maximum WOT. This prevents outliers on rpm signal from closing the throttle in flight due
|
||||
// to rpm sensor failure or bad signal quality
|
||||
_control_output = constrain_float(_idle_output + (_rotor_ramp_output * (((desired_throttle * _governor_thrcurve) + _governor_output) - _idle_output)), _idle_output + (_rotor_ramp_output * ((desired_throttle * _governor_thrcurve)) - _idle_output), 1.0f);
|
||||
} else {
|
||||
// hold governor output at zero, status is off and use the throttle curve
|
||||
// hold governor output at zero, engage status is false and use the throttle curve
|
||||
// this is failover for in-flight failure of the speed sensor
|
||||
_governor_output = 0.0f;
|
||||
_governor_engage = false;
|
||||
|
||||
@ -13,10 +13,11 @@
|
||||
#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 100
|
||||
|
||||
// RSC governor defaults
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_SET_DEFAULT 1500
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE 25
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT 30
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_TC 90
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_SETPNT_DEFAULT 1500
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE_DEFAULT 25
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT 30
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_THRCURVE_DEFAULT 90
|
||||
#define AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT 100
|
||||
|
||||
// rotor controller states
|
||||
enum RotorControlState {
|
||||
@ -62,12 +63,13 @@ public:
|
||||
float get_idle_output() { return _idle_output; }
|
||||
void set_idle_output(float idle_output) { _idle_output = idle_output; }
|
||||
|
||||
// set engine governor parameters
|
||||
// set rotor speed governor parameters
|
||||
void set_governor_disengage(float governor_disengage) {_governor_disengage = governor_disengage; }
|
||||
void set_governor_droop_setting(float governor_droop_setting) { _governor_droop_setting = governor_droop_setting; }
|
||||
void set_governor_droop_response(float governor_droop_response) { _governor_droop_response = governor_droop_response; }
|
||||
void set_governor_output(float governor_output) {_governor_output = governor_output; }
|
||||
void set_governor_setpoint(float governor_setpoint) { _governor_setpoint = governor_setpoint; }
|
||||
void set_governor_tc(float governor_tc) {_governor_tc = governor_tc; }
|
||||
void set_governor_reference(float governor_reference) { _governor_reference = governor_reference; }
|
||||
void set_governor_range(float governor_range) { _governor_range = governor_range; }
|
||||
void set_governor_thrcurve(float governor_thrcurve) {_governor_thrcurve = governor_thrcurve; }
|
||||
|
||||
// get_desired_speed
|
||||
float get_desired_speed() const { return _desired_speed; }
|
||||
@ -117,25 +119,26 @@ private:
|
||||
|
||||
// internal variables
|
||||
RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED; // motor control mode, Passthrough or Setpoint
|
||||
float _critical_speed = 0.0f; // rotor speed below which flight is not possible
|
||||
float _idle_output = 0.0f; // motor output idle speed
|
||||
float _desired_speed = 0.0f; // latest desired rotor speed from pilot
|
||||
float _control_output = 0.0f; // latest logic controlled output
|
||||
float _rotor_ramp_output = 0.0f; // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f)
|
||||
float _rotor_runup_output = 0.0f; // scalar used to store status of rotor run-up time (0.0-1.0f)
|
||||
int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed
|
||||
int8_t _runup_time = 0; // time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
|
||||
bool _runup_complete = false; // flag for determining if runup is complete
|
||||
float _thrcrv_poly[4][4]; // spline polynomials for throttle curve interpolation
|
||||
uint16_t _power_slewrate = 0; // slewrate for throttle (percentage per second)
|
||||
float _collective_in; // collective in for throttle curve calculation, range 0-1.0f
|
||||
float _rotor_rpm; // rotor rpm from speed sensor for governor
|
||||
float _governor_disengage = 0.0f; // throttle percentage where governor disenages to allow return to flight idle
|
||||
float _governor_droop_setting = 0.0f; // governor droop setting, range 0-100%
|
||||
float _governor_output = 0.0f; // governor output for rotor speed control
|
||||
float _governor_setpoint = 0.0f; // governor speed setpoint, range 800-3500 rpm
|
||||
bool _governor_engage = false; // RSC governor status flag for soft-start
|
||||
float _governor_tc = 0.0f; // governor throttle curve gain, range 50-100%
|
||||
float _critical_speed; // rotor speed below which flight is not possible
|
||||
float _idle_output; // motor output idle speed
|
||||
float _desired_speed; // latest desired rotor speed from pilot
|
||||
float _control_output; // latest logic controlled output
|
||||
float _rotor_ramp_output; // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f)
|
||||
float _rotor_runup_output; // scalar used to store status of rotor run-up time (0.0-1.0f)
|
||||
int8_t _ramp_time; // time in seconds for the output to the main rotor's ESC to reach full speed
|
||||
int8_t _runup_time; // time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
|
||||
bool _runup_complete; // flag for determining if runup is complete
|
||||
float _thrcrv_poly[4][4]; // spline polynomials for throttle curve interpolation
|
||||
uint16_t _power_slewrate; // slewrate for throttle (percentage per second)
|
||||
float _collective_in; // collective in for throttle curve calculation, range 0-1.0f
|
||||
float _rotor_rpm; // rotor rpm from speed sensor for governor
|
||||
float _governor_disengage; // throttle percentage where governor disenages to allow return to flight idle
|
||||
float _governor_output; // governor output for rotor speed control
|
||||
float _governor_range; // RPM range +/- governor rpm reference setting where governor is operational
|
||||
float _governor_reference; // sets rotor speed for governor
|
||||
float _governor_droop_response; // governor response to droop under load
|
||||
bool _governor_engage; // RSC governor status flag for soft-start
|
||||
float _governor_thrcurve; // governor throttle curve gain, range 50-100%
|
||||
|
||||
// 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);
|
||||
@ -178,16 +181,18 @@ public:
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
void set_gov_enable(int8_t setenable) {enable = setenable; }
|
||||
int16_t get_setpoint() { return setpoint; }
|
||||
int16_t get_reference() { return reference; }
|
||||
float get_range() { return range; }
|
||||
float get_disengage() { return disengage; }
|
||||
float get_droop_setting() { return droop_setting; }
|
||||
float get_tc() { return tc; }
|
||||
float get_droop_response() { return droop_response; }
|
||||
float get_thrcurve() { return thrcurve; }
|
||||
|
||||
private:
|
||||
AP_Int8 enable;
|
||||
AP_Float disengage; // sets the throttle percent where the governor disengages for return to flight idle
|
||||
AP_Int16 setpoint; // sets rotor speed for governor
|
||||
AP_Float droop_setting;// governor response to droop under load
|
||||
AP_Float tc; // governor throttle curve weighting, range 50-100%
|
||||
AP_Int16 reference; // sets rotor speed for governor
|
||||
AP_Float range; // RPM range +/- governor rpm reference setting where governor is operational
|
||||
AP_Float disengage; // sets the throttle percent where the governor disengages for return to flight idle
|
||||
AP_Float droop_response; // governor response to droop under load
|
||||
AP_Float thrcurve; // governor throttle curve weighting, range 50-100%
|
||||
|
||||
};
|
||||
|
||||
@ -231,9 +231,10 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
|
||||
} else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
|
||||
_main_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
|
||||
_main_rotor.set_governor_disengage(_rsc_gov.get_disengage()*0.01f);
|
||||
_main_rotor.set_governor_droop_setting(_rsc_gov.get_droop_setting()*0.01f);
|
||||
_main_rotor.set_governor_setpoint(_rsc_gov.get_setpoint());
|
||||
_main_rotor.set_governor_tc(_rsc_gov.get_tc()*0.01f);
|
||||
_main_rotor.set_governor_droop_response(_rsc_gov.get_droop_response()*0.01f);
|
||||
_main_rotor.set_governor_reference(_rsc_gov.get_reference());
|
||||
_main_rotor.set_governor_range(_rsc_gov.get_range());
|
||||
_main_rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user