aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: add frame_class and frame_type strings

Browse Source 
move frame_class/type string assignments down into subclasses
This commit is contained in:
Mark Whitehorn 2020-11-05 12:59:35 -07:00 committed by Peter Barker
parent 814393c9c2
commit 6b2184a53a
13 changed files with 90 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Motors/AP_Motors6DOF.cpp
View File

@ -132,6 +132,7 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type
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:
_frame_class_string = "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);
@ -141,6 +142,7 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type
break;
case SUB_FRAME_VECTORED_6DOF_90DEG:
_frame_class_string = "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);
@ -152,6 +154,7 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type
break;
case SUB_FRAME_VECTORED_6DOF:
_frame_class_string = "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);
@ -163,6 +166,7 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type
break;
case SUB_FRAME_VECTORED:
_frame_class_string = "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);
@ -176,6 +180,7 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type
//break;
case SUB_FRAME_SIMPLEROV_3:
_frame_class_string = "SIMPLEROV_3";
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);
@ -183,6 +188,7 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type
case SUB_FRAME_SIMPLEROV_4:
case SUB_FRAME_SIMPLEROV_5:
default:
_frame_class_string = "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, 1.0f, 0, 0, -1.0f, 0, 0, 3);
@ -283,9 +289,9 @@ float AP_Motors6DOF::get_current_limit_max_throttle()
// 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) {
if ((sub_frame_t)_active_frame_class == SUB_FRAME_VECTORED) {
output_armed_stabilizing_vectored();
} else if ((sub_frame_t)_last_frame_class == SUB_FRAME_VECTORED_6DOF) {
} else if ((sub_frame_t)_active_frame_class == SUB_FRAME_VECTORED_6DOF) {
output_armed_stabilizing_vectored_6dof();
} else {
uint8_t i; // general purpose counter

2
libraries/AP_Motors/AP_Motors6DOF.h
View File

@ -29,6 +29,8 @@ public:
SUB_FRAME_CUSTOM
} sub_frame_t;
const char* get_frame_string() override { return _frame_class_string; };
// Override parent
void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;

2
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -49,6 +49,8 @@ public:
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t get_motor_mask() override;
const char* get_frame_string() override { return "COAX"; }
protected:
// output - sends commands to the motors
void output_armed_stabilizing() override;

2
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -159,6 +159,8 @@ public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
const char* get_frame_string() override { return "HELI"; }
protected:
// manual servo modes (used for setup)

2
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -99,6 +99,8 @@ public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
const char* get_frame_string() override { return "HELI_DUAL"; }
protected:
// init_outputs

2
libraries/AP_Motors/AP_MotorsHeli_Quad.h
View File

@ -77,6 +77,8 @@ public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
const char* get_frame_string() override { return "HELI_QUAD"; }
protected:
// init_outputs

55
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -23,8 +23,8 @@ extern const AP_HAL::HAL& hal;
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;
_active_frame_class = frame_class;
_active_frame_type = frame_type;
if (frame_class == MOTOR_FRAME_SCRIPTING_MATRIX) {
// if Scripting frame class, do nothing scripting must call its own dedicated init function
@ -114,9 +114,11 @@ void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz)
void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
{
// exit immediately if armed or no change
if (armed() || (frame_class == _last_frame_class && _last_frame_type == frame_type)) {
if (armed() || (frame_class == _active_frame_class && _active_frame_type == frame_type)) {
return;
}
_active_frame_class = frame_class;
_active_frame_type = frame_type;
init(frame_class, frame_type);
@ -554,15 +556,18 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
switch (frame_class) {
case MOTOR_FRAME_QUAD:
_frame_class_string = "QUAD";
_mav_type = MAV_TYPE_QUADROTOR;
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);
@ -570,12 +575,14 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
case MOTOR_FRAME_TYPE_NYT_PLUS:
_frame_type_string = "NYT_PLUS";
add_motor(AP_MOTORS_MOT_1, 90, 0, 2);
add_motor(AP_MOTORS_MOT_2, -90, 0, 4);
add_motor(AP_MOTORS_MOT_3, 0, 0, 1);
add_motor(AP_MOTORS_MOT_4, 180, 0, 3);
break;
case MOTOR_FRAME_TYPE_NYT_X:
_frame_type_string = "NYT_X";
add_motor(AP_MOTORS_MOT_1, 45, 0, 1);
add_motor(AP_MOTORS_MOT_2, -135, 0, 3);
add_motor(AP_MOTORS_MOT_3, -45, 0, 4);
@ -585,6 +592,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
case MOTOR_FRAME_TYPE_BF_X:
// betaflight quad X order
// see: https://fpvfrenzy.com/betaflight-motor-order/
_frame_type_string = "BF_X";
add_motor(AP_MOTORS_MOT_1, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,1);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,3);
@ -592,6 +600,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
case MOTOR_FRAME_TYPE_BF_X_REV:
// betaflight quad X order, reversed motors
_frame_type_string = "X_REV";
add_motor(AP_MOTORS_MOT_1, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
@ -600,6 +609,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
case MOTOR_FRAME_TYPE_DJI_X:
// DJI quad X order
// see https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
_frame_type_string = "DJI_X";
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
@ -608,12 +618,14 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
case MOTOR_FRAME_TYPE_CW_X:
// "clockwise X" motor order. Motors are ordered clockwise from front right
// matching test order
_frame_type_string = "CW_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_CW, 2);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
break;
case MOTOR_FRAME_TYPE_V:
_frame_type_string = "V";
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
add_motor(AP_MOTORS_MOT_2, -135, 1.0000f, 3);
add_motor(AP_MOTORS_MOT_3, -45, -0.7981f, 4);
@ -621,6 +633,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
case MOTOR_FRAME_TYPE_H:
// H frame set-up - same as X but motors spin in opposite directiSons
_frame_type_string = "H";
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
@ -642,6 +655,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle
motors 3's yaw factor should be changed to -sin(radians(40))
*/
_frame_type_string = "VTAIL";
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
@ -661,6 +675,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
- Yaw control is entirely in the rear motors
- Roll is is entirely in the front motors
*/
_frame_type_string = "ATAIL";
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
@ -668,6 +683,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
case MOTOR_FRAME_TYPE_PLUSREV:
// plus with reversed motor directions
_frame_type_string = "PLUSREV";
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
@ -675,15 +691,18 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
default:
// quad frame class does not support this frame type
_frame_type_string = "UNSUPPORTED";
success = false;
break;
}
break; // quad
case MOTOR_FRAME_HEXA:
_frame_class_string = "HEXA";
_mav_type = MAV_TYPE_HEXAROTOR;
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
_frame_type_string = "PLUS";
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
@ -692,6 +711,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_X:
_frame_type_string = "X";
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
@ -701,6 +721,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
case MOTOR_FRAME_TYPE_H:
// H is same as X except middle motors are closer to center
_frame_type_string = "H";
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
@ -709,6 +730,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
break;
case MOTOR_FRAME_TYPE_DJI_X:
_frame_type_string = "DJI_X";
add_motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
add_motor(AP_MOTORS_MOT_3, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
@ -717,6 +739,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_6, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
case MOTOR_FRAME_TYPE_CW_X:
_frame_type_string = "CW_X";
add_motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_3, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
@ -726,15 +749,18 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
default:
// hexa frame class does not support this frame type
_frame_type_string = "UNSUPPORTED";
success = false;
break;
}
break;
case MOTOR_FRAME_OCTA:
_frame_class_string = "OCTA";
_mav_type = MAV_TYPE_OCTOROTOR;
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
_frame_type_string = "PLUS";
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
@ -745,6 +771,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_X:
_frame_type_string = "X";
add_motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
@ -755,6 +782,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_8, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_V:
_frame_type_string = "V";
add_motor_raw(AP_MOTORS_MOT_1, 0.83f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_2, -0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_3, 0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
@ -765,6 +793,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor_raw(AP_MOTORS_MOT_8, 0.50f, -1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
break;
case MOTOR_FRAME_TYPE_H:
_frame_type_string = "H";
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, -1.0f, 0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
@ -775,6 +804,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor_raw(AP_MOTORS_MOT_8, -1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_I:
_frame_type_string = "I";
add_motor_raw(AP_MOTORS_MOT_1, 0.333f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, -0.333f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
@ -785,6 +815,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor_raw(AP_MOTORS_MOT_8, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
break;
case MOTOR_FRAME_TYPE_DJI_X:
_frame_type_string = "DJI_X";
add_motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8);
add_motor(AP_MOTORS_MOT_3, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
@ -795,6 +826,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_8, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
case MOTOR_FRAME_TYPE_CW_X:
_frame_type_string = "CW_X";
add_motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_3, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
@ -806,6 +838,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
default:
// octa frame class does not support this frame type
_frame_type_string = "UNSUPPORTED";
success = false;
break;
} // octa frame type
@ -813,8 +846,10 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
case MOTOR_FRAME_OCTAQUAD:
_mav_type = MAV_TYPE_OCTOROTOR;
_frame_class_string = "OCTAQUAD";
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
_frame_type_string = "PLUS";
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
@ -825,6 +860,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
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, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
@ -835,6 +871,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
break;
case MOTOR_FRAME_TYPE_V:
_frame_type_string = "V";
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
add_motor(AP_MOTORS_MOT_2, -45, -0.7981f, 7);
add_motor(AP_MOTORS_MOT_3, -135, 1.0000f, 5);
@ -846,6 +883,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
case MOTOR_FRAME_TYPE_H:
// H frame set-up - same as X but motors spin in opposite directions
_frame_type_string = "H";
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
@ -856,6 +894,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
break;
case MOTOR_FRAME_TYPE_CW_X:
_frame_type_string = "CW_X";
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_3, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
@ -867,6 +906,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
default:
// octaquad frame class does not support this frame type
_frame_type_string = "UNSUPPORTED";
success = false;
break;
}
@ -874,8 +914,10 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
case MOTOR_FRAME_DODECAHEXA: {
_mav_type = MAV_TYPE_DODECAROTOR;
_frame_class_string = "DODECAHEXA";
switch (frame_type) {
case MOTOR_FRAME_TYPE_PLUS:
_frame_type_string = "PLUS";
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); // forward-top
add_motor(AP_MOTORS_MOT_2, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); // forward-bottom
add_motor(AP_MOTORS_MOT_3, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); // forward-right-top
@ -890,6 +932,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_12, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12); // forward-left-bottom
break;
case MOTOR_FRAME_TYPE_X:
_frame_type_string = "X";
add_motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); // forward-right-top
add_motor(AP_MOTORS_MOT_2, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); // forward-right-bottom
add_motor(AP_MOTORS_MOT_3, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); // right-top
@ -905,6 +948,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
default:
// dodeca-hexa frame class does not support this frame type
_frame_type_string = "UNSUPPORTED";
success = false;
break;
}}
@ -912,9 +956,11 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
case MOTOR_FRAME_Y6:
_mav_type = MAV_TYPE_HEXAROTOR;
_frame_class_string = "Y6";
switch (frame_type) {
case MOTOR_FRAME_TYPE_Y6B:
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
_frame_type_string = "Y6B";
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_3, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
@ -924,6 +970,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
case MOTOR_FRAME_TYPE_Y6F:
// Y6 motor layout for FireFlyY6
_frame_type_string = "Y6F";
add_motor_raw(AP_MOTORS_MOT_1, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
@ -932,6 +979,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
break;
default:
_frame_type_string = "default";
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
@ -978,6 +1026,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
default:
// matrix doesn't support the configured class
_frame_class_string = "UNSUPPORTED";
success = false;
_mav_type = MAV_TYPE_GENERIC;
break;

9
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -70,6 +70,8 @@ public:
// using copter motors for forward flight
float get_roll_factor(uint8_t i) override { return _roll_factor[i]; }
const char* get_frame_string() override { return _frame_class_string; }
const char* get_type_string() override { return _frame_type_string; }
// disable the use of motor torque to control yaw. Used when an external mechanism such
// as vectoring is used for yaw control
void disable_yaw_torque(void) override;
@ -107,13 +109,16 @@ protected:
float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1)
float _thrust_rpyt_out[AP_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
uint8_t _test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence
motor_frame_class _last_frame_class; // most recently requested frame class (i.e. quad, hexa, octa, etc)
motor_frame_type _last_frame_type; // most recently requested frame type (i.e. plus, x, v, etc)
// motor failure handling
float _thrust_rpyt_out_filt[AP_MOTORS_MAX_NUM_MOTORS]; // filtered thrust outputs with 1 second time constant
uint8_t _motor_lost_index; // index number of the lost motor
motor_frame_class _active_frame_class; // active frame class (i.e. quad, hexa, octa, etc)
motor_frame_type _active_frame_type; // active frame type (i.e. plus, x, v, etc)
const char* _frame_class_string = ""; // string representation of frame class
const char* _frame_type_string = ""; // string representation of frame type
private:
static AP_MotorsMatrix *_singleton;
};

1
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -177,7 +177,6 @@ void AP_MotorsSingle::output_armed_stabilizing()
// combine roll, pitch and yaw on each actuator
// front servo
actuator[0] = rp_scale * roll_thrust - yaw_thrust;
// right servo
actuator[1] = rp_scale * pitch_thrust - yaw_thrust;

2
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -49,6 +49,8 @@ public:
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t get_motor_mask() override;
const char* get_frame_string() override { return "SINGLE"; }
protected:
// output - sends commands to the motors
void output_armed_stabilizing() override;

2
libraries/AP_Motors/AP_MotorsTailsitter.h
View File

@ -33,6 +33,8 @@ public:
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t get_motor_mask() override;
const char* get_frame_string() override { return "TAILSITTER"; }
protected:
// calculate motor outputs
void output_armed_stabilizing() override;

2
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -54,6 +54,8 @@ public:
// using copter motors for forward flight
float get_roll_factor(uint8_t i) override;
const char* get_frame_string() override { return "TRI"; }
protected:
// output - sends commands to the motors
void output_armed_stabilizing() override;

7
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -47,6 +47,10 @@ public:
MOTOR_FRAME_DECA = 14,
MOTOR_FRAME_SCRIPTING_MATRIX = 15,
};
// return string corresponding to frame_class
virtual const char* get_frame_string() = 0;
enum motor_frame_type {
MOTOR_FRAME_TYPE_PLUS = 0,
MOTOR_FRAME_TYPE_X = 1,
@ -66,6 +70,9 @@ public:
MOTOR_FRAME_TYPE_BF_X_REV = 18, // X frame, betaflight ordering, reversed motors
};
// return string corresponding to frame_type
virtual const char* get_type_string() { return ""; }
// Constructor
AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);