zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors: New AP_Motors6DOF used by Sub

master
Rustom Jehangir 9 years ago committed by Andrew Tridgell
parent
3da7c95e9b
commit
237ba87ca4
5 changed files with 551 additions and 1 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      libraries/AP_Motors/AP_Motors.h
  2. 479
      libraries/AP_Motors/AP_Motors6DOF.cpp
  3. 63
      libraries/AP_Motors/AP_Motors6DOF.h
  4. 2
      libraries/AP_Motors/AP_MotorsMatrix.h
  5. 7
      libraries/AP_Motors/AP_Motors_Class.h

1
libraries/AP_Motors/AP_Motors.h
Unescape View File

@ -8,3 +8,4 @@ @@ -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
Unescape View File

@ -0,0 +1,479 @@ @@ -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
Unescape View File

@ -0,0 +1,63 @@ @@ -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
Unescape View File

@ -61,7 +61,7 @@ protected: @@ -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
Unescape View File

@ -72,12 +72,17 @@ public: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save