From b71c6bfd76a73aa0933b0443b875febc704d881a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 9 Nov 2013 13:38:33 +0900 Subject: [PATCH] TradHeli: add RSC_RUNUP_TIME param and rotor speed estimate --- libraries/AP_Motors/AP_MotorsHeli.cpp | 59 +++++++++++++++++++-------- libraries/AP_Motors/AP_MotorsHeli.h | 10 ++++- 2 files changed, 49 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 11aac0d193..3058e78dad 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -199,12 +199,20 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Param: RSC_RAMP_TIME // @DisplayName: RSC Ramp Time - // @Description: Time in seconds for the main rotor to reach full speed + // @Description: Time in seconds for the output to the main rotor's ESC to reach full speed // @Range: 0 60 // @Units: Seconds // @User: Standard AP_GROUPINFO("RSC_RAMP_TIME", 22, 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: Seconds + // @User: Standard + AP_GROUPINFO("RSC_RUNUP_TIME", 23, AP_MotorsHeli,_rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME), + // @Param: TAIL_SPEED // @DisplayName: Direct Drive VarPitch Tail ESC speed // @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch @@ -387,6 +395,14 @@ void AP_MotorsHeli::recalc_scalers() } _rsc_ramp_increment = 1000.0f / (_rsc_ramp_time * 100.0f); + // recalculate rotor runup increment + if (_rsc_runup_time <= 0 ) { + _rsc_runup_time = 1; + } + if (_rsc_runup_time < _rsc_ramp_time) { + _rsc_runup_time = _rsc_ramp_time; + } + _rsc_runup_increment = 1000.0f / (_rsc_runup_time * 100.0f); } // @@ -674,6 +690,7 @@ void AP_MotorsHeli::rsc_control() // shut down main rotor if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE) { _rotor_out = 0; + _rotor_speed_estimate = 0; write_rsc(_rotor_out); } return; @@ -691,7 +708,7 @@ void AP_MotorsHeli::rsc_control() // shutting down main rotor rotor_ramp(0); // if completed shutting down main motor then shut-down tail rotor. Note: this does nothing if not using direct drive vairable pitch tail - if (_rotor_out == 0) { + if (_rotor_speed_estimate <= 0) { tail_ramp(0); } } @@ -699,7 +716,7 @@ void AP_MotorsHeli::rsc_control() // direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) { // output fixed-pitch speed control if Ch8 is high - if (_rotor_desired > 0 || _rotor_out > 0) { + if (_rotor_desired > 0 || _rotor_speed_estimate > 0) { // copy yaw output to tail esc write_aux(_servo_4->servo_out); }else{ @@ -712,8 +729,6 @@ void AP_MotorsHeli::rsc_control() // result put in _rotor_out and sent to ESC void AP_MotorsHeli::rotor_ramp(int16_t rotor_target) { - bool ramp_down = false; // true if we are ramping down so that we immediately output the target to the rotor ESC - // return immediately if not ramping required if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) { _rotor_out = rotor_target; @@ -723,36 +738,44 @@ void AP_MotorsHeli::rotor_ramp(int16_t rotor_target) // range check rotor_target rotor_target = constrain_int16(rotor_target,0,1000); - // ramp towards target + // initialise rotor_out to our estimated rotor speed + _rotor_out = _rotor_speed_estimate; + + // ramp rotor esc output towards target if (_rotor_out < rotor_target) { + // ramp up slowly to target _rotor_out += _rsc_ramp_increment; if (_rotor_out > rotor_target) { _rotor_out = rotor_target; } - }else if(_rotor_out > rotor_target) { - _rotor_out -= _rsc_ramp_increment; - if (_rotor_out < rotor_target) { + }else{ + // ramping down happens instantly _rotor_out = rotor_target; } - ramp_down = true; + + // ramp rotor speed estimate towards rotor out + if (_rotor_speed_estimate < _rotor_out) { + _rotor_speed_estimate += _rsc_runup_increment; + if (_rotor_speed_estimate > _rotor_out) { + _rotor_speed_estimate = _rotor_out; + } + }else{ + _rotor_speed_estimate -= _rsc_runup_increment; + if (_rotor_speed_estimate < _rotor_out) { + _rotor_speed_estimate = _rotor_out; + } } // set runup complete flag - if (!_heliflags.motor_runup_complete && rotor_target > 0 && _rotor_out >= rotor_target) { + if (!_heliflags.motor_runup_complete && rotor_target > 0 && _rotor_speed_estimate >= rotor_target) { _heliflags.motor_runup_complete = true; } - if (_heliflags.motor_runup_complete && rotor_target == 0 && _rotor_out <= 0) { + if (_heliflags.motor_runup_complete && rotor_target == 0 && _rotor_speed_estimate <= 0) { _heliflags.motor_runup_complete = false; } // output to rsc servo - if (ramp_down) { - // if ramping down we immediately output the target to the motors - // the _rotor_out is just an estimate of how fast the rotor is actually spinning - write_rsc(rotor_target); - }else{ write_rsc(_rotor_out); - } } // tail_ramp - ramps tail motor towards target. Only used for direct drive variable pitch tails diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 2113bb5087..e502e54457 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -73,7 +73,8 @@ #define AP_MOTORS_HELI_RSC_SETPOINT 500 // default main rotor ramp up time in seconds -#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 seconds (most people use exterrnal govenors so we can ramp up output to rotor quickly) +#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to full power (most people use exterrnal govenors so we can ramp up quickly) +#define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed #define AP_MOTORS_HELI_TAIL_RAMP_INCREMENT 5 // 5 is 2 seconds for direct drive tail rotor to reach to full speed (5 = (2sec*100hz)/1000) // motor run-up time default in 100th of seconds @@ -117,6 +118,8 @@ public: _rotor_desired(0), _rotor_out(0), _rsc_ramp_increment(0.0f), + _rsc_runup_increment(0.0f), + _rotor_speed_estimate(0.0f), _tail_direct_drive_out(0) { AP_Param::setup_object_defaults(this, var_info); @@ -265,7 +268,8 @@ private: AP_Int16 _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. AP_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active - AP_Int8 _rsc_ramp_time; // Time in seconds to ramp up the main rotor to full speed + AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach full speed + AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode AP_Int8 _manual_collective_min; // Minimum collective position while pilot directly controls the collective AP_Int8 _manual_collective_max; // Maximum collective position while pilot directly controls the collective @@ -285,6 +289,8 @@ private: int16_t _rotor_desired; // latest desired rotor speed from pilot float _rotor_out; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000) float _rsc_ramp_increment; // the amount we can increase the rotor output during each 100hz iteration + float _rsc_runup_increment; // the amount we can increase the rotor's estimated speed during each 100hz iteration + float _rotor_speed_estimate; // estimated speed of the main rotor (0~1000) int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type };