zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors: Tradheli - implement enable params for throttle curve and governor

mission-4.1.18
bnsgeyer 6 years ago committed by Randy Mackay
parent
51d4029f03
commit
3629541a20
7 changed files with 239 additions and 119 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 91
      libraries/AP_Motors/AP_MotorsHeli.cpp
  2. 22
      libraries/AP_Motors/AP_MotorsHeli.h
  3. 23
      libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
  4. 23
      libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
  5. 110
      libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
  6. 63
      libraries/AP_Motors/AP_MotorsHeli_RSC.h
  7. 26
      libraries/AP_Motors/AP_MotorsHeli_Single.cpp

91
libraries/AP_Motors/AP_MotorsHeli.cpp
Unescape View File

@ -153,77 +153,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { @@ -153,77 +153,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @User: Standard
AP_GROUPINFO("RSC_SLEWRATE", 19, AP_MotorsHeli, _rsc_slewrate, 0),
// @Param: RSC_THRCRV_0
// @DisplayName: Throttle Servo Position for 0 percent collective
// @Description: Throttle Servo Position for 0 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_THRCRV_0", 20, AP_MotorsHeli, _rsc_thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT),
// @Param: RSC_THRCRV_25
// @DisplayName: Throttle Servo Position for 25 percent collective
// @Description: Throttle Servo Position for 25 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_THRCRV_25", 21, AP_MotorsHeli, _rsc_thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT),
// @Param: RSC_THRCRV_50
// @DisplayName: Throttle Servo Position for 50 percent collective
// @Description: Throttle Servo Position for 50 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_THRCRV_50", 22, AP_MotorsHeli, _rsc_thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT),
// @Param: RSC_THRCRV_75
// @DisplayName: Throttle Servo Position for 75 percent collective
// @Description: Throttle Servo Position for 75 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_THRCRV_75", 23, AP_MotorsHeli, _rsc_thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT),
// @Param: RSC_THRCRV_100
// @DisplayName: Throttle Servo Position for 100 percent collective
// @Description: Throttle Servo Position for 100 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_THRCRV_100", 24, AP_MotorsHeli, _rsc_thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT),
// indices 20 to 25 was throttle curve. Do not use this index in the future.
// @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),
// @Group: RSC_CRV_
// @Path: AP_MotorsHeli_RSC.cpp
AP_SUBGROUPINFO(_rsc_thrcrv, "RSC_CRV_", 27, AP_MotorsHeli, RSCThrCrvInt16Param),
// @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),
// @Group: RSC_GOV_
// @Path: AP_MotorsHeli_RSC.cpp
AP_SUBGROUPINFO(_rsc_gov, "RSC_GOV_", 28, AP_MotorsHeli, RSCGovFloatParam),
AP_GROUPEND
};
@ -559,5 +497,18 @@ void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in) @@ -559,5 +497,18 @@ void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in)
SRV_Channels::set_output_pwm_trimmed(function, pwm);
}
// enable_parameters - enables the rsc parameters for the rsc mode
void AP_MotorsHeli::enable_rsc_parameters(void)
{
if (_rsc_mode == (int8_t)ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT || _rsc_mode == (int8_t)ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
_rsc_thrcrv.set_thrcrv_enable(1);
} else {
_rsc_thrcrv.set_thrcrv_enable(0);
}
if (_rsc_mode == (int8_t)ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
_rsc_gov.set_gov_enable(1);
} else {
_rsc_gov.set_gov_enable(0);
}
}

22
libraries/AP_Motors/AP_MotorsHeli.h
Unescape View File

@ -31,17 +31,6 @@ @@ -31,17 +31,6 @@
// RSC output defaults
#define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0
#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 250
#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 320
#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 380
#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
@ -147,6 +136,9 @@ public: @@ -147,6 +136,9 @@ public:
// support passing init_targets_on_arming flag to greater code
bool init_targets_on_arming() const { return _heliflags.init_targets_on_arming; }
void enable_rsc_parameters(void);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -162,6 +154,9 @@ protected: @@ -162,6 +154,9 @@ protected:
SERVO_CONTROL_MODE_MANUAL_OSCILLATE,
};
RSCThrCrvInt16Param _rsc_thrcrv;
RSCGovFloatParam _rsc_gov;
// output - sends commands to the motors
void output_armed_stabilizing() override;
void output_armed_zero_throttle();
@ -225,13 +220,8 @@ protected: @@ -225,13 +220,8 @@ protected:
AP_Int16 _land_collective_min; // Minimum collective when landed or landing
AP_Int16 _rsc_critical; // Rotor speed below which flight is not possible
AP_Int16 _rsc_idle_output; // Rotor control output while at idle
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

23
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
Unescape View File

@ -215,19 +215,23 @@ void AP_MotorsHeli_Dual::set_rpm(int16_t rotor_rpm) @@ -215,19 +215,23 @@ void AP_MotorsHeli_Dual::set_rpm(int16_t rotor_rpm)
// calculate_armed_scalars
void AP_MotorsHeli_Dual::calculate_armed_scalars()
{
float thrcrv[5];
for (uint8_t i = 0; i < 5; i++) {
thrcrv[i]=_rsc_thrcrv[i]*0.001f;
}
// Set common RSC variables
_rotor.set_ramp_time(_rsc_ramp_time);
_rotor.set_runup_time(_rsc_runup_time);
_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);
_rotor.set_slewrate(_rsc_slewrate);
// Set rsc mode specific parameters
if (_rsc_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) {
_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
} 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);
}
}
// calculate_scalars
@ -263,6 +267,7 @@ void AP_MotorsHeli_Dual::calculate_scalars() @@ -263,6 +267,7 @@ void AP_MotorsHeli_Dual::calculate_scalars()
// set mode of main rotor controller and trigger recalculation of scalars
_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
enable_rsc_parameters();
calculate_armed_scalars();
}

23
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
Unescape View File

@ -105,19 +105,23 @@ void AP_MotorsHeli_Quad::set_rpm(int16_t rotor_rpm) @@ -105,19 +105,23 @@ void AP_MotorsHeli_Quad::set_rpm(int16_t rotor_rpm)
// calculate_armed_scalars
void AP_MotorsHeli_Quad::calculate_armed_scalars()
{
float thrcrv[5];
for (uint8_t i = 0; i < 5; i++) {
thrcrv[i]=_rsc_thrcrv[i]*0.001f;
}
// Set common RSC variables
_rotor.set_ramp_time(_rsc_ramp_time);
_rotor.set_runup_time(_rsc_runup_time);
_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);
_rotor.set_slewrate(_rsc_slewrate);
// Set rsc mode specific parameters
if (_rsc_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) {
_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
} 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);
}
}
// calculate_scalars
@ -139,6 +143,7 @@ void AP_MotorsHeli_Quad::calculate_scalars() @@ -139,6 +143,7 @@ void AP_MotorsHeli_Quad::calculate_scalars()
// set mode of main rotor controller and trigger recalculation of scalars
_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
enable_rsc_parameters();
calculate_armed_scalars();
}

110
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
Unescape View File

@ -20,6 +20,112 @@ @@ -20,6 +20,112 @@
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo RSCThrCrvInt16Param::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable settings for RSC Setpoint
// @Description: Automatically set when RSC Setpoint mode is selected. Should not be set manually.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, RSCThrCrvInt16Param, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: 0
// @DisplayName: Throttle Servo Position in percent for 0 percent collective
// @Description: Throttle Servo Position in percent for 0 percent collective. This is on a scale from 0 to 100, where 100 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("0", 2, RSCThrCrvInt16Param, thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT),
// @Param: 25
// @DisplayName: Throttle Servo Position for 25 percent collective
// @Description: Throttle Servo Position for 25 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("25", 3, RSCThrCrvInt16Param, thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT),
// @Param: 50
// @DisplayName: Throttle Servo Position for 50 percent collective
// @Description: Throttle Servo Position for 50 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("50", 4, RSCThrCrvInt16Param, thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT),
// @Param: 75
// @DisplayName: Throttle Servo Position for 75 percent collective
// @Description: Throttle Servo Position for 75 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("75", 5, RSCThrCrvInt16Param, thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT),
// @Param: 100
// @DisplayName: Throttle Servo Position for 100 percent collective
// @Description: Throttle Servo Position for 100 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("100", 6, RSCThrCrvInt16Param, thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT),
AP_GROUPEND
};
const AP_Param::GroupInfo RSCGovFloatParam::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable settings for RSC Governor
// @Description: Automatically set when RSC Governor mode is selected. Should not be set manually.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, RSCGovFloatParam, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: SETPNT
// @DisplayName: Governor RPM 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),
// @Param: 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("DISGAG", 3, RSCGovFloatParam, disengage, AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE),
// @Param: 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("DROOP", 4, RSCGovFloatParam, droop_setting, AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT),
// @Param: TC
// @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_GROUPEND
};
RSCThrCrvInt16Param::RSCThrCrvInt16Param(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
RSCGovFloatParam::RSCGovFloatParam(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
// init_servo - servo initialization on start-up
void AP_MotorsHeli_RSC::init_servo()
{
@ -33,7 +139,7 @@ void AP_MotorsHeli_RSC::init_servo() @@ -33,7 +139,7 @@ void AP_MotorsHeli_RSC::init_servo()
// set_power_output_range
// TODO: Look at possibly calling this at a slower rate. Doesn't need to be called every cycle.
void AP_MotorsHeli_RSC::set_throttle_curve(float thrcrv[5], uint16_t slewrate)
void AP_MotorsHeli_RSC::set_throttle_curve(float thrcrv[5])
{
// Ensure user inputs are within parameter limits
@ -43,7 +149,6 @@ void AP_MotorsHeli_RSC::set_throttle_curve(float thrcrv[5], uint16_t slewrate) @@ -43,7 +149,6 @@ void AP_MotorsHeli_RSC::set_throttle_curve(float thrcrv[5], uint16_t slewrate)
// Calculate the spline polynomials for the throttle curve
splinterp5(thrcrv,_thrcrv_poly);
_power_slewrate = slewrate;
}
// output - update value to send to ESC/Servo
@ -238,4 +343,3 @@ float AP_MotorsHeli_RSC::calculate_desired_throttle(float collective_in) @@ -238,4 +343,3 @@ float AP_MotorsHeli_RSC::calculate_desired_throttle(float collective_in)
return throttle;
}

63
libraries/AP_Motors/AP_MotorsHeli_RSC.h
Unescape View File

@ -5,6 +5,22 @@ @@ -5,6 +5,22 @@
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
// default main rotor speed (ch8 out) as a number from 0 ~ 1000
#define AP_MOTORS_HELI_RSC_SETPOINT 700
// Throttle Curve Defaults
#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 250
#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 320
#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 380
#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
// rotor controller states
enum RotorControlState {
ROTOR_CONTROL_STOP = 0,
@ -83,8 +99,11 @@ public: @@ -83,8 +99,11 @@ public:
// set_runup_time
void set_runup_time(int8_t runup_time) { _runup_time = runup_time; }
// set_slewrate
void set_slewrate(int16_t slewrate) { _power_slewrate = slewrate; }
// set_throttle_curve
void set_throttle_curve(float thrcrv[5], uint16_t slewrate);
void set_throttle_curve(float thrcrv[5]);
// set_collective. collective for throttle curve calculation
void set_collective(float collective) { _collective_in = collective; }
@ -133,3 +152,45 @@ private: @@ -133,3 +152,45 @@ private:
// calculate_desired_throttle - uses throttle curve and collective input to determine throttle setting
float calculate_desired_throttle(float collective_in);
};
class RSCThrCrvInt16Param {
public:
RSCThrCrvInt16Param(void);
static const struct AP_Param::GroupInfo var_info[];
void set_thrcrv_enable(int8_t setenable) {enable = setenable; }
float * get_thrcrv() {
static float throttlecurve[5];
for (uint8_t i = 0; i < 5; i++) {
throttlecurve[i] = (float)thrcrv[i] * 0.001f;
}
return throttlecurve;
}
private:
AP_Int8 enable;
AP_Int16 thrcrv[5]; // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective
};
class RSCGovFloatParam {
public:
RSCGovFloatParam(void);
static const struct AP_Param::GroupInfo var_info[];
void set_gov_enable(int8_t setenable) {enable = setenable; }
int16_t get_setpoint() { return setpoint; }
float get_disengage() { return disengage; }
float get_droop_setting() { return droop_setting; }
float get_tc() { return tc; }
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%
};

26
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
Unescape View File

@ -218,21 +218,24 @@ void AP_MotorsHeli_Single::set_rpm(int16_t rotor_rpm) @@ -218,21 +218,24 @@ void AP_MotorsHeli_Single::set_rpm(int16_t rotor_rpm)
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single::calculate_armed_scalars()
{
float thrcrv[5];
for (uint8_t i = 0; i < 5; i++) {
thrcrv[i]=_rsc_thrcrv[i]*0.001f;
}
// Set common RSC variables
_main_rotor.set_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
_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);
}
_main_rotor.set_slewrate(_rsc_slewrate);
// Set rsc mode specific parameters
if (_rsc_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) {
_main_rotor.set_throttle_curve(_rsc_thrcrv.get_thrcrv());
} 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);
}
}
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single::calculate_scalars()
@ -253,6 +256,7 @@ void AP_MotorsHeli_Single::calculate_scalars() @@ -253,6 +256,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
// send setpoints to main rotor controller and trigger recalculation of scalars
_main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
enable_rsc_parameters();
calculate_armed_scalars();
// send setpoints to DDVP rotor controller and trigger recalculation of scalars

Write Preview
Loading…
Cancel
Save