| 0,0 → 1,40 |
|---|
| #include <inttypes.h> |
| #include "attitude.h" |
| #include "uart0.h" |
| #include "configuration.h" |
| #include "dongfangMath.h" |
| |
| // For scope debugging only! |
| #include "output.h" |
| |
| // = cos^2(45 degs). |
| // const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2); |
| const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9); |
| |
| // Takes 380 - 400 usec. Way too slow. |
| // With static MINPROJECTION: 220 usec. |
| uint16_t AC_getThrottle(uint16_t throttle) { |
| int32_t projection; |
| uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
| int16_t deltaThrottle, y; |
| |
| projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
| projection >>= 8; |
| |
| if (projection < 0) { |
| // Case not yet considered! |
| y = 0; |
| } else { |
| if (projection < MINPROJECTION && projection >= 0) { |
| projection = MINPROJECTION; |
| } else if (projection > -MINPROJECTION && projection < 0) { |
| projection = -MINPROJECTION; |
| } else { |
| } |
| y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection |
| - throttle; |
| } |
| deltaThrottle = ((int32_t)y * effect) >> 6; |
| debugOut.analog[8] = deltaThrottle; |
| return throttle + deltaThrottle; |
| } |