# Individual Motor Outputs
###### tags: `research`
- [Copter Motors Library](https://ardupilot.org/dev/docs/code-overview-copter-motors-library.html)
## Inputs to the Motors Library
> 擷取至: [AC_AttitudeControl_Multi.cpp](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp#L329)
```cpp
void AC_AttitudeControl_Multi::rate_controller_run()
{
...
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
...
}
```
> 擷取至: [AP_Motors_Class.h](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Motors/AP_Motors_Class.h#L101)
```cpp
// set_roll, set_pitch, set_yaw, set_throttle
void set_roll(float roll_in) { _roll_in = roll_in; }; // range -1 ~ +1
void set_roll_ff(float roll_in) { _roll_in_ff = roll_in; }; // range -1 ~ +1
void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1
void set_pitch_ff(float pitch_in) { _pitch_in_ff = pitch_in; }; // range -1 ~ +1
void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1
void set_yaw_ff(float yaw_in) { _yaw_in_ff = yaw_in; }; // range -1 ~ +1
```
<details>
<summary>_motors的定義 (點我展開)</summary>
> 擷取至: [system.cpp](https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/system.cpp#L432)
```cpp
void Copter::allocate_motors(void)
{
switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
#if FRAME_CONFIG != HELI_FRAME
case AP_Motors::MOTOR_FRAME_QUAD:
case AP_Motors::MOTOR_FRAME_HEXA:
case AP_Motors::MOTOR_FRAME_Y6:
case AP_Motors::MOTOR_FRAME_OCTA:
case AP_Motors::MOTOR_FRAME_OCTAQUAD:
case AP_Motors::MOTOR_FRAME_DODECAHEXA:
case AP_Motors::MOTOR_FRAME_DECA:
case AP_Motors::MOTOR_FRAME_SCRIPTING_MATRIX:
default:
motors = new AP_MotorsMatrix(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsMatrix::var_info;
break;
case AP_Motors::MOTOR_FRAME_TRI:
motors = new AP_MotorsTri(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsTri::var_info;
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_TRICOPTER);
break;
case AP_Motors::MOTOR_FRAME_SINGLE:
motors = new AP_MotorsSingle(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsSingle::var_info;
break;
case AP_Motors::MOTOR_FRAME_COAX:
motors = new AP_MotorsCoax(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsCoax::var_info;
break;
case AP_Motors::MOTOR_FRAME_TAILSITTER:
motors = new AP_MotorsTailsitter(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsTailsitter::var_info;
break;
case AP_Motors::MOTOR_FRAME_6DOF_SCRIPTING:
#ifdef ENABLE_SCRIPTING
motors = new AP_MotorsMatrix_6DoF_Scripting(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsMatrix_6DoF_Scripting::var_info;
#endif // ENABLE_SCRIPTING
break;
case AP_Motors::MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX:
#ifdef ENABLE_SCRIPTING
motors = new AP_MotorsMatrix_Scripting_Dynamic(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsMatrix_Scripting_Dynamic::var_info;
#endif // ENABLE_SCRIPTING
break;
#else // FRAME_CONFIG == HELI_FRAME
case AP_Motors::MOTOR_FRAME_HELI_DUAL:
motors = new AP_MotorsHeli_Dual(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsHeli_Dual::var_info;
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
break;
case AP_Motors::MOTOR_FRAME_HELI_QUAD:
motors = new AP_MotorsHeli_Quad(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsHeli_Quad::var_info;
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
break;
case AP_Motors::MOTOR_FRAME_HELI:
default:
motors = new AP_MotorsHeli_Single(copter.scheduler.get_loop_rate_hz());
motors_var_info = AP_MotorsHeli_Single::var_info;
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
break;
#endif
}
...
#if FRAME_CONFIG != HELI_FRAME
if ((AP_Motors::motor_frame_class)g2.frame_class.get() == AP_Motors::MOTOR_FRAME_6DOF_SCRIPTING) {
#ifdef ENABLE_SCRIPTING
attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
ac_var_info = AC_AttitudeControl_Multi_6DoF::var_info;
#endif // ENABLE_SCRIPTING
} else {
attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
ac_var_info = AC_AttitudeControl_Multi::var_info;
}
#else
attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
ac_var_info = AC_AttitudeControl_Heli::var_info;
#endif
```
> 擷取至: [AC_AttitudeControl.h](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AC_AttitudeControl/AC_AttitudeControl.h#L48)
```cpp
AC_AttitudeControl( AP_AHRS_View &ahrs,
const AP_Vehicle::MultiCopter &aparm,
AP_Motors& motors,
float dt) :
...
_motors(motors)
{
AP_Param::setup_object_defaults(this, var_info);
}
```
</details>
## motor mixing table
> 擷取至: [AP_MotorsMatrix.cpp](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Motors/AP_MotorsMatrix.cpp#L563)
```cpp
void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{
...
switch (frame_class) {
case MOTOR_FRAME_QUAD:
...
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
_frame_type_string = "PLUS";
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_X:
_frame_type_string = "X";
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
...
```
計算方式:
```cpp
row_factor = cosf(radians(degree + 90))
pitch_factor = cosf(radians(degree))
if (CW)
yaw_factor = -1
else
yaw_factor = 1
throttle_factor = 1
```
### MOTOR_FRAME_TYPE_PLUS
| Motor | throttle factor | pitch factor | roll factor | yaw factor |
| :---: | :-------------: | :----------: | :---------: | :--------: |
| 1 | 1 | 0 | -1 | 1 |
| 2 | 1 | 0 | 1 | 1 |
| 3 | 1 | 1 | 0 | -1 |
| 4 | 1 | -1 | 0 | -1 |
### MOTOR_FRAME_TYPE_X
| Motor | throttle factor | pitch factor | roll factor | yaw factor |
| :---: | :-------------: | :-------------------: | :-------------------: | :--------: |
| 1 | 1 | $\frac{\sqrt{2}}{2}$ | $-\frac{\sqrt{2}}{2}$ | 1 |
| 2 | 1 | $-\frac{\sqrt{2}}{2}$ | $\frac{\sqrt{2}}{2}$ | 1 |
| 3 | 1 | $\frac{\sqrt{2}}{2}$ | $\frac{\sqrt{2}}{2}$ | -1 |
| 4 | 1 | $-\frac{\sqrt{2}}{2}$ | $-\frac{\sqrt{2}}{2}$ | -1 |
<details>
<summary>add_motor 儲存 roll, pitch, yaw factors 的過程</summary>
> 擷取至: [AP_MotorsMatrix.cpp](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Motors/AP_MotorsMatrix.cpp#L504)
```cpp
// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor)
{
if (initialised_ok()) {
// do not allow motors to be set if the current frame type has init correctly
return;
}
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
// enable motor
motor_enabled[motor_num] = true;
// set roll, pitch, yaw and throttle factors
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
_throttle_factor[motor_num] = throttle_factor;
// set order that motor appears in test
_test_order[motor_num] = testing_order;
// call parent class method
add_motor_num(motor_num);
}
}
```
> 擷取至: [AP_MotorsMatrix.cpp](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Motors/AP_MotorsMatrix.cpp#L532)
```cpp
// add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
{
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)
{
add_motor_raw(
motor_num,
cosf(radians(roll_factor_in_degrees + 90)),
cosf(radians(pitch_factor_in_degrees)),
yaw_factor,
testing_order);
}
```
> 擷取至: [AP_MotorsMatrix.cpp](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Motors/AP_MotorsMatrix.h#L10)
```cpp
#define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1
#define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1
```
</details>
## roll, pitch and yaw rate to motor commands
> 擷取至: [AP_MotorsMatrix.cpp](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Motors/AP_MotorsMatrix.h#L10)
```cpp
void AP_MotorsMatrix::output_armed_stabilizing()
{
...
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];
...
}
}
...
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
...
}
}
...
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * _throttle_factor[i]) + (rpy_scale * _thrust_rpyt_out[i]);
}
}
...
}
```
<details>
<summary>完整程式碼</summary>
```cpp
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed_stabilizing()
{
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_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float throttle_thrust_max; // throttle thrust maximum value, 0.0 - 1.0
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 yaw_allowed = 1.0f; // amount of yaw we can fit in
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
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;
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;
// If thrust boost is active then do not limit maximum thrust
throttle_thrust_max = _thrust_boost_ratio + (1.0f - _thrust_boost_ratio) * _throttle_thrust_max * compensation_gain;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= throttle_thrust_max) {
throttle_thrust = throttle_thrust_max;
limit.throttle_upper = true;
}
// ensure that throttle_avg_max is between the input throttle and the maximum throttle
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);
// calculate the highest allowed average thrust that will provide maximum control range
throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
// 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
// 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
// Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
// To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
// Y6 : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75, ATC_RAT_PIT_IMAX = 0.75, ATC_RAT_YAW_IMAX = 0.375
// Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
// Hex : MOT_YAW_HEADROOM = 0, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25, ATC_RAT_PIT_IMAX = 0.25, ATC_RAT_YAW_IMAX = 0.25
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5, ATC_RAT_PIT_IMAX = 0.5, ATC_RAT_YAW_IMAX = 0.25
// Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.
// 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
float rp_low = 1.0f; // lowest thrust value
float rp_high = -1.0f; // highest thrust value
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
// calculate the thrust outputs for roll and pitch
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
// record lowest roll + pitch command
if (_thrust_rpyt_out[i] < rp_low) {
rp_low = _thrust_rpyt_out[i];
}
// record highest roll + pitch command
if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) {
rp_high = _thrust_rpyt_out[i];
}
// Check the maximum yaw control that can be used on this channel
// Exclude any lost motors if thrust boost is enabled
if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)){
if (is_positive(yaw_thrust * _yaw_factor[i])) {
yaw_allowed = MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]));
} else {
yaw_allowed = MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]));
}
}
}
}
// calculate the maximum yaw control that can be used
// todo: make _yaw_headroom 0 to 1
float yaw_allowed_min = (float)_yaw_headroom / 1000.0f;
// increase yaw headroom to 50% if thrust boost enabled
yaw_allowed_min = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed_min;
// Let yaw access minimum amount of head room
yaw_allowed = MAX(yaw_allowed, yaw_allowed_min);
// Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
if (_thrust_boost && motor_enabled[_motor_lost_index]) {
// record highest roll + pitch command
if (_thrust_rpyt_out[_motor_lost_index] > rp_high) {
rp_high = _thrust_boost_ratio * rp_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
}
// Check the maximum yaw control that can be used on this channel
// Exclude any lost motors if thrust boost is enabled
if (!is_zero(_yaw_factor[_motor_lost_index])){
if (is_positive(yaw_thrust * _yaw_factor[_motor_lost_index])) {
yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index]), 0.0f)/_yaw_factor[_motor_lost_index]));
} else {
yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index], 0.0f)/_yaw_factor[_motor_lost_index]));
}
}
}
if (fabsf(yaw_thrust) > yaw_allowed) {
// not all commanded yaw can be used
yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
limit.yaw = true;
}
// add yaw control to thrust outputs
float rpy_low = 1.0f; // lowest thrust value
float rpy_high = -1.0f; // highest thrust value
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
// record lowest roll + pitch + yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll + pitch + yaw command
// Exclude any lost motors if thrust boost is enabled
if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) {
rpy_high = _thrust_rpyt_out[i];
}
}
}
// Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
if (_thrust_boost) {
// record highest roll + pitch + yaw command
if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) {
rpy_high = _thrust_boost_ratio * rpy_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
}
}
// calculate any scaling needed to make the combined thrust outputs fit within the output range
if (rpy_high - rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high - rpy_low);
}
if (throttle_avg_max + rpy_low < 0) {
rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
}
// calculate how close the motors can come to the desired throttle
rpy_high *= rpy_scale;
rpy_low *= rpy_scale;
throttle_thrust_best_rpy = -rpy_low;
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f) {
// Full range is being used by roll, pitch, and yaw.
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
thr_adj = 0.0f;
} else {
if (thr_adj < 0.0f) {
// Throttle can't be reduced to desired value
// todo: add lower limit flag and ensure it is handled correctly in altitude controller
thr_adj = 0.0f;
} 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;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * _throttle_factor[i]) + (rpy_scale * _thrust_rpyt_out[i]);
}
}
// determine throttle thrust for harmonic notch
// compensation_gain can never be zero
_throttle_out = throttle_thrust_best_plus_adj / compensation_gain;
// check for failed motor
check_for_failed_motor(throttle_thrust_best_plus_adj);
}
```
</details>
## output signals
## Reference
- http://autoquad.org/wiki/wiki/configuring-autoquad-flightcontroller/frame-motor-mixing-table/
Google Drive
Gist
Clipboard
Sign in with Wallet
