WebSVN - FlightCtrl - Diff - Rev 1956 and 1960 - /branches/dongfang_FC

 //#include "analog.h"
 //#include "rc.h"
 // Necessary for external control and motor test
-#include "uart0.h"
-// for scope debugging
-// #include "rc.h"
 #include "uart0.h"
 #include "twimaster.h"
 #include "attitude.h"
 #include "controlMixer.h"
 #include "commands.h"
 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...
-uint16_t Ki = 10300 / 33;
-uint8_t RequiredMotors = 0;
+uint16_t Ki = 10300 / 33;
 /************************************************************************/
 /*  Filter for motor value smoothing (necessary???)                     */
 /************************************************************************/
 int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
-  switch (dynamicParams.UserParams[5]) {
+  switch (dynamicParams.motorSmoothing) {
   case 0:
     return newvalue;
   case 1:
 /*  Neutral Readings                                                    */
 /************************************************************************/
 void flight_setNeutral() {
   MKFlags |= MKFLAG_CALIBRATE;
   // not really used here any more.
+  /*
   dynamicParams.KalmanK = -1;
   dynamicParams.KalmanMaxDrift = 0;
   dynamicParams.KalmanMaxFusion = 32;
+  */
   controlMixer_initVariables();
+}
 void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor,
+}
 void setNormalFlightParameters(void) {
   setFlightParameters(
                       dynamicParams.IFactor,
-                      dynamicParams.GyroP,
+                      dynamicParams.gyroP,
-                      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
+                      staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
-                      dynamicParams.GyroP,
+                      dynamicParams.gyroP,
-                      dynamicParams.UserParams[6]
+                      dynamicParams.yawIFactor
                       );
+}
   calculateFlightAttitude();
   controlMixer_update();
   throttleTerm = controls[CONTROL_THROTTLE];
   // This check removed. Is done on a per-motor basis, after output matrix multiplication.
-  if (throttleTerm < staticParams.MinThrottle + 10)
+  if (throttleTerm < staticParams.minThrottle + 10)
-    throttleTerm = staticParams.MinThrottle + 10;
+    throttleTerm = staticParams.minThrottle + 10;
-  else if (throttleTerm > staticParams.MaxThrottle - 20)
+  else if (throttleTerm > staticParams.maxThrottle - 20)
-    throttleTerm = (staticParams.MaxThrottle - 20);
+    throttleTerm = (staticParams.maxThrottle - 20);
   /************************************************************************/
   /* RC-signal is bad                                                     */
     if (emergencyFlightTime) {
       // continue emergency flight
       emergencyFlightTime--;
       if (isFlying > 256) {
         // We're probably still flying. Descend slowly.
-        throttleTerm = staticParams.EmergencyGas; // Set emergency throttle
+        throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle
-        MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing
+        MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing
         setStableFlightParameters();
       } else {
         MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
+      }
     } else {
       // end emergency flight (just cut the motors???)
-      MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING);
+      MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT);
+    }
   } else {
     // signal is acceptable
     if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
       // Reset emergency landing control variables.
-      MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
+      MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
       // The time is in whole seconds.
-      emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488;
+      emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488;
+    }
     // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
     if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
     /*
      * When standing on the ground, do not apply I controls and zero the yaw stick.
      * Probably to avoid integration effects that will cause the copter to spin
      * or flip when taking off.
      */
-    if (isFlying < 256) {
+      if (isFlying < 256) {
-      IPart[PITCH] = IPart[ROLL] = 0;
+        IPart[PITCH] = IPart[ROLL] = 0;
-      // TODO: Don't stomp on other modules' variables!!!
+        // TODO: Don't stomp on other modules' variables!!!
-      // controlYaw = 0;
+        // controlYaw = 0;
-      PDPartYaw = 0; // instead.
+        PDPartYaw = 0; // instead.
-      if (isFlying == 250) {
+        if (isFlying == 250) {
-        // HC_setGround();
+          // HC_setGround();
-        updateCompassCourse = 1;
+          updateCompassCourse = 1;
-        yawAngleDiff = 0;
+          yawAngleDiff = 0;
+        }
+      } 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);
+      }
-    } 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);
-    }
     commands_handleCommands();
-    // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
     setNormalFlightParameters();
-    // }
   } // end else (not bad signal case)
-  // end part1a: 750-800 usec.
-  /*
-   * Looping the H&I way basically is just a matter of turning off attitude angle measurement
-   * by integration (because 300 deg/s gyros are too slow) and turning down the throttle.
-   * This is the throttle part.
-   */
-  if (looping) {
-    if (throttleTerm > staticParams.LoopGasLimit)
-      throttleTerm = staticParams.LoopGasLimit;
-  }
   /************************************************************************/
   /*  Yawing                                                              */
   /************************************************************************/
-  if (abs(controls[CONTROL_YAW]) > 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)) {
+    if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) {
       updateCompassCourse = 1;
+    }
+  }
-  //  yawControlRate = controlYaw;
   // 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.
   CHECK_MIN_MAX(yawAngleDiff, -50000, 50000);
   /************************************************************************/
   /* Compass is currently not supported.                                  */
   /************************************************************************/
-  if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
+  if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
     updateCompass();
+  }
   /* 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++) {
-    if (looping & ((1 << 4) << axis)) {
-      PPart[axis] = 0;
-    } else { // TODO: Where do the 44000 come from???
+  for (axis = PITCH; axis <= ROLL; axis++) {
-      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.
      * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
      * where pfactor is in [0..1].
-     */
-    PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis]
+     */
-        * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis]
+    PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis]
-        * (int16_t) dynamicParams.GyroD) / 16;
+                                                                                         * (int16_t) dynamicParams.gyroD) / 16;
     CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
+  }
    */
   if (missingMotor) {
     // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
     if (isFlying > 1 && isFlying < 50 && throttleTerm > 0)
       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?
+    }
     //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;
+  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
   if (yawTerm < -(tmp_int - throttleTerm)) {
     yawTerm = -(tmp_int - throttleTerm);
     debugOut.digital[0] |= DEBUG_CLIP;
   } else if (yawTerm > (tmp_int - throttleTerm)) {
       // "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] - 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
+    tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability
         * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
     // TODO: From which planet comes the 16000?
   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)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
-    tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH];
+    tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
-    tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL];
+    tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
-    tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW];
+    tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];
     tmp = tmp >> 6;
     motorFilters[i] = motorFilter(tmp, motorFilters[i]);
     CHECK_MIN_MAX(tmp, 1, 255);
     throttle = tmp;
     if (i < 4) debugOut.analog[22 + i] = throttle;
-    if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) {
+    if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
       motor[i].SetPoint = throttle;
     } else if (motorTestActive) {
       motor[i].SetPoint = motorTest[i];
     debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
     debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
     debugOut.analog[16] = gyroPFactor;
     debugOut.analog[17] = gyroIFactor;
-    debugOut.analog[18] = dynamicParams.GyroD;
+    debugOut.analog[18] = dynamicParams.gyroD;
+  }

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 1956 → 1960