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

 #include <stdlib.h>
 #include <avr/io.h>
 #include "attitude.h"
 #include "dongfangMath.h"
 #include "commands.h"
 // For scope debugging only!
 #include "rc.h"
 // where our main data flow comes from.
 #include "analog.h"
 #include "configuration.h"
 #include "output.h"
 // Some calculations are performed depending on some stick related things.
 #include "controlMixer.h"
 #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 /*
  * Gyro readings, as read from the analog module. It would have been nice to flow
  * them around between the different calculations as a struct or array (doing
  * things functionally without side effects) but this is shorter and probably
  * faster too.
  * The variables are overwritten at each attitude calculation invocation - the values
  * are not preserved or reused.
  */
 int16_t rate_ATT[2], yawRate;
 // With different (less) filtering
 int16_t rate_PID[2];
 int16_t differential[2];
 /*
  * Gyro readings, after performing "axis coupling" - that is, the transfomation
  * of rotation rates from the airframe-local coordinate system to a ground-fixed
  * coordinate system. If axis copling is disabled, the gyro readings will be
  * copied into these directly.
  * These are global for the same pragmatic reason as with the gyro readings.
  * The variables are overwritten at each attitude calculation invocation - the values
  * are not preserved or reused.
  */
 int16_t ACRate[2], ACYawRate;
 /*
  * Gyro integrals. These are the rotation angles of the airframe compared to the
  * horizontal plane, yaw relative to yaw at start.
  */
 int32_t attitude[2];
 //int readingHeight = 0;
 // Yaw angle and compass stuff.
 int32_t headingError;
 // The difference between the above 2 (heading - course) on a -180..179 degree interval.
 // Not necessary. Never read anywhere.
 // int16_t compassOffCourse = 0;
 uint16_t ignoreCompassTimer = 0;// 500;
 int32_t heading; // Yaw Gyro Integral supported by compass
 int16_t yawGyroDrift;
 int16_t correctionSum[2] = { 0, 0 };
 // For NaviCTRL use.
 int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0;
 /*
  * Experiment: Compensating for dynamic-induced gyro biasing.
  */
 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.
  * If acc. sensors are not used, set to zero.
  * TODO: One could use inverse sine to calculate the angles more
  * accurately, but since: 1) the angles are rather small at times when
  * it makes sense to set the integrals (standing on ground, or flying at
  * constant speed, and 2) at small angles a, sin(a) ~= constant * a,
  * it is hardly worth the trouble.
  ************************************************************************/
 int32_t getAngleEstimateFromAcc(uint8_t axis) {
   //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
   return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm;
   // return 342L * filteredAcc[axis];
+}
 void setStaticAttitudeAngles(void) {
 #ifdef ATTITUDE_USE_ACC_SENSORS
   attitude[PITCH] = getAngleEstimateFromAcc(PITCH);
   attitude[ROLL] = getAngleEstimateFromAcc(ROLL);
 #else
   attitude[PITCH] = attitude[ROLL] = 0;
 #endif
+}
 /************************************************************************
  * Neutral Readings
  ************************************************************************/
 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;
   driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
   correctionSum[PITCH] = correctionSum[ROLL] = 0;
   // Calibrate hardware.
   analog_setNeutral();
   // reset gyro integrals to acc guessing
   setStaticAttitudeAngles();
 #ifdef USE_MK3MAG
   attitude_resetHeadingToMagnetic();
 #endif
   // Servo_On(); //enable servo output
+}
 /************************************************************************
  * Get sensor data from the analog module, and release the ADC
  * TODO: Ultimately, the analog module could do this (instead of dumping
  * the values into variables).
  * The rate variable end up in a range of about [-1024, 1023].
  *************************************************************************/
 void getAnalogData(void) {
   uint8_t axis;
   analog_update();
   for (axis = PITCH; axis <= ROLL; axis++) {
     rate_PID[axis] = gyro_PID[axis] + driftComp[axis];
     rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];
     differential[axis] = gyroD[axis];
     averageAcc[axis] += acc[axis];
+  }
   averageAccCount++;
   yawRate = yawGyro + driftCompYaw;
+}
 /*
  * This is the standard flight-style coordinate system transformation
  * (from airframe-local axes to a ground-based system). For some reason
  * the MK uses a left-hand coordinate system. The tranformation has been
  * changed accordingly.
  */
 void trigAxisCoupling(void) {
   int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
   int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;
   int16_t cospitch = cos_360(pitchAngleInDegrees);
   int16_t cosroll = cos_360(rollAngleInDegrees);
   int16_t sinroll = sin_360(rollAngleInDegrees);
   ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll
       - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR);
   ACRate[ROLL] = rate_ATT[ROLL]
       + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
           >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees))
           >> LOG_MATH_UNIT_FACTOR);
   ACYawRate =
       ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
           / cospitch;
+}
 // 480 usec with axis coupling - almost no time without.
 void integrate(void) {
   // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
   uint8_t axis;
   if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) {
     trigAxisCoupling();
   } else {
     ACRate[PITCH] = rate_ATT[PITCH];
     ACRate[ROLL] = rate_ATT[ROLL];
     ACYawRate = yawRate;
+  }
   /*
    * Yaw
    * Calculate yaw gyro integral (~ to rotation angle)
    * Limit heading proportional to 0 deg to 360 deg
    */
   heading += ACYawRate;
   intervalWrap(&heading, YAWOVER360);
   headingError += ACYawRate;
   /*
    * Pitch axis integration and range boundary wrap.
    */
   for (axis = PITCH; axis <= ROLL; axis++) {
     attitude[axis] += ACRate[axis];
     if (attitude[axis] > PITCHROLLOVER180) {
       attitude[axis] -= PITCHROLLOVER360;
     } else if (attitude[axis] <= -PITCHROLLOVER180) {
       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
-  // 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;
+  uint8_t gyroActivityWeighted = ca / staticParams.rateTolerance;
-  accPart /=  accVectorWeighted;
-  if (controlActivityFlash < controlActivityWeighted) {
-          debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
-  } else {
+  if (!gyroActivityWeighted) gyroActivityWeighted = 1;
+  uint8_t accPart = staticParams.zerothOrderCorrection / gyroActivityWeighted;
-          debugOut.digital[0] |= DEBUG_ACC0THORDER;
+  debugOut.analog[15] = gyroActivityWeighted;
-  }
+  debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
-  if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1;
+  debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
+  if (gyroActivityWeighted < 8) {
-  if (accFlash < accVectorWeighted) {
+    debugOut.digital[0] |= DEBUG_ACC0THORDER;
-          debugOut.digital[1] &= ~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++) {
-    accDerived = getAngleEstimateFromAcc(axis);
+    int32_t 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) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER;
     correctionSum[axis] += attitude[axis] - temp;
+  }
+}
 /************************************************************************
  * 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
  * DRIFTCORRECTION_TIME cycles are summed up. This number is
  * 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. = 488/2
 #define DRIFTCORRECTION_TIME 256L
 void driftCorrection(void) {
   static int16_t timer = DRIFTCORRECTION_TIME;
   int16_t deltaCorrection;
   int16_t round;
   uint8_t axis;
   if (!--timer) {
     timer = DRIFTCORRECTION_TIME;
     for (axis = PITCH; axis <= ROLL; axis++) {
       // Take the sum of corrections applied, add it to delta
       if (correctionSum[axis] >= 0)
         round = DRIFTCORRECTION_TIME / 2;
       else
         round = -DRIFTCORRECTION_TIME / 2;
       deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
       // Add the delta to the compensation. So positive delta means, gyro should have higher value.
       driftComp[axis] += deltaCorrection / staticParams.driftCompDivider;
       CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit);
       // DebugOut.Analog[11 + axis] = correctionSum[axis];
       // DebugOut.Analog[16 + axis] = correctionSum[axis];
       // debugOut.analog[28 + axis] = driftComp[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) {
   if (commands_isCalibratingCompass())
     return;
   // Compass is off, skip.
   if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED))
       return;
   // Compass is invalid, skip.
   if (magneticHeading < 0)
     return;
   heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
   //targetHeading = heading;
   headingError = 0;
+}
 void correctHeadingToMagnetic(void) {
   int32_t error;
   if (commands_isCalibratingCompass()) {
     //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;
     return;
+  }
   // Spinning fast, skip
   if (abs(yawRate) > 128) {
     // debugOut.analog[30] = -3;
     return;
+  }
   // Otherwise invalidated, skip
   if (ignoreCompassTimer) {
     ignoreCompassTimer--;
     //debugOut.analog[30] = -4;
     return;
+  }
   //debugOut.analog[30] = magneticHeading;
   // TODO: Find computational cost of this.
   error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading);
   if (error <= -YAWOVER180) error += YAWOVER360;
   else if (error > YAWOVER180) error -= YAWOVER360;
   // We only correct errors larger than the resolution of the compass, or else we would keep rounding the
   // better resolution of the gyros to the worse resolution of the compass all the time.
   // The correction should really only serve to compensate for gyro drift.
   if(abs(error) < GYRO_DEG_FACTOR_YAW) return;
   int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
   //debugOut.analog[30] = correction;
   debugOut.digital[0] &= ~DEBUG_COMPASS;
   debugOut.digital[1] &= ~DEBUG_COMPASS;
   if (correction > 0) {
           debugOut.digital[0] ^= DEBUG_COMPASS;
   } else if (correction < 0) {
           debugOut.digital[1] ^= DEBUG_COMPASS;
+  }
   // 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).
   heading += correction;
   headingError += correction;
   intervalWrap(&heading, YAWOVER360);
   // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all
   // when the compass corrects the heading - it only corrects numbers!) we want to add:
   // This will however cause drift to remain uncorrected!
   // headingError += correction;
   //debugOut.analog[29] = 0;
+}
 #endif
 /************************************************************************
  * Main procedure.
  ************************************************************************/
 void calculateFlightAttitude(void) {
   getAnalogData();
-  calculateAccVector();
+  // calculateAccVector();
   integrate();
 #ifdef ATTITUDE_USE_ACC_SENSORS
-  if (staticParams.maxControlActivity) {
-    correctIntegralsByAcc0thOrder_old();
+  correctIntegralsByAcc0thOrder();
-  } else {
-    correctIntegralsByAcc0thOrder_new();
-  }
   driftCorrection();
 #endif
   // We are done reading variables from the analog module.
   // Interrupt-driven sensor reading may restart.
   startAnalogConversionCycle();
 #ifdef USE_MK3MAG
   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 - Rev 2088 → 2089