diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index e804b6366a..f487660f2b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -61,10 +61,10 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @Param: SV_MAN // @DisplayName: Manual Servo Mode - // @Description: Pass radio inputs directly to servos for set-up. Do not set this manually! - // @Values: 0:Disabled,1:Enabled + // @Description: Manual servo override for swash set-up. Do not set this manually! + // @Values: 0:Disabled,1:Passthrough,2:Center // @User: Standard - AP_GROUPINFO("SV_MAN", 6, AP_MotorsHeli, _servo_manual, 0), + AP_GROUPINFO("SV_MAN", 6, AP_MotorsHeli, _servo_manual, AP_MOTORS_HELI_MANUAL_OFF), // @Param: GOV_SETPOINT // @DisplayName: External Motor Governor Setpoint @@ -162,7 +162,7 @@ void AP_MotorsHeli::Init() set_update_rate(_speed_hz); // ensure inputs are not passed through to servos on start-up - _servo_manual = 0; + _servo_manual = AP_MOTORS_HELI_MANUAL_OFF; // initialise Servo/PWM ranges and endpoints init_outputs(); @@ -209,7 +209,7 @@ void AP_MotorsHeli::output() void AP_MotorsHeli::output_armed_stabilizing() { // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_manual == 1) { + if (_servo_manual != AP_MOTORS_HELI_MANUAL_OFF) { reset_flight_controls(); } @@ -221,7 +221,7 @@ void AP_MotorsHeli::output_armed_stabilizing() void AP_MotorsHeli::output_armed_not_stabilizing() { // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_manual == 1) { + if (_servo_manual != AP_MOTORS_HELI_MANUAL_OFF) { reset_flight_controls(); } @@ -235,7 +235,7 @@ void AP_MotorsHeli::output_armed_not_stabilizing() void AP_MotorsHeli::output_armed_zero_throttle() { // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_manual == 1) { + if (_servo_manual != AP_MOTORS_HELI_MANUAL_OFF) { reset_flight_controls(); } @@ -247,12 +247,19 @@ void AP_MotorsHeli::output_armed_zero_throttle() // 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) { + // if manual override (i.e. when setting up swash) + if (_servo_manual == AP_MOTORS_HELI_MANUAL_PASSTHROUGH) { + // pass pilot commands straight through to swash _roll_control_input = _roll_radio_passthrough; _pitch_control_input = _pitch_radio_passthrough; _throttle_control_input = _throttle_radio_passthrough; _yaw_control_input = _yaw_radio_passthrough; + } else if (_servo_manual == AP_MOTORS_HELI_MANUAL_CENTER) { + // center and fixate the swash + _roll_control_input = 0; + _pitch_control_input = 0; + _throttle_control_input = 500; + _yaw_control_input = 0; } // ensure swash servo endpoints haven't been moved @@ -347,7 +354,7 @@ void AP_MotorsHeli::reset_radio_passthrough() void AP_MotorsHeli::reset_flight_controls() { reset_radio_passthrough(); - _servo_manual = 0; + _servo_manual = AP_MOTORS_HELI_MANUAL_OFF; init_outputs(); calculate_scalars(); } diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 0b58aae89e..718cee3480 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -54,6 +54,11 @@ #define AP_MOTORS_HELI_NOFLYBAR 0 #define AP_MOTORS_HELI_FLYBAR 1 +// manual servo modes (used for setup) +#define AP_MOTORS_HELI_MANUAL_OFF 0 +#define AP_MOTORS_HELI_MANUAL_PASSTHROUGH 1 +#define AP_MOTORS_HELI_MANUAL_CENTER 2 + class AP_HeliControls; /// @class AP_MotorsHeli