5,7 → 5,7 |
#include "dongfangMath.h" |
|
// For scope debugging only! |
#include "rc.h" |
#include "output.h" |
|
// = cos^2(45 degs). |
const int32_t MINPROJECTION = (int32_t) MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
14,15 → 14,15 |
// 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; |
|
// part1 start: 150 usec |
// It's factor (int32_t)MATH_UNIT_FACTOR^2 too high. |
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
projection = 0; |
|
// (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
// part1 end. |
|
uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
int16_t deltaThrottle; |
|
if (projection < MINPROJECTION && projection >= 0) { |
// projection = MINPROJECTION; |
deltaThrottle = 0; |
29,7 → 29,7 |
} else if (projection > -MINPROJECTION && projection < 0) { |
// projection = -MINPROJECITON; |
deltaThrottle = 0; |
} else |
} else { |
/* |
* We need delta throttle = constant/projection1 |
* (constant * MATH_UNIT_FACTOR^2) / projection. |
37,6 → 37,7 |
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; |
} |