diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index f99b6a38b4..24f7dedbca 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -28,28 +28,6 @@ void AP_MotorsHeli_RSC::init_servo() RC_Channel_aux::set_aux_channel_default(_aux_fn, _default_channel); } -// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters -void AP_MotorsHeli_RSC::recalc_scalers() -{ - // recalculate rotor ramp up increment - if (_ramp_time <= 0) { - _ramp_time = 1; - } - - _ramp_increment = 1.0f / (_ramp_time * _loop_rate); - - // recalculate rotor runup increment - if (_runup_time <= 0 ) { - _runup_time = 1; - } - - if (_runup_time < _ramp_time) { - _runup_time = _ramp_time; - } - - _runup_increment = 1.0f / (_runup_time * _loop_rate); -} - // set_power_output_range void AP_MotorsHeli_RSC::set_power_output_range(uint16_t power_low, uint16_t power_high) { @@ -67,7 +45,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) update_rotor_ramp(0.0f); // control output forced to zero - _control_output = 0; + _control_output = 0.0f; break; case ROTOR_CONTROL_IDLE: @@ -102,6 +80,11 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input) { + // sanity check ramp time + if (_ramp_time <= 0) { + _ramp_time = 1; + } + // ramp output upwards towards target if (_rotor_ramp_output < rotor_ramp_input) { // allow control output to jump to estimated speed @@ -109,7 +92,7 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input) _rotor_ramp_output = _rotor_runup_output; } // ramp up slowly to target - _rotor_ramp_output += _ramp_increment; + _rotor_ramp_output += (1.0f / (_ramp_time * _loop_rate)); if (_rotor_ramp_output > rotor_ramp_input) { _rotor_ramp_output = rotor_ramp_input; } @@ -122,14 +105,23 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input) // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut void AP_MotorsHeli_RSC::update_rotor_runup() { + // sanity check runup time + if (_runup_time < _ramp_time) { + _runup_time = _ramp_time; + } + if (_runup_time <= 0 ) { + _runup_time = 1; + } + // ramp speed estimate towards control out + float runup_increment = 1.0f / (_runup_time * _loop_rate); if (_rotor_runup_output < _rotor_ramp_output) { - _rotor_runup_output += _runup_increment; + _rotor_runup_output += runup_increment; if (_rotor_runup_output > _rotor_ramp_output) { _rotor_runup_output = _rotor_ramp_output; } }else{ - _rotor_runup_output -= _runup_increment; + _rotor_runup_output -= runup_increment; if (_rotor_runup_output < _rotor_ramp_output) { _rotor_runup_output = _rotor_ramp_output; } @@ -155,21 +147,21 @@ void AP_MotorsHeli_RSC::update_rotor_runup() } // get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value -int16_t AP_MotorsHeli_RSC::get_rotor_speed() const +float AP_MotorsHeli_RSC::get_rotor_speed() const { // if no actual measured rotor speed is available, estimate speed based on rotor runup scalar. - return (_rotor_runup_output * _max_speed); + return _rotor_runup_output; } // write_rsc - outputs pwm onto output rsc channel -// servo_out parameter is of the range 0 ~ 1000 -void AP_MotorsHeli_RSC::write_rsc(int16_t servo_out) +// servo_out parameter is of the range 0 ~ 1 +void AP_MotorsHeli_RSC::write_rsc(float servo_out) { if (_control_mode == ROTOR_CONTROL_MODE_DISABLED){ // do not do servo output to avoid conflicting with other output on the channel // ToDo: We should probably use RC_Channel_Aux to avoid this problem return; } else { - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_heli_rsc, servo_out); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_heli_rsc, servo_out * 1000.0f); } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 5d886df3d7..a5b10d17fc 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -38,26 +38,26 @@ public: void set_control_mode(RotorControlMode mode) { _control_mode = mode; } // set_critical_speed - void set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; } + void set_critical_speed(float critical_speed) { _critical_speed = critical_speed; } // get_critical_speed - int16_t get_critical_speed() const { return _critical_speed; } + float get_critical_speed() const { return _critical_speed; } // set_idle_output - void set_idle_output(int16_t idle_output) { _idle_output = idle_output; } float get_idle_output() { return _idle_output; } + void set_idle_output(float idle_output) { _idle_output = idle_output; } // get_desired_speed - int16_t get_desired_speed() const { return _desired_speed; } + float get_desired_speed() const { return _desired_speed; } // set_desired_speed - void set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; } + void set_desired_speed(float desired_speed) { _desired_speed = desired_speed; } // get_control_speed - int16_t get_control_output() const { return _control_output; } + float get_control_output() const { return _control_output; } // get_rotor_speed - return estimated or measured rotor speed - int16_t get_rotor_speed() const; + float get_rotor_speed() const; // is_runup_complete bool is_runup_complete() const { return _runup_complete; } @@ -74,9 +74,6 @@ public: // set_motor_load void set_motor_load(float load) { _load_feedforward = load; } - // recalc_scalers - void recalc_scalers(); - // output - update value to send to ESC/Servo void output(RotorControlState state); @@ -91,22 +88,19 @@ private: // internal variables RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED; // motor control mode, Passthrough or Setpoint - int16_t _critical_speed = 0; // rotor speed below which flight is not possible - int16_t _idle_output = 0; // motor output idle speed - int16_t _max_speed = 1000; // rotor maximum speed. Placeholder value until we have measured speed input (ToDo) - int16_t _desired_speed = 0; // latest desired rotor speed from pilot - int16_t _control_output = 0; // latest logic controlled output - float _rotor_ramp_output = 0; // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f) - float _rotor_runup_output = 0; // scalar used to store status of rotor run-up time (0.0-1.0f) - float _ramp_increment = 0; // the amount to increase/decrease the rotor ramp scalar during each iteration + float _critical_speed = 0.0f; // rotor speed below which flight is not possible + float _idle_output = 0.0f; // motor output idle speed + float _desired_speed = 0.0f; // latest desired rotor speed from pilot + float _control_output = 0.0f; // latest logic controlled output + float _rotor_ramp_output = 0.0f; // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f) + float _rotor_runup_output = 0.0f; // scalar used to store status of rotor run-up time (0.0-1.0f) int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed int8_t _runup_time = 0; // time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time - float _runup_increment = 0; // the amount to increase/decrease the rotor run-up scalar during each iteration bool _runup_complete = false; // flag for determining if runup is complete - uint16_t _power_output_low = 0; // setpoint for power output at minimum rotor power - uint16_t _power_output_high = 0; // setpoint for power output at maximum rotor power - uint16_t _power_output_range = 0; // maximum range of output power - float _load_feedforward = 0; // estimate of motor load, range 0-1.0f + float _power_output_low = 0.0f; // setpoint for power output at minimum rotor power + float _power_output_high = 0.0f; // setpoint for power output at maximum rotor power + float _power_output_range = 0.0f; // maximum range of output power + float _load_feedforward = 0.0f; // estimate of motor load, range 0-1.0f // 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); @@ -114,6 +108,6 @@ private: // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut void update_rotor_runup(); - // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000 - void write_rsc(int16_t servo_out); + // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1 + void write_rsc(float servo_out); };