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;
  *************************************************************************/
 void getAnalogData(void) {
-  uint8_t axis;
+        uint8_t axis;
-  for (axis=PITCH; axis <=ROLL; axis++) {
+        for (axis = PITCH; axis <= ROLL; axis++) {
+                rate_PID[axis] = (gyro_PID[axis] + driftComp[axis])
-    rate_PID[axis]     = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
+                                / 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];
+        }
-  int16_t cospitch = int_cos(angle[PITCH]);
+        int16_t cospitch = int_cos(angle[PITCH]);
-  int16_t cosroll =  int_cos(angle[ROLL]);
+        int16_t cosroll = int_cos(angle[ROLL]);
-  int16_t sinroll =  int_sin(angle[ROLL]);
+        int16_t sinroll = int_sin(angle[ROLL]);
-  int16_t tanpitch = int_tan(angle[PITCH]);
+        int16_t tanpitch = int_tan(angle[PITCH]);
 #define ANTIOVF 512
-  ACRate[PITCH] =                 ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR;
+        ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
+                        * sinroll) / (int32_t) 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));
+        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 + ((int32_t)yawRate * cosroll) / cospitch;
+        ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch
+                        + ((int32_t) yawRate * cosroll) / cospitch;
+}
 // 480 usec with axis coupling - almost no time without.
 void integrate(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] <= dynamicParams.UserParams[7]) {
+        if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
+                        <= 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;
-    /*
+                /*
-     * 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] = (10 * accDerived) / 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 {
-  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
-      DebugOut.Analog[18+axis] = deltaCorrection / staticParams.GyroAccTrim;
+                                        / staticParams.GyroAccTrim;
-      DebugOut.Analog[28+axis] = driftComp[axis];
+                        DebugOut.Analog[28 + axis] = driftComp[axis];
-      correctionSum[axis] = 0;
+                        correctionSum[axis] = 0;
-  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++;
-      compassCalState++;
+                        if (compassCalState < 5)
+                                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
+                if (abs(yawRate) > 128) { // spinning fast
+                        error = 0;
+                } else {
+                        // compassHeading - yawGyroHeading, on a -180..179 deg interval.
-      error = 0;
+                        error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180;
-    }
+                }
-    if(!badCompassHeading && w < 25) {
+                if (!ignoreCompassTimer && 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 = (w * dynamicParams.CompassYawEffect) / 32;
+                w = dynamicParams.CompassYawEffect - w;
+                */
+                w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32;
-    w = dynamicParams.CompassYawEffect - w;
+                // As readable formula:
+                // w = dynamicParams.CompassYawEffect * (1-w/32);
-    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)
-        if (v > w) v = w;
+                                        v = w;
+                                else if (v < -w)
-        else if (v < -w) v = -w;
+                                        v = -w;
-        yawAngleDiff += v;
-      }
-      else
+                                yawAngleDiff += v;
-        { // wait a while
+                        } else { // wait a while
-          badCompassHeading--;
+                                ignoreCompassTimer--;
+                        }
-    } else {  // ignore compass at extreme attitudes for a while
+                } else { // ignore compass at extreme attitudes for a while
-      badCompassHeading = 500;
+                        ignoreCompassTimer = 500;
+                }
+        }
+}

Subversion Repositories FlightCtrl

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