zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors: add frame_class and frame_type strings 

move frame_class/type string assignments down into subclasses
zr-v5.1
Mark Whitehorn 4 years ago committed by Peter Barker
parent
814393c9c2
commit
6b2184a53a
13 changed files with 90 additions and 8 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. 10
      libraries/AP_Motors/AP_Motors6DOF.cpp
  2. 2
      libraries/AP_Motors/AP_Motors6DOF.h
  3. 2
      libraries/AP_Motors/AP_MotorsCoax.h
  4. 2
      libraries/AP_Motors/AP_MotorsHeli.h
  5. 2
      libraries/AP_Motors/AP_MotorsHeli_Dual.h
  6. 2
      libraries/AP_Motors/AP_MotorsHeli_Quad.h
  7. 55
      libraries/AP_Motors/AP_MotorsMatrix.cpp
  8. 9
      libraries/AP_Motors/AP_MotorsMatrix.h
  9. 1
      libraries/AP_Motors/AP_MotorsSingle.cpp
  10. 2
      libraries/AP_Motors/AP_MotorsSingle.h
  11. 2
      libraries/AP_Motors/AP_MotorsTailsitter.h
  12. 2
      libraries/AP_Motors/AP_MotorsTri.h
  13. 7
      libraries/AP_Motors/AP_Motors_Class.h

10
libraries/AP_Motors/AP_Motors6DOF.cpp
Unescape View File

@ -132,6 +132,7 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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() @@ -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
Unescape View File

@ -29,6 +29,8 @@ public: @@ -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
Unescape View File

@ -49,6 +49,8 @@ public: @@ -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
Unescape View File

@ -159,6 +159,8 @@ public: @@ -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
Unescape View File

@ -99,6 +99,8 @@ public: @@ -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
Unescape View File

@ -77,6 +77,8 @@ public: @@ -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
Unescape View File

@ -23,8 +23,8 @@ extern const AP_HAL::HAL& hal; @@ -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) @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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
Unescape View File

@ -70,6 +70,8 @@ public: @@ -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: @@ -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
Unescape View File

@ -177,7 +177,6 @@ void AP_MotorsSingle::output_armed_stabilizing() @@ -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
Unescape View File

@ -49,6 +49,8 @@ public: @@ -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
Unescape View File

@ -33,6 +33,8 @@ public: @@ -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
Unescape View File

@ -54,6 +54,8 @@ public: @@ -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
Unescape View File

@ -47,6 +47,10 @@ public: @@ -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: @@ -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);

Write Preview
Loading…
Cancel
Save