dynamic and static phase angle

controller/controller.ino

@@ -19,6 +19,7 @@
 // Pin 3  -> Pitch servo
 // Pin 4  -> Roll servo 
 // Pin 5  -> Tail servo / ESC  
+// Pin A0 -> Tail servo / ESC  
 
 //=================== Code ===================
 
@@ -59,11 +60,14 @@ void loop() {
     autoLevel(input);
   #endif  
 
-  float output[3]; PIDcontroller(input, output);
-  
+  float output[4]; PIDcontroller(input, output);
+
   #ifdef USING_TAIL_ROTOR
     output[2] += torqueBias( input[3] );          // anti-torque
   #endif
+
+  output[3] = output[2] + input[3];
+  output[2] -= input[3];
 
   // move servos
   servoPosition(output);   

controller/imu.h

@@ -30,11 +30,11 @@ basicMPU6050<LP_FILTER , GYRO_SENS , ACCEL_SENS, ADDRESS_A0,
  * match roll, pitch and yaw axes, (x, y, z) respectively. 
 */
 
-float gyroX() { return  imu.gy(); } 
-float gyroY() { return -imu.gz(); }
-float gyroZ() { return  imu.gx(); }
+float gyroX() { return -imu.gy(); } 
+float gyroY() { return  imu.gx(); }
+float gyroZ() { return  imu.gz(); }
 
 // sign of accelerometer axes may differ from gyroscope 
 float accelX() { return -imu.ay(); }
-float accelY() { return  imu.az(); }
-float accelZ() { return  imu.ax(); }
+float accelY() { return  imu.ax(); }
+float accelZ() { return  imu.az(); }

controller/parameters.h

@@ -2,35 +2,37 @@
 /*=========================================================*/
 //                  1. Control inputs
 //----------------------------------------------------------
-#define GAIN_PITCH       -0.8                 // Pitch servo
-#define GAIN_ROLL        -0.8                 // Roll servo
-#define GAIN_YAW         -1.4                 // Yaw servo / Tail ESC
+#define GAIN_PITCH       0.8                 // Pitch servo
+#define GAIN_ROLL        0.8                 // Roll servo
+#define GAIN_YAW         1.2                 // Yaw servo / Tail ESC
 
 //----------------------------------------------------------
 //                  2A. Heading stabilization
-//----------------------------------------------------------
+//---------------------------s-------------------------------
 /* NOTE: For a given axis, the propotional, integral and 
          derivative terms must have the same sign */
   // I. Proportional:                         // Proportional gain. Adjusts damping response. Increase to retard rotation. 
-#define GAIN_PROP_ROLL    80
-#define GAIN_PROP_PITCH   120
-#define GAIN_PROP_YAW     35
+#define GAIN_PROP_ROLL    100.0
+#define GAIN_PROP_PITCH   400.0
+#define GAIN_PROP_YAW     15.0
 
   // II. Integral:                            // Integral gain. Adjusts spring respose. Increase to have a stronger restoring force.
-#define GAIN_INT_ROLL     400
-#define GAIN_INT_PITCH    600
-#define GAIN_INT_YAW      200
+#define GAIN_INT_ROLL     800.0
+#define GAIN_INT_PITCH    1000.0
+#define GAIN_INT_YAW      150.0
 
   // III. Derivative:                         // Derivative gain. Reduces oscillations of proportional term. Magnifies vibration noise.
-#define GAIN_DERIV_ROLL   2.0
-#define GAIN_DERIV_PITCH  3.5
+#define GAIN_DERIV_ROLL   8.0
+#define GAIN_DERIV_PITCH  12.0
 #define GAIN_DERIV_YAW    0.5
 
-  // IV. Other:
-#define PHASE_ANGLE      -25       // deg      // Pitch-roll coupling angle. Adjust until control axes respond independently.
-
-//#define NEGATE_ROLL                            // Uncomment to reverse roll deflection
-#define NEGATE_PITCH                           // Uncomment to reverse pitch deflection
+  // IV. Phase angle
+#define STAT_ANGLE        55.0     // deg      // Pitch-roll coupling angle. Adjust until control axes respond independently.
+#define DYN_ANGLE         55.0     // deg
+
+  // V. Other:      
+#define NEGATE_ROLL                            // Uncomment to reverse roll deflection
+//#define NEGATE_PITCH                           // Uncomment to reverse pitch deflection
 //#define NEGATE_YAW                             // Uncomment to reverse yaw deflection
 
 /* NOTE: must enable integral decay to use */
@@ -42,16 +44,16 @@
 //----------------------------------------------------------
 /* NOTE: must enable auto level to use */
   // I. Gains
-#define GAIN_ANG_PITCH    500                 // Gain to set pitch input to self-level aircraft
-#define GAIN_ANG_ROLL     500                 // Gain to set roll input to self-level aircraft
+#define GAIN_ANG_PITCH    0                 // Gain to set pitch input to self-level aircraft
+#define GAIN_ANG_ROLL     0                 // Gain to set roll input to self-level aircraft
   // II. Trim
-#define TRIM_ANG_PITCH    1.0                 // trim to self-level aircraft in pitch
-#define TRIM_ANG_ROLL     5                   // trim to self-level aircraft in roll
+#define TRIM_ANG_PITCH    0.0                 // trim to self-level aircraft in pitch
+#define TRIM_ANG_ROLL     0                   // trim to self-level aircraft in roll
 
 //----------------------------------------------------------
 //                  3. Servo trims
 //----------------------------------------------------------
-#define TRIM_PITCH        60      // us       // Pitch servp
+#define TRIM_PITCH        0       // us       // Pitch servp
 #define TRIM_ROLL         0       // us       // Roll servo
 #define TRIM_YAW          0       // us       // Yaw servo / Tail ESC
 
@@ -61,7 +63,7 @@
 #define NUMBER_MEAN       50                  // number of readings used for input calibration
 #define INPUT_DEADBAND    24      // us       // deadband near center-stick to prevent integrator drift.
 #define INPUT_CHANGE      4       // us       // Change in PWM signal needed to update receiver inputs
-#define ALPHA             0.80                /* Gain of alpha-beta filter applied to sensor input. 
+#define ALPHA             0.6                /* Gain of alpha-beta filter applied to sensor input. 
                                                A smaller value smoothens the derivative at the cost of slower response */
 //----------------------------------------------------------
 //                  5. Tail rotor counter-torque
@@ -74,12 +76,12 @@
 //                  6. Output PWM signal
 //----------------------------------------------------------
 #define PWM_MID           1500    // us       // Center/middle servo position
-#define PWM_CHANGE        650     // us       // Maximum change in servo position
+#define PWM_CHANGE        550     // us       // Maximum change in servo position
 
 //----------------------------------------------------------
 //                  7. Settings
 //---------------------------------------------------------- 
-#define USING_WEIGHT_SHIFT                    // Uncomment for weight-shift control 
+//#define USING_WEIGHT_SHIFT                    // Uncomment for weight-shift control 
 //#define USING_TAIL_ROTOR                      // Uncomment for tail rotor correction
 //#define USING_INTEGRAL_DECAY                  // Uncomment to enable integral decay
 //#define USING_AUTO_LEVEL                      // Uncomment to enable self-leveling correction

controller/pid.h

@@ -28,6 +28,7 @@ void PIDcontroller( float* input, float* output ) {
   static float angvel_s[3] = {0};   // smoothed proportional
 
   float angvel[] = { gyroX(), gyroY(), gyroZ() };
+  float angvel_t[] = { input[0], input[1], input[2] };
 
   for( int i = 0; i < 3; i += 1 ) {
     // alpha-beta filter:
@@ -36,24 +37,49 @@ void PIDcontroller( float* input, float* output ) {
     angacc[i] += BETA*dvel / dt;                // derivative
 
     // target rate
-    float angvel_t = input[i] * SCALE;          // target angular velocity
-    angvel[i] -= angvel_t; 
+    angvel_t[i] *= SCALE;                       // target angular velocity
+    float dangvel = angvel[i] - angvel_t[i]; 
 
     // integrate
     angvel[i] += angacc[i] * 0.5 * dt;           // trapezoidal rule. Use derivative to improve area slice. 
-    angle[i] += GAIN_INT[i] * angvel[i] * dt;
+    angle[i] += GAIN_INT[i] * dangvel * dt;
 
     #ifdef USING_INTEGRAL_DECAY
       angle[i] -= DECAY[i] * angle[i] * dt;       
     #endif
 
     angle[i] = constrain(angle[i], -PWM_CHANGE, PWM_CHANGE);  // prevent windup
-
-    // sum terms
-    output[i] = GAIN_PROP[i]*angvel[i] + angle[i] + GAIN_DERIV[i]*angacc[i];
   }
 
-  // 2. parameters:
+  // yaw axis
+  output[2] = GAIN_PROP[2]*(angvel[2] - angvel_t[2]) + angle[2] + GAIN_DERIV[2]*angacc[2];
+
+  // 3. Phase angle mixing
+
+    // dynamic angle
+  constexpr float DYN_ANG = DYN_ANGLE * DEG_TO_RAD;
+  constexpr float SIN_DYN = sin(DYN_ANG);
+  constexpr float COS_DYN = cos(DYN_ANG);
+
+  float dyn_x = GAIN_PROP[0]*angvel[0] + GAIN_DERIV[0]*angacc[0];
+  float dyn_y = GAIN_PROP[1]*angvel[1] + GAIN_DERIV[1]*angacc[1];
+
+  output[0] = dyn_x*COS_DYN + dyn_y*SIN_DYN;
+  output[1] = dyn_y*COS_DYN - dyn_x*SIN_DYN; 
+
+    // static angle
+  constexpr float ANGLE = DEG_TO_RAD * STAT_ANGLE;
+  constexpr float SIN_ANG = sin(ANGLE);
+  constexpr float COS_ANG = cos(ANGLE);
+
+  float stat_x = -GAIN_PROP[0]*angvel_t[0] + angle[0];
+  float stat_y = -GAIN_PROP[1]*angvel_t[1] + angle[1];
+
+  float output_0 = output[0];
+  output[0] += stat_x*COS_ANG + stat_y*SIN_ANG;
+  output[1] += stat_y*COS_ANG - stat_x*SIN_ANG;
+
+  // 3. parameters:
 
   #ifdef USING_WEIGHT_SHIFT
     float accel = accelZ();
@@ -90,10 +116,10 @@ void autoLevel(float* output) {
   output[0] -= float(GAIN_ANG_PITCH) * pitch;   
 }
 
-/* counter main-rotor torque via yaw bias */
+/* counter main-rotor torque via yaw bias
 float torqueBias(float input) {
   constexpr float SCALE = 0.5 / PWM_CHANGE;
   input = input < 0 ? 0 : input;
   input *= SCALE;
   return pow(input, TORQUE_POWER) * TORQUE_GAIN;  
-}
+} */

controller/receiver.h

@@ -79,7 +79,7 @@ void filterInputs(float* output) {
 
     // center output
     output[index] = filter[index] - pwm_mean[index];
-    output[index] = deadband( output[index], -INPUT_DEADBAND, INPUT_DEADBAND);
+    output[index] = deadband( output[index], -INPUT_DEADBAND, INPUT_DEADBAND );
   }
   // scale output
   output[0] *= float(GAIN_ROLL);    

controller/servos.h

@@ -1,20 +1,21 @@
 //=========== Servos / Tail ESC ============
 #include <Servo.h>
-Servo servo[3];
+Servo servo[4];
 
 //---------------------- Constants -------------------------
 
 constexpr float PWM_MID_SERVO[] = { PWM_MID + TRIM_PITCH, 
                                     PWM_MID + TRIM_ROLL, 
+                                    PWM_MID + TRIM_YAW, 
                                     PWM_MID + TRIM_YAW };
 
-constexpr int SERVO_PIN[] = {3,4,5};    // Output pins: roll, pitch, yaw
+constexpr int SERVO_PIN[] = {3,4,5,A0};    // Output pins: roll, pitch, yaw1, yaw2
 
 //---------------------------------------------------------
 
 /* initialize and center servos */
 void setupServos() {  
-  for( int i=0; i < 3; i += 1 ) {
+  for( int i=0; i < 4; i += 1 ) {
     pinMode( SERVO_PIN[i], OUTPUT);
     servo[i].attach( SERVO_PIN[i] );
     servo[i].writeMicroseconds( PWM_MID_SERVO[i] );  
@@ -23,17 +24,7 @@ void setupServos() {
 
 /* set servo positions */
 void servoPosition( float* output ) {
-  constexpr float ANGLE = DEG_TO_RAD * PHASE_ANGLE;
-  constexpr float SIN_ANG = sin(ANGLE);
-  constexpr float COS_ANG = cos(ANGLE);
-
-  // mix pitch and roll via phase angle
-  float output_0 = output[0];
-  output[0] = output_0*COS_ANG + output[1]*SIN_ANG;
-  output[1] = output[1]*COS_ANG - output_0*SIN_ANG;
-
-  // set positions 
-  for( int i=0; i < 3; i += 1 ) {
+  for( int i=0; i < 4; i += 1 ) {
     output[i] = constrain( output[i], -PWM_CHANGE, PWM_CHANGE);
     servo[i].writeMicroseconds( PWM_MID_SERVO[i] + output[i] );  
   }