Rev 2051 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2051 | Rev 2052 | ||
|---|---|---|---|
| Line 117... | Line 117... | ||
| 117 | // Calibrate hardware. |
117 | // Calibrate hardware. |
| 118 | analog_setNeutral(); |
118 | analog_setNeutral(); |
| Line 119... | Line 119... | ||
| 119 | 119 | ||
| 120 | // reset gyro integrals to acc guessing |
120 | // reset gyro integrals to acc guessing |
| - | 121 | setStaticAttitudeAngles(); |
|
| 121 | setStaticAttitudeAngles(); |
122 | #ifdef USE_MK3MAG |
| - | 123 | attitude_resetHeadingToMagnetic(); |
|
| 122 | attitude_resetHeadingToMagnetic(); |
124 | #endif |
| 123 | // Servo_On(); //enable servo output |
125 | // Servo_On(); //enable servo output |
| Line 124... | Line 126... | ||
| 124 | } |
126 | } |
| 125 | 127 | ||
| Line 329... | Line 331... | ||
| 329 | temp = filteredAcc[2] >> 3; |
331 | temp = filteredAcc[2] >> 3; |
| 330 | accVector += temp * temp; |
332 | accVector += temp * temp; |
| 331 | //debugOut.analog[18] = accVector; |
333 | //debugOut.analog[18] = accVector; |
| 332 | } |
334 | } |
| Line -... | Line 335... | ||
| - | 335 | ||
| 333 | 336 | #ifdef USE_MK3MAG |
|
| 334 | void attitude_resetHeadingToMagnetic(void) { |
337 | void attitude_resetHeadingToMagnetic(void) { |
| 335 | if (commands_isCalibratingCompass()) |
338 | if (commands_isCalibratingCompass()) |
| Line 336... | Line 339... | ||
| 336 | return; |
339 | return; |
| 337 | 340 | ||
| 338 | // Compass is off, skip. |
341 | // Compass is off, skip. |
| Line 339... | Line 342... | ||
| 339 | if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
342 | if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) |
| 340 | return; |
343 | return; |
| 341 | 344 | ||
| Line 406... | Line 409... | ||
| 406 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
409 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
| 407 | // This will however cause drift to remain uncorrected! |
410 | // This will however cause drift to remain uncorrected! |
| 408 | // headingError += correction; |
411 | // headingError += correction; |
| 409 | debugOut.analog[29] = 0; |
412 | debugOut.analog[29] = 0; |
| 410 | } |
413 | } |
| - | 414 | #endif |
|
| Line 411... | Line 415... | ||
| 411 | 415 | ||
| 412 | /************************************************************************ |
416 | /************************************************************************ |
| 413 | * Main procedure. |
417 | * Main procedure. |
| 414 | ************************************************************************/ |
418 | ************************************************************************/ |
| Line 424... | Line 428... | ||
| 424 | 428 | ||
| 425 | // We are done reading variables from the analog module. |
429 | // We are done reading variables from the analog module. |
| 426 | // Interrupt-driven sensor reading may restart. |
430 | // Interrupt-driven sensor reading may restart. |
| Line -... | Line 431... | ||
| - | 431 | startAnalogConversionCycle(); |
|
| 427 | startAnalogConversionCycle(); |
432 | |
| 428 | 433 | #ifdef USE_MK3MAG |
|
| 429 | if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
434 | if (staticParams.bitConfig & (CFG_COMPASS_ENABLED | CFG_GPS_ENABLED)) { |
| - | 435 | correctHeadingToMagnetic(); |
|
| 430 | correctHeadingToMagnetic(); |
436 | } |
| Line 431... | Line 437... | ||
| 431 | } |
437 | #endif |
| 432 | } |
438 | } |
| 433 | 439 | ||