mirror of https://github.com/ArduPilot/ardupilot
430 lines
16 KiB
C++
430 lines
16 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
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/>.
|
|
*/
|
|
|
|
/*
|
|
* AP_MotorsMatrix.cpp - ArduCopter motors library
|
|
* Code by RandyMackay. DIYDrones.com
|
|
*
|
|
*/
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_MotorsMatrix.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
// Init
|
|
void AP_MotorsMatrix::Init()
|
|
{
|
|
// setup the motors
|
|
setup_motors();
|
|
|
|
// 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 )
|
|
{
|
|
uint8_t i;
|
|
|
|
// record requested speed
|
|
_speed_hz = speed_hz;
|
|
|
|
// check each enabled motor
|
|
uint32_t mask = 0;
|
|
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
|
|
if( motor_enabled[i] ) {
|
|
mask |= 1U << i;
|
|
}
|
|
}
|
|
rc_set_freq( mask, _speed_hz );
|
|
}
|
|
|
|
// set frame orientation (normally + or X)
|
|
void AP_MotorsMatrix::set_frame_orientation( uint8_t new_orientation )
|
|
{
|
|
// return if nothing has changed
|
|
if( new_orientation == _flags.frame_orientation ) {
|
|
return;
|
|
}
|
|
|
|
// call parent
|
|
AP_Motors::set_frame_orientation( new_orientation );
|
|
|
|
// setup the motors
|
|
setup_motors();
|
|
|
|
// enable fast channels or instant pwm
|
|
set_update_rate(_speed_hz);
|
|
}
|
|
|
|
// enable - starts allowing signals to be sent to motors
|
|
void AP_MotorsMatrix::enable()
|
|
{
|
|
int8_t i;
|
|
|
|
// enable output channels
|
|
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
|
|
if( motor_enabled[i] ) {
|
|
rc_enable_ch(i);
|
|
}
|
|
}
|
|
}
|
|
|
|
// output_min - sends minimum values out to the motors
|
|
void AP_MotorsMatrix::output_min()
|
|
{
|
|
int8_t i;
|
|
|
|
// 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, _throttle_radio_min);
|
|
}
|
|
}
|
|
hal.rcout->push();
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
// 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 rc_map_mask(mask);
|
|
}
|
|
|
|
// 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_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
|
|
roll_thrust = _roll_in * get_compensation_gain();
|
|
pitch_thrust = _pitch_in * get_compensation_gain();
|
|
yaw_thrust = _yaw_in * get_compensation_gain();
|
|
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;
|
|
limit.throttle_lower = true;
|
|
}
|
|
if (throttle_thrust >= _throttle_thrust_max) {
|
|
throttle_thrust = _throttle_thrust_max;
|
|
limit.throttle_upper = true;
|
|
}
|
|
|
|
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
|
|
|
|
// 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_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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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<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
|
|
if (_thrust_rpyt_out[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_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);
|
|
}
|
|
|
|
// 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<AP_MOTORS_MAX_NUM_MOTORS; i++) {
|
|
if (motor_enabled[i]) {
|
|
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + rpy_scale*_thrust_rpyt_out[i];
|
|
}
|
|
}
|
|
|
|
// 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
|
|
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
|
|
if (motor_enabled[i]) {
|
|
_thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i], 0.0f, 1.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
// exit immediately if not armed
|
|
if (!armed()) {
|
|
return;
|
|
}
|
|
|
|
// loop through all the possible orders spinning any motors that match that description
|
|
hal.rcout->cork();
|
|
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
|
|
rc_write(i, pwm);
|
|
}
|
|
}
|
|
hal.rcout->push();
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
// 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
|
|
_test_order[motor_num] = testing_order;
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
// remove_all_motors - removes all motor definitions
|
|
void AP_MotorsMatrix::remove_all_motors()
|
|
{
|
|
for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
|
|
remove_motor(i);
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
}
|
|
}
|