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

  uint8_t Height_Gain;            // Value : 0-50
  uint8_t Height_ACC_Effect;      // Value : 0-250
  */
 int32_t getHeight(void) {
-        return groundPressure - filteredAirPressure;
+  return groundPressure - filteredAirPressure;
+}
 void HC_setGround(void) {
-        groundPressure = filteredAirPressure;
+  groundPressure = filteredAirPressure;
-        // This should also happen when height control is enabled in-flight.
+  // This should also happen when height control is enabled in-flight.
-        rampedTargetHeight = getHeight();
+  rampedTargetHeight = getHeight();
-        maxHeight = 0;
+  maxHeight = 0;
-        iHeight = 0;
+  iHeight = 0;
 }
 void HC_update(void) {
-        int32_t height = getHeight();
+  int32_t height = getHeight();
-        static uint8_t setHeightLatch = 0;
+  static uint8_t setHeightLatch = 0;
-        if (height > maxHeight)
+  if (height > maxHeight)
-                maxHeight = height;
+    maxHeight = height;
-        if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
+  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 switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
-                // DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
+    // DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
-                if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40) {
+    if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
-                        // Switch is ON
+      // Switch is ON
-                        if (setHeightLatch <= LATCH_TIME) {
+      if (setHeightLatch <= LATCH_TIME) {
-                                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.MaxHeight * 100; //getHeight() + 10 * 100;
-        }
-        if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
-                heightRampingTimer = 0;
-                if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {
-                        rampedTargetHeight += staticParams.Height_Gain;
-                } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {
+          // 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.
-                        rampedTargetHeight -= staticParams.Height_Gain;
+    // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
+    targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100;
+  }
+  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
+    heightRampingTimer = 0;
+    if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) {
+      rampedTargetHeight += staticParams.heightSlewRate;
+    } else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) {
+      rampedTargetHeight -= staticParams.heightSlewRate;
-                }
+    }
-        }
+  }
-        // height, in meters (so the division factor is: 100)
+  // height, in meters (so the division factor is: 100)
-        debugOut.analog[30] = height / 10;
+  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 height = getHeight();
-        int32_t heightError = rampedTargetHeight - height;
+  int32_t heightError = rampedTargetHeight - height;
-        static int32_t lastHeight;
+  static int32_t lastHeight;
-        int16_t dHeight = height - lastHeight;
+  int16_t dHeight = height - lastHeight;
-        lastHeight = height;
+  lastHeight = height;
-        // DebugOut.Analog[20] = dHeight;
+  // DebugOut.Analog[20] = dHeight;
-        // DebugOut.Analog[21] = dynamicParams.MaxHeight;
+  // DebugOut.Analog[21] = dynamicParams.MaxHeight;
-        // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
+  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
-        // iHeight += heightError;
+  // iHeight += heightError;
-        if (dHeight > 0) {
+  if (dHeight > 0) {
-                debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
+    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
-                debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
+    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
-        } else if (dHeight < 0) {
+  } else if (dHeight < 0) {
-                debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
+    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
-                debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
+    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
-        }
+  }
-        /*
+  /*
-         if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
+    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 < -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[0] = 1;}
-         else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
+    else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
-         */
+  */
-        int16_t dThrottle = heightError * staticParams.HeightP / 1000
+  int16_t dThrottle = heightError * staticParams.heightP / 1000
-                        /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
+    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
-                                        * staticParams.HeightD;
+    * staticParams.heightD;
-        // the "minGas" is now a limit for how much up / down the throttle can be varied
+  // the "minGas" is now a limit for how much up / down the throttle can be varied
-        if (dThrottle > staticParams.HeightMinGas)
+  if (dThrottle > staticParams.heightMaxThrottleChange)
-                dThrottle = staticParams.HeightMinGas;
+    dThrottle = staticParams.heightMaxThrottleChange;
-        else if (dThrottle < -staticParams.HeightMinGas)
+  else if (dThrottle < -staticParams.heightMaxThrottleChange)
-                dThrottle = -staticParams.HeightMinGas;
+    dThrottle = -staticParams.heightMaxThrottleChange;
         // TODO: Eliminate repetition.
-        if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
+  if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) {
-                if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH)
+    if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH)
-                                || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) {
+        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
-                        // If switch is not in use --> Just apply height control.
+      // If switch is not in use --> Just apply height control.
-                        // If switch is in use     --> only apply height control when switch is also ON.
+      // If switch is in use     --> only apply height control when switch is also ON.
-                        throttle += dThrottle;
+      throttle += dThrottle;
-                }
+    }
-        }
+  }
-        /* Experiment: Find hover-throttle */
+  /* Experiment: Find hover-throttle */
-        stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
+  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
-                        * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
+                                * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
-        stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
+  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
-                        - 1) + throttle) / HOVERTHROTTLEFILTER;
+                                                          - 1) + throttle) / HOVERTHROTTLEFILTER;
-        if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
+  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
-                        && stronglyFilteredHeightDiff > -3) {
+      && stronglyFilteredHeightDiff > -3) {
-                hoverThrottle = stronglyFilteredThrottle;
+    hoverThrottle = stronglyFilteredThrottle;
-                debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
+    debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
-                // DebugOut.Analog[18] = hoverThrottle;
+    // DebugOut.Analog[18] = hoverThrottle;
-        } else
+  } else
-                debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
+    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
-    debugOut.analog[20] = dThrottle;
+  debugOut.analog[20] = dThrottle;
-    debugOut.analog[21] = hoverThrottle;
+  debugOut.analog[21] = hoverThrottle;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/heightControl.c - Rev 1955 → 1960