WebSVN - FlightCtrl - Diff - Rev 2058 and 2059 - /branches/dongfang_FC

  * directly. This is the "gyroAccFactor" stuff in the original code.
  * 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.
  ************************************************************************/
+#define LOG_DIVIDER 12
+#define DIVIDER (1L << LOG_DIVIDER)
 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;
   int32_t temp;
-  uint8_t ca = controlActivity >> 8;
+  uint16_t ca = controlActivity >> 6;
-  uint8_t highControlActivity = (ca > staticParams.maxControlActivity);
+  uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance;
-  if (highControlActivity) {
+  if (!controlActivityWeighted) controlActivityWeighted = 1;
-    debugOut.digital[1] |= DEBUG_ACC0THORDER;
-  } else {
+  uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance;
-    debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
-  }
+  if (!accVectorWeighted) accVectorWeighted = 1;
-  if (accVector <= dynamicParams.maxAccVector) {
+  uint8_t accPart = staticParams.zerothOrderCorrection;
-    debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
+  int32_t accDerived;
-    uint8_t permilleAcc = staticParams.zerothOrderCorrection;
+  debugOut.analog[14] = controlActivity;
-    int32_t accDerived;
+  debugOut.analog[15] = accVector;
-    /*
+  /*
-     if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
+  if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
-     permilleAcc /= 2;
+  permilleAcc /= 2;
-     debugFullWeight = 0;
+  debugFullWeight = 0;
-     }
+  }
-     if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
+  if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
-     permilleAcc /= 2;
+  permilleAcc /= 2;
-     debugFullWeight = 0;
+  debugFullWeight = 0;
-     */
+  */
-    if (highControlActivity) { // reduce effect during stick control activity
+  debugOut.analog[20] = controlActivityWeighted;
-      permilleAcc /= 4;
-      if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity
+  debugOut.analog[21] = accVectorWeighted;
-        permilleAcc /= 4;
-      }
-    }
+  debugOut.analog[24] = accVector;
+  accPart /=  controlActivityWeighted;
+  accPart /=  accVectorWeighted;
-    /*
+  /*
-     * Add to each sum: The amount by which the angle is changed just below.
+   * Add to each sum: The amount by which the angle is changed just below.
-     */
+   */
-    for (axis = PITCH; axis <= ROLL; axis++) {
+  for (axis = PITCH; axis <= ROLL; axis++) {
-      accDerived = getAngleEstimateFromAcc(axis);
+    accDerived = getAngleEstimateFromAcc(axis);
-      //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
+    //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
-      // 1000 * the correction amount that will be added to the gyro angle in next line.
+    // 1000 * the correction amount that will be added to the gyro angle in next line.
-      temp = attitude[axis];
+    temp = attitude[axis];
-      attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
-          + (int32_t) permilleAcc * accDerived) / 1000L;
+    attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER;
-      correctionSum[axis] += attitude[axis] - temp;
-    }
-  } else {
-    // experiment: Kill drift compensation updates when not flying smooth.
-    // correctionSum[PITCH] = correctionSum[ROLL] = 0;
-    debugOut.digital[0] |= DEBUG_ACC0THORDER;
+    correctionSum[axis] += attitude[axis] - temp;
+  }
+}
 void correctHeadingToMagnetic(void) {
   int32_t error;
   if (commands_isCalibratingCompass()) {
-    debugOut.analog[30] = -1;
+    //debugOut.analog[30] = -1;
     return;
+  }
   // Compass is off, skip.
   // Naaah this is assumed.
   // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
   //     return;
   // Compass is invalid, skip.
   if (magneticHeading < 0) {
-    debugOut.analog[30] = -2;
+    //debugOut.analog[30] = -2;
     return;
+  }
   // Spinning fast, skip
   if (abs(yawRate) > 128) {
-    debugOut.analog[30] = -3;
+    // debugOut.analog[30] = -3;
     return;
+  }
   // Otherwise invalidated, skip
   if (ignoreCompassTimer) {
     ignoreCompassTimer--;
-    debugOut.analog[30] = -4;
+    //debugOut.analog[30] = -4;
     return;
   int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
   //debugOut.analog[30] = correction;
-  // The correction is added both to current heading (the direction in which the copter thinks it is pointing)
-  // and to the target heading (the direction to which it maneuvers to point). That means, this correction has
-  // no effect on control at all!!! It only has effect on the values of the two variables. However, these values
+  // The correction is added both to current heading (the direction in which the copter thinks it is pointing)
+  // and to the heading error (the angle of yaw that the copter is off the set heading).
-  // could have effect on control elsewhere, like in compassControl.c .
+  heading += correction;
-  heading += correction;
+  headingError += correction;
   intervalWrap(&heading, YAWOVER360);

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c @ 2062 - Rev 2058 → 2059