diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index f401f2dfb2..ac820dce61 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -346,7 +346,7 @@ void AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm) bool AP_MotorsHeli::allow_arming() const { // returns false if main rotor speed is not zero - if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _rotor_speed_estimate > 0) { + if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _main_rotor.get_estimated_speed() > 0) { return false; } @@ -366,34 +366,50 @@ bool AP_MotorsHeli::parameter_check() const return true; } +// set_desired_rotor_speed +void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed) +{ + _main_rotor.set_desired_speed(desired_speed); + + if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.set_desired_speed(_direct_drive_tailspeed); + } else { + _tail_rotor.set_desired_speed(0); + } +} + // return true if the main rotor is up to speed bool AP_MotorsHeli::rotor_runup_complete() const { - // if we have no control of motors, assume pilot has spun them up - if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) { - return true; - } - return _heliflags.rotor_runup_complete; } // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters void AP_MotorsHeli::recalc_scalers() { - // recalculate rotor ramp up increment - if (_rsc_ramp_time <= 0) { - _rsc_ramp_time = 1; + if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_SETPOINT) { + _tail_rotor.set_ramp_time(0); + _tail_rotor.set_runup_time(0); + _tail_rotor.set_critical_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); } - _rsc_ramp_increment = 1000.0f / (_rsc_ramp_time * _loop_rate); - // recalculate rotor runup increment - if (_rsc_runup_time <= 0 ) { - _rsc_runup_time = 1; + _main_rotor.recalc_scalers(); + + if (_rsc_mode != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.set_ramp_time(0); + _tail_rotor.set_runup_time(0); + _tail_rotor.set_critical_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); } - if (_rsc_runup_time < _rsc_ramp_time) { - _rsc_runup_time = _rsc_ramp_time; - } - _rsc_runup_increment = 1000.0f / (_rsc_runup_time * _loop_rate); + + _tail_rotor.recalc_scalers(); } // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) @@ -413,18 +429,21 @@ void AP_MotorsHeli::output_armed_not_stabilizing() // sends commands to the motors void AP_MotorsHeli::output_armed_stabilizing() { - // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash - if (_servo_manual == 1) { - _roll_control_input = _roll_radio_passthrough; - _pitch_control_input = _pitch_radio_passthrough; - _throttle_control_input = _throttle_radio_passthrough; - _yaw_control_input = _yaw_radio_passthrough; - } - move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - // update rotor and direct drive esc speeds - rsc_control(); + if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.output_armed(); + + if (!_tail_rotor.is_runup_complete()) + { + _heliflags.rotor_runup_complete = false; + return; + } + } + + _main_rotor.output_armed(); + + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); } // output_armed_zero_throttle - sends commands to the motors @@ -438,8 +457,15 @@ void AP_MotorsHeli::output_armed_zero_throttle() // output_disarmed - sends commands to the motors void AP_MotorsHeli::output_disarmed() { - // stabilizing servos always operate for helicopters - output_armed_stabilizing(); + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.output_disarmed(); + } + + _main_rotor.output_disarmed(); + + _heliflags.rotor_runup_complete = false; } // reset_swash - free up swash for maximum movements. Used for set-up @@ -555,6 +581,14 @@ void AP_MotorsHeli::calculate_roll_pitch_collective_factors() // void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) { + // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash + if (_servo_manual == 1) { + _roll_control_input = _roll_radio_passthrough; + _pitch_control_input = _pitch_radio_passthrough; + _throttle_control_input = _throttle_radio_passthrough; + _yaw_control_input = _yaw_radio_passthrough; + } + int16_t yaw_offset = 0; int16_t coll_out_scaled; @@ -628,7 +662,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll // 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 ((_desired_rotor_speed > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { + if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { // sanity check collective_yaw_effect _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTOR_HELI_COLYAW_RANGE, AP_MOTOR_HELI_COLYAW_RANGE); yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm); @@ -673,158 +707,6 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll } } -// rsc_control - update value to send to tail and main rotor's ESC -// desired_rotor_speed is a desired speed from 0 to 1000 -void AP_MotorsHeli::rsc_control() -{ - // if disarmed output minimums - if (!armed()) { - // shut down tail rotor - if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) { - _tail_direct_drive_out = 0; - write_aux(_tail_direct_drive_out); - } - // 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; - } - - // ramp up or down main rotor and tail - if (_desired_rotor_speed > 0) { - // ramp up tail rotor (this does nothing if not using direct drive variable pitch tail) - tail_ramp(_direct_drive_tailspeed); - // note: this always returns true if not using direct drive variable pitch tail - if (tail_rotor_runup_complete()) { - rotor_ramp(_desired_rotor_speed); - } - }else{ - // shutting down main rotor - rotor_ramp(0); - // shut-down tail rotor. Note: this does nothing if not using direct drive vairable pitch tail - tail_ramp(0); - } - - // 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 (_desired_rotor_speed > 0 || _rotor_speed_estimate > 0) { - // copy yaw output to tail esc - write_aux(_servo_4.servo_out); - }else{ - write_aux(0); - } - } -} - -// rotor_ramp - ramps rotor towards target -// result put in _rotor_out and sent to ESC -void AP_MotorsHeli::rotor_ramp(int16_t rotor_target) -{ - // return immediately if not ramping required - if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) { - _rotor_out = rotor_target; - return; - } - - // range check rotor_target - rotor_target = constrain_int16(rotor_target,0,1000); - - // ramp rotor esc output towards target - if (_rotor_out < rotor_target) { - // allow rotor out to jump to rotor's current speed - if (_rotor_out < _rotor_speed_estimate) { - _rotor_out = _rotor_speed_estimate; - } - // ramp up slowly to target - _rotor_out += _rsc_ramp_increment; - if (_rotor_out > rotor_target) { - _rotor_out = rotor_target; - } - }else{ - // ramping down happens instantly - _rotor_out = rotor_target; - } - - // 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.rotor_runup_complete && rotor_target > 0 && _rotor_speed_estimate >= rotor_target) { - _heliflags.rotor_runup_complete = true; - } - if (_heliflags.rotor_runup_complete && _rotor_speed_estimate <= _rsc_critical) { - _heliflags.rotor_runup_complete = false; - } - - // output to rsc servo - write_rsc(_rotor_out); -} - -// tail_ramp - ramps tail motor towards target. Only used for direct drive variable pitch tails -// results put into _tail_direct_drive_out and sent to ESC -void AP_MotorsHeli::tail_ramp(int16_t tail_target) -{ - // return immediately if not ramping required - if (_tail_type != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_direct_drive_out = tail_target; - return; - } - - // range check tail_target - tail_target = constrain_int16(tail_target,0,1000); - - // ramp towards target - if (_tail_direct_drive_out < tail_target) { - _tail_direct_drive_out += AP_MOTORS_HELI_TAIL_RAMP_INCREMENT; - if (_tail_direct_drive_out >= tail_target) { - _tail_direct_drive_out = tail_target; - } - }else if(_tail_direct_drive_out > tail_target) { - _tail_direct_drive_out -= AP_MOTORS_HELI_TAIL_RAMP_INCREMENT; - if (_tail_direct_drive_out < tail_target) { - _tail_direct_drive_out = tail_target; - } - } - - // output to tail servo - write_aux(_tail_direct_drive_out); -} - -// return true if the tail rotor is up to speed -bool AP_MotorsHeli::tail_rotor_runup_complete() -{ - // always return true if not using direct drive variable pitch tails - if (_tail_type != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { - return true; - } - - // check speed - return (armed() && _tail_direct_drive_out >= _direct_drive_tailspeed); -} - -// write_rsc - outputs pwm onto output rsc channel (ch8) -// servo_out parameter is of the range 0 ~ 1000 -void AP_MotorsHeli::write_rsc(int16_t servo_out) -{ - _servo_rsc.servo_out = servo_out; - _servo_rsc.calc_pwm(); - hal.rcout->write(AP_MOTORS_HELI_RSC, _servo_rsc.radio_out); -} - // write_aux - outputs pwm onto output aux channel (ch7) // servo_out parameter is of the range 0 ~ 1000 void AP_MotorsHeli::write_aux(int16_t servo_out) diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 74f496da69..3d604c2bcd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -11,6 +11,7 @@ #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library #include "AP_Motors.h" +#include "AP_MotorsHeli_RSC.h" // maximum number of swashplate servos #define AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS 3 @@ -103,7 +104,9 @@ public: _servo_1(swash_servo_1), _servo_2(swash_servo_2), _servo_3(swash_servo_3), - _servo_4(yaw_servo) + _servo_4(yaw_servo), + _main_rotor(servo_rotor, AP_MOTORS_HELI_RSC, loop_rate), + _tail_rotor(servo_aux, AP_MOTORS_HELI_AUX, loop_rate) { AP_Param::setup_object_defaults(this, var_info); @@ -170,19 +173,19 @@ public: int16_t get_rsc_setpoint() const { return _rsc_setpoint; } // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000 - void set_desired_rotor_speed(int16_t desired_speed) { _desired_rotor_speed = desired_speed; } + void set_desired_rotor_speed(int16_t desired_speed); // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000 - int16_t get_desired_rotor_speed() const { return _desired_rotor_speed; } + int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); } // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000 - int16_t get_estimated_rotor_speed() { return _rotor_speed_estimate; } + int16_t get_estimated_rotor_speed() { return _main_rotor.get_estimated_speed(); } // return true if the main rotor is up to speed bool rotor_runup_complete() const; // rotor_speed_above_critical - return true if rotor speed is above that critical for flight - bool rotor_speed_above_critical() const { return _rotor_speed_estimate > _rsc_critical; } + bool rotor_speed_above_critical() const { return _main_rotor.get_estimated_speed() > _main_rotor.get_critical_speed(); } // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters void recalc_scalers(); @@ -235,22 +238,6 @@ private: // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position void calculate_roll_pitch_collective_factors(); - // rsc_control - main function to update values to send to main rotor and tail rotor ESCs - void rsc_control(); - - // rotor_ramp - ramps rotor towards target. result put rotor_out and sent to ESC - void rotor_ramp(int16_t rotor_target); - - // tail_ramp - ramps tail motor towards target. Only used for direct drive variable pitch tails - // results put into _tail_direct_drive_out and sent to ESC - void tail_ramp(int16_t tail_target); - - // return true if the tail rotor is up to speed - bool tail_rotor_runup_complete(); - - // write_rsc - outputs pwm onto output rsc channel (ch8). servo_out parameter is of the range 0 ~ 1000 - void write_rsc(int16_t servo_out); - // write_aux - outputs pwm onto output aux channel (ch7). servo_out parameter is of the range 0 ~ 1000 void write_aux(int16_t servo_out); @@ -262,6 +249,9 @@ private: RC_Channel& _servo_3; // swash plate servo #3 RC_Channel& _servo_4; // tail servo + AP_MotorsHeli_RSC _main_rotor; // main rotor + AP_MotorsHeli_RSC _tail_rotor; // tail rotor + // flags bitmask struct heliflags_type { uint8_t swash_initialised : 1; // true if swash has been initialised @@ -303,12 +293,6 @@ private: float _collective_scalar_manual = 1; // collective scalar to reduce the range of the collective movement while collective is being controlled manually (i.e. directly by the pilot) int16_t _collective_out = 0; // actual collective pitch value. Required by the main code for calculating cruise throttle int16_t _collective_mid_pwm = 0; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000) - int16_t _desired_rotor_speed = 0; // latest desired rotor speed from pilot - float _rotor_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000) - float _rsc_ramp_increment = 0.0f; // the amount we can increase the rotor output during each 100hz iteration - float _rsc_runup_increment = 0.0f; // the amount we can increase the rotor's estimated speed during each 100hz iteration - float _rotor_speed_estimate = 0.0f; // estimated speed of the main rotor (0~1000) - int16_t _tail_direct_drive_out = 0; // current ramped speed of output on ch7 when using direct drive variable pitch tail type int16_t _delta_phase_angle = 0; // phase angle dynamic compensation int16_t _roll_radio_passthrough = 0; // roll control PWM direct from radio, used for manual control int16_t _pitch_radio_passthrough = 0; // pitch control PWM direct from radio, used for manual control diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp new file mode 100644 index 0000000000..06dad8cf14 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -0,0 +1,104 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include + +#include "AP_MotorsHeli_RSC.h" + +extern const AP_HAL::HAL& hal; + +// 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 = 1000.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 = 1000.0f / (_runup_time * _loop_rate); +} + +void AP_MotorsHeli_RSC::output_armed() +{ + // 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; + } + + // 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; + } + + // 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; + } + } + + // set runup complete flag + if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) { + _runup_complete = true; + } + + if (_runup_complete && _estimated_speed <= _critical_speed) { + _runup_complete = false; + } + + // output to rsc servo + write_rsc(_speed_out); +} + +void AP_MotorsHeli_RSC::output_disarmed() +{ + write_rsc(0); +} + +// 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_output.servo_out = servo_out; + _servo_output.calc_pwm(); + + hal.rcout->write(_servo_output_channel, _servo_output.radio_out); +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h new file mode 100644 index 0000000000..41ee908282 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -0,0 +1,76 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_MOTORS_HELI_RSC_H__ +#define __AP_MOTORS_HELI_RSC_H__ + +#include +#include +#include +#include + +class AP_MotorsHeli_RSC { +public: + AP_MotorsHeli_RSC(RC_Channel& servo_output, + int8_t servo_output_channel, + uint16_t loop_rate) : + _servo_output(servo_output), + _servo_output_channel(servo_output_channel), + _loop_rate(loop_rate) + {}; + + // set_critical_speed + void set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; } + + // get_critical_speed + int16_t get_critical_speed() const { return _critical_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_estimated_speed + int16_t get_estimated_speed() const { return _estimated_speed; } + + // is_runup_complete + bool is_runup_complete() const { return _runup_complete; } + + // set_ramp_time + void set_ramp_time (int8_t ramp_time) { _ramp_time = ramp_time; } + + // set_runup_time + void set_runup_time (int8_t runup_time) { _runup_time = runup_time; } + + // recalc_scalers + void recalc_scalers(); + + // output_armed + void output_armed (); + + // output_disarmed + void output_disarmed (); + +private: + + // external variables + RC_Channel& _servo_output; + int8_t _servo_output_channel; // output channel to rotor esc + float _loop_rate; // main loop rate + + // internal variables + int16_t _critical_speed = 0; // rotor speed below which flight is not possible + 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) + 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 + 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 we can increase the rotor's estimated speed during each 100hz iteration + bool _runup_complete = false; // flag for determining if runup is complete + + // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000 + void write_rsc(int16_t servo_out); +}; + +#endif // AP_MOTORS_HELI_RSC_H \ No newline at end of file