Rev 2048 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2048 | Rev 2052 | ||
|---|---|---|---|
| 1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
| 2 | #include "attitude.h" |
2 | #include "attitude.h" |
| 3 | #include "uart0.h" |
- | |
| 4 | #include "configuration.h" |
3 | #include "configuration.h" |
| 5 | #include "dongfangMath.h" |
4 | #include "dongfangMath.h" |
| 6 | #include "controlMixer.h" |
5 | #include "controlMixer.h" |
| 7 | 6 | ||
| 8 | // For scope debugging only! |
7 | // For scope debugging only! |
| 9 | #include "output.h" |
8 | #include "output.h" |
| 10 | 9 | ||
| 11 | // = cos^2(45 degs). |
10 | // = cos^2(45 degs). |
| 12 | // const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2); |
11 | // const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2); |
| 13 | const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9); |
12 | const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9); |
| 14 | 13 | ||
| 15 | // Takes 380 - 400 usec. Way too slow. |
14 | // Takes 380 - 400 usec. Way too slow. |
| 16 | // With static MINPROJECTION: 220 usec. |
15 | // With static MINPROJECTION: 220 usec. |
| 17 | void AC_getPRTY(int16_t* PRTY) { |
16 | void AC_getPRTY(int16_t* PRTY) { |
| 18 | int16_t throttle = PRTY[CONTROL_THROTTLE]; |
17 | int16_t throttle = PRTY[CONTROL_THROTTLE]; |
| 19 | int32_t projection; |
18 | int32_t projection; |
| 20 | uint8_t effect = dynamicParams.attitudeControl; // Userparam 3 |
19 | uint8_t effect = dynamicParams.attitudeControl; // Userparam 3 |
| 21 | int16_t deltaThrottle, y; |
20 | int16_t deltaThrottle, y; |
| 22 | 21 | ||
| 23 | int16_t rollAngleInDegrees = attitude[ROLL]/GYRO_DEG_FACTOR_PITCHROLL; |
22 | int16_t rollAngleInDegrees = attitude[ROLL]/GYRO_DEG_FACTOR_PITCHROLL; |
| 24 | int16_t pitchAngleInDegrees = attitude[PITCH]/GYRO_DEG_FACTOR_PITCHROLL; |
23 | int16_t pitchAngleInDegrees = attitude[PITCH]/GYRO_DEG_FACTOR_PITCHROLL; |
| 25 | 24 | ||
| 26 | projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees); |
25 | projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees); |
| 27 | projection >>= 8; |
26 | projection >>= 8; |
| 28 | 27 | ||
| 29 | if (projection < 0) { |
28 | if (projection < 0) { |
| 30 | // Case not yet considered! |
29 | // Case not yet considered! |
| 31 | y = 0; |
30 | y = 0; |
| 32 | } else { |
31 | } else { |
| 33 | if (projection < MINPROJECTION && projection >= 0) { |
32 | if (projection < MINPROJECTION && projection >= 0) { |
| 34 | projection = MINPROJECTION; |
33 | projection = MINPROJECTION; |
| 35 | } else if (projection > -MINPROJECTION && projection < 0) { |
34 | } else if (projection > -MINPROJECTION && projection < 0) { |
| 36 | projection = -MINPROJECTION; |
35 | projection = -MINPROJECTION; |
| 37 | } else { |
36 | } else { |
| 38 | } |
37 | } |
| 39 | y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle; |
38 | y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle; |
| 40 | } |
39 | } |
| 41 | deltaThrottle = ((int32_t)y * effect) >> 6; |
40 | deltaThrottle = ((int32_t)y * effect) >> 6; |
| 42 | // debugOut.analog[8] = deltaThrottle; |
41 | // debugOut.analog[8] = deltaThrottle; |
| 43 | PRTY[CONTROL_THROTTLE] = throttle; |
42 | PRTY[CONTROL_THROTTLE] = throttle; |
| 44 | } |
43 | } |
| 45 | 44 | ||