Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: New AP_Motors6DOF used by Sub

Browse Source
This commit is contained in:
Rustom Jehangir 2016-01-01 14:20:32 -08:00 committed by Andrew Tridgell
parent 3da7c95e9b
commit 237ba87ca4
5 changed files with 551 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Motors/AP_Motors.h
View File

@ -8,3 +8,4 @@
#include "AP_MotorsSingle.h"
#include "AP_MotorsCoax.h"
#include "AP_MotorsTailsitter.h"
#include "AP_Motors6DOF.h"

479
libraries/AP_Motors/AP_Motors6DOF.cpp Normal file
View File

@ -0,0 +1,479 @@
/*
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_Motors6DOF.cpp - ArduSub motors library
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_Motors6DOF.h"
extern const AP_HAL::HAL& hal;
// parameters for the motor class
const AP_Param::GroupInfo AP_Motors6DOF::var_info[] = {
AP_NESTEDGROUPINFO(AP_MotorsMulticopter, 0),
// @Param: 1_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("1_DIRECTION", 1, AP_Motors6DOF, _motor_reverse[0], 1),
// @Param: 2_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("2_DIRECTION", 2, AP_Motors6DOF, _motor_reverse[1], 1),
// @Param: 3_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("3_DIRECTION", 3, AP_Motors6DOF, _motor_reverse[2], 1),
// @Param: 4_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("4_DIRECTION", 4, AP_Motors6DOF, _motor_reverse[3], 1),
// @Param: 5_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("5_DIRECTION", 5, AP_Motors6DOF, _motor_reverse[4], 1),
// @Param: 6_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("6_DIRECTION", 6, AP_Motors6DOF, _motor_reverse[5], 1),
// @Param: 7_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("7_DIRECTION", 7, AP_Motors6DOF, _motor_reverse[6], 1),
// @Param: 8_DIRECTION
// @DisplayName: Motor normal or reverse
// @Description: Used to change motor rotation directions without changing wires
// @Values: 1:normal,-1:reverse
// @User: Standard
AP_GROUPINFO("8_DIRECTION", 8, AP_Motors6DOF, _motor_reverse[7], 1),
// @Param: FV_CPLNG_K
// @DisplayName: Forward/vertical to pitch decoupling factor
// @Description: Used to decouple pitch from forward/vertical motion. 0 to disable, 1.2 normal
// @Range: 0.0 1.5
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("FV_CPLNG_K", 9, AP_Motors6DOF, _forwardVerticalCouplingFactor, 1.0),
AP_GROUPEND
};
void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{
// remove existing motors
for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
remove_motor(i);
}
// hard coded config for supported frames
switch ((sub_frame_t)frame_class) {
// Motor # Roll Factor Pitch Factor Yaw Factor Throttle Factor Forward Factor Lateral Factor Testing Order
case SUB_FRAME_BLUEROV1:
add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, -1.0f, 0, 1.0f, 0, 1);
add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, 1.0f, 0, 1.0f, 0, 2);
add_motor_raw_6dof(AP_MOTORS_MOT_3, -0.5f, 0.5f, 0, 0.45f, 0, 0, 3);
add_motor_raw_6dof(AP_MOTORS_MOT_4, 0.5f, 0.5f, 0, 0.45f, 0, 0, 4);
add_motor_raw_6dof(AP_MOTORS_MOT_5, 0, -1.0f, 0, 1.0f, 0, 0, 5);
add_motor_raw_6dof(AP_MOTORS_MOT_6, -0.25f, 0, 0, 0, 0, 1.0f, 6);
break;
case SUB_FRAME_VECTORED_6DOF_90DEG:
add_motor_raw_6dof(AP_MOTORS_MOT_1, 1.0f, 1.0f, 0, 1.0f, 0, 0, 1);
add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, 1.0f, 0, 1.0f, 0, 2);
add_motor_raw_6dof(AP_MOTORS_MOT_3, 1.0f, -1.0f, 0, 1.0f, 0, 0, 3);
add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 0, 0, 0, 1.0f, 4);
add_motor_raw_6dof(AP_MOTORS_MOT_5, 0, 0, 0, 0, 0, 1.0f, 5);
add_motor_raw_6dof(AP_MOTORS_MOT_6, -1.0f, 1.0f, 0, 1.0f, 0, 0, 6);
add_motor_raw_6dof(AP_MOTORS_MOT_7, 0, 0, -1.0f, 0, 1.0f, 0, 7);
add_motor_raw_6dof(AP_MOTORS_MOT_8, -1.0f, -1.0f, 0, 1.0f, 0, 0, 8);
break;
case SUB_FRAME_VECTORED_6DOF:
add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, 1.0f, 0, -1.0f, 1.0f, 1);
add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, -1.0f, 0, -1.0f, -1.0f, 2);
add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, -1.0f, 0, 1.0f, 1.0f, 3);
add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 1.0f, 0, 1.0f, -1.0f, 4);
add_motor_raw_6dof(AP_MOTORS_MOT_5, 1.0f, -1.0f, 0, -1.0f, 0, 0, 5);
add_motor_raw_6dof(AP_MOTORS_MOT_6, -1.0f, -1.0f, 0, -1.0f, 0, 0, 6);
add_motor_raw_6dof(AP_MOTORS_MOT_7, 1.0f, 1.0f, 0, -1.0f, 0, 0, 7);
add_motor_raw_6dof(AP_MOTORS_MOT_8, -1.0f, 1.0f, 0, -1.0f, 0, 0, 8);
break;
case SUB_FRAME_VECTORED:
add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, 1.0f, 0, -1.0f, 1.0f, 1);
add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, -1.0f, 0, -1.0f, -1.0f, 2);
add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, -1.0f, 0, 1.0f, 1.0f, 3);
add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 1.0f, 0, 1.0f, -1.0f, 4);
add_motor_raw_6dof(AP_MOTORS_MOT_5, 1.0f, 0, 0, -1.0f, 0, 0, 5);
add_motor_raw_6dof(AP_MOTORS_MOT_6, -1.0f, 0, 0, -1.0f, 0, 0, 6);
break;
case SUB_FRAME_CUSTOM:
// Put your custom motor setup here
//break;
case SUB_FRAME_SIMPLEROV_3:
case SUB_FRAME_SIMPLEROV_4:
case SUB_FRAME_SIMPLEROV_5:
default:
add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, -1.0f, 0, 1.0f, 0, 1);
add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, 1.0f, 0, 1.0f, 0, 2);
add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, 0, -1.0f, 0, 0, 3);
add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 0, -1.0f, 0, 0, 4);
add_motor_raw_6dof(AP_MOTORS_MOT_5, 0, 0, 0, 0, 0, 1.0f, 5);
break;
}
}
void AP_Motors6DOF::add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float lat_fac, uint8_t testing_order)
{
//Parent takes care of enabling output and setting up masks
add_motor_raw(motor_num, roll_fac, pitch_fac, yaw_fac, testing_order);
//These are additional parameters for an ROV
_throttle_factor[motor_num] = throttle_fac;
_forward_factor[motor_num] = forward_fac;
_lateral_factor[motor_num] = lat_fac;
}
// output_min - sends minimum values out to the motors
void AP_Motors6DOF::output_min()
{
int8_t i;
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = false;
limit.throttle_upper = false;
// fill the motor_out[] array for HIL use and send minimum value to each motor
// ToDo find a field to store the minimum pwm instead of hard coding 1500
hal.rcout->cork();
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, 1500);
}
}
hal.rcout->push();
}
int16_t AP_Motors6DOF::calc_thrust_to_pwm(float thrust_in) const
{
return constrain_int16(1500 + thrust_in * 400, _throttle_radio_min, _throttle_radio_max);
}
void AP_Motors6DOF::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_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] = 1500;
}
}
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] = 1500;
}
}
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();
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
// ToDo calculate headroom for rpy to be added for stabilization during full throttle/forward/lateral commands
void AP_Motors6DOF::output_armed_stabilizing()
{
if ((sub_frame_t)_last_frame_class == SUB_FRAME_VECTORED) {
output_armed_stabilizing_vectored();
} else if ((sub_frame_t)_last_frame_class == SUB_FRAME_VECTORED_6DOF) {
output_armed_stabilizing_vectored_6dof();
} else {
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, +/- 1.0
float forward_thrust; // forward thrust input value, +/- 1.0
float lateral_thrust; // lateral thrust input value, +/- 1.0
roll_thrust = _roll_in;
pitch_thrust = _pitch_in;
yaw_thrust = _yaw_in;
throttle_thrust = get_throttle_bidirectional();
forward_thrust = _forward_in;
lateral_thrust = _lateral_in;
float rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= -_throttle_thrust_max) {
throttle_thrust = -_throttle_thrust_max;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
// calculate roll, pitch and yaw for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = roll_thrust * _roll_factor[i] +
pitch_thrust * _pitch_factor[i] +
yaw_thrust * _yaw_factor[i];
}
}
// calculate linear command for each motor
// linear factors should be 0.0 or 1.0 for now
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
linear_out[i] = throttle_thrust * _throttle_factor[i] +
forward_thrust * _forward_factor[i] +
lateral_thrust * _lateral_factor[i];
}
}
// Calculate final output for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpy_out[i] + linear_out[i]),-1.0f,1.0f);
}
}
}
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
// ToDo calculate headroom for rpy to be added for stabilization during full throttle/forward/lateral commands
void AP_Motors6DOF::output_armed_stabilizing_vectored()
{
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, +/- 1.0
float forward_thrust; // forward thrust input value, +/- 1.0
float lateral_thrust; // lateral thrust input value, +/- 1.0
roll_thrust = _roll_in;
pitch_thrust = _pitch_in;
yaw_thrust = _yaw_in;
throttle_thrust = get_throttle_bidirectional();
forward_thrust = _forward_in;
lateral_thrust = _lateral_in;
float rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= -_throttle_thrust_max) {
throttle_thrust = -_throttle_thrust_max;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
// calculate roll, pitch and yaw for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = roll_thrust * _roll_factor[i] +
pitch_thrust * _pitch_factor[i] +
yaw_thrust * _yaw_factor[i];
}
}
float forward_coupling_limit = 1-_forwardVerticalCouplingFactor*float(fabs(throttle_thrust));
if (forward_coupling_limit < 0) {
forward_coupling_limit = 0;
}
int8_t forward_coupling_direction[] = {-1,-1,1,1,0,0,0,0};
// calculate linear command for each motor
// linear factors should be 0.0 or 1.0 for now
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
float forward_thrust_limited = forward_thrust;
// The following statements decouple forward/vertical hydrodynamic coupling on
// vectored ROVs. This is done by limiting the maximum output of the "rear" vectored
// thruster (where "rear" depends on direction of travel).
if (!is_zero(forward_thrust_limited)) {
if ((forward_thrust < 0) == (forward_coupling_direction[i] < 0) && forward_coupling_direction[i] != 0) {
forward_thrust_limited = constrain_float(forward_thrust, -forward_coupling_limit, forward_coupling_limit);
}
}
linear_out[i] = throttle_thrust * _throttle_factor[i] +
forward_thrust_limited * _forward_factor[i] +
lateral_thrust * _lateral_factor[i];
}
}
// Calculate final output for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpy_out[i] + linear_out[i]), -1.0f, 1.0f);
}
}
}
// Band Aid fix for motor normalization issues.
// TODO: find a global solution for managing saturation that works for all vehicles
void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof()
{
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, +/- 1.0
float forward_thrust; // forward thrust input value, +/- 1.0
float lateral_thrust; // lateral thrust input value, +/- 1.0
roll_thrust = _roll_in;
pitch_thrust = _pitch_in;
yaw_thrust = _yaw_in;
throttle_thrust = get_throttle_bidirectional();
forward_thrust = _forward_in;
lateral_thrust = _lateral_in;
float rpt_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
float yfl_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
float rpt_max;
float yfl_max;
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= -_throttle_thrust_max) {
throttle_thrust = -_throttle_thrust_max;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
// calculate roll, pitch and Throttle for each motor (only used by vertical thrusters)
rpt_max = 1; //Initialized to 1 so that normalization will only occur if value is saturated
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpt_out[i] = roll_thrust * _roll_factor[i] +
pitch_thrust * _pitch_factor[i] +
throttle_thrust * _throttle_factor[i];
if (fabs(rpt_out[i]) > rpt_max) {
rpt_max = fabs(rpt_out[i]);
}
}
}
// calculate linear/yaw command for each motor (only used for translational thrusters)
// linear factors should be 0.0 or 1.0 for now
yfl_max = 1; //Initialized to 1 so that normalization will only occur if value is saturated
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
yfl_out[i] = yaw_thrust * _yaw_factor[i] +
forward_thrust * _forward_factor[i] +
lateral_thrust * _lateral_factor[i];
if (fabs(yfl_out[i]) > yfl_max) {
yfl_max = fabs(yfl_out[i]);
}
}
}
// Calculate final output for each motor and normalize if necessary
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpt_out[i]/rpt_max + yfl_out[i]/yfl_max),-1.0f,1.0f);
}
}
}

63
libraries/AP_Motors/AP_Motors6DOF.h Normal file
View File

@ -0,0 +1,63 @@
/// @file AP_Motors6DOF.h
/// @brief Motor control class for ROVs with direct control over 6DOF (or fewer) in movement
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include "AP_MotorsMatrix.h"
/// @class AP_MotorsMatrix
class AP_Motors6DOF : public AP_MotorsMatrix {
public:
AP_Motors6DOF(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz) {
AP_Param::setup_object_defaults(this, var_info);
};
// Supported frame types
typedef enum {
SUB_FRAME_BLUEROV1,
SUB_FRAME_VECTORED,
SUB_FRAME_VECTORED_6DOF,
SUB_FRAME_VECTORED_6DOF_90DEG,
SUB_FRAME_SIMPLEROV_3,
SUB_FRAME_SIMPLEROV_4,
SUB_FRAME_SIMPLEROV_5,
SUB_FRAME_CUSTOM
} sub_frame_t;
// Override parent
void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;
// Override parent
void output_min() override;
// Map thrust input -1~1 to pwm output 1100~1900
int16_t calc_thrust_to_pwm(float thrust_in) const;
// output_to_motors - sends minimum values out to the motors
void output_to_motors() override;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
//Override MotorsMatrix method
void add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float climb_fac, float forward_fac, float lat_fac, uint8_t testing_order);
void output_armed_stabilizing() override;
void output_armed_stabilizing_vectored();
void output_armed_stabilizing_vectored_6dof();
// Parameters
AP_Int8 _motor_reverse[8];
AP_Float _forwardVerticalCouplingFactor;
float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent)
float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward
float _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to lateral (left/right)
};

2
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -61,7 +61,7 @@ protected:
void remove_motor(int8_t motor_num);
// configures the motors for the defined frame_class and frame_type
void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type);
virtual void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type);
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
void normalise_rpy_factors();

7
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -72,12 +72,17 @@ public:
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1
void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max,0.0f,1.0f); }; // range 0 ~ 1
void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
void set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1
void set_lateral(float lateral_in) { _lateral_in = lateral_in; }; // range -1 ~ +1
// accessors for roll, pitch, yaw and throttle inputs to motors
float get_roll() const { return _roll_in; }
float get_pitch() const { return _pitch_in; }
float get_yaw() const { return _yaw_in; }
float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); }
float get_throttle_bidirectional() const { return constrain_float(2*(_throttle_filter.get()-0.5f),-1.0f,1.0f); }
float get_forward() const { return _forward_in; }
float get_lateral() const { return _lateral_in; }
virtual float get_throttle_hover() const = 0;
// spool up states
@ -188,6 +193,8 @@ protected:
float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1
float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1
float _throttle_in; // last throttle input from set_throttle caller
float _forward_in; // last forward input from set_forward caller
float _lateral_in; // last lateral input from set_lateral caller
float _throttle_avg_max; // last throttle input from set_throttle_avg_max
LowPassFilterFloat _throttle_filter; // throttle input filter
spool_up_down_desired _spool_desired; // desired spool state