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 )
{
2012-08-17 03:20:26 -03:00
int8_t i ;
// 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 ] ) {
2014-02-10 02:59:58 -04:00
mask | = 1U < < pgm_read_byte ( & _motor_to_channel_map [ i ] ) ;
2012-08-17 03:20:26 -03:00
}
}
2013-01-10 00:52:46 -04:00
hal . rcout - > 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 ] ) {
2014-02-10 02:59:58 -04:00
hal . rcout - > enable_ch ( pgm_read_byte ( & _motor_to_channel_map [ 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 ;
2013-07-25 12:45:59 -03:00
// set limits flags
limit . roll_pitch = true ;
limit . yaw = true ;
limit . throttle_lower = true ;
limit . throttle_upper = false ;
2012-08-17 03:20:26 -03:00
// fill the motor_out[] array for HIL use and send minimum value to each motor
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2015-05-14 22:00:46 -03:00
hal . rcout - > write ( pgm_read_byte ( & _motor_to_channel_map [ i ] ) , _throttle_radio_min ) ;
2012-08-17 03:20:26 -03:00
}
}
2012-04-02 05:26:37 -03:00
}
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 ;
}
}
return mask ;
}
2015-04-02 17:54:15 -03:00
void AP_MotorsMatrix : : output_armed_not_stabilizing ( )
{
int8_t i ;
2015-05-14 22:00:46 -03:00
int16_t throttle_radio_output ; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t motor_out [ AP_MOTORS_MAX_NUM_MOTORS ] ; // final outputs sent to the motors
int16_t out_min_pwm = _throttle_radio_min + _min_throttle ; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _throttle_radio_max ; // maximum pwm value we can send to the motors
2015-04-02 17:54:15 -03:00
// initialize limits flags
limit . roll_pitch = true ;
limit . yaw = true ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
2015-07-20 05:05:06 -03:00
int16_t thr_in_min = rel_pwm_to_thr_range ( _spin_when_armed_ramped ) ;
if ( _throttle_control_input < = thr_in_min ) {
_throttle_control_input = thr_in_min ;
2015-04-02 17:54:15 -03:00
limit . throttle_lower = true ;
}
2015-05-14 22:00:46 -03:00
if ( _throttle_control_input > = _hover_out ) {
_throttle_control_input = _hover_out ;
2015-04-02 17:54:15 -03:00
limit . throttle_upper = true ;
}
2015-05-14 22:00:46 -03:00
throttle_radio_output = calc_throttle_radio_output ( ) ;
2015-04-02 17:54:15 -03:00
// set output throttle
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2015-05-14 22:00:46 -03:00
motor_out [ i ] = throttle_radio_output ;
2015-04-02 17:54:15 -03:00
}
}
2015-05-14 22:00:46 -03:00
if ( throttle_radio_output > = out_min_pwm ) {
2015-04-02 17:54:15 -03:00
// apply thrust curve and voltage scaling
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
motor_out [ i ] = apply_thrust_curve_and_volt_scaling ( motor_out [ i ] , out_min_pwm , out_max_pwm ) ;
}
}
}
// send output to each motor
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
hal . rcout - > write ( pgm_read_byte ( & _motor_to_channel_map [ i ] ) , motor_out [ i ] ) ;
}
}
}
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
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
void AP_MotorsMatrix : : output_armed_stabilizing ( )
2013-05-26 23:21:31 -03:00
{
int8_t i ;
2015-05-14 22:00:46 -03:00
int16_t roll_pwm ; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t pitch_pwm ; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t yaw_pwm ; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400
int16_t throttle_radio_output ; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min_pwm = _throttle_radio_min + _min_throttle ; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _throttle_radio_max ; // maximum pwm value we can send to the motors
int16_t out_mid_pwm = ( out_min_pwm + out_max_pwm ) / 2 ; // mid pwm value we can send to the motors
int16_t out_best_thr_pwm ; // the is the best throttle we can come up which provides good control without climbing
float rpy_scale = 1.0 ; // this is used to scale the roll, pitch and yaw to fit within the motor limits
2013-05-26 23:21:31 -03:00
int16_t rpy_out [ AP_MOTORS_MAX_NUM_MOTORS ] ; // buffer so we don't have to multiply coefficients multiple times.
2014-02-09 04:51:21 -04:00
int16_t motor_out [ AP_MOTORS_MAX_NUM_MOTORS ] ; // final outputs sent to the motors
2013-05-26 23:21:31 -03:00
int16_t rpy_low = 0 ; // lowest motor value
int16_t rpy_high = 0 ; // highest motor value
int16_t yaw_allowed ; // amount of yaw we can fit in
2013-07-25 12:45:59 -03:00
int16_t thr_adj ; // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided)
2013-05-26 23:21:31 -03:00
2015-04-02 17:54:15 -03:00
// initialize limits flags
2013-07-21 01:58:24 -03:00
limit . roll_pitch = false ;
limit . yaw = false ;
2013-07-25 12:45:59 -03:00
limit . throttle_lower = false ;
limit . throttle_upper = false ;
2013-05-26 23:21:31 -03:00
2015-05-14 22:00:46 -03:00
// Ensure throttle is within bounds of 0 to 1000
2015-07-20 05:05:06 -03:00
int16_t thr_in_min = rel_pwm_to_thr_range ( _min_throttle ) ;
if ( _throttle_control_input < = thr_in_min ) {
_throttle_control_input = thr_in_min ;
2013-09-12 10:27:44 -03:00
limit . throttle_lower = true ;
}
2015-05-14 22:00:46 -03:00
if ( _throttle_control_input > = _max_throttle ) {
_throttle_control_input = _max_throttle ;
2013-09-12 10:27:44 -03:00
limit . throttle_upper = true ;
}
2013-05-26 23:21:31 -03:00
2015-05-14 22:00:46 -03:00
roll_pwm = calc_roll_pwm ( ) ;
pitch_pwm = calc_pitch_pwm ( ) ;
yaw_pwm = calc_yaw_pwm ( ) ;
throttle_radio_output = calc_throttle_radio_output ( ) ;
2013-05-26 23:21:31 -03:00
2015-04-02 17:54:15 -03:00
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2015-05-14 22:00:46 -03:00
rpy_out [ i ] = roll_pwm * _roll_factor [ i ] * get_compensation_gain ( ) +
pitch_pwm * _pitch_factor [ i ] * get_compensation_gain ( ) ;
2013-05-26 23:21:31 -03:00
2015-04-02 17:54:15 -03:00
// record lowest roll pitch command
if ( rpy_out [ i ] < rpy_low ) {
rpy_low = rpy_out [ i ] ;
}
// record highest roll pich command
if ( rpy_out [ i ] > rpy_high ) {
rpy_high = rpy_out [ i ] ;
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
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. mid throttle - average of highest and lowest motor (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 mid point 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 favour 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 favour reducing throttle instead of better yaw control because the pilot has commanded it
int16_t motor_mid = ( rpy_low + rpy_high ) / 2 ;
2015-05-14 22:00:46 -03:00
out_best_thr_pwm = min ( out_mid_pwm - motor_mid , max ( throttle_radio_output , throttle_radio_output * max ( 0 , 1.0f - _throttle_thr_mix ) + get_hover_throttle_as_pwm ( ) * _throttle_thr_mix ) ) ;
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
yaw_allowed = min ( out_max_pwm - out_best_thr_pwm , out_best_thr_pwm - out_min_pwm ) - ( rpy_high - rpy_low ) / 2 ;
yaw_allowed = max ( yaw_allowed , _yaw_headroom ) ;
2015-05-14 22:00:46 -03:00
if ( yaw_pwm > = 0 ) {
2015-04-02 17:54:15 -03:00
// if yawing right
2015-05-14 22:00:46 -03:00
if ( yaw_allowed > yaw_pwm * get_compensation_gain ( ) ) {
yaw_allowed = yaw_pwm * get_compensation_gain ( ) ; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
2013-05-26 23:21:31 -03:00
} else {
2015-04-02 17:54:15 -03:00
limit . yaw = true ;
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
} else {
// if yawing left
yaw_allowed = - yaw_allowed ;
2015-05-14 22:00:46 -03:00
if ( yaw_allowed < yaw_pwm * get_compensation_gain ( ) ) {
yaw_allowed = yaw_pwm * get_compensation_gain ( ) ; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
2015-04-02 17:54:15 -03:00
} else {
limit . yaw = 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-04-02 17:54:15 -03:00
// add yaw to intermediate numbers for each motor
rpy_low = 0 ;
rpy_high = 0 ;
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
rpy_out [ i ] = rpy_out [ i ] +
yaw_allowed * _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
if ( rpy_out [ i ] < rpy_low ) {
rpy_low = rpy_out [ i ] ;
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
// record highest roll+pitch+yaw command
if ( rpy_out [ i ] > rpy_high ) {
rpy_high = rpy_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-05-14 22:00:46 -03:00
thr_adj = throttle_radio_output - out_best_thr_pwm ;
2015-04-02 17:54:15 -03:00
// calculate upper and lower limits of thr_adj
int16_t thr_adj_max = max ( out_max_pwm - ( out_best_thr_pwm + rpy_high ) , 0 ) ;
// if we are increasing the throttle (situation #2 above)..
if ( thr_adj > 0 ) {
// increase throttle as close as possible to requested throttle
// without going over out_max_pwm
if ( thr_adj > thr_adj_max ) {
thr_adj = thr_adj_max ;
// we haven't even been able to apply full throttle command
limit . throttle_upper = true ;
2013-05-26 23:21:31 -03:00
}
2015-04-02 17:54:15 -03:00
} else if ( thr_adj < 0 ) {
// decrease throttle as close as possible to requested throttle
// without going under out_min_pwm or over out_max_pwm
// earlier code ensures we can't break both boundaries
int16_t thr_adj_min = min ( out_min_pwm - ( out_best_thr_pwm + rpy_low ) , 0 ) ;
if ( thr_adj > thr_adj_max ) {
thr_adj = thr_adj_max ;
limit . throttle_upper = true ;
}
if ( thr_adj < thr_adj_min ) {
thr_adj = thr_adj_min ;
}
}
2013-05-26 23:21:31 -03:00
2015-04-02 17:54:15 -03:00
// do we need to reduce roll, pitch, yaw command
// earlier code does not allow both limit's to be passed simultaneously with abs(_yaw_factor)<1
if ( ( rpy_low + out_best_thr_pwm ) + thr_adj < out_min_pwm ) {
2015-07-20 08:25:39 -03:00
// protect against divide by zero
if ( rpy_low ! = 0 ) {
rpy_scale = ( float ) ( out_min_pwm - thr_adj - out_best_thr_pwm ) / rpy_low ;
}
2015-04-02 17:54:15 -03:00
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit . roll_pitch = true ;
limit . yaw = true ;
} else if ( ( rpy_high + out_best_thr_pwm ) + thr_adj > out_max_pwm ) {
2015-07-20 08:25:39 -03:00
// protect against divide by zero
if ( rpy_high ! = 0 ) {
rpy_scale = ( float ) ( out_max_pwm - thr_adj - out_best_thr_pwm ) / rpy_high ;
}
2015-04-02 17:54:15 -03:00
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit . roll_pitch = true ;
limit . yaw = true ;
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
motor_out [ i ] = out_best_thr_pwm + thr_adj +
rpy_scale * rpy_out [ i ] ;
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
// apply thrust curve and voltage scaling
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
motor_out [ i ] = apply_thrust_curve_and_volt_scaling ( motor_out [ i ] , out_min_pwm , out_max_pwm ) ;
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-04-02 17:54:15 -03:00
// clip motor output if required (shouldn't be)
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
motor_out [ i ] = constrain_int16 ( motor_out [ i ] , out_min_pwm , out_max_pwm ) ;
2013-05-26 23:21:31 -03:00
}
}
// send output to each motor
for ( i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
2014-02-10 02:59:58 -04:00
hal . rcout - > write ( pgm_read_byte ( & _motor_to_channel_map [ i ] ) , motor_out [ i ] ) ;
2013-05-26 23:21:31 -03:00
}
}
}
2012-04-02 05:26:37 -03:00
// output_disarmed - sends commands to the motors
void AP_MotorsMatrix : : output_disarmed ( )
{
2012-08-17 03:20:26 -03:00
// Send minimum values to all motors
output_min ( ) ;
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
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
hal . rcout - > write ( pgm_read_byte ( & _motor_to_channel_map [ i ] ) , pwm ) ;
2012-08-17 03:20:26 -03:00
}
}
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
// disable this channel from being used by RC_Channel_aux
RC_Channel_aux : : disable_aux_channel ( _motor_to_channel_map [ 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
}