Rev 1612 | 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), |
||
7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
||
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 |
||
17 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
||
18 | // + eindeutig als Ursprung verlinkt werden |
||
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) |
||
35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
||
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 |
||
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
49 | // + POSSIBILITY OF SUCH DAMAGE. |
||
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
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 | |||
62 | // where our main data flow comes from. |
||
63 | #include "analog.h" |
||
64 | |||
65 | #include "configuration.h" |
||
66 | |||
67 | // Some calculations are performed depending on some stick related things. |
||
68 | #include "controlMixer.h" |
||
69 | |||
70 | // For Servo_On / Off |
||
71 | // #include "timer2.h" |
||
72 | |||
73 | #ifdef USE_MK3MAG |
||
74 | #include "mk3mag.h" |
||
75 | #include "gps.h" |
||
76 | #endif |
||
77 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
78 | |||
79 | /* |
||
80 | * Gyro readings, as read from the analog module. It would have been nice to flow |
||
81 | * them around between the different calculations as a struct or array (doing |
||
82 | * things functionally without side effects) but this is shorter and probably |
||
83 | * faster too. |
||
84 | * The variables are overwritten at each attitude calculation invocation - the values |
||
85 | * are not preserved or reused. |
||
86 | */ |
||
87 | int16_t pitchRate, rollRate, yawRate; |
||
88 | |||
89 | // With different (less) filtering |
||
90 | int16_t pitchRate_PID, rollRate_PID; |
||
91 | int16_t pitchDifferential, rollDifferential; |
||
92 | |||
93 | /* |
||
94 | * Gyro readings, after performing "axis coupling" - that is, the transfomation |
||
95 | * of rotation rates from the airframe-local coordinate system to a ground-fixed |
||
96 | * coordinate system. If axis copling is disabled, the gyro readings will be |
||
97 | * copied into these directly. |
||
98 | * These are global for the same pragmatic reason as with the gyro readings. |
||
99 | * The variables are overwritten at each attitude calculation invocation - the values |
||
100 | * are not preserved or reused. |
||
101 | */ |
||
102 | int16_t ACPitchRate, ACRollRate, ACYawRate; |
||
103 | |||
104 | /* |
||
105 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
||
106 | * horizontal plane, yaw relative to yaw at start. |
||
107 | */ |
||
108 | int32_t pitchAngle, rollAngle, yawAngle; |
||
109 | |||
110 | int readingHeight = 0; |
||
111 | |||
112 | // compass course |
||
113 | int16_t compassHeading = -1; // negative angle indicates invalid data. |
||
114 | int16_t compassCourse = -1; |
||
115 | int16_t compassOffCourse = 0; |
||
116 | uint16_t updateCompassCourse = 0; |
||
117 | uint8_t compassCalState = 0; |
||
118 | |||
119 | // uint8_t FunnelCourse = 0; |
||
120 | uint16_t badCompassHeading = 500; |
||
121 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
||
122 | |||
1616 | dongfang | 123 | #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
124 | #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
||
125 | #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
||
1612 | dongfang | 126 | |
127 | int32_t pitchCorrectionSum = 0, rollCorrectionSum = 0; |
||
128 | |||
129 | /* |
||
130 | * Experiment: Compensating for dynamic-induced gyro biasing. |
||
131 | */ |
||
132 | int16_t dynamicOffsetPitch = 0, dynamicOffsetRoll = 0, dynamicOffsetYaw = 0; |
||
133 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
||
134 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
||
135 | // int16_t dynamicCalCount; |
||
136 | |||
137 | /************************************************************************ |
||
138 | * Set inclination angles from the acc. sensor data. |
||
139 | * If acc. sensors are not used, set to zero. |
||
140 | * TODO: One could use inverse sine to calculate the angles more |
||
1616 | dongfang | 141 | * accurately, but since: 1) the angles are rather small at times when |
142 | * it makes sense to set the integrals (standing on ground, or flying at |
||
1612 | dongfang | 143 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
144 | * it is hardly worth the trouble. |
||
145 | ************************************************************************/ |
||
146 | |||
147 | int32_t getPitchAngleEstimateFromAcc(void) { |
||
148 | return GYRO_ACC_FACTOR * (int32_t)filteredPitchAxisAcc; |
||
149 | } |
||
150 | |||
151 | int32_t getRollAngleEstimateFromAcc(void) { |
||
152 | return GYRO_ACC_FACTOR * (int32_t)filteredRollAxisAcc; |
||
153 | } |
||
154 | |||
155 | void setStaticAttitudeAngles(void) { |
||
156 | #ifdef ATTITUDE_USE_ACC_SENSORS |
||
157 | pitchAngle = getPitchAngleEstimateFromAcc(); |
||
158 | rollAngle = getRollAngleEstimateFromAcc(); |
||
159 | #else |
||
160 | pitchAngle = 0; |
||
161 | rollAngle = 0; |
||
162 | #endif |
||
163 | } |
||
164 | |||
165 | /************************************************************************ |
||
166 | * Neutral Readings |
||
167 | ************************************************************************/ |
||
168 | void attitude_setNeutral(void) { |
||
169 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
||
170 | dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
||
171 | |||
172 | dynamicOffsetPitch = dynamicOffsetRoll = 0; |
||
173 | |||
174 | // Calibrate hardware. |
||
175 | analog_calibrate(); |
||
176 | |||
177 | // reset gyro readings |
||
178 | pitchRate = rollRate = yawRate = 0; |
||
179 | |||
180 | // reset gyro integrals to acc guessing |
||
181 | setStaticAttitudeAngles(); |
||
182 | yawAngle = 0; |
||
183 | |||
184 | // update compass course to current heading |
||
185 | compassCourse = compassHeading; |
||
186 | // Inititialize YawGyroIntegral value with current compass heading |
||
187 | yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
||
188 | |||
189 | // Servo_On(); //enable servo output |
||
190 | } |
||
191 | |||
192 | /************************************************************************ |
||
193 | * Get sensor data from the analog module, and release the ADC |
||
194 | * TODO: Ultimately, the analog module could do this (instead of dumping |
||
195 | * the values into variables). |
||
196 | *************************************************************************/ |
||
197 | void getAnalogData(void) { |
||
198 | // For the differential calculation. Diff. is not supported right now. |
||
199 | // int16_t d2Pitch, d2Roll; |
||
200 | pitchRate_PID = (hiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR; |
||
201 | pitchRate = (filteredHiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR; |
||
202 | pitchDifferential = pitchGyroD; |
||
203 | |||
204 | rollRate_PID = (hiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR; |
||
205 | rollRate = (filteredHiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR; |
||
206 | rollDifferential = rollGyroD; |
||
207 | |||
208 | yawRate = yawGyro + dynamicOffsetYaw; |
||
209 | |||
210 | // We are done reading variables from the analog module. Interrupt-driven sensor reading may restart. |
||
211 | analogDataReady = 0; |
||
212 | analog_start(); |
||
213 | } |
||
214 | |||
215 | /************************************************************************ |
||
216 | * Axis coupling, H&I Style |
||
217 | * Currently not working (and there is a bug in it, |
||
218 | * which causes unstable flight in heading-hold mode). |
||
219 | ************************************************************************/ |
||
220 | void H_and_I_axisCoupling(void) { |
||
221 | int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0; |
||
222 | int16_t CouplingNickRoll = 0, CouplingRollNick = 0; |
||
223 | |||
224 | tmp13 = (rollRate * pitchAngle) / 2048L; |
||
225 | tmp13 *= dynamicParams.AxisCoupling2; // 65 |
||
226 | tmp13 /= 4096L; |
||
227 | CouplingNickRoll = tmp13; |
||
228 | |||
229 | tmp14 = (pitchRate * rollAngle) / 2048L; |
||
230 | tmp14 *= dynamicParams.AxisCoupling2; // 65 |
||
231 | tmp14 /= 4096L; |
||
232 | CouplingRollNick = tmp14; |
||
233 | |||
234 | tmp14 -= tmp13; |
||
235 | |||
236 | ACYawRate = yawRate + tmp14; |
||
237 | |||
238 | /* |
||
239 | if(!dynamicParams.AxisCouplingYawCorrection) ACYawRate = yawRate - tmp14 / 2; // force yaw |
||
240 | else ACYawRate |
||
241 | */ |
||
242 | |||
243 | tmpl = ((yawRate + tmp14) * pitchAngle) / 2048L; |
||
244 | tmpl *= dynamicParams.AxisCoupling1; |
||
245 | tmpl /= 4096L; |
||
246 | |||
247 | tmpl2 = ((yawRate + tmp14) * rollAngle) / 2048L; |
||
248 | tmpl2 *= dynamicParams.AxisCoupling1; |
||
249 | tmpl2 /= 4096L; |
||
250 | |||
251 | // if(abs(yawRate > 64)) { |
||
252 | // if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
||
253 | // } |
||
254 | |||
255 | ACPitchRate = pitchRate - tmpl2 + tmpl / 100L; |
||
256 | ACRollRate = rollRate + tmpl - tmpl2 / 100L; |
||
257 | } |
||
258 | |||
259 | /* |
||
260 | * This is the standard flight-style coordinate system transformation |
||
261 | * (from airframe-local axes to a ground-based system). For some reason |
||
262 | * the MK uses a left-hand coordinate system. The tranformation has been |
||
263 | * changed accordingly. |
||
264 | */ |
||
265 | void trigAxisCoupling(void) { |
||
266 | int16_t cospitch = int_cos(pitchAngle); |
||
267 | int16_t cosroll = int_cos(rollAngle); |
||
268 | int16_t sinroll = int_sin(rollAngle); |
||
269 | int16_t tanpitch = int_tan(pitchAngle); |
||
270 | #define ANTIOVF 1024 |
||
271 | ACPitchRate = ((int32_t)pitchRate * cosroll + (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
||
272 | ACRollRate = rollRate + (((int32_t)pitchRate * sinroll / ANTIOVF * tanpitch - (int32_t)yawRate * int_cos(rollAngle) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
||
273 | ACYawRate = (-(int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
||
274 | } |
||
275 | |||
276 | void integrate(void) { |
||
277 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
||
278 | if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
||
279 | // The rotary rate limiter bit is abused for selecting axis coupling algorithm instead. |
||
280 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) |
||
281 | trigAxisCoupling(); |
||
282 | else |
||
283 | H_and_I_axisCoupling(); |
||
284 | } else { |
||
285 | ACPitchRate = pitchRate; |
||
286 | ACRollRate = rollRate; |
||
287 | ACYawRate = yawRate; |
||
288 | } |
||
289 | |||
290 | DebugOut.Analog[3] = pitchRate; |
||
291 | DebugOut.Analog[3 + 3] = ACPitchRate; |
||
292 | DebugOut.Analog[4] = rollRate; |
||
293 | DebugOut.Analog[4 + 3] = ACRollRate; |
||
294 | DebugOut.Analog[5] = yawRate; |
||
295 | DebugOut.Analog[5 + 3] = ACYawRate; |
||
296 | |||
297 | /* |
||
298 | DebugOut.Analog[9] = int_cos(pitchAngle); |
||
299 | DebugOut.Analog[10] = int_sin(pitchAngle); |
||
300 | DebugOut.Analog[11] = int_tan(pitchAngle); |
||
301 | */ |
||
302 | |||
303 | /* |
||
304 | * Yaw |
||
305 | * Calculate yaw gyro integral (~ to rotation angle) |
||
306 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
||
307 | */ |
||
308 | yawGyroHeading += ACYawRate; |
||
309 | yawAngle += ACYawRate; |
||
1616 | dongfang | 310 | if(yawGyroHeading >= YAWOVER360) yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
311 | else if(yawGyroHeading < 0) yawGyroHeading += YAWOVER360; |
||
1612 | dongfang | 312 | |
313 | /* |
||
314 | * Pitch axis integration and range boundary wrap. |
||
315 | */ |
||
316 | pitchAngle += ACPitchRate; |
||
1616 | dongfang | 317 | if(pitchAngle > PITCHROLLOVER180) { |
318 | pitchAngle -= PITCHROLLOVER360; |
||
319 | } else if (pitchAngle <= -PITCHROLLOVER180) { |
||
320 | pitchAngle += PITCHROLLOVER360; |
||
1612 | dongfang | 321 | } |
322 | |||
323 | /* |
||
324 | * Pitch axis integration and range boundary wrap. |
||
325 | */ |
||
326 | rollAngle += ACRollRate; |
||
1616 | dongfang | 327 | if(rollAngle > PITCHROLLOVER180) { |
328 | rollAngle -= PITCHROLLOVER360; |
||
329 | } else if (rollAngle <= -PITCHROLLOVER180) { |
||
330 | rollAngle += PITCHROLLOVER360; |
||
1612 | dongfang | 331 | } |
332 | } |
||
333 | |||
334 | /************************************************************************ |
||
335 | * A kind of 0'th order integral correction, that corrects the integrals |
||
336 | * directly. This is the "gyroAccFactor" stuff in the original code. |
||
337 | * There is (there) also what I would call a "minus 1st order correction" |
||
338 | * - it corrects the differential of the integral = the gyro offsets. |
||
339 | * That should only be necessary with drifty gyros like ENC-03. |
||
340 | ************************************************************************/ |
||
341 | void correctIntegralsByAcc0thOrder(void) { |
||
342 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
||
343 | // are less than ....., or reintroduce Kalman. |
||
344 | // Well actually the Z axis acc. check is not so silly. |
||
345 | if(!looping && //((ZAxisAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction |
||
346 | ZAxisAcc >= -dynamicParams.UserParams[7] && ZAxisAcc <= dynamicParams.UserParams[7]) { |
||
347 | DebugOut.Digital[0] = 1; |
||
348 | |||
349 | uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
||
350 | uint8_t debugFullWeight = 1; |
||
351 | |||
352 | int32_t accDerivedPitch = getPitchAngleEstimateFromAcc(); |
||
353 | int32_t accDerivedRoll = getRollAngleEstimateFromAcc(); |
||
354 | |||
355 | if((maxControlPitch > 64) || (maxControlRoll > 64)) { // reduce effect during stick commands |
||
356 | permilleAcc /= 2; |
||
357 | debugFullWeight = 0; |
||
358 | } |
||
359 | |||
360 | if(abs(controlYaw) > 25) { // reduce further if yaw stick is active |
||
361 | permilleAcc /= 2; |
||
362 | debugFullWeight = 0; |
||
363 | } |
||
364 | |||
365 | /* |
||
366 | * Add to each sum: The amount by which the angle is changed just below. |
||
367 | */ |
||
368 | pitchCorrectionSum += permilleAcc * (accDerivedPitch - pitchAngle); |
||
369 | rollCorrectionSum += permilleAcc * (accDerivedRoll - rollAngle); |
||
370 | |||
371 | // There should not be a risk of overflow here, since the integrals do not exceed a few 100000. |
||
372 | pitchAngle = ((int32_t)(1000 - permilleAcc) * pitchAngle + (int32_t)permilleAcc * accDerivedPitch) / 1000L; |
||
373 | rollAngle = ((int32_t)(1000 - permilleAcc) * rollAngle + (int32_t)permilleAcc * accDerivedRoll) / 1000L; |
||
374 | |||
375 | DebugOut.Digital[1] = debugFullWeight; |
||
376 | } else { |
||
377 | DebugOut.Digital[0] = 0; |
||
378 | } |
||
379 | } |
||
380 | |||
381 | /************************************************************************ |
||
382 | * This is an attempt to correct not the error in the angle integrals |
||
383 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
||
384 | * cause of it: Gyro drift, vibration and rounding errors. |
||
385 | * All the corrections made in correctIntegralsByAcc0thOrder over |
||
386 | * MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is |
||
387 | * then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx. |
||
388 | * correction that should have been applied to each iteration to fix |
||
389 | * the error. This is then added to the dynamic offsets. |
||
390 | ************************************************************************/ |
||
391 | // 2 times / sec. |
||
392 | #define DRIFTCORRECTION_TIME 488/2 |
||
393 | void driftCompensation(void) { |
||
394 | static int16_t timer = DRIFTCORRECTION_TIME; |
||
395 | int16_t deltaCompensation; |
||
396 | if (! --timer) { |
||
397 | timer = DRIFTCORRECTION_TIME; |
||
398 | deltaCompensation = ((pitchCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME); |
||
399 | CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp); |
||
400 | dynamicOffsetPitch += deltaCompensation / staticParams.GyroAccTrim; |
||
401 | |||
402 | deltaCompensation = ((rollCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME); |
||
403 | CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp); |
||
404 | dynamicOffsetRoll += deltaCompensation / staticParams.GyroAccTrim; |
||
405 | |||
406 | pitchCorrectionSum = rollCorrectionSum = 0; |
||
407 | |||
408 | DebugOut.Analog[28] = dynamicOffsetPitch; |
||
409 | DebugOut.Analog[29] = dynamicOffsetRoll; |
||
410 | } |
||
411 | } |
||
412 | |||
413 | /************************************************************************ |
||
414 | * Main procedure. |
||
415 | ************************************************************************/ |
||
416 | void calculateFlightAttitude(void) { |
||
417 | getAnalogData(); |
||
418 | integrate(); |
||
419 | #ifdef ATTITUDE_USE_ACC_SENSORS |
||
420 | correctIntegralsByAcc0thOrder(); |
||
421 | driftCompensation(); |
||
422 | #endif |
||
423 | } |
||
424 | |||
425 | /* |
||
426 | void updateCompass(void) { |
||
427 | int16_t w, v, r,correction, error; |
||
428 | |||
429 | if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
||
430 | setCompassCalState(); |
||
431 | } else { |
||
432 | // get maximum attitude angle |
||
433 | w = abs(pitchAngle / 512); |
||
434 | v = abs(rollAngle / 512); |
||
435 | if(v > w) w = v; |
||
436 | correction = w / 8 + 1; |
||
437 | // calculate the deviation of the yaw gyro heading and the compass heading |
||
438 | if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
||
439 | else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
||
440 | if(abs(yawRate) > 128) { // spinning fast |
||
441 | error = 0; |
||
442 | } |
||
443 | if(!badCompassHeading && w < 25) { |
||
444 | if(updateCompassCourse) { |
||
445 | beep(200); |
||
446 | yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
||
447 | compassCourse = (int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
||
448 | updateCompassCourse = 0; |
||
449 | } |
||
450 | } |
||
451 | yawGyroHeading += (error * 8) / correction; |
||
452 | w = (w * dynamicParams.CompassYawEffect) / 32; |
||
453 | w = dynamicParams.CompassYawEffect - w; |
||
454 | if(w >= 0) { |
||
455 | if(!badCompassHeading) { |
||
456 | v = 64 + (maxControlPitch + maxControlRoll) / 8; |
||
457 | // calc course deviation |
||
458 | r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
||
459 | v = (r * w) / v; // align to compass course |
||
460 | // limit yaw rate |
||
461 | w = 3 * dynamicParams.CompassYawEffect; |
||
462 | if (v > w) v = w; |
||
463 | else if (v < -w) v = -w; |
||
464 | yawAngle += v; |
||
465 | } |
||
466 | else |
||
467 | { // wait a while |
||
468 | badCompassHeading--; |
||
469 | } |
||
470 | } |
||
471 | else { // ignore compass at extreme attitudes for a while |
||
472 | badCompassHeading = 500; |
||
473 | } |
||
474 | } |
||
475 | } |
||
476 | */ |
||
477 | |||
478 | /* |
||
479 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
||
480 | * and to compensate them away. It brings about some improvement, but no miracles. |
||
481 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
||
482 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
||
483 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
||
484 | * speed unfortunately... must find a better way) |
||
485 | */ |
||
486 | /* |
||
487 | void attitude_startDynamicCalibration(void) { |
||
488 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
||
489 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
||
490 | } |
||
491 | |||
492 | void attitude_continueDynamicCalibration(void) { |
||
493 | // measure dynamic offset now... |
||
494 | dynamicCalPitch += hiResPitchGyro; |
||
495 | dynamicCalRoll += hiResRollGyro; |
||
496 | dynamicCalYaw += rawYawGyroSum; |
||
497 | dynamicCalCount++; |
||
498 | |||
499 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
||
500 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
||
501 | // manual mode |
||
502 | dynamicOffsetPitch = dynamicParams.UserParam7 - 128; |
||
503 | dynamicOffsetRoll = dynamicParams.UserParam8 - 128; |
||
504 | } else { |
||
505 | // use the sampled value (does not seem to work so well....) |
||
506 | dynamicOffsetPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
||
507 | dynamicOffsetRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
||
508 | dynamicOffsetYaw = -dynamicCalYaw / dynamicCalCount; |
||
509 | } |
||
510 | |||
511 | // keep resetting these meanwhile, to avoid accumulating errors. |
||
512 | setStaticAttitudeIntegrals(); |
||
513 | yawAngle = 0; |
||
514 | } |
||
515 | */ |