WebSVN - FlightCtrl - Diff - Rev 2051 and 2052 - /branches/dongfang_FC

 #include <stdlib.h>
 #include <avr/io.h>
 #include "eeprom.h"
 #include "flight.h"
 #include "output.h"
+#include "uart0.h"
-// Necessary for external control and motor test
-#include "uart0.h"
+// Necessary for external control and motor test
 #include "twimaster.h"
 #include "attitude.h"
 #include "controlMixer.h"
+#include "commands.h"
+#include "heightControl.h"
+#ifdef USE_MK3MAG
+#include "mk3mag.h"
+#include "compassControl.h"
-#include "commands.h"
+#endif
 #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
   // Mixer Fractions that are combined for Motor Control
   int16_t yawTerm, throttleTerm, term[2];
   // PID controller variables
   int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */;
-  static int32_t IPart[2] = { 0, 0 };
+  static int32_t IPart[2] = {0, 0};
   static uint16_t emergencyFlightTime;
   static int8_t debugDataTimer = 1;
   /* This part could be abstracted, as having yet another control input   */
   /* to the control mixer: An emergency autopilot control.                */
   /************************************************************************/
   if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
-    if (controlMixer_didReceiveSignal) beepRCAlarm();
+    if (controlMixer_didReceiveSignal) beepRCAlarm();  // Only make alarm if a control signal was received before the signal loss.
     if (emergencyFlightTime) {
       // continue emergency flight
       emergencyFlightTime--;
       if (isFlying > 256) {
     // signal is acceptable
     if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
       // Reset emergency landing control variables.
       MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
       // The time is in whole seconds.
+      if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP)
+        emergencyFlightTime = 0xffff;
+      else
-      emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488;
+        emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP;
+    }
     // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
     if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
      */
       if (isFlying < 256) {
         IPart[PITCH] = IPart[ROLL] = 0;
         PDPartYaw = 0;
         if (isFlying == 250) {
-          // HC_setGround();
+          HC_setGround();
+#ifdef USE_MK3MAG
           attitude_resetHeadingToMagnetic();
+          compass_setTakeoffHeading(heading);
+#endif
           // Set target heading to the one just gotten off compass.
           // targetHeading = heading;
+        }
       } else {
         // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
     commands_handleCommands();
     setNormalFlightParameters();
-  } // end else (not bad signal case)
-  /************************************************************************/
-  /*  Yawing                                                              */
-  /************************************************************************/
-  /*
-  if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated
-    ignoreCompassTimer = 1000;
-    if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) {
-      //targetHeading = heading;
-      // YGBSM!!!
-    }
-  }
-  */
+  } // end else (not bad signal case)
 #if defined (USE_NAVICTRL)
   /************************************************************************/
   /* GPS is currently not supported.                                      */
   /************************************************************************/
-  if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
+  if(staticParams.GlobalConfig & CFG_GPS_ENABLED) {
     GPS_Main();
     MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
   } else {
   // The P-part is the P of the PID controller. That's the angle integrals (not rates).
   for (axis = PITCH; axis <= ROLL; axis++) {
     PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral
     PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5);
     PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
-    CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL);
+    //CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL);
+    if (PDPart[axis] < -6L*GYRO_DEG_FACTOR_PITCHROLL) {
+      PDPart[axis] =- 6L*GYRO_DEG_FACTOR_PITCHROLL;
+      debugOut.digital[0] |= DEBUG_FLIGHTCLIP;
+    } else if (PDPart[axis] > 6L*GYRO_DEG_FACTOR_PITCHROLL) {
+      PDPart[axis] = 6L*GYRO_DEG_FACTOR_PITCHROLL;
+      debugOut.digital[0] |= DEBUG_FLIGHTCLIP;
+    }
+  }
 #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
   // This is where control affects the target heading. It also (later) directly controls yaw.
+    }
     tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability
+        * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
+    //CHECK_MIN_MAX(IPart[axis], -25L*GYRO_DEG_FACTOR_PITCHROLL, 25L*GYRO_DEG_FACTOR_PITCHROLL);
-        * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
+    if (IPart[axis] < -25L*GYRO_DEG_FACTOR_PITCHROLL) {
+      IPart[axis] =- 25L*GYRO_DEG_FACTOR_PITCHROLL;
+      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
+    } else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) {
+      PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL;
+      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
     // 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] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch
         term[axis] += (dynamicParams.levelCorrection[axis] - 128);

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 2051 → 2052