Rev 2139 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2139 | Rev 2142 | ||
|---|---|---|---|
| Line 137... | Line 137... | ||
| 137 | int16_t value; |
137 | int16_t value; |
| 138 | for (uint8_t axis=0; axis<4; axis++) { |
138 | for (uint8_t axis=0; axis<4; axis++) { |
| 139 | value = controlServos[axis]; |
139 | value = controlServos[axis]; |
| Line 140... | Line 140... | ||
| 140 | 140 | ||
| 141 | // Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal. |
141 | // Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal. |
| 142 | int16_t min = (staticParams.servos[axis].minValue * SERVO_NORMAL_LIMIT) >> 6; |
142 | int16_t min = (int16_t)staticParams.servos[axis].minValue * (SERVO_NORMAL_LIMIT >> 6); |
| Line 143... | Line 143... | ||
| 143 | int16_t max = (staticParams.servos[axis].maxValue * SERVO_NORMAL_LIMIT) >> 6; |
143 | int16_t max = (int16_t)staticParams.servos[axis].maxValue * (SERVO_NORMAL_LIMIT >> 6); |
| 144 | 144 | ||
| Line 145... | Line 145... | ||
| 145 | if (value < min) value = min; |
145 | if (value < -min) value = -min; |
| 146 | else if (value > max) value = max; |
146 | else if (value > max) value = max; |
| Line -... | Line 147... | ||
| - | 147 | ||
| - | 148 | if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT; |
|
| 147 | 149 | else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT; |
|
| 148 | if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT; |
150 | |
| 149 | else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT; |
151 | debugOut.analog[24+axis] = value; |
| Line 150... | Line 152... | ||
| 150 | 152 | ||
| Line 171... | Line 173... | ||
| 171 | 173 | ||
| 172 | recalculateServoTimes = 0; |
174 | recalculateServoTimes = 0; |
| 173 | } |
175 | } |
| Line -... | Line 176... | ||
| - | 176 | */ |
|
| - | 177 | ||
| 174 | */ |
178 | uint8_t finalServoMap[] = {1,0,0,2,4,5,6,7}; |
| 175 | 179 | ||
| 176 | ISR(TIMER2_COMPA_vect) { |
180 | ISR(TIMER2_COMPA_vect) { |
| 177 | static uint16_t remainingPulseTime; |
181 | static uint16_t remainingPulseTime; |
| Line 178... | Line 182... | ||
| 178 | static uint8_t servoIndex = 0; |
182 | static uint8_t servoIndex = 0; |
| 179 | static uint16_t sumOfPulseTimes = 0; |
183 | static uint16_t sumOfPulseTimes = 0; |
| 180 | 184 | ||
| 181 | if (!remainingPulseTime) { |
185 | if (!remainingPulseTime) { |
| 182 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
186 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
| 183 | if (servoIndex < staticParams.servoCount) { |
187 | if (servoIndex < staticParams.servoCount) { |
| 184 | // There are more signals to output. |
188 | // There are more signals to output. |
| 185 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
189 | sumOfPulseTimes += (remainingPulseTime = servoValues[finalServoMap[servoIndex]]); |
| 186 | servoIndex++; |
190 | servoIndex++; |
| 187 | } else { |
191 | } else { |