Rev 2116 | Rev 2125 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2116 | Rev 2119 | ||
---|---|---|---|
Line 79... | Line 79... | ||
79 | // Enable timer output compare match A Interrupt only |
79 | // Enable timer output compare match A Interrupt only |
80 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
80 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
81 | TIMSK2 |= (1 << OCIE2A); |
81 | TIMSK2 |= (1 << OCIE2A); |
Line 82... | Line 82... | ||
82 | 82 | ||
83 | for (uint8_t axis=0; axis<2; axis++) |
83 | for (uint8_t axis=0; axis<2; axis++) |
Line 84... | Line 84... | ||
84 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
84 | previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR; |
85 | 85 | ||
Line 86... | Line 86... | ||
86 | SREG = sreg; |
86 | SREG = sreg; |
Line 91... | Line 91... | ||
91 | *****************************************************/ |
91 | *****************************************************/ |
92 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
92 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
93 | int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
93 | int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
94 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
94 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
95 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
95 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
96 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
96 | value *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor; |
97 | value = value >> 8; |
97 | value = value >> 8; |
98 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
98 | if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
99 | return -value; |
99 | return -value; |
100 | return value; |
100 | return value; |
101 | } |
101 | } |
Line 102... | Line 102... | ||
102 | 102 | ||
103 | // With constant-speed limitation. |
103 | // With constant-speed limitation. |
104 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
104 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
105 | int16_t diff = manualValue - previousManualValues[axis]; |
105 | int16_t diff = manualValue - previousManualValues[axis]; |
106 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
106 | uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed; |
107 | if (diff > maxSpeed) diff = maxSpeed; |
107 | if (diff > maxSpeed) diff = maxSpeed; |
108 | else if (diff < -maxSpeed) diff = -maxSpeed; |
108 | else if (diff < -maxSpeed) diff = -maxSpeed; |
109 | manualValue = previousManualValues[axis] + diff; |
109 | manualValue = previousManualValues[axis] + diff; |
110 | previousManualValues[axis] = manualValue; |
110 | previousManualValues[axis] = manualValue; |
111 | return manualValue; |
111 | return manualValue; |
Line 112... | Line 112... | ||
112 | } |
112 | } |
113 | 113 | ||
114 | // add stabilization and manual, apply soft position limits. |
114 | // add stabilization and manual, apply soft position limits. |
115 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
115 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
116 | int16_t featuredServoValue(uint8_t axis) { |
116 | int16_t featuredServoValue(uint8_t axis) { |
117 | int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
117 | int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR); |
118 | value += calculateStabilizedServoAxis(axis); |
118 | value += calculateStabilizedServoAxis(axis); |
119 | int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
119 | int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR; |
120 | if (value < limit) value = limit; |
120 | if (value < limit) value = limit; |
121 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
121 | limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR; |
122 | if (value > limit) value = limit; |
122 | if (value > limit) value = limit; |
123 | value -= (128 * SCALE_FACTOR); |
123 | value -= (128 * SCALE_FACTOR); |
124 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
124 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
125 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
125 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
126 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
126 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
Line 127... | Line 127... | ||
127 | return value + NEUTRAL_PULSELENGTH; |
127 | return value + NEUTRAL_PULSELENGTH; |
128 | } |
128 | } |
129 | 129 | ||
130 | void calculateControlServoValues(void) { |
130 | void calculateControlServoValues(void) { |
131 | int16_t value; |
131 | int16_t value; |
132 | int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR; |
132 | //int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR; |
133 | int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR; |
- | |
134 | for (uint8_t axis=0; axis<4; axis++) { |
133 | //int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR; |
135 | value = controlServos[axis]; |
134 | for (uint8_t axis=0; axis<4; axis++) { |
136 | value *= 2; |
135 | value = controlServos[axis]; |
137 | if (value < minLimit) value = minLimit; |
136 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
138 | else if (value > maxLimit) value = maxLimit; |
137 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
139 | servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
138 | servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
140 | } |
139 | } |