WebSVN - FlightCtrl - Diff - Rev 1805 and 1821 - /branches/dongfang_FC

  * 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;
-uint8_t gyroPFactor, gyroIFactor;       // the PD factors for the attitude control
+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...
 /************************************************************************/
 /*  Filter for motor value smoothing (necessary???)                     */
 /************************************************************************/
 int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
-  switch(dynamicParams.UserParams[5]) {
+        switch (dynamicParams.UserParams[5]) {
-  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
+                else
+                        return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
-      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
+        default:
-  default: return newvalue;
+                return newvalue;
+        }
+}
 /************************************************************************/
 /*  Neutral Readings                                                    */
 /************************************************************************/
 void flight_setNeutral() {
-  MKFlags |= MKFLAG_CALIBRATE;
+        MKFlags |= MKFLAG_CALIBRATE;
-  // not really used here any more.
+        // not really used here any more.
-  dynamicParams.KalmanK = -1;
+        dynamicParams.KalmanK = -1;
-  dynamicParams.KalmanMaxDrift = 0;
+        dynamicParams.KalmanMaxDrift = 0;
+        dynamicParams.KalmanMaxFusion = 32;
-  dynamicParams.KalmanMaxFusion = 32;
+        controlMixer_initVariables();
-  controlMixer_initVariables();
+}
-}
+void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor,
-void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
+                uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
-  Ki = 10300 / _Ki;
+        Ki = 10300 / _Ki;
-  gyroPFactor = _gyroPFactor;
+        gyroPFactor = _gyroPFactor;
-  gyroIFactor = _gyroIFactor;
+        gyroIFactor = _gyroIFactor;
-  yawPFactor = _yawPFactor;
-  yawIFactor = _yawIFactor;
+        yawPFactor = _yawPFactor;
-}
+        yawIFactor = _yawIFactor;
-void setNormalFlightParameters(void) {
   setFlightParameters(dynamicParams.IFactor + 1,
                       dynamicParams.GyroP + 10,
-                      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
+void setNormalFlightParameters(void) {
-                      dynamicParams.GyroP + 10,
+        setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10,
-                      dynamicParams.UserParams[6]
-                      );
+                        staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
                         dynamicParams.GyroP + 10, dynamicParams.UserParams[6]);
+}
 void setStableFlightParameters(void) {
-  /************************************************************************/
+        /************************************************************************/
-  /* Compass is currently not supported.                                  */
+        /* Compass is currently not supported.                                  */
-  /************************************************************************/
+        /************************************************************************/
-  if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
+        if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
-    updateCompass();
+                updateCompass();
-  }
+        }
 #if defined (USE_NAVICTRL)
-  /************************************************************************/
+        /************************************************************************/
-  /* GPS is currently not supported.                                      */
+        /* GPS is currently not supported.                                      */
-  /************************************************************************/
+        /************************************************************************/
-  if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
+        if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
-    GPS_Main();
+                GPS_Main();
-    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
+                MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
-  } else {
+        } else {
-    // GPSStickPitch = 0;
+                // GPSStickPitch = 0;
-    // GPSStickRoll = 0;
+                // GPSStickRoll = 0;
-  }
+        }
 #endif
-  // end part 1: 750-800 usec.
+        // end part 1: 750-800 usec.
-  // start part 3: 350 - 400 usec.
+        // start part 3: 350 - 400 usec.
 #define SENSOR_LIMIT  (4096 * 4)
-    /************************************************************************/
+        /************************************************************************/
-    /* Calculate control feedback from angle (gyro integral)                */
+        /* Calculate control feedback from angle (gyro integral)                */
-    /* and angular velocity (gyro signal)                                   */
+        /* and angular velocity (gyro signal)                                   */
-    /************************************************************************/
+        /************************************************************************/
-    // The P-part is the P of the PID controller. That's the angle integrals (not rates).
+        // The P-part is the P of the PID controller. That's the angle integrals (not rates).
-  for (axis=PITCH; axis<=ROLL; axis++) {
+        for (axis = PITCH; axis <= ROLL; axis++) {
-    if(looping & ((1<<4)<<axis)) {
+                if (looping & ((1 << 4) << axis)) {
-      PPart[axis] = 0;
+                        PPart[axis] = 0;
-    } else { // TODO: Where do the 44000 come from???
+                } else { // TODO: Where do the 44000 come from???
+                        PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
-      PPart[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.
-     * 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
-     * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
-     * where pfactor is in [0..1].
+                 * where pfactor is in [0..1].
-     */
+                 */
+                PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis]
-    PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING))
+                                * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis]
-      + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16;
+                                * (int16_t) dynamicParams.GyroD) / 16;
-    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
+                CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
-  }
+        }
-  PDPartYaw =
+        PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L
-    (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
+                        / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000
-  + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
+                        / CONTROL_SCALING));
-  // limit control feedback
+        // limit control feedback
-  CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
+        CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
-  /*
+        /*
-   * Compose throttle term.
+         * Compose throttle term.
-   * If a Bl-Ctrl is missing, prevent takeoff.
+         * If a Bl-Ctrl is missing, prevent takeoff.
-   */
+         */
-  if(missingMotor) {
+        if (missingMotor) {
-    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
+                // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
-    if(isFlying > 1 && isFlying < 50 && throttleTerm > 0)
+                if (isFlying > 1 && isFlying < 50 && throttleTerm > 0)
-      isFlying = 1; // keep within lift off condition
+                        isFlying = 1; // keep within lift off condition
-    throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
+                throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
-  }
+        }
-  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
+        // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
-  throttleTerm *= CONTROL_SCALING;
+        throttleTerm *= CONTROL_SCALING;
-    DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);
+                 DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 1805 → 1821