| 119,7 → 119,9 |
|---|
| |
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| #ifdef USE_MK3MAG |
| attitude_resetHeadingToMagnetic(); |
| #endif |
| // Servo_On(); //enable servo output |
| } |
| |
| 331,12 → 333,13 |
|---|
| //debugOut.analog[18] = accVector; |
| } |
| |
| #ifdef USE_MK3MAG |
| void attitude_resetHeadingToMagnetic(void) { |
| if (commands_isCalibratingCompass()) |
| return; |
| |
| // Compass is off, skip. |
| if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
| if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) |
| return; |
| |
| // Compass is invalid, skip. |
| 408,6 → 411,7 |
|---|
| // headingError += correction; |
| debugOut.analog[29] = 0; |
| } |
| #endif |
| |
| /************************************************************************ |
| * Main procedure. |
| 426,9 → 430,11 |
|---|
| // Interrupt-driven sensor reading may restart. |
| startAnalogConversionCycle(); |
| |
| if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
| #ifdef USE_MK3MAG |
| if (staticParams.bitConfig & (CFG_COMPASS_ENABLED | CFG_GPS_ENABLED)) { |
| correctHeadingToMagnetic(); |
| } |
| #endif |
| } |
| |
| /* |