WebSVN - FlightCtrl - Diff - Rev 1926 and 1927 - /branches/dongfang_FC

 /*
  * Gyro integrals. These are the rotation angles of the airframe compared to the
  * horizontal plane, yaw relative to yaw at start. Not really used for anything else
  * than diagnostics.
  */
-int32_t angle[2];
+int32_t angle[3];
 /*
  * Error integrals. Stick is always positive. Gyro is configurable positive or negative.
  * These represent the deviation of the attitude angle from the desired on each axis.
  */
-int32_t error[3];
-// Yaw angle and compass stuff.
-// This is updated/written from MM3. Negative angle indicates invalid data.
-int16_t compassHeading = -1;
-// This is NOT updated from MM3. Negative angle indicates invalid data.
-int16_t compassCourse = -1;
-// The difference between the above 2 (heading - course) on a -180..179 degree interval.
-// Not necessary. Never read anywhere.
-// int16_t compassOffCourse = 0;
-uint8_t updateCompassCourse = 0;
-uint8_t compassCalState = 0;
-uint16_t ignoreCompassTimer = 500;
+int32_t error[3];
 int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
   analog_calibrate();
   // reset gyro integrals to acc guessing
-  setStaticAttitudeAngles();
-  // update compass course to current heading
-  compassCourse = compassHeading;
+  setStaticAttitudeAngles();
   // Inititialize YawGyroIntegral value with current compass heading
-  yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
+  angle[YAW] = 0;
 void integrate(void) {
   // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
   uint8_t axis;
-    // TODO: Multiply on a factor. Wont work without...
-  error[PITCH] += control[CONTROL_ELEVATOR];
-  error[ROLL] += control[CONTROL_AILERONS];
   error[YAW] += control[CONTROL_RUDDER];
+    // TODO: Multiply on a factor on control. Wont work without...
   if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
-      error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
+    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
+    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
+    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
+    angle[PITCH] += rate_ATT[PITCH];
-      error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
+    angle[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
-      error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
+    angle[YAW] += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
-  } else {
+} else {
-      error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
+    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
+    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
+    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
+    angle[PITCH] += rate_ATT[PITCH];
-      error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
+    angle[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
+    angle[YAW] += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
+  }
-      error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 // TODO: Configurable.
 #define ERRORLIMIT 1000
-    for (axis=PITCH; axis<=YAW; axis++) {
+for (axis=PITCH; axis<=YAW; axis++) {
   if (error[axis] > ERRORLIMIT) {
         error[axis] = ERRORLIMIT;
     } else if (angle[axis] <= -ERRORLIMIT) {
         angle[axis] = -ERRORLIMIT;
-    }
-    }
-  /*
-   * Yaw
-   * Calculate yaw gyro integral (~ to rotation angle)
-   * Limit yawGyroHeading proportional to 0 deg to 360 deg
-   */
-  yawGyroHeading += ACYawRate;
-  if (yawGyroHeading >= YAWOVER360) {
-    yawGyroHeading -= YAWOVER360; // 360 deg. wrap
-  } else if (yawGyroHeading < 0) {
     yawGyroHeading += YAWOVER360;
+}
   /*
    * Pitch axis integration and range boundary wrap.
    */
   for (axis = PITCH; axis <= ROLL; axis++) {
     angle[axis] += rate_ATT[axis];
     if (angle[axis] > PITCHROLLOVER180) {
       angle[axis] -= PITCHROLLOVER360;
+    } else if (angle[axis] <= -PITCHROLLOVER180) {
+      angle[axis] += PITCHROLLOVER360;
+    }
+  }
+    /*
+     * Yaw
+     * Calculate yaw gyro integral (~ to rotation angle)
+     * Limit yawGyroHeading proportional to 0 deg to 360 deg
+     */
+    if (angle[YAW] >= YAWOVER360) {
+        angle[YAW] -= YAWOVER360; // 360 deg. wrap
-    } else if (angle[axis] <= -PITCHROLLOVER180) {
+    } else if (angle[YAW] < 0) {
-      angle[axis] += PITCHROLLOVER360;
+        angle[YAW] += YAWOVER360;
+    }
   // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
   // are less than ....., or reintroduce Kalman.
   // Well actually the Z axis acc. check is not so silly.
   uint8_t axis;
   int32_t temp;
-  if (acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
+  if (acc[Z] >= -staticParams.accCorrectionZAccLimit && acc[Z]
       <= dynamicParams.UserParams[7]) {
     DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
     uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/attitude.c - Rev 1926 → 1927