WebSVN - FlightCtrl - Diff - Rev 2024 and 2099 - /branches/dongfang_FC

-/************************************************************************/
-/* Flight Attitude                                                      */
-/************************************************************************/
 #include <stdlib.h>
 #include <avr/io.h>
+#include <stdlib.h>
 #include "attitude.h"
+#include "dongfangMath.h"
-#include "dongfangMath.h"
+#include "commands.h"
 // For scope debugging only!
 #include "output.h"
 // Some calculations are performed depending on some stick related things.
-#include "controlMixer.h"
-// For Servo_On / Off
-// #include "timer2.h"
+#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;
+int16_t rate_ATT[3];
 // With different (less) filtering
-int16_t rate_PID[2];
+int16_t rate_PID[3];
-int16_t differential[3];
-/*
- * Gyro integrals. These are the rotation angles of the airframe compared to the
- * horizontal plane, yaw relative to yaw at start. Not really used for anything else
- * than diagnostics.
- */
-int32_t angle[3];
+int16_t differential[3];
-/*
- * Error integrals. Stick is always positive. Gyro is configurable positive or negative.
- * These represent the deviation of the attitude angle from the desired on each axis.
- */
-int32_t error[3];
-int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
 /*
-int16_t correctionSum[2] = { 0, 0 };
+ * Gyro integrals. These are the rotation angles of the airframe compared to the
  * horizontal plane, yaw relative to yaw at start.
-// For NaviCTRL use.
+ */
-int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0;
+int32_t attitude[3];
 /*
  * Experiment: Compensating for dynamic-induced gyro biasing.
  * 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) {
-  return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
+  return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
+}
+*/
-void setStaticAttitudeAngles(void) {
-#ifdef ATTITUDE_USE_ACC_SENSORS
-  angle[PITCH] = getAngleEstimateFromAcc(PITCH);
-  angle[ROLL] = getAngleEstimateFromAcc(ROLL);
-#else
+void setStaticAttitudeAngles(void) {
-  angle[PITCH] = angle[ROLL] = 0;
-#endif
+  attitude[PITCH] = attitude[ROLL] = 0;
+}
 /************************************************************************
  * Neutral Readings
- ************************************************************************/
-void attitude_setNeutral(void) {
-  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
-  driftComp[PITCH] = driftComp[ROLL];
-  correctionSum[PITCH] = correctionSum[ROLL] = 0;
+ ************************************************************************/
+void attitude_setNeutral(void) {
-  // Calibrate hardware.
+    // Calibrate hardware.
-  analog_calibrate();
+  analog_setNeutral();
-  // reset gyro integrals to acc guessing
-  setStaticAttitudeAngles();
-  // Inititialize YawGyroIntegral value with current compass heading
   angle[YAW] = 0;
   // reset gyro integrals to acc guessing
-  // Servo_On(); //enable servo output
+  setStaticAttitudeAngles();
+}
  * 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++) {
+  for (axis = PITCH; axis <= YAW; axis++) {
-    rate_PID[axis] = gyro_PID[axis] + driftComp[axis];
+    rate_PID[axis] = gyro_PID[axis];
-    rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];
+    rate_ATT[axis] = gyro_ATT[axis];
-    differential[axis] = gyroD[axis];
-    averageAcc[axis] += acc[axis];
+    differential[axis] = gyroD[axis];
-  }
-  differential[YAW] = gyroD[YAW];
-  averageAccCount++;
-  yawRate = yawGyro + driftCompYaw;
-  // We are done reading variables from the analog module.
-  // Interrupt-driven sensor reading may restart.
-  analogDataReady = 0;
   analog_start();
+}
 void integrate(void) {
-  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
-  uint8_t axis;
-    // TODO: Multiply on a factor on control. Wont work without...
-  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
-    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
-    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.servoDirections & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
-    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.servoDirections & 4 ? yawRate : -yawRate);
-    angle[PITCH] += rate_ATT[PITCH];
-    angle[ROLL]  += rate_ATT[ROLL];
-    angle[YAW] += yawRate;
-} else {
-    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
-    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
-    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? yawRate : -yawRate);
-    angle[PITCH] += rate_ATT[PITCH];
+  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
-    angle[ROLL]  += rate_ATT[ROLL];
-    angle[YAW] += yawRate;
+  uint8_t axis;
-  }
-// TODO: Configurable.
-#define ERRORLIMIT 1000
-for (axis=PITCH; axis<=YAW; axis++) {
-  if (error[axis] > ERRORLIMIT) {
+  /*
-        error[axis] = ERRORLIMIT;
+   * Yaw
-    } else if (angle[axis] <= -ERRORLIMIT) {
-        angle[axis] = -ERRORLIMIT;
    * Calculate yaw gyro integral (~ to rotation angle)
    * Limit heading proportional to 0 deg to 360 deg
    */
-  /*
-   * Pitch axis integration and range boundary wrap.
-   */
-  for (axis = PITCH; axis <= ROLL; axis++) {
-    angle[axis] += rate_ATT[axis];
-    if (angle[axis] > PITCHROLLOVER180) {
-      angle[axis] -= PITCHROLLOVER360;
-    } else if (angle[axis] <= -PITCHROLLOVER180) {
-      angle[axis] += PITCHROLLOVER360;
-    }
-  }
-    /*
-     * Yaw
-     * Calculate yaw gyro integral (~ to rotation angle)
-     * Limit yawGyroHeading proportional to 0 deg to 360 deg
-     */
-    if (angle[YAW] >= YAWOVER360) {
-        angle[YAW] -= YAWOVER360; // 360 deg. wrap
-    } else if (angle[YAW] < 0) {
-        angle[YAW] += YAWOVER360;
-    }
-}
-/************************************************************************
- * 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.
- ************************************************************************/
-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;
-  if (acc[Z] >= -staticParams.zerothOrderGyroCorrectionZAccLimit && acc[Z]
-      <= dynamicParams.UserParams[7]) {
-    DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
-    uint8_t permilleAcc = staticParams.zerothOrderGyroCorrectionFactorx1000;
-    uint8_t debugFullWeight = 1;
-    int32_t accDerived;
-    if ((control[YAW] < -64) || (control[YAW] > 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 (debugFullWeight)
-      DebugOut.Digital[1] |= DEBUG_ACC0THORDER;
-    else
-      DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
+  /*
-    /*
-     * 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] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
-      // 1000 * the correction amount that will be added to the gyro angle in next line.
+   * Pitch axis integration and range boundary wrap.
-      temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
-      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
-          + (int32_t) permilleAcc * accDerived) / 1000L;
-      correctionSum[axis] += angle[axis] - temp;
-    }
-  } else {
-    DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
-    DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
-    // DebugOut.Analog[9] = 0;
-    // DebugOut.Analog[10] = 0;
-    DebugOut.Analog[16] = 0;
-    DebugOut.Analog[17] = 0;
-    // experiment: Kill drift compensation updates when not flying smooth.
-    correctionSum[PITCH] = correctionSum[ROLL] = 0;
-  }
-}
-/************************************************************************
- * 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.secondOrderGyroCorrectionDivisor;
+  for (axis = PITCH; axis <= YAW; axis++) {
-      CHECK_MIN_MAX(driftComp[axis], -staticParams.secondOrderGyroCorrectionLimit, staticParams.secondOrderGyroCorrectionLimit);
-      // DebugOut.Analog[11 + axis] = correctionSum[axis];
+    attitude[axis] += rate_ATT[axis];
-      DebugOut.Analog[16 + axis] = correctionSum[axis];
+    if (attitude[axis] > OVER180) {
-      DebugOut.Analog[28 + axis] = driftComp[axis];
+      attitude[axis] -= OVER360;
     } else if (attitude[axis] <= -OVER180) {
-      correctionSum[axis] = 0;
+      attitude[axis] += OVER360;
  ************************************************************************/
 void calculateFlightAttitude(void) {
   getAnalogData();
   integrate();
-  DebugOut.Analog[3] = rate_PID[PITCH];
-  DebugOut.Analog[4] = rate_PID[ROLL];
-  DebugOut.Analog[5] = yawRate;
-#ifdef ATTITUDE_USE_ACC_SENSORS
+  // We are done reading variables from the analog module.
-  correctIntegralsByAcc0thOrder();
+  // Interrupt-driven sensor reading may restart.
-  driftCorrection();
-#endif
+  startAnalogConversionCycle();

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/attitude.c - Rev 2024 → 2099