Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors:Heli_RSC Governor - add method of droop compensation

Browse Source 
- 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 2019-02-28 19:43:03 -06:00 committed by Randy Mackay
parent 4120e29614
commit b1046c7b80
5 changed files with 82 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -228,9 +228,10 @@ void AP_MotorsHeli_Dual::calculate_armed_scalars()
} else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { } else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv()); _rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
_rotor.set_governor_disengage(_rsc_gov.get_disengage()*0.01f); _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_droop_response(_rsc_gov.get_droop_response()*0.01f);
_rotor.set_governor_setpoint(_rsc_gov.get_setpoint()); _rotor.set_governor_reference(_rsc_gov.get_reference());
_rotor.set_governor_tc(_rsc_gov.get_tc()*0.01f); _rotor.set_governor_range(_rsc_gov.get_range());
_rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f);
} }
} }

7
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -118,9 +118,10 @@ void AP_MotorsHeli_Quad::calculate_armed_scalars()
} else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { } else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv()); _rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
_rotor.set_governor_disengage(_rsc_gov.get_disengage()*0.01f); _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_droop_response(_rsc_gov.get_droop_response()*0.01f);
_rotor.set_governor_setpoint(_rsc_gov.get_setpoint()); _rotor.set_governor_reference(_rsc_gov.get_reference());
_rotor.set_governor_tc(_rsc_gov.get_tc()*0.01f); _rotor.set_governor_range(_rsc_gov.get_range());
_rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f);
} }
} }

50
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -82,12 +82,12 @@ const AP_Param::GroupInfo RSCGovFloatParam::var_info[] = {
AP_GROUPINFO_FLAGS("ENABLE", 1, RSCGovFloatParam, enable, 0, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO_FLAGS("ENABLE", 1, RSCGovFloatParam, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: SETPNT // @Param: SETPNT
// @DisplayName: Governor RPM Setting // @DisplayName: Governor RPM Reference Setting
// @Description: Main rotor rpm setting that governor maintains when engaged // @Description: Main rotor rpm setting that governor maintains when engaged
// @Range: 800 3500 // @Range: 800 3500
// @Increment: 10 // @Increment: 10
// @User: Standard // @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 // @Param: DISGAG
// @DisplayName: Throttle Percentage for Governor Disengage // @DisplayName: Throttle Percentage for Governor Disengage
@ -95,23 +95,31 @@ const AP_Param::GroupInfo RSCGovFloatParam::var_info[] = {
// @Range: 0 50 // @Range: 0 50
// @Increment: 1 // @Increment: 1
// @User: Standard // @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 // @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 // @Description: Governor droop response under load, 0-100%. Higher value is quicker response but may cause surging
// @Range: 0 100 // @Range: 0 100
// @Increment: 1 // @Increment: 1
// @User: Standard // @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 // @DisplayName: Governor Throttle Curve Gain
// @Description: Percentage of throttle curve gain in governor output // @Description: Percentage of throttle curve gain in governor output
// @Range: 50 100 // @Range: 50 100
// @Increment: 1 // @Increment: 1
// @User: Standard // @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 AP_GROUPEND
}; };
@ -198,26 +206,30 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
// governor provides two modes of throttle control - governor engaged // governor provides two modes of throttle control - governor engaged
// or throttle curve if governor is out of range or sensor failed // or throttle curve if governor is out of range or sensor failed
float desired_throttle = calculate_desired_throttle(_collective_in); float desired_throttle = calculate_desired_throttle(_collective_in);
// governor is engaged if within -100/+50 rpm of setpoint // governor is active if within user-set range from reference speed
if ((_rotor_rpm >= (_governor_setpoint - 100.0f)) && (_rotor_rpm <= (_governor_setpoint + 50.0f))) { if ((_rotor_rpm >= (_governor_reference - _governor_range)) && (_rotor_rpm <= (_governor_reference + _governor_range))) {
float governor_droop = ((_rotor_rpm - _governor_setpoint) * desired_throttle) * -0.01f; float governor_droop = constrain_float(_governor_reference - _rotor_rpm,0.0f,_governor_range);
// use 33% of governor output for soft-starting governor to rated speed // if rpm has not reached 40% of the operational range from reference speed, governor
// this will be over-ridden during autorotation bailout via the throttle curve // remains in pre-engage status, no reference speed compensation due to droop
if (_governor_engage && _rotor_rpm < (_governor_setpoint - 40.0f)) { // this provides a soft-start function that engages the governor less aggressively
_governor_output = (governor_droop * _governor_droop_setting) * 0.33f; if (_governor_engage && _rotor_rpm < (_governor_reference - (_governor_range * 0.4f))) {
} else { _governor_output = ((_rotor_rpm - _governor_reference) * desired_throttle) * _governor_droop_response * -0.01f;
// governor has reached normal flight status, switch governor on and use full governor control } else {
// normal flight status, governor fully engaged with reference speed compensation for droop
_governor_engage = true; _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 // check for governor disengage for return to flight idle power
if (desired_throttle <= _governor_disengage) { if (desired_throttle <= _governor_disengage) {
_governor_output = 0.0f; _governor_output = 0.0f;
_governor_engage = false; _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 { } 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 // this is failover for in-flight failure of the speed sensor
_governor_output = 0.0f; _governor_output = 0.0f;
_governor_engage = false; _governor_engage = false;

73
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -13,10 +13,11 @@
#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 100 #define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 100
// RSC governor defaults // RSC governor defaults
#define AP_MOTORS_HELI_RSC_GOVERNOR_SET_DEFAULT 1500 #define AP_MOTORS_HELI_RSC_GOVERNOR_SETPNT_DEFAULT 1500
#define AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE 25 #define AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE_DEFAULT 25
#define AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT 30 #define AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT 30
#define AP_MOTORS_HELI_RSC_GOVERNOR_TC 90 #define AP_MOTORS_HELI_RSC_GOVERNOR_THRCURVE_DEFAULT 90
#define AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT 100
// rotor controller states // rotor controller states
enum RotorControlState { enum RotorControlState {
@ -62,12 +63,13 @@ public:
float get_idle_output() { return _idle_output; } float get_idle_output() { return _idle_output; }
void set_idle_output(float idle_output) { _idle_output = 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_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_output(float governor_output) {_governor_output = governor_output; }
void set_governor_setpoint(float governor_setpoint) { _governor_setpoint = governor_setpoint; } void set_governor_reference(float governor_reference) { _governor_reference = governor_reference; }
void set_governor_tc(float governor_tc) {_governor_tc = governor_tc; } 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 // get_desired_speed
float get_desired_speed() const { return _desired_speed; } float get_desired_speed() const { return _desired_speed; }
@ -117,25 +119,26 @@ private:
// internal variables // internal variables
RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED; // motor control mode, Passthrough or Setpoint 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 _critical_speed; // rotor speed below which flight is not possible
float _idle_output = 0.0f; // motor output idle speed float _idle_output; // motor output idle speed
float _desired_speed = 0.0f; // latest desired rotor speed from pilot float _desired_speed; // latest desired rotor speed from pilot
float _control_output = 0.0f; // latest logic controlled output float _control_output; // 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_ramp_output; // 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) float _rotor_runup_output; // 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 _ramp_time; // 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 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 = false; // flag for determining if runup is complete bool _runup_complete; // flag for determining if runup is complete
float _thrcrv_poly[4][4]; // spline polynomials for throttle curve interpolation float _thrcrv_poly[4][4]; // spline polynomials for throttle curve interpolation
uint16_t _power_slewrate = 0; // slewrate for throttle (percentage per second) uint16_t _power_slewrate; // slewrate for throttle (percentage per second)
float _collective_in; // collective in for throttle curve calculation, range 0-1.0f float _collective_in; // collective in for throttle curve calculation, range 0-1.0f
float _rotor_rpm; // rotor rpm from speed sensor for governor 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_disengage; // 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; // governor output for rotor speed control
float _governor_output = 0.0f; // governor output for rotor speed control float _governor_range; // RPM range +/- governor rpm reference setting where governor is operational
float _governor_setpoint = 0.0f; // governor speed setpoint, range 800-3500 rpm float _governor_reference; // sets rotor speed for governor
bool _governor_engage = false; // RSC governor status flag for soft-start float _governor_droop_response; // governor response to droop under load
float _governor_tc = 0.0f; // governor throttle curve gain, range 50-100% 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 // 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); void update_rotor_ramp(float rotor_ramp_input, float dt);
@ -178,16 +181,18 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
void set_gov_enable(int8_t setenable) {enable = setenable; } 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_disengage() { return disengage; }
float get_droop_setting() { return droop_setting; } float get_droop_response() { return droop_response; }
float get_tc() { return tc; } float get_thrcurve() { return thrcurve; }
private: private:
AP_Int8 enable; AP_Int8 enable;
AP_Float disengage; // sets the throttle percent where the governor disengages for return to flight idle AP_Int16 reference; // sets rotor speed for governor
AP_Int16 setpoint; // sets rotor speed for governor AP_Float range; // RPM range +/- governor rpm reference setting where governor is operational
AP_Float droop_setting;// governor response to droop under load AP_Float disengage; // sets the throttle percent where the governor disengages for return to flight idle
AP_Float tc; // governor throttle curve weighting, range 50-100% AP_Float droop_response; // governor response to droop under load
AP_Float thrcurve; // governor throttle curve weighting, range 50-100%
}; };

7
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -231,9 +231,10 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
} else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { } else if (_rsc_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
_main_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv()); _main_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
_main_rotor.set_governor_disengage(_rsc_gov.get_disengage()*0.01f); _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_droop_response(_rsc_gov.get_droop_response()*0.01f);
_main_rotor.set_governor_setpoint(_rsc_gov.get_setpoint()); _main_rotor.set_governor_reference(_rsc_gov.get_reference());
_main_rotor.set_governor_tc(_rsc_gov.get_tc()*0.01f); _main_rotor.set_governor_range(_rsc_gov.get_range());
_main_rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f);
} }
} }