Rev 1978 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1978 | Rev 1980 | ||
|---|---|---|---|
| Line 140... | Line 140... | ||
| 140 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
140 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
| 141 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
141 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
| 142 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
142 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
| 143 | // int16_t dynamicCalCount; |
143 | // int16_t dynamicCalCount; |
| Line -... | Line 144... | ||
| - | 144 | ||
| - | 145 | uint16_t accVector; |
|
| 144 | 146 | ||
| 145 | /************************************************************************ |
147 | /************************************************************************ |
| 146 | * Set inclination angles from the acc. sensor data. |
148 | * Set inclination angles from the acc. sensor data. |
| 147 | * If acc. sensors are not used, set to zero. |
149 | * If acc. sensors are not used, set to zero. |
| 148 | * TODO: One could use inverse sine to calculate the angles more |
150 | * TODO: One could use inverse sine to calculate the angles more |
| Line 294... | Line 296... | ||
| 294 | uint8_t axis; |
296 | uint8_t axis; |
| 295 | int32_t temp; |
297 | int32_t temp; |
| 296 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
298 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| 297 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
299 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| Line 298... | Line 300... | ||
| 298 | 300 | ||
| - | 301 | if (accVector <= dynamicParams.maxAccVector) { |
|
| - | 302 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
|
| 299 | if (1 /*controlActivity <= dynamicParams.maxControlActivityForAcc*/) { |
303 | |
| 300 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
304 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
| Line 301... | Line 305... | ||
| 301 | int32_t accDerived; |
305 | int32_t accDerived; |
| 302 | 306 | ||
| Line 311... | Line 315... | ||
| 311 | debugFullWeight = 0; |
315 | debugFullWeight = 0; |
| 312 | */ |
316 | */ |
| Line 313... | Line 317... | ||
| 313 | 317 | ||
| 314 | if (controlActivity > 10000) { // reduce effect during stick control activity |
318 | if (controlActivity > 10000) { // reduce effect during stick control activity |
| 315 | permilleAcc /= 4; |
319 | permilleAcc /= 4; |
| 316 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
320 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| 317 | if (controlActivity > 20000) { // reduce effect during stick control activity |
321 | if (controlActivity > 20000) { // reduce effect during stick control activity |
| 318 | permilleAcc /= 4; |
322 | permilleAcc /= 4; |
| 319 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
323 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| 320 | } |
324 | } |
| Line 379... | Line 383... | ||
| 379 | correctionSum[axis] = 0; |
383 | correctionSum[axis] = 0; |
| 380 | } |
384 | } |
| 381 | } |
385 | } |
| 382 | } |
386 | } |
| Line -... | Line 387... | ||
| - | 387 | ||
| - | 388 | void calculateAccVector(void) { |
|
| - | 389 | uint16_t temp; |
|
| - | 390 | temp = filteredAcc[0]/4; |
|
| - | 391 | accVector = temp * temp; |
|
| - | 392 | temp = filteredAcc[1]/4; |
|
| - | 393 | accVector += temp * temp; |
|
| - | 394 | temp = filteredAcc[2]/4; |
|
| - | 395 | accVector += temp * temp; |
|
| - | 396 | debugOut.analog[19] = accVector; |
|
| - | 397 | } |
|
| 383 | 398 | ||
| 384 | /************************************************************************ |
399 | /************************************************************************ |
| 385 | * Main procedure. |
400 | * Main procedure. |
| 386 | ************************************************************************/ |
401 | ************************************************************************/ |
| 387 | void calculateFlightAttitude(void) { |
402 | void calculateFlightAttitude(void) { |
| - | 403 | getAnalogData(); |
|
| 388 | getAnalogData(); |
404 | calculateAccVector(); |
| Line 389... | Line 405... | ||
| 389 | integrate(); |
405 | integrate(); |
| 390 | 406 | ||
| 391 | #ifdef ATTITUDE_USE_ACC_SENSORS |
407 | #ifdef ATTITUDE_USE_ACC_SENSORS |