Rev 2015 | Rev 2018 | Go to most recent revision | Show entire file | Regard 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; |