Rev 2048 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2048 | Rev 2051 | ||
---|---|---|---|
Line 51... | Line 51... | ||
51 | int32_t attitude[2]; |
51 | int32_t attitude[2]; |
Line 52... | Line 52... | ||
52 | 52 | ||
Line 53... | Line 53... | ||
53 | //int readingHeight = 0; |
53 | //int readingHeight = 0; |
54 | - | ||
55 | // Yaw angle and compass stuff. |
- | |
56 | - | ||
57 | // This is updated/written from MM3. Negative angle indicates invalid data. |
- | |
58 | int16_t magneticHeading = -1; |
- | |
59 | - | ||
60 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
- | |
61 | // int16_t headingInDegrees = -1; |
54 | |
Line 62... | Line 55... | ||
62 | 55 | // Yaw angle and compass stuff. |
|
63 | int32_t targetHeading; |
56 | int32_t headingError; |
64 | 57 | ||
Line 65... | Line 58... | ||
65 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
58 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
Line 66... | Line 59... | ||
66 | // Not necessary. Never read anywhere. |
59 | // Not necessary. Never read anywhere. |
67 | // int16_t compassOffCourse = 0; |
60 | // int16_t compassOffCourse = 0; |
Line 68... | Line 61... | ||
68 | 61 | ||
Line 201... | Line 194... | ||
201 | * Calculate yaw gyro integral (~ to rotation angle) |
194 | * Calculate yaw gyro integral (~ to rotation angle) |
202 | * Limit heading proportional to 0 deg to 360 deg |
195 | * Limit heading proportional to 0 deg to 360 deg |
203 | */ |
196 | */ |
204 | heading += ACYawRate; |
197 | heading += ACYawRate; |
205 | intervalWrap(&heading, YAWOVER360); |
198 | intervalWrap(&heading, YAWOVER360); |
- | 199 | ||
- | 200 | headingError += ACYawRate; |
|
- | 201 | ||
- | 202 | debugOut.analog[27] = heading / 100; |
|
- | 203 | ||
206 | /* |
204 | /* |
207 | * Pitch axis integration and range boundary wrap. |
205 | * Pitch axis integration and range boundary wrap. |
208 | */ |
206 | */ |
209 | for (axis = PITCH; axis <= ROLL; axis++) { |
207 | for (axis = PITCH; axis <= ROLL; axis++) { |
210 | attitude[axis] += ACRate[axis]; |
208 | attitude[axis] += ACRate[axis]; |
Line 344... | Line 342... | ||
344 | // Compass is invalid, skip. |
342 | // Compass is invalid, skip. |
345 | if (magneticHeading < 0) |
343 | if (magneticHeading < 0) |
346 | return; |
344 | return; |
Line 347... | Line 345... | ||
347 | 345 | ||
348 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
346 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
- | 347 | //targetHeading = heading; |
|
Line 349... | Line 348... | ||
349 | targetHeading = heading; |
348 | headingError = 0; |
350 | 349 | ||
Line 351... | Line 350... | ||
351 | debugOut.digital[0] ^= DEBUG_COMPASS; |
350 | debugOut.digital[0] ^= DEBUG_COMPASS; |
352 | } |
351 | } |
Line 353... | Line -... | ||
353 | - | ||
354 | void correctHeadingToMagnetic(void) { |
- | |
355 | int32_t error; |
352 | |
- | 353 | void correctHeadingToMagnetic(void) { |
|
356 | 354 | int32_t error; |
|
- | 355 | ||
Line 357... | Line 356... | ||
357 | debugOut.analog[27] = heading; |
356 | if (commands_isCalibratingCompass()) { |
358 | 357 | debugOut.analog[29] = 1; |
|
359 | if (commands_isCalibratingCompass()) |
358 | return; |
360 | return; |
359 | } |
Line 361... | Line 360... | ||
361 | 360 | ||
362 | // Compass is off, skip. |
361 | // Compass is off, skip. |
- | 362 | // Naaah this is assumed. |
|
363 | // Naaah this is assumed. |
363 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
- | 364 | // return; |
|
Line 364... | Line 365... | ||
364 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
365 | |
365 | // return; |
366 | // Compass is invalid, skip. |
- | 367 | if (magneticHeading < 0) { |
|
366 | 368 | debugOut.analog[29] = 2; |
|
- | 369 | return; |
|
Line 367... | Line 370... | ||
367 | // Compass is invalid, skip. |
370 | } |
368 | if (magneticHeading < 0) |
371 | |
369 | return; |
372 | // Spinning fast, skip |
- | 373 | if (abs(yawRate) > 128) { |
|
370 | 374 | debugOut.analog[29] = 3; |
|
371 | // Spinning fast, skip |
375 | return; |
Line 372... | Line 376... | ||
372 | if (abs(yawRate) > 128) |
376 | } |
373 | return; |
377 | |
- | 378 | // Otherwise invalidated, skip |
|
- | 379 | if (ignoreCompassTimer) { |
|
Line 374... | Line 380... | ||
374 | 380 | ignoreCompassTimer--; |
|
375 | // Otherwise invalidated, skip |
381 | debugOut.analog[29] = 4; |
376 | if (ignoreCompassTimer) { |
382 | return; |
377 | ignoreCompassTimer--; |
383 | } |
Line 378... | Line 384... | ||
378 | return; |
384 | |
- | 385 | // TODO: Find computational cost of this. |
|
Line 379... | Line 386... | ||
379 | } |
386 | error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
380 | 387 | if (error <= -YAWOVER180) error += YAWOVER360; |
|
381 | // TODO: Find computational cost of this. |
388 | else if (error > YAWOVER180) error -= YAWOVER360; |
382 | error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
389 | |
383 | 390 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
|
384 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
391 | // better resolution of the gyros to the worse resolution of the compass all the time. |
Line -... | Line 392... | ||
- | 392 | // The correction should really only serve to compensate for gyro drift. |
|
- | 393 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
|
385 | // better resolution of the gyros to the worse resolution of the compass all the time. |
394 | |
386 | // The correction should really only serve to compensate for gyro drift. |
395 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
387 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
- | |
388 | 396 | debugOut.analog[30] = correction; |
|
389 | int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8; |
397 | |
Line 390... | Line 398... | ||
390 | 398 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
|
391 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
399 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
392 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
400 | // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |