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