Rev 1872 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1872 | Rev 1877 | ||
---|---|---|---|
Line 6... | Line 6... | ||
6 | 6 | ||
7 | // For scope debugging only! |
7 | // For scope debugging only! |
Line 8... | Line 8... | ||
8 | #include "output.h" |
8 | #include "output.h" |
- | 9 | ||
9 | 10 | // = cos^2(45 degs). |
|
Line 10... | Line 11... | ||
10 | // = cos^2(45 degs). |
11 | const int32_t FACTORSQUARED = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2)); |
11 | const int32_t MINPROJECTION = (int32_t) MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
12 | const int32_t MINPROJECTION = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2-1)); |
12 | 13 | ||
13 | // Takes 380 - 400 usec. Way too slow. |
14 | // Takes 380 - 400 usec. Way too slow. |
14 | // With static MINPROJECTION: 220 usec. |
15 | // With static MINPROJECTION: 220 usec. |
15 | uint16_t AC_getThrottle(uint16_t throttle) { |
16 | uint16_t AC_getThrottle(uint16_t throttle) { |
16 | int32_t projection; |
17 | int32_t projection; |
17 | uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
- | |
18 | int16_t deltaThrottle; |
18 | uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
19 | - | ||
20 | // part1 start: 150 usec |
19 | int16_t deltaThrottle; |
21 | // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high. |
- | |
22 | projection = 0; |
20 | |
23 | 21 | projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
|
24 | // (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
- | |
25 | // part1 end. |
22 | |
26 | if (projection < MINPROJECTION && projection >= 0) { |
23 | /* |
27 | // projection = MINPROJECTION; |
24 | if (projection < MINPROJECTION && projection >= 0) { |
28 | deltaThrottle = 0; |
25 | deltaThrottle = 0; |
29 | } else if (projection > -MINPROJECTION && projection < 0) { |
26 | } else if (projection > -MINPROJECTION && projection < 0) { |
30 | // projection = -MINPROJECITON; |
27 | // projection = -MINPROJECITON; |
31 | deltaThrottle = 0; |
- | |
32 | } else { |
- | |
33 | /* |
28 | deltaThrottle = 0; |
34 | * We need delta throttle = constant/projection1 |
29 | } else { |
35 | * (constant * MATH_UNIT_FACTOR^2) / projection. |
- | |
36 | */ |
30 | */ |
37 | deltaThrottle = ((int32_t) effect * (int32_t) MATH_UNIT_FACTOR |
- | |
38 | * (int32_t) MATH_UNIT_FACTOR) / (projection / 10) - effect * 10; |
31 | |
39 | // DebugOut.Analog[13] = deltaThrottle; |
32 | deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2)); |
40 | } |
33 | // DebugOut.Analog[13] = deltaThrottle; |
41 | 34 | ||
42 | return throttle + deltaThrottle; |
- | |
43 | } |
- | |
44 | /* |
- | |
45 | har: R = e * k/p |
- |