From 5cd4b789188bcf3d1dc33c70a91ad453b1ba6de6 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 22 Jan 2016 10:19:43 +0900 Subject: [PATCH] AP_MotorsSingle: remove reverse parameters No longer necessary because we can use individual servo reverse params --- libraries/AP_Motors/AP_MotorsSingle.cpp | 27 ++++++------------------- libraries/AP_Motors/AP_MotorsSingle.h | 7 ++----- 2 files changed, 8 insertions(+), 26 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 7640d42df6..1c79c87b8c 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -35,23 +35,9 @@ const AP_Param::GroupInfo AP_MotorsSingle::var_info[] = { // parameters 30 ~ 39 reserved for tricopter // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) - // @Param: ROLL_SV_REV - // @DisplayName: Reverse roll feedback - // @Description: Ensure the feedback is negative - // @Values: -1:Reversed,1:Normal - AP_GROUPINFO("ROLL_SV_REV", 40, AP_MotorsSingle, _roll_reverse, AP_MOTORS_SING_POSITIVE), - - // @Param: PITCH_SV_REV - // @DisplayName: Reverse pitch feedback - // @Description: Ensure the feedback is negative - // @Values: -1:Reversed,1:Normal - AP_GROUPINFO("PITCH_SV_REV", 41, AP_MotorsSingle, _pitch_reverse, AP_MOTORS_SING_POSITIVE), - - // @Param: YAW_SV_REV - // @DisplayName: Reverse yaw feedback - // @Description: Ensure the feedback is negative - // @Values: -1:Reversed,1:Normal - AP_GROUPINFO("YAW_SV_REV", 42, AP_MotorsSingle, _yaw_reverse, AP_MOTORS_SING_POSITIVE), + // 40 was ROLL_SV_REV + // 41 was PITCH_SV_REV + // 42 was YAW_SV_REV // @Param: SV_SPEED // @DisplayName: Servo speed @@ -216,10 +202,9 @@ void AP_MotorsSingle::output_armed_stabilizing() float actuator_max = 0.0f; // maximum actuator value // apply voltage and air pressure compensation - // todo: we shouldn't need input reversing with servo reversing - roll_thrust = _roll_reverse * get_roll_thrust() * get_compensation_gain(); - pitch_thrust = _pitch_reverse * get_pitch_thrust() * get_compensation_gain(); - yaw_thrust = _yaw_reverse * get_yaw_thrust() * get_compensation_gain(); + roll_thrust = get_roll_thrust() * get_compensation_gain(); + pitch_thrust = get_pitch_thrust() * get_compensation_gain(); + yaw_thrust = get_yaw_thrust() * get_compensation_gain(); throttle_thrust = get_throttle() * get_compensation_gain(); // sanity check throttle is above zero and below current limited throttle diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index f12e04bf1c..f548220334 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -66,11 +66,8 @@ protected: // calc_yaw_radio_output - calculate final pwm output for yaw channel int16_t calc_pivot_radio_output(float yaw_input, const RC_Channel& servo); - // We shouldn't need roll, pitch, and yaw reversing with servo reversing. - AP_Int8 _roll_reverse; // Reverse roll output - AP_Int8 _pitch_reverse; // Reverse pitch output - AP_Int8 _yaw_reverse; // Reverse yaw output - AP_Int16 _servo_speed; // servo speed + // servo speed + AP_Int16 _servo_speed; RC_Channel _servo1; RC_Channel _servo2;