From 8799094278c1f495905c902ef2356e723e86efa1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Jan 2017 15:01:30 +1100 Subject: [PATCH] AP_Motors: use SRV_Channel for tri tail servo --- libraries/AP_Motors/AP_MotorsTri.cpp | 53 ++++++++-------------------- libraries/AP_Motors/AP_MotorsTri.h | 8 ++--- 2 files changed, 17 insertions(+), 44 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 9e14bb71ed..d577b5844d 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -20,6 +20,7 @@ */ #include #include +#include #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; diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 9751981e1b..75d7ff1ecd 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -4,7 +4,7 @@ #include #include // ArduPilot Mega Vector/Matrix math Library -#include // RC Channel Library +#include #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 - AP_Int16 _yaw_servo_min; // Minimum pwm of yaw servo - AP_Int16 _yaw_servo_max; // Maximum pwm of yaw servo + + SRV_Channel *_yaw_servo; // yaw output channel 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;