Subversion Repositories FlightCtrl

Rev

Rev 1961 | Rev 1970 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1775 - 1
#include <inttypes.h>
2
#include "attitude.h"
3
#include "uart0.h"
4
#include "configuration.h"
5
 
6
// for digital / LED debug.
7
#include "output.h"
8
 
9
// For scope debugging only!
10
#include "rc.h"
11
 
12
#define INTEGRAL_LIMIT 100000
13
 
14
/*
1821 - 15
 #define DEBUGINTEGRAL 0
16
 #define DEBUGDIFFERENTIAL 0
17
 #define DEBUGHOVERTHROTTLE 0
18
 #define DEBUGHEIGHTSWITCH 0
19
 */
1775 - 20
 
21
#define LATCH_TIME 40
22
 
23
int32_t groundPressure;
24
int32_t targetHeight;
25
int32_t rampedTargetHeight;
26
 
27
uint8_t heightRampingTimer = 0;
1961 - 28
int32_t maxHeightThisFlight;
1775 - 29
int32_t iHeight;
30
 
31
int32_t getHeight(void) {
1960 - 32
  return groundPressure - filteredAirPressure;
1775 - 33
}
34
 
35
void HC_setGround(void) {
1960 - 36
  groundPressure = filteredAirPressure;
37
  // This should also happen when height control is enabled in-flight.
38
  rampedTargetHeight = getHeight();
1961 - 39
  maxHeightThisFlight = 0;
1960 - 40
  iHeight = 0;
1775 - 41
}
42
 
43
void HC_update(void) {
1960 - 44
  int32_t height = getHeight();
45
  static uint8_t setHeightLatch = 0;
46
 
1961 - 47
  if (height > maxHeightThisFlight)
48
    maxHeightThisFlight = height;
1960 - 49
 
1961 - 50
  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
1960 - 51
    // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
52
    if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
53
      // Switch is ON
54
      if (setHeightLatch <= LATCH_TIME) {
55
        if (setHeightLatch == LATCH_TIME) {
56
          // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
57
          targetHeight = height;
1775 - 58
        }
1960 - 59
        // Time not yet reached.
60
        setHeightLatch++;
61
      }
62
    } else {
63
      // Switch is OFF.
64
      setHeightLatch = 0;
65
    }
66
  } else {
67
    // Switch is not activated; take the "max-height" as the target height.
1961 - 68
    targetHeight = (uint16_t) dynamicParams.heightSetting * 100;
1960 - 69
  }
70
 
71
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
72
    heightRampingTimer = 0;
1961 - 73
    if (rampedTargetHeight < targetHeight) {
74
      // climbing
75
      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
76
        rampedTargetHeight += staticParams.heightSlewRate;
77
      } else {
78
        rampedTargetHeight = targetHeight;
79
      }
80
    } else if (rampedTargetHeight != targetHeight) {
81
      // descending
82
      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
83
        rampedTargetHeight -= staticParams.heightSlewRate;
84
      } else {
85
        rampedTargetHeight = targetHeight;
86
      }
1960 - 87
    }
88
  }
89
 
90
  // height, in meters (so the division factor is: 100)
91
  debugOut.analog[30] = height / 10;
1775 - 92
}
93
 
94
// takes 180-200 usec (with integral term). That is too heavy!!!
95
// takes 100 usec without integral term.
96
uint16_t HC_getThrottle(uint16_t throttle) {
1960 - 97
  int32_t height = getHeight();
98
  int32_t heightError = rampedTargetHeight - height;
99
 
100
  static int32_t lastHeight;
101
 
102
  int16_t dHeight = height - lastHeight;
103
  lastHeight = height;
104
 
105
  // DebugOut.Analog[20] = dHeight;
106
  // DebugOut.Analog[21] = dynamicParams.MaxHeight;
107
 
108
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
109
  // iHeight += heightError;
110
 
111
  if (dHeight > 0) {
112
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
113
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
114
  } else if (dHeight < 0) {
1961 - 115
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
1960 - 116
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
117
  }
118
 
119
  /*
120
    if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
121
    else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
122
    else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
123
    else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
124
  */
125
 
1961 - 126
  int16_t dThrottle = ((heightError * staticParams.heightP) << 10)
127
    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD;
1960 - 128
 
129
  // the "minGas" is now a limit for how much up / down the throttle can be varied
1961 - 130
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
131
    dThrottle = staticParams.heightControlMaxThrottleChange;
132
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
133
    dThrottle = -staticParams.heightControlMaxThrottleChange;
1960 - 134
 
1961 - 135
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
136
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
1960 - 137
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
138
      // If switch is not in use --> Just apply height control.
139
      // If switch is in use     --> only apply height control when switch is also ON.
140
      throttle += dThrottle;
141
    }
142
  }
1961 - 143
 
144
  /* Experiment: Find hover-throttle */
145
 
146
#define DEFAULT_HOVERTHROTTLE 50
147
int32_t stronglyFilteredHeightDiff = 0;
148
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
149
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
150
#define HOVERTHROTTLEFILTER 25
1960 - 151
 
152
  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
153
                                * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
154
  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
155
                                                          - 1) + throttle) / HOVERTHROTTLEFILTER;
156
 
157
  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
158
      && stronglyFilteredHeightDiff > -3) {
159
    hoverThrottle = stronglyFilteredThrottle;
160
    debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
161
    // DebugOut.Analog[18] = hoverThrottle;
162
  } else
163
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
164
 
165
  debugOut.analog[20] = dThrottle;
166
  debugOut.analog[21] = hoverThrottle;
167
 
168
  return throttle;
1775 - 169
}
170
 
171
/*
1821 - 172
 For a variometer thingy:
173
 When switch is thrown on, freeze throttle (capture it into variable)
174
 For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
175
 Output = frozen throttle + whatever is computed +/-. Integral?
176
 */