diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index d900b079e6..26f2094013 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -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); + } +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 8d86b41963..cbd70a6ec8 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -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 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index f6a8501983..b7fa3289ac 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -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(_rsc_mode.get())); + enable_rsc_parameters(); calculate_armed_scalars(); } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index fd3265f882..869ed59570 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -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(_rsc_mode.get())); + enable_rsc_parameters(); calculate_armed_scalars(); } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index b918a34b9e..c35fb821af 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -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; } - diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 4dbac6732b..4b4b6405c7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -5,6 +5,22 @@ #include #include +// 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% + +}; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index a2e8564725..0bcc3d29b9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -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(_rsc_mode.get())); + enable_rsc_parameters(); calculate_armed_scalars(); // send setpoints to DDVP rotor controller and trigger recalculation of scalars