Rev 2026 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2026 | Rev 2032 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + Nur für den privaten Gebrauch |
3 | // + Nur f�r den privaten Gebrauch |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
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 und nicht-kommerziellen Gebrauch zulässig ist. |
7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist. |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
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 |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
17 | // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
17 | // + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
21 | // + Benutzung auf eigene Gefahr |
21 | // + Benutzung auf eigene Gefahr |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
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 |
25 | // + mit unserer Zustimmung zul�ssig |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
30 | // + this list of conditions and the following disclaimer. |
30 | // + this list of conditions and the following disclaimer. |
Line 154... | Line 154... | ||
154 | * it is hardly worth the trouble. |
154 | * it is hardly worth the trouble. |
155 | ************************************************************************/ |
155 | ************************************************************************/ |
Line 156... | Line 156... | ||
156 | 156 | ||
157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
- | 159 | return GYRO_ACC_FACTOR * filteredAcc[axis];// + correctionTerm; |
|
159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];// + correctionTerm; |
160 | // return 342L * filteredAcc[axis]; |
Line 160... | Line 161... | ||
160 | } |
161 | } |
161 | 162 | ||
162 | void setStaticAttitudeAngles(void) { |
163 | void setStaticAttitudeAngles(void) { |
Line 171... | Line 172... | ||
171 | /************************************************************************ |
172 | /************************************************************************ |
172 | * Neutral Readings |
173 | * Neutral Readings |
173 | ************************************************************************/ |
174 | ************************************************************************/ |
174 | void attitude_setNeutral(void) { |
175 | void attitude_setNeutral(void) { |
175 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
176 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
176 | dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
177 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
Line 177... | Line 178... | ||
177 | 178 | ||
178 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
179 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
Line 179... | Line 180... | ||
179 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
180 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
Line 329... | Line 330... | ||
329 | /* |
330 | /* |
330 | * Add to each sum: The amount by which the angle is changed just below. |
331 | * Add to each sum: The amount by which the angle is changed just below. |
331 | */ |
332 | */ |
332 | for (axis = PITCH; axis <= ROLL; axis++) { |
333 | for (axis = PITCH; axis <= ROLL; axis++) { |
333 | accDerived = getAngleEstimateFromAcc(axis); |
334 | accDerived = getAngleEstimateFromAcc(axis); |
334 | debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
335 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
335 | - | ||
336 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
336 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
337 | temp = angle[axis]; |
337 | temp = angle[axis]; |
338 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
338 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
339 | + (int32_t) permilleAcc * accDerived) / 1000L; |
339 | + (int32_t) permilleAcc * accDerived) / 1000L; |
340 | correctionSum[axis] += angle[axis] - temp; |
340 | correctionSum[axis] += angle[axis] - temp; |
341 | } |
341 | } |
342 | } else { |
342 | } else { |
343 | debugOut.analog[9] = 0; |
343 | //debugOut.analog[9] = 0; |
344 | debugOut.analog[10] = 0; |
344 | //debugOut.analog[10] = 0; |
345 | // experiment: Kill drift compensation updates when not flying smooth. |
345 | // experiment: Kill drift compensation updates when not flying smooth. |
346 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
346 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
347 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
347 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
348 | } |
348 | } |
- | 349 | ||
- | 350 | int32_t accDerived = getAngleEstimateFromAcc(0); |
|
- | 351 | debugOut.analog[9 + 0] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
|
- | 352 | ||
- | 353 | accDerived = getAngleEstimateFromAcc(1); |
|
- | 354 | debugOut.analog[9 + 1] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
|
349 | } |
355 | } |
Line 350... | Line 356... | ||
350 | 356 | ||
351 | /************************************************************************ |
357 | /************************************************************************ |
352 | * This is an attempt to correct not the error in the angle integrals |
358 | * This is an attempt to correct not the error in the angle integrals |