diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index c6fb1e9c8e..c342a19014 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -65,55 +65,11 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @User: Standard AP_GROUPINFO("SV_MAN", 6, AP_MotorsHeli, _servo_mode, SERVO_CONTROL_MODE_AUTOMATED), - // @Param: RSC_SETPOINT - // @DisplayName: External Motor Governor Setpoint - // @Description: PWM in microseconds passed to the external motor governor when external governor is enabled - // @Range: 0 1000 - // @Units: PWM - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("RSC_SETPOINT", 7, AP_MotorsHeli, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT), - - // @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, 4:Governor - // @User: Standard - AP_GROUPINFO("RSC_MODE", 8, AP_MotorsHeli, _rsc_mode, (int8_t)ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH), + // indices 7 and 8 were RSC parameters which were moved to RSC library. Do not use these indices in the future. // index 9 was LAND_COL_MIN. Do not use this index in the future. - // @Param: RSC_RAMP_TIME - // @DisplayName: RSC Ramp Time - // @Description: Time in seconds for the output to the main rotor's ESC to reach full speed - // @Range: 0 60 - // @Units: s - // @User: Standard - AP_GROUPINFO("RSC_RAMP_TIME", 10, AP_MotorsHeli, _rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME), - - // @Param: RSC_RUNUP_TIME - // @DisplayName: RSC Runup Time - // @Description: Time in seconds for the main rotor to reach full speed. Must be longer than RSC_RAMP_TIME - // @Range: 0 60 - // @Units: s - // @User: Standard - AP_GROUPINFO("RSC_RUNUP_TIME", 11, AP_MotorsHeli, _rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME), - - // @Param: RSC_CRITICAL - // @DisplayName: Critical Rotor Speed - // @Description: Rotor speed below which flight is not possible - // @Range: 0 1000 - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("RSC_CRITICAL", 12, AP_MotorsHeli, _rsc_critical, AP_MOTORS_HELI_RSC_CRITICAL), - - // @Param: RSC_IDLE - // @DisplayName: Rotor Speed Output at Idle - // @Description: Rotor speed output while armed but rotor control speed is not engaged - // @Range: 0 500 - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("RSC_IDLE", 13, AP_MotorsHeli, _rsc_idle_output, AP_MOTORS_HELI_RSC_IDLE_DEFAULT), + // indices 10-13 were RSC parameters which were moved to RSC library. Do not use these indices in the future. // index 14 was RSC_POWER_LOW. Do not use this index in the future. @@ -138,23 +94,13 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // index 18 was RSC_POWER_NEGC. Do not use this index in the future. - // @Param: RSC_SLEWRATE - // @DisplayName: Throttle servo slew rate - // @Description: This controls the maximum rate at which the throttle output can change, as a percentage per second. A value of 100 means the throttle can change over its full range in one second. A value of zero gives unlimited slew rate. - // @Range: 0 500 - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("RSC_SLEWRATE", 19, AP_MotorsHeli, _rsc_slewrate, 0), - - // indices 20 to 25 was throttle curve. Do not use this index in the future. + // index 19 was RSC_SLEWRATE and was moved to RSC library. Do not use this index in the future. - // @Group: RSC_CRV_ - // @Path: AP_MotorsHeli_RSC.cpp - AP_SUBGROUPINFO(_rsc_thrcrv, "RSC_CRV_", 27, AP_MotorsHeli, RSCThrCrvParam), + // indices 20 to 24 was throttle curve. Do not use this index in the future. - // @Group: RSC_GOV_ + // @Group: RSC_ // @Path: AP_MotorsHeli_RSC.cpp - AP_SUBGROUPINFO(_rsc_gov, "RSC_GOV_", 28, AP_MotorsHeli, RSCGovParam), + AP_SUBGROUPINFO(_main_rotor, "RSC_", 25, AP_MotorsHeli, AP_MotorsHeli_RSC), AP_GROUPEND }; @@ -423,34 +369,42 @@ void AP_MotorsHeli::output_logic() // parameter_check - check if helicopter specific parameters are sensible bool AP_MotorsHeli::parameter_check(bool display_msg) const { - // returns false if _rsc_setpoint is not higher than _rsc_critical as this would not allow rotor_runup_complete to ever return true - if (_rsc_critical >= _rsc_setpoint) { + // returns false if RSC Mode is not set to a valid control mode + if (_main_rotor._rsc_mode.get() <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _main_rotor._rsc_mode.get() > (int8_t)ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { if (display_msg) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_CRITICAL too large"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_MODE invalid"); } return false; } - // returns false if RSC Mode is not set to a valid control mode - if (_rsc_mode <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _rsc_mode > (int8_t)ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { + // returns false if rsc_setpoint is out of range + if ( _main_rotor._rsc_setpoint.get() > 100 || _main_rotor._rsc_setpoint.get() < 10){ if (display_msg) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_MODE invalid"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_SETPOINT out of range"); } return false; } - // returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate - if (_rsc_runup_time <= _rsc_ramp_time){ + // returns false if idle output is out of range + if ( _main_rotor._idle_output.get() > 100 || _main_rotor._idle_output.get() < 0){ if (display_msg) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RUNUP_TIME too small"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_IDLE out of range"); } return false; } - // returns false if idle output is higher than critical rotor speed as this could block runup_complete from going false - if ( _rsc_idle_output >= _rsc_critical){ + // returns false if _rsc_critical is not between 0 and 100 + if (_main_rotor._critical_speed.get() > 100 || _main_rotor._critical_speed.get() < 0) { if (display_msg) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_IDLE too large"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_CRITICAL out of range"); + } + return false; + } + + // returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate + if (_main_rotor._runup_time.get() <= _main_rotor._ramp_time.get()){ + if (display_msg) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RUNUP_TIME too small"); } return false; } @@ -502,18 +456,3 @@ 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 6ffb074d54..accd845524 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -20,22 +20,12 @@ #define AP_MOTORS_HELI_COLLECTIVE_MAX 1750 #define AP_MOTORS_HELI_COLLECTIVE_MID 1500 -// default main rotor speed (ch8 out) as a number from 0 ~ 1000 -#define AP_MOTORS_HELI_RSC_SETPOINT 700 - -// default main rotor critical speed -#define AP_MOTORS_HELI_RSC_CRITICAL 500 - -// RSC output defaults -#define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0 - -// 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 - // flybar types #define AP_MOTORS_HELI_NOFLYBAR 0 +// rsc function output channels. +#define AP_MOTORS_HELI_RSC CH_8 + class AP_HeliControls; /// @class AP_MotorsHeli @@ -45,7 +35,8 @@ public: /// Constructor AP_MotorsHeli( uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_Motors(loop_rate, speed_hz) + AP_Motors(loop_rate, speed_hz), + _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC) { AP_Param::setup_object_defaults(this, var_info); }; @@ -83,11 +74,11 @@ public: // set_inverted_flight - enables/disables inverted flight void set_inverted_flight(bool inverted) { _heliflags.inverted_flight = inverted; } - // get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT) - uint8_t get_rsc_mode() const { return _rsc_mode; } + // get_rsc_mode - gets the current rotor speed control method + uint8_t get_rsc_mode() const { return _main_rotor.get_control_mode(); } // get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1) - float get_rsc_setpoint() const { return _rsc_setpoint * 0.001f; } + float get_rsc_setpoint() const { return _main_rotor._rsc_setpoint.get() * 0.01f; } // set_rpm - for rotor speed governor virtual void set_rpm(float rotor_rpm) = 0; @@ -133,9 +124,6 @@ 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[]; @@ -151,14 +139,14 @@ protected: SERVO_CONTROL_MODE_MANUAL_OSCILLATE, }; - RSCThrCrvParam _rsc_thrcrv; - RSCGovParam _rsc_gov; - // output - sends commands to the motors void output_armed_stabilizing() override; void output_armed_zero_throttle(); void output_disarmed(); + // external objects we depend upon + AP_MotorsHeli_RSC _main_rotor; // main rotor + // update_motor_controls - sends commands to motor controllers virtual void update_motor_control(RotorControlState state) = 0; @@ -202,6 +190,7 @@ protected: uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up uint8_t inverted_flight : 1; // true for inverted flight uint8_t init_targets_on_arming : 1; // 0 if targets were initialized, 1 if targets were not initialized after arming + uint8_t save_rsc_mode : 1; // used to determine the rsc mode needs to be saved while disarmed } _heliflags; // parameters @@ -210,13 +199,6 @@ protected: 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 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 - 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_slewrate; // throttle slew rate (percentage per second) AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup // internal variables diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 1b429cb8bf..2ccfa1d157 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -134,7 +134,7 @@ bool AP_MotorsHeli_Dual::init_outputs() } // set rotor servo range - _rotor.init_servo(); + _main_rotor.init_servo(); } @@ -192,7 +192,7 @@ void AP_MotorsHeli_Dual::output_test_seq(uint8_t motor_seq, int16_t pwm) break; case 7: // main rotor - rc_write(AP_MOTORS_HELI_DUAL_RSC, pwm); + rc_write(AP_MOTORS_HELI_RSC, pwm); break; default: // do nothing @@ -203,35 +203,32 @@ void AP_MotorsHeli_Dual::output_test_seq(uint8_t motor_seq, int16_t pwm) // set_desired_rotor_speed void AP_MotorsHeli_Dual::set_desired_rotor_speed(float desired_speed) { - _rotor.set_desired_speed(desired_speed); + _main_rotor.set_desired_speed(desired_speed); } // set_rotor_rpm - used for governor with speed sensor void AP_MotorsHeli_Dual::set_rpm(float rotor_rpm) { - _rotor.set_rotor_rpm(rotor_rpm); + _main_rotor.set_rotor_rpm(rotor_rpm); } // calculate_armed_scalars void AP_MotorsHeli_Dual::calculate_armed_scalars() { - // 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_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_response(_rsc_gov.get_droop_response()*0.01f); - _rotor.set_governor_reference(_rsc_gov.get_reference()); - _rotor.set_governor_range(_rsc_gov.get_range()); - _rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f); + if (_main_rotor._rsc_mode.get() == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT || _main_rotor._rsc_mode.get() == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { + _main_rotor.set_throttle_curve(); + } + // keeps user from changing RSC mode while armed + if (_main_rotor._rsc_mode.get() != _main_rotor.get_control_mode()) { + _main_rotor.reset_rsc_mode_param(); + _heliflags.save_rsc_mode = true; + gcs().send_text(MAV_SEVERITY_CRITICAL, "RSC control mode change failed"); + } + // saves rsc mode parameter when disarmed if it had been reset while armed + if (_heliflags.save_rsc_mode && !_flags.armed) { + _main_rotor._rsc_mode.save(); + _heliflags.save_rsc_mode = false; } } @@ -267,8 +264,7 @@ void AP_MotorsHeli_Dual::calculate_scalars() _swashplate2.calculate_roll_pitch_collective_factors(); // set mode of main rotor controller and trigger recalculation of scalars - _rotor.set_control_mode(static_cast(_rsc_mode.get())); - enable_rsc_parameters(); + _main_rotor.set_control_mode(static_cast(_main_rotor._rsc_mode.get())); calculate_armed_scalars(); } @@ -341,7 +337,7 @@ uint16_t AP_MotorsHeli_Dual::get_motor_mask() if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) { mask |= 1U << AP_MOTORS_MOT_8; } - mask |= 1U << AP_MOTORS_HELI_DUAL_RSC; + mask |= 1U << AP_MOTORS_HELI_RSC; return mask; } @@ -349,7 +345,7 @@ uint16_t AP_MotorsHeli_Dual::get_motor_mask() void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state) { // Send state update to motors - _rotor.output(state); + _main_rotor.output(state); if (state == ROTOR_CONTROL_STOP) { // set engine run enable aux output to not run position to kill engine when disarmed @@ -360,7 +356,7 @@ void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state) } // Check if rotors are run-up - _heliflags.rotor_runup_complete = _rotor.is_runup_complete(); + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); } // @@ -462,7 +458,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c // feed power estimate into main rotor controller // ToDo: add main rotor cyclic power? - _rotor.set_collective(fabsf(collective_out)); + _main_rotor.set_collective(fabsf(collective_out)); // compute swashplate tilt float swash1_pitch = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH, pitch_out, roll_out, yaw_out, collective_out_scaled); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index aaf10234ae..2959bb8866 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -13,9 +13,6 @@ #include "AP_MotorsHeli_RSC.h" #include "AP_MotorsHeli_Swash.h" -// rsc function output channel -#define AP_MOTORS_HELI_DUAL_RSC CH_8 - // tandem modes #define AP_MOTORS_HELI_DUAL_MODE_TANDEM 0 // tandem mode (rotors front and aft) #define AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE 1 // transverse mode (rotors side by side) @@ -42,8 +39,7 @@ public: // constructor AP_MotorsHeli_Dual(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_MotorsHeli(loop_rate, speed_hz), - _rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_DUAL_RSC) + AP_MotorsHeli(loop_rate, speed_hz) { AP_Param::setup_object_defaults(this, var_info); }; @@ -65,19 +61,19 @@ public: void set_desired_rotor_speed(float desired_speed) override; // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000 - float get_main_rotor_speed() const override { return _rotor.get_rotor_speed(); } + 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 ~ 1000 - float get_desired_rotor_speed() const override { return _rotor.get_rotor_speed(); } + float get_desired_rotor_speed() const override { return _main_rotor.get_rotor_speed(); } // 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(); } + 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 override { return _rotor.get_governor_output(); } + float get_governor_output() const override { return _main_rotor.get_governor_output(); } // get_control_output - float get_control_output() const override { return _rotor.get_control_output(); } + float get_control_output() const override { return _main_rotor.get_control_output(); } // calculate_scalars - recalculates various scalars used void calculate_scalars() override; @@ -118,7 +114,6 @@ protected: void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override; // objects we depend upon - AP_MotorsHeli_RSC _rotor; // main rotor controller AP_MotorsHeli_Swash _swashplate1; // swashplate1 AP_MotorsHeli_Swash _swashplate2; // swashplate2 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 4c2b172fe1..1104d69b17 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -15,7 +15,7 @@ #include #include - +#include #include "AP_MotorsHeli_Quad.h" extern const AP_HAL::HAL& hal; @@ -58,7 +58,7 @@ bool AP_MotorsHeli_Quad::init_outputs() } // set rotor servo range - _rotor.init_servo(); + _main_rotor.init_servo(); _flags.initialised_ok = true; @@ -82,7 +82,7 @@ void AP_MotorsHeli_Quad::output_test_seq(uint8_t motor_seq, int16_t pwm) break; case AP_MOTORS_HELI_QUAD_NUM_MOTORS+1: // main rotor - rc_write(AP_MOTORS_HELI_QUAD_RSC, pwm); + rc_write(AP_MOTORS_HELI_RSC, pwm); break; default: // do nothing @@ -93,35 +93,32 @@ void AP_MotorsHeli_Quad::output_test_seq(uint8_t motor_seq, int16_t pwm) // set_desired_rotor_speed void AP_MotorsHeli_Quad::set_desired_rotor_speed(float desired_speed) { - _rotor.set_desired_speed(desired_speed); + _main_rotor.set_desired_speed(desired_speed); } // set_rotor_rpm - used for governor with speed sensor void AP_MotorsHeli_Quad::set_rpm(float rotor_rpm) { - _rotor.set_rotor_rpm(rotor_rpm); + _main_rotor.set_rotor_rpm(rotor_rpm); } // calculate_armed_scalars void AP_MotorsHeli_Quad::calculate_armed_scalars() { - // 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_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_response(_rsc_gov.get_droop_response()*0.01f); - _rotor.set_governor_reference(_rsc_gov.get_reference()); - _rotor.set_governor_range(_rsc_gov.get_range()); - _rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f); + if (_main_rotor._rsc_mode.get() == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT || _main_rotor._rsc_mode.get() == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { + _main_rotor.set_throttle_curve(); + } + // keeps user from changing RSC mode while armed + if (_main_rotor._rsc_mode.get() != _main_rotor.get_control_mode()) { + _main_rotor.reset_rsc_mode_param(); + gcs().send_text(MAV_SEVERITY_CRITICAL, "RSC control mode change failed"); + _heliflags.save_rsc_mode = true; + } + // saves rsc mode parameter when disarmed if it had been reset while armed + if (_heliflags.save_rsc_mode && !_flags.armed) { + _main_rotor._rsc_mode.save(); + _heliflags.save_rsc_mode = false; } } @@ -143,8 +140,7 @@ void AP_MotorsHeli_Quad::calculate_scalars() calculate_roll_pitch_collective_factors(); // set mode of main rotor controller and trigger recalculation of scalars - _rotor.set_control_mode(static_cast(_rsc_mode.get())); - enable_rsc_parameters(); + _main_rotor.set_control_mode(static_cast(_main_rotor._rsc_mode.get())); calculate_armed_scalars(); } @@ -178,7 +174,7 @@ uint16_t AP_MotorsHeli_Quad::get_motor_mask() for (uint8_t i=0; i _rotor.get_critical_speed(); } + 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 override { return _rotor.get_governor_output(); } + float get_governor_output() const override { return _main_rotor.get_governor_output(); } // get_control_output - float get_control_output() const override { return _rotor.get_control_output(); } + float get_control_output() const override { return _main_rotor.get_control_output(); } // calculate_scalars - recalculates various scalars used void calculate_scalars() override; @@ -95,9 +91,6 @@ protected: // move_actuators - moves swash plate to attitude of parameters passed in void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override; - // objects we depend upon - AP_MotorsHeli_RSC _rotor; // main rotor controller - // rate factors float _rollFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS]; float _pitchFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS]; diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 60882930f7..82949e73db 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -20,120 +20,159 @@ extern const AP_HAL::HAL& hal; -const AP_Param::GroupInfo RSCThrCrvParam::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, RSCThrCrvParam, enable, 0, AP_PARAM_FLAG_ENABLE), - - // @Param: 000 - // @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. +const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = { + + // @Param: SETPOINT + // @DisplayName: External Motor Governor Setpoint + // @Description: Throttle (HeliRSC Servo) output in percent to the external motor governor when motor interlock enabled (throttle hold off). // @Range: 0 100 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("000", 2, RSCThrCrvParam, thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT), + AP_GROUPINFO("SETPOINT", 1, AP_MotorsHeli_RSC, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT), + + // @Param: MODE + // @DisplayName: Rotor Speed Control Mode + // @Description: Selects the type of rotor speed control used to determine throttle output to the HeliRSC servo channel when motor interlock is enabled (throttle hold off). RC Passthrough sends the input from the RC Motor Interlock channel as throttle output. External Gov SetPoint sends the RSC SetPoint parameter value as throttle output. Throttle Curve uses the 5 point throttle curve to determine throttle output based on the collective output. Governor is ArduCopter's built-in governor that uses the throttle curve for a feed forward throttle command to determine throttle output. + // @Values: 1:RC Passthrough, 2:External Gov SetPoint, 3:Throttle Curve, 4:Governor + // @User: Standard + AP_GROUPINFO("MODE", 2, AP_MotorsHeli_RSC, _rsc_mode, (int8_t)ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH), + + // @Param: RAMP_TIME + // @DisplayName: RSC Throttle Output Ramp Time + // @Description: Time in seconds for throttle output (HeliRSC servo) to ramp from ground idle (RSC_IDLE) to flight idle throttle setting when motor interlock is enabled (throttle hold off). + // @Range: 0 60 + // @Units: s + // @User: Standard + AP_GROUPINFO("RAMP_TIME", 3, AP_MotorsHeli_RSC, _ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME), + + // @Param: RUNUP_TIME + // @DisplayName: Rotor Runup Time + // @Description: Actual time in seconds for the main rotor to reach full speed after motor interlock is enabled (throttle hold off). Must be at least one second longer than the Throttle Ramp Time that is set with RSC_RAMP_TIME. + // @Range: 0 60 + // @Units: s + // @User: Standard + AP_GROUPINFO("RUNUP_TIME", 4, AP_MotorsHeli_RSC, _runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME), - // @Param: 025 - // @DisplayName: Throttle Servo Position in percent for 25 percent collective - // @Description: Throttle Servo Position in percent for 25 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. + // @Param: CRITICAL + // @DisplayName: Critical Rotor Speed + // @Description: Percentage of normal rotor speed where flight is no longer possible. However currently the rotor runup/rundown is estimated using the RSC_RUNUP_TIME parameter. Estimated rotor speed increases/decreases between 0 (rotor stopped) to 1 (rotor at normal speed) in the RSC_RUNUP_TIME in seconds. This parameter should be set so that the estimated rotor speed goes below critical in approximately 3 seconds. So if you had a 10 second runup time then set RSC_CRITICAL to 70%. // @Range: 0 100 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("025", 3, RSCThrCrvParam, thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT), + AP_GROUPINFO("CRITICAL", 5, AP_MotorsHeli_RSC, _critical_speed, AP_MOTORS_HELI_RSC_CRITICAL), - // @Param: 050 - // @DisplayName: Throttle Servo Position in percent for 50 percent collective - // @Description: Throttle Servo Position in percent for 50 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 + // @Param: IDLE + // @DisplayName: RSC Throttle Output at Idle + // @Description: Throttle output (HeliRSC Servo) in percent while armed but motor interlock is disabled (throttle hold on). FOR COMBUSTION ENGINES. Sets the engine ground idle throttle percentage with clutch disengaged. This must be set to zero for electric helicopters under most situations. If the ESC has an autorotation window this can be set to keep the autorotation window open in the ESC. Consult the operating manual for your ESC to set it properly for this purpose + // @Range: 0 50 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("050", 4, RSCThrCrvParam, thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT), + AP_GROUPINFO("IDLE", 6, AP_MotorsHeli_RSC, _idle_output, AP_MOTORS_HELI_RSC_IDLE_DEFAULT), + + // @Param: SLEWRATE + // @DisplayName: RSC Throttle Output Slew Rate + // @Description: This controls the maximum rate at which the throttle output (HeliRSC servo) can change, as a percentage per second. A value of 100 means the throttle can change over its full range in one second. A value of zero gives unlimited slew rate. + // @Range: 0 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("SLEWRATE", 7, AP_MotorsHeli_RSC, _power_slewrate, 0), - // @Param: 075 - // @DisplayName: Throttle Servo Position in percent for 75 percent collective - // @Description: Throttle Servo Position in percent for 75 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. + // @Param: THRCRV_0 + // @DisplayName: RSC Throttle Output at 0% collective + // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at the minimum collective pitch position. The 0 percent collective is defined by H_COL_MIN. Example: if the setup has -2 degree to +10 degree collective pitch setup, this setting would correspond to -2 degree of pitch. // @Range: 0 100 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("075", 5, RSCThrCrvParam, thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT), + AP_GROUPINFO("THRCRV_0", 8, AP_MotorsHeli_RSC, _thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT), - // @Param: 100 - // @DisplayName: Throttle Servo Position in percent for 100 percent collective - // @Description: Throttle Servo Position in percent for 100 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. + // @Param: THRCRV_25 + // @DisplayName: RSC Throttle Output at 25% collective + // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at 25% of full collective travel where he 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, the total range is 12 degrees. 25% of 12 degrees is 3 degrees, so this setting would correspond to +1 degree of pitch. // @Range: 0 100 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("100", 6, RSCThrCrvParam, thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT), + AP_GROUPINFO("THRCRV_25", 9, AP_MotorsHeli_RSC, _thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT), - AP_GROUPEND -}; + // @Param: THRCRV_50 + // @DisplayName: RSC Throttle Output at 50% collective + // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at 50% of full collective travel where he 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, the total range is 12 degrees. 50% of 12 degrees is 6 degrees, so this setting would correspond to +4 degree of pitch. + // @Range: 0 100 + // @Units: % + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("THRCRV_50", 10, AP_MotorsHeli_RSC, _thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT), -const AP_Param::GroupInfo RSCGovParam::var_info[] = { + // @Param: THRCRV_75 + // @DisplayName: RSC Throttle Output at 75% collective + // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at 75% of full collective travel where he 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, the total range is 12 degrees. 75% of 12 degrees is 9 degrees, so this setting would correspond to +7 degree of pitch. + // @Range: 0 100 + // @Units: % + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("THRCRV_75", 11, AP_MotorsHeli_RSC, _thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT), - // @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, RSCGovParam, enable, 0, AP_PARAM_FLAG_ENABLE), + // @Param: THRCRV_100 + // @DisplayName: RSC Throttle Output at 100% collective + // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at the minimum collective pitch position. The 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, this setting would correspond to +10 degree of pitch. + // @Range: 0 100 + // @Units: % + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("THRCRV_100", 12, AP_MotorsHeli_RSC, _thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT), - // @Param: SETPNT - // @DisplayName: Governor RPM Reference Setting - // @Description: Main rotor rpm setting that governor maintains when engaged + // @Param: GOV_SETPNT + // @DisplayName: Main Rotor Governor RPM Setpoint + // @Description: Main rotor rpm setting that governor maintains when engaged. Set to the rotor rpm your helicopter runs in flight. When a speed sensor is installed the rotor governor maintains this speed. For governor operation this should be set 10 rpm higher than the actual desired headspeed to allow for governor droop // @Range: 800 3500 + // @Units: RPM // @Increment: 10 // @User: Standard - AP_GROUPINFO("SETPNT", 2, RSCGovParam, reference, AP_MOTORS_HELI_RSC_GOVERNOR_SETPNT_DEFAULT), + AP_GROUPINFO("GOV_SETPNT", 13, AP_MotorsHeli_RSC, _governor_reference, AP_MOTORS_HELI_RSC_GOVERNOR_SETPNT_DEFAULT), - // @Param: DISGAG + // @Param: GOV_DISGAG // @DisplayName: Throttle Percentage for Governor Disengage - // @Description: Percentage of throttle where the governor will disenage to allow return to flight idle power + // @Description: Percentage of throttle where the governor will disengage to allow return to flight idle power. Typically should be set to the same value as flight idle throttle (the very lowest throttle setting on the throttle curve). The governor disengage can be disabled by setting this value to zero and using the pull-down from the governor TCGAIN to reduce power to flight idle with the collective at it's lowest throttle setting on the throttle curve. // @Range: 0 50 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("DISGAG", 3, RSCGovParam, disengage, AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE_DEFAULT), + AP_GROUPINFO("GOV_DISGAG", 14, AP_MotorsHeli_RSC, _governor_disengage, AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE_DEFAULT), - // @Param: DROOP + // @Param: GOV_DROOP // @DisplayName: Governor Droop Response Setting - // @Description: Governor droop response under load, 0-100%. Higher value is quicker response but may cause surging - // @Range: 0 100 + // @Description: Governor droop response under load, normal settings of 0-100%. Higher value is quicker response but may cause surging. Setting to zero disables the governor. Adjust this to be as aggressive as possible without getting surging or over-run on headspeed when the governor engages. Setting over 100% is allowable for some two-stage turbine engines to provide scheduling of the gas generator for proper torque response of the N2 spool + // @Range: 0 150 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("DROOP", 4, RSCGovParam, droop_response, AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT), + AP_GROUPINFO("GOV_DROOP", 15, AP_MotorsHeli_RSC, _governor_droop_response, AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT), - // @Param: THRCURVE + // @Param: GOV_TCGAIN // @DisplayName: Governor Throttle Curve Gain - // @Description: Percentage of throttle curve gain in governor output + // @Description: Percentage of throttle curve gain in governor output. This provides a type of feedforward response to sudden loading or unloading of the engine. If headspeed drops excessively during sudden heavy load, increase the throttle curve gain. If the governor runs with excessive droop more than 15 rpm lower than the speed setting, increase this setting until the governor runs at 8-10 rpm droop from the speed setting. The throttle curve must be properly tuned to fly the helicopter without the governor for this setting to work properly. // @Range: 50 100 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("THRCURVE", 5, RSCGovParam, thrcurve, AP_MOTORS_HELI_RSC_GOVERNOR_THRCURVE_DEFAULT), + AP_GROUPINFO("GOV_TCGAIN", 16, AP_MotorsHeli_RSC, _governor_tcgain, AP_MOTORS_HELI_RSC_GOVERNOR_TCGAIN_DEFAULT), - // @Param: RANGE + // @Param: GOV_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 + // @Units: RPM // @Increment: 10 // @User: Standard - AP_GROUPINFO("RANGE", 6, RSCGovParam, range, AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT), + AP_GROUPINFO("GOV_RANGE", 17, AP_MotorsHeli_RSC, _governor_range, AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT), AP_GROUPEND }; -RSCThrCrvParam::RSCThrCrvParam(void) -{ - AP_Param::setup_object_defaults(this, var_info); -} - -RSCGovParam::RSCGovParam(void) -{ - AP_Param::setup_object_defaults(this, var_info); -} - // init_servo - servo initialization on start-up void AP_MotorsHeli_RSC::init_servo() { @@ -147,16 +186,16 @@ 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]) +void AP_MotorsHeli_RSC::set_throttle_curve() { - + float thrcrv[5]; // Ensure user inputs are within parameter limits + // Scale throttle curve parameters for (uint8_t i = 0; i < 5; i++) { - thrcrv[i] = constrain_float(thrcrv[i], 0.0f, 1.0f); + thrcrv[i] = constrain_float(_thrcrv[i] * 0.01f, 0.0f, 1.0f); } // Calculate the spline polynomials for the throttle curve splinterp5(thrcrv,_thrcrv_poly); - } // output - update value to send to ESC/Servo @@ -188,7 +227,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) update_rotor_ramp(0.0f, dt); // set rotor control speed to idle speed parameter, this happens instantly and ignore ramping - _control_output = _idle_output; + _control_output = get_idle_output(); break; case ROTOR_CONTROL_ACTIVE: @@ -197,43 +236,43 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) if ((_control_mode == ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SPEED_SETPOINT)) { // set control rotor speed to ramp slewed value between idle and desired speed - _control_output = _idle_output + (_rotor_ramp_output * (_desired_speed - _idle_output)); + _control_output = get_idle_output() + (_rotor_ramp_output * (_desired_speed - get_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); - _control_output = _idle_output + (_rotor_ramp_output * (desired_throttle - _idle_output)); + _control_output = get_idle_output() + (_rotor_ramp_output * (desired_throttle - get_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 active if within user-set range from reference speed - if ((_rotor_rpm >= (_governor_reference - _governor_range)) && (_rotor_rpm <= (_governor_reference + _governor_range))) { - float governor_droop = constrain_float(_governor_reference - _rotor_rpm,0.0f,_governor_range); + if ((_rotor_rpm >= ((float)_governor_reference - _governor_range)) && (_rotor_rpm <= ((float)_governor_reference + _governor_range))) { + float governor_droop = constrain_float((float)_governor_reference - _rotor_rpm,0.0f,_governor_range); // if rpm has not reached 40% of the operational range from reference speed, governor // remains in pre-engage status, no reference speed compensation due to droop // this provides a soft-start function that engages the governor less aggressively - if (_governor_engage && _rotor_rpm < (_governor_reference - (_governor_range * 0.4f))) { - _governor_output = ((_rotor_rpm - _governor_reference) * desired_throttle) * _governor_droop_response * -0.01f; + if (_governor_engage && _rotor_rpm < ((float)_governor_reference - (_governor_range * 0.4f))) { + _governor_output = ((_rotor_rpm - (float)_governor_reference) * desired_throttle) * get_governor_droop_response() * -0.01f; } else { // normal flight status, governor fully engaged with reference speed compensation for droop _governor_engage = true; - _governor_output = ((_rotor_rpm - (_governor_reference + governor_droop)) * desired_throttle) * _governor_droop_response * -0.01f; + _governor_output = ((_rotor_rpm - ((float)_governor_reference + governor_droop)) * desired_throttle) * get_governor_droop_response() * -0.01f; } // check for governor disengage for return to flight idle power - if (desired_throttle <= _governor_disengage) { + if (desired_throttle <= get_governor_disengage()) { _governor_output = 0.0f; _governor_engage = false; } // 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); + _control_output = constrain_float(get_idle_output() + (_rotor_ramp_output * (((desired_throttle * get_governor_tcgain()) + _governor_output) - get_idle_output())), get_idle_output() + (_rotor_ramp_output * ((desired_throttle * get_governor_tcgain())) - get_idle_output()), 1.0f); } else { // 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 _governor_output = 0.0f; _governor_engage = false; - _control_output = _idle_output + (_rotor_ramp_output * (desired_throttle - _idle_output)); + _control_output = get_idle_output() + (_rotor_ramp_output * (desired_throttle - get_idle_output())); } } break; @@ -316,7 +355,7 @@ void AP_MotorsHeli_RSC::update_rotor_runup(float dt) } // if rotor speed is less than critical speed, then run-up is not complete // this will prevent the case where the target rotor speed is less than critical speed - if (_runup_complete && (get_rotor_speed() <= _critical_speed)) { + if (_runup_complete && (get_rotor_speed() <= get_critical_speed())) { _runup_complete = false; } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 6079bb281c..bb96595da9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -5,6 +5,19 @@ #include #include +// default main rotor speed (ch8 out) as a number from 0 ~ 100 +#define AP_MOTORS_HELI_RSC_SETPOINT 70 + +// default main rotor critical speed +#define AP_MOTORS_HELI_RSC_CRITICAL 50 + +// RSC output defaults +#define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0 + +// 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 + // Throttle Curve Defaults #define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 25 #define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 32 @@ -16,7 +29,7 @@ #define AP_MOTORS_HELI_RSC_GOVERNOR_SETPNT_DEFAULT 1500 #define AP_MOTORS_HELI_RSC_GOVERNOR_DISENGAGE_DEFAULT 25 #define AP_MOTORS_HELI_RSC_GOVERNOR_DROOP_DEFAULT 30 -#define AP_MOTORS_HELI_RSC_GOVERNOR_THRCURVE_DEFAULT 90 +#define AP_MOTORS_HELI_RSC_GOVERNOR_TCGAIN_DEFAULT 90 #define AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT 100 // rotor controller states @@ -45,7 +58,9 @@ public: uint8_t default_channel) : _aux_fn(aux_fn), _default_channel(default_channel) - {}; + { + AP_Param::setup_object_defaults(this, var_info); + }; // init_servo - servo initialization on start-up void init_servo(); @@ -53,28 +68,25 @@ public: // set_control_mode - sets control mode void set_control_mode(RotorControlMode mode) { _control_mode = mode; } + // reset_rsc_mode_param - resets rsc mode param to current control mode + void reset_rsc_mode_param() { _rsc_mode.set((uint8_t)_control_mode); } + + // get_control_mode - gets control mode + uint8_t get_control_mode() const { return _control_mode; } + // set_critical_speed void set_critical_speed(float critical_speed) { _critical_speed = critical_speed; } - // get_critical_speed - float get_critical_speed() const { return _critical_speed; } - // set_idle_output - float get_idle_output() { return _idle_output; } void set_idle_output(float idle_output) { _idle_output = idle_output; } // set rotor speed governor parameters - void set_governor_disengage(float governor_disengage) {_governor_disengage = governor_disengage; } - 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_reference(float governor_reference) { _governor_reference = governor_reference; } - 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 float get_desired_speed() const { return _desired_speed; } - // set_desired_speed + // set_desired_speed - this requires input to be 0-1 void set_desired_speed(float desired_speed) { _desired_speed = desired_speed; } // get_control_speed @@ -98,11 +110,8 @@ 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]); + void set_throttle_curve(); // set_collective. collective for throttle curve calculation void set_collective(float collective) { _collective_in = collective; } @@ -110,6 +119,17 @@ public: // output - update value to send to ESC/Servo void output(RotorControlState state); + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + // parameters + 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 _ramp_time; // Time in seconds for the output to the main rotor's ESC to reach setpoint + AP_Int8 _runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time + AP_Int16 _critical_speed; // Rotor speed below which flight is not possible + AP_Int16 _idle_output; // Rotor control output while at idle + private: uint64_t _last_update_us; @@ -119,26 +139,16 @@ private: // internal variables RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED; // motor control mode, Passthrough or Setpoint - float _critical_speed; // rotor speed below which flight is not possible - float _idle_output; // motor output idle speed float _desired_speed; // latest desired rotor speed from pilot float _control_output; // latest logic controlled output 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; // scalar used to store status of rotor run-up time (0.0-1.0f) - int8_t _ramp_time; // time in seconds for the output to the main rotor's ESC to reach full speed - 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; // flag for determining if runup is complete float _thrcrv_poly[4][4]; // spline polynomials for throttle curve interpolation - uint16_t _power_slewrate; // slewrate for throttle (percentage per second) float _collective_in; // collective in for throttle curve calculation, range 0-1.0f float _rotor_rpm; // rotor rpm from speed sensor for governor - float _governor_disengage; // throttle percentage where governor disenages to allow return to flight idle float _governor_output; // governor output for rotor speed control - float _governor_range; // RPM range +/- governor rpm reference setting where governor is operational - float _governor_reference; // sets rotor speed for governor - float _governor_droop_response; // governor response to droop under load 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 void update_rotor_ramp(float rotor_ramp_input, float dt); @@ -151,48 +161,22 @@ private: // calculate_desired_throttle - uses throttle curve and collective input to determine throttle setting float calculate_desired_throttle(float collective_in); -}; - -class RSCThrCrvParam { -public: - RSCThrCrvParam(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.01f; - } - 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 + // parameters + AP_Int16 _power_slewrate; // throttle slew rate (percentage per second) + AP_Int16 _thrcrv[5]; // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective + AP_Int16 _governor_reference; // sets rotor speed for governor + AP_Float _governor_range; // RPM range +/- governor rpm reference setting where governor is operational + AP_Float _governor_disengage; // sets the throttle percent where the governor disengages for return to flight idle + AP_Float _governor_droop_response; // governor response to droop under load + AP_Float _governor_tcgain; // governor throttle curve weighting, range 50-100% + + // parameter accessors to allow conversions + float get_critical_speed() const { return _critical_speed * 0.01; } + float get_idle_output() { return _idle_output * 0.01; } + float get_governor_disengage() { return _governor_disengage * 0.01; } + float get_governor_droop_response() { return _governor_droop_response * 0.01; } + float get_governor_tcgain() { return _governor_tcgain * 0.01; } }; -class RSCGovParam { -public: - RSCGovParam(void); - - static const struct AP_Param::GroupInfo var_info[]; - - void set_gov_enable(int8_t setenable) {enable = setenable; } - int16_t get_reference() { return reference; } - float get_range() { return range; } - float get_disengage() { return disengage; } - float get_droop_response() { return droop_response; } - float get_thrcurve() { return thrcurve; } - -private: - AP_Int8 enable; - AP_Int16 reference; // sets rotor speed for governor - AP_Float range; // RPM range +/- governor rpm reference setting where governor is operational - AP_Float disengage; // sets the throttle percent where the governor disengages for return to flight idle - AP_Float droop_response; // governor response to droop under load - AP_Float thrcurve; // 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 8004361107..4601220dd9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -192,7 +192,7 @@ void AP_MotorsHeli_Single::output_test_seq(uint8_t motor_seq, int16_t pwm) break; case 5: // main rotor - rc_write(AP_MOTORS_HELI_SINGLE_RSC, pwm); + rc_write(AP_MOTORS_HELI_RSC, pwm); break; default: // do nothing @@ -206,7 +206,7 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(float desired_speed) _main_rotor.set_desired_speed(desired_speed); // always send desired speed to tail rotor control, will do nothing if not DDVP not enabled - _tail_rotor.set_desired_speed(_direct_drive_tailspeed*0.001f); + _tail_rotor.set_desired_speed(_direct_drive_tailspeed*0.01f); } // set_rotor_rpm - used for governor with speed sensor @@ -218,23 +218,20 @@ void AP_MotorsHeli_Single::set_rpm(float rotor_rpm) // calculate_scalars - recalculates various scalers used. void AP_MotorsHeli_Single::calculate_armed_scalars() { - // 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_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_response(_rsc_gov.get_droop_response()*0.01f); - _main_rotor.set_governor_reference(_rsc_gov.get_reference()); - _main_rotor.set_governor_range(_rsc_gov.get_range()); - _main_rotor.set_governor_thrcurve(_rsc_gov.get_thrcurve()*0.01f); + if (_main_rotor._rsc_mode.get() == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT || _main_rotor._rsc_mode.get() == ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { + _main_rotor.set_throttle_curve(); + } + // keeps user from changing RSC mode while armed + if (_main_rotor._rsc_mode.get() != _main_rotor.get_control_mode()) { + _main_rotor.reset_rsc_mode_param(); + gcs().send_text(MAV_SEVERITY_CRITICAL, "RSC control mode change failed"); + _heliflags.save_rsc_mode = true; + } + // saves rsc mode parameter when disarmed if it had been reset while armed + if (_heliflags.save_rsc_mode && !_flags.armed) { + _main_rotor._rsc_mode.save(); + _heliflags.save_rsc_mode = false; } } @@ -256,17 +253,16 @@ void AP_MotorsHeli_Single::calculate_scalars() _swashplate.calculate_roll_pitch_collective_factors(); // send setpoints to main rotor controller and trigger recalculation of scalars - _main_rotor.set_control_mode(static_cast(_rsc_mode.get())); - enable_rsc_parameters(); + _main_rotor.set_control_mode(static_cast(_main_rotor._rsc_mode.get())); calculate_armed_scalars(); // send setpoints to DDVP rotor controller and trigger recalculation of scalars if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SPEED_SETPOINT); - _tail_rotor.set_ramp_time(_rsc_ramp_time); - _tail_rotor.set_runup_time(_rsc_runup_time); - _tail_rotor.set_critical_speed(_rsc_critical*0.001f); - _tail_rotor.set_idle_output(_rsc_idle_output*0.001f); + _tail_rotor.set_ramp_time(_main_rotor._ramp_time.get()); + _tail_rotor.set_runup_time(_main_rotor._runup_time.get()); + _tail_rotor.set_critical_speed(_main_rotor._critical_speed.get()); + _tail_rotor.set_idle_output(_main_rotor._idle_output.get()); } else { _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_DISABLED); _tail_rotor.set_ramp_time(0); @@ -282,7 +278,7 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() { // heli uses channels 1,2,3,4 and 8 // setup fast channels - uint32_t mask = 1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_RSC; + uint32_t mask = 1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_RSC; if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) { mask |= 1U << 4; @@ -539,6 +535,14 @@ void AP_MotorsHeli_Single::servo_test() // parameter_check - check if helicopter specific parameters are sensible bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const { + // returns false if direct drive tailspeed is outside of range + if ((_direct_drive_tailspeed < 0) || (_direct_drive_tailspeed > 100)){ + if (display_msg) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_TAIL_SPEED out of range"); + } + return false; + } + // returns false if Phase Angle is outside of range for H3 swashplate if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H3 && (_swashplate.get_phase_angle() > 30 || _swashplate.get_phase_angle() < -30)){ if (display_msg) { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 49eb71ba84..e908a5cc2f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -10,7 +10,6 @@ #include "AP_MotorsHeli_Swash.h" // rsc and extgyro function output channels. -#define AP_MOTORS_HELI_SINGLE_RSC CH_8 #define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7 #define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7 @@ -21,7 +20,7 @@ #define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH 3 // direct-drive variable pitch defaults -#define AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT 500 +#define AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT 50 // default external gyro gain #define AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN 350 @@ -39,7 +38,6 @@ public: AP_MotorsHeli_Single(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : AP_MotorsHeli(loop_rate, speed_hz), - _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC), _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC), _swashplate() { @@ -123,7 +121,6 @@ protected: void servo_test() override; // external objects we depend upon - AP_MotorsHeli_RSC _main_rotor; // main rotor AP_MotorsHeli_RSC _tail_rotor; // tail rotor AP_MotorsHeli_Swash _swashplate; // swashplate