diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index ef3c0ab822..903d7e14a6 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -385,17 +385,21 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc } } -// add_motor using just position and prop direction +// add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order) { - // call raw motor set-up method + add_motor(motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order); +} + +// 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(angle_degrees + 90)), // roll factor - cosf(radians(angle_degrees)), // pitch factor - yaw_factor, // yaw factor + cosf(radians(roll_factor_in_degrees + 90)), + cosf(radians(pitch_factor_in_degrees)), + yaw_factor, testing_order); - } // remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index eac0b8cb89..389688df79 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -49,6 +49,9 @@ public: // add_motor using just position and yaw_factor (or prop direction) void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order); + // add_motor using separate roll and pitch factors (for asymmetrical frames) and prop direction + void add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order); + // remove_motor void remove_motor(int8_t motor_num); diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp index 8a831f265c..81ad765601 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ b/libraries/AP_Motors/AP_MotorsQuad.cpp @@ -49,39 +49,45 @@ void AP_MotorsQuad::setup_motors() 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); + }else if(_flags.frame_orientation == AP_MOTORS_VTAIL_FRAME) { + /* + 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 - } else if (_flags.frame_orientation == AP_MOTORS_VTAIL_FRAME) { - /* Lynxmotion Hunter Vtail 400/500 + 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. - Roll control comes only from the front motors, Yaw control only from the rear motors - roll factor is measured by the angle perpendicular to that of the prop arm to the roll axis (x) - pitch factor is measured by the angle perpendicular to the prop arm to the pitch axis (y) + 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)) + */ - assumptions: - 20 20 - \ / 3_____________1 - \ / | - \ / | - 40 \/ 40 20 | 20 - Tail / \ - 2 4 + 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); + } else if (_flags.frame_orientation == AP_MOTORS_ATAIL_FRAME) { + /* + 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 - All angles measured from their closest axis - - 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)) - */ - - // front right: 70 degrees right of roll axis, 20 degrees up of pitch axis, no yaw - add_motor_raw(AP_MOTORS_MOT_1, cosf(radians(160)), cosf(radians(-70)), 0, 1); - // back left: no roll, 70 degrees down of pitch axis, full yaw - add_motor_raw(AP_MOTORS_MOT_2, 0, cosf(radians(160)), AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); - // front left: 70 degrees left of roll axis, 20 degrees up of pitch axis, no yaw - add_motor_raw(AP_MOTORS_MOT_3, cosf(radians(20)), cosf(radians(70)), 0, 4); - // back right: no roll, 70 degrees down of pitch axis, full yaw - add_motor_raw(AP_MOTORS_MOT_4, 0, cosf(radians(-160)), AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + 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); }else{ // X frame set-up add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 0dbc57e2b9..befa01a1d2 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -39,6 +39,7 @@ #define AP_MOTORS_V_FRAME 2 #define AP_MOTORS_H_FRAME 3 // same as X frame but motors spin in opposite direction #define AP_MOTORS_VTAIL_FRAME 4 // Lynxmotion Hunter VTail 400/500 +#define AP_MOTORS_ATAIL_FRAME 5 // A-Shaped VTail Quads #define AP_MOTORS_NEW_PLUS_FRAME 10 // NEW frames are same as original 4 but with motor orders changed to be clockwise from the front #define AP_MOTORS_NEW_X_FRAME 11 #define AP_MOTORS_NEW_V_FRAME 12 @@ -170,7 +171,7 @@ protected: AP_Int8 _throttle_curve_enabled; // enable throttle curve AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range - AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min + AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min // internal variables RC_Channel& _rc_roll; // roll input in from users is held in servo_out