WebSVN - FlightCtrl - Diff - Rev 2164 and 2189 - /branches/dongfang_FC

 #include <stdlib.h>
 #include <avr/io.h>
+#include <math.h>
-#include "eeprom.h"
+//#include "eeprom.h"
 #include "flight.h"
 #include "output.h"
-#include "uart0.h"
+//#include "uart0.h"
 // Necessary for external control and motor test
-#include "twimaster.h"
+//#include "twimaster.h"
+#include "attitude.h"
-#include "attitude.h"
+#include "analog.h"
 #include "controlMixer.h"
 #include "commands.h"
+#include "heightControl.h"
+#include "definitions.h"
-#include "heightControl.h"
+#include "debug.h"
 #ifdef USE_MK3MAG
 #include "mk3mag.h"
-#include "compassControl.h"
-#endif
-#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
-/*
- * 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;
+#include "compassControl.h"
-uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
+#endif
+int16_t targetHeading;
-uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
+int32_t IPart[2];
-uint8_t ki;
+FlightMode_t flight_flightMode = FM_UNINITALIZED;
-int32_t IPart[2];
+int16_t minThrottle, maxThrottle;
+/************************************************************************/
-/************************************************************************/
+/*  Filter for motor value smoothing (necessary???)                     */
-/*  Filter for motor value smoothing (necessary???)                     */
+/************************************************************************/
-/************************************************************************/
+/*
-int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
+ int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
-  switch (staticParams.motorSmoothing) {
+ switch (staticParams.motorSmoothing) {
-  case 0:
+ case 0:
-    return newvalue;
+ return newvalue;
-  case 1:
+ case 1:
-    return (oldvalue + newvalue) / 2;
+ return (oldvalue + newvalue) / 2;
-  case 2:
+ case 2:
-    if (newvalue > oldvalue)
+ if (newvalue > oldvalue)
-      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
+ return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
-    else
+ else
-      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
+ return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
-  case 3:
+ case 3:
-    if (newvalue < oldvalue)
+ if (newvalue < oldvalue)
-      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
+ return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
+ else
+ return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
+ default:
+ return newvalue;
+ }
+ }
+ */
+void resetIParts(void) {
+  IPart[X] = IPart[Y] = 0;
+}
-    else
+void flight_setMode(FlightMode_t _flightMode) {
-      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
+  if (flight_flightMode != _flightMode) {
+    resetIParts();
-  default:
+    flight_flightMode = _flightMode;
+  }
+}
+// To be called only when necessary.
+void flight_setParameters(void) {
+  minThrottle = staticParams.minThrottle << LOG_CONTROL_BYTE_SCALING;
-    return newvalue;
+  maxThrottle = staticParams.maxThrottle << LOG_CONTROL_BYTE_SCALING;
-  }
+}
-}
+// A heuristic when the yaw attitude cannot be used for yaw control because the airframe is near-vertical or inverted.
-void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,
+// Fum of abs(pitch) and abs(roll) greater than 75 degrees or about 1.3 radians.
-    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
+uint8_t isHeadingUnaccountable(void) {
+  int16_t x = attitude[X];
-  ki = _ki;
+  if (x<0) x = -x;
-  gyroPFactor = _gyroPFactor;
+  int16_t y = attitude[Y];
-  gyroIFactor = _gyroIFactor;
+  if (y<0) y = -y;
-  yawPFactor = _yawPFactor;
+  int32_t result = (int32_t)x + y;
-  yawIFactor = _yawIFactor;
   return result >= (long)(1.3 * INT16DEG_PI_FACTOR);
+}
 void flight_setGround() {
-  // Just reset all I terms.
+void flight_setGround(void) {
-  IPart[PITCH] = IPart[ROLL] = 0;
+  resetIParts();
-  headingError = 0;
+  targetHeading = attitude[Z];
+}
-void flight_takeOff() {
+void flight_takeOff(void) {
   // This is for GPS module to mark home position.
   // TODO: What a disgrace, change it.
-  MKFlags |= MKFLAG_CALIBRATE;
+  // MKFlags |= MKFLAG_CALIBRATE;
   HC_setGround();
 #ifdef USE_MK3MAG
   attitude_resetHeadingToMagnetic();
-  compass_setTakeoffHeading(heading);
-#endif
-}
   compass_setTakeoffHeading(attitude[YAW]);
-/************************************************************************/
+#endif
-/*  Main Flight Control                                                 */
 /************************************************************************/
-void flight_control(void) {
-  int16_t tmp_int;
-  // Mixer Fractions that are combined for Motor Control
-  int16_t yawTerm, throttleTerm, term[2];
+/************************************************************************/
+/*  Main Flight Control                                                 */
-  // PID controller variables
+/************************************************************************/
-  int16_t PDPart;
+void flight_control(void) {
   static int8_t debugDataTimer = 1;
   // PID controller variables
-  // High resolution motor values for smoothing of PID motor outputs
+  float PID;
    * Probably to avoid integration effects that will cause the copter to spin
    * or flip when taking off.
    */
   if (isFlying < 256) {
     flight_setGround();
-    if (isFlying == 250)
+    if (isFlying == 255)
       flight_takeOff();
+  }
   // This check removed. Is done on a per-motor basis, after output matrix multiplication.
-  if (throttleTerm < staticParams.minThrottle + 10)
+  if (throttleTerm < minThrottle)
-    throttleTerm = staticParams.minThrottle + 10;
+    throttleTerm = minThrottle;
-  else if (throttleTerm > staticParams.maxThrottle - 20)
+  else if (throttleTerm > maxThrottle)
-    throttleTerm = (staticParams.maxThrottle - 20);
+    throttleTerm = maxThrottle;
+  // This is where control affects the target heading. It also (later) directly controls yaw.
-  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
+  targetHeading -= ((int32_t)controls[CONTROL_YAW] * YAW_STICK_INTEGRATION_SCALER_LSHIFT16) >> 16;
-  throttleTerm *= CONTROL_SCALING;
+  int16_t headingError;
-// end part 1: 750-800 usec.
+  if (isHeadingUnaccountable()) {
-// start part 3: 350 - 400 usec.
+    // inverted flight. Attitude[Z] is off by PI and we should react in the oppisite direction!
+    debugOut.digital[0] |= DEBUG_INVERTED;
+    headingError = 0;
-#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
+  } else {
-// This is where control affects the target heading. It also (later) directly controls yaw.
+    debugOut.digital[0] &= ~DEBUG_INVERTED;
+    headingError = attitude[Z] - targetHeading;
+  }
-  headingError -= controls[CONTROL_YAW];
   // Ahaa. Wait. Here is pretty much same check.
+  if (headingError < -YAW_I_LIMIT) {
-  if (headingError < -YAW_I_LIMIT)
+    headingError = -YAW_I_LIMIT;
-    headingError = -YAW_I_LIMIT;
+    targetHeading = attitude[Z] + YAW_I_LIMIT;
-  else if (headingError > YAW_I_LIMIT)
-    headingError = YAW_I_LIMIT;
-  PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
-// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
-  PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
-  // Lets not limit P and D.
-// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
-  /*
-   * 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
-   * between current throttle and maximum throttle).
-   */
-#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
-  yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
-// Limit yawTerm
-  debugOut.digital[0] &= ~DEBUG_CLIP;
-  if (throttleTerm > MIN_YAWGAS) {
-    if (yawTerm < -throttleTerm / 2) {
-      debugOut.digital[0] |= DEBUG_CLIP;
-      yawTerm = -throttleTerm / 2;
+  } else if (headingError > YAW_I_LIMIT) {
-    } else if (yawTerm > throttleTerm / 2) {
-      debugOut.digital[0] |= DEBUG_CLIP;
-      yawTerm = throttleTerm / 2;
-    }
-  } else {
-    if (yawTerm < -MIN_YAWGAS / 2) {
-      debugOut.digital[0] |= DEBUG_CLIP;
-      yawTerm = -MIN_YAWGAS / 2;
-    } else if (yawTerm > MIN_YAWGAS / 2) {
-      debugOut.digital[0] |= DEBUG_CLIP;
-      yawTerm = MIN_YAWGAS / 2;
+    headingError = YAW_I_LIMIT;
+    targetHeading = attitude[Z] - YAW_I_LIMIT;
+  }
+  //debugOut.analog[24] = targetHeading;
+  //debugOut.analog[25] = attitude[Z];
+  //debugOut.analog[26] = headingError;
+  //debugOut.analog[27] = positiveDynamic;
-    }
+  //debugOut.analog[28] = negativeDynamic;
-  }
-  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
+  // Yaw P part.
-  if (yawTerm < -(tmp_int - throttleTerm)) {
-    yawTerm = -(tmp_int - throttleTerm);
-    debugOut.digital[0] |= DEBUG_CLIP;
+  PID = ((int32_t)gyro_control[Z] * YAW_RATE_SCALER_LSHIFT16 * dynamicParams.yawGyroP) >> 16;
-  } else if (yawTerm > (tmp_int - throttleTerm)) {
-    yawTerm = (tmp_int - throttleTerm);
-    debugOut.digital[0] |= DEBUG_CLIP;
+  if (flight_flightMode != FM_RATE) {
-  }
     PID += ((int32_t)headingError * ATT_P_SCALER_LSHIFT16 * dynamicParams.yawGyroI) >> 16;
   debugOut.digital[1] &= ~DEBUG_CLIP;
-  tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
+  yawTerm = PID + controls[CONTROL_YAW];
-  //tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
+  // yawTerm = limitYawToThrottle(yawTerm);
-  /************************************************************************/
-  /* Calculate control feedback from angle (gyro integral)                */
-  /* and angular velocity (gyro signal)                                   */
+  /************************************************************************/
-  /************************************************************************/
+  /* Calculate control feedback from angle (gyro integral)                */
-  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
+  /* and angular velocity (gyro signal)                                   */
-  for (axis = PITCH; axis <= ROLL; axis++) {
+  /************************************************************************/
-    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
+  for (uint8_t axis = CONTROL_ROLL; axis <= CONTROL_PITCH; axis++) {
+    if (flight_flightMode == FM_RETURN_TO_LEVEL) {
-    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
+      // Control.
-    // In HH mode, the I part is summed from P and D of gyros minus stick.
+      // The P part is attitude error.
+      PID = (((int32_t)attitude[axis] * ATT_P_SCALER_LSHIFT16 * dynamicParams.attGyroP) >> 16) + controls[axis];
+      // The I part is attitude error integral.
-    if (gyroIFactor) {
+      IPart[axis] += PID;
+      // The D part is rate.
+      PID += ((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.attGyroD) >> 16;
-      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
+    } else {
-      //if (axis == 0) debugOut.analog[28] = iDiff;
-      PDPart += iDiff;
-      IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
-    } else {
+      // We want: Normal stick gain, full stick deflection should drive gyros halfway towards saturation.
-      IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
-    }
-    // So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
+      // If that is not enough, then fully towards saturation.
-    // attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
-    // We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
-    // That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
+      PID = (((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.rateGyroP) >> 16) + controls[axis];
-    // So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
+      IPart[axis] += PID;
-    // With normal Ki, limit I parts to +/- 205 (of about 1024)
-    if (IPart[axis] < -64000) {
-      IPart[axis] = -64000;
-      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
-    } else if (IPart[axis] > 64000) {
-      IPart[axis] = 64000;
+      PID -= ((int32_t)gyroD[axis] * dynamicParams.rateGyroD) >> 8;
-      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
-    }
-    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
-    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
-    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
-    /*
+    }
-     * 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
-     * (max. pitch or roll term is the throttle value).
-     * OOPS: Is not applied at all.
-     * TODO: Why a growing function of yaw?
      */
+    // PDPart += (gyroD[axis] * (int16_t) dynamicParams.gyroD) / 16;
+    // Right now, let us ignore I.
+    // term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
+    term[axis] = PID;
-    if (term[axis] < -tmp_int) {
+    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
       debugOut.digital[1] |= DEBUG_CLIP;
-      term[axis] = -tmp_int;
+    debugOut.analog[12 + axis] = term[axis];
     } else if (term[axis] > tmp_int) {
-      debugOut.digital[1] |= DEBUG_CLIP;
-      term[axis] = tmp_int;
-    }
-  }
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  // Universal Mixer
-  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if (!(--debugDataTimer)) {
-    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
-    debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
-    debugOut.analog[1] = attitude[ROLL]  / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
-    debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
-    debugOut.analog[16] = acc[PITCH];
-    debugOut.analog[17] = acc[ROLL];
+  //debugOut.analog[14] = yawTerm;
-    debugOut.analog[3] = rate_ATT[PITCH];
-    debugOut.analog[4] = rate_ATT[ROLL];
-    debugOut.analog[5] = yawRate;
-    debugOut.analog[6] = getAngleEstimateFromAcc(PITCH) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;
+  //debugOut.analog[15] = throttleTerm;
-    debugOut.analog[7] = getAngleEstimateFromAcc(ROLL) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;
-    debugOut.analog[8] = acc[Z];
-    debugOut.analog[9] = controls[CONTROL_PITCH];
-    debugOut.analog[10] = controls[CONTROL_ROLL];
-  }
-  /*
-  debugOut.analog[6] = term[PITCH];
-  debugOut.analog[7] = term[ROLL];
-  debugOut.analog[8] = yawTerm;
-  debugOut.analog[9] = throttleTerm;
-  */
-  for (i = 0; i < MAX_MOTORS; i++) {
-    int32_t tmp;
-    uint8_t throttle;
-    tmp = (int32_t) throttleTerm * motorMixer.matrix[i][MIX_THROTTLE];
-    tmp += (int32_t) term[PITCH] * motorMixer.matrix[i][MIX_PITCH];
-    tmp += (int32_t) term[ROLL] * motorMixer.matrix[i][MIX_ROLL];
-    tmp += (int32_t) yawTerm * motorMixer.matrix[i][MIX_YAW];
-    tmp = tmp >> 6;
-    motorFilters[i] = motorFilter(tmp, motorFilters[i]);
-    // Now we scale back down to a 0..255 range.
-    tmp = motorFilters[i] / MOTOR_SCALING;
-    // So this was the THIRD time a throttle was limited. But should the limitation
-    // apply to the common throttle signal (the one used for setting the "power" of
-    // all motors together) or should it limit the throttle set for each motor,
-    // including mix components of pitch, roll and yaw? I think only the common
-    // throttle should be limited.
-    // --> WRONG. This caused motors to stall completely in tight maneuvers.
-    // Apply to individual signals instead.
-    CHECK_MIN_MAX(tmp, 1, 255);
-    throttle = tmp;
-    /*
-    if (i < 4)
-      debugOut.analog[10 + i] = throttle;
-      */
-    if ((MKFlags & MKFLAG_MOTOR_RUN) && motorMixer.matrix[i][MIX_THROTTLE] > 0) {
+  //debugOut.analog[15] = IPart[0] / 1000;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c @ 2751 - Rev 2164 → 2189