diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index d9b094f027..7685594fc5 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -43,63 +43,76 @@ void AP_MotorsHeli_RSC::recalc_scalers() _runup_increment = 1000.0f / (_runup_time * _loop_rate); } +// output - update value to send to ESC/Servo void AP_MotorsHeli_RSC::output(uint8_t state) { switch (state){ + case ROTOR_CONTROL_STOP: + _control_speed = 0; // ramp input to zero + _control_out = 0; // force ramp output to zero + _estimated_speed = 0; // force speed estimate to zero + break; - case ROTOR_CONTROL_STOP: - - _estimated_speed = 0; - _speed_out = 0; - write_rsc(0); - break; - - case ROTOR_CONTROL_IDLE: - case ROTOR_CONTROL_ACTIVE: - - // ramp rotor esc output towards target - if (_speed_out < _desired_speed) { - // allow rotor out to jump to rotor's current speed - if (_speed_out < _estimated_speed) { - _speed_out = _estimated_speed; + case ROTOR_CONTROL_IDLE: + _control_speed = _idle_speed; // set control speed to idle speed + if (_control_out < _idle_speed){ + _control_out = _idle_speed; // if control output is less than idle speed, force ramp function to jump to idle speed } + break; - // ramp up slowly to target - _speed_out += _ramp_increment; - if (_speed_out > _desired_speed) { - _speed_out = _desired_speed; - } - } else { - // ramping down happens instantly - _speed_out = _desired_speed; + case ROTOR_CONTROL_ACTIVE: + _control_speed = _desired_speed; // set control speed to desired speed + break; + } + + // run speed ramp function to slew output smoothly, also updates estimated speed + speed_ramp(_control_speed); + + // output to rsc servo + write_rsc(_control_out); +} + +// speed_ramp - ramps speed towards target, result put in _control_out +void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target) +{ + // range check speed_target + speed_target = constrain_int16(speed_target,0,1000); + + // ramp output upwards towards target + if (_control_out < speed_target) { + // allow control output to jump to estimated speed + if (_control_out < _estimated_speed) { + _control_out = _estimated_speed; } - - // ramp rotor speed estimate towards speed out - if (_estimated_speed < _speed_out) { - _estimated_speed += _runup_increment; - if (_estimated_speed > _speed_out) { - _estimated_speed = _speed_out; - } - } else { - _estimated_speed -= _runup_increment; - if (_estimated_speed < _speed_out) { - _estimated_speed = _speed_out; - } + // ramp up slowly to target + _control_out += _ramp_increment; + if (_control_out > speed_target) { + _control_out = speed_target; } + }else{ + // ramping down happens instantly + _control_out = speed_target; + } - // set runup complete flag - if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) { - _runup_complete = true; + // ramp speed estimate towards control out + if (_estimated_speed < _control_out) { + _estimated_speed += _runup_increment; + if (_estimated_speed > _control_out) { + _estimated_speed = _control_out; } - - if (_runup_complete && _estimated_speed <= _critical_speed) { - _runup_complete = false; + }else{ + _estimated_speed -= _runup_increment; + if (_estimated_speed < _control_out) { + _estimated_speed = _control_out; } + } - // output to rsc servo - write_rsc(_speed_out); - - break; + // set runup complete flag + if (!_runup_complete && speed_target > 0 && _estimated_speed >= speed_target) { + _runup_complete = true; + } + if (_runup_complete && _estimated_speed <= _critical_speed) { + _runup_complete = false; } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index a2fcda5e9c..35feacc6f4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -28,12 +28,18 @@ public: // get_critical_speed int16_t get_critical_speed() const { return _critical_speed; } + // set_idle_speed + void set_idle_speed(int16_t idle_speed) { _idle_speed = idle_speed; } + // get_desired_speed int16_t get_desired_speed() const { return _desired_speed; } // set_desired_speed void set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; } + // get_control_speed + int16_t get_control_speed() const { return _control_speed; } + // get_estimated_speed int16_t get_estimated_speed() const { return _estimated_speed; } @@ -49,7 +55,7 @@ public: // recalc_scalers void recalc_scalers(); - // output_armed + // output - update value to send to ESC/Servo void output(uint8_t state); private: @@ -61,8 +67,10 @@ private: // internal variables int16_t _critical_speed = 0; // rotor speed below which flight is not possible + int16_t _idle_speed = 0; // motor output idle speed int16_t _desired_speed = 0; // latest desired rotor speed from pilot - float _speed_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000) + int16_t _control_speed = 0; // latest logic controlled rotor speed + float _control_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000) float _estimated_speed = 0; // estimated speed of the main rotor (0~1000) float _ramp_increment = 0; // the amount we can increase the rotor output during each 100hz iteration int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed @@ -70,6 +78,9 @@ private: float _runup_increment = 0; // the amount we can increase the rotor's estimated speed during each 100hz iteration bool _runup_complete = false; // flag for determining if runup is complete + // speed_ramp - ramps speed towards target, result put in _control_out + void speed_ramp(int16_t rotor_target); + // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000 void write_rsc(int16_t servo_out); }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 692a4deca8..d0dff476a0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -224,10 +224,12 @@ void AP_MotorsHeli_Single::recalc_scalers() _main_rotor.set_ramp_time(0); _main_rotor.set_runup_time(0); _main_rotor.set_critical_speed(0); + _main_rotor.set_idle_speed(0); } else { _main_rotor.set_ramp_time(_rsc_ramp_time); _main_rotor.set_runup_time(_rsc_runup_time); _main_rotor.set_critical_speed(_rsc_critical); + _main_rotor.set_idle_speed(_rsc_idle); } _main_rotor.recalc_scalers(); @@ -236,10 +238,12 @@ void AP_MotorsHeli_Single::recalc_scalers() _tail_rotor.set_ramp_time(0); _tail_rotor.set_runup_time(0); _tail_rotor.set_critical_speed(0); + _tail_rotor.set_idle_speed(0); } else { _tail_rotor.set_ramp_time(_rsc_ramp_time); _tail_rotor.set_runup_time(_rsc_runup_time); _tail_rotor.set_critical_speed(_rsc_critical); + _tail_rotor.set_idle_speed(_rsc_idle); } _tail_rotor.recalc_scalers(); @@ -494,7 +498,7 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16 // rudder feed forward based on collective // the feed-forward is not required when the motor is shut down and not creating torque // also not required if we are using external gyro - if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { + if ((_main_rotor.get_control_speed() > 0) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { // sanity check collective_yaw_effect _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE); yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);