Subversion Repositories FlightCtrl

Rev

Rev 1796 | Rev 1867 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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