mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 07:28:29 -04:00
AP_Motors: Tradheli - implement enable params for throttle curve and governor
This commit is contained in:
bnsgeyer
Randy Mackay
parent
51d4029f03
commit
3629541a20
@ -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),
|
||||
// indices 20 to 25 was throttle curve. Do not use this index in the future.
|
||||
|
||||
// @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),
|
||||
// @Group: RSC_CRV_
|
||||
// @Path: AP_MotorsHeli_RSC.cpp
|
||||
AP_SUBGROUPINFO(_rsc_thrcrv, "RSC_CRV_", 27, AP_MotorsHeli, RSCThrCrvInt16Param),
|
||||
|
||||
// @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),
|
||||
|
||||
// @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),
|
||||
// @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)
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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:
|
||||
// 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:
|
||||
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:
|
||||
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
|
||||
|
||||
@ -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()
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
// 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)
|
||||
// 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)
|
||||
return throttle;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -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:
|
||||
// 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:
|
||||
// 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%
|
||||
|
||||
};
|
||||
|
||||
@ -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()
|
||||
|
||||
// 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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user