2015-07-22 10:46:53 -03:00
/*
* 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>
2017-01-03 05:56:57 -04:00
# include <SRV_Channel/SRV_Channel.h>
2015-07-22 10:46:53 -03:00
# include "AP_MotorsHeli_Single.h"
2015-11-30 16:16:04 -04:00
# include <GCS_MAVLink/GCS.h>
2015-07-22 10:46:53 -03:00
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 ) ,
2018-03-08 13:16:31 -04:00
2019-01-12 00:17:15 -04:00
// Indices 1-3 were used by servo position params and should not be used
2018-03-08 13:16:31 -04:00
2015-07-22 10:46:53 -03:00
// @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 ) ,
2019-03-16 11:20:04 -03:00
// Indice 5 was used by SWASH_TYPE and should not be used
2015-07-22 10:46:53 -03:00
// @Param: GYR_GAIN
// @DisplayName: External Gyro Gain
2017-05-15 20:21:53 -03:00
// @Description: PWM in microseconds sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
2015-07-22 10:46:53 -03:00
// @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
2019-01-12 00:17:15 -04:00
// Index 7 was used for phase angle and should not be used
2015-07-22 10:46:53 -03:00
// @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
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-07-22 10:46:53 -03:00
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
2016-03-17 02:03:36 -03:00
// @Values: 0:NoFlybar,1:Flybar
2015-07-22 10:46:53 -03:00
// @User: Standard
AP_GROUPINFO ( " FLYBAR_MODE " , 9 , AP_MotorsHeli_Single , _flybar_mode , AP_MOTORS_HELI_NOFLYBAR ) ,
2018-03-08 13:16:31 -04:00
2015-07-22 10:46:53 -03:00
// @Param: TAIL_SPEED
// @DisplayName: Direct Drive VarPitch Tail ESC speed
2017-05-15 20:21:53 -03:00
// @Description: Direct Drive VarPitch Tail ESC speed in PWM microseconds. Only used when TailType is DirectDrive VarPitch
2015-07-22 10:46:53 -03:00
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
2018-03-08 13:16:31 -04:00
AP_GROUPINFO ( " TAIL_SPEED " , 10 , AP_MotorsHeli_Single , _direct_drive_tailspeed , AP_MOTORS_HELI_SINGLE_DDVP_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
2017-05-15 20:21:53 -03:00
// @Description: PWM in microseconds sent to external gyro on ch7 when tail type is Servo w/ ExtGyro. A value of zero means to use H_GYR_GAIN
2015-08-09 08:02:54 -03:00
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " GYR_GAIN_ACRO " , 11 , AP_MotorsHeli_Single , _ext_gyro_gain_acro , 0 ) ,
2016-02-08 07:11:55 -04:00
2019-03-16 11:20:04 -03:00
// Indices 16-19 were used by RSC_PWM_MIN, RSC_PWM_MAX, RSC_PWM_REV, and COL_CTRL_DIR and should not be used
2018-03-28 12:20:59 -03:00
2019-02-27 20:00:07 -04:00
// @Group: H3_SW_
// @Path: AP_MotorsHeli_Swash.cpp
2019-03-16 11:20:04 -03:00
AP_SUBGROUPINFO ( _swashplate , " SW_ " , 20 , AP_MotorsHeli_Single , AP_MotorsHeli_Swash ) ,
2019-01-12 00:17:15 -04:00
2015-07-22 10:46:53 -03:00
AP_GROUPEND
} ;
2018-07-11 16:33:00 -03:00
# define YAW_SERVO_MAX_ANGLE 4500
2015-07-22 10:46:53 -03:00
// 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 ;
2019-03-16 11:20:04 -03:00
if ( _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_90 | | _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_45 ) {
2019-01-12 00:17:15 -04:00
mask | = 1U < < ( AP_MOTORS_MOT_5 ) ;
}
2016-01-04 06:24:06 -04:00
rc_set_freq ( mask , _speed_hz ) ;
2015-07-22 10:46:53 -03:00
}
2015-08-12 12:41:40 -03:00
// init_outputs - initialise Servo/PWM ranges and endpoints
2017-01-03 05:56:57 -04:00
bool AP_MotorsHeli_Single : : init_outputs ( )
2015-08-12 12:41:40 -03:00
{
2017-01-03 05:56:57 -04:00
if ( ! _flags . initialised_ok ) {
2018-07-11 16:33:00 -03:00
// map primary swash servos
for ( uint8_t i = 0 ; i < AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS ; i + + ) {
add_motor_num ( CH_1 + i ) ;
}
2019-03-16 11:20:04 -03:00
if ( _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_90 | | _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_45 ) {
2019-01-12 00:17:15 -04:00
add_motor_num ( CH_5 ) ;
}
2018-07-11 16:33:00 -03:00
// yaw servo
add_motor_num ( CH_4 ) ;
// initialize main rotor servo
_main_rotor . init_servo ( ) ;
2017-11-25 20:15:42 -04:00
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH ) {
_tail_rotor . init_servo ( ) ;
2018-07-11 16:33:00 -03:00
} else if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO ) {
// external gyro output
add_motor_num ( AP_MOTORS_HELI_SINGLE_EXTGYRO ) ;
2017-01-03 05:56:57 -04:00
}
}
2019-02-03 19:19:13 -04:00
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO ) {
2018-07-11 16:33:00 -03:00
// External Gyro uses PWM output thus servo endpoints are forced
SRV_Channels : : set_output_min_max ( SRV_Channels : : get_motor_function ( AP_MOTORS_HELI_SINGLE_EXTGYRO ) , 1000 , 2000 ) ;
}
2015-08-12 12:41:40 -03:00
// reset swash servo range and endpoints
2018-07-11 16:33:00 -03:00
for ( uint8_t i = 0 ; i < AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS ; i + + ) {
reset_swash_servo ( SRV_Channels : : get_motor_function ( i ) ) ;
}
2019-03-16 11:20:04 -03:00
if ( _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_90 | | _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_45 ) {
2019-01-12 00:17:15 -04:00
reset_swash_servo ( SRV_Channels : : get_motor_function ( 4 ) ) ;
}
2015-08-12 12:41:40 -03:00
2018-07-11 16:33:00 -03:00
// yaw servo is an angle from -4500 to 4500
SRV_Channels : : set_angle ( SRV_Channel : : k_motor4 , YAW_SERVO_MAX_ANGLE ) ;
2015-08-12 12:41:40 -03:00
2018-07-11 16:33:00 -03:00
_flags . initialised_ok = true ;
2017-01-03 05:56:57 -04:00
return true ;
2015-07-22 10:46:53 -03:00
}
2018-04-27 13:22:07 -03:00
// output_test_seq - spin a motor at the pwm value specified
2015-07-22 10:46:53 -03:00
// 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
2018-04-27 13:22:07 -03:00
void AP_MotorsHeli_Single : : output_test_seq ( uint8_t motor_seq , int16_t pwm )
2015-07-22 10:46:53 -03:00
{
// exit immediately if not armed
if ( ! armed ( ) ) {
return ;
}
// output to motors and servos
switch ( motor_seq ) {
case 1 :
// swash servo 1
2016-01-04 01:56:54 -04:00
rc_write ( AP_MOTORS_MOT_1 , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
case 2 :
// swash servo 2
2016-01-04 01:56:54 -04:00
rc_write ( AP_MOTORS_MOT_2 , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
case 3 :
// swash servo 3
2016-01-04 01:56:54 -04:00
rc_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 ) {
2018-07-11 16:33:00 -03:00
rc_write ( AP_MOTORS_HELI_SINGLE_EXTGYRO , _ext_gyro_gain_acro ) ;
2015-08-09 08:02:54 -03:00
} else {
2018-07-11 16:33:00 -03:00
rc_write ( AP_MOTORS_HELI_SINGLE_EXTGYRO , _ext_gyro_gain_std ) ;
2015-08-09 08:02:54 -03:00
}
2015-07-22 10:46:53 -03:00
}
2016-01-04 01:56:54 -04:00
rc_write ( AP_MOTORS_MOT_4 , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
case 5 :
// main rotor
2019-08-07 23:52:17 -03:00
rc_write ( AP_MOTORS_HELI_RSC , pwm ) ;
2015-07-22 10:46:53 -03:00
break ;
default :
// do nothing
break ;
}
}
// set_desired_rotor_speed
2016-02-03 04:59:44 -04:00
void AP_MotorsHeli_Single : : set_desired_rotor_speed ( float desired_speed )
2015-07-22 10:46:53 -03:00
{
_main_rotor . set_desired_speed ( desired_speed ) ;
2018-03-08 13:16:31 -04:00
// always send desired speed to tail rotor control, will do nothing if not DDVP not enabled
2019-08-07 23:52:17 -03:00
_tail_rotor . set_desired_speed ( _direct_drive_tailspeed * 0.01f ) ;
2015-07-22 10:46:53 -03:00
}
2019-02-03 19:19:13 -04:00
// set_rotor_rpm - used for governor with speed sensor
2019-05-19 14:20:17 -03:00
void AP_MotorsHeli_Single : : set_rpm ( float rotor_rpm )
2019-02-03 19:19:13 -04:00
{
_main_rotor . set_rotor_rpm ( rotor_rpm ) ;
}
2015-11-14 18:14:03 -04:00
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single : : calculate_armed_scalars ( )
{
2019-02-14 20:28:48 -04:00
// Set rsc mode specific parameters
2019-08-07 23:52:17 -03:00
if ( _main_rotor . _rsc_mode . get ( ) = = ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT | | _main_rotor . _rsc_mode . get ( ) = = ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT ) {
_main_rotor . set_throttle_curve ( ) ;
}
// keeps user from changing RSC mode while armed
if ( _main_rotor . _rsc_mode . get ( ) ! = _main_rotor . get_control_mode ( ) ) {
_main_rotor . reset_rsc_mode_param ( ) ;
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " RSC control mode change failed " ) ;
_heliflags . save_rsc_mode = true ;
}
// saves rsc mode parameter when disarmed if it had been reset while armed
if ( _heliflags . save_rsc_mode & & ! _flags . armed ) {
_main_rotor . _rsc_mode . save ( ) ;
_heliflags . save_rsc_mode = false ;
2019-02-14 20:28:48 -04:00
}
}
2015-11-14 18:14:03 -04: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 ) ;
2016-02-05 21:39:47 -04:00
// calculate collective mid point as a number from 0 to 1
_collective_mid_pct = ( ( float ) ( _collective_mid - _collective_min ) ) / ( ( float ) ( _collective_max - _collective_min ) ) ;
2015-08-12 12:41:40 -03:00
2019-01-12 00:17:15 -04:00
// configure swashplate and update scalars
2019-03-16 11:20:04 -03:00
_swashplate . configure ( ) ;
2019-01-12 00:17:15 -04:00
_swashplate . calculate_roll_pitch_collective_factors ( ) ;
2015-08-12 12:41:40 -03:00
// send setpoints to main rotor controller and trigger recalculation of scalars
2019-08-07 23:52:17 -03:00
_main_rotor . set_control_mode ( static_cast < RotorControlMode > ( _main_rotor . _rsc_mode . get ( ) ) ) ;
2015-11-14 18:14:03 -04:00
calculate_armed_scalars ( ) ;
2018-03-08 13:16:31 -04:00
// send setpoints to DDVP 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 ) ;
2019-08-07 23:52:17 -03:00
_tail_rotor . set_ramp_time ( _main_rotor . _ramp_time . get ( ) ) ;
_tail_rotor . set_runup_time ( _main_rotor . _runup_time . get ( ) ) ;
_tail_rotor . set_critical_speed ( _main_rotor . _critical_speed . get ( ) ) ;
_tail_rotor . set_idle_output ( _main_rotor . _idle_output . get ( ) ) ;
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
}
}
// 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 ( )
{
2018-07-11 16:45:59 -03:00
// heli uses channels 1,2,3,4 and 8
// setup fast channels
2019-08-07 23:52:17 -03:00
uint32_t mask = 1U < < 0 | 1U < < 1 | 1U < < 2 | 1U < < 3 | 1U < < AP_MOTORS_HELI_RSC ;
2018-07-11 16:45:59 -03:00
2019-03-16 11:20:04 -03:00
if ( _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_90 | | _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_45 ) {
2019-01-12 00:17:15 -04:00
mask | = 1U < < 4 ;
}
2018-07-11 16:45:59 -03:00
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO ) {
mask | = 1U < < AP_MOTORS_HELI_SINGLE_EXTGYRO ;
}
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH ) {
mask | = 1U < < AP_MOTORS_HELI_SINGLE_TAILRSC ;
}
return rc_map_mask ( mask ) ;
2015-07-22 10:46:53 -03:00
}
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-10-14 12:19:16 -03:00
if ( state = = ROTOR_CONTROL_STOP ) {
// set engine run enable aux output to not run position to kill engine when disarmed
2017-01-03 05:56:57 -04:00
SRV_Channels : : set_output_limit ( SRV_Channel : : k_engine_run_enable , SRV_Channel : : SRV_CHANNEL_LIMIT_MIN ) ;
2015-10-14 12:19:16 -03:00
} else {
// else if armed, set engine run enable output to run position
2017-01-03 05:56:57 -04:00
SRV_Channels : : set_output_limit ( SRV_Channel : : k_engine_run_enable , SRV_Channel : : SRV_CHANNEL_LIMIT_MAX ) ;
2015-10-14 12:19: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-12 12:41:40 -03:00
// move_actuators - moves swash plate and tail rotor
2015-07-22 10:46:53 -03:00
// - expected ranges:
2016-02-02 08:24:39 -04:00
// roll : -1 ~ +1
// pitch: -1 ~ +1
// collective: 0 ~ 1
// yaw: -1 ~ +1
2015-07-22 10:46:53 -03:00
//
2016-02-02 08:24:39 -04:00
void AP_MotorsHeli_Single : : move_actuators ( float roll_out , float pitch_out , float coll_in , float yaw_out )
2015-07-22 10:46:53 -03:00
{
2016-02-02 08:24:39 -04:00
float yaw_offset = 0.0f ;
2015-07-22 10:46:53 -03:00
// initialize limits flag
2019-07-27 02:37:31 -03:00
limit . roll = false ;
limit . pitch = false ;
2015-07-22 10:46:53 -03:00
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
2017-08-27 20:51:33 -03:00
if ( _heliflags . inverted_flight ) {
coll_in = 1 - coll_in ;
}
2018-03-08 13:16:31 -04:00
2014-09-18 22:54:26 -03:00
// rescale roll_out and pitch_out into the min and max ranges to provide linear motion
2015-08-12 12:41:40 -03:00
// across the input range instead of stopping when the input hits the constrain value
2014-09-18 22:54:26 -03:00
// these calculations are based on an assumption of the user specified cyclic_max
2016-02-02 08:24:39 -04:00
// coming into this equation at 4500 or less
2016-04-16 06:58:46 -03:00
float total_out = norm ( pitch_out , roll_out ) ;
2014-09-18 22:54:26 -03:00
2016-02-02 08:24:39 -04:00
if ( total_out > ( _cyclic_max / 4500.0f ) ) {
float ratio = ( float ) ( _cyclic_max / 4500.0f ) / total_out ;
2014-09-18 22:54:26 -03:00
roll_out * = ratio ;
pitch_out * = ratio ;
2019-07-27 02:37:31 -03:00
limit . roll = true ;
limit . pitch = true ;
2015-08-12 12:41:40 -03:00
}
2015-07-22 10:46:53 -03:00
2015-08-12 12:41:40 -03:00
// constrain collective input
2016-02-02 08:24:39 -04:00
float collective_out = coll_in ;
if ( collective_out < = 0.0f ) {
collective_out = 0.0f ;
2015-08-12 12:41:40 -03:00
limit . throttle_lower = true ;
}
2016-02-02 08:24:39 -04:00
if ( collective_out > = 1.0f ) {
collective_out = 1.0f ;
2015-08-12 12:41:40 -03:00
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
2019-04-29 23:24:32 -03:00
if ( _heliflags . landing_collective & & collective_out < _collective_mid_pct ) {
collective_out = _collective_mid_pct ;
2015-08-12 12:41:40 -03:00
limit . throttle_lower = true ;
}
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
2015-10-14 15:57:51 -03:00
if ( _servo_mode = = SERVO_CONTROL_MODE_AUTOMATED ) {
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
2016-02-03 05:00:50 -04:00
if ( ( _main_rotor . get_control_output ( ) > _main_rotor . get_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 ) ;
2016-08-09 18:58:06 -03:00
// the 4.5 scaling factor is to bring the values in line with previous releases
yaw_offset = _collective_yaw_effect * fabsf ( collective_out - _collective_mid_pct ) / 4.5f ;
2015-07-22 10:46:53 -03:00
}
2015-08-12 12:41:40 -03:00
} else {
2016-02-02 08:24:39 -04:00
yaw_offset = 0.0f ;
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?
2018-03-23 01:09:14 -03:00
_main_rotor . set_collective ( fabsf ( collective_out ) ) ;
2015-08-11 21:20:28 -03:00
2018-03-28 12:20:59 -03:00
// scale collective pitch for swashplate servos
2018-03-23 01:09:14 -03:00
float collective_scalar = ( ( float ) ( _collective_max - _collective_min ) ) * 0.001f ;
2018-03-28 12:20:59 -03:00
float collective_out_scaled = collective_out * collective_scalar + ( _collective_min - 1000 ) * 0.001f ;
2019-01-12 00:17:15 -04:00
// get servo positions from swashplate library
_servo1_out = _swashplate . get_servo_out ( CH_1 , pitch_out , roll_out , collective_out_scaled ) ;
_servo2_out = _swashplate . get_servo_out ( CH_2 , pitch_out , roll_out , collective_out_scaled ) ;
_servo3_out = _swashplate . get_servo_out ( CH_3 , pitch_out , roll_out , collective_out_scaled ) ;
2019-03-16 11:20:04 -03:00
if ( _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_90 | | _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_45 ) {
2019-01-12 00:17:15 -04:00
_servo5_out = _swashplate . get_servo_out ( CH_4 , pitch_out , roll_out , collective_out_scaled ) ;
2018-03-28 12:20:59 -03:00
}
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-07-22 10:46:53 -03:00
}
2015-08-06 05:25:31 -03:00
// move_yaw
2016-02-02 08:25:31 -04:00
void AP_MotorsHeli_Single : : move_yaw ( float yaw_out )
2015-07-22 10:46:53 -03:00
{
2016-02-02 08:25:31 -04:00
// sanity check yaw_out
if ( yaw_out < - 1.0f ) {
yaw_out = - 1.0f ;
limit . yaw = true ;
}
if ( yaw_out > 1.0f ) {
yaw_out = 1.0f ;
2015-07-22 10:46:53 -03:00
limit . yaw = true ;
}
2018-12-28 02:32:05 -04:00
_servo4_out = yaw_out ;
}
void AP_MotorsHeli_Single : : output_to_motors ( )
{
if ( ! _flags . initialised_ok ) {
return ;
}
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
rc_write_swash ( AP_MOTORS_MOT_1 , _servo1_out ) ;
rc_write_swash ( AP_MOTORS_MOT_2 , _servo2_out ) ;
rc_write_swash ( AP_MOTORS_MOT_3 , _servo3_out ) ;
2019-01-12 00:17:15 -04:00
// get servo positions from swashplate library and write to servo for 4 servo of 4 servo swashplate
2019-03-16 11:20:04 -03:00
if ( _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_90 | | _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H4_45 ) {
2019-01-12 00:17:15 -04:00
rc_write_swash ( AP_MOTORS_MOT_5 , _servo5_out ) ;
}
2018-12-28 02:32:05 -04:00
if ( _tail_type ! = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH ) {
rc_write_angle ( AP_MOTORS_MOT_4 , _servo4_out * YAW_SERVO_MAX_ANGLE ) ;
2017-09-20 09:59:20 -03:00
}
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 ) {
2018-07-11 16:33:00 -03:00
rc_write ( AP_MOTORS_HELI_SINGLE_EXTGYRO , 1000 + _ext_gyro_gain_acro ) ;
2015-08-09 08:02:54 -03:00
} else {
2018-07-11 16:33:00 -03:00
rc_write ( AP_MOTORS_HELI_SINGLE_EXTGYRO , 1000 + _ext_gyro_gain_std ) ;
2015-08-09 08:02:54 -03:00
}
2015-07-22 10:46:53 -03:00
}
2018-12-28 02:32:05 -04:00
2019-04-09 09:15:45 -03:00
switch ( _spool_state ) {
case SpoolState : : SHUT_DOWN :
2018-12-28 02:32:05 -04:00
// sends minimum values out to the motors
update_motor_control ( ROTOR_CONTROL_STOP ) ;
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH ) {
rc_write_angle ( AP_MOTORS_MOT_4 , - YAW_SERVO_MAX_ANGLE ) ;
}
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : GROUND_IDLE :
2018-12-28 02:32:05 -04:00
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control ( ROTOR_CONTROL_IDLE ) ;
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH ) {
rc_write_angle ( AP_MOTORS_MOT_4 , - YAW_SERVO_MAX_ANGLE ) ;
}
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : SPOOLING_UP :
case SpoolState : : THROTTLE_UNLIMITED :
2018-12-28 02:32:05 -04:00
// set motor output based on thrust requests
update_motor_control ( ROTOR_CONTROL_ACTIVE ) ;
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH ) {
// constrain output so that motor never fully stops
_servo4_out = constrain_float ( _servo4_out , - 0.9f , 1.0f ) ;
// output yaw servo to tail rsc
rc_write_angle ( AP_MOTORS_MOT_4 , _servo4_out * YAW_SERVO_MAX_ANGLE ) ;
}
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : SPOOLING_DOWN :
2018-12-28 02:32:05 -04:00
// sends idle output to motors and wait for rotor to stop
update_motor_control ( ROTOR_CONTROL_IDLE ) ;
if ( _tail_type = = AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH ) {
rc_write_angle ( AP_MOTORS_MOT_4 , - YAW_SERVO_MAX_ANGLE ) ;
}
break ;
}
2015-07-22 10:46:53 -03:00
}
2015-10-21 17:00:36 -03:00
// servo_test - move servos through full range of movement
void AP_MotorsHeli_Single : : servo_test ( )
{
2015-10-30 14:38:04 -03:00
_servo_test_cycle_time + = 1.0f / _loop_rate ;
if ( ( _servo_test_cycle_time > = 0.0f & & _servo_test_cycle_time < 0.5f ) | | // Tilt swash back
( _servo_test_cycle_time > = 6.0f & & _servo_test_cycle_time < 6.5f ) ) {
2016-12-01 12:18:30 -04:00
_pitch_test + = ( 1.0f / ( _loop_rate / 2.0f ) ) ;
2016-02-25 13:13:02 -04:00
_oscillate_angle + = 8 * M_PI / _loop_rate ;
2016-02-05 21:38:18 -04:00
_yaw_test = 0.5f * sinf ( _oscillate_angle ) ;
2015-10-30 14:38:04 -03:00
} else if ( ( _servo_test_cycle_time > = 0.5f & & _servo_test_cycle_time < 4.5f ) | | // Roll swash around
( _servo_test_cycle_time > = 6.5f & & _servo_test_cycle_time < 10.5f ) ) {
2016-02-25 13:13:02 -04:00
_oscillate_angle + = M_PI / ( 2 * _loop_rate ) ;
2016-02-05 21:38:18 -04:00
_roll_test = sinf ( _oscillate_angle ) ;
_pitch_test = cosf ( _oscillate_angle ) ;
_yaw_test = sinf ( _oscillate_angle ) ;
2015-10-30 14:38:04 -03:00
} else if ( ( _servo_test_cycle_time > = 4.5f & & _servo_test_cycle_time < 5.0f ) | | // Return swash to level
( _servo_test_cycle_time > = 10.5f & & _servo_test_cycle_time < 11.0f ) ) {
2016-12-01 12:18:30 -04:00
_pitch_test - = ( 1.0f / ( _loop_rate / 2.0f ) ) ;
2016-02-25 13:13:02 -04:00
_oscillate_angle + = 8 * M_PI / _loop_rate ;
2016-02-05 21:38:18 -04:00
_yaw_test = 0.5f * sinf ( _oscillate_angle ) ;
2015-10-30 14:38:04 -03:00
} else if ( _servo_test_cycle_time > = 5.0f & & _servo_test_cycle_time < 6.0f ) { // Raise swash to top
2018-09-23 17:12:09 -03:00
_collective_test + = ( 1.0f / _loop_rate ) ;
2016-02-25 13:13:02 -04:00
_oscillate_angle + = 2 * M_PI / _loop_rate ;
2016-02-05 21:38:18 -04:00
_yaw_test = sinf ( _oscillate_angle ) ;
2015-10-30 14:38:04 -03:00
} else if ( _servo_test_cycle_time > = 11.0f & & _servo_test_cycle_time < 12.0f ) { // Lower swash to bottom
2018-09-23 17:12:09 -03:00
_collective_test - = ( 1.0f / _loop_rate ) ;
2016-02-25 13:13:02 -04:00
_oscillate_angle + = 2 * M_PI / _loop_rate ;
2016-02-05 21:38:18 -04:00
_yaw_test = sinf ( _oscillate_angle ) ;
2015-10-30 14:38:04 -03:00
} else { // reset cycle
_servo_test_cycle_time = 0.0f ;
_oscillate_angle = 0.0f ;
_collective_test = 0.0f ;
_roll_test = 0.0f ;
_pitch_test = 0.0f ;
_yaw_test = 0.0f ;
2015-10-21 20:47:24 -03:00
// decrement servo test cycle counter at the end of the cycle
if ( _servo_test_cycle_counter > 0 ) {
_servo_test_cycle_counter - - ;
}
2015-10-21 17:00:36 -03:00
}
// over-ride servo commands to move servos through defined ranges
2018-09-23 17:12:09 -03:00
_throttle_filter . reset ( constrain_float ( _collective_test , 0.0f , 1.0f ) ) ;
_roll_in = constrain_float ( _roll_test , - 1.0f , 1.0f ) ;
_pitch_in = constrain_float ( _pitch_test , - 1.0f , 1.0f ) ;
_yaw_in = constrain_float ( _yaw_test , - 1.0f , 1.0f ) ;
2015-10-21 17:00:36 -03:00
}
2015-11-30 16:16:04 -04:00
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli_Single : : parameter_check ( bool display_msg ) const
{
2019-08-07 23:52:17 -03:00
// returns false if direct drive tailspeed is outside of range
if ( ( _direct_drive_tailspeed < 0 ) | | ( _direct_drive_tailspeed > 100 ) ) {
if ( display_msg ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_TAIL_SPEED out of range " ) ;
}
return false ;
}
2019-01-12 00:17:15 -04:00
// returns false if Phase Angle is outside of range for H3 swashplate
2019-03-16 11:20:04 -03:00
if ( _swashplate . get_swash_type ( ) = = SWASHPLATE_TYPE_H3 & & ( _swashplate . get_phase_angle ( ) > 30 | | _swashplate . get_phase_angle ( ) < - 30 ) ) {
2015-11-30 16:16:04 -04:00
if ( display_msg ) {
2019-01-12 00:17:15 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_H3_PHANG out of range " ) ;
2015-11-30 16:16:04 -04:00
}
return false ;
}
// returns false if Acro External Gyro Gain is outside of range
if ( ( _ext_gyro_gain_acro < 0 ) | | ( _ext_gyro_gain_acro > 1000 ) ) {
if ( display_msg ) {
2017-07-09 01:15:34 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_GYR_GAIN_ACRO out of range " ) ;
2015-11-30 16:16:04 -04:00
}
return false ;
}
// returns false if Standard External Gyro Gain is outside of range
if ( ( _ext_gyro_gain_std < 0 ) | | ( _ext_gyro_gain_std > 1000 ) ) {
if ( display_msg ) {
2017-07-09 01:15:34 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_GYR_GAIN out of range " ) ;
2015-11-30 16:16:04 -04:00
}
return false ;
}
// check parent class parameters
return AP_MotorsHeli : : parameter_check ( display_msg ) ;
}