Subversion Repositories FlightCtrl

Rev

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 
  }