AP_Motors: add copter tailsitter differential torque frames

TForce1 · Dec 17, 2019 · 68dcab3 · 68dcab3
1 parent 95da094
commit 68dcab3
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 0 deletions.
diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp
@@ -67,6 +67,11 @@ void AP_MotorsMatrixTS::output_to_motors()
 // includes new scaling stability patch
 void AP_MotorsMatrixTS::output_armed_stabilizing()
 {
+    if (enable_yaw_torque) {
+        AP_MotorsMatrix::output_armed_stabilizing();
+        return;
+    }
+
     float   roll_thrust;                // roll thrust input value, +/- 1.0
     float   pitch_thrust;               // pitch thrust input value, +/- 1.0
     float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
@@ -127,6 +132,7 @@ void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_
     }
 
     bool success = false;
+    enable_yaw_torque = true;
 
     switch (frame_class) {
 
@@ -140,6 +146,15 @@ void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_
         case MOTOR_FRAME_QUAD:
             switch (frame_type) {
                 case MOTOR_FRAME_TYPE_PLUS:
+                    // differential torque for yaw: rotation directions specified below
+                    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_NYT_PLUS:
                     // motors 1,2 on wings, motors 3,4 on vertical tail/subfin
                     // motors 1,2 are counter-rotating, as are motors 3,4
                     // left wing motor is CW (looking from front)
@@ -148,14 +163,29 @@ void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_
                     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);
+
+                    enable_yaw_torque = false;
                     success = true;
                     break;
                 case MOTOR_FRAME_TYPE_X:
                     // PLUS_TS layout rotated 45 degrees about X axis
+                    // differential torque for yaw: rotation directions specified below
+                    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_NYT_X:
+                    // PLUS_TS layout rotated 45 degrees about X axis
+                    // no differential torque for yaw: wing and fin motors counter-rotating
                     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);
                     add_motor(AP_MOTORS_MOT_4,  135, 0, 2);
+
+                    enable_yaw_torque = false;
                     success = true;
                     break;
                 default:

diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.h b/libraries/AP_Motors/AP_MotorsMatrixTS.h
@@ -22,6 +22,8 @@ class AP_MotorsMatrixTS : public AP_MotorsMatrix {
     virtual void        output_motor_mask(float thrust, uint8_t mask, float rudder_dt) override;
 
 protected:
+    bool enable_yaw_torque;    // differential torque for yaw control
+
     // configures the motors for the defined frame_class and frame_type
     virtual void        setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;
 

diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h
@@ -59,6 +59,8 @@ class AP_Motors {
         MOTOR_FRAME_TYPE_DJI_X = 13, // X frame, DJI ordering
         MOTOR_FRAME_TYPE_CW_X = 14, // X frame, clockwise ordering
         MOTOR_FRAME_TYPE_I = 15, // (sideways H) octo only
+        MOTOR_FRAME_TYPE_NYT_PLUS = 16, // plus frame, no differential torque for yaw
+        MOTOR_FRAME_TYPE_NYT_X = 17, // X frame, no differential torque for yaw
     };
 
     // Constructor