Rev 2069 | Rev 2164 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2069 | Rev 2070 | ||
---|---|---|---|
Line 6... | Line 6... | ||
6 | 6 | ||
Line 7... | Line 7... | ||
7 | #define COARSERESOLUTION 1 |
7 | #define COARSERESOLUTION 1 |
8 | 8 | ||
- | 9 | #ifdef COARSERESOLUTION |
|
9 | #ifdef COARSERESOLUTION |
10 | #define NEUTRAL_PULSELENGTH 938 |
10 | #define NEUTRAL_PULSELENGTH 938 |
11 | #define STABILIZATION_LOG_DIVIDER 6 |
11 | #define SERVOLIMIT 500 |
12 | #define SERVOLIMIT 500 |
Line 12... | Line 13... | ||
12 | #define SCALE_FACTOR 4 |
13 | #define SCALE_FACTOR 4 |
Line 13... | Line 14... | ||
13 | #define CS2 ((1<<CS21)|(1<<CS20)) |
14 | #define CS2 ((1<<CS21)|(1<<CS20)) |
- | 15 | ||
14 | 16 | #else |
|
15 | #else |
17 | |
16 | 18 | #define NEUTRAL_PULSELENGTH 3750 |
|
17 | #define NEUTRAL_PULSELENGTH 3750 |
19 | #define STABILIZATION_LOG_DIVIDER 4 |
Line 103... | Line 105... | ||
103 | uint8_t maxValue; |
105 | uint8_t maxValue; |
104 | uint8_t flags; |
106 | uint8_t flags; |
105 | } servo_t;*/ |
107 | } servo_t;*/ |
Line 106... | Line 108... | ||
106 | 108 | ||
107 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
109 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
108 | int32_t value = attitude[axis] / 64L; // between -500000 to 500000 extreme limits. Just about |
110 | int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
109 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
111 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
110 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
112 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
111 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
113 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
112 | value /= 256L; |
114 | value = value >> 8; |
113 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
115 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
114 | return -value; |
116 | return -value; |
115 | return value; |
117 | return value; |