Subversion Repositories FlightCtrl

Rev

Rev 2086 | Rev 2089 | 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 "configuration.h"
2048 - 5
#include "controlMixer.h"
1775 - 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
 
2069 - 14
#define LATCH_TIME 3
1775 - 15
 
2088 - 16
// This will translate a height value to a target height in centimeters.
17
// Currently: One step is 50 cm and zero is 5 meters below the takeoff altitude. With max. 255 steps the max.
18
// target height is then about 120m.
19
#define HEIGHT_GAIN 50L
20
#define HEIGHT_FORMULA(x) ((x) * HEIGHT_GAIN - 500L)
21
 
2074 - 22
int32_t setHeight;
1775 - 23
int32_t targetHeight;
24
 
2086 - 25
uint16_t hc_testOscPrescaler = 0;
26
uint8_t hc_testOscTimer = 0;
2074 - 27
 
1961 - 28
int32_t maxHeightThisFlight;
1775 - 29
int32_t iHeight;
30
 
31
void HC_setGround(void) {
2033 - 32
  analog_setGround();
1960 - 33
  // This should also happen when height control is enabled in-flight.
2074 - 34
  setHeight = targetHeight = analog_getHeight();
2086 - 35
  hc_testOscTimer = 0;
1961 - 36
  maxHeightThisFlight = 0;
1960 - 37
  iHeight = 0;
1775 - 38
}
39
 
2075 - 40
uint8_t HC_isSwitchOn(void) {
2069 - 41
  return (dynamicParams.heightSetting >= 255/3);
42
}
43
 
2039 - 44
void HC_periodicTask(void) {
2033 - 45
  int32_t height = analog_getHeight();
1960 - 46
  static uint8_t setHeightLatch = 0;
47
 
1961 - 48
  if (height > maxHeightThisFlight)
49
    maxHeightThisFlight = height;
1960 - 50
 
2059 - 51
  // debugOut.analog[25] = dynamicParams.heightSetting;
52
 
1961 - 53
  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
2069 - 54
    if (HC_isSwitchOn()) {
1960 - 55
      // Switch is ON
56
      if (setHeightLatch <= LATCH_TIME) {
2069 - 57
        if (setHeightLatch == LATCH_TIME) {
58
          // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
2074 - 59
          setHeight = height;
2086 - 60
          hc_testOscTimer = 0;
2071 - 61
          iHeight = 0;
2069 - 62
        }
63
        // Time not yet reached.
64
        setHeightLatch++;
1960 - 65
      }
66
    } else {
67
      // Switch is OFF.
68
      setHeightLatch = 0;
69
    }
70
  } else {
71
    // Switch is not activated; take the "max-height" as the target height.
2088 - 72
    setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it)
1960 - 73
  }
74
 
2069 - 75
  /*
1960 - 76
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
77
    heightRampingTimer = 0;
1961 - 78
    if (rampedTargetHeight < targetHeight) {
79
      // climbing
80
      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
81
        rampedTargetHeight += staticParams.heightSlewRate;
82
      } else {
83
        rampedTargetHeight = targetHeight;
84
      }
2026 - 85
    } else {
1961 - 86
      // descending
87
      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
88
        rampedTargetHeight -= staticParams.heightSlewRate;
89
      } else {
90
        rampedTargetHeight = targetHeight;
91
      }
1960 - 92
    }
93
  }
2069 - 94
  */
2074 - 95
  //      uint8_t heightControlTestOscPeriod;
96
  //      uint8_t heightControlTestOscAmplitude;
1970 - 97
 
2086 - 98
  hc_testOscPrescaler++;
99
  if (hc_testOscPrescaler == 488) {
100
          hc_testOscPrescaler = 0;
101
          hc_testOscTimer++;
102
          if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {
103
                  hc_testOscTimer = 0;
2079 - 104
                  if (staticParams.heightControlTestOscAmplitude)
105
                          iHeight = 0;
2086 - 106
          } else if (hc_testOscTimer == staticParams.heightControlTestOscPeriod/2) {
2079 - 107
                  if (staticParams.heightControlTestOscAmplitude)
108
                          iHeight = 0;
2074 - 109
          }
110
  }
111
 
2086 - 112
  if (hc_testOscTimer < staticParams.heightControlTestOscPeriod/2)
2074 - 113
          targetHeight = setHeight;
114
  else
2088 - 115
          targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * HEIGHT_GAIN;
2074 - 116
 
117
  //if (staticParams.)
1960 - 118
  // height, in meters (so the division factor is: 100)
2059 - 119
  // debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
1971 - 120
  // Calculated 0 alt number: 108205
1980 - 121
  // Experimental 0 alt number: 117100
1775 - 122
}
123
 
2074 - 124
#define LOG_PHEIGHT_SCALE 10
125
#define LOG_IHEIGHT_SCALE 24
126
#define LOG_DHEIGHT_SCALE 6
127
 
1775 - 128
// takes 180-200 usec (with integral term). That is too heavy!!!
129
// takes 100 usec without integral term.
2048 - 130
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
131
  HC_periodicTask();
132
  int16_t throttle = PRTY[CONTROL_THROTTLE];
2033 - 133
  int32_t height = analog_getHeight();
2073 - 134
  int32_t heightError = targetHeight - height;
2033 - 135
  int16_t dHeight = analog_getDHeight();
1960 - 136
 
2058 - 137
  debugOut.analog[22] = height/10L;
138
  debugOut.analog[23] = dHeight;
2055 - 139
 
2026 - 140
  if (heightError > 0) {
1960 - 141
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
2035 - 142
  } else {
1961 - 143
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
2035 - 144
  }
145
 
146
  if (dHeight > 0) {
1960 - 147
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
2035 - 148
  } else {
149
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
1960 - 150
  }
2032 - 151
 
2073 - 152
  int32_t heightErrorForIntegral = heightError;
2074 - 153
  int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE;
2073 - 154
 
155
  if (heightErrorForIntegral > heightErrorForIntegralLimit) {
156
    heightErrorForIntegral = heightErrorForIntegralLimit;
157
  } else if (heightErrorForIntegral < -heightErrorForIntegralLimit) {
158
    heightErrorForIntegral =- heightErrorForIntegralLimit;
159
  }
160
 
2033 - 161
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
2073 - 162
  iHeight += heightErrorForIntegral;
2032 - 163
 
2033 - 164
#define IHEIGHT_SCALE 24
165
  // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
2035 - 166
  int16_t dThrottleI =  (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
2033 - 167
 
2073 - 168
  if (dThrottleI > staticParams.heightControlMaxIntegralOut) {
169
    dThrottleI = staticParams.heightControlMaxIntegralOut;
170
    iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
171
  } else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) {
172
    dThrottleI = -staticParams.heightControlMaxIntegralOut;
173
    iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
2033 - 174
  }
175
 
2074 - 176
  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE;
177
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE;
2033 - 178
 
2073 - 179
  debugOut.analog[10] = dThrottleP;
180
  debugOut.analog[11] = dThrottleI;
181
  debugOut.analog[12] = dThrottleD;
182
  debugOut.analog[13] = heightError/10;
2032 - 183
 
2045 - 184
  //debugOut.analog[27] = dynamicParams.heightP;
185
  //debugOut.analog[28] = dynamicParams.heightI;
186
  //debugOut.analog[29] = dynamicParams.heightD;
2035 - 187
 
2071 - 188
  int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD;
2035 - 189
 
1961 - 190
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
191
    dThrottle = staticParams.heightControlMaxThrottleChange;
192
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
193
    dThrottle = -staticParams.heightControlMaxThrottleChange;
2026 - 194
 
2055 - 195
  /*
2035 - 196
  debugOut.analog[19] = throttle;
197
  debugOut.analog[20] = dThrottle;
198
  debugOut.analog[21] = height;
199
  debugOut.analog[22] = rampedTargetHeight;
200
  debugOut.analog[23] = heightError;
2055 - 201
  */
2026 - 202
 
1961 - 203
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
2069 - 204
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) || HC_isSwitchOn()) {
1960 - 205
      // If switch is not in use --> Just apply height control.
206
      // If switch is in use     --> only apply height control when switch is also ON.
207
      throttle += dThrottle;
208
    }
209
  }
1961 - 210
 
211
  /* Experiment: Find hover-throttle */
212
 
213
#define DEFAULT_HOVERTHROTTLE 50
214
int32_t stronglyFilteredHeightDiff = 0;
2036 - 215
// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
1961 - 216
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
217
#define HOVERTHROTTLEFILTER 25
1960 - 218
 
219
  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
220
                                * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
221
  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
222
                                                          - 1) + throttle) / HOVERTHROTTLEFILTER;
223
 
2035 - 224
  /*
1960 - 225
  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
226
      && stronglyFilteredHeightDiff > -3) {
227
    hoverThrottle = stronglyFilteredThrottle;
228
    debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
229
  } else
230
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
2035 - 231
 
232
    */
1960 - 233
 
2048 - 234
  PRTY[CONTROL_THROTTLE] = throttle;
1775 - 235
}
236
 
237
/*
1821 - 238
 For a variometer thingy:
239
 When switch is thrown on, freeze throttle (capture it into variable)
240
 For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
241
 Output = frozen throttle + whatever is computed +/-. Integral?
242
 */