8,8 → 8,8 |
#include "output.h" |
|
// = cos^2(45 degs). |
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)); |
// 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. |
16,22 → 16,25 |
uint16_t AC_getThrottle(uint16_t throttle) { |
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]); |
|
/* |
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) { |
deltaThrottle = 0; |
projection = MINPROJECTION; |
} else if (projection > -MINPROJECTION && projection < 0) { |
// projection = -MINPROJECITON; |
deltaThrottle = 0; |
projection = -MINPROJECTION; |
} else { |
*/ |
|
deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2)); |
// DebugOut.Analog[13] = deltaThrottle; |
|
} |
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; |
} |
|