| 81,7 → 81,7 |
|---|
| TIMSK2 |= (1 << OCIE2A); |
| |
| for (uint8_t axis=0; axis<2; axis++) |
| previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
| previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR; |
| |
| SREG = sreg; |
| } |
| 93,9 → 93,9 |
|---|
| 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 *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor; |
| value = value >> 8; |
| if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
| if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
| return -value; |
| return value; |
| } |
| 103,7 → 103,7 |
|---|
| // With constant-speed limitation. |
| uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
| int16_t diff = manualValue - previousManualValues[axis]; |
| uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
| uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed; |
| if (diff > maxSpeed) diff = maxSpeed; |
| else if (diff < -maxSpeed) diff = -maxSpeed; |
| manualValue = previousManualValues[axis] + diff; |
| 114,11 → 114,11 |
|---|
| // add stabilization and manual, apply soft position limits. |
| // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
| int16_t featuredServoValue(uint8_t axis) { |
| int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
| int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR); |
| value += calculateStabilizedServoAxis(axis); |
| int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
| int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR; |
| if (value < limit) value = limit; |
| limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
| limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR; |
| if (value > limit) value = limit; |
| value -= (128 * SCALE_FACTOR); |
| if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
| 129,13 → 129,12 |
|---|
| |
| void calculateControlServoValues(void) { |
| int16_t value; |
| int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR; |
| int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR; |
| //int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR; |
| //int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR; |
| for (uint8_t axis=0; axis<4; axis++) { |
| value = controlServos[axis]; |
| value *= 2; |
| if (value < minLimit) value = minLimit; |
| else if (value > maxLimit) value = maxLimit; |
| if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
| else if (value > SERVOLIMIT) value = SERVOLIMIT; |
| servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
| } |
| debugOut.analog[24] = servoValues[0]; |