Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | 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
#include "controlMixer.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
/*
16
#define DEBUGINTEGRAL 0
17
#define DEBUGDIFFERENTIAL 0
18
#define DEBUGHOVERTHROTTLE 0
19
#define DEBUGHEIGHTSWITCH 0
20
*/
21
 
22
#define LATCH_TIME 40
23
 
24
int32_t groundPressure;
25
 
26
int32_t targetHeight;
27
int32_t rampedTargetHeight;
28
 
29
#define DEFAULT_HOVERTHROTTLE 50
30
int32_t stronglyFilteredHeightDiff = 0;
31
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
32
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
33
#define HOVERTHROTTLEFILTER 25
34
 
35
uint8_t heightRampingTimer = 0;
36
int32_t maxHeight;
37
int32_t iHeight;
38
/*
39
  These parameters are free to take:
40
        uint8_t HeightMinGas;           // Value : 0-100
41
        uint8_t HeightD;                // Value : 0-250
42
        uint8_t MaxHeight;              // Value : 0-32
43
        uint8_t HeightP;                // Value : 0-32
44
        uint8_t Height_Gain;            // Value : 0-50
45
        uint8_t Height_ACC_Effect;      // Value : 0-250
46
 
47
 */
48
 
49
int32_t getHeight(void) {
50
  return groundPressure - filteredAirPressure;
51
}
52
 
53
void HC_setGround(void) {
54
  groundPressure = filteredAirPressure;
55
  // This should also happen when height control is enabled in-flight.
56
  rampedTargetHeight = getHeight();
57
  maxHeight = 0;
58
  iHeight = 0;
59
}
60
 
61
void HC_update(void) {
62
  int32_t height = getHeight();
63
  static uint8_t setHeightLatch = 0;
64
  if (height > maxHeight)
65
    maxHeight = height;
66
 
67
  if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
68
    DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
69
    if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) {
70
      if (setHeightLatch <= LATCH_TIME) {
71
        if (setHeightLatch == LATCH_TIME) {
72
          targetHeight = height;
73
          DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
74
        }
75
        setHeightLatch++;
76
      }
77
    } else {
78
      setHeightLatch = 0;
79
      DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
80
    }
81
  } else {
82
    DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
83
    targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;
84
  }
85
 
86
  if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) {
87
    heightRampingTimer = 0;
88
    if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {
89
      rampedTargetHeight += staticParams.Height_Gain;
90
    } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {
91
      rampedTargetHeight -= staticParams.Height_Gain;
92
    }
93
  }
94
 
95
  //DebugOut.Analog[16] = (int16_t)(height / 10);
96
  //DebugOut.Analog[17] = (int16_t)(rampedTargetHeight / 10);
97
}
98
 
99
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
100
// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH
101
// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH
102
 
103
// takes 180-200 usec (with integral term). That is too heavy!!!
104
// takes 100 usec without integral term.
105
uint16_t HC_getThrottle(uint16_t throttle) {
106
  int32_t height = getHeight();
107
  int32_t heightError = rampedTargetHeight - height;
108
 
109
  static int32_t lastHeight;
110
 
111
  int16_t dHeight = height - lastHeight;
112
  lastHeight = height;
113
 
114
  // DebugOut.Analog[20] = dHeight;
115
  // DebugOut.Analog[21] = dynamicParams.MaxHeight;
116
 
117
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... 
118
  // iHeight += heightError;
119
 
120
  if (dHeight > 0) {
121
    DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF;
122
    DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF;
123
  } else if (dHeight < 0) {
124
    DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF;
125
    DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF;
126
  }
127
 
128
  /*
129
  if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
130
  else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
131
  else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
132
  else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
133
  */
134
 
135
  int16_t dThrottle = heightError * staticParams.HeightP / 1000 /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */ - dHeight * staticParams.HeightD;
136
 
137
  // the "minGas" is now a limit for how much up / down the throttle can be varied
138
  if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas;
139
  else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas;
140
 
141
  //DebugOut.Analog[18] = dThrottle;
142
  //DebugOut.Analog[19] = iHeight / 10000L;
143
 
144
  // TODO: Eliminate repitition.
145
  if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
146
    if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
147
      // If switch in use, we only apply height control when switch is also on.
148
      if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) {
149
        throttle += dThrottle;
150
      }
151
    } else {
152
      // Switch not in use - just apply height control.
153
      throttle += dThrottle;
154
    }
155
  }
156
 
157
  /* Experiment: Find hover-throttle */
158
  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
159
  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER - 1) + throttle) / HOVERTHROTTLEFILTER;
160
 
161
  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 && stronglyFilteredHeightDiff > -3) {
162
    hoverThrottle = stronglyFilteredThrottle;
163
    DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE;
164
    DebugOut.Analog[18] = hoverThrottle;
165
  } else
166
    DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE;
167
  return throttle;
168
}