2013-05-29 20:51:34 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
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/>.
*/
2012-04-02 05:26:37 -03:00
/*
2012-08-17 03:20:26 -03:00
* AP_MotorsMatrix . cpp - ArduCopter motors library
* Code by RandyMackay . DIYDrones . com
*
*/
2015-08-11 03:28:44 -03:00
# include <AP_HAL/AP_HAL.h>
2012-04-02 05:26:37 -03:00
# include "AP_MotorsMatrix.h"
2012-10-26 20:59:07 -03:00
extern const AP_HAL : : HAL & hal ;
2012-04-02 05:26:37 -03:00
// Init
void AP_MotorsMatrix : : Init ( )
{
2012-08-17 03:20:26 -03:00
// setup the motors
setup_motors ( ) ;
2012-04-02 05:26:37 -03:00
2012-08-17 03:20:26 -03:00
// enable fast channels or instant pwm
set_update_rate ( _speed_hz ) ;
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
2012-04-02 05:26:37 -03:00
void AP_MotorsMatrix : : set_update_rate ( uint16_t speed_hz )
{
2015-09-29 00:00:50 -03:00
uint8_t i ;
2012-08-17 03:20:26 -03:00
// record requested speed
_speed_hz = speed_hz ;
// check each enabled motor
2013-01-10 00:52:46 -04:00
uint32_t mask = 0 ;
2012-08-17 03:20:26 -03:00
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2015-09-29 00:00:50 -03:00
mask | = 1U < < i ;
2012-08-17 03:20:26 -03:00
}
}
2016-01-04 06:24:06 -04:00
rc_set_freq ( mask , _speed_hz ) ;
2012-04-02 05:26:37 -03:00
}
// set frame orientation (normally + or X)
void AP_MotorsMatrix : : set_frame_orientation ( uint8_t new_orientation )
{
2012-08-17 03:20:26 -03:00
// return if nothing has changed
2013-09-12 10:27:44 -03:00
if ( new_orientation = = _flags . frame_orientation ) {
2012-08-17 03:20:26 -03:00
return ;
}
2012-04-02 05:26:37 -03:00
2012-08-17 03:20:26 -03:00
// call parent
AP_Motors : : set_frame_orientation ( new_orientation ) ;
2012-04-02 05:26:37 -03:00
2012-08-17 03:20:26 -03:00
// setup the motors
setup_motors ( ) ;
2012-04-02 05:26:37 -03:00
2012-08-17 03:20:26 -03:00
// enable fast channels or instant pwm
set_update_rate ( _speed_hz ) ;
2012-04-02 05:26:37 -03:00
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsMatrix : : enable ( )
{
2012-08-17 03:20:26 -03:00
int8_t i ;
// enable output channels
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2016-01-04 06:24:06 -04:00
rc_enable_ch ( i ) ;
2012-08-17 03:20:26 -03:00
}
}
2012-04-02 05:26:37 -03:00
}
// output_min - sends minimum values out to the motors
void AP_MotorsMatrix : : output_min ( )
{
2012-08-17 03:20:26 -03:00
int8_t i ;
2016-01-18 01:08:50 -04:00
// send output to each motor
2015-09-28 19:03:32 -03:00
hal . rcout - > cork ( ) ;
2012-08-17 03:20:26 -03:00
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2016-01-04 01:56:54 -04:00
rc_write ( i , _throttle_radio_min ) ;
2012-08-17 03:20:26 -03:00
}
}
2015-09-28 19:03:32 -03:00
hal . rcout - > push ( ) ;
2012-04-02 05:26:37 -03:00
}
2016-01-18 01:18:34 -04:00
void AP_MotorsMatrix : : output_to_motors ( )
{
int8_t i ;
int16_t motor_out [ AP_MOTORS_MAX_NUM_MOTORS ] ; // final pwm values sent to the motor
switch ( _multicopter_flags . spool_mode ) {
case SHUT_DOWN :
// sends minimum values out to the motors
// set motor output based on thrust requests
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
motor_out [ i ] = _throttle_radio_min ;
}
}
break ;
case SPIN_WHEN_ARMED :
// sends output to motors when armed but not flying
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
motor_out [ i ] = constrain_int16 ( _throttle_radio_min + _throttle_low_end_pct * _min_throttle , _throttle_radio_min , _throttle_radio_min + _min_throttle ) ;
}
}
break ;
case SPOOL_UP :
case THROTTLE_UNLIMITED :
case SPOOL_DOWN :
// set motor output based on thrust requests
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
motor_out [ i ] = calc_thrust_to_pwm ( _thrust_rpyt_out [ i ] ) ;
}
}
break ;
}
// send output to each motor
hal . rcout - > cork ( ) ;
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
rc_write ( i , motor_out [ i ] ) ;
}
}
hal . rcout - > push ( ) ;
}
2014-07-26 04:29:48 -03:00
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsMatrix : : get_motor_mask ( )
{
uint16_t mask = 0 ;
for ( uint8_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
mask | = 1U < < i ;
}
}
2016-01-04 16:35:24 -04:00
return rc_map_mask ( mask ) ;
2014-07-26 04:29:48 -03:00
}
2013-05-26 23:21:31 -03:00
// output_armed - sends commands to the motors
// includes new scaling stability patch
2015-04-02 17:54:15 -03:00
void AP_MotorsMatrix : : output_armed_stabilizing ( )
2013-05-26 23:21:31 -03:00
{
2015-12-03 01:48:35 -04:00
uint8_t i ; // general purpose counter
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
float throttle_thrust_best_rpy ; // throttle providing maximum roll, pitch and yaw range without climbing
float throttle_thrust_rpy_mix ; // partial calculation of throttle_thrust_best_rpy
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 yaw_allowed = 1.0f ; // amount of yaw we can fit in
float unused_range ; // amount of yaw we can fit in the current channel
float thr_adj ; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct ( ) ; // throttle hover thrust value, 0.0 - 1.0
// apply voltage and air pressure compensation
2016-01-21 22:09:18 -04:00
roll_thrust = _roll_in * get_compensation_gain ( ) ;
pitch_thrust = _pitch_in * get_compensation_gain ( ) ;
yaw_thrust = _yaw_in * get_compensation_gain ( ) ;
2015-12-03 01:48:35 -04:00
throttle_thrust = get_throttle ( ) * get_compensation_gain ( ) ;
// sanity check throttle is above zero and below current limited throttle
if ( throttle_thrust < = 0.0f ) {
throttle_thrust = 0.0f ;
2013-09-12 10:27:44 -03:00
limit . throttle_lower = true ;
}
2015-12-03 01:48:35 -04:00
if ( throttle_thrust > = _throttle_thrust_max ) {
throttle_thrust = _throttle_thrust_max ;
2013-09-12 10:27:44 -03:00
limit . throttle_upper = true ;
}
2013-05-26 23:21:31 -03:00
2015-12-03 01:48:35 -04:00
throttle_thrust_rpy_mix = MAX ( throttle_thrust , throttle_thrust * MAX ( 0.0f , 1.0f - _throttle_rpy_mix ) + throttle_thrust_hover * _throttle_rpy_mix ) ;
2013-05-26 23:21:31 -03:00
2015-12-03 01:48:35 -04:00
// calculate throttle that gives most possible room for yaw which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
2015-04-02 17:54:15 -03:00
// 2. the higher of:
// a) the pilot's throttle input
2015-12-03 01:48:35 -04:00
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
2015-04-02 17:54:15 -03:00
// 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.
2015-12-03 01:48:35 -04:00
// 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
2015-04-02 17:54:15 -03:00
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
2015-12-03 01:48:35 -04:00
throttle_thrust_best_rpy = MIN ( 0.5f , throttle_thrust_rpy_mix ) ;
// calculate roll and pitch for each motor
// calculate the amount of yaw input that each motor can accept
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
_thrust_rpyt_out [ i ] = roll_thrust * _roll_factor [ i ] + pitch_thrust * _pitch_factor [ i ] ;
if ( ! is_zero ( _yaw_factor [ i ] ) ) {
if ( yaw_thrust * _yaw_factor [ i ] > 0.0f ) {
unused_range = fabsf ( ( 1.0 - ( throttle_thrust_best_rpy + _thrust_rpyt_out [ i ] ) ) / _yaw_factor [ i ] ) ;
if ( yaw_allowed > unused_range ) {
yaw_allowed = unused_range ;
}
} else {
unused_range = fabsf ( ( throttle_thrust_best_rpy + _thrust_rpyt_out [ i ] ) / _yaw_factor [ i ] ) ;
if ( yaw_allowed > unused_range ) {
yaw_allowed = unused_range ;
}
}
}
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
}
2013-05-26 23:21:31 -03:00
2015-12-03 01:48:35 -04:00
// todo: make _yaw_headroom 0 to 1
yaw_allowed = MAX ( yaw_allowed , ( float ) _yaw_headroom / 1000.0f ) ;
if ( fabsf ( yaw_thrust ) > yaw_allowed ) {
yaw_thrust = constrain_float ( yaw_thrust , - yaw_allowed , yaw_allowed ) ;
limit . yaw = true ;
}
2015-04-02 17:54:15 -03:00
// add yaw to intermediate numbers for each motor
2015-12-03 01:48:35 -04:00
rpy_low = 0.0f ;
rpy_high = 0.0f ;
2015-04-02 17:54:15 -03:00
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2015-12-03 01:48:35 -04:00
_thrust_rpyt_out [ i ] = _thrust_rpyt_out [ i ] + yaw_thrust * _yaw_factor [ i ] ;
2013-05-26 23:21:31 -03:00
2015-04-02 17:54:15 -03:00
// record lowest roll+pitch+yaw command
2015-12-03 01:48:35 -04:00
if ( _thrust_rpyt_out [ i ] < rpy_low ) {
rpy_low = _thrust_rpyt_out [ i ] ;
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
// record highest roll+pitch+yaw command
2015-12-03 01:48:35 -04:00
if ( _thrust_rpyt_out [ i ] > rpy_high ) {
rpy_high = _thrust_rpyt_out [ i ] ;
2013-07-25 12:45:59 -03:00
}
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
}
2013-05-26 23:21:31 -03:00
2015-04-02 17:54:15 -03:00
// check everything fits
2015-12-03 01:48:35 -04:00
throttle_thrust_best_rpy = MIN ( 0.5f - ( rpy_low + rpy_high ) / 2.0 , throttle_thrust_rpy_mix ) ;
if ( is_zero ( rpy_low ) ) {
rpy_scale = 1.0f ;
} else {
rpy_scale = constrain_float ( - throttle_thrust_best_rpy / rpy_low , 0.0f , 1.0f ) ;
2015-04-02 17:54:15 -03:00
}
2013-05-26 23:21:31 -03:00
2015-12-03 01:48:35 -04:00
// calculate how close the motors can come to the desired throttle
thr_adj = throttle_thrust - throttle_thrust_best_rpy ;
if ( rpy_scale < 1.0f ) {
// Full range is being used by roll, pitch, and yaw.
2015-04-02 17:54:15 -03:00
limit . roll_pitch = true ;
limit . yaw = true ;
2015-12-03 01:48:35 -04:00
if ( thr_adj > 0.0f ) {
limit . throttle_upper = true ;
2015-07-20 08:25:39 -03:00
}
2015-12-03 01:48:35 -04:00
thr_adj = 0.0f ;
} else {
if ( thr_adj < - ( throttle_thrust_best_rpy + rpy_low ) ) {
// Throttle can't be reduced to desired value
thr_adj = - ( throttle_thrust_best_rpy + rpy_low ) ;
} else if ( thr_adj > 1.0f - ( throttle_thrust_best_rpy + rpy_high ) ) {
// Throttle can't be increased to desired value
thr_adj = 1.0f - ( throttle_thrust_best_rpy + rpy_high ) ;
limit . throttle_upper = true ;
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
}
2013-05-26 23:21:31 -03:00
2015-12-03 01:48:35 -04:00
// add scaled roll, pitch, constrained yaw and throttle for each motor
2015-04-02 17:54:15 -03:00
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2015-12-03 01:48:35 -04:00
_thrust_rpyt_out [ i ] = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_rpyt_out [ i ] ;
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
}
2015-02-21 03:55:13 -04:00
2015-12-03 01:48:35 -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
2015-04-02 17:54:15 -03:00
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2015-12-03 01:48:35 -04:00
_thrust_rpyt_out [ i ] = constrain_float ( _thrust_rpyt_out [ i ] , 0.0f , 1.0f ) ;
2013-05-26 23:21:31 -03:00
}
}
}
2012-04-02 05:26:37 -03:00
2014-04-28 04:29:09 -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_MotorsMatrix : : output_test ( uint8_t motor_seq , int16_t pwm )
2012-04-02 05:26:37 -03:00
{
2014-04-28 04:29:09 -03:00
// exit immediately if not armed
2015-04-02 17:54:15 -03:00
if ( ! armed ( ) ) {
2014-04-28 04:29:09 -03:00
return ;
2012-08-17 03:20:26 -03:00
}
// loop through all the possible orders spinning any motors that match that description
2015-09-28 19:03:32 -03:00
hal . rcout - > cork ( ) ;
2014-04-28 04:29:09 -03:00
for ( uint8_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] & & _test_order [ i ] = = motor_seq ) {
// turn on this motor
2016-01-04 01:56:54 -04:00
rc_write ( i , pwm ) ;
2012-08-17 03:20:26 -03:00
}
}
2015-09-28 19:03:32 -03:00
hal . rcout - > push ( ) ;
2012-04-02 05:26:37 -03:00
}
// add_motor
2013-05-14 06:03:34 -03:00
void AP_MotorsMatrix : : add_motor_raw ( int8_t motor_num , float roll_fac , float pitch_fac , float yaw_fac , uint8_t testing_order )
2012-04-02 05:26:37 -03:00
{
2012-08-17 03:20:26 -03:00
// ensure valid motor number is provided
if ( motor_num > = 0 & & motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
// increment number of motors if this motor is being newly motor_enabled
if ( ! motor_enabled [ motor_num ] ) {
motor_enabled [ motor_num ] = true ;
}
// set roll, pitch, thottle factors and opposite motor (for stability patch)
_roll_factor [ motor_num ] = roll_fac ;
_pitch_factor [ motor_num ] = pitch_fac ;
_yaw_factor [ motor_num ] = yaw_fac ;
// set order that motor appears in test
2013-05-14 06:03:34 -03:00
_test_order [ motor_num ] = testing_order ;
2014-02-07 09:03:34 -04:00
2016-01-04 02:48:55 -04:00
uint8_t chan ;
if ( RC_Channel_aux : : find_channel ( ( RC_Channel_aux : : Aux_servo_function_t ) ( RC_Channel_aux : : k_motor1 + motor_num ) ,
chan ) ) {
_motor_map [ motor_num ] = chan ;
_motor_map_mask | = 1U < < motor_num ;
} else {
// disable this channel from being used by RC_Channel_aux
RC_Channel_aux : : disable_aux_channel ( motor_num ) ;
}
2012-08-17 03:20:26 -03:00
}
2012-04-02 05:26:37 -03:00
}
2014-08-18 02:25:24 -03:00
// add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal
2013-05-14 06:03:34 -03:00
void AP_MotorsMatrix : : add_motor ( int8_t motor_num , float angle_degrees , float yaw_factor , uint8_t testing_order )
2012-04-02 05:26:37 -03:00
{
2014-08-18 02:25:24 -03:00
add_motor ( motor_num , angle_degrees , angle_degrees , yaw_factor , testing_order ) ;
}
// add_motor using position and prop direction. Roll and Pitch factors can differ (for asymmetrical frames)
void AP_MotorsMatrix : : add_motor ( int8_t motor_num , float roll_factor_in_degrees , float pitch_factor_in_degrees , float yaw_factor , uint8_t testing_order )
{
2012-08-17 03:20:26 -03:00
add_motor_raw (
motor_num ,
2014-08-18 02:25:24 -03:00
cosf ( radians ( roll_factor_in_degrees + 90 ) ) ,
cosf ( radians ( pitch_factor_in_degrees ) ) ,
yaw_factor ,
2012-08-17 03:20:26 -03:00
testing_order ) ;
2012-04-02 05:26:37 -03:00
}
// remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor
void AP_MotorsMatrix : : remove_motor ( int8_t motor_num )
{
2012-08-17 03:20:26 -03:00
// ensure valid motor number is provided
if ( motor_num > = 0 & & motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
// disable the motor, set all factors to zero
motor_enabled [ motor_num ] = false ;
_roll_factor [ motor_num ] = 0 ;
_pitch_factor [ motor_num ] = 0 ;
_yaw_factor [ motor_num ] = 0 ;
}
2012-04-02 05:26:37 -03:00
}
// remove_all_motors - removes all motor definitions
void AP_MotorsMatrix : : remove_all_motors ( )
{
2012-08-17 03:20:26 -03:00
for ( int8_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
remove_motor ( i ) ;
}
2012-10-26 20:59:07 -03:00
}
2016-02-04 07:45:49 -04:00
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
void AP_MotorsMatrix : : normalise_rpy_factors ( )
{
float roll_fac = 0.0f ;
float pitch_fac = 0.0f ;
float yaw_fac = 0.0f ;
// find maximum roll, pitch and yaw factors
for ( uint8_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
if ( roll_fac < fabsf ( _roll_factor [ i ] ) ) {
roll_fac = fabsf ( _roll_factor [ i ] ) ;
}
if ( pitch_fac < fabsf ( _pitch_factor [ i ] ) ) {
pitch_fac = fabsf ( _pitch_factor [ i ] ) ;
}
if ( yaw_fac < fabsf ( _yaw_factor [ i ] ) ) {
yaw_fac = fabsf ( _yaw_factor [ i ] ) ;
}
}
}
// scale factors back to -0.5 to +0.5 for each axis
for ( uint8_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
if ( ! is_zero ( roll_fac ) ) {
_roll_factor [ i ] = 0.5f * _roll_factor [ i ] / roll_fac ;
}
if ( ! is_zero ( pitch_fac ) ) {
_pitch_factor [ i ] = 0.5f * _pitch_factor [ i ] / pitch_fac ;
}
if ( ! is_zero ( yaw_fac ) ) {
_yaw_factor [ i ] = 0.5f * _yaw_factor [ i ] / yaw_fac ;
}
}
}
}