WebSVN - FlightCtrl - Diff - Rev 2088 and 2089 - /branches/dongfang_FC

  */
 int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0;
 // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
 // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
 // int16_t dynamicCalCount;
+// uint16_t accVector;
-uint16_t accVector;
+// uint32_t gyroActivity;
 /************************************************************************
  * Set inclination angles from the acc. sensor data.
   // Calibrate hardware.
   analog_setNeutral();
   // reset gyro integrals to acc guessing
+  setStaticAttitudeAngles();
   setStaticAttitudeAngles();
 #ifdef USE_MK3MAG
   attitude_resetHeadingToMagnetic();
 #endif
   // Servo_On(); //enable servo output
       attitude[axis] += PITCHROLLOVER360;
+    }
+  }
+}
-void correctIntegralsByAcc0thOrder_old(void) {
-  uint8_t axis;
-  int32_t temp;
-  uint8_t ca = controlActivity >> 8;
-  uint8_t highControlActivity = (ca > staticParams.maxControlActivity);
-  if (highControlActivity) {
-    debugOut.digital[1] |= DEBUG_ACC0THORDER;
-  } else {
-    debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
-  }
-  if (accVector <= staticParams.maxAccVector) {
-    debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
-    uint8_t permilleAcc = staticParams.zerothOrderCorrection / 8;
-    int32_t accDerived;
-    /*
-     if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
-     permilleAcc /= 2;
-     debugFullWeight = 0;
-     }
-     if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
-     permilleAcc /= 2;
-     debugFullWeight = 0;
-     */
-    if (highControlActivity) { // reduce effect during stick control activity
-      permilleAcc /= 4;
-      if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity
-        permilleAcc /= 4;
-      }
-    }
-    /*
-     * 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] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
-      // 1000 * the correction amount that will be added to the gyro angle in next line.
-      temp = attitude[axis];
-      attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
-          + (int32_t) permilleAcc * accDerived) / 1000L;
-      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;
-  }
-}
 /************************************************************************
  * 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 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_new(void) {
+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;
-  // for debug LEDs, to be removed with that.
-  static uint8_t controlActivityFlash=1;
-  static uint8_t accFlash=1;
-#define CF_MAX 10
-  // [1..n[=off    [n..10]=on
-  // 1 -->1=on, 2=on, ..., 10=on
-  // 2 -->1=off,2=on, ..., 10=on
-  // 10-->1=off,2=off,..., 10=on
+  int32_t temp;
-  // 11-->1=off,2=off,..., 10=off
-  uint16_t ca = controlActivity >> 6;
-  uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance;
-  if (!controlActivityWeighted) controlActivityWeighted = 1;
-  uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance;
-  if (!accVectorWeighted) accVectorWeighted = 1;
-  uint8_t accPart = staticParams.zerothOrderCorrection;
-  int32_t accDerived;
-  debugOut.analog[14] = controlActivity;
-  debugOut.analog[15] = accVector;
-  debugOut.analog[20] = controlActivityWeighted;
-  debugOut.analog[21] = accVectorWeighted;
+  uint16_t ca = gyroActivity >> 8;
-  debugOut.analog[24] = accVector;
+  debugOut.analog[14] = ca;
-  accPart /=  controlActivityWeighted;
-  accPart /=  accVectorWeighted;
-  if (controlActivityFlash < controlActivityWeighted) {
-          debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
+  uint8_t gyroActivityWeighted = ca / staticParams.rateTolerance;
+  if (!gyroActivityWeighted) gyroActivityWeighted = 1;
+  uint8_t accPart = staticParams.zerothOrderCorrection / gyroActivityWeighted;
-  } else {
-          debugOut.digital[0] |= DEBUG_ACC0THORDER;
+  debugOut.analog[15] = gyroActivityWeighted;
-  }
+  debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
+  debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
-  if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1;
   if (gyroActivityWeighted < 8) {
-  if (accFlash < accVectorWeighted) {
-          debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
+    debugOut.digital[0] |= DEBUG_ACC0THORDER;
   } else {
-          debugOut.digital[1] |= DEBUG_ACC0THORDER;
+  if (gyroActivityWeighted <= 2) {
     debugOut.digital[1] |= DEBUG_ACC0THORDER;
-  if (++accFlash > CF_MAX+1) accFlash=1;
+  }
   /*
    * Add to each sum: The amount by which the angle is changed just below.
    */
   for (axis = PITCH; axis <= ROLL; axis++) {
       correctionSum[axis] = 0;
+    }
+  }
+}
 /*
 void calculateAccVector(void) {
   int16_t temp;
   temp = filteredAcc[0] >> 3;
   accVector = temp * temp;
   temp = filteredAcc[1] >> 3;
   accVector += temp * temp;
   temp = filteredAcc[2] >> 3;
   accVector += temp * temp;
+}
 */
 #ifdef USE_MK3MAG
 void attitude_resetHeadingToMagnetic(void) {
 /************************************************************************
  * Main procedure.
  ************************************************************************/
 void calculateFlightAttitude(void) {
   getAnalogData();
-  calculateAccVector();
+  // calculateAccVector();
   integrate();
-#ifdef ATTITUDE_USE_ACC_SENSORS
-  if (staticParams.maxControlActivity) {
+#ifdef ATTITUDE_USE_ACC_SENSORS
-    correctIntegralsByAcc0thOrder_old();
-  } else {
-    correctIntegralsByAcc0thOrder_new();
   correctIntegralsByAcc0thOrder();
   driftCorrection();
 #endif
   if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
     correctHeadingToMagnetic();
+  }
 #endif
+}
-/*
- * This is part of an experiment to measure average sensor offsets caused by motor vibration,
- * and to compensate them away. It brings about some improvement, but no miracles.
- * As long as the left stick is kept in the start-motors position, the dynamic compensation
- * will measure the effect of vibration, to use for later compensation. So, one should keep
- * the stick in the start-motors position for a few seconds, till all motors run (at the wrong
- * speed unfortunately... must find a better way)
- */
-/*
- void attitude_startDynamicCalibration(void) {
- dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0;
- savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000;
- }
- void attitude_continueDynamicCalibration(void) {
- // measure dynamic offset now...
- dynamicCalPitch += hiResPitchGyro;
- dynamicCalRoll += hiResRollGyro;
- dynamicCalYaw += rawYawGyroSum;
- dynamicCalCount++;
- // Param6: Manual mode. The offsets are taken from Param7 and Param8.
- if (dynamicParams.UserParam6 || 1) { // currently always enabled.
- // manual mode
- driftCompPitch = dynamicParams.UserParam7 - 128;
- driftCompRoll = dynamicParams.UserParam8 - 128;
- } else {
- // use the sampled value (does not seem to work so well....)
- driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
- driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
- driftCompYaw = -dynamicCalYaw / dynamicCalCount;
- }
- // keep resetting these meanwhile, to avoid accumulating errors.
- setStaticAttitudeIntegrals();
- yawAngle = 0;
- }
- */

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c @ 2089 - Rev 2088 → 2089