WebSVN - FlightCtrl - Diff - Rev 1775 and 1805 - /branches/dongfang_FC

 #include "controlMixer.h"
 // For Servo_On / Off
-// #include "timer2.h"
-#ifdef USE_MK3MAG
-#include "mk3mag.h"
-#include "gps.h"
+// #include "timer2.h"
 #endif
 #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
  */
 int32_t angle[2], yawAngleDiff;
 int readingHeight = 0;
+// Yaw angle and compass stuff.
+// This is updated/written from MM3. Negative angle indicates invalid data.
+int16_t compassHeading = -1;
-// compass course
+// This is NOT updated from MM3. Negative angle indicates invalid data.
+int16_t compassCourse = -1;
-int16_t compassHeading       = -1; // negative angle indicates invalid data.
+// The difference between the above 2 (heading - course) on a -180..179 degree interval.
+// Not necessary. Never read anywhere.
-int16_t compassCourse        = -1;
+// int16_t compassOffCourse = 0;
-int16_t compassOffCourse     = 0;
-uint16_t updateCompassCourse = 0;
+uint8_t updateCompassCourse = 0;
+uint8_t compassCalState = 0;
-uint8_t compassCalState      = 0;
+uint16_t ignoreCompassTimer = 500;
 uint16_t badCompassHeading = 500;
 int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
 int16_t yawGyroDrift;
 #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
 #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
 #define YAWOVER360       (GYRO_DEG_FACTOR_YAW * 360L)
-int16_t correctionSum[2] = {0,0};
+int16_t correctionSum[2] = { 0, 0 };
 // For NaviCTRL use.
-int16_t averageAcc[2] = {0,0}, averageAccCount = 0;
+int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0;
 /*
  * Experiment: Compensating for dynamic-induced gyro biasing.
  * constant speed, and 2) at small angles a, sin(a) ~= constant * a,
  * it is hardly worth the trouble.
  ************************************************************************/
 int32_t getAngleEstimateFromAcc(uint8_t axis) {
-  return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];
+        return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
+}
 void setStaticAttitudeAngles(void) {
 #ifdef ATTITUDE_USE_ACC_SENSORS
-  angle[PITCH] = getAngleEstimateFromAcc(PITCH);
+        angle[PITCH] = getAngleEstimateFromAcc(PITCH);
-  angle[ROLL] = getAngleEstimateFromAcc(ROLL);
+        angle[ROLL] = getAngleEstimateFromAcc(ROLL);
 #else
-  angle[PITCH] = angle[ROLL] = 0;
+        angle[PITCH] = angle[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.
+        // 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_calibrate();
-  // reset gyro readings
-  // rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0;
+        dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
-  // reset gyro integrals to acc guessing
-  setStaticAttitudeAngles();
+        driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
-  yawAngleDiff = 0;
+        correctionSum[PITCH] = correctionSum[ROLL] = 0;
+        // Calibrate hardware.
+        analog_calibrate();
+        // reset gyro readings
+        // rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0;
+        // reset gyro integrals to acc guessing
+        setStaticAttitudeAngles();
+        yawAngleDiff = 0;
-  // update compass course to current heading
+        // update compass course to current heading
-  compassCourse = compassHeading;
+        compassCourse = compassHeading;
-  // Inititialize YawGyroIntegral value with current compass heading
+        // Inititialize YawGyroIntegral value with current compass heading
-  yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
+        yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
-  // Servo_On(); //enable servo output
+        // 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 values into variables).
+ * The rate variable end up in a range of about [-1024, 1023].
+ *************************************************************************/
- * The rate variable end up in a range of about [-1024, 1023].
+void getAnalogData(void) {
- *************************************************************************/
+        uint8_t axis;
-void getAnalogData(void) {
-  uint8_t axis;
+        for (axis = PITCH; axis <= ROLL; axis++) {
+                rate_PID[axis] = (gyro_PID[axis] + driftComp[axis])
-  for (axis=PITCH; axis <=ROLL; axis++) {
+                                / HIRES_GYRO_INTEGRATION_FACTOR;
-    rate_PID[axis]     = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
+                rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis])
-    rate_ATT[axis]     = (gyro_ATT[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
+                                / HIRES_GYRO_INTEGRATION_FACTOR;
-    differential[axis] = gyroD[axis];
+                differential[axis] = gyroD[axis];
-    averageAcc[axis]  += acc[axis];
+                averageAcc[axis] += acc[axis];
-  }
+        }
-  averageAccCount++;
+        averageAccCount++;
-  yawRate = yawGyro + driftCompYaw;
+        yawRate = yawGyro + driftCompYaw;
-  // We are done reading variables from the analog module.
+        // We are done reading variables from the analog module.
-  // Interrupt-driven sensor reading may restart.
+        // Interrupt-driven sensor reading may restart.
-  analogDataReady = 0;
+        analogDataReady = 0;
-  analog_start();
+        analog_start();
 }
 /*
  * 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.
- * changed accordingly.
+ */
+void trigAxisCoupling(void) {
+        int16_t cospitch = int_cos(angle[PITCH]);
+        int16_t cosroll = int_cos(angle[ROLL]);
- */
+        int16_t sinroll = int_sin(angle[ROLL]);
+        int16_t tanpitch = int_tan(angle[PITCH]);
-void trigAxisCoupling(void) {
+#define ANTIOVF 512
-  int16_t cospitch = int_cos(angle[PITCH]);
+        ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
-  int16_t cosroll =  int_cos(angle[ROLL]);
+                        * sinroll) / (int32_t) MATH_UNIT_FACTOR;
-  int16_t sinroll =  int_sin(angle[ROLL]);
+        ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll
-  int16_t tanpitch = int_tan(angle[PITCH]);
+                        / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL])
-#define ANTIOVF 512
+                        / ANTIOVF * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF
-  ACRate[PITCH] =                 ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR;
+                        * MATH_UNIT_FACTOR));
-  ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t)rate_ATT[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR));
+        ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch
-  ACYawRate =                     ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch;
+                        + ((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.
+        // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
-  uint8_t axis;
+        uint8_t axis;
-  if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) {
+        if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) {
-    // The rotary rate limiter bit is abused for selecting axis coupling algorithm instead.
+                // The rotary rate limiter bit is abused for selecting axis coupling algorithm instead.
-    trigAxisCoupling();
+                trigAxisCoupling();
-  } else {
+        } else {
-    ACRate[PITCH] = rate_ATT[PITCH];
+                ACRate[PITCH] = rate_ATT[PITCH];
-    ACRate[ROLL]  = rate_ATT[ROLL];
+                ACRate[ROLL] = rate_ATT[ROLL];
-    ACYawRate     = yawRate;
+                ACYawRate = yawRate;
-  }
+        }
-  /*
+        /*
-   * Yaw
+         * Yaw
-   * Calculate yaw gyro integral (~ to rotation angle)
+         * Calculate yaw gyro integral (~ to rotation angle)
-   * Limit yawGyroHeading proportional to 0 deg to 360 deg
+         * Limit yawGyroHeading proportional to 0 deg to 360 deg
-   */
+         */
-  yawGyroHeading += ACYawRate;
+        yawGyroHeading += ACYawRate;
-  yawAngleDiff += yawRate;
+        yawAngleDiff += yawRate;
-  if(yawGyroHeading >= YAWOVER360) {
+        if (yawGyroHeading >= YAWOVER360) {
-    yawGyroHeading -= YAWOVER360;  // 360 deg. wrap
+                yawGyroHeading -= YAWOVER360; // 360 deg. wrap
-  } else if(yawGyroHeading < 0) {
+        } else if (yawGyroHeading < 0) {
-    yawGyroHeading += YAWOVER360;
+                yawGyroHeading += YAWOVER360;
-  }
+        }
-  /*
+        /*
-   * Pitch axis integration and range boundary wrap.
+         * Pitch axis integration and range boundary wrap.
-   */
+         */
-  for (axis=PITCH; axis<=ROLL; axis++) {
+        for (axis = PITCH; axis <= ROLL; axis++) {
-    angle[axis] += ACRate[axis];
+                angle[axis] += ACRate[axis];
-    if(angle[axis] > PITCHROLLOVER180) {
+                if (angle[axis] > PITCHROLLOVER180) {
-      angle[axis] -= PITCHROLLOVER360;
+                        angle[axis] -= PITCHROLLOVER360;
-    } else if (angle[axis] <= -PITCHROLLOVER180) {
+                } else if (angle[axis] <= -PITCHROLLOVER180) {
-      angle[axis] += PITCHROLLOVER360;
+                        angle[axis] += PITCHROLLOVER360;
+                }
-  }
+        }
 }
 /************************************************************************
  * 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
- * There is (there) also a drift compensation
+ * - it corrects the differential of the integral = the gyro offsets.
- * - it corrects the differential of the integral = the gyro offsets.
+ * That should only be necessary with drifty gyros like ENC-03.
- * That should only be necessary with drifty gyros like ENC-03.
+ ************************************************************************/
- ************************************************************************/
+void correctIntegralsByAcc0thOrder(void) {
-void correctIntegralsByAcc0thOrder(void) {
+        // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
-  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
+        // are less than ....., or reintroduce Kalman.
-  // are less than ....., or reintroduce Kalman.
+        // Well actually the Z axis acc. check is not so silly.
-  // Well actually the Z axis acc. check is not so silly.
+        uint8_t axis;
-  uint8_t axis;
+        int32_t correction;
-  int32_t correction;
+        if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
-  if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) {
+                        <= dynamicParams.UserParams[7]) {
-    DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
+                DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
-    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
+                uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
-    uint8_t debugFullWeight = 1;
+                uint8_t debugFullWeight = 1;
-    int32_t accDerived;
+                int32_t accDerived;
-    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
+                if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
-      permilleAcc /= 2;
+                        permilleAcc /= 2;
-      debugFullWeight = 0;
+                        debugFullWeight = 0;
-    }
+                }
+                if (debugFullWeight)
-    if (debugFullWeight)
+                        DebugOut.Digital[1] |= DEBUG_ACC0THORDER;
-      DebugOut.Digital[1] |= DEBUG_ACC0THORDER;
+                else
-    else
+                        DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
-      DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
+                /*
+                 * 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++) {
-     */
+                        accDerived = getAngleEstimateFromAcc(axis);
-    for (axis=PITCH; axis<=ROLL; axis++) {
+                        DebugOut.Analog[9 + axis] = (10 * accDerived)
-      accDerived = getAngleEstimateFromAcc(axis);
+                                        / GYRO_DEG_FACTOR_PITCHROLL;
-      DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
+                        // 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.
+                        correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
-      correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
+                        angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis]
-      angle[axis] = ((int32_t)(1000L - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L;
+                                        + (int32_t) permilleAcc * accDerived) / 1000L;
-      correctionSum[axis] += angle[axis] - correction;
+                        correctionSum[axis] += angle[axis] - correction;
-      DebugOut.Analog[16+axis] = angle[axis] - correction;
+                        DebugOut.Analog[16 + axis] = angle[axis] - correction;
-    }
+                }
-  } else {
+        } else {
-    DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
+                DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
-    DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
+                DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
-    DebugOut.Analog[9] = 0;
+                DebugOut.Analog[9] = 0;
  * 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;
+        static int16_t timer = DRIFTCORRECTION_TIME;
-  int16_t deltaCorrection;
+        int16_t deltaCorrection;
-  uint8_t axis;
+        uint8_t axis;
-  if (! --timer) {
+        if (!--timer) {
-    timer = DRIFTCORRECTION_TIME;
+                timer = DRIFTCORRECTION_TIME;
-    for (axis=PITCH; axis<=ROLL; axis++) {
+                for (axis = PITCH; axis <= ROLL; axis++) {
-      // Take the sum of corrections applied, add it to delta
+                        // Take the sum of corrections applied, add it to delta
+                        deltaCorrection = (correctionSum[axis]
-      deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME;
+                                        * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2)
+                                        / DRIFTCORRECTION_TIME;
-      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
+                        // Add the delta to the compensation. So positive delta means, gyro should have higher value.
-      driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
+                        driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
-      CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
+                        CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
-      // DebugOut.Analog[11 + axis] = correctionSum[axis];
+                        // DebugOut.Analog[11 + axis] = correctionSum[axis];
-      DebugOut.Analog[18+axis] = deltaCorrection / staticParams.GyroAccTrim;
+                        DebugOut.Analog[18 + axis] = deltaCorrection
+                                        / staticParams.GyroAccTrim;
-      DebugOut.Analog[28+axis] = driftComp[axis];
+                        DebugOut.Analog[28 + axis] = driftComp[axis];
-      correctionSum[axis] = 0;
+                        correctionSum[axis] = 0;
-    }
+                }
-  }
+        }
+}
 /************************************************************************
  * Main procedure.
  ************************************************************************/
 void calculateFlightAttitude(void) {
-  // part1: 550 usec.
+        // part1: 550 usec.
-  // part1a: 550 usec.
+        // part1a: 550 usec.
-  // part1b: 60 usec.
+        // part1b: 60 usec.
-  getAnalogData();
+        getAnalogData();
-  // end part1b
+        // end part1b
-  integrate();
+        integrate();
-  // end part1a
+        // end part1a
-  DebugOut.Analog[6] = ACRate[PITCH];
+        DebugOut.Analog[6] = ACRate[PITCH];
-  DebugOut.Analog[7] = ACRate[ROLL];
+        DebugOut.Analog[7] = ACRate[ROLL];
-  DebugOut.Analog[8] = ACYawRate;
+        DebugOut.Analog[8] = ACYawRate;
-  DebugOut.Analog[3] = rate_PID[PITCH];
+        DebugOut.Analog[3] = rate_PID[PITCH];
-  DebugOut.Analog[4] = rate_PID[ROLL];
+        DebugOut.Analog[4] = rate_PID[ROLL];
-  DebugOut.Analog[5] = yawRate;
+        DebugOut.Analog[5] = yawRate;
 #ifdef ATTITUDE_USE_ACC_SENSORS
-  correctIntegralsByAcc0thOrder();
+        correctIntegralsByAcc0thOrder();
-  driftCorrection();
+        driftCorrection();
 #endif
-  // end part1
+        // end part1
+}
 void updateCompass(void) {
-  int16_t w, v, r,correction, error;
+        int16_t w, v, r, correction, error;
-  if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
+        if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
+                if (controlMixer_testCompassCalState()) {
-    if (controlMixer_testCompassCalState()) {
+                        compassCalState++;
+                        if (compassCalState < 5)
-      compassCalState++;
+                                beepNumber(compassCalState);
-      if(compassCalState < 5) beepNumber(compassCalState);
+                        else
-      else beep(1000);
+                                beep(1000);
-    }
+                }
-  } else {
+        } else {
-    // get maximum attitude angle
+                // get maximum attitude angle
-    w = abs(angle[PITCH] / 512);
+                w = abs(angle[PITCH] / 512);
+                v = abs(angle[ROLL] / 512);
-    v = abs(angle[ROLL]  / 512);
+                if (v > w)
-    if(v > w) w = v;
+                        w = v;
+                correction = w/8 + 1;
-    correction = w / 8 + 1;
+                // calculate the deviation of the yaw gyro heading and the compass heading
-    // calculate the deviation of the yaw gyro heading and the compass heading
+                if (compassHeading < 0)
-    if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
+                        error = 0; // disable yaw drift compensation if compass heading is undefined
-    else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180;
+                else
+                if (abs(yawRate) > 128) { // spinning fast
+                        error = 0;
+                } else {
-    if(abs(yawRate) > 128) { // spinning fast
+                        // compassHeading - yawGyroHeading, on a -180..179 deg interval.
-      error = 0;
+                        error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180;
-    }
+                }
+                if (!ignoreCompassTimer && w < 25) {
-    if(!badCompassHeading && w < 25) {
+                        yawGyroDrift += error;
-      yawGyroDrift += error;
+                        // Basically this gets set if we are in "fix" mode, and when starting.
-      if(updateCompassCourse) {
+                        if (updateCompassCourse) {
-        beep(200);
+                                beep(200);
-        yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
+                                yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
-        compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
+                                compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
-        updateCompassCourse = 0;
+                                updateCompassCourse = 0;
-      }
+                        }
+                }
+                yawGyroHeading += (error * 8) / correction;
-    }
-    yawGyroHeading += (error * 8) / correction;
+                /*
+                w = (w * dynamicParams.CompassYawEffect) / 32;
+                w = dynamicParams.CompassYawEffect - w;
+                */
-    w = (w * dynamicParams.CompassYawEffect) / 32;
+                w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32;
+                // As readable formula:
+                // w = dynamicParams.CompassYawEffect * (1-w/32);
-    w = dynamicParams.CompassYawEffect - w;
-    if(w >= 0) {
+                if (w >= 0) { // maxAttitudeAngle < 32
-      if(!badCompassHeading) {
+                        if (!ignoreCompassTimer) {
-        v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;
+                                v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;
-        // calc course deviation
+                                // yawGyroHeading - compassCourse on a -180..179 degree interval.
-        r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180;
+                                r = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) % 360) - 180;
-        v = (r * w) / v; // align to compass course
+                                v = (r * w) / v; // align to compass course
-        // limit yaw rate
+                                // limit yaw rate
+                                w = 3 * dynamicParams.CompassYawEffect;
-        w = 3 * dynamicParams.CompassYawEffect;
+                                if (v > w)
+                                        v = w;
-        if (v > w) v = w;
+                                else if (v < -w)
-        else if (v < -w) v = -w;
+                                        v = -w;
+                                yawAngleDiff += v;
-        yawAngleDiff += v;
+                        } else { // wait a while
-      }
+                                ignoreCompassTimer--;
-      else
+                        }
+                } else { // ignore compass at extreme attitudes for a while
-        { // wait a while
+                        ignoreCompassTimer = 500;
-          badCompassHeading--;
-        }
-    } else {  // ignore compass at extreme attitudes for a while
-      badCompassHeading = 500;
+                }
+        }
+}
  * 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) {
+ void attitude_startDynamicCalibration(void) {
-  dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0;
+ dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0;
-  savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000;
+ savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000;
-  }
+ }
-  void attitude_continueDynamicCalibration(void) {
+ void attitude_continueDynamicCalibration(void) {
-  // measure dynamic offset now...
+ // measure dynamic offset now...
-  dynamicCalPitch += hiResPitchGyro;
+ dynamicCalPitch += hiResPitchGyro;
-  dynamicCalRoll += hiResRollGyro;
+ dynamicCalRoll += hiResRollGyro;
-  dynamicCalYaw += rawYawGyroSum;
+ dynamicCalYaw += rawYawGyroSum;
-  dynamicCalCount++;
+ dynamicCalCount++;
-  // Param6: Manual mode. The offsets are taken from Param7 and Param8.
+ // Param6: Manual mode. The offsets are taken from Param7 and Param8.
-  if (dynamicParams.UserParam6 || 1) { // currently always enabled.
+ if (dynamicParams.UserParam6 || 1) { // currently always enabled.
-  // manual mode
+ // manual mode
-  driftCompPitch = dynamicParams.UserParam7 - 128;
+ driftCompPitch = dynamicParams.UserParam7 - 128;
-  driftCompRoll = dynamicParams.UserParam8 - 128;
+ driftCompRoll = dynamicParams.UserParam8 - 128;
-  } else {
+ } else {
-  // use the sampled value (does not seem to work so well....)
+ // use the sampled value (does not seem to work so well....)
-  driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
+ driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
-  driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
+ driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
-  driftCompYaw = -dynamicCalYaw / dynamicCalCount;
+ driftCompYaw = -dynamicCalYaw / dynamicCalCount;
-  }
+ }
-  // keep resetting these meanwhile, to avoid accumulating errors.
+ // keep resetting these meanwhile, to avoid accumulating errors.
-  setStaticAttitudeIntegrals();
+ setStaticAttitudeIntegrals();
-  yawAngle = 0;
+ yawAngle = 0;
-  }
+ }
-*/
+ */

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c @ 1816 - Rev 1775 → 1805