WebSVN - FlightCtrl - Diff - Rev 1645 and 1646 - /branches/dongfang_FC

 #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
 #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
 #define YAWOVER360       (GYRO_DEG_FACTOR_YAW * 360L)
-int32_t correctionSum[2] = {0,0};
+int16_t correctionSum[2] = {0,0};
 /*
 void attitude_setNeutral(void) {
   // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
   dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
+  dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0;
-  dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0;
+  correctionSum[PITCH] = correctionSum[ROLL] = 0;
   // Calibrate hardware.
   setStaticAttitudeAngles();
   yawAngle = 0;
   // update compass course to current heading
+  compassCourse = compassHeading;
   compassCourse = compassHeading;
   // Inititialize YawGyroIntegral value with current compass heading
   yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
   for (axis=PITCH; axis <=ROLL; axis++) {
     rate_PID[axis]     = (gyro_PID[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
     rate[axis]         = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
     differential[axis] = gyroD[axis];
+  }
   yawRate = yawGyro + dynamicOffsetYaw;
   // We are done reading variables from the analog module.
   // Interrupt-driven sensor reading may restart.
     ACRate[PITCH] = rate[PITCH];
     ACRate[ROLL]  = rate[ROLL];
     ACYawRate = yawRate;
+  }
-  DebugOut.Analog[3] = ACRate[PITCH];
-  DebugOut.Analog[4] = ACRate[ROLL];
-  DebugOut.Analog[5] = ACYawRate;
   /*
    * Yaw
    * Calculate yaw gyro integral (~ to rotation angle)
    * Limit yawGyroHeading proportional to 0 deg to 360 deg
+}
 /************************************************************************
  * A kind of 0'th order integral correction, that corrects the integrals
  * directly. This is the "gyroAccFactor" stuff in the original code.
- * There is (there) also what I would call a  "minus 1st order correction"
+ * There is (there) also a drift compensation
  * - it corrects the differential of the integral = the gyro offsets.
  * That should only be necessary with drifty gyros like ENC-03.
  ************************************************************************/
 void correctIntegralsByAcc0thOrder(void) {
   // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
   // are less than ....., or reintroduce Kalman.
   // Well actually the Z axis acc. check is not so silly.
   uint8_t axis;
-  if(!looping && //((ZAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction
+  int32_t correction;
-     ZAcc >= -dynamicParams.UserParams[7] && ZAcc <= dynamicParams.UserParams[7]) {
+  if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) {
     DebugOut.Digital[0] = 1;
     uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
     uint8_t debugFullWeight = 1;
-    int32_t accDerived[2];
+    int32_t accDerived;
     if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
     /*
      * Add to each sum: The amount by which the angle is changed just below.
      */
     for (axis=PITCH; axis<=ROLL; axis++) {
+      accDerived = getAngleEstimateFromAcc(axis);
+      DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
-      accDerived[axis] = getAngleEstimateFromAcc(axis);
+      // 1000 * the correction amount that will be added to the gyro angle in next line.
+      correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
+      angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L;
-      correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]);
+      correctionSum[axis] += angle[axis] - correction;
+      // There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
+      // change = ((1000 - permilleAcc) * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis]
+      // = (1000 * angle[axis] - permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis]
+      // = (- permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000
+      // = permilleAcc * (accDerived - angle[axis]) / 1000
-      // There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
+      // Experiment: Do not acutally apply the correction. See if drift compensation does that.
-      angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L;
+      // angle[axis] += correction / 1000;
+    }
 /************************************************************************
  * This is an attempt to correct not the error in the angle integrals
  * (that happens in correctIntegralsByAcc0thOrder above) but rather the
  * cause of it: Gyro drift, vibration and rounding errors.
  * All the corrections made in correctIntegralsByAcc0thOrder over
- * MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is
+ * DRIFTCORRECTION_TIME cycles are summed up. This number is
- * then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx.
+ * then divided by DRIFTCORRECTION_TIME to get the approx.
  * correction that should have been applied to each iteration to fix
  * the error. This is then added to the dynamic offsets.
  ************************************************************************/
-// 2 times / sec.
+// 2 times / sec. = 488/2
-#define DRIFTCORRECTION_TIME 488/2
+#define DRIFTCORRECTION_TIME 256L
-void driftCompensation(void) {
+void driftCorrection(void) {
   static int16_t timer = DRIFTCORRECTION_TIME;
-  int16_t deltaCompensation;
+  int16_t deltaCorrection;
   uint8_t axis;
   if (! --timer) {
     timer = DRIFTCORRECTION_TIME;
     for (axis=PITCH; axis<=ROLL; axis++) {
+      // Take the sum of corrections applied, add it to delta
-      deltaCompensation = ((correctionSum[axis] + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME);
+      deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME;
-      CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp);
+      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
-      dynamicOffset[axis] += deltaCompensation / staticParams.GyroAccTrim;
+      dynamicOffset[axis] += deltaCorrection / staticParams.GyroAccTrim;
+      CHECK_MIN_MAX(dynamicOffset[axis], -staticParams.DriftComp, staticParams.DriftComp);
+      DebugOut.Analog[11 + axis] = correctionSum[axis];
+      DebugOut.Analog[28 + axis] = dynamicOffset[axis];
       correctionSum[axis] = 0;
-      DebugOut.Analog[28 + axis] = dynamicOffset;
+    }
+  }
+}
 /************************************************************************
  * Main procedure.
  ************************************************************************/
 void calculateFlightAttitude(void) {
   getAnalogData();
+  integrate();
+  DebugOut.Analog[6] = ACRate[PITCH];
+  DebugOut.Analog[7] = ACRate[ROLL];
+  DebugOut.Analog[8] = ACYawRate;
+  DebugOut.Analog[3] = rate_PID[PITCH];
+  DebugOut.Analog[4] = rate_PID[ROLL];
+  DebugOut.Analog[5] = yawRate;
   integrate();
 #ifdef ATTITUDE_USE_ACC_SENSORS
   correctIntegralsByAcc0thOrder();
-  driftCompensation();
+  driftCorrection();
 #endif
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c - Rev 1645 → 1646