WebSVN - FlightCtrl - Diff - Rev 1946 and 1955 - /branches/dongfang_FC

   yawPFactor = _yawPFactor;
   yawIFactor = _yawIFactor;
+}
 void setNormalFlightParameters(void) {
+  setFlightParameters(dynamicParams.IFactor,
-  setFlightParameters(dynamicParams.IFactor, dynamicParams.GyroP,
+      dynamicParams.GyroP,
+      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
-      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
+      dynamicParams.GyroP,
+      dynamicParams.UserParams[6]
-      dynamicParams.GyroP, dynamicParams.UserParams[6]);
+  );
+}
 void setStableFlightParameters(void) {
    * 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;
   // Limit yawTerm
-  DebugOut.Digital[0] &= ~DEBUG_CLIP;
+  debugOut.digital[0] &= ~DEBUG_CLIP;
   if (throttleTerm > MIN_YAWGAS) {
     if (yawTerm < -throttleTerm / 2) {
-      DebugOut.Digital[0] |= DEBUG_CLIP;
+      debugOut.digital[0] |= DEBUG_CLIP;
       yawTerm = -throttleTerm / 2;
     } else if (yawTerm > throttleTerm / 2) {
-      DebugOut.Digital[0] |= DEBUG_CLIP;
+      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;
+      debugOut.digital[0] |= DEBUG_CLIP;
       yawTerm = -MIN_YAWGAS / 2;
     } else if (yawTerm > MIN_YAWGAS / 2) {
-      DebugOut.Digital[0] |= DEBUG_CLIP;
+      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;
+    debugOut.digital[0] |= DEBUG_CLIP;
   } else if (yawTerm > (tmp_int - throttleTerm)) {
     yawTerm = (tmp_int - throttleTerm);
-    DebugOut.Digital[0] |= DEBUG_CLIP;
+    debugOut.digital[0] |= DEBUG_CLIP;
+  }
   // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
-  DebugOut.Digital[1] &= ~DEBUG_CLIP;
+  debugOut.digital[1] &= ~DEBUG_CLIP;
   for (axis = PITCH; axis <= ROLL; axis++) {
     /*
      * Compose pitch and roll terms. This is finally where the sticks come into play.
      * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
      * (max. pitch or roll term is the throttle value).
      * TODO: Why a growing function of yaw?
      */
     if (term[axis] < -tmp_int) {
-      DebugOut.Digital[1] |= DEBUG_CLIP;
+      debugOut.digital[1] |= DEBUG_CLIP;
     } else if (term[axis] > tmp_int) {
-      DebugOut.Digital[1] |= DEBUG_CLIP;
+      debugOut.digital[1] |= DEBUG_CLIP;
+    }
     CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
+  }
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   // Universal Mixer
   // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  DebugOut.Analog[12] = term[PITCH];
+  debugOut.analog[12] = term[PITCH];
-  DebugOut.Analog[13] = term[ROLL];
+  debugOut.analog[13] = term[ROLL];
-  //DebugOut.Analog[14] = yawTerm;
+  debugOut.analog[14] = yawTerm;
-  DebugOut.Analog[15] = throttleTerm;
+  debugOut.analog[15] = throttleTerm;
     // --> WRONG. This caused motors to stall completely in tight maneuvers.
     // Apply to individual signals instead.
     CHECK_MIN_MAX(tmp, 1, 255);
     throttle = tmp;
-    if (i < 4) DebugOut.Analog[22 + i] = throttle;
+    if (i < 4) debugOut.analog[22 + i] = throttle;
-    if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) {
+    if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) {
       motor[i].SetPoint = throttle;
     } else if (motorTestActive) {
       motor[i].SetPoint = motorTest[i];
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   // Debugging
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if (!(--debugDataTimer)) {
     debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
-    DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
+    debugOut.analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
-    DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
+    debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
-    DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
+    debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
-    DebugOut.Analog[6] = 64 >> 4;
+    debugOut.analog[16] = gyroPFactor;
-    DebugOut.Analog[7] = -64 >> 4;
+    debugOut.analog[17] = gyroIFactor;
-    /*
-     DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);
-     DebugOut.Analog[24] = controlYaw;
-     DebugOut.Analog[25] = yawAngleDiff / 100L;
-     DebugOut.Analog[26] = accNoisePeak[PITCH];
-     DebugOut.Analog[27] = accNoisePeak[ROLL];
-     DebugOut.Analog[30] = gyroNoisePeak[PITCH];
+    debugOut.analog[18] = dynamicParams.GyroD;
-     DebugOut.Analog[31] = gyroNoisePeak[ROLL];
-     */
+  }
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c @ 1877 - Rev 1946 → 1955