AP_Motors: use SRV_Channel for tri tail servo

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -20,6 +20,7 @@
  */
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
 #include "AP_MotorsTri.h"
 
 extern const AP_HAL::HAL& hal;
@@ -32,40 +33,6 @@ const AP_Param::GroupInfo AP_MotorsTri::var_info[] = {
     // parameters 30 ~ 39 reserved for tricopter
     // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files)
 
-    // @Param: YAW_SV_REV
-    // @DisplayName: Yaw Servo Reverse
-    // @Description: Yaw servo reversing. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
-    // @Values: -1:Reversed,1:Normal
-    // @User: Standard
-    AP_GROUPINFO("YAW_SV_REV", 31,     AP_MotorsTri,  _yaw_reverse, 1),
-
-    // @Param: YAW_SV_TRIM
-    // @DisplayName: Yaw Servo Trim/Center
-    // @Description: Trim or center position of yaw servo
-    // @Range: 1250 1750
-    // @Units: PWM
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("YAW_SV_TRIM", 32,     AP_MotorsTri,  _yaw_servo_trim, 1500),
-
-    // @Param: YAW_SV_MIN
-    // @DisplayName: Yaw Servo Min PWM
-    // @Description: Yaw servo's minimum pwm value
-    // @Range: 1000 1400
-    // @Units: PWM
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("YAW_SV_MIN", 33,     AP_MotorsTri,  _yaw_servo_min, 1250),
-
-    // @Param: YAW_SV_MAX
-    // @DisplayName: Yaw Servo Max PWM
-    // @Description: Yaw servo's maximum pwm value
-    // @Range: 1600 2000
-    // @Units: PWM
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("YAW_SV_MAX", 34,     AP_MotorsTri,  _yaw_servo_max, 1750),
-
     // @Param: YAW_SV_ANGLE
     // @DisplayName: Yaw Servo Max Lean Angle
     // @Description: Yaw servo's maximum lean angle
@@ -93,6 +60,14 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
     motor_enabled[AP_MOTORS_MOT_2] = true;
     motor_enabled[AP_MOTORS_MOT_4] = true;
 
+    // find the yaw servo
+    _yaw_servo = SRV_Channels::get_channel_for(SRV_Channel::k_motor7, AP_MOTORS_CH_TRI_YAW);
+    if (!_yaw_servo) {
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "MotorsTri: unable to setup yaw channel");
+        // don't set initialised_ok
+        return;
+    }
+    
     // allow mapping of motor7
     add_motor_num(AP_MOTORS_CH_TRI_YAW);
 
@@ -139,7 +114,7 @@ void AP_MotorsTri::output_to_motors()
             rc_write(AP_MOTORS_MOT_1, get_pwm_output_min());
             rc_write(AP_MOTORS_MOT_2, get_pwm_output_min());
             rc_write(AP_MOTORS_MOT_4, get_pwm_output_min());
-            rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
+            rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
             hal.rcout->push();
             break;
         case SPIN_WHEN_ARMED:
@@ -148,7 +123,7 @@ void AP_MotorsTri::output_to_motors()
             rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm());
             rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm());
             rc_write(AP_MOTORS_MOT_4, calc_spin_up_to_pwm());
-            rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
+            rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
             hal.rcout->push();
             break;
         case SPOOL_UP:
@@ -338,14 +313,14 @@ int16_t AP_MotorsTri::calc_yaw_radio_output(float yaw_input, float yaw_input_max
 {
     int16_t ret;
 
-    if (_yaw_reverse < 0) {
+    if (_yaw_servo->get_reversed()) {
         yaw_input = -yaw_input;
     }
 
     if (yaw_input >= 0){
-        ret = (_yaw_servo_trim + (yaw_input/yaw_input_max * (_yaw_servo_max - _yaw_servo_trim)));
+        ret = (_yaw_servo->get_trim() + (yaw_input/yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim())));
     } else {
-        ret = (_yaw_servo_trim + (yaw_input/yaw_input_max * (_yaw_servo_trim - _yaw_servo_min)));
+        ret = (_yaw_servo->get_trim() + (yaw_input/yaw_input_max * (_yaw_servo->get_trim() - _yaw_servo->get_output_min())));
     }
 
     return ret;

libraries/AP_Motors/AP_MotorsTri.h

@@ -4,7 +4,7 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
-#include <RC_Channel/RC_Channel.h>     // RC Channel Library
+#include <SRV_Channel/SRV_Channel.h>
 #include "AP_MotorsMulticopter.h"
 
 // tail servo uses channel 7
@@ -62,10 +62,8 @@ protected:
     int16_t             calc_yaw_radio_output(float yaw_input, float yaw_input_max);        // calculate radio output for yaw servo, typically in range of 1100-1900
 
     // parameters
-    AP_Int8         _yaw_reverse;                       // Reverse yaw output
+
-    AP_Int16        _yaw_servo_trim;                    // Trim or center position of yaw servo
+    SRV_Channel     *_yaw_servo; // yaw output channel
-    AP_Int16        _yaw_servo_min;                     // Minimum pwm of yaw servo
-    AP_Int16        _yaw_servo_max;                     // Maximum pwm of yaw servo
     AP_Float        _yaw_servo_angle_max_deg;           // Maximum lean angle of yaw servo in degrees
     float           _pivot_angle;                       // Angle of yaw pivot
     float           _thrust_right;