2015-07-22 10:46:53 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* This program is free software : you can redistribute it and / or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation , either version 3 of the License , or
* ( at your option ) any later version .
*
* This program is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
* along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
# include <stdlib.h>
2015-08-15 13:42:46 -03:00
# include <AP_HAL/AP_HAL.h>
2015-07-22 10:46:53 -03:00
# include "AP_MotorsHeli_Single.h"
extern const AP_HAL : : HAL & hal ;
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_MotorsHeli_Single : : var_info [ ] = {
2015-07-22 10:46:53 -03:00
AP_NESTEDGROUPINFO ( AP_MotorsHeli , 0 ) ,
// @Param: SV1_POS
// @DisplayName: Servo 1 Position
// @Description: Angular location of swash servo #1
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO ( " SV1_POS " , 1 , AP_MotorsHeli_Single , _servo1_pos , AP_MOTORS_HELI_SINGLE_SERVO1_POS ) ,
// @Param: SV2_POS
// @DisplayName: Servo 2 Position
// @Description: Angular location of swash servo #2
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO ( " SV2_POS " , 2 , AP_MotorsHeli_Single , _servo2_pos , AP_MOTORS_HELI_SINGLE_SERVO2_POS ) ,
// @Param: SV3_POS
// @DisplayName: Servo 3 Position
// @Description: Angular location of swash servo #3
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO ( " SV3_POS " , 3 , AP_MotorsHeli_Single , _servo3_pos , AP_MOTORS_HELI_SINGLE_SERVO3_POS ) ,
// @Param: TAIL_TYPE
// @DisplayName: Tail Type
// @Description: Tail type selection. Simpler yaw controller used if external gyro is selected
// @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch
// @User: Standard
AP_GROUPINFO ( " TAIL_TYPE " , 4 , AP_MotorsHeli_Single , _tail_type , AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO ) ,
// @Param: SWASH_TYPE
// @DisplayName: Swash Type
// @Description: Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
// @Values: 0:3-Servo CCPM, 1:H1 Mechanical Mixing
// @User: Standard
AP_GROUPINFO ( " SWASH_TYPE " , 5 , AP_MotorsHeli_Single , _swash_type , AP_MOTORS_HELI_SINGLE_SWASH_CCPM ) ,
// @Param: GYR_GAIN
// @DisplayName: External Gyro Gain
// @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
2015-08-09 08:02:54 -03:00
AP_GROUPINFO ( " GYR_GAIN " , 6 , AP_MotorsHeli_Single , _ext_gyro_gain_std , AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN ) ,
2015-07-22 10:46:53 -03:00
// @Param: PHANG
// @DisplayName: Swashplate Phase Angle Compensation
// @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
// @Range: -90 90
// @Units: Degrees
// @User: Advanced
// @Increment: 1
AP_GROUPINFO ( " PHANG " , 7 , AP_MotorsHeli_Single , _phase_angle , 0 ) ,
// @Param: COLYAW
// @DisplayName: Collective-Yaw Mixing
// @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
// @Range: -10 10
// @Increment: 0.1
AP_GROUPINFO ( " COLYAW " , 8 , AP_MotorsHeli_Single , _collective_yaw_effect , 0 ) ,
// @Param: FLYBAR_MODE
// @DisplayName: Flybar Mode Selector
// @Description: Flybar present or not. Affects attitude controller used during ACRO flight mode
// @Range: 0:NoFlybar 1:Flybar
// @User: Standard
AP_GROUPINFO ( " FLYBAR_MODE " , 9 , AP_MotorsHeli_Single , _flybar_mode , AP_MOTORS_HELI_NOFLYBAR ) ,
// @Param: TAIL_SPEED
// @DisplayName: Direct Drive VarPitch Tail ESC speed
// @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
2015-08-12 14:46:13 -03:00
AP_GROUPINFO ( " TAIL_SPEED " , 10 , AP_MotorsHeli_Single , _direct_drive_tailspeed , AP_MOTORS_HELI_SINGLE_DDVPT_SPEED_DEFAULT ) ,
2015-07-22 10:46:53 -03:00
2015-08-09 08:02:54 -03:00
// @Param: GYR_GAIN_ACRO
// @DisplayName: External Gyro Gain for ACRO
// @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro. A value of zero means to use H_GYR_GAIN
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " GYR_GAIN_ACRO " , 11 , AP_MotorsHeli_Single , _ext_gyro_gain_acro , 0 ) ,
2015-07-22 10:46:53 -03:00
AP_GROUPEND
} ;
// set update rate to motors - a value in hertz
void AP_MotorsHeli_Single : : set_update_rate ( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz ;
// setup fast channels
uint32_t mask =
2015-09-29 00:00:16 -03:00
1U < < AP_MOTORS_MOT_1 |
1U < < AP_MOTORS_MOT_2 |
1U < < AP_MOTORS_MOT_3 |
1U < < AP_MOTORS_MOT_4 ;
2015-07-22 10:46:53 -03:00
hal . rcout - > set_freq ( mask , _speed_hz ) ;
}
2015-08-12 12:41:40 -03:00
// enable - starts allowing signals to be sent to motors and servos
2015-07-22 10:46:53 -03:00
void AP_MotorsHeli_Single : : enable ( )
{
// enable output channels
2015-09-29 00:00:16 -03:00
hal . rcout - > enable_ch ( AP_MOTORS_MOT_1 ) ; // swash servo 1
hal . rcout - > enable_ch ( AP_MOTORS_MOT_2 ) ; // swash servo 2
hal . rcout - > enable_ch ( AP_MOTORS_MOT_3 ) ; // swash servo 3
hal . rcout - > enable_ch ( AP_MOTORS_MOT_4 ) ; // yaw
2015-08-12 12:41:40 -03:00
hal . rcout - > enable_ch ( AP_MOTORS_HELI_SINGLE_AUX ) ; // output for gyro gain or direct drive variable pitch tail motor
hal . rcout - > enable_ch ( AP_MOTORS_HELI_SINGLE_RSC ) ; // output for main rotor esc
// disable channels 7 and 8 from being used by RC_Channel_aux
2015-09-29 00:00:16 -03:00
RC_Channel_aux : : disable_aux_channel ( AP_MOTORS_HELI_SINGLE_AUX ) ;
RC_Channel_aux : : disable_aux_channel ( AP_MOTORS_HELI_SINGLE_RSC ) ;
2015-08-12 12:41:40 -03:00
}
// init_outputs - initialise Servo/PWM ranges and endpoints
void AP_MotorsHeli_Single : : init_outputs ( )
{
// reset swash servo range and endpoints
reset_swash_servo ( _swash_servo_1 ) ;
reset_swash_servo ( _swash_servo_2 ) ;
reset_swash_servo ( _swash_servo_3 ) ;
_yaw_servo . set_angle ( 4500 ) ;
// set main rotor servo range
// tail rotor servo use range as set in vehicle code for rc7
_main_rotor . init_servo ( ) ;
2015-07-22 10:46:53 -03:00
}
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Single : : output_test ( uint8_t motor_seq , int16_t pwm )
{
// exit immediately if not armed
if ( ! armed ( ) ) {
return ;
}
// output to motors and servos
switch ( motor_seq ) {
case 1 :
// swash servo 1
2015-09-29 00:00:16 -03:00
hal . rcout - > write ( AP_MOTORS_MOT_1 , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
case 2 :
// swash servo 2
2015-09-29 00:00:16 -03:00
hal . rcout - > write ( AP_MOTORS_MOT_2 , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
case 3 :
// swash servo 3
2015-09-29 00:00:16 -03:00
hal . rcout - > write ( AP_MOTORS_MOT_3 , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
case 4 :
// external gyro & tail servo
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO ) {
2015-08-09 08:02:54 -03:00
if ( _acro_tail & & _ext_gyro_gain_acro > 0 ) {
write_aux ( _ext_gyro_gain_acro ) ;
} else {
write_aux ( _ext_gyro_gain_std ) ;
}
2015-07-22 10:46:53 -03:00
}
2015-09-29 00:00:16 -03:00
hal . rcout - > write ( AP_MOTORS_MOT_4 , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
case 5 :
// main rotor
2015-09-29 00:00:16 -03:00
hal . rcout - > write ( AP_MOTORS_HELI_SINGLE_RSC , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
default :
// do nothing
break ;
}
}
// set_desired_rotor_speed
void AP_MotorsHeli_Single : : set_desired_rotor_speed ( int16_t desired_speed )
{
_main_rotor . set_desired_speed ( desired_speed ) ;
2015-08-12 20:20:30 -03:00
// always send desired speed to tail rotor control, will do nothing if not DDVPT not enabled
_tail_rotor . set_desired_speed ( _direct_drive_tailspeed ) ;
2015-07-22 10:46:53 -03:00
}
2015-08-12 12:41:40 -03:00
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single : : calculate_scalars ( )
2015-07-22 10:46:53 -03:00
{
2015-08-12 12:41:40 -03:00
// range check collective min, max and mid
if ( _collective_min > = _collective_max ) {
_collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN ;
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX ;
}
_collective_mid = constrain_int16 ( _collective_mid , _collective_min , _collective_max ) ;
// calculate collective mid point as a number from 0 to 1000
_collective_mid_pwm = ( ( float ) ( _collective_mid - _collective_min ) ) / ( ( float ) ( _collective_max - _collective_min ) ) * 1000.0f ;
// calculate maximum collective pitch range from full positive pitch to zero pitch
_collective_range = 1000 - _collective_mid_pwm ;
// determine roll, pitch and collective input scaling
_roll_scaler = ( float ) _roll_max / 4500.0f ;
_pitch_scaler = ( float ) _pitch_max / 4500.0f ;
_collective_scalar = ( ( float ) ( _collective_max - _collective_min ) ) / 1000.0f ;
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors ( ) ;
// send setpoints to main rotor controller and trigger recalculation of scalars
2015-08-28 03:23:26 -03:00
_main_rotor . set_control_mode ( static_cast < RotorControlMode > ( _rsc_mode . get ( ) ) ) ;
2015-08-10 21:08:27 -03:00
_main_rotor . set_ramp_time ( _rsc_ramp_time ) ;
_main_rotor . set_runup_time ( _rsc_runup_time ) ;
_main_rotor . set_critical_speed ( _rsc_critical ) ;
2015-08-11 21:20:28 -03:00
_main_rotor . set_idle_output ( _rsc_idle_output ) ;
_main_rotor . set_power_output_range ( _rsc_power_low , _rsc_power_high ) ;
2015-07-22 10:46:53 -03:00
_main_rotor . recalc_scalers ( ) ;
2015-08-12 12:41:40 -03:00
// send setpoints to tail rotor controller and trigger recalculation of scalars
2015-08-12 20:20:30 -03:00
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH ) {
2015-08-28 03:23:26 -03:00
_tail_rotor . set_control_mode ( ROTOR_CONTROL_MODE_SPEED_SETPOINT ) ;
2015-08-12 14:46:13 -03:00
_tail_rotor . set_ramp_time ( AP_MOTORS_HELI_SINGLE_DDVPT_RAMP_TIME ) ;
_tail_rotor . set_runup_time ( AP_MOTORS_HELI_SINGLE_DDVPT_RUNUP_TIME ) ;
2015-07-22 10:46:53 -03:00
_tail_rotor . set_critical_speed ( _rsc_critical ) ;
2015-08-11 21:20:28 -03:00
_tail_rotor . set_idle_output ( _rsc_idle_output ) ;
2015-08-11 15:31:20 -03:00
} else {
2015-08-28 03:23:26 -03:00
_tail_rotor . set_control_mode ( ROTOR_CONTROL_MODE_DISABLED ) ;
2015-08-11 15:31:20 -03:00
_tail_rotor . set_ramp_time ( 0 ) ;
_tail_rotor . set_runup_time ( 0 ) ;
_tail_rotor . set_critical_speed ( 0 ) ;
2015-08-11 21:20:28 -03:00
_tail_rotor . set_idle_output ( 0 ) ;
2015-07-22 10:46:53 -03:00
}
_tail_rotor . recalc_scalers ( ) ;
}
2015-08-12 12:41:40 -03:00
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void AP_MotorsHeli_Single : : calculate_roll_pitch_collective_factors ( )
{
if ( _swash_type = = AP_MOTORS_HELI_SINGLE_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing
// roll factors
_rollFactor [ CH_1 ] = cosf ( radians ( _servo1_pos + 90 - ( _phase_angle + _delta_phase_angle ) ) ) ;
_rollFactor [ CH_2 ] = cosf ( radians ( _servo2_pos + 90 - ( _phase_angle + _delta_phase_angle ) ) ) ;
_rollFactor [ CH_3 ] = cosf ( radians ( _servo3_pos + 90 - ( _phase_angle + _delta_phase_angle ) ) ) ;
// pitch factors
_pitchFactor [ CH_1 ] = cosf ( radians ( _servo1_pos - ( _phase_angle + _delta_phase_angle ) ) ) ;
_pitchFactor [ CH_2 ] = cosf ( radians ( _servo2_pos - ( _phase_angle + _delta_phase_angle ) ) ) ;
_pitchFactor [ CH_3 ] = cosf ( radians ( _servo3_pos - ( _phase_angle + _delta_phase_angle ) ) ) ;
// collective factors
_collectiveFactor [ CH_1 ] = 1 ;
_collectiveFactor [ CH_2 ] = 1 ;
_collectiveFactor [ CH_3 ] = 1 ;
} else { //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor [ CH_1 ] = 1 ;
_rollFactor [ CH_2 ] = 0 ;
_rollFactor [ CH_3 ] = 0 ;
// pitch factors
_pitchFactor [ CH_1 ] = 0 ;
_pitchFactor [ CH_2 ] = 1 ;
_pitchFactor [ CH_3 ] = 0 ;
// collective factors
_collectiveFactor [ CH_1 ] = 0 ;
_collectiveFactor [ CH_2 ] = 0 ;
_collectiveFactor [ CH_3 ] = 1 ;
}
}
2015-07-22 10:46:53 -03:00
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsHeli_Single : : get_motor_mask ( )
{
// heli uses channels 1,2,3,4,7 and 8
return ( 1U < < 0 | 1U < < 1 | 1U < < 2 | 1U < < 3 | 1U < < AP_MOTORS_HELI_SINGLE_AUX | 1U < < AP_MOTORS_HELI_SINGLE_RSC ) ;
}
2015-08-12 14:22:39 -03:00
// update_motor_controls - sends commands to motor controllers
2015-08-28 03:23:26 -03:00
void AP_MotorsHeli_Single : : update_motor_control ( RotorControlState state )
2015-08-11 16:12:16 -03:00
{
2015-08-12 14:22:39 -03:00
// Send state update to motors
_tail_rotor . output ( state ) ;
_main_rotor . output ( state ) ;
2015-08-11 16:12:16 -03:00
2015-08-12 14:22:39 -03:00
// Check if both rotors are run-up, tail rotor controller always returns true if not enabled
_heliflags . rotor_runup_complete = ( _main_rotor . is_runup_complete ( ) & & _tail_rotor . is_runup_complete ( ) ) ;
2015-07-22 10:46:53 -03:00
}
2015-08-11 21:37:56 -03:00
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
void AP_MotorsHeli_Single : : set_delta_phase_angle ( int16_t angle )
{
angle = constrain_int16 ( angle , - 90 , 90 ) ;
_delta_phase_angle = angle ;
calculate_roll_pitch_collective_factors ( ) ;
}
2015-07-22 10:46:53 -03:00
//
2015-08-12 12:41:40 -03:00
// move_actuators - moves swash plate and tail rotor
2015-07-22 10:46:53 -03:00
// - expected ranges:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 0 ~ 1000
// yaw: -4500 ~ 4500
//
2015-08-12 12:41:40 -03:00
void AP_MotorsHeli_Single : : move_actuators ( int16_t roll_out , int16_t pitch_out , int16_t coll_in , int16_t yaw_out )
2015-07-22 10:46:53 -03:00
{
int16_t yaw_offset = 0 ;
int16_t coll_out_scaled ;
// initialize limits flag
limit . roll_pitch = false ;
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
2015-08-12 12:41:40 -03:00
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified roll_max and pitch_max
// coming into this equation at 4500 or less, and based on the original assumption of the
// total _servo_x.servo_out range being -4500 to 4500.
roll_out = roll_out * _roll_scaler ;
if ( roll_out < - _roll_max ) {
roll_out = - _roll_max ;
limit . roll_pitch = true ;
}
if ( roll_out > _roll_max ) {
roll_out = _roll_max ;
limit . roll_pitch = true ;
}
2015-07-22 10:46:53 -03:00
2015-08-12 12:41:40 -03:00
// scale pitch and update limits
pitch_out = pitch_out * _pitch_scaler ;
if ( pitch_out < - _pitch_max ) {
pitch_out = - _pitch_max ;
limit . roll_pitch = true ;
}
if ( pitch_out > _pitch_max ) {
pitch_out = _pitch_max ;
limit . roll_pitch = true ;
}
2015-07-22 10:46:53 -03:00
2015-08-12 12:41:40 -03:00
// constrain collective input
_collective_out = coll_in ;
if ( _collective_out < = 0 ) {
_collective_out = 0 ;
limit . throttle_lower = true ;
}
if ( _collective_out > = 1000 ) {
_collective_out = 1000 ;
limit . throttle_upper = true ;
}
2015-07-22 10:46:53 -03:00
2015-08-12 12:41:40 -03:00
// ensure not below landed/landing collective
if ( _heliflags . landing_collective & & _collective_out < _land_collective_min ) {
_collective_out = _land_collective_min ;
limit . throttle_lower = true ;
}
2015-07-22 10:46:53 -03:00
2015-08-12 12:41:40 -03:00
// scale collective pitch
coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000 ;
2015-07-22 10:46:53 -03:00
2015-08-12 12:41:40 -03:00
// if servo output not in manual mode, process pre-compensation factors
if ( _servo_manual = = 0 ) {
2015-07-22 10:46:53 -03:00
// rudder feed forward based on collective
2015-08-10 17:25:28 -03:00
// the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque
2015-07-22 10:46:53 -03:00
// also not required if we are using external gyro
2015-08-11 21:20:28 -03:00
if ( ( _main_rotor . get_control_output ( ) > _rsc_idle_output ) & & _tail_type ! = AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO ) {
2015-07-22 10:46:53 -03:00
// sanity check collective_yaw_effect
_collective_yaw_effect = constrain_float ( _collective_yaw_effect , - AP_MOTORS_HELI_SINGLE_COLYAW_RANGE , AP_MOTORS_HELI_SINGLE_COLYAW_RANGE ) ;
yaw_offset = _collective_yaw_effect * abs ( _collective_out - _collective_mid_pwm ) ;
}
2015-08-12 12:41:40 -03:00
} else {
yaw_offset = 0 ;
2015-07-22 10:46:53 -03:00
}
2015-08-11 21:20:28 -03:00
// feed power estimate into main rotor controller
// ToDo: include tail rotor power?
// ToDo: add main rotor cyclic power?
2015-08-12 21:41:56 -03:00
_main_rotor_power = ( ( float ) ( abs ( _collective_out - _collective_mid_pwm ) ) / _collective_range ) ;
2015-08-11 21:20:28 -03:00
_main_rotor . set_motor_load ( _main_rotor_power ) ;
2015-07-22 10:46:53 -03:00
// swashplate servos
2015-08-07 19:42:21 -03:00
_swash_servo_1 . servo_out = ( _rollFactor [ CH_1 ] * roll_out + _pitchFactor [ CH_1 ] * pitch_out ) / 10 + _collectiveFactor [ CH_1 ] * coll_out_scaled + ( _swash_servo_1 . radio_trim - 1500 ) ;
_swash_servo_2 . servo_out = ( _rollFactor [ CH_2 ] * roll_out + _pitchFactor [ CH_2 ] * pitch_out ) / 10 + _collectiveFactor [ CH_2 ] * coll_out_scaled + ( _swash_servo_2 . radio_trim - 1500 ) ;
2015-07-22 10:46:53 -03:00
if ( _swash_type = = AP_MOTORS_HELI_SINGLE_SWASH_H1 ) {
2015-08-07 19:42:21 -03:00
_swash_servo_1 . servo_out + = 500 ;
_swash_servo_2 . servo_out + = 500 ;
2015-07-22 10:46:53 -03:00
}
2015-08-07 19:42:21 -03:00
_swash_servo_3 . servo_out = ( _rollFactor [ CH_3 ] * roll_out + _pitchFactor [ CH_3 ] * pitch_out ) / 10 + _collectiveFactor [ CH_3 ] * coll_out_scaled + ( _swash_servo_3 . radio_trim - 1500 ) ;
2015-07-22 10:46:53 -03:00
// use servo_out to calculate pwm_out and radio_out
2015-08-07 19:42:21 -03:00
_swash_servo_1 . calc_pwm ( ) ;
_swash_servo_2 . calc_pwm ( ) ;
_swash_servo_3 . calc_pwm ( ) ;
2015-07-22 10:46:53 -03:00
2015-09-28 19:03:32 -03:00
hal . rcout - > cork ( ) ;
2015-07-22 10:46:53 -03:00
// actually move the servos
2015-09-29 00:00:16 -03:00
hal . rcout - > write ( AP_MOTORS_MOT_1 , _swash_servo_1 . radio_out ) ;
hal . rcout - > write ( AP_MOTORS_MOT_2 , _swash_servo_2 . radio_out ) ;
hal . rcout - > write ( AP_MOTORS_MOT_3 , _swash_servo_3 . radio_out ) ;
2015-09-28 19:03:32 -03:00
2015-07-22 10:46:53 -03:00
// update the yaw rate using the tail rotor/servo
2015-08-06 05:25:31 -03:00
move_yaw ( yaw_out + yaw_offset ) ;
2015-09-28 19:03:32 -03:00
hal . rcout - > push ( ) ;
2015-07-22 10:46:53 -03:00
}
2015-08-06 05:25:31 -03:00
// move_yaw
void AP_MotorsHeli_Single : : move_yaw ( int16_t yaw_out )
2015-07-22 10:46:53 -03:00
{
2015-08-07 19:42:21 -03:00
_yaw_servo . servo_out = constrain_int16 ( yaw_out , - 4500 , 4500 ) ;
2015-07-22 10:46:53 -03:00
2015-08-07 19:42:21 -03:00
if ( _yaw_servo . servo_out ! = yaw_out ) {
2015-07-22 10:46:53 -03:00
limit . yaw = true ;
}
2015-08-07 19:42:21 -03:00
_yaw_servo . calc_pwm ( ) ;
2015-07-22 10:46:53 -03:00
2015-09-29 00:00:16 -03:00
hal . rcout - > write ( AP_MOTORS_MOT_4 , _yaw_servo . radio_out ) ;
2015-07-22 10:46:53 -03:00
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO ) {
// output gain to exernal gyro
2015-08-09 08:02:54 -03:00
if ( _acro_tail & & _ext_gyro_gain_acro > 0 ) {
write_aux ( _ext_gyro_gain_acro ) ;
} else {
write_aux ( _ext_gyro_gain_std ) ;
}
2015-07-22 10:46:53 -03:00
} else if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH & & _main_rotor . get_desired_speed ( ) > 0 ) {
// output yaw servo to tail rsc
2015-08-07 19:42:21 -03:00
write_aux ( _yaw_servo . servo_out ) ;
2015-07-22 10:46:53 -03:00
}
}
// write_aux - outputs pwm onto output aux channel (ch7)
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli_Single : : write_aux ( int16_t servo_out )
{
_servo_aux . servo_out = servo_out ;
_servo_aux . calc_pwm ( ) ;
hal . rcout - > write ( AP_MOTORS_HELI_SINGLE_AUX , _servo_aux . radio_out ) ;
2015-08-28 03:23:26 -03:00
}