Subversion Repositories FlightCtrl

Rev

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