8,6 → 8,7 |
|
#ifdef COARSERESOLUTION |
#define NEUTRAL_PULSELENGTH 938 |
#define STABILIZATION_LOG_DIVIDER 6 |
#define SERVOLIMIT 500 |
#define SCALE_FACTOR 4 |
#define CS2 ((1<<CS21)|(1<<CS20)) |
15,6 → 16,7 |
#else |
|
#define NEUTRAL_PULSELENGTH 3750 |
#define STABILIZATION_LOG_DIVIDER 4 |
#define SERVOLIMIT 2000 |
#define SCALE_FACTOR 16 |
#define CS2 (1<<CS21) |
105,11 → 107,11 |
} servo_t;*/ |
|
int16_t calculateStabilizedServoAxis(uint8_t axis) { |
int32_t value = attitude[axis] / 64L; // between -500000 to 500000 extreme limits. Just about |
int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
// With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
// That is a divisor of about 1<<14. Same conclusion as H&I. |
value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
value /= 256L; |
value = value >> 8; |
if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
return -value; |
return value; |