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 - add support for rotor speed governor with droop speed control

Browse Source
This commit is contained in:
ChristopherOlson 2019-02-03 17:19:13 -06:00 committed by Randy Mackay
parent 9d03b44cf7
commit cca58e393a
10 changed files with 179 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -77,7 +77,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @Param: RSC_MODE
// @DisplayName: Rotor Speed Control Mode
// @Description: Determines the method of rotor speed control
// @Values: 1:Ch8 Input, 2:SetPoint, 3:Throttle Curve
// @Values: 1:Ch8 Input, 2:SetPoint, 3:Throttle Curve, 4:Governor
// @User: Standard
AP_GROUPINFO("RSC_MODE", 8, AP_MotorsHeli, _rsc_mode, (int8_t)ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH),
@ -193,6 +193,38 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @User: Standard
AP_GROUPINFO("RSC_THRCRV_100", 24, AP_MotorsHeli, _rsc_thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT),
// @Param: RSC_GOV_SET
// @DisplayName: Governor RPM Setting
// @Description: Main rotor rpm setting that governor maintains when engaged
// @Range: 800 3500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_GOV_SET", 25, AP_MotorsHeli, _rsc_governor_setpoint, AP_MOTORS_HELI_RSC_GOVERNOR_SET_DEFAULT),
// @Param: RSC_GOV_DISGAG
// @DisplayName: Throttle Percentage for Governor Disengage
// @Description: Percentage of throttle where the governor will disenage to allow return to flight idle power
// @Range: 0 50
// @Increment: 1
// @User: Standard
AP_GROUPINFO("RSC_GOV_DISGAG", 26, AP_MotorsHeli, _rsc_governor_disengage, AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE),
// @Param: RSC_GOV_DROOP
// @DisplayName: Governor Droop 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("RSC_GOV_DROOP", 27, AP_MotorsHeli, _rsc_governor_droop_setting, AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT),
// @Param: RSC_GOV_TC
// @DisplayName: Governor Throttle Curve Gain
// @Description: Percentage of throttle curve gain in governor output
// @Range: 50 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("RSC_GOV_TC", 28, AP_MotorsHeli, _rsc_governor_tc, AP_MOTORS_HELI_RSC_GOVERNOR_TC),
AP_GROUPEND
};

25
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -37,6 +37,12 @@
#define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 500
#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 1000
// 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
// 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
@ -97,13 +103,16 @@ public:
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1)
float get_rsc_setpoint() const { return _rsc_setpoint * 0.001f; }
// set_rpm - for rotor speed governor
virtual void set_rpm(int16_t rotor_rpm) = 0;
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
virtual void set_desired_rotor_speed(float desired_speed) = 0;
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1
virtual float get_desired_rotor_speed() const = 0;
// get_main_rotor_speed - gets estimated or measured main rotor speed
// get_main_rotor_speed - estimated rotor speed when no governor or speed sensor used
virtual float get_main_rotor_speed() const = 0;
// return true if the main rotor is up to speed
@ -112,6 +121,12 @@ public:
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
virtual bool rotor_speed_above_critical() const = 0;
//get rotor governor output
virtual float get_governor_output() const = 0;
//get engine throttle output
virtual float get_control_output() const = 0;
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
virtual uint16_t get_motor_mask() override = 0;
@ -202,8 +217,8 @@ protected:
AP_Int16 _collective_min; // Lowest possible servo position for the swashplate
AP_Int16 _collective_max; // Highest possible servo position for the swashplate
AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner
AP_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv
AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner
AP_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabled
AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active
AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach setpoint
AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
@ -213,6 +228,10 @@ protected:
AP_Int16 _rsc_thrcrv[5]; // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective
AP_Int16 _rsc_slewrate; // throttle slew rate (percentage per second)
AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup
AP_Float _rsc_governor_disengage; // sets the throttle percent where the governor disengages for return to flight idle
AP_Int16 _rsc_governor_setpoint; // sets rotor speed for governor
AP_Float _rsc_governor_droop_setting;// governor response to droop under load
AP_Float _rsc_governor_tc; // governor throttle curve weighting, range 50-100%
// internal variables
float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range

10
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -206,6 +206,12 @@ void AP_MotorsHeli_Dual::set_desired_rotor_speed(float desired_speed)
_rotor.set_desired_speed(desired_speed);
}
// set_rotor_rpm - used for governor with speed sensor
void AP_MotorsHeli_Dual::set_rpm(int16_t rotor_rpm)
{
_rotor.set_rotor_rpm(rotor_rpm);
}
// calculate_armed_scalars
void AP_MotorsHeli_Dual::calculate_armed_scalars()
{
@ -218,6 +224,10 @@ void AP_MotorsHeli_Dual::calculate_armed_scalars()
_rotor.set_critical_speed(_rsc_critical*0.001f);
_rotor.set_idle_output(_rsc_idle_output*0.001f);
_rotor.set_throttle_curve(thrcrv, (uint16_t)_rsc_slewrate.get());
_rotor.set_governor_disengage(_rsc_governor_disengage*0.01f);
_rotor.set_governor_droop_setting(_rsc_governor_droop_setting*0.01f);
_rotor.set_governor_setpoint(_rsc_governor_setpoint);
_rotor.set_governor_tc(_rsc_governor_tc*0.01f);
}
// calculate_scalars

9
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -58,6 +58,9 @@ public:
// output_to_motors - sends values out to the motors
void output_to_motors() override;
// set_rpm - for rotor speed governor
void set_rpm(int16_t rotor_rpm) override;
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(float desired_speed) override;
@ -70,6 +73,12 @@ public:
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
bool rotor_speed_above_critical() const override { return _rotor.get_rotor_speed() > _rotor.get_critical_speed(); }
// get_governor_output
float get_governor_output() const { return _rotor.get_governor_output(); }
// get_control_output
float get_control_output() const { return _rotor.get_control_output(); }
// calculate_scalars - recalculates various scalars used
void calculate_scalars() override;

10
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -96,6 +96,12 @@ void AP_MotorsHeli_Quad::set_desired_rotor_speed(float desired_speed)
_rotor.set_desired_speed(desired_speed);
}
// set_rotor_rpm - used for governor with speed sensor
void AP_MotorsHeli_Quad::set_rpm(int16_t rotor_rpm)
{
_rotor.set_rotor_rpm(rotor_rpm);
}
// calculate_armed_scalars
void AP_MotorsHeli_Quad::calculate_armed_scalars()
{
@ -108,6 +114,10 @@ void AP_MotorsHeli_Quad::calculate_armed_scalars()
_rotor.set_critical_speed(_rsc_critical*0.001f);
_rotor.set_idle_output(_rsc_idle_output*0.001f);
_rotor.set_throttle_curve(thrcrv, (uint16_t)_rsc_slewrate.get());
_rotor.set_governor_disengage(_rsc_governor_disengage*0.01f);
_rotor.set_governor_droop_setting(_rsc_governor_droop_setting*0.01f);
_rotor.set_governor_setpoint(_rsc_governor_setpoint);
_rotor.set_governor_tc(_rsc_governor_tc*0.01f);
}
// calculate_scalars

9
libraries/AP_Motors/AP_MotorsHeli_Quad.h
View File

@ -39,6 +39,9 @@ public:
// output_to_motors - sends values out to the motors
void output_to_motors() override;
// set_rpm - for rotor speed governor
void set_rpm(int16_t rotor_rpm) override;
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(float desired_speed) override;
@ -51,6 +54,12 @@ public:
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
bool rotor_speed_above_critical() const override { return _rotor.get_rotor_speed() > _rotor.get_critical_speed(); }
// get_governor_output
float get_governor_output() const { return _rotor.get_governor_output(); }
// get_control_output
float get_control_output() const { return _rotor.get_control_output(); }
// calculate_scalars - recalculates various scalars used
void calculate_scalars() override;

31
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -87,8 +87,37 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
_control_output = _idle_output + (_rotor_ramp_output * (_desired_speed - _idle_output));
} else if (_control_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) {
// throttle output from throttle curve based on collective position
float desired_throttle = calculate_desired_throttle(_collective_in);
float desired_throttle = calculate_desired_throttle(_collective_in);
_control_output = _idle_output + (_rotor_ramp_output * (desired_throttle - _idle_output));
} else if (_control_mode == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
// 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_engage = true;
_governor_output = governor_droop * _governor_droop_setting;
}
// 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);
} else {
// hold governor output at zero, status is off and use the throttle curve
// this is failover for in-flight failure of the speed sensor
_governor_output = 0.0f;
_governor_engage = false;
_control_output = _idle_output + (_rotor_ramp_output * (desired_throttle - _idle_output));
}
}
break;
}

46
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -49,6 +49,13 @@ public:
float get_idle_output() { return _idle_output; }
void set_idle_output(float idle_output) { _idle_output = idle_output; }
// set engine 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_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; }
// get_desired_speed
float get_desired_speed() const { return _desired_speed; }
@ -58,9 +65,15 @@ public:
// get_control_speed
float get_control_output() const { return _control_output; }
// get_rotor_speed - return estimated or measured rotor speed
// get_rotor_speed - estimated rotor speed when no governor or rpm sensor is used
float get_rotor_speed() const;
// set_rotor_rpm - when speed sensor is available for governor
void set_rotor_rpm(int16_t rotor_rpm) {_rotor_rpm = rotor_rpm; }
// get_governor_output
float get_governor_output() const { return _governor_output; }
// is_runup_complete
bool is_runup_complete() const { return _runup_complete; }
@ -88,18 +101,25 @@ 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 _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
int16_t _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
int16_t _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%
// 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);

12
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -209,6 +209,12 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(float desired_speed)
_tail_rotor.set_desired_speed(_direct_drive_tailspeed*0.001f);
}
// set_rotor_rpm - used for governor with speed sensor
void AP_MotorsHeli_Single::set_rpm(int16_t rotor_rpm)
{
_main_rotor.set_rotor_rpm(rotor_rpm);
}
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single::calculate_armed_scalars()
{
@ -221,7 +227,11 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
_main_rotor.set_critical_speed(_rsc_critical*0.001f);
_main_rotor.set_idle_output(_rsc_idle_output*0.001f);
_main_rotor.set_throttle_curve(thrcrv, (uint16_t)_rsc_slewrate.get());
}
_main_rotor.set_governor_disengage(_rsc_governor_disengage*0.01f);
_main_rotor.set_governor_droop_setting(_rsc_governor_droop_setting*0.01f);
_main_rotor.set_governor_setpoint(_rsc_governor_setpoint);
_main_rotor.set_governor_tc(_rsc_governor_tc*0.01f);
}
// calculate_scalars - recalculates various scalers used.

11
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -60,7 +60,10 @@ public:
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
void set_desired_rotor_speed(float desired_speed) override;
// get_main_rotor_speed - gets estimated or measured main rotor speed
// set_rpm - for rotor speed governor
void set_rpm(int16_t rotor_rpm) override;
// get_main_rotor_speed - estimated rotor speed when no speed sensor or governor is used
float get_main_rotor_speed() const override { return _main_rotor.get_rotor_speed(); }
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1
@ -69,6 +72,12 @@ public:
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
bool rotor_speed_above_critical() const override { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
// get_governor_output
float get_governor_output() const { return _main_rotor.get_governor_output(); }
// get_control_output
float get_control_output() const { return _main_rotor.get_control_output(); }
// calculate_scalars - recalculates various scalars used
void calculate_scalars() override;