WebSVN - FlightCtrl - Diff - Rev 1874 and 1908 - /branches/dongfang_FC

   // Fire the main flight attitude calculation, including integration of angles.
   // We want that to kick as early as possible, not to delay new AD sampling further.
   calculateFlightAttitude();
   controlMixer_update();
-  throttleTerm = controlThrottle;
+  throttleTerm = controls[CONTROL_THROTTLE];
   // This check removed. Is done on a per-motor basis, after output matrix multiplication.
   if (throttleTerm < staticParams.MinThrottle + 10)
+  }
   /************************************************************************/
   /*  Yawing                                                              */
   /************************************************************************/
-  if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
+  if (abs(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated
     ignoreCompassTimer = 1000;
     if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
       updateCompassCourse = 1;
+    }
   //  yawControlRate = controlYaw;
   // Trim drift of yawAngleDiff with controlYaw.
   // TODO: We want NO feedback of control related stuff to the attitude related stuff.
   // This seems to be used as: Difference desired <--> real heading.
-  yawAngleDiff -= controlYaw;
+  yawAngleDiff -= controls[CONTROL_YAW];
   // limit the effect
    * 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 - controlYaw * CONTROL_SCALING;
+  yawTerm = PDPartYaw - 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;
      */
     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.
-      IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos.
+      IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) 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...
-      IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
+      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.
+    }
     tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability
         * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
     // 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] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch
+    term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch
     /*
   // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[12] = term[PITCH];
   DebugOut.Analog[13] = term[ROLL];
-  DebugOut.Analog[14] = yawTerm;
+  //DebugOut.Analog[14] = yawTerm;
   DebugOut.Analog[15] = throttleTerm;
+  for (i = 0; i < MAX_MOTORS; i++) {
+    int32_t tmp;
+    uint8_t throttle;
+    tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE];
+    tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH];
+    tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL];
+    tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW];
+    tmp = tmp >> 6;
+    motorFilters[i] = motorFilter(tmp, motorFilters[i]);
+    // Now we scale back down to a 0..255 range.
+    tmp = motorFilters[i] / MOTOR_SCALING;
+    // So this was the THIRD time a throttle was limited. But should the limitation
+    // apply to the common throttle signal (the one used for setting the "power" of
+    // all motors together) or should it limit the throttle set for each motor,
+    // including mix components of pitch, roll and yaw? I think only the common
+    // throttle should be limited.
+    // --> WRONG. This caused motors to stall completely in tight maneuvers.
+    // Apply to individual signals instead.
+    CHECK_MIN_MAX(tmp, 1, 255);
+    throttle = tmp;
-  for (i = 0; i < MAX_MOTORS; i++) {
+    if (i < 4) DebugOut.Analog[22 + i] = throttle;
-    int32_t tmp;
-    if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) {
-      tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE];
-      tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH];
-      tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL];
-      tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW];
-      tmp = tmp >> 6;
-      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
-      // Now we scale back down to a 0..255 range.
-      tmp = motorFilters[i] / MOTOR_SCALING;
-      // So this was the THIRD time a throttle was limited. But should the limitation
-      // apply to the common throttle signal (the one used for setting the "power" of
-      // all motors together) or should it limit the throttle set for each motor,
-      // including mix components of pitch, roll and yaw? I think only the common
-      // throttle should be limited.
-      // --> WRONG. This caused motors to stall completely in tight maneuvers.
-      // Apply to individual signals instead.
-      CHECK_MIN_MAX(tmp, 1, 255);
+    if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) {
-      motor[i].SetPoint = tmp;
+      motor[i].SetPoint = throttle;
     } else if (motorTestActive) {
       motor[i].SetPoint = motorTest[i];
     } else {
-      motor[i].SetPoint = 0;
-    }
-    if (i < 4)
+      motor[i].SetPoint = 0;
       DebugOut.Analog[22 + i] = motor[i].SetPoint;
+  }

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 1874 → 1908