Subversion Repositories FlightCtrl

Rev

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