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

 #include <inttypes.h>
 #include "attitude.h"
 #include "uart0.h"
 #include "configuration.h"
 #include "controlMixer.h"
 // for digital / LED debug.
 #include "output.h"
 // For scope debugging only!
 #include "rc.h"
 #define INTEGRAL_LIMIT 100000
 /*
  #define DEBUGINTEGRAL 0
  #define DEBUGDIFFERENTIAL 0
  #define DEBUGHOVERTHROTTLE 0
  #define DEBUGHEIGHTSWITCH 0
  */
 #define LATCH_TIME 40
 int32_t groundPressure;
 int32_t targetHeight;
 int32_t rampedTargetHeight;
-#define DEFAULT_HOVERTHROTTLE 50
-int32_t stronglyFilteredHeightDiff = 0;
-uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
-uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
-#define HOVERTHROTTLEFILTER 25
 uint8_t heightRampingTimer = 0;
-int32_t maxHeight;
+int32_t maxHeightThisFlight;
-int32_t iHeight;
-/*
- These parameters are free to take:
- uint8_t HeightMinGas;           // Value : 0-100
- uint8_t HeightD;                // Value : 0-250
- uint8_t MaxHeight;              // Value : 0-32
- uint8_t HeightP;                // Value : 0-32
- uint8_t Height_Gain;            // Value : 0-50
- uint8_t Height_ACC_Effect;      // Value : 0-250
- */
+int32_t iHeight;
 int32_t getHeight(void) {
   return groundPressure - filteredAirPressure;
+}
 void HC_setGround(void) {
   groundPressure = filteredAirPressure;
   // This should also happen when height control is enabled in-flight.
   rampedTargetHeight = getHeight();
-  maxHeight = 0;
+  maxHeightThisFlight = 0;
   iHeight = 0;
+}
 void HC_update(void) {
   int32_t height = getHeight();
   static uint8_t setHeightLatch = 0;
-  if (height > maxHeight)
+  if (height > maxHeightThisFlight)
-    maxHeight = height;
+    maxHeightThisFlight = height;
-  if (staticParams.bitConfig & CFG_HEIGHT_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 (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
-    // DebugOut.Digital[0] |= DEBUG_HEIGHT_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.
           targetHeight = height;
-          // DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
+        }
         // Time not yet reached.
         setHeightLatch++;
+      }
     } else {
       // Switch is OFF.
       setHeightLatch = 0;
-      // DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
+    }
   } else {
     // Switch is not activated; take the "max-height" as the target height.
-    // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
-    targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100;
+    targetHeight = (uint16_t) dynamicParams.heightSetting * 100;
+  }
   if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
     heightRampingTimer = 0;
+    if (rampedTargetHeight < targetHeight) {
+      // climbing
-    if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) {
+      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
-      rampedTargetHeight += staticParams.heightSlewRate;
+        rampedTargetHeight += staticParams.heightSlewRate;
+      } else {
+        rampedTargetHeight = targetHeight;
+      }
-    } else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) {
+    } else if (rampedTargetHeight != targetHeight) {
+      // descending
+      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
-      rampedTargetHeight -= staticParams.heightSlewRate;
+        rampedTargetHeight -= staticParams.heightSlewRate;
+      } else {
+        rampedTargetHeight = targetHeight;
+      }
+    }
+  }
   // height, in meters (so the division factor is: 100)
   debugOut.analog[30] = height / 10;
+}
-// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
-// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH
-// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH
 // 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 = getHeight();
   int32_t heightError = rampedTargetHeight - height;
   static int32_t lastHeight;
   int16_t dHeight = height - lastHeight;
   lastHeight = height;
   // DebugOut.Analog[20] = dHeight;
   // DebugOut.Analog[21] = dynamicParams.MaxHeight;
   // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
   // iHeight += heightError;
   if (dHeight > 0) {
     debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
     debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
   } else if (dHeight < 0) {
-    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
     debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
+    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
+  }
   /*
     if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
     else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
     else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
     else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
   */
-  int16_t dThrottle = heightError * staticParams.heightP / 1000
+  int16_t dThrottle = ((heightError * staticParams.heightP) << 10)
-    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
-    * staticParams.heightD;
+    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD;
   // the "minGas" is now a limit for how much up / down the throttle can be varied
-  if (dThrottle > staticParams.heightMaxThrottleChange)
+  if (dThrottle > staticParams.heightControlMaxThrottleChange)
-    dThrottle = staticParams.heightMaxThrottleChange;
+    dThrottle = staticParams.heightControlMaxThrottleChange;
-  else if (dThrottle < -staticParams.heightMaxThrottleChange)
+  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
-    dThrottle = -staticParams.heightMaxThrottleChange;
+    dThrottle = -staticParams.heightControlMaxThrottleChange;
-        // TODO: Eliminate repetition.
-  if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) {
+  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
-    if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH)
+    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;
   debugOut.analog[21] = hoverThrottle;
   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 @ 1899 - Rev 1960 → 1961