Subversion Repositories FlightCtrl

Rev

Rev 2052 | Rev 2058 | 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
 
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.
2048 - 87
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
88
  HC_periodicTask();
89
  int16_t throttle = PRTY[CONTROL_THROTTLE];
2033 - 90
  int32_t height = analog_getHeight();
1960 - 91
  int32_t heightError = rampedTargetHeight - height;
2033 - 92
  int16_t dHeight = analog_getDHeight();
1960 - 93
 
2055 - 94
  debugOut.analog[14] = height/100L;
95
  debugOut.analog[15] = dHeight;
96
 
2026 - 97
  if (heightError > 0) {
1960 - 98
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
2035 - 99
  } else {
1961 - 100
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
2035 - 101
  }
102
 
103
  if (dHeight > 0) {
1960 - 104
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
2035 - 105
  } else {
106
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
1960 - 107
  }
2032 - 108
 
2033 - 109
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
110
  iHeight += heightError;
2032 - 111
 
2033 - 112
#define IHEIGHT_SCALE 24
113
  // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
2035 - 114
  int16_t dThrottleI =  (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
2033 - 115
 
2035 - 116
  if (dThrottleI > staticParams.heightControlMaxIntegral) {
117
    dThrottleI = staticParams.heightControlMaxIntegral;
118
    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
119
  } else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
120
    dThrottleI = -staticParams.heightControlMaxIntegral;
121
    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
2033 - 122
  }
123
 
2035 - 124
  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
125
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
2033 - 126
 
2055 - 127
  //debugOut.analog[24] = dThrottleP;
128
  //debugOut.analog[25] = dThrottleI;
129
  //debugOut.analog[26] = dThrottleD;
2032 - 130
 
2045 - 131
  //debugOut.analog[27] = dynamicParams.heightP;
132
  //debugOut.analog[28] = dynamicParams.heightI;
133
  //debugOut.analog[29] = dynamicParams.heightD;
2035 - 134
 
135
  int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD;
136
 
1961 - 137
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
138
    dThrottle = staticParams.heightControlMaxThrottleChange;
139
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
140
    dThrottle = -staticParams.heightControlMaxThrottleChange;
2026 - 141
 
2055 - 142
  /*
2035 - 143
  debugOut.analog[19] = throttle;
144
  debugOut.analog[20] = dThrottle;
145
  debugOut.analog[21] = height;
146
  debugOut.analog[22] = rampedTargetHeight;
147
  debugOut.analog[23] = heightError;
2055 - 148
  */
2026 - 149
 
1961 - 150
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
151
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
1960 - 152
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
153
      // If switch is not in use --> Just apply height control.
154
      // If switch is in use     --> only apply height control when switch is also ON.
155
      throttle += dThrottle;
156
    }
157
  }
1961 - 158
 
159
  /* Experiment: Find hover-throttle */
160
 
161
#define DEFAULT_HOVERTHROTTLE 50
162
int32_t stronglyFilteredHeightDiff = 0;
2036 - 163
// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
1961 - 164
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
165
#define HOVERTHROTTLEFILTER 25
1960 - 166
 
167
  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
168
                                * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
169
  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
170
                                                          - 1) + throttle) / HOVERTHROTTLEFILTER;
171
 
2035 - 172
  /*
1960 - 173
  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
174
      && stronglyFilteredHeightDiff > -3) {
175
    hoverThrottle = stronglyFilteredThrottle;
176
    debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
177
  } else
178
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
2035 - 179
 
180
    */
1960 - 181
 
2048 - 182
  PRTY[CONTROL_THROTTLE] = throttle;
1775 - 183
}
184
 
185
/*
1821 - 186
 For a variometer thingy:
187
 When switch is thrown on, freeze throttle (capture it into variable)
188
 For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
189
 Output = frozen throttle + whatever is computed +/-. Integral?
190
 */