WebSVN - FlightCtrl - Diff - Rev 2033 and 2035 - /branches/dongfang_FC

 #include <inttypes.h>
 #include "analog.h"
 #include "attitude.h"
 #include "uart0.h"
 #include "configuration.h"
 // for digital / LED debug.
 #include "output.h"
 // For scope debugging only!
 #include "rc.h"
 #define INTEGRAL_LIMIT 100000
 #define LATCH_TIME 40
 int32_t targetHeight;
 int32_t rampedTargetHeight;
 uint8_t heightRampingTimer = 0;
 int32_t maxHeightThisFlight;
 int32_t iHeight;
 void HC_setGround(void) {
   analog_setGround();
   // This should also happen when height control is enabled in-flight.
   rampedTargetHeight = targetHeight = analog_getHeight();
   maxHeightThisFlight = 0;
   iHeight = 0;
+}
 void HC_update(void) {
   int32_t height = analog_getHeight();
   static uint8_t setHeightLatch = 0;
   if (height > maxHeightThisFlight)
     maxHeightThisFlight = height;
   if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
     // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
     if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
       // Switch is ON
       if (setHeightLatch <= LATCH_TIME) {
         if (setHeightLatch == LATCH_TIME) {
           // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
           rampedTargetHeight = targetHeight = height;
+        }
         // Time not yet reached.
         setHeightLatch++;
+      }
     } else {
       // Switch is OFF.
       setHeightLatch = 0;
+    }
   } else {
     // Switch is not activated; take the "max-height" as the target height.
     targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it)
+  }
   if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
     heightRampingTimer = 0;
     if (rampedTargetHeight < targetHeight) {
       // climbing
       if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
         rampedTargetHeight += staticParams.heightSlewRate;
       } else {
         rampedTargetHeight = targetHeight;
+      }
     } else {
       // descending
       if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
         rampedTargetHeight -= staticParams.heightSlewRate;
       } else {
         rampedTargetHeight = targetHeight;
+      }
+    }
+  }
   // height, in meters (so the division factor is: 100)
-  // debugOut.analog[30] = filteredAirPressure / 10;
   debugOut.analog[30] = (117100 - filteredAirPressure) / 100;
   // Calculated 0 alt number: 108205
   // Experimental 0 alt number: 117100
+}
 // takes 180-200 usec (with integral term). That is too heavy!!!
 // takes 100 usec without integral term.
 uint16_t HC_getThrottle(uint16_t throttle) {
   int32_t height = analog_getHeight();
   int32_t heightError = rampedTargetHeight - height;
   //static int32_t lastHeight;
   int16_t dHeight = analog_getDHeight();
   //lastHeight = height;
   if (heightError > 0) {
     debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
-    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
-  } else if (heightError < 0) {
+  } else {
     debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
+  }
+  if (dHeight > 0) {
     debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
+  } else {
+    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
+  }
   // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
   iHeight += heightError;
 #define IHEIGHT_SCALE 24
   // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
-  int16_t dThrottle =  (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE);
+  int16_t dThrottleI =  (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
-  // dt = (iHeight * staticParams.heightI) >> (IHEIGHT_SCALE) = staticParams.heightControlMaxIntegral
-  // (iHeight * staticParams.heightI) = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE)
-  // iHeight = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) /staticParams.heightI
-  if (dThrottle > staticParams.heightControlMaxIntegral) {
+  if (dThrottleI > staticParams.heightControlMaxIntegral) {
-    dThrottle = staticParams.heightControlMaxIntegral;
+    dThrottleI = staticParams.heightControlMaxIntegral;
-    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI;
+    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
-  } else if (dThrottle < -staticParams.heightControlMaxIntegral) {
+  } else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
-    dThrottle = -staticParams.heightControlMaxIntegral;
+    dThrottleI = -staticParams.heightControlMaxIntegral;
-    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI;
+    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
+  }
-  }
+  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
+  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
+  debugOut.analog[24] = dThrottleP;
+  debugOut.analog[25] = dThrottleI;
+  debugOut.analog[26] = dThrottleD;
+  debugOut.analog[27] = dynamicParams.heightP;
+  debugOut.analog[28] = dynamicParams.heightI;
-  debugOut.analog[27] = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE);
+  debugOut.analog[29] = dynamicParams.heightD;
-  dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7);
+  int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD;
   if (dThrottle > staticParams.heightControlMaxThrottleChange)
     dThrottle = staticParams.heightControlMaxThrottleChange;
   else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
     dThrottle = -staticParams.heightControlMaxThrottleChange;
-  debugOut.analog[19] = rampedTargetHeight;
+  debugOut.analog[19] = throttle;
-  debugOut.analog[21] = dThrottle;
+  debugOut.analog[20] = dThrottle;
+  debugOut.analog[21] = height;
+  debugOut.analog[22] = rampedTargetHeight;
-  debugOut.analog[26] = height;
+  debugOut.analog[23] = heightError;
   if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
     if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
         || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
       // If switch is not in use --> Just apply height control.
       // If switch is in use     --> only apply height control when switch is also ON.
       throttle += dThrottle;
+    }
+  }
   /* Experiment: Find hover-throttle */
 #define DEFAULT_HOVERTHROTTLE 50
 int32_t stronglyFilteredHeightDiff = 0;
 uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
 uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
 #define HOVERTHROTTLEFILTER 25
   stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
                                 * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
   stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
                                                           - 1) + throttle) / HOVERTHROTTLEFILTER;
   /*
   if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
       && stronglyFilteredHeightDiff > -3) {
     hoverThrottle = stronglyFilteredThrottle;
     debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
-    // DebugOut.Analog[18] = hoverThrottle;
   } else
     debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
-  //debugOut.analog[20] = dThrottle;
+    */
   return throttle;
+}
 /*
  For a variometer thingy:
  When switch is thrown on, freeze throttle (capture it into variable)
  For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
  Output = frozen throttle + whatever is computed +/-. Integral?
  */

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/heightControl.c @ 1795 - Rev 2033 → 2035