Rev 1805 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1805 | Rev 1821 | ||
|---|---|---|---|
| Line 237... | Line 237... | ||
| 237 | int16_t tanpitch = int_tan(angle[PITCH]); |
237 | int16_t tanpitch = int_tan(angle[PITCH]); |
| 238 | #define ANTIOVF 512 |
238 | #define ANTIOVF 512 |
| 239 | ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
239 | ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
| 240 | * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
240 | * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
| 241 | ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
241 | ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
| 242 | / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) |
242 | / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF |
| 243 | / ANTIOVF * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF |
243 | * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
| 244 | * MATH_UNIT_FACTOR)); |
- | |
| 245 | ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
244 | ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
| 246 | + ((int32_t) yawRate * cosroll) / cospitch; |
245 | + ((int32_t) yawRate * cosroll) / cospitch; |
| 247 | } |
246 | } |
| Line 248... | Line 247... | ||
| 248 | 247 | ||
| Line 325... | Line 324... | ||
| 325 | /* |
324 | /* |
| 326 | * Add to each sum: The amount by which the angle is changed just below. |
325 | * Add to each sum: The amount by which the angle is changed just below. |
| 327 | */ |
326 | */ |
| 328 | for (axis = PITCH; axis <= ROLL; axis++) { |
327 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 329 | accDerived = getAngleEstimateFromAcc(axis); |
328 | accDerived = getAngleEstimateFromAcc(axis); |
| 330 | DebugOut.Analog[9 + axis] = (10 * accDerived) |
329 | DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| 331 | / GYRO_DEG_FACTOR_PITCHROLL; |
- | |
| Line 332... | Line 330... | ||
| 332 | 330 | ||
| 333 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
331 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
| 334 | correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
332 | correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| 335 | angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis] |
333 | angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis] |
| Line 368... | Line 366... | ||
| 368 | uint8_t axis; |
366 | uint8_t axis; |
| 369 | if (!--timer) { |
367 | if (!--timer) { |
| 370 | timer = DRIFTCORRECTION_TIME; |
368 | timer = DRIFTCORRECTION_TIME; |
| 371 | for (axis = PITCH; axis <= ROLL; axis++) { |
369 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 372 | // Take the sum of corrections applied, add it to delta |
370 | // Take the sum of corrections applied, add it to delta |
| 373 | deltaCorrection = (correctionSum[axis] |
371 | deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR |
| 374 | * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) |
372 | + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
| 375 | / DRIFTCORRECTION_TIME; |
- | |
| 376 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
373 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| 377 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
374 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| 378 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
375 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| 379 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
376 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| Line 380... | Line 377... | ||
| 380 | 377 | ||
| 381 | DebugOut.Analog[18 + axis] = deltaCorrection |
- | |
| 382 | / staticParams.GyroAccTrim; |
378 | DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
| Line 383... | Line 379... | ||
| 383 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
379 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
| 384 | 380 | ||
| 385 | correctionSum[axis] = 0; |
381 | correctionSum[axis] = 0; |
| Line 430... | Line 426... | ||
| 430 | // get maximum attitude angle |
426 | // get maximum attitude angle |
| 431 | w = abs(angle[PITCH] / 512); |
427 | w = abs(angle[PITCH] / 512); |
| 432 | v = abs(angle[ROLL] / 512); |
428 | v = abs(angle[ROLL] / 512); |
| 433 | if (v > w) |
429 | if (v > w) |
| 434 | w = v; |
430 | w = v; |
| 435 | correction = w/8 + 1; |
431 | correction = w / 8 + 1; |
| 436 | // calculate the deviation of the yaw gyro heading and the compass heading |
432 | // calculate the deviation of the yaw gyro heading and the compass heading |
| 437 | if (compassHeading < 0) |
433 | if (compassHeading < 0) |
| 438 | error = 0; // disable yaw drift compensation if compass heading is undefined |
434 | error = 0; // disable yaw drift compensation if compass heading is undefined |
| 439 | else |
- | |
| 440 | if (abs(yawRate) > 128) { // spinning fast |
435 | else if (abs(yawRate) > 128) { // spinning fast |
| 441 | error = 0; |
436 | error = 0; |
| 442 | } else { |
437 | } else { |
| 443 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
438 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
| 444 | error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
439 | error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
| - | 440 | % 360) - 180; |
|
| 445 | } |
441 | } |
| 446 | if (!ignoreCompassTimer && w < 25) { |
442 | if (!ignoreCompassTimer && w < 25) { |
| 447 | yawGyroDrift += error; |
443 | yawGyroDrift += error; |
| 448 | // Basically this gets set if we are in "fix" mode, and when starting. |
444 | // Basically this gets set if we are in "fix" mode, and when starting. |
| 449 | if (updateCompassCourse) { |
445 | if (updateCompassCourse) { |
| Line 454... | Line 450... | ||
| 454 | } |
450 | } |
| 455 | } |
451 | } |
| 456 | yawGyroHeading += (error * 8) / correction; |
452 | yawGyroHeading += (error * 8) / correction; |
| Line 457... | Line 453... | ||
| 457 | 453 | ||
| 458 | /* |
454 | /* |
| 459 | w = (w * dynamicParams.CompassYawEffect) / 32; |
455 | w = (w * dynamicParams.CompassYawEffect) / 32; |
| 460 | w = dynamicParams.CompassYawEffect - w; |
456 | w = dynamicParams.CompassYawEffect - w; |
| 461 | */ |
457 | */ |
| - | 458 | w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) |
|
| Line 462... | Line 459... | ||
| 462 | w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32; |
459 | / 32; |
| 463 | 460 | ||
| Line 464... | Line 461... | ||
| 464 | // As readable formula: |
461 | // As readable formula: |
| 465 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
462 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
| 466 | 463 | ||
| 467 | if (w >= 0) { // maxAttitudeAngle < 32 |
464 | if (w >= 0) { // maxAttitudeAngle < 32 |
| - | 465 | if (!ignoreCompassTimer) { |
|
| 468 | if (!ignoreCompassTimer) { |
466 | v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
| - | 467 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
|
| 469 | v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
468 | r |
| 470 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
469 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
| 471 | r = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) % 360) - 180; |
470 | % 360) - 180; |
| 472 | v = (r * w) / v; // align to compass course |
471 | v = (r * w) / v; // align to compass course |
| 473 | // limit yaw rate |
472 | // limit yaw rate |