/*
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 .
*/
/*
* AP_MotorsMatrix.cpp - ArduCopter motors library
* Code by RandyMackay. DIYDrones.com
*
*/
#include
#include "AP_MotorsMatrix.h"
extern const AP_HAL::HAL& hal;
// init
void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// record requested frame class and type
_last_frame_class = frame_class;
_last_frame_type = frame_type;
// setup the motors
setup_motors(frame_class, frame_type);
// enable fast channels or instant pwm
set_update_rate(_speed_hz);
}
// set update rate to motors - a value in hertz
void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz;
uint16_t mask = 0;
for (uint8_t i=0; i= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_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
// 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
throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
// calculate roll and pitch for each motor
// calculate the amount of yaw input that each motor can accept
for (i=0; i 0.0f) {
unused_range = fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
} else {
unused_range = fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
}
}
}
}
// 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;
}
// add yaw to intermediate numbers for each motor
rpy_low = 0.0f;
rpy_high = 0.0f;
for (i=0; i rpy_high) {
rpy_high = _thrust_rpyt_out[i];
}
}
}
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_avg_max);
if (is_zero(rpy_low) && is_zero(rpy_high)){
rpy_scale = 1.0f;
} else if (is_zero(rpy_low)) {
rpy_scale = constrain_float((1.0f-throttle_thrust_best_rpy)/rpy_high, 0.0f, 1.0f);
} else if (is_zero(rpy_high)) {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
} else {
rpy_scale = constrain_float(MIN(-throttle_thrust_best_rpy/rpy_low, (1.0f-throttle_thrust_best_rpy)/rpy_high), 0.0f, 1.0f);
}
// 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.
limit.roll_pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
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;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i= 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
_test_order[motor_num] = testing_order;
// call parent class method
add_motor_num(motor_num);
}
}
// 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);
}
// remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor
void AP_MotorsMatrix::remove_motor(int8_t motor_num)
{
// 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;
}
}
void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{
// remove existing motors
for (int8_t i=0; i