Rev 2015 | Rev 2018 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2015 | Rev 2017 | ||
|---|---|---|---|
| Line 2... | Line 2... | ||
| 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 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 132... | Line 132... | ||
| 132 | * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed |
132 | * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed |
| 133 | * 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed |
133 | * 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed |
| 134 | * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
134 | * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
| 135 | */ |
135 | */ |
| Line -... | Line 136... | ||
| - | 136 | ||
| - | 137 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
|
| - | 138 | ||
| 136 | 139 | /* |
|
| 137 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
140 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
| 138 | static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
141 | static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
| 139 | // Pitch to Pitch part |
142 | // Pitch to Pitch part |
| 140 | int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
143 | int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
| Line 156... | Line 159... | ||
| 156 | // of 2 11 bit numbers, so 12 bits. We have 4 bits left... |
159 | // of 2 11 bit numbers, so 12 bits. We have 4 bits left... |
| 157 | result[0] = (result[0]*11) >> 4; |
160 | result[0] = (result[0]*11) >> 4; |
| 158 | result[1] = (result[1]*11) >> 4; |
161 | result[1] = (result[1]*11) >> 4; |
| 159 | } |
162 | } |
| 160 | } |
163 | } |
| 161 | 164 | */ |
|
| 162 | /* |
165 | /* |
| 163 | * Air pressure |
166 | * Air pressure |
| 164 | */ |
167 | */ |
| 165 | volatile uint8_t rangewidth = 105; |
168 | volatile uint8_t rangewidth = 105; |
| Line 299... | Line 302... | ||
| 299 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
302 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
| 300 | } |
303 | } |
| Line 301... | Line 304... | ||
| 301 | 304 | ||
| 302 | void startAnalogConversionCycle(void) { |
305 | void startAnalogConversionCycle(void) { |
| - | 306 | analogDataReady = 0; |
|
| 303 | analogDataReady = 0; |
307 | |
| 304 | // Stop the sampling. Cycle is over. |
308 | // Stop the sampling. Cycle is over. |
| 305 | for (uint8_t i = 0; i < 8; i++) { |
309 | for (uint8_t i = 0; i < 8; i++) { |
| 306 | sensorInputs[i] = 0; |
310 | sensorInputs[i] = 0; |
| 307 | } |
311 | } |
| Line 338... | Line 342... | ||
| 338 | int16_t tempOffsetGyro[2], tempGyro; |
342 | int16_t tempOffsetGyro[2], tempGyro; |
| Line 339... | Line 343... | ||
| 339 | 343 | ||
| 340 | debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
344 | debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
| 341 | for (uint8_t axis=0; axis<2; axis++) { |
345 | for (uint8_t axis=0; axis<2; axis++) { |
| 342 | tempGyro = rawGyroValue(axis); |
- | |
| 343 | 346 | tempGyro = rawGyroValue(axis); |
|
| 344 | /* |
347 | /* |
| 345 | * Process the gyro data for the PID controller. |
348 | * Process the gyro data for the PID controller. |
| 346 | */ |
349 | */ |
| 347 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
350 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
| Line 384... | Line 387... | ||
| 384 | /* |
387 | /* |
| 385 | * Now process the data for attitude angles. |
388 | * Now process the data for attitude angles. |
| 386 | */ |
389 | */ |
| 387 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
390 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
| Line -... | Line 391... | ||
| - | 391 | ||
| - | 392 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
|
| - | 393 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
|
| - | 394 | ||
| - | 395 | debugOut.analog[22 + 0] = gyro_PID[0]; |
|
| - | 396 | debugOut.analog[22 + 1] = gyro_PID[1]; |
|
| - | 397 | ||
| - | 398 | debugOut.analog[24 + 0] = gyro_ATT[0]; |
|
| - | 399 | debugOut.analog[24 + 1] = gyro_ATT[1]; |
|
| 388 | 400 | ||
| 389 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
401 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
| 390 | // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
402 | // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
| Line 391... | Line 403... | ||
| 391 | // gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
403 | // gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |