WebSVN - FlightCtrl - Diff - Rev 2047 and 2048 - /branches/dongfang_FC

-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Copyright (c) 04.2007 Holger Buss
-// + Nur f�r den privaten Gebrauch
-// + www.MikroKopter.com
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
-// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist.
-// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
-// + bzgl. der Nutzungsbedingungen aufzunehmen.
-// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
-// + Verkauf von Luftbildaufnahmen, usw.
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
-// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
-// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
-// + eindeutig als Ursprung verlinkt und genannt werden
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
-// + Benutzung auf eigene Gefahr
-// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-// + mit unserer Zustimmung zul�ssig
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
-// + this list of conditions and the following disclaimer.
-// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
-// +     from this software without specific prior written permission.
-// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
-// +     for non-commercial use (directly or indirectly)
-// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
-// +     with our written permission
-// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
-// +     clearly linked as origin
-// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
-// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// +  POSSIBILITY OF SUCH DAMAGE.
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-/************************************************************************/
-/* Flight Attitude                                                      */
-/************************************************************************/
 #include <stdlib.h>
 #include <avr/io.h>
 #include "attitude.h"
+#include "dongfangMath.h"
-#include "dongfangMath.h"
+#include "commands.h"
 // For scope debugging only!
 /*
  * Gyro integrals. These are the rotation angles of the airframe compared to the
  * horizontal plane, yaw relative to yaw at start.
  */
-int32_t angle[2], yawAngleDiff;
+int32_t attitude[2];
-int readingHeight = 0;
+//int readingHeight = 0;
 // Yaw angle and compass stuff.
+// This is updated/written from MM3. Negative angle indicates invalid data.
+int16_t magneticHeading = -1;
 // This is updated/written from MM3. Negative angle indicates invalid data.
-int16_t magneticHeading = -1;
+// This is NOT updated from MM3. Negative angle indicates invalid data.
 // int16_t headingInDegrees = -1;
-// This is NOT updated from MM3. Negative angle indicates invalid data.
-int16_t compassCourse = -1;
-// The difference between the above 2 (heading - course) on a -180..179 degree interval.
+int32_t targetHeading;
 // Not necessary. Never read anywhere.
-// int16_t compassOffCourse = 0;
-uint8_t updateCompassCourse = 0;
-uint8_t compassCalState = 0;
-uint16_t ignoreCompassTimer = 500;
+// The difference between the above 2 (heading - course) on a -180..179 degree interval.
 // Not necessary. Never read anywhere.
-int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
+// int16_t compassOffCourse = 0;
  * 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 (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm;
   // return 342L * filteredAcc[axis];
+}
 void setStaticAttitudeAngles(void) {
 #ifdef ATTITUDE_USE_ACC_SENSORS
-  angle[PITCH] = getAngleEstimateFromAcc(PITCH);
+  attitude[PITCH] = getAngleEstimateFromAcc(PITCH);
-  angle[ROLL] = getAngleEstimateFromAcc(ROLL);
+  attitude[ROLL] = getAngleEstimateFromAcc(ROLL);
 #else
-  angle[PITCH] = angle[ROLL] = 0;
+  attitude[PITCH] = attitude[ROLL] = 0;
 #endif
+}
   // Calibrate hardware.
   analog_setNeutral();
   // reset gyro integrals to acc guessing
-  setStaticAttitudeAngles();
-  yawAngleDiff = 0;
-  // update compass course to current heading
+  setStaticAttitudeAngles();
-  compassCourse = magneticHeading;
-  // Inititialize YawGyroIntegral value with current compass heading
-  yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
   attitude_resetHeadingToMagnetic();
   // Servo_On(); //enable servo output
+}
  * (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 = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
+  int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
-  int16_t pitchAngleInDegrees = angle[PITCH] / 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[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))
-  ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll
+          >> LOG_MATH_UNIT_FACTOR);
-      + (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;
-  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
+  ACYawRate =
       ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
-  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
+          / cospitch;
+}
+  }
   /*
    * Yaw
    * Calculate yaw gyro integral (~ to rotation angle)
-   * Limit yawGyroHeading proportional to 0 deg to 360 deg
+   * Limit heading proportional to 0 deg to 360 deg
    */
-  yawGyroHeading += ACYawRate;
-  yawAngleDiff += yawRate;
+  heading += ACYawRate;
-  if (yawGyroHeading >= YAWOVER360) {
-    yawGyroHeading -= YAWOVER360; // 360 deg. wrap
-  } else if (yawGyroHeading < 0) {
-    yawGyroHeading += YAWOVER360;
-  }
   intervalWrap(&heading, YAWOVER360);
   /*
    * Pitch axis integration and range boundary wrap.
    */
   for (axis = PITCH; axis <= ROLL; axis++) {
-    angle[axis] += ACRate[axis];
+    attitude[axis] += ACRate[axis];
-    if (angle[axis] > PITCHROLLOVER180) {
+    if (attitude[axis] > PITCHROLLOVER180) {
-      angle[axis] -= PITCHROLLOVER360;
+      attitude[axis] -= PITCHROLLOVER360;
-    } else if (angle[axis] <= -PITCHROLLOVER180) {
+    } else if (attitude[axis] <= -PITCHROLLOVER180) {
-      angle[axis] += PITCHROLLOVER360;
+      attitude[axis] += PITCHROLLOVER360;
+    }
+  }
+}
   int32_t temp;
   uint8_t ca = controlActivity >> 8;
   uint8_t highControlActivity = (ca > staticParams.maxControlActivity);
-        if (highControlActivity) {
+  if (highControlActivity) {
-      debugOut.digital[1] |= DEBUG_ACC0THORDER;
+    debugOut.digital[1] |= DEBUG_ACC0THORDER;
-        } else {
+  } else {
-          debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
+    debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
-        }
+  }
   if (accVector <= dynamicParams.maxAccVector) {
-    debugOut.digital[0] &= ~ DEBUG_ACC0THORDER;
+    debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
     uint8_t permilleAcc = staticParams.zerothOrderCorrection;
     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. 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
       permilleAcc /= 4;
-      if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity
+      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 = angle[axis];
+      temp = attitude[axis];
-      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
+      attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
           + (int32_t) permilleAcc * accDerived) / 1000L;
   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)
+      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];
       // debugOut.analog[28 + axis] = driftComp[axis];
       correctionSum[axis] = 0;
+    }
+  }
+}
 void calculateAccVector(void) {
-        int16_t temp;
+  int16_t temp;
-        temp = filteredAcc[0] >> 3;
+  temp = filteredAcc[0] >> 3;
-        accVector = temp * temp;
+  accVector = temp * temp;
-        temp = filteredAcc[1] >> 3;
+  temp = filteredAcc[1] >> 3;
-        accVector += temp * temp;
+  accVector += temp * temp;
-        temp = filteredAcc[2] >> 3;
+  temp = filteredAcc[2] >> 3;
+  accVector += temp * temp;
+  //debugOut.analog[18] = accVector;
+}
+void attitude_resetHeadingToMagnetic(void) {
+  if (commands_isCalibratingCompass())
+    return;
+  // Compass is off, skip.
+  if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
+      return;
+  // Compass is invalid, skip.
+  if (magneticHeading < 0)
+    return;
+  heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
+  targetHeading = heading;
+  debugOut.digital[0] ^= DEBUG_COMPASS;
+}
+void correctHeadingToMagnetic(void) {
+  int32_t error;
+  debugOut.analog[27] = heading;
+  if (commands_isCalibratingCompass())
+    return;
+  // Compass is off, skip.
+  // Naaah this is assumed.
+  // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
+  //     return;
+  // Compass is invalid, skip.
+  if (magneticHeading < 0)
+    return;
+  // Spinning fast, skip
+  if (abs(yawRate) > 128)
+    return;
+  // Otherwise invalidated, skip
+  if (ignoreCompassTimer) {
+    ignoreCompassTimer--;
+    return;
+  }
+  // TODO: Find computational cost of this.
+  error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW;
+  // 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 * (int32_t)dynamicParams.compassYawEffect) >> 8;
+  // 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
+  // could have effect on control elsewhere, like in compassControl.c .
+  heading += correction;
+  intervalWrap(&heading, YAWOVER360);
+  targetHeading += correction;
+  intervalWrap(&targetHeading, YAWOVER360);
         accVector += temp * temp;
-        //debugOut.analog[18] = accVector;
+  debugOut.digital[1] ^= DEBUG_COMPASS;
+}
 #endif
   // We are done reading variables from the analog module.
   // Interrupt-driven sensor reading may restart.
-  startAnalogConversionCycle();
-}
-void updateCompass(void) {
-  int16_t w, v, r, correction, error;
+  startAnalogConversionCycle();
-  if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
-    if (controlMixer_testCompassCalState()) {
-      compassCalState++;
-      if (compassCalState < 5)
-        beepNumber(compassCalState);
-      else
-        beep(1000);
-    }
-  } else {
-    // get maximum attitude angle
-    w = abs(angle[PITCH] / 512);
-    v = abs(angle[ROLL] / 512);
-    if (v > w)
-      w = v;
-    correction = w / 8 + 1;
-    // calculate the deviation of the yaw gyro heading and the compass heading
-    if (magneticHeading < 0)
-      error = 0; // disable yaw drift compensation if compass heading is undefined
-    else if (abs(yawRate) > 128) { // spinning fast
-      error = 0;
-    } else {
-      // compassHeading - yawGyroHeading, on a -180..179 deg interval.
-      error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW))
-          % 360) - 180;
-    }
-    if (!ignoreCompassTimer && w < 25) {
-      yawGyroDrift += error;
-      // Basically this gets set if we are in "fix" mode, and when starting.
-      if (updateCompassCourse) {
-        beep(200);
-        yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
-        compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
-        updateCompassCourse = 0;
-      }
-    }
-    yawGyroHeading += (error * 8) / correction;
-    /*
-     w = (w * dynamicParams.CompassYawEffect) / 32;
-     w = dynamicParams.CompassYawEffect - w;
-     */
-    w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect)
-        / 32;
-    // As readable formula:
-    // w = dynamicParams.CompassYawEffect * (1-w/32);
-    if (w >= 0) { // maxAttitudeAngle < 32
-      if (!ignoreCompassTimer) {
-        /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/
-        v = 64 + controlActivity / 100;
-        // yawGyroHeading - compassCourse on a -180..179 degree interval.
-        r
-            = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse)
-                % 360) - 180;
-        v = (r * w) / v; // align to compass course
-        // limit yaw rate
-        w = 3 * dynamicParams.compassYawEffect;
-        if (v > w)
-          v = w;
-        else if (v < -w)
-          v = -w;
-        yawAngleDiff += v;
-      } else { // wait a while
-        ignoreCompassTimer--;
-      }
-    } else { // ignore compass at extreme attitudes for a while
-      ignoreCompassTimer = 500;
+  if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
     correctHeadingToMagnetic();
+  }
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c - Rev 2047 → 2048