aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: use SRV_Channel for tri tail servo

Browse Source
This commit is contained in:
Andrew Tridgell 2017-01-09 15:01:30 +11:00
parent 3c1517f583
commit 8799094278
2 changed files with 17 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -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;

8
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -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
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;