Subversion Repositories FlightCtrl

Rev

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

Rev 2033 Rev 2035
Line 75... Line 75...
75 
      }
 75 
      }
76 
    }
 76 
    }
77 
  }
 77 
  }
Line 78... Line 78...
78 
 
 78 
 
79 
  // height, in meters (so the division factor is: 100)
 - 
 
80 
  // debugOut.analog[30] = filteredAirPressure / 10;
 79 
  // height, in meters (so the division factor is: 100)
81 
  debugOut.analog[30] = (117100 - filteredAirPressure) / 100;
 80 
  debugOut.analog[30] = (117100 - filteredAirPressure) / 100;
82 
  // Calculated 0 alt number: 108205
 81 
  // Calculated 0 alt number: 108205
83 
  // Experimental 0 alt number: 117100
 82 
  // Experimental 0 alt number: 117100
Line 93... Line 92...
93 
  int16_t dHeight = analog_getDHeight();
 92 
  int16_t dHeight = analog_getDHeight();
94 
  //lastHeight = height;
 93 
  //lastHeight = height;
Line 95... Line 94...
95 
 
 94 
 
96 
  if (heightError > 0) {
 95 
  if (heightError > 0) {
97 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
 - 
 
98 
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
 96 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
99 
  } else if (heightError < 0) {
 97 
  } else {
- 
 
 98 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
- 
 
 99 
  }
- 
 
 100 
 
100 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
 101 
  if (dHeight > 0) {
- 
 
 102 
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
- 
 
 103 
  } else {
101 
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
 104 
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
Line 102... Line 105...
102 
  }
 105 
  }
103 
 
 106 
 
Line 104... Line 107...
104 
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
 107 
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
105 
  iHeight += heightError;
 108 
  iHeight += heightError;
106 
 
 109 
 
Line 107... Line -...
107 
#define IHEIGHT_SCALE 24
 - 
 
108 
  // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
 - 
 
109 
  int16_t dThrottle =  (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE);
 - 
 
110 
 
 - 
 
111 
  // dt = (iHeight * staticParams.heightI) >> (IHEIGHT_SCALE) = staticParams.heightControlMaxIntegral
 110 
#define IHEIGHT_SCALE 24
112 
  // (iHeight * staticParams.heightI) = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE)
 111 
  // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
113 
  // iHeight = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) /staticParams.heightI
 112 
  int16_t dThrottleI =  (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
114 
 
 113 
 
115 
  if (dThrottle > staticParams.heightControlMaxIntegral) {
 114 
  if (dThrottleI > staticParams.heightControlMaxIntegral) {
116 
    dThrottle = staticParams.heightControlMaxIntegral;
 115 
    dThrottleI = staticParams.heightControlMaxIntegral;
117 
    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI;
 116 
    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
Line -... Line 117...
- 
 
 117 
  } else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
118 
  } else if (dThrottle < -staticParams.heightControlMaxIntegral) {
 118 
    dThrottleI = -staticParams.heightControlMaxIntegral;
Line -... Line 119...
- 
 
 119 
    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
- 
 
 120 
  }
- 
 
 121 
 
- 
 
 122 
  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
- 
 
 123 
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
- 
 
 124 
 
119 
    dThrottle = -staticParams.heightControlMaxIntegral;
 125 
  debugOut.analog[24] = dThrottleP;
- 
 
 126 
  debugOut.analog[25] = dThrottleI;
- 
 
 127 
  debugOut.analog[26] = dThrottleD;
Line 120... Line 128...
120 
    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI;
 128 
 
121 
  }
 129 
  debugOut.analog[27] = dynamicParams.heightP;
122 
 
 130 
  debugOut.analog[28] = dynamicParams.heightI;
123 
  debugOut.analog[27] = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE);
 131 
  debugOut.analog[29] = dynamicParams.heightD;
Line 124... Line 132...
124 
 
 132 
 
125 
  dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7);
 133 
  int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD;
126 
 
 134 
 
- 
 
 135 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
- 
 
 136 
    dThrottle = staticParams.heightControlMaxThrottleChange;
Line 127... Line 137...
127 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
 137 
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
128 
    dThrottle = staticParams.heightControlMaxThrottleChange;
 138 
    dThrottle = -staticParams.heightControlMaxThrottleChange;
129 
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
 139 
 
130 
    dThrottle = -staticParams.heightControlMaxThrottleChange;
 140 
  debugOut.analog[19] = throttle;
Line 153... Line 163...
153 
  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
 163 
  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
154 
                                * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
 164 
                                * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
155 
  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
 165 
  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
156 
                                                          - 1) + throttle) / HOVERTHROTTLEFILTER;
 166 
                                                          - 1) + throttle) / HOVERTHROTTLEFILTER;
Line -... Line 167...
- 
 
 167 
 
157 
 
 168 
  /*
158 
  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
 169 
  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
159 
      && stronglyFilteredHeightDiff > -3) {
 170 
      && stronglyFilteredHeightDiff > -3) {
160 
    hoverThrottle = stronglyFilteredThrottle;
 171 
    hoverThrottle = stronglyFilteredThrottle;
161 
    debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
 - 
 
162 
    // DebugOut.Analog[18] = hoverThrottle;
 172 
    debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
163 
  } else
 173 
  } else
164 
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
 174 
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
165 
 
 175 
 
Line 166... Line 176...
166 
  //debugOut.analog[20] = dThrottle;
 176 
    */
167 
 
 177 
 
Line 168... Line 178...
168 
  return throttle;
 178 
  return throttle;