WebSVN - FlightCtrl - Diff - Rev 2019 and 2026 - /branches/dongfang_FC

 #include <avr/io.h>
 #include "eeprom.h"
 #include "flight.h"
 #include "output.h"
-// Only for debug. Remove.
-//#include "analog.h"
-//#include "rc.h"
 // Necessary for external control and motor test
 #include "uart0.h"
 #include "twimaster.h"
 #include "attitude.h"
   /* 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
-    beepRCAlarm();
+    if (controlMixer_didReceiveSignal) beepRCAlarm();
     if (emergencyFlightTime) {
       // continue emergency flight
       emergencyFlightTime--;
       if (isFlying > 256) {
      * Probably to avoid integration effects that will cause the copter to spin
      * or flip when taking off.
      */
       if (isFlying < 256) {
         IPart[PITCH] = IPart[ROLL] = 0;
-        // TODO: Don't stomp on other modules' variables!!!
-        // controlYaw = 0;
+        PDPartYaw = 0;
-        PDPartYaw = 0; // instead.
         if (isFlying == 250) {
           // HC_setGround();
           updateCompassCourse = 1;
           yawAngleDiff = 0;
+        }
   /* and angular velocity (gyro signal)                                   */
   /************************************************************************/
   // 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] = 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
+    PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING));
-     * where pfactor is in [0..1].
-     */
-    PDPart[axis] += (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
+    PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
     CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
+  }
   PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
       yawTerm = -throttleTerm / 2;
     } else if (yawTerm > throttleTerm / 2) {
       debugOut.digital[0] |= DEBUG_CLIP;
       yawTerm = throttleTerm / 2;
+    }
-    //CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (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;
+    }
-    //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
+  }
   // FIXME: Throttle may exceed maxThrottle (there is no check no more).
   tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
   if (yawTerm < -(tmp_int - throttleTerm)) {
     yawTerm = -(tmp_int - throttleTerm);
     debugOut.digital[0] |= DEBUG_CLIP;
     if (term[axis] < -tmp_int) {
       debugOut.digital[1] |= DEBUG_CLIP;
     } else if (term[axis] > tmp_int) {
       debugOut.digital[1] |= DEBUG_CLIP;
+    }
-    CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
+  }
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   // Universal Mixer

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 2019 → 2026