diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index a658871700..eed4781a5d 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -168,13 +168,28 @@ void AP_MotorsHeli::Init() // ensure inputs are not passed through to servos on start-up _servo_manual = 0; - // initialise swash plate + // initialise Servo/PWM ranges and endpoints init_outputs(); // calculate all scalars calculate_scalars(); } +// output_min - sets servos to neutral point with motors stopped +void AP_MotorsHeli::output_min() +{ + // move swash to mid + move_actuators(0,0,500,0); + + update_motor_control(ROTOR_CONTROL_STOP); + + // override limits flags + limit.roll_pitch = true; + limit.yaw = true; + limit.throttle_lower = true; + limit.throttle_upper = false; +} + // output - sends commands to the servos void AP_MotorsHeli::output() { @@ -194,6 +209,68 @@ void AP_MotorsHeli::output() } }; +// sends commands to the motors +void AP_MotorsHeli::output_armed_stabilizing() +{ + // if manual override active after arming, deactivate it and reinitialize servos + if (_servo_manual == 1) { + reset_flight_controls(); + } + + move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + update_motor_control(ROTOR_CONTROL_ACTIVE); +} + +void AP_MotorsHeli::output_armed_not_stabilizing() +{ + // if manual override active after arming, deactivate it and reinitialize servos + if (_servo_manual == 1) { + reset_flight_controls(); + } + + // helicopters always run stabilizing flight controls + move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + update_motor_control(ROTOR_CONTROL_ACTIVE); +} + +// output_armed_zero_throttle - sends commands to the motors +void AP_MotorsHeli::output_armed_zero_throttle() +{ + // if manual override active after arming, deactivate it and reinitialize servos + if (_servo_manual == 1) { + reset_flight_controls(); + } + + move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + update_motor_control(ROTOR_CONTROL_IDLE); +} + +// output_disarmed - sends commands to the motors +void AP_MotorsHeli::output_disarmed() +{ + // 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; + } + + // ensure swash servo endpoints haven't been moved + init_outputs(); + + // continuously recalculate scalars to allow setup + calculate_scalars(); + + // helicopters always run stabilizing flight controls + move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + update_motor_control(ROTOR_CONTROL_STOP); +} + // parameter_check - check if helicopter specific parameters are sensible bool AP_MotorsHeli::parameter_check() const { @@ -257,3 +334,12 @@ void AP_MotorsHeli::reset_radio_passthrough() _throttle_radio_passthrough = 500; _yaw_radio_passthrough = 0; } + +// reset_flight_controls - resets all controls and scalars to flight status +void AP_MotorsHeli::reset_flight_controls() +{ + reset_radio_passthrough(); + _servo_manual = 0; + init_outputs(); + calculate_scalars(); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 4e99ff2d1a..b5dbbd89bb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -89,8 +89,8 @@ public: // enable - starts allowing signals to be sent to motors virtual void enable() = 0; - // output_min - sets servos to neutral point - virtual void output_min() = 0; + // output_min - sets servos to neutral point with motors stopped + void output_min(); // output_test - spin a motor at the pwm value specified // motor_seq is the motor's sequence number from 1 to the number of motors on the frame @@ -172,10 +172,16 @@ public: protected: // output - sends commands to the motors - virtual void output_armed_stabilizing() = 0; - virtual void output_armed_not_stabilizing() = 0; - virtual void output_armed_zero_throttle() = 0; - virtual void output_disarmed() = 0; + void output_armed_stabilizing(); + void output_armed_not_stabilizing(); + void output_armed_zero_throttle(); + void output_disarmed(); + + // update_motor_controls - sends commands to motor controllers + virtual void update_motor_control(uint8_t state) = 0; + + // reset_flight_controls - resets all controls and scalars to flight status + void reset_flight_controls(); // update the throttle input filter void update_throttle_filter(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index b463a00791..b8538770cc 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -317,131 +317,15 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC); } -// output_min - sets servos to neutral point -void AP_MotorsHeli_Single::output_min() +// update_motor_controls - sends commands to motor controllers +void AP_MotorsHeli_Single::update_motor_control(uint8_t state) { - // move swash to mid - move_actuators(0,0,500,0); + // Send state update to motors + _tail_rotor.output(state); + _main_rotor.output(state); - _main_rotor.output(ROTOR_CONTROL_STOP); - _tail_rotor.output(ROTOR_CONTROL_STOP); - - // override limits flags - limit.roll_pitch = true; - limit.yaw = true; - limit.throttle_lower = true; - limit.throttle_upper = false; -} - -// sends commands to the motors -void AP_MotorsHeli_Single::output_armed_stabilizing() -{ - // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_manual == 1) { - reset_radio_passthrough(); - _servo_manual = 0; - init_outputs(); - calculate_scalars(); - } - - move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.output(ROTOR_CONTROL_ACTIVE); - - if (!_tail_rotor.is_runup_complete()) - { - _heliflags.rotor_runup_complete = false; - return; - } - } - - _main_rotor.output(ROTOR_CONTROL_ACTIVE); - - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); -} - -void AP_MotorsHeli_Single::output_armed_not_stabilizing() -{ - // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_manual == 1) { - reset_radio_passthrough(); - _servo_manual = 0; - init_outputs(); - calculate_scalars(); - } - - move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.output(ROTOR_CONTROL_ACTIVE); - - if (!_tail_rotor.is_runup_complete()) - { - _heliflags.rotor_runup_complete = false; - return; - } - } - - _main_rotor.output(ROTOR_CONTROL_ACTIVE); - - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); -} - -// output_armed_zero_throttle - sends commands to the motors -void AP_MotorsHeli_Single::output_armed_zero_throttle() -{ - // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_manual == 1) { - reset_radio_passthrough(); - _servo_manual = 0; - init_outputs(); - calculate_scalars(); - } - - move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.output(ROTOR_CONTROL_IDLE); - - if (!_tail_rotor.is_runup_complete()) - { - _heliflags.rotor_runup_complete = false; - return; - } - } - - _main_rotor.output(ROTOR_CONTROL_IDLE); - - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); -} - -// output_disarmed - sends commands to the motors -void AP_MotorsHeli_Single::output_disarmed() -{ - // 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; - } - - // ensure swash servo endpoints haven't been moved - init_outputs(); - - // continuously recalculate scalars to allow setup - calculate_scalars(); - - move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.output(ROTOR_CONTROL_STOP); - } - - _main_rotor.output(ROTOR_CONTROL_STOP); - - _heliflags.rotor_runup_complete = false; + // Check if both rotors are run-up, tail rotor controller always returns true if not enabled + _heliflags.rotor_runup_complete = ( _main_rotor.is_runup_complete() && _tail_rotor.is_runup_complete() ); } // set_delta_phase_angle for setting variable phase angle compensation and force diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 45e8145044..d46599125e 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -78,9 +78,6 @@ public: // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 void output_test(uint8_t motor_seq, int16_t pwm); - // output_min - sets servos to neutral point - void output_min(); - // allow_arming - returns true if main rotor is spinning and it is ok to arm bool allow_arming() const; @@ -128,15 +125,12 @@ public: protected: - // output - sends commands to the motors - void output_armed_stabilizing(); - void output_armed_not_stabilizing(); - void output_armed_zero_throttle(); - void output_disarmed(); - // init_outputs - initialise Servo/PWM ranges and endpoints void init_outputs(); + // update_motor_controls - sends commands to motor controllers + void update_motor_control(uint8_t state); + // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position void calculate_roll_pitch_collective_factors();