Copter: Added support for V-Shaped and A-Shaped VTail Quadcopter frames

Signed-off-by: NullVoxPopuli <LPSego3+dev@gmail.com>

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(roll_factor_in_degrees + 90)),
-        cosf(radians(angle_degrees)),                    // pitch factor
+        cosf(radians(pitch_factor_in_degrees)),
-        yaw_factor,                                      // yaw factor
+        yaw_factor,
         testing_order);
-
 }
 
 // remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor

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);
 

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) {
+            Roll control comes only from the front motors, Yaw control only from the rear motors.
-        /* Lynxmotion Hunter Vtail 400/500
+            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)
-
-           assumptions:
-                                20      20
-            \      /          3_____________1
-             \    /                  |
-              \  /                   |
-           40  \/  40            20  |  20
-              Tail                  / \
-                                   2   4
-
-           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(AP_MOTORS_MOT_1, 60, 60, 0, 1);
-      add_motor_raw(AP_MOTORS_MOT_1, cosf(radians(160)), cosf(radians(-70)), 0, 1);
+        add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
-      // back left: no roll, 70 degrees down of pitch axis, full yaw
+        add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
-      add_motor_raw(AP_MOTORS_MOT_2, 0, cosf(radians(160)), AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
+        add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
-      // front left: 70 degrees left of roll axis, 20 degrees up of pitch axis, no yaw
+    } else if (_flags.frame_orientation == AP_MOTORS_ATAIL_FRAME) {
-      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
+            The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
-      add_motor_raw(AP_MOTORS_MOT_4, 0, cosf(radians(-160)), AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
+            - 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);
     }else{
         // X frame set-up
         add_motor(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);

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