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

  */
 // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
 uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
-uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
-// Some integral weight constant...
+uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
-uint16_t Ki = 10300 / 33;
+uint8_t invKi = 64;
 /************************************************************************/
 /*  Filter for motor value smoothing (necessary???)                     */
   dynamicParams.KalmanMaxFusion = 32;
   */
   controlMixer_initVariables();
+}
-void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor,
+void setFlightParameters(uint8_t _invKi, uint8_t _gyroPFactor,
     uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
-  Ki = 10300 / _Ki;
+  invKi = _invKi;
   gyroPFactor = _gyroPFactor;
   gyroIFactor = _gyroIFactor;
   yawPFactor = _yawPFactor;
   yawIFactor = _yawIFactor;
                       staticParams.yawIFactor
                       );
+}
 void setStableFlightParameters(void) {
-  setFlightParameters(33, 90, 120, 90, 120);
+  setFlightParameters(0, 90, 120, 90, 120);
+}
 /************************************************************************/
 /*  Main Flight Control                                                 */
 /************************************************************************/
 void flight_control(void) {
-  int16_t tmp_int;
+  uint16_t tmp_int;
   // 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 */;
+  int16_t PDPart;
   static int32_t IPart[2] = {0, 0};
     /*
      * 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.
      */
-      if (isFlying < 256) {
+    if (isFlying < 256) {
-        IPart[PITCH] = IPart[ROLL] = 0;
+      IPart[PITCH] = IPart[ROLL] = 0;
-        PDPartYaw = 0;
         if (isFlying == 250) {
           HC_setGround();
 #ifdef USE_MK3MAG
           attitude_resetHeadingToMagnetic();
           compass_setTakeoffHeading(heading);
 #endif
           // Set target heading to the one just gotten off compass.
           // targetHeading = heading;
+        }
-      } else {
+   } else {
-        // 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);
-      }
+   }
     commands_handleCommands();
     setNormalFlightParameters();
-  } // end else (not bad signal case)
-#if defined (USE_NAVICTRL)
-  /************************************************************************/
-  /* GPS is currently not supported.                                      */
-  /************************************************************************/
-  if(staticParams.GlobalConfig & CFG_GPS_ENABLED) {
-    GPS_Main();
-    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
-  } else {
   } // end else (not bad signal case)
 #endif
-  // end part 1: 750-800 usec.
-  // start part 3: 350 - 400 usec.
-  /************************************************************************/
-  /* 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).
-  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);
-    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;
-    }
   // end part 1: 750-800 usec.
   // start part 3: 350 - 400 usec.
 #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
   // This is where control affects the target heading. It also (later) directly controls yaw.
   headingError -= controls[CONTROL_YAW];
   debugOut.analog[28] = headingError / 100;
   if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT;
   if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT;
-  PDPartYaw =  (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3);
-  // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
-  PDPartYaw += (int32_t)(yawRate * yawPFactor) /  (GYRO_DEG_FACTOR_PITCHROLL >> 6);
   PDPart =  (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
-  // limit control feedback
+  // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
-  // CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
+  PDPart += (int32_t)(yawRate * yawPFactor) /  (GYRO_DEG_FACTOR_YAW >> 5);
    * 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 = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING;
+  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;
   } else if (yawTerm > (tmp_int - throttleTerm)) {
     yawTerm = (tmp_int - throttleTerm);
     debugOut.digital[0] |= DEBUG_CLIP;
+  }
   // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
+  debugOut.digital[1] &= ~DEBUG_CLIP;
+  tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + abs(yawTerm) / 2)) >> 6;
+  /************************************************************************/
+  /* Calculate control feedback from angle (gyro integral)                */
+  /* and angular velocity (gyro signal)                                   */
+  /************************************************************************/
-  debugOut.digital[1] &= ~DEBUG_CLIP;
+  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
   for (axis = PITCH; axis <= ROLL; axis++) {
+    int16_t iDiff;
+    iDiff = PDPart = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
+    PDPart += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
-    /*
+    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
-     * Compose pitch and roll terms. This is finally where the sticks come into play.
+    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
-     */
+    // In HH mode, the I part is summed from P and D of gyros minus stick.
-    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.
+    if (gyroIFactor) {
-      IPart[axis] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos.
+      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 {
-      // "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...
+    } else {
-      IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
+      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.
-    }
+    }
-    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);
+    // With normal Ki, limit effect to +/- 205 (of 1024!!!)
-    if (IPart[axis] < -25L*GYRO_DEG_FACTOR_PITCHROLL) {
+    if (IPart[axis] < -64000) {
-      IPart[axis] =- 25L*GYRO_DEG_FACTOR_PITCHROLL;
+      IPart[axis] = -64000;
       debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
-    } else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) {
+    } else if (IPart[axis] > 64000) {
-      PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL;
+      IPart[axis] = 64000;
-      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
-    }
+      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
+    }
     // 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] = PDPart - controls[axis] + ((int32_t)IPart[axis] * invKi) >> 14;
         term[axis] += (dynamicParams.levelCorrection[axis] - 128);
         /*

Subversion Repositories FlightCtrl

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