From 71866be652b0415845ce629a23204b0045956075 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 2 Feb 2016 21:25:31 +0900 Subject: [PATCH] AP_MotorsHeliSingle: move_yaw in -1 to +1 range --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 19 +++++++++++-------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 2 +- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 1b77cb2e27..1056180223 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -436,17 +436,19 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float } // move_yaw -void AP_MotorsHeli_Single::move_yaw(int16_t yaw_out) +void AP_MotorsHeli_Single::move_yaw(float yaw_out) { - _yaw_servo.servo_out = constrain_int16(yaw_out, -4500, 4500); - - if (_yaw_servo.servo_out != yaw_out) { + // sanity check yaw_out + if (yaw_out < -1.0f) { + yaw_out = -1.0f; + limit.yaw = true; + } + if (yaw_out > 1.0f) { + yaw_out = 1.0f; limit.yaw = true; } - _yaw_servo.calc_pwm(); - - rc_write(AP_MOTORS_MOT_4, _yaw_servo.radio_out); + rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(yaw_out, _yaw_servo)); if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { // output gain to exernal gyro @@ -457,7 +459,8 @@ void AP_MotorsHeli_Single::move_yaw(int16_t yaw_out) } } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) { // output yaw servo to tail rsc - write_aux(_yaw_servo.servo_out); + // To-Do: fix this messy calculation + write_aux((yaw_out*0.5f+1.0f) * 1000.0f); } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 2fb6967e90..857f88ebe1 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -146,7 +146,7 @@ protected: void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out); // move_yaw - moves the yaw servo - void move_yaw(int16_t yaw_out); + void move_yaw(float yaw_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);