WebSVN - FlightCtrl - Diff - Rev 1634 and 1645 - /branches/dongfang_FC

 #include <avr/io.h>
 #include "eeprom.h"
 #include "flight.h"
 // Only for debug. Remove.
-#include "analog.h"
+//#include "analog.h"
-#include "rc.h"
+//#include "rc.h"
 // Necessary for external control and motor test
 #include "uart0.h"
 /*
  * These are no longer maintained, just left at 0. The original implementation just summed the acc.
  * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
  */
-int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
+// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
 // MK flags
   dynamicParams.KalmanK = -1;
   dynamicParams.KalmanMaxDrift = 0;
   dynamicParams.KalmanMaxFusion = 32;
-  controlMixer_initVariables();
-  // TODO: Move off.
-  // RC_Quality = 100;
+  controlMixer_initVariables();
+}
 /************************************************************************/
+    }
     if(motorTestActive) motorTestActive--;
+  }
   /*
-  DebugOut.Analog[12] = Motor[0].SetPoint; // Front
+  DebugOut.Analog[] = Motor[0].SetPoint; // Front
-  DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
+  DebugOut.Analog[] = Motor[1].SetPoint; // Rear
-  DebugOut.Analog[14] = Motor[3].SetPoint; // Left
+  DebugOut.Analog[] = Motor[3].SetPoint; // Left
-  DebugOut.Analog[15] = Motor[2].SetPoint; // Right
+  DebugOut.Analog[] = Motor[2].SetPoint; // Right
   */
   // Start I2C Interrupt Mode
   I2C_Start(TWI_STATE_MOTOR_TX);
 /*  Main Flight Control                                                 */
 /************************************************************************/
 void flight_control(void) {
   int16_t tmp_int;
     // Mixer Fractions that are combined for Motor Control
-  int16_t yawTerm, throttleTerm, pitchTerm, rollTerm;
+  int16_t yawTerm, throttleTerm, term[2];
   // PID controller variables
-  int16_t PDPartPitch, PDPartRoll, PDPartYaw, PPartPitch, PPartRoll;
+  int16_t PDPart[2], PDPartYaw, PPart[2];
-  static int32_t IPartPitch = 0, IPartRoll = 0;
   static int32_t IPart[2] = {0,0};
   static int32_t setPointYaw = 0;
   // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
   // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0;
-  // static int32_t CorrectionPitch, CorrectionRoll;
-  static uint16_t emergencyFlightTime;
-  // No support for altitude control right now.
-  // static uint8_t HeightControlActive = 0;
+  // static int32_t CorrectionPitch, CorrectionRoll;
   // static int16_t HeightControlGas = 0;
   static uint16_t emergencyFlightTime;
   static int8_t debugDataTimer = 1;
   // High resolution motor values for smoothing of PID motor outputs
   static int16_t motorFilters[MAX_MOTORS];
     } else
       /*
        * When standing on the ground, do not apply I controls and zero the yaw stick.
        * Probably to avoid integration effects that will cause the copter to spin
        * or flip when taking off.
-       * TODO: What was the value of IPartPitch? At 1st run of this, it's 0 already.
        */
       if(isFlying < 256) {
-        IPartPitch = 0;
+            IPart[PITCH] = IPart[ROLL] = 0;
-        IPartRoll = 0;
-        // TODO: Don't stomp on other modules' variables!!!
+            // TODO: Don't stomp on other modules' variables!!!
-        controlYaw = 0;
+            controlYaw = 0;
-        if(isFlying == 250) {
+            if(isFlying == 250) {
-          updateCompassCourse = 1;
+              updateCompassCourse = 1;
-          yawAngle = 0;
+              yawAngle = 0;
-          setPointYaw = 0;
+              setPointYaw = 0;
         }
       } else {
-        // DebugOut.Digital[1] = 0;
+            // DebugOut.Digital[1] = 0;
-        // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
+            // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
-        // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
+            // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
-        MKFlags |= (MKFLAG_FLY);
+            MKFlags |= (MKFLAG_FLY);
+      }
     /*
      * Get the current command (start/stop motors, calibrate), if any.
       updateCompass();
+      }
     */
-#if defined (USE_MK3MAG)
-      // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#if defined (USE_MK3MAG)
     /************************************************************************/
     /* GPS is currently not supported.                                      */
     /************************************************************************/
     /*
       // GPSStickPitch = 0;
       // GPSStickRoll = 0;
+      }
     */
 #endif
+#define SENSOR_LIMIT  (4096 * 4)
     /************************************************************************/
     /* Calculate control feedback from angle (gyro integral)                */
     /* and angular velocity (gyro signal)                                   */
     /************************************************************************/
+    // The P-part is the P of the PID controller. That's the angle integrals (not rates).
-    // The P-part is the P of the PID controller. That's the angle integrals (not rates).
+  for (axis=PITCH; axis<=ROLL; axis++) {
-  if(looping & LOOPING_PITCH_AXIS) {
+    if(looping & (1<<(4+axis))) {
-    PPartPitch = 0;
+      PPart[axis] = 0;
-  } else { // TODO: Where do the 44000 come from???
+    } else { // TODO: Where do the 44000 come from???
-    PPartPitch = pitchAngle * gyroIFactor / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
+      PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
-  }
+    }
+    /*
+     * Now blend in the D-part - proportional to the Differential of the integral = the rate.
+     * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
+     * where pfactor is in [0..1].
-  // Now blend in the D-part - proportional to the Differential of the integral = the rate.
+     */
-  PDPartPitch = PPartPitch + (int32_t)((int32_t)pitchRate_PID * gyroPFactor / (256L / STICK_GAIN))
+    PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING))
-    + (pitchDifferential * (int16_t)dynamicParams.GyroD) / 16;
+      + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16;
-  // The P-part is actually the I-part...
-  if(looping & LOOPING_ROLL_AXIS) {
-    PPartRoll = 0;
-  } else { // TODO: Where do the 44000 come from???
-    PPartRoll = (rollAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
+    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
-  }
-  // Now blend in the P-part - proportional to the Differential of the integral = the rate
-  PDPartRoll = PPartRoll + (int32_t)((int32_t)rollRate_PID * gyroPFactor / (256L / STICK_GAIN))
-    + (rollDifferential * (int16_t)dynamicParams.GyroD) / 16;
+  }
-  PDPartYaw =  (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / STICK_GAIN)
+  PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
-    + (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / STICK_GAIN));
-  // limit control feedback
-#define SENSOR_LIMIT  (4096 * 4)
+    + (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
   CHECK_MIN_MAX(PDPartPitch, -SENSOR_LIMIT, SENSOR_LIMIT);
-  CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
+  // limit control feedback
   CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
   /*
    * Height control was here.
    */
   if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
-  throttleTerm *= STICK_GAIN;
+  throttleTerm *= CONTROL_SCALING;
+  /*
+   * Compose yaw term.
+   * The yaw term is limited: Absolute value is max. = the throttle term / 2.
+   * However, at low throttle the yaw term is limited to a fixed value,
-  /*
+   * and at high throttle it is limited by the throttle reserve (the difference
-   * Compose yaw term.
+   * between current throttle and maximum throttle).
    */
-#define MIN_YAWGAS (40 * STICK_GAIN)  // yaw also below this gas value
+#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
-  yawTerm = PDPartYaw - setPointYaw * STICK_GAIN;
+  yawTerm = PDPartYaw - setPointYaw * CONTROL_SCALING;
-  // limit yawTerm
-  if(throttleTerm > MIN_YAWGAS) {
-    /*
-     * -throttle/2 < -20 <= yawTerm <= 20 < throttle/2
+  // limit yawTerm
-     */
+  if(throttleTerm > MIN_YAWGAS) {
-    CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
-  } else {
-    /*
-     * -20 <= yawTerm <= 20
+    CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
-     */
+  } else {
     CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
-  }
-  tmp_int = staticParams.MaxThrottle * STICK_GAIN;
   /*
-   * throttle-MaxThrottle <= yawTerm <= MaxThrottle-throttle
-   */
-  CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
-  /*
-   * Compose pitch and roll terms. This is finally where the sticks come into play.
-   */
-  if(gyroIFactor) {
-    // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
-    // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
-    // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
-    IPartPitch += PPartPitch - controlPitch; // Integrate difference between P part (the angle) and the stick pos.
-    IPartRoll += PPartRoll - controlRoll;    // I-part for attitude control OK
-  } else {
-    // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
-    IPartPitch += PDPartPitch - controlPitch; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
-    IPartRoll += PDPartRoll - controlRoll;    // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
-  }
-  // TODO: From which planet comes the 16000?
-  CHECK_MIN_MAX(IPartPitch, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
-  // Add (P, D) parts minus stick pos. to the scaled-down I part.
-  pitchTerm = PDPartPitch - controlPitch + IPartPitch / Ki;    // PID-controller for pitch
-  CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
-  rollTerm = PDPartRoll - controlRoll + IPartRoll / Ki;  // PID-controller for roll
-  /*
-   * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
    * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
+  tmp_int = staticParams.MaxThrottle * CONTROL_SCALING;
+  CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
+  tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;
+  for (axis=PITCH; axis<=ROLL; axis++) {
+    /*
+     * Compose pitch and roll terms. This is finally where the sticks come into play.
+     */
+    if(gyroIFactor) {
+      // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
+      // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
+      // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
+      IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos.
+    } else {
+      // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
+      // To keep up with a full stick PDPart should be about 156...
+      IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
+    }
+    // TODO: From which planet comes the 16000?
+    CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L));
+    // Add (P, D) parts minus stick pos. to the scaled-down I part.
+    term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki;    // PID-controller for pitch
-   * (max. pitch or roll term is the throttle value).
+    /*
+     * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
-   * TODO: Why a growing function of yaw?
+     * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
+     * (max. pitch or roll term is the throttle value).
-   */
+     * TODO: Why a growing function of yaw?
-  tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;
+     */
+    CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
   CHECK_MIN_MAX(pitchTerm, -tmp_int, tmp_int);
   CHECK_MIN_MAX(rollTerm, -tmp_int, tmp_int);
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  // Universal Mixer
-  // Universal Mixer
+  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   for(i = 0; i < MAX_MOTORS; i++) {
     int16_t tmp;
     if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered.
       tmp =  ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L;
-      tmp += ((int32_t)pitchTerm    * Mixer.Motor[i][MIX_PITCH])    / 64L;
+      tmp += ((int32_t)term[PITCH]  * Mixer.Motor[i][MIX_PITCH])    / 64L;
-      tmp += ((int32_t)rollTerm     * Mixer.Motor[i][MIX_ROLL])     / 64L;
+      tmp += ((int32_t)term[ROLL]   * Mixer.Motor[i][MIX_ROLL])     / 64L;
       tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;
       motorFilters[i] = motorFilter(tmp, motorFilters[i]);
-      tmp = motorFilters[i] / STICK_GAIN;
+      tmp = motorFilters[i] / CONTROL_SCALING;
       CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
       Motor[i].SetPoint = tmp;
+    }
     else Motor[i].SetPoint = 0;
+  }
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  //  Debugwerte zuordnen
+  // Debugging
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!(--debugDataTimer)) {
     debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
-    DebugOut.Analog[0]  = (10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
+    DebugOut.Analog[0]  = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
-    DebugOut.Analog[1]  = (10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
+    DebugOut.Analog[1]  = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
     DebugOut.Analog[2]  = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
-    DebugOut.Analog[9]  = setPointYaw;
+    // DebugOut.Analog[9]  = setPointYaw;
-    DebugOut.Analog[10] = yawIFactor;
+    // DebugOut.Analog[10] = yawIFactor;
-    DebugOut.Analog[11] = gyroIFactor;
+    // DebugOut.Analog[11] = gyroIFactor;
     // DebugOut.Analog[12] = RC_getVariable(0);
     // DebugOut.Analog[13] = dynamicParams.UserParams[0];
-    DebugOut.Analog[14] = RC_getVariable(4);
+    // DebugOut.Analog[14] = RC_getVariable(4);
-    DebugOut.Analog[15] = dynamicParams.UserParams[4];
+    // DebugOut.Analog[15] = dynamicParams.UserParams[4];
     /* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */
-    // 12..15 are the controls.
-    // DebugOut.Analog[16] = pitchAxisAcc;
-    // DebugOut.Analog[17] = rollAxisAcc;
+    // 12..15 are the controls.
+    // DebugOut.Analog[16] = pitchAxisAcc;
+    // DebugOut.Analog[17] = rollAxisAcc;
+    // DebugOut.Analog[18] = ZAxisAcc;
-    // DebugOut.Analog[18] = ZAxisAcc;
+    DebugOut.Analog[19] = throttleTerm;
-    DebugOut.Analog[19] = throttleTerm;
+    DebugOut.Analog[20] = term[PITCH];
-    DebugOut.Analog[20] = pitchTerm;
+    DebugOut.Analog[21] = term[ROLL];
-    DebugOut.Analog[21] = rollTerm;
+    DebugOut.Analog[22] = yawTerm;
     DebugOut.Analog[22] = yawTerm;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c @ 1725 - Rev 1634 → 1645