Subversion Repositories FlightCtrl

Rev

Rev 1960 | Rev 1963 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1960 Rev 1961
Line 20... Line 20...
20 
 */
 20 
 */
Line 21... Line 21...
21 
 
 21 
 
Line 22... Line 22...
22 
#define LATCH_TIME 40
 22 
#define LATCH_TIME 40
23 
 
 - 
 
24 
int32_t groundPressure;
 23 
 
25 
 
 24 
int32_t groundPressure;
Line 26... Line -...
26 
int32_t targetHeight;
 - 
 
27 
int32_t rampedTargetHeight;
 - 
 
28 
 
 - 
 
29 
#define DEFAULT_HOVERTHROTTLE 50
 - 
 
30 
int32_t stronglyFilteredHeightDiff = 0;
 - 
 
31 
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
 - 
 
32 
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
 25 
int32_t targetHeight;
33 
#define HOVERTHROTTLEFILTER 25
 26 
int32_t rampedTargetHeight;
34 
 
 27 
 
35 
uint8_t heightRampingTimer = 0;
 - 
 
36 
int32_t maxHeight;
 - 
 
37 
int32_t iHeight;
 - 
 
38 
/*
 - 
 
39 
 These parameters are free to take:
 - 
 
40 
 uint8_t HeightMinGas;           // Value : 0-100
 - 
 
41 
 uint8_t HeightD;                // Value : 0-250
 - 
 
42 
 uint8_t MaxHeight;              // Value : 0-32
 - 
 
43 
 uint8_t HeightP;                // Value : 0-32
 - 
 
Line 44... Line 28...
44 
 uint8_t Height_Gain;            // Value : 0-50
 28 
uint8_t heightRampingTimer = 0;
45 
 uint8_t Height_ACC_Effect;      // Value : 0-250
 29 
int32_t maxHeightThisFlight;
46 
 */
 30 
int32_t iHeight;
Line 47... Line 31...
47 
 
 31 
 
48 
int32_t getHeight(void) {
 32 
int32_t getHeight(void) {
49 
  return groundPressure - filteredAirPressure;
 33 
  return groundPressure - filteredAirPressure;
50 
}
 34 
}
51 
 
 35 
 
52 
void HC_setGround(void) {
 36 
void HC_setGround(void) {
53 
  groundPressure = filteredAirPressure;
 37 
  groundPressure = filteredAirPressure;
Line 54... Line 38...
54 
  // This should also happen when height control is enabled in-flight.
 38 
  // This should also happen when height control is enabled in-flight.
55 
  rampedTargetHeight = getHeight();
 39 
  rampedTargetHeight = getHeight();
56 
  maxHeight = 0;
 40 
  maxHeightThisFlight = 0;
Line 57... Line 41...
57 
  iHeight = 0;
 41 
  iHeight = 0;
58 
}
 42 
}
Line 59... Line 43...
59 
 
 43 
 
60 
void HC_update(void) {
 44 
void HC_update(void) {
61 
  int32_t height = getHeight();
 - 
 
62 
  static uint8_t setHeightLatch = 0;
 45 
  int32_t height = getHeight();
63 
 
 46 
  static uint8_t setHeightLatch = 0;
64 
  if (height > maxHeight)
 47 
 
65 
    maxHeight = height;
 48 
  if (height > maxHeightThisFlight)
66 
 
 49 
    maxHeightThisFlight = height;
67 
  if (staticParams.bitConfig & CFG_HEIGHT_SWITCH) {
 50 
 
68 
    // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
 - 
 
69 
    // DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
 51 
  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
70 
    if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
 52 
    // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
71 
      // Switch is ON
 53 
    if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
72 
      if (setHeightLatch <= LATCH_TIME) {
 54 
      // Switch is ON
73 
        if (setHeightLatch == LATCH_TIME) {
 55 
      if (setHeightLatch <= LATCH_TIME) {
74 
          // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
 56 
        if (setHeightLatch == LATCH_TIME) {
75 
          targetHeight = height;
 57 
          // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
76 
          // DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
 - 
 
77 
        }
 58 
          targetHeight = height;
78 
        // Time not yet reached.
 59 
        }
79 
        setHeightLatch++;
 60 
        // Time not yet reached.
80 
      }
 - 
 
81 
    } else {
 61 
        setHeightLatch++;
82 
      // Switch is OFF.
 62 
      }
Line 83... Line 63...
83 
      setHeightLatch = 0;
 63 
    } else {
84 
      // DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
 64 
      // Switch is OFF.
- 
 
 65 
      setHeightLatch = 0;
- 
 
 66 
    }
85 
    }
 67 
  } else {
86 
  } else {
 68 
    // Switch is not activated; take the "max-height" as the target height.
- 
 
 69 
    targetHeight = (uint16_t) dynamicParams.heightSetting * 100;
- 
 
 70 
  }
- 
 
 71 
 
87 
    // Switch is not activated; take the "max-height" as the target height.
 72 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
- 
 
 73 
    heightRampingTimer = 0;
- 
 
 74 
    if (rampedTargetHeight < targetHeight) {
88 
    // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
 75 
      // climbing
- 
 
 76 
      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
- 
 
 77 
        rampedTargetHeight += staticParams.heightSlewRate;
- 
 
 78 
      } else {
89 
    targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100;
 79 
        rampedTargetHeight = targetHeight;
90 
  }
 80 
      }
Line 91... Line 81...
91 
 
 81 
    } else if (rampedTargetHeight != targetHeight) {
92 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
 82 
      // descending
93 
    heightRampingTimer = 0;
 83 
      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
Line 94... Line -...
94 
    if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) {
 - 
 
95 
      rampedTargetHeight += staticParams.heightSlewRate;
 - 
 
96 
    } else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) {
 - 
 
97 
      rampedTargetHeight -= staticParams.heightSlewRate;
 - 
 
98 
    }
 84 
        rampedTargetHeight -= staticParams.heightSlewRate;
99 
  }
 85 
      } else {
100 
 
 86 
        rampedTargetHeight = targetHeight;
101 
  // height, in meters (so the division factor is: 100)
 87 
      }
102 
  debugOut.analog[30] = height / 10;
 88 
    }
Line 125... Line 111...
125 
 
 111 
 
126 
  if (dHeight > 0) {
 112 
  if (dHeight > 0) {
127 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
 113 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
128 
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
 114 
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
129 
  } else if (dHeight < 0) {
 - 
 
130 
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
 115 
  } else if (dHeight < 0) {
- 
 
 116 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
131 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
 117 
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
Line 132... Line 118...
132 
  }
 118 
  }
133 
 
 119 
 
134 
  /*
 120 
  /*
135 
    if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
 121 
    if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
136 
    else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
 122 
    else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
137 
    else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
 123 
    else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
Line 138... Line 124...
138 
    else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
 124 
    else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
139 
  */
 125 
  */
140 
 
 - 
 
Line 141... Line 126...
141 
  int16_t dThrottle = heightError * staticParams.heightP / 1000
 126 
 
142 
    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
 127 
  int16_t dThrottle = ((heightError * staticParams.heightP) << 10)
143 
    * staticParams.heightD;
 128 
    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD;
144 
 
 129 
 
145 
  // the "minGas" is now a limit for how much up / down the throttle can be varied
 130 
  // the "minGas" is now a limit for how much up / down the throttle can be varied
146 
  if (dThrottle > staticParams.heightMaxThrottleChange)
 131 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
147 
    dThrottle = staticParams.heightMaxThrottleChange;
 - 
 
148 
  else if (dThrottle < -staticParams.heightMaxThrottleChange)
 132 
    dThrottle = staticParams.heightControlMaxThrottleChange;
149 
    dThrottle = -staticParams.heightMaxThrottleChange;
 133 
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
150 
 
 134 
    dThrottle = -staticParams.heightControlMaxThrottleChange;
151 
        // TODO: Eliminate repetition.
 135 
 
152 
  if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) {
 136 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
153 
    if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH)
 137 
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
154 
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
 138 
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
155 
      // If switch is not in use --> Just apply height control.
 139 
      // If switch is not in use --> Just apply height control.
156 
      // If switch is in use     --> only apply height control when switch is also ON.
 140 
      // If switch is in use     --> only apply height control when switch is also ON.
157 
      throttle += dThrottle;
 141 
      throttle += dThrottle;
- 
 
 142 
    }
- 
 
 143 
  }
- 
 
 144 
 
- 
 
 145 
  /* Experiment: Find hover-throttle */
- 
 
 146 
 
- 
 
 147 
#define DEFAULT_HOVERTHROTTLE 50
- 
 
 148 
int32_t stronglyFilteredHeightDiff = 0;
158 
    }
 149 
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
159 
  }
 150 
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
160 
 
 151 
#define HOVERTHROTTLEFILTER 25
161 
  /* Experiment: Find hover-throttle */
 152