2013-08-29 02:34:34 -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/>.
*/
2015-08-11 03:28:44 -03:00
# include <AP_HAL/AP_HAL.h>
# include <AP_Math/AP_Math.h>
2017-01-09 00:01:30 -04:00
# include <GCS_MAVLink/GCS.h>
2012-04-02 05:26:37 -03:00
# include "AP_MotorsTri.h"
2012-10-26 20:59:07 -03:00
extern const AP_HAL : : HAL & hal ;
2012-04-02 05:26:37 -03:00
// init
2016-12-14 01:46:42 -04:00
void AP_MotorsTri : : init ( motor_frame_class frame_class , motor_frame_type frame_type )
2012-04-02 05:26:37 -03:00
{
2016-04-28 09:36:41 -03:00
add_motor_num ( AP_MOTORS_MOT_1 ) ;
add_motor_num ( AP_MOTORS_MOT_2 ) ;
add_motor_num ( AP_MOTORS_MOT_4 ) ;
2019-04-19 21:59:40 -03:00
2012-08-17 03:20:26 -03:00
// set update rate for the 3 motors (but not the servo on channel 7)
set_update_rate ( _speed_hz ) ;
2013-05-19 05:28:39 -03:00
// set the motor_enabled flag so that the ESCs can be calibrated like other frame types
motor_enabled [ AP_MOTORS_MOT_1 ] = true ;
motor_enabled [ AP_MOTORS_MOT_2 ] = true ;
motor_enabled [ AP_MOTORS_MOT_4 ] = true ;
2014-02-07 09:02:31 -04:00
2020-09-17 18:08:02 -03:00
# if !APM_BUILD_TYPE(APM_BUILD_ArduPlane) // Tilt Rotors do not need a yaw servo
2017-01-09 00:01:30 -04:00
// find the yaw servo
2020-09-17 18:08:02 -03:00
if ( ! SRV_Channels : : get_channel_for ( SRV_Channel : : k_motor7 , AP_MOTORS_CH_TRI_YAW ) ) {
2017-07-09 01:15:34 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_ERROR , " MotorsTri: unable to setup yaw channel " ) ;
2017-01-09 00:01:30 -04:00
// don't set initialised_ok
return ;
}
2020-09-17 18:08:02 -03:00
# endif
2019-04-19 21:59:40 -03:00
2016-04-21 04:32:25 -03:00
// allow mapping of motor7
add_motor_num ( AP_MOTORS_CH_TRI_YAW ) ;
2016-12-14 01:46:42 -04:00
2019-10-12 07:38:51 -03:00
SRV_Channels : : set_angle ( SRV_Channels : : get_motor_function ( AP_MOTORS_CH_TRI_YAW ) , _yaw_servo_angle_max_deg * 100 ) ;
2019-03-19 17:19:38 -03:00
// check for reverse tricopter
if ( frame_type = = MOTOR_FRAME_TYPE_PLUSREV ) {
_pitch_reversed = true ;
}
2016-12-14 01:46:42 -04:00
// record successful initialisation if what we setup was the desired frame_class
2019-05-03 02:27:09 -03:00
set_initialised_ok ( frame_class = = MOTOR_FRAME_TRI ) ;
2016-12-14 01:46:42 -04:00
}
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void AP_MotorsTri : : set_frame_class_and_type ( motor_frame_class frame_class , motor_frame_type frame_type )
{
2019-03-19 17:19:38 -03:00
// check for reverse tricopter
if ( frame_type = = MOTOR_FRAME_TYPE_PLUSREV ) {
_pitch_reversed = true ;
} else {
_pitch_reversed = false ;
}
2019-05-03 02:27:09 -03:00
set_initialised_ok ( ( frame_class = = MOTOR_FRAME_TRI ) & & SRV_Channels : : function_assigned ( SRV_Channel : : k_motor7 ) ) ;
2012-04-02 05:26:37 -03:00
}
2012-09-13 09:31:13 -03:00
// set update rate to motors - a value in hertz
2019-04-19 21:59:40 -03:00
void AP_MotorsTri : : set_update_rate ( uint16_t speed_hz )
2012-04-02 05:26:37 -03:00
{
2012-08-17 03:20:26 -03:00
// record requested speed
_speed_hz = speed_hz ;
2012-04-02 05:26:37 -03:00
2012-08-17 03:20:26 -03:00
// set update rate for the 3 motors (but not the servo on channel 7)
2013-01-10 00:52:46 -04:00
uint32_t mask =
2015-09-29 00:01:12 -03:00
1U < < AP_MOTORS_MOT_1 |
1U < < AP_MOTORS_MOT_2 |
1U < < AP_MOTORS_MOT_4 ;
2016-01-04 06:24:06 -04:00
rc_set_freq ( mask , _speed_hz ) ;
2012-04-02 05:26:37 -03:00
}
2016-01-19 23:15:10 -04:00
void AP_MotorsTri : : output_to_motors ( )
{
2019-04-09 09:15:45 -03:00
switch ( _spool_state ) {
case SpoolState : : SHUT_DOWN :
2016-01-19 23:15:10 -04:00
// sends minimum values out to the motors
2018-11-30 10:34:14 -04:00
rc_write ( AP_MOTORS_MOT_1 , output_to_pwm ( 0 ) ) ;
rc_write ( AP_MOTORS_MOT_2 , output_to_pwm ( 0 ) ) ;
rc_write ( AP_MOTORS_MOT_4 , output_to_pwm ( 0 ) ) ;
2019-10-12 07:38:51 -03:00
rc_write_angle ( AP_MOTORS_CH_TRI_YAW , 0 ) ;
2016-01-19 23:15:10 -04:00
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : GROUND_IDLE :
2016-01-19 23:15:10 -04:00
// sends output to motors when armed but not flying
2019-01-21 07:13:00 -04:00
set_actuator_with_slew ( _actuator [ 1 ] , actuator_spin_up_to_ground_idle ( ) ) ;
set_actuator_with_slew ( _actuator [ 2 ] , actuator_spin_up_to_ground_idle ( ) ) ;
set_actuator_with_slew ( _actuator [ 4 ] , actuator_spin_up_to_ground_idle ( ) ) ;
2018-11-30 10:34:14 -04:00
rc_write ( AP_MOTORS_MOT_1 , output_to_pwm ( _actuator [ 1 ] ) ) ;
rc_write ( AP_MOTORS_MOT_2 , output_to_pwm ( _actuator [ 2 ] ) ) ;
rc_write ( AP_MOTORS_MOT_4 , output_to_pwm ( _actuator [ 4 ] ) ) ;
2019-10-12 07:38:51 -03:00
rc_write_angle ( AP_MOTORS_CH_TRI_YAW , 0 ) ;
2016-01-19 23:15:10 -04:00
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : SPOOLING_UP :
case SpoolState : : THROTTLE_UNLIMITED :
case SpoolState : : SPOOLING_DOWN :
2016-01-19 23:15:10 -04:00
// set motor output based on thrust requests
2018-11-30 10:34:14 -04:00
set_actuator_with_slew ( _actuator [ 1 ] , thrust_to_actuator ( _thrust_right ) ) ;
set_actuator_with_slew ( _actuator [ 2 ] , thrust_to_actuator ( _thrust_left ) ) ;
set_actuator_with_slew ( _actuator [ 4 ] , thrust_to_actuator ( _thrust_rear ) ) ;
rc_write ( AP_MOTORS_MOT_1 , output_to_pwm ( _actuator [ 1 ] ) ) ;
rc_write ( AP_MOTORS_MOT_2 , output_to_pwm ( _actuator [ 2 ] ) ) ;
rc_write ( AP_MOTORS_MOT_4 , output_to_pwm ( _actuator [ 4 ] ) ) ;
2019-10-12 07:38:51 -03:00
rc_write_angle ( AP_MOTORS_CH_TRI_YAW , degrees ( _pivot_angle ) * 100 ) ;
2016-01-19 23:15:10 -04:00
break ;
}
}
2014-07-26 04:27:39 -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_MotorsTri : : get_motor_mask ( )
{
2015-05-25 10:26:48 -03:00
// tri copter uses channels 1,2,4 and 7
2018-08-24 02:39:19 -03:00
uint16_t motor_mask = ( 1U < < AP_MOTORS_MOT_1 ) |
( 1U < < AP_MOTORS_MOT_2 ) |
( 1U < < AP_MOTORS_MOT_4 ) |
( 1U < < AP_MOTORS_CH_TRI_YAW ) ;
uint16_t mask = rc_map_mask ( motor_mask ) ;
// add parent's mask
mask | = AP_MotorsMulticopter : : get_motor_mask ( ) ;
return mask ;
2014-07-26 04:27:39 -03:00
}
2016-01-19 23:14:20 -04:00
// output_armed - sends commands to the motors
// includes new scaling stability patch
2015-04-02 17:54:15 -03:00
void AP_MotorsTri : : output_armed_stabilizing ( )
{
2016-01-19 23:14:20 -04:00
float roll_thrust ; // roll thrust input value, +/- 1.0
float pitch_thrust ; // pitch thrust input value, +/- 1.0
float yaw_thrust ; // yaw thrust input value, +/- 1.0
float throttle_thrust ; // throttle thrust input value, 0.0 - 1.0
2018-01-14 21:31:31 -04:00
float throttle_avg_max ; // throttle thrust average maximum value, 0.0 - 1.0
2016-01-19 23:14:20 -04:00
float throttle_thrust_best_rpy ; // throttle providing maximum roll, pitch and yaw range without climbing
float rpy_scale = 1.0f ; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float rpy_low = 0.0f ; // lowest motor value
float rpy_high = 0.0f ; // highest motor value
float thr_adj ; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
2019-10-12 07:38:51 -03:00
SRV_Channels : : set_angle ( SRV_Channels : : get_motor_function ( AP_MOTORS_CH_TRI_YAW ) , _yaw_servo_angle_max_deg * 100 ) ;
2016-03-25 05:56:58 -03:00
// sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
_yaw_servo_angle_max_deg = constrain_float ( _yaw_servo_angle_max_deg , AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN , AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX ) ;
2016-01-19 23:14:20 -04:00
// apply voltage and air pressure compensation
2018-01-24 03:42:10 -04:00
const float compensation_gain = get_compensation_gain ( ) ;
2019-06-27 06:32:30 -03:00
roll_thrust = ( _roll_in + _roll_in_ff ) * compensation_gain ;
pitch_thrust = ( _pitch_in + _pitch_in_ff ) * compensation_gain ;
yaw_thrust = ( _yaw_in + _yaw_in_ff ) * compensation_gain * sinf ( radians ( _yaw_servo_angle_max_deg ) ) ; // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
2018-01-24 03:42:10 -04:00
throttle_thrust = get_throttle ( ) * compensation_gain ;
throttle_avg_max = _throttle_avg_max * compensation_gain ;
2016-03-19 11:15:05 -03:00
2019-03-19 17:19:38 -03:00
// check for reversed pitch
if ( _pitch_reversed ) {
pitch_thrust * = - 1.0f ;
}
2016-03-19 11:15:05 -03:00
// calculate angle of yaw pivot
_pivot_angle = safe_asin ( yaw_thrust ) ;
if ( fabsf ( _pivot_angle ) > radians ( _yaw_servo_angle_max_deg ) ) {
limit . yaw = true ;
_pivot_angle = constrain_float ( _pivot_angle , - radians ( _yaw_servo_angle_max_deg ) , radians ( _yaw_servo_angle_max_deg ) ) ;
}
float pivot_thrust_max = cosf ( _pivot_angle ) ;
2016-01-19 23:14:20 -04:00
float thrust_max = 1.0f ;
// sanity check throttle is above zero and below current limited throttle
if ( throttle_thrust < = 0.0f ) {
throttle_thrust = 0.0f ;
2014-10-04 11:27:11 -03:00
limit . throttle_lower = true ;
}
2016-01-19 23:14:20 -04:00
if ( throttle_thrust > = _throttle_thrust_max ) {
throttle_thrust = _throttle_thrust_max ;
2014-10-04 11:27:11 -03:00
limit . throttle_upper = true ;
}
2012-08-17 03:20:26 -03:00
2018-01-14 21:31:31 -04:00
throttle_avg_max = constrain_float ( throttle_avg_max , throttle_thrust , _throttle_thrust_max ) ;
2015-01-21 07:16:34 -04:00
2016-01-19 23:14:20 -04:00
// The following mix may be offer less coupling between axis but needs testing
//_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
//_thrust_left = roll_thrust * 0.5f + pitch_thrust * 1.0f;
//_thrust_rear = 0;
2012-08-17 03:20:26 -03:00
2016-01-19 23:14:20 -04:00
_thrust_right = roll_thrust * - 0.5f + pitch_thrust * 0.5f ;
_thrust_left = roll_thrust * 0.5f + pitch_thrust * 0.5f ;
_thrust_rear = pitch_thrust * - 0.5f ;
2013-09-12 14:36:58 -03:00
2016-01-19 23:14:20 -04:00
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
2013-07-16 00:46:34 -03:00
2016-01-19 23:14:20 -04:00
// record lowest roll pitch command
2019-04-19 21:59:40 -03:00
rpy_low = MIN ( _thrust_right , _thrust_left ) ;
rpy_high = MAX ( _thrust_right , _thrust_left ) ;
if ( rpy_low > _thrust_rear ) {
2016-01-19 23:14:20 -04:00
rpy_low = _thrust_rear ;
}
// check to see if the rear motor will reach maximum thrust before the front two motors
2019-04-19 21:59:40 -03:00
if ( ( 1.0f - rpy_high ) > ( pivot_thrust_max - _thrust_rear ) ) {
2016-01-19 23:14:20 -04:00
thrust_max = pivot_thrust_max ;
rpy_high = _thrust_rear ;
}
2015-04-02 17:54:15 -03:00
2016-01-19 23:14:20 -04:00
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible room margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// check everything fits
2019-04-19 21:59:40 -03:00
throttle_thrust_best_rpy = MIN ( 0.5f * thrust_max - ( rpy_low + rpy_high ) / 2.0 , throttle_avg_max ) ;
if ( is_zero ( rpy_low ) ) {
2016-01-19 23:14:20 -04:00
rpy_scale = 1.0f ;
} else {
2019-04-19 21:59:40 -03:00
rpy_scale = constrain_float ( - throttle_thrust_best_rpy / rpy_low , 0.0f , 1.0f ) ;
2016-01-19 23:14:20 -04:00
}
2013-07-16 00:46:34 -03:00
2016-01-19 23:14:20 -04:00
// calculate how close the motors can come to the desired throttle
thr_adj = throttle_thrust - throttle_thrust_best_rpy ;
2019-04-19 21:59:40 -03:00
if ( rpy_scale < 1.0f ) {
2016-01-19 23:14:20 -04:00
// Full range is being used by roll, pitch, and yaw.
2019-07-27 02:37:31 -03:00
limit . roll = true ;
limit . pitch = true ;
2019-04-19 21:59:40 -03:00
if ( thr_adj > 0.0f ) {
2016-01-19 23:14:20 -04:00
limit . throttle_upper = true ;
2013-07-16 00:46:34 -03:00
}
2016-01-19 23:14:20 -04:00
thr_adj = 0.0f ;
2019-04-19 21:59:40 -03:00
} else {
if ( thr_adj < - ( throttle_thrust_best_rpy + rpy_low ) ) {
2016-01-19 23:14:20 -04:00
// Throttle can't be reduced to desired value
2019-04-19 21:59:40 -03:00
thr_adj = - ( throttle_thrust_best_rpy + rpy_low ) ;
} else if ( thr_adj > thrust_max - ( throttle_thrust_best_rpy + rpy_high ) ) {
2016-01-19 23:14:20 -04:00
// Throttle can't be increased to desired value
2019-04-19 21:59:40 -03:00
thr_adj = thrust_max - ( throttle_thrust_best_rpy + rpy_high ) ;
2016-01-19 23:14:20 -04:00
limit . throttle_upper = true ;
2013-07-16 00:46:34 -03:00
}
2016-01-19 23:14:20 -04:00
}
2013-07-16 00:46:34 -03:00
2019-07-10 04:26:12 -03:00
// determine throttle thrust for harmonic notch
const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj ;
// compensation_gain can never be zero
_throttle_out = throttle_thrust_best_plus_adj / compensation_gain ;
2016-01-19 23:14:20 -04:00
// add scaled roll, pitch, constrained yaw and throttle for each motor
2019-07-10 04:26:12 -03:00
_thrust_right = throttle_thrust_best_plus_adj + rpy_scale * _thrust_right ;
_thrust_left = throttle_thrust_best_plus_adj + rpy_scale * _thrust_left ;
_thrust_rear = throttle_thrust_best_plus_adj + rpy_scale * _thrust_rear ;
2016-01-19 23:14:20 -04:00
// scale pivot thrust to account for pivot angle
2016-03-25 05:56:58 -03:00
// we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range
2019-04-19 21:59:40 -03:00
_thrust_rear = _thrust_rear / cosf ( _pivot_angle ) ;
2016-01-19 23:14:20 -04:00
// constrain all outputs to 0.0f to 1.0f
// test code should be run with these lines commented out as they should not do anything
_thrust_right = constrain_float ( _thrust_right , 0.0f , 1.0f ) ;
_thrust_left = constrain_float ( _thrust_left , 0.0f , 1.0f ) ;
_thrust_rear = constrain_float ( _thrust_rear , 0.0f , 1.0f ) ;
2012-04-02 05:26:37 -03:00
}
2018-04-27 13:22:07 -03:00
// output_test_seq - spin a motor at the pwm value specified
2014-04-28 04:30:26 -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_MotorsTri : : output_test_seq ( uint8_t motor_seq , int16_t pwm )
2012-04-02 05:26:37 -03:00
{
2014-04-28 04:30:26 -03:00
// exit immediately if not armed
2015-04-02 17:54:15 -03:00
if ( ! armed ( ) ) {
2014-04-28 04:30:26 -03:00
return ;
}
2012-10-26 20:59:07 -03:00
2014-04-28 04:30:26 -03:00
// output to motors and servos
switch ( motor_seq ) {
case 1 :
// front right motor
2016-01-04 01:56:54 -04:00
rc_write ( AP_MOTORS_MOT_1 , pwm ) ;
2014-04-28 04:30:26 -03:00
break ;
case 2 :
// back motor
2016-01-04 01:56:54 -04:00
rc_write ( AP_MOTORS_MOT_4 , pwm ) ;
2014-04-28 04:30:26 -03:00
break ;
case 3 :
// back servo
2016-01-04 01:56:54 -04:00
rc_write ( AP_MOTORS_CH_TRI_YAW , pwm ) ;
2014-04-28 04:30:26 -03:00
break ;
case 4 :
// front left motor
2016-01-04 01:56:54 -04:00
rc_write ( AP_MOTORS_MOT_2 , pwm ) ;
2014-04-28 04:30:26 -03:00
break ;
default :
// do nothing
break ;
}
2012-10-26 20:59:07 -03:00
}
2015-05-14 22:00:46 -03:00
2016-05-01 19:00:45 -03:00
/*
call vehicle supplied thrust compensation if set . This allows for
vehicle specific thrust compensation for motor arrangements such as
the forward motors tilting
*/
void AP_MotorsTri : : thrust_compensation ( void )
{
if ( _thrust_compensation_callback ) {
// convert 3 thrust values into an array indexed by motor number
float thrust [ 4 ] { _thrust_right , _thrust_left , 0 , _thrust_rear } ;
// apply vehicle supplied compensation function
_thrust_compensation_callback ( thrust , 4 ) ;
// extract compensated thrust values
_thrust_right = thrust [ 0 ] ;
2019-04-19 21:59:40 -03:00
_thrust_left = thrust [ 1 ] ;
_thrust_rear = thrust [ 3 ] ;
2016-05-01 19:00:45 -03:00
}
}
2017-05-08 20:05:30 -03:00
/*
override tricopter tail servo output in output_motor_mask
*/
2018-12-29 11:44:44 -04:00
void AP_MotorsTri : : output_motor_mask ( float thrust , uint8_t mask , float rudder_dt )
2017-05-08 20:05:30 -03:00
{
// normal multicopter output
2018-12-29 11:44:44 -04:00
AP_MotorsMulticopter : : output_motor_mask ( thrust , mask , rudder_dt ) ;
2017-05-08 20:05:30 -03:00
// and override yaw servo
2019-10-12 07:38:51 -03:00
rc_write_angle ( AP_MOTORS_CH_TRI_YAW , 0 ) ;
2017-05-08 20:05:30 -03:00
}
2018-12-29 11:44:44 -04:00
float AP_MotorsTri : : get_roll_factor ( uint8_t i )
{
float ret = 0.0f ;
switch ( i ) {
// right motor
case AP_MOTORS_MOT_1 :
ret = - 1.0f ;
break ;
2019-04-19 21:59:40 -03:00
// left motor
2018-12-29 11:44:44 -04:00
case AP_MOTORS_MOT_2 :
ret = 1.0f ;
break ;
}
return ret ;
}