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