Details | 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! |
||
| 8 | #include "rc.h" |
||
| 9 | |||
| 10 | // = cos^2(45 degs). |
||
| 11 | const int32_t MINPROJECTION = (int32_t)MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
||
| 12 | |||
| 13 | // Takes 380 - 400 usec. Way too slow. |
||
| 14 | // With static MINPROJECTION: 220 usec. |
||
| 15 | uint16_t AC_getThrottle(uint16_t throttle) { |
||
| 16 | int32_t projection; |
||
| 17 | |||
| 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]); |
||
| 21 | // part1 end. |
||
| 22 | |||
| 23 | uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
||
| 24 | int16_t deltaThrottle; |
||
| 25 | |||
| 26 | if (projection < MINPROJECTION && projection >= 0) { |
||
| 27 | // projection = MINPROJECTION; |
||
| 28 | deltaThrottle = 0; |
||
| 29 | } else if (projection >- MINPROJECTION && projection<0) { |
||
| 30 | // projection = -MINPROJECITON; |
||
| 31 | deltaThrottle = 0; |
||
| 32 | } else |
||
| 33 | /* |
||
| 34 | * We need delta throttle = constant/projection1 |
||
| 35 | * (constant * MATH_UNIT_FACTOR^2) / projection. |
||
| 36 | */ |
||
| 37 | deltaThrottle = ((int32_t)effect * (int32_t)MATH_UNIT_FACTOR * (int32_t)MATH_UNIT_FACTOR) / (projection / 10) - effect * 10; |
||
| 38 | // DebugOut.Analog[13] = deltaThrottle; |
||
| 39 | |||
| 40 | return throttle + deltaThrottle; |
||
| 41 | } |
||
| 42 | /* |
||
| 43 | har: R = e * k/p |
||
| 44 | vil R = e * ( 1 - k/p ) |
||
| 45 | = e - ek/p |
||
| 46 | */ |