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;
}
// add_motor using position and prop direction. Roll and Pitch factors can differ (for asymmetrical frames)
void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order)
{
add_motor_raw(
motor_num,
cosf(radians(roll_factor_in_degrees + 90)),
cosf(radians(pitch_factor_in_degrees)),
yaw_factor,
testing_order);
}
void AP_MotorsMatrix::add_motor2(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
{
// call raw motor set-up method
add_motor_raw(
motor_num,
cosf(radians(angle_degrees + 90))*sqrt(2), // roll factor
cosf(radians(angle_degrees))*sqrt(2), // pitch factor
yaw_factor, // yaw factor
testing_order);
}
// add_motor using just position and prop direction
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t testing_order)
{
// call raw motor set-up method
add_motor_raw(
motor_num,
cos(radians(angle_degrees + 90)), // roll factor
cos(radians(angle_degrees)), // pitch factor
(float)direction, // yaw factor
testing_order);
}
// setup_motors - configures the motors for a octa
void AP_MotorsOcta::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
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);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
// V frame set-up
add_motor_raw(AP_MOTORS_MOT_1, 1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, -0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, -1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_7, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_8, 0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
}else {
// X frame set-up
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);
add_motor(AP_MOTORS_MOT_4, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}
// normalise factors to magnitude 0.5
normalise_rpy_factors();
}
// setup_motors - configures the motors for a hexa
void AP_MotorsY6::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// MultiWii set-up
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
}
// setup_motors - configures the motors for a octa
void AP_MotorsOcta::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_MOTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_MOTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_MOTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_MOTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_MOTOR_CW, 3);
}else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
// V frame set-up
add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, -0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, -1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_7, -1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_8, 0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CW, 5);
}else {
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 22.5, AP_MOTORS_MATRIX_MOTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_MOTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 67.5, AP_MOTORS_MATRIX_MOTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_4, 157.5, AP_MOTORS_MATRIX_MOTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -22.5, AP_MOTORS_MATRIX_MOTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_MOTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -67.5, AP_MOTORS_MATRIX_MOTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 112.5, AP_MOTORS_MATRIX_MOTOR_CW, 3);
}
}
// setup_motors - configures the motors for a hexa
void AP_MotorsY6::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
if (_flags.frame_orientation >= AP_MOTORS_NEW_PLUS_FRAME) {
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_3, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_6, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
}else{
// original Y6 motor definition
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
}
}
void AP_MotorsMatrix::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);
}
bool success = false;
switch (frame_class) {
case MOTOR_FRAME_QUAD:
switch (frame_type) {
case MOTOR_FRAME_TYPE_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);
success = true;
break;
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
success = true;
break;
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 2);
success = true;
break;
case MOTOR_FRAME_TYPE_H:
// H frame set-up - same as X but motors spin in opposite directiSons
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);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
success = true;
break;
case MOTOR_FRAME_TYPE_VTAIL:
/*
Tested with: Lynxmotion Hunter Vtail 400
- inverted rear outward blowing motors (at a 40 degree angle)
- should also work with non-inverted rear outward blowing motors
- no roll in rear motors
- no yaw in front motors
- should fly like some mix between a tricopter and X Quadcopter
Roll control comes only from the front motors, Yaw control only from the rear motors.
Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.
Note: if we want the front motors to help with yaw,
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))
*/
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);
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
success = true;
break;
case MOTOR_FRAME_TYPE_ATAIL:
/*
The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
- The Yaw factors are reversed, because the rear motors are facing different directions
With V-Shaped VTails, the props make a V-Shape when spinning, but with
A-Shaped VTails, the props make an A-Shape when spinning.
- Rear thrust on a V-Shaped V-Tail Quad is outward
- Rear thrust on an A-Shaped V-Tail Quad is inward
Still functions the same as the V-Shaped VTail mixing below:
- Yaw control is entirely in the rear motors
- Roll is is entirely in the front motors
*/
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);
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
success = true;
break;
default:
// quad frame class does not support this frame type
break;
}
break; // quad
case MOTOR_FRAME_HEXA:
switch (frame_type) {
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
success = true;
break;
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
success = true;
break;
default:
// hexa frame class does not support this frame type
break;
}
break;
case MOTOR_FRAME_OCTA:
switch (frame_type) {
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
success = true;
break;
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
success = true;
break;
case MOTOR_FRAME_TYPE_V:
add_motor_raw(AP_MOTORS_MOT_1, 1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, -0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, -1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_7, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_8, 0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
success = true;
break;
case MOTOR_FRAME_TYPE_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);
add_motor_raw(AP_MOTORS_MOT_4, -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_7, 1.0f, 0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_8, -1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
success = true;
break;
default:
// octa frame class does not support this frame type
break;
} // octa frame type
break;
case MOTOR_FRAME_OCTAQUAD:
switch (frame_type) {
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
success = true;
break;
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
success = true;
break;
case MOTOR_FRAME_TYPE_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);
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 3);
add_motor(AP_MOTORS_MOT_5, -45, 0.7981f, 8);
add_motor(AP_MOTORS_MOT_6, 45, -0.7981f, 2);
add_motor(AP_MOTORS_MOT_7, 135, 1.0000f, 4);
add_motor(AP_MOTORS_MOT_8, -135, -1.0000f, 6);
success = true;
break;
case MOTOR_FRAME_TYPE_H:
// H frame set-up - same as X but motors spin in opposite directions
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);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
success = true;
break;
default:
// octaquad frame class does not support this frame type
break;
}
break;
case MOTOR_FRAME_DODECAHEXA: {
switch (frame_type) {
case MOTOR_FRAME_TYPE_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
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); // forward-right-bottom
add_motor(AP_MOTORS_MOT_5, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); // back-right-top
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); // back-right-bottom
add_motor(AP_MOTORS_MOT_7, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); // back-top
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); // back-bottom
add_motor(AP_MOTORS_MOT_9, -120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9); // back-left-top
add_motor(AP_MOTORS_MOT_10, -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10); // back-left-bottom
add_motor(AP_MOTORS_MOT_11, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11); // forward-left-top
add_motor(AP_MOTORS_MOT_12, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12); // forward-left-bottom
success = true;
break;
case MOTOR_FRAME_TYPE_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
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); // right-bottom
add_motor(AP_MOTORS_MOT_5, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); // back-right-top
add_motor(AP_MOTORS_MOT_6, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); // back-right-bottom
add_motor(AP_MOTORS_MOT_7, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); // back-left-top
add_motor(AP_MOTORS_MOT_8, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); // back-left-bottom
add_motor(AP_MOTORS_MOT_9, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9); // left-top
add_motor(AP_MOTORS_MOT_10, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10); // left-bottom
add_motor(AP_MOTORS_MOT_11, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11); // forward-left-top
add_motor(AP_MOTORS_MOT_12, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12); // forward-left-bottom
success = true;
break;
default:
// dodeca-hexa frame class does not support this frame type
break;
}}
break;
case MOTOR_FRAME_Y6:
switch (frame_type) {
case MOTOR_FRAME_TYPE_Y6B:
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
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);
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
success = true;
break;
case MOTOR_FRAME_TYPE_Y6F:
// Y6 motor layout for FireFlyY6
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);
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
success = true;
break;
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);
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
success = true;
break;
}
break;
default:
// matrix doesn't support the configured class
break;
} // switch frame_class
// normalise factors to magnitude 0.5
normalise_rpy_factors();
_flags.initialised_ok = success;
}
