From eaeaa3811adb384702e0aed1d71a5e92271b9eae Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Sun, 13 May 2012 12:36:46 +0900
Subject: [PATCH] ArduCopter: bug fix for reversing tri servo

Extended AP_MotorsTri class to take in pointer to rc_tail servo (rc_7) and we use this servo's REV parameter to determine whether to reverse the output to the tail servo or not
---
 ArduCopter/ArduCopter.pde            | 6 ++++++
 libraries/AP_Motors/AP_MotorsTri.cpp | 4 +++-
 libraries/AP_Motors/AP_MotorsTri.h   | 5 ++++-
 3 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 37d1dafc7..d2b1ba7d2 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -411,6 +411,12 @@ static byte 	old_control_mode = STABILIZE;
 	#else
 		MOTOR_CLASS	motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4);
 	#endif
+#elif FRAME_CONFIG == TRI_FRAME  // tri constructor requires additional rc_7 argument to allow tail servo reversing
+	#if INSTANT_PWM == 1
+		MOTOR_CLASS	motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7, AP_MOTORS_SPEED_INSTANT_PWM);   // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2
+	#else
+		MOTOR_CLASS	motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);
+	#endif
 #else
 	#if INSTANT_PWM == 1
 		MOTOR_CLASS	motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, AP_MOTORS_SPEED_INSTANT_PWM);   // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2
diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp
index 34bcbc445..c49ce87e4 100644
--- a/libraries/AP_Motors/AP_MotorsTri.cpp
+++ b/libraries/AP_Motors/AP_MotorsTri.cpp
@@ -128,7 +128,9 @@ void AP_MotorsTri::output_armed()
 
 	// also send out to tail command (we rely on any auto pilot to have updated the rc_yaw->radio_out to the correct value)
 	// note we do not save the radio_out to the motor_out array so it may not appear in the ch7out in the status screen of the mission planner
-	if( _rc_yaw->get_reverse() == true ) {
+	// note: we use _rc_tail's (aka channel 7's) REV parameter to control whether the servo is reversed or not but this is a bit nonsensical.
+	//       a separate servo object (including min, max settings etc) would be better or at least a separate parameter to specify the direction of the tail servo
+	if( _rc_tail->get_reverse() == true ) {
 		_rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_trim - (_rc_yaw->radio_out - _rc_yaw->radio_trim));
 	}else{
 		_rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_out);
diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h
index 14616ec7b..95e047647 100644
--- a/libraries/AP_Motors/AP_MotorsTri.h
+++ b/libraries/AP_Motors/AP_MotorsTri.h
@@ -21,7 +21,9 @@ class AP_MotorsTri : public AP_Motors {
 public: 
 
 	/// Constructor 
-	AP_MotorsTri( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
+	AP_MotorsTri( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, RC_Channel* rc_tail, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
+		AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
+		_rc_tail(rc_tail) {};
 
 	// init
 	virtual void Init();
@@ -46,6 +48,7 @@ protected:
 	virtual void output_armed();
 	virtual void output_disarmed();
 
+	RC_Channel*	_rc_tail;	// REV parameter used from this channel to determine direction of tail servo movement
 }; 
 
 #endif  // AP_MOTORSTRI
\ No newline at end of file
-- 
GitLab