WebSVN - FlightCtrl - Diff - Rev 2049 and 2051 - /branches/dongfang_FC

         PDPartYaw = 0;
         if (isFlying == 250) {
           // HC_setGround();
           attitude_resetHeadingToMagnetic();
           // Set target heading to the one just gotten off compass.
-          targetHeading = heading;
+          // targetHeading = heading;
+        }
       } else {
         // 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.
         MKFlags |= (MKFLAG_FLY);
       // YGBSM!!!
+    }
+  }
   */
-  targetHeading -= controls[CONTROL_YAW];
-  debugOut.analog[28] = targetHeading;
 #if defined (USE_NAVICTRL)
   /************************************************************************/
   /* GPS is currently not supported.                                      */
   /************************************************************************/
   } else {
+  }
 #endif
   // end part 1: 750-800 usec.
   // start part 3: 350 - 400 usec.
-#define SENSOR_LIMIT  (4096 * 4)
   /************************************************************************/
   /* 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], -SENSOR_LIMIT, SENSOR_LIMIT);
+    CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL);
+  }
-  }
+#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];
-  int32_t headingDiff = heading - targetHeading; // apparently yaw is reverse on output.
+  debugOut.analog[28] = headingError / 100;
-  if (headingDiff > YAWOVER180) headingDiff -= YAWOVER360;
-  else if (headingDiff <= -YAWOVER180) headingDiff += YAWOVER360;
-  // TODO: Not quite right: We want to limit targetHeading to be max. 50000 from heading. This is the wrong var. fixed.
-  CHECK_MIN_MAX(headingDiff, -50000, 50000);
+  if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT;
+  if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT;
   debugOut.analog[29] = headingDiff;
   PDPartYaw =  (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3);
-  PDPartYaw =  (int32_t)(headingDiff * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3);
+  // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
     // 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] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch
         term[axis] += (dynamicParams.levelCorrection[axis] - 128);
-    /*
+        /*
      * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
      * 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?
      */

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 2049 → 2051