| 8,41 → 8,30 |
|---|
| #include "output.h" |
| |
| // = cos^2(45 degs). |
| const int32_t MINPROJECTION = (int32_t) MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
| const int32_t FACTORSQUARED = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2)); |
| const int32_t MINPROJECTION = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2-1)); |
| |
| // 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; |
| int32_t projection; |
| uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
| int16_t deltaThrottle; |
| |
| projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
| |
| /* |
| if (projection < MINPROJECTION && projection >= 0) { |
| deltaThrottle = 0; |
| } else if (projection > -MINPROJECTION && projection < 0) { |
| // projection = -MINPROJECITON; |
| deltaThrottle = 0; |
| } else { |
| */ |
| |
| deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2)); |
| // DebugOut.Analog[13] = deltaThrottle; |
| |
| return throttle + deltaThrottle; |
| } |
| |
| // part1 start: 150 usec |
| // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high. |
| projection = 0; |
| |
| // (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
| // part1 end. |
| if (projection < MINPROJECTION && projection >= 0) { |
| // projection = MINPROJECTION; |
| deltaThrottle = 0; |
| } else if (projection > -MINPROJECTION && projection < 0) { |
| // projection = -MINPROJECITON; |
| deltaThrottle = 0; |
| } else { |
| /* |
| * We need delta throttle = constant/projection1 |
| * (constant * MATH_UNIT_FACTOR^2) / projection. |
| */ |
| deltaThrottle = ((int32_t) effect * (int32_t) MATH_UNIT_FACTOR |
| * (int32_t) MATH_UNIT_FACTOR) / (projection / 10) - effect * 10; |
| // DebugOut.Analog[13] = deltaThrottle; |
| } |
| |
| return throttle + deltaThrottle; |
| } |
| /* |
| har: R = e * k/p |
| vil R = e * ( 1 - k/p ) |
| = e - ek/p |
| */ |