Rev 1990 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
||
3 | // + Nur für den privaten Gebrauch |
||
4 | // + www.MikroKopter.com |
||
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
||
1963 | - | 7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
1612 | dongfang | 8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
||
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
||
11 | // + Verkauf von Luftbildaufnahmen, usw. |
||
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
||
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
||
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
||
1963 | - | 17 | // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
||
1612 | dongfang | 19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
||
21 | // + Benutzung auf eigene Gefahr |
||
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
||
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
||
25 | // + mit unserer Zustimmung zulässig |
||
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
||
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
||
30 | // + this list of conditions and the following disclaimer. |
||
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
||
32 | // + from this software without specific prior written permission. |
||
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
||
34 | // + for non-commercial use (directly or indirectly) |
||
1868 | - | 35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
1612 | dongfang | 36 | // + with our written permission |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
||
38 | // + clearly linked as origin |
||
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
||
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
||
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
||
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||
1963 | - | 47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
48 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||
1612 | dongfang | 49 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
50 | // + POSSIBILITY OF SUCH DAMAGE. |
||
51 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
52 | /************************************************************************/ |
||
53 | /* Flight Attitude */ |
||
54 | /************************************************************************/ |
||
55 | |||
56 | #include <stdlib.h> |
||
57 | #include <avr/io.h> |
||
58 | |||
59 | #include "attitude.h" |
||
60 | #include "dongfangMath.h" |
||
61 | |||
1775 | - | 62 | // For scope debugging only! |
63 | #include "rc.h" |
||
64 | |||
1612 | dongfang | 65 | // where our main data flow comes from. |
66 | #include "analog.h" |
||
67 | |||
68 | #include "configuration.h" |
||
1775 | - | 69 | #include "output.h" |
1612 | dongfang | 70 | |
71 | // Some calculations are performed depending on some stick related things. |
||
72 | #include "controlMixer.h" |
||
73 | |||
74 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
75 | |||
76 | /* |
||
77 | * Gyro readings, as read from the analog module. It would have been nice to flow |
||
78 | * them around between the different calculations as a struct or array (doing |
||
79 | * things functionally without side effects) but this is shorter and probably |
||
80 | * faster too. |
||
81 | * The variables are overwritten at each attitude calculation invocation - the values |
||
82 | * are not preserved or reused. |
||
83 | */ |
||
1775 | - | 84 | int16_t rate_ATT[2], yawRate; |
1612 | dongfang | 85 | |
86 | // With different (less) filtering |
||
1645 | - | 87 | int16_t rate_PID[2]; |
88 | int16_t differential[2]; |
||
1612 | dongfang | 89 | |
90 | /* |
||
91 | * Gyro readings, after performing "axis coupling" - that is, the transfomation |
||
92 | * of rotation rates from the airframe-local coordinate system to a ground-fixed |
||
93 | * coordinate system. If axis copling is disabled, the gyro readings will be |
||
94 | * copied into these directly. |
||
95 | * These are global for the same pragmatic reason as with the gyro readings. |
||
96 | * The variables are overwritten at each attitude calculation invocation - the values |
||
97 | * are not preserved or reused. |
||
98 | */ |
||
1645 | - | 99 | int16_t ACRate[2], ACYawRate; |
1612 | dongfang | 100 | |
101 | /* |
||
102 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
||
103 | * horizontal plane, yaw relative to yaw at start. |
||
104 | */ |
||
1775 | - | 105 | int32_t angle[2], yawAngleDiff; |
1612 | dongfang | 106 | |
107 | int readingHeight = 0; |
||
108 | |||
1805 | - | 109 | // Yaw angle and compass stuff. |
110 | |||
111 | // This is updated/written from MM3. Negative angle indicates invalid data. |
||
112 | int16_t compassHeading = -1; |
||
113 | |||
114 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
||
115 | int16_t compassCourse = -1; |
||
116 | |||
117 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
||
118 | // Not necessary. Never read anywhere. |
||
119 | // int16_t compassOffCourse = 0; |
||
120 | |||
121 | uint8_t updateCompassCourse = 0; |
||
122 | uint8_t compassCalState = 0; |
||
123 | uint16_t ignoreCompassTimer = 500; |
||
124 | |||
1612 | dongfang | 125 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
1775 | - | 126 | int16_t yawGyroDrift; |
1612 | dongfang | 127 | |
1616 | dongfang | 128 | #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
129 | #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
||
130 | #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
||
1612 | dongfang | 131 | |
1805 | - | 132 | int16_t correctionSum[2] = { 0, 0 }; |
1612 | dongfang | 133 | |
1775 | - | 134 | // For NaviCTRL use. |
1805 | - | 135 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
1775 | - | 136 | |
1612 | dongfang | 137 | /* |
138 | * Experiment: Compensating for dynamic-induced gyro biasing. |
||
139 | */ |
||
1805 | - | 140 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
1612 | dongfang | 141 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
142 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
||
143 | // int16_t dynamicCalCount; |
||
144 | |||
1980 | - | 145 | uint16_t accVector; |
146 | |||
1612 | dongfang | 147 | /************************************************************************ |
148 | * Set inclination angles from the acc. sensor data. |
||
149 | * If acc. sensors are not used, set to zero. |
||
150 | * TODO: One could use inverse sine to calculate the angles more |
||
1616 | dongfang | 151 | * accurately, but since: 1) the angles are rather small at times when |
152 | * it makes sense to set the integrals (standing on ground, or flying at |
||
1612 | dongfang | 153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
154 | * it is hardly worth the trouble. |
||
155 | ************************************************************************/ |
||
156 | |||
1645 | - | 157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
1991 | - | 158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];// + correctionTerm; |
||
1612 | dongfang | 160 | } |
161 | |||
162 | void setStaticAttitudeAngles(void) { |
||
163 | #ifdef ATTITUDE_USE_ACC_SENSORS |
||
1869 | - | 164 | angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
165 | angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
||
1612 | dongfang | 166 | #else |
1869 | - | 167 | angle[PITCH] = angle[ROLL] = 0; |
1612 | dongfang | 168 | #endif |
169 | } |
||
170 | |||
171 | /************************************************************************ |
||
172 | * Neutral Readings |
||
173 | ************************************************************************/ |
||
174 | void attitude_setNeutral(void) { |
||
1869 | - | 175 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
1960 | - | 176 | dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
1612 | dongfang | 177 | |
1869 | - | 178 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
179 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
||
1612 | dongfang | 180 | |
1869 | - | 181 | // Calibrate hardware. |
1961 | - | 182 | analog_setNeutral(); |
1612 | dongfang | 183 | |
1869 | - | 184 | // reset gyro integrals to acc guessing |
185 | setStaticAttitudeAngles(); |
||
186 | yawAngleDiff = 0; |
||
1612 | dongfang | 187 | |
1869 | - | 188 | // update compass course to current heading |
189 | compassCourse = compassHeading; |
||
1805 | - | 190 | |
1869 | - | 191 | // Inititialize YawGyroIntegral value with current compass heading |
192 | yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW; |
||
1805 | - | 193 | |
1869 | - | 194 | // Servo_On(); //enable servo output |
1612 | dongfang | 195 | } |
196 | |||
197 | /************************************************************************ |
||
198 | * Get sensor data from the analog module, and release the ADC |
||
199 | * TODO: Ultimately, the analog module could do this (instead of dumping |
||
1645 | - | 200 | * the values into variables). |
201 | * The rate variable end up in a range of about [-1024, 1023]. |
||
1612 | dongfang | 202 | *************************************************************************/ |
203 | void getAnalogData(void) { |
||
1869 | - | 204 | uint8_t axis; |
1612 | dongfang | 205 | |
1955 | - | 206 | analog_update(); |
207 | |||
1869 | - | 208 | for (axis = PITCH; axis <= ROLL; axis++) { |
1963 | - | 209 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
210 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
||
1869 | - | 211 | differential[axis] = gyroD[axis]; |
212 | averageAcc[axis] += acc[axis]; |
||
213 | } |
||
1775 | - | 214 | |
1869 | - | 215 | averageAccCount++; |
216 | yawRate = yawGyro + driftCompYaw; |
||
1805 | - | 217 | |
1869 | - | 218 | // We are done reading variables from the analog module. |
219 | // Interrupt-driven sensor reading may restart. |
||
1955 | - | 220 | startAnalogConversionCycle(); |
1612 | dongfang | 221 | } |
222 | |||
223 | /* |
||
224 | * This is the standard flight-style coordinate system transformation |
||
225 | * (from airframe-local axes to a ground-based system). For some reason |
||
226 | * the MK uses a left-hand coordinate system. The tranformation has been |
||
227 | * changed accordingly. |
||
228 | */ |
||
229 | void trigAxisCoupling(void) { |
||
1869 | - | 230 | int16_t cospitch = int_cos(angle[PITCH]); |
231 | int16_t cosroll = int_cos(angle[ROLL]); |
||
232 | int16_t sinroll = int_sin(angle[ROLL]); |
||
1866 | - | 233 | |
1870 | - | 234 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
1869 | - | 235 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
1866 | - | 236 | |
1870 | - | 237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
238 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
||
1869 | - | 239 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
1866 | - | 240 | |
1870 | - | 241 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
1872 | - | 242 | |
243 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
||
1612 | dongfang | 244 | } |
245 | |||
1775 | - | 246 | // 480 usec with axis coupling - almost no time without. |
1612 | dongfang | 247 | void integrate(void) { |
1869 | - | 248 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
249 | uint8_t axis; |
||
1872 | - | 250 | |
1963 | - | 251 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) { |
1869 | - | 252 | trigAxisCoupling(); |
253 | } else { |
||
254 | ACRate[PITCH] = rate_ATT[PITCH]; |
||
255 | ACRate[ROLL] = rate_ATT[ROLL]; |
||
256 | ACYawRate = yawRate; |
||
257 | } |
||
1612 | dongfang | 258 | |
1869 | - | 259 | /* |
260 | * Yaw |
||
261 | * Calculate yaw gyro integral (~ to rotation angle) |
||
262 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
||
263 | */ |
||
264 | yawGyroHeading += ACYawRate; |
||
265 | yawAngleDiff += yawRate; |
||
1612 | dongfang | 266 | |
1869 | - | 267 | if (yawGyroHeading >= YAWOVER360) { |
268 | yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
||
269 | } else if (yawGyroHeading < 0) { |
||
270 | yawGyroHeading += YAWOVER360; |
||
271 | } |
||
1805 | - | 272 | |
1869 | - | 273 | /* |
274 | * Pitch axis integration and range boundary wrap. |
||
275 | */ |
||
276 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
277 | angle[axis] += ACRate[axis]; |
||
278 | if (angle[axis] > PITCHROLLOVER180) { |
||
279 | angle[axis] -= PITCHROLLOVER360; |
||
280 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
||
281 | angle[axis] += PITCHROLLOVER360; |
||
282 | } |
||
283 | } |
||
1612 | dongfang | 284 | } |
285 | |||
286 | /************************************************************************ |
||
287 | * A kind of 0'th order integral correction, that corrects the integrals |
||
288 | * directly. This is the "gyroAccFactor" stuff in the original code. |
||
1646 | - | 289 | * There is (there) also a drift compensation |
1612 | dongfang | 290 | * - it corrects the differential of the integral = the gyro offsets. |
291 | * That should only be necessary with drifty gyros like ENC-03. |
||
292 | ************************************************************************/ |
||
293 | void correctIntegralsByAcc0thOrder(void) { |
||
1869 | - | 294 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
295 | // are less than ....., or reintroduce Kalman. |
||
296 | // Well actually the Z axis acc. check is not so silly. |
||
297 | uint8_t axis; |
||
298 | int32_t temp; |
||
1908 | - | 299 | |
1988 | - | 300 | uint8_t ca = controlActivity >> 8; |
301 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
||
302 | |||
303 | if (highControlActivity) { |
||
304 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
||
305 | } else { |
||
306 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
||
307 | } |
||
308 | |||
1980 | - | 309 | if (accVector <= dynamicParams.maxAccVector) { |
310 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
||
311 | |||
1960 | - | 312 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
1869 | - | 313 | int32_t accDerived; |
1612 | dongfang | 314 | |
1908 | - | 315 | /* |
1869 | - | 316 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
317 | permilleAcc /= 2; |
||
318 | debugFullWeight = 0; |
||
319 | } |
||
1953 | - | 320 | |
321 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
||
322 | permilleAcc /= 2; |
||
323 | debugFullWeight = 0; |
||
1908 | - | 324 | */ |
1953 | - | 325 | |
1988 | - | 326 | if (highControlActivity) { // reduce effect during stick control activity |
1908 | - | 327 | permilleAcc /= 4; |
1986 | - | 328 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
1908 | - | 329 | permilleAcc /= 4; |
330 | } |
||
1989 | - | 331 | } |
1775 | - | 332 | |
1869 | - | 333 | /* |
334 | * Add to each sum: The amount by which the angle is changed just below. |
||
335 | */ |
||
336 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
337 | accDerived = getAngleEstimateFromAcc(axis); |
||
1955 | - | 338 | debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
1805 | - | 339 | |
1869 | - | 340 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
1963 | - | 341 | temp = angle[axis]; |
1869 | - | 342 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
343 | + (int32_t) permilleAcc * accDerived) / 1000L; |
||
344 | correctionSum[axis] += angle[axis] - temp; |
||
345 | } |
||
346 | } else { |
||
1955 | - | 347 | debugOut.analog[9] = 0; |
348 | debugOut.analog[10] = 0; |
||
1869 | - | 349 | // experiment: Kill drift compensation updates when not flying smooth. |
1963 | - | 350 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
1989 | - | 351 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
1869 | - | 352 | } |
1612 | dongfang | 353 | } |
354 | |||
355 | /************************************************************************ |
||
356 | * This is an attempt to correct not the error in the angle integrals |
||
357 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
||
358 | * cause of it: Gyro drift, vibration and rounding errors. |
||
359 | * All the corrections made in correctIntegralsByAcc0thOrder over |
||
1646 | - | 360 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
361 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
||
1612 | dongfang | 362 | * correction that should have been applied to each iteration to fix |
363 | * the error. This is then added to the dynamic offsets. |
||
364 | ************************************************************************/ |
||
1646 | - | 365 | // 2 times / sec. = 488/2 |
366 | #define DRIFTCORRECTION_TIME 256L |
||
367 | void driftCorrection(void) { |
||
1869 | - | 368 | static int16_t timer = DRIFTCORRECTION_TIME; |
369 | int16_t deltaCorrection; |
||
1872 | - | 370 | int16_t round; |
1869 | - | 371 | uint8_t axis; |
1872 | - | 372 | |
1869 | - | 373 | if (!--timer) { |
374 | timer = DRIFTCORRECTION_TIME; |
||
375 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
376 | // Take the sum of corrections applied, add it to delta |
||
1872 | - | 377 | if (correctionSum[axis] >=0) |
378 | round = DRIFTCORRECTION_TIME / 2; |
||
379 | else |
||
380 | round = -DRIFTCORRECTION_TIME / 2; |
||
381 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
||
1869 | - | 382 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
1960 | - | 383 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
384 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
||
1869 | - | 385 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
1955 | - | 386 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
387 | debugOut.analog[28 + axis] = driftComp[axis]; |
||
1775 | - | 388 | |
1869 | - | 389 | correctionSum[axis] = 0; |
390 | } |
||
391 | } |
||
1612 | dongfang | 392 | } |
393 | |||
1980 | - | 394 | void calculateAccVector(void) { |
395 | uint16_t temp; |
||
396 | temp = filteredAcc[0]/4; |
||
397 | accVector = temp * temp; |
||
398 | temp = filteredAcc[1]/4; |
||
399 | accVector += temp * temp; |
||
400 | temp = filteredAcc[2]/4; |
||
401 | accVector += temp * temp; |
||
1986 | - | 402 | debugOut.analog[18] = accVector; |
403 | debugOut.analog[19] = dynamicParams.maxAccVector; |
||
1980 | - | 404 | } |
405 | |||
1612 | dongfang | 406 | /************************************************************************ |
407 | * Main procedure. |
||
408 | ************************************************************************/ |
||
1805 | - | 409 | void calculateFlightAttitude(void) { |
1869 | - | 410 | getAnalogData(); |
1980 | - | 411 | calculateAccVector(); |
1869 | - | 412 | integrate(); |
1775 | - | 413 | |
1612 | dongfang | 414 | #ifdef ATTITUDE_USE_ACC_SENSORS |
1869 | - | 415 | correctIntegralsByAcc0thOrder(); |
416 | driftCorrection(); |
||
1612 | dongfang | 417 | #endif |
418 | } |
||
419 | |||
1775 | - | 420 | void updateCompass(void) { |
1869 | - | 421 | int16_t w, v, r, correction, error; |
1805 | - | 422 | |
1869 | - | 423 | if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
424 | if (controlMixer_testCompassCalState()) { |
||
425 | compassCalState++; |
||
426 | if (compassCalState < 5) |
||
427 | beepNumber(compassCalState); |
||
428 | else |
||
429 | beep(1000); |
||
430 | } |
||
431 | } else { |
||
432 | // get maximum attitude angle |
||
433 | w = abs(angle[PITCH] / 512); |
||
434 | v = abs(angle[ROLL] / 512); |
||
435 | if (v > w) |
||
436 | w = v; |
||
437 | correction = w / 8 + 1; |
||
438 | // calculate the deviation of the yaw gyro heading and the compass heading |
||
439 | if (compassHeading < 0) |
||
440 | error = 0; // disable yaw drift compensation if compass heading is undefined |
||
441 | else if (abs(yawRate) > 128) { // spinning fast |
||
442 | error = 0; |
||
443 | } else { |
||
444 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
||
445 | error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
||
446 | % 360) - 180; |
||
447 | } |
||
448 | if (!ignoreCompassTimer && w < 25) { |
||
449 | yawGyroDrift += error; |
||
450 | // Basically this gets set if we are in "fix" mode, and when starting. |
||
451 | if (updateCompassCourse) { |
||
452 | beep(200); |
||
453 | yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW; |
||
454 | compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
||
455 | updateCompassCourse = 0; |
||
456 | } |
||
457 | } |
||
458 | yawGyroHeading += (error * 8) / correction; |
||
1805 | - | 459 | |
1869 | - | 460 | /* |
461 | w = (w * dynamicParams.CompassYawEffect) / 32; |
||
462 | w = dynamicParams.CompassYawEffect - w; |
||
463 | */ |
||
1960 | - | 464 | w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
1869 | - | 465 | / 32; |
1805 | - | 466 | |
1869 | - | 467 | // As readable formula: |
468 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
||
1805 | - | 469 | |
1869 | - | 470 | if (w >= 0) { // maxAttitudeAngle < 32 |
471 | if (!ignoreCompassTimer) { |
||
1908 | - | 472 | /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
473 | v = 64 + controlActivity / 100; |
||
1869 | - | 474 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
475 | r |
||
476 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
||
477 | % 360) - 180; |
||
478 | v = (r * w) / v; // align to compass course |
||
479 | // limit yaw rate |
||
1960 | - | 480 | w = 3 * dynamicParams.compassYawEffect; |
1869 | - | 481 | if (v > w) |
482 | v = w; |
||
483 | else if (v < -w) |
||
484 | v = -w; |
||
485 | yawAngleDiff += v; |
||
486 | } else { // wait a while |
||
487 | ignoreCompassTimer--; |
||
488 | } |
||
489 | } else { // ignore compass at extreme attitudes for a while |
||
490 | ignoreCompassTimer = 500; |
||
491 | } |
||
492 | } |
||
1775 | - | 493 | } |
1612 | dongfang | 494 | |
495 | /* |
||
496 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
||
497 | * and to compensate them away. It brings about some improvement, but no miracles. |
||
498 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
||
499 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
||
500 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
||
501 | * speed unfortunately... must find a better way) |
||
502 | */ |
||
503 | /* |
||
1805 | - | 504 | void attitude_startDynamicCalibration(void) { |
505 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
||
506 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
||
507 | } |
||
1612 | dongfang | 508 | |
1805 | - | 509 | void attitude_continueDynamicCalibration(void) { |
510 | // measure dynamic offset now... |
||
511 | dynamicCalPitch += hiResPitchGyro; |
||
512 | dynamicCalRoll += hiResRollGyro; |
||
513 | dynamicCalYaw += rawYawGyroSum; |
||
514 | dynamicCalCount++; |
||
515 | |||
516 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
||
517 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
||
518 | // manual mode |
||
519 | driftCompPitch = dynamicParams.UserParam7 - 128; |
||
520 | driftCompRoll = dynamicParams.UserParam8 - 128; |
||
521 | } else { |
||
522 | // use the sampled value (does not seem to work so well....) |
||
523 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
||
524 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
||
525 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
||
526 | } |
||
527 | |||
528 | // keep resetting these meanwhile, to avoid accumulating errors. |
||
529 | setStaticAttitudeIntegrals(); |
||
530 | yawAngle = 0; |
||
531 | } |
||
532 | */ |