imu angle as integral

controller/controller.ino

@@ -56,18 +56,11 @@ void loop() {
   // controller output
   imu.updateBias();
 
-  #ifdef USING_AUTO_LEVEL
-    autoLevel(input);
-  #endif  
-
   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

@@ -35,6 +35,6 @@ 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.ax(); }
-float accelZ() { return  imu.az(); }
+float accelX() { return  imu.ay(); }
+float accelY() { return -imu.ax(); }
+float accelZ() { return -imu.az(); }

controller/parameters.h

@@ -43,12 +43,8 @@
 //                  2B. Auto level
 //----------------------------------------------------------
 /* NOTE: must enable auto level to use */
-  // I. Gains
-#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    0.0                 // trim to self-level aircraft in pitch
-#define TRIM_ANG_ROLL     0                   // trim to self-level aircraft in roll
+#define TRIM_ANG_ROLL   -20.0                   // trim to self-level aircraft in roll
 
 //----------------------------------------------------------
 //                  3. Servo trims
@@ -84,4 +80,4 @@
 //#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
+#define USING_AUTO_LEVEL                      // Uncomment to enable self-leveling correction

controller/pid.h

@@ -28,7 +28,26 @@ 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] };
+  float angvel_t[] = { input[0]*SCALE, 
+                       input[1]*SCALE,
+                       input[2]*SCALE };
+
+  #ifdef USING_AUTO_LEVEL    
+    // offsets
+    constexpr float ANG_ROLL = TRIM_ANG_ROLL * DEG_TO_RAD;
+    constexpr float ANG_PITCH = TRIM_ANG_PITCH * DEG_TO_RAD;
+
+    // yaw change
+    float dyaw = -0.5 * angvel_t[2] * dt;
+    fusion.rotateHeading( dyaw, SMALL_ANGLE );
+
+    // heading
+    updateFusion();
+
+    angle[0] = GAIN_INT[0] * ( fusion.roll() - ANG_ROLL ); 
+    angle[1] = GAIN_INT[1] * ( fusion.pitch() - ANG_PITCH );   
+    angle[2] = GAIN_INT[2] * fusion.yaw();  
+  #endif
 
   for( int i = 0; i < 3; i += 1 ) {
     // alpha-beta filter:
@@ -37,16 +56,18 @@ void PIDcontroller( float* input, float* output ) {
     angacc[i] += BETA*dvel / dt;                // derivative
 
     // target rate
-    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] * dangvel * dt;
-
-    #ifdef USING_INTEGRAL_DECAY
-      angle[i] -= DECAY[i] * angle[i] * dt;       
-    #endif
+
+    #ifndef USING_AUTO_LEVEL
+      angle[i] += GAIN_INT[i] * dangvel * dt;
+
+      #ifdef USING_INTEGRAL_DECAY
+        angle[i] -= DECAY[i] * angle[i] * dt;       
+      #endif
+   #endif
 
     angle[i] = constrain(angle[i], -PWM_CHANGE, PWM_CHANGE);  // prevent windup
   }
@@ -101,25 +122,10 @@ void PIDcontroller( float* input, float* output ) {
   #endif
 }
 
-/* bias controls to self upright */
-void autoLevel(float* output) {
-  constexpr float ANG_ROLL = TRIM_ANG_ROLL * DEG_TO_RAD;
-  constexpr float ANG_PITCH = TRIM_ANG_PITCH * DEG_TO_RAD;
-
-    // horizontal angles
-  updateFusion();
-  float pitch = fusion.roll() - ANG_ROLL;   
-  float roll = fusion.pitch() - ANG_PITCH; 
-
-    // control rates
-  output[1] -= float(GAIN_ANG_ROLL) * roll; 
-  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/servos.h

@@ -1,21 +1,20 @@
 //=========== Servos / Tail ESC ============
 #include <Servo.h>
-Servo servo[4];
+Servo servo[3];
 
 //---------------------- 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,A0};    // Output pins: roll, pitch, yaw1, yaw2
+constexpr int SERVO_PIN[] = {3,4,5};    // Output pins: roll, pitch, yaw
 
 //---------------------------------------------------------
 
 /* initialize and center servos */
 void setupServos() {  
-  for( int i=0; i < 4; i += 1 ) {
+  for( int i=0; i < 3; i += 1 ) {
     pinMode( SERVO_PIN[i], OUTPUT);
     servo[i].attach( SERVO_PIN[i] );
     servo[i].writeMicroseconds( PWM_MID_SERVO[i] );  
@@ -24,7 +23,7 @@ void setupServos() {
 
 /* set servo positions */
 void servoPosition( float* output ) {
-  for( int i=0; i < 4; i += 1 ) {
+  for( int i=0; i < 3; i += 1 ) {
     output[i] = constrain( output[i], -PWM_CHANGE, PWM_CHANGE);
     servo[i].writeMicroseconds( PWM_MID_SERVO[i] + output[i] );  
   }