Rev 2052 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2052 | Rev 2055 | ||
|---|---|---|---|
| Line 199... | Line 199... | ||
| 199 | heading += ACYawRate; |
199 | heading += ACYawRate; |
| 200 | intervalWrap(&heading, YAWOVER360); |
200 | intervalWrap(&heading, YAWOVER360); |
| Line 201... | Line 201... | ||
| 201 | 201 | ||
| Line 202... | Line -... | ||
| 202 | headingError += ACYawRate; |
- | |
| 203 | - | ||
| 204 | debugOut.analog[27] = heading / 100; |
202 | headingError += ACYawRate; |
| 205 | 203 | ||
| 206 | /* |
204 | /* |
| 207 | * Pitch axis integration and range boundary wrap. |
205 | * Pitch axis integration and range boundary wrap. |
| 208 | */ |
206 | */ |
| Line 266... | Line 264... | ||
| 266 | /* |
264 | /* |
| 267 | * Add to each sum: The amount by which the angle is changed just below. |
265 | * Add to each sum: The amount by which the angle is changed just below. |
| 268 | */ |
266 | */ |
| 269 | for (axis = PITCH; axis <= ROLL; axis++) { |
267 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 270 | accDerived = getAngleEstimateFromAcc(axis); |
268 | accDerived = getAngleEstimateFromAcc(axis); |
| 271 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
269 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| 272 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
270 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
| 273 | temp = attitude[axis]; |
271 | temp = attitude[axis]; |
| 274 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
272 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| 275 | + (int32_t) permilleAcc * accDerived) / 1000L; |
273 | + (int32_t) permilleAcc * accDerived) / 1000L; |
| 276 | correctionSum[axis] += attitude[axis] - temp; |
274 | correctionSum[axis] += attitude[axis] - temp; |
| 277 | } |
275 | } |
| 278 | } else { |
276 | } else { |
| 279 | debugOut.analog[9] = 0; |
- | |
| 280 | debugOut.analog[10] = 0; |
- | |
| 281 | // experiment: Kill drift compensation updates when not flying smooth. |
277 | // experiment: Kill drift compensation updates when not flying smooth. |
| 282 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
278 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| 283 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
279 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| 284 | } |
280 | } |
| 285 | } |
281 | } |
| Line 355... | Line 351... | ||
| 355 | 351 | ||
| 356 | void correctHeadingToMagnetic(void) { |
352 | void correctHeadingToMagnetic(void) { |
| Line 357... | Line 353... | ||
| 357 | int32_t error; |
353 | int32_t error; |
| 358 | 354 | ||
| 359 | if (commands_isCalibratingCompass()) { |
355 | if (commands_isCalibratingCompass()) { |
| 360 | debugOut.analog[29] = 1; |
356 | //debugOut.analog[29] = 1; |
| Line 361... | Line 357... | ||
| 361 | return; |
357 | return; |
| 362 | } |
358 | } |
| 363 | 359 | ||
| 364 | // Compass is off, skip. |
360 | // Compass is off, skip. |
| Line 365... | Line 361... | ||
| 365 | // Naaah this is assumed. |
361 | // Naaah this is assumed. |
| 366 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
362 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
| 367 | // return; |
363 | // return; |
| 368 | 364 | ||
| 369 | // Compass is invalid, skip. |
365 | // Compass is invalid, skip. |
| Line 370... | Line 366... | ||
| 370 | if (magneticHeading < 0) { |
366 | if (magneticHeading < 0) { |
| 371 | debugOut.analog[29] = 2; |
367 | //debugOut.analog[29] = 2; |
| 372 | return; |
368 | return; |
| 373 | } |
369 | } |
| 374 | 370 | ||
| Line 375... | Line 371... | ||
| 375 | // Spinning fast, skip |
371 | // Spinning fast, skip |
| 376 | if (abs(yawRate) > 128) { |
372 | if (abs(yawRate) > 128) { |
| 377 | debugOut.analog[29] = 3; |
373 | //debugOut.analog[29] = 3; |
| 378 | return; |
374 | return; |
| 379 | } |
375 | } |
| 380 | 376 | ||
| Line 381... | Line 377... | ||
| 381 | // Otherwise invalidated, skip |
377 | // Otherwise invalidated, skip |
| 382 | if (ignoreCompassTimer) { |
378 | if (ignoreCompassTimer) { |
| Line 394... | Line 390... | ||
| 394 | // better resolution of the gyros to the worse resolution of the compass all the time. |
390 | // better resolution of the gyros to the worse resolution of the compass all the time. |
| 395 | // The correction should really only serve to compensate for gyro drift. |
391 | // The correction should really only serve to compensate for gyro drift. |
| 396 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
392 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
| Line 397... | Line 393... | ||
| 397 | 393 | ||
| 398 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
394 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
| Line 399... | Line 395... | ||
| 399 | debugOut.analog[30] = correction; |
395 | //debugOut.analog[30] = correction; |
| 400 | 396 | ||
| 401 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
397 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
| 402 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
398 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
| Line 407... | Line 403... | ||
| 407 | 403 | ||
| 408 | // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
404 | // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
| 409 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
405 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
| 410 | // This will however cause drift to remain uncorrected! |
406 | // This will however cause drift to remain uncorrected! |
| 411 | // headingError += correction; |
407 | // headingError += correction; |
| 412 | debugOut.analog[29] = 0; |
408 | //debugOut.analog[29] = 0; |
| 413 | } |
409 | } |
| Line 414... | Line 410... | ||
| 414 | #endif |
410 | #endif |
| 415 | 411 | ||