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

 // #include "rc.h"
 #include "timer2.h"
 #include "attitude.h"
-#include "controlMixer.h"
-#include "commands.h"
-#ifdef USE_MK3MAG
-#include "gps.h"
-#endif
+#include "controlMixer.h"
 #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.
+/*
+ * target-directions integrals.
- * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
+ */
+int32_t target[3];
- */
+/*
+ * Error integrals.
+ */
+int32_t error[3];
-// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
-int8_t pitchPFactor, rollPFactor, yawPFactor;
-int8_t pitchDFactor, rollDFactor, yawDFactor;
-int32_t IPart[2] = {0,0};
-/************************************************************************/
-/*  Filter for motor value smoothing (necessary???)                     */
-/************************************************************************/
-int16_t outputFilter(int16_t newvalue, int16_t oldvalue) {
-  switch (dynamicParams.UserParams[5]) {
-  case 0:
-    return newvalue;
-  case 1:
-    return (oldvalue + newvalue) / 2;
-  case 2:
+uint8_t pFactor[3];
-    if (newvalue > oldvalue)
-      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
-    else
-      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
-  case 3:
-    if (newvalue < oldvalue)
-      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
-    else
+uint8_t dFactor[3];
-      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
+uint8_t iFactor[3];
-  default:
+uint8_t reverse[3];
-    return newvalue;
+int32_t IPart[3] = { 0, 0, 0 };
-  }
+int16_t servos[MAX_SERVOS];
-}
+/************************************************************************/
+/*  Neutral Readings                                                    */
 /************************************************************************/
+#define CONTROL_CONFIG_SCALE 10
+void flight_setGround(void) {
+  IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
+  target[PITCH] = attitude[PITCH];
+  target[ROLL] = attitude[ROLL];
+  target[YAW] = attitude[YAW];
+}
+void switchToFlightMode(FlightMode_t flightMode) {
+  if (flightMode == FLIGHT_MODE_MANUAL) {
+    pFactor[PITCH] = 0;
+    pFactor[ROLL] = 0;
+    pFactor[YAW] = 0;
+  } else if (flightMode == FLIGHT_MODE_RATE) {
+    pFactor[PITCH] = 0; //staticParams...;
+    pFactor[ROLL] = 0; //staticParams...;
+    pFactor[YAW] = 0; //staticParams...;
+  }
+  if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) {
+    iFactor[PITCH] = 0;
+    iFactor[ROLL] = 0;
+    iFactor[YAW] = 0;
+  } else if (flightMode == FLIGHT_MODE_ANGLES) {
-/*  Neutral Readings                                                    */
-/************************************************************************/
-#define CONTROL_CONFIG_SCALE 10
-void flight_setNeutral() {
-  // not really used here any more.
-  controlMixer_initVariables();
-}
-void setFlightParameters
-(
+    iFactor[PITCH] = 0; //staticParams...;
- uint8_t _pitchPFactor,
- uint8_t _rollPFactor,
- uint8_t _yawPFactor,
- uint8_t _pitchDFactor,
+    iFactor[ROLL] = 0; //staticParams...;
- uint8_t _rollDFactor,
+    iFactor[YAW] = 0; //staticParams...;
- uint8_t _yawDFactor
- ) {
-  pitchPFactor = _pitchPFactor;
+    // When entering this mode, we want to avoid jerks from accumulated uncorrected errors.
-  rollPFactor = _rollPFactor;
-  yawPFactor = _yawPFactor;
-  pitchDFactor = _pitchDFactor;
-  rollDFactor = _rollDFactor;
-  yawDFactor = _yawDFactor;
-}
-void setNormalFlightParameters(void) {
-  setFlightParameters
-    (
-     dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE,     // 12 seems good
-     dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE,      // 9 seems good
-     dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2),   // 24 seems too little
+    target[PITCH] = attitude[PITCH];
     target[ROLL] = attitude[ROLL];
-     dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE,
+    target[YAW] = attitude[YAW];
      dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE,
      dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE
-     );
+  dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE;
-}
+  dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE;
+  dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE;
 void setStableFlightParameters(void) {
-  setFlightParameters(0, 0, 0, 0, 0, 0);
+  // TODO: Set reverse also.
+}
 /************************************************************************/
 /*  Main Flight Control                                                 */
 /************************************************************************/
-void flight_control(void) {
-  // Mixer Fractions that are combined for Motor Control
+void flight_control(void) {
+  // Mixer Fractions that are combined for Motor Control
-  int16_t yawTerm, throttleTerm, term[2];
+  int16_t throttleTerm, term[3];
   // PID controller variables
+  int16_t PDPart[3];
-  int16_t PDPart[2], PDPartYaw;
+  static int8_t debugDataTimer = 1;
-  static int8_t debugDataTimer = 1;
+  // High resolution motor values for smoothing of PID motor outputs
-  // High resolution motor values for smoothing of PID motor outputs
+  // static int16_t outputFilters[MAX_OUTPUTS];
-  static int16_t outputFilters[MAX_OUTPUTS];
-  uint8_t i;
+  uint8_t axis;
-  // Fire the main flight attitude calculation, including integration of angles.
-  // We want that to kick as early as possible, not to delay new AD sampling further.
+  // TODO: Check modern version.
-  calculateFlightAttitude();
+  // calculateFlightAttitude();
-  controlMixer_update();
-  throttleTerm = control[CONTROL_THROTTLE];
-  /************************************************************************/
-  /* RC-signal is bad                                                     */
-  /************************************************************************/
-  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy
-    RED_ON;
-    beepRCAlarm();
-    setStableFlightParameters();
-  } else {
-    commands_handleCommands();
-    setNormalFlightParameters();
-  }
-  /************************************************************************/
-  /* Calculate control feedback from angle (gyro integral)                */
-  /* and angular velocity (gyro signal)                                   */
-  /************************************************************************/
-  PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor /
-                  (256L / CONTROL_SCALING))
-  + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16;
-  PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /
+  // TODO: Check modern version.
+  // controlMixer_update();
+  throttleTerm = controls[CONTROL_THROTTLE];
+  // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
+  target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator;
+  target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons;
+  target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder;
+  for (axis = PITCH; axis <= YAW; axis++) {
+    if (target[axis] > OVER180) {
+      target[axis] -= OVER360;
-                  (256L / CONTROL_SCALING))
+    } else if (attitude[axis] <= -OVER180) {
+      attitude[axis] += OVER360;
+    }
+    if (reverse[axis])
+      error[axis] = attitude[axis] + target[axis];
+    else
+      error[axis] = attitude[axis] - target[axis];
-  + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;
+    if (error[axis] > OVER180) {
+      error[axis] -= OVER360;
+    } else if (error[axis] <= -OVER180) {
+      error[axis] += OVER360;
+    }
-  PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING)
+    /************************************************************************/
+    /* Calculate control feedback from angle (gyro integral)                */
+    /* and angular velocity (gyro signal)                                   */
+    /************************************************************************/
-  + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;
+    PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6)
+        + ((differential[axis] * (int16_t) dFactor[axis]) >> 4);
-  /************************************************************************/
+    if (reverse[axis])
-  /* Stick signals are positive and gyros are negative...                 */
+      PDPart[axis] = -PDPart[axis];
-  /************************************************************************/
-  IPart[PITCH] = error[PITCH]; // * some factor configurable.
+    int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
-  IPart[ROLL] = error[ROLL];
+    if (reverse[axis])
-    // TODO: Add ipart. Or add/subtract depending, not sure.
+      PDPart[axis] += anglePart;
-  term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? PDPart[PITCH] : -PDPart[PITCH]);
+    else
-  term[ROLL] = control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? PDPart[ROLL] : -PDPart[ROLL]);
+      PDPart[axis] -= anglePart;
-  yawTerm = control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? PDPartYaw : -PDPartYaw);
+    // Add I parts here... these are integrated errors.
+    // When an error wraps, actually its I part should be negated or something...
+    term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  }
+  debugOut.analog[12] = term[PITCH];
-  // Universal Mixer
+  debugOut.analog[13] = term[ROLL];
+  debugOut.analog[14] = throttleTerm;
+  debugOut.analog[15] = term[YAW];
-  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
+  for (uint8_t i = 0; i < MAX_SERVOS; i++) {
+    int16_t tmp;
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    if (servoTestActive) {
+      servos[i] = ((int16_t) servoTest[i] - 128) * 4;
-  DebugOut.Analog[12] = term[PITCH];
-  DebugOut.Analog[13] = term[ROLL];
+    } else {
-  DebugOut.Analog[14] = throttleTerm;
-  DebugOut.Analog[15] = yawTerm;
+      // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
       switch (i) {
-  for (i = 0; i < MAX_OUTPUTS; i++) {
+      case 0:
-    int16_t tmp;
+        tmp = term[ROLL];
-      if (outputTestActive) {
+        break;
-          outputs[i].SetPoint = outputTest[i] * 4;
+      case 1:
-      } else {
+        tmp = term[PITCH];
-        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
+        break;
-        switch(i) {
+      case 2:
-        case 0: tmp = term[ROLL]; break;
+        tmp = throttleTerm;
-        case 1: tmp = term[PITCH]; break;
+        break;
-        case 2: tmp = throttleTerm - 310; break;
+      case 3:
-        case 3: tmp = yawTerm; break;
+        tmp = term[YAW];
-        default: tmp = 0;
+        break;
-        }
+      default:
+        tmp = 0;
-      outputFilters[i] = outputFilter(tmp, outputFilters[i]);
+      }
-      // Now we scale back down to a 0..255 range.
+      // These are all signed and in the same units as the RC stuff in rc.c.
-      tmp = outputFilters[i];
+      servos[i] = tmp;
-      outputs[i].SetPoint = tmp;
+    }
-    }
+  }
-  }
+  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  // Debugging
-  // Debugging
+  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  if (!(--debugDataTimer)) {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c - Rev 2025 → 2099