Rev 1872 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1775 | - | 1 | #include <inttypes.h> |
2 | #include "attitude.h" |
||
3 | #include "uart0.h" |
||
4 | #include "configuration.h" |
||
5 | #include "dongfangMath.h" |
||
6 | |||
7 | // For scope debugging only! |
||
1872 | - | 8 | #include "output.h" |
1775 | - | 9 | |
10 | // = cos^2(45 degs). |
||
1877 | - | 11 | const int32_t FACTORSQUARED = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2)); |
12 | const int32_t MINPROJECTION = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2-1)); |
||
1775 | - | 13 | |
14 | // Takes 380 - 400 usec. Way too slow. |
||
15 | // With static MINPROJECTION: 220 usec. |
||
16 | uint16_t AC_getThrottle(uint16_t throttle) { |
||
1877 | - | 17 | int32_t projection; |
18 | uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
||
19 | int16_t deltaThrottle; |
||
20 | |||
21 | projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
||
22 | |||
23 | /* |
||
24 | if (projection < MINPROJECTION && projection >= 0) { |
||
25 | deltaThrottle = 0; |
||
26 | } else if (projection > -MINPROJECTION && projection < 0) { |
||
27 | // projection = -MINPROJECITON; |
||
28 | deltaThrottle = 0; |
||
29 | } else { |
||
30 | */ |
||
31 | |||
32 | deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2)); |
||
33 | // DebugOut.Analog[13] = deltaThrottle; |
||
34 | |||
35 | return throttle + deltaThrottle; |
||
36 | } |
||
1775 | - | 37 |