Subversion Repositories FlightCtrl

Rev

Rev 2135 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2135 Rev 2142
Line 8... Line 8...
8 
 
 8 
 
Line 9... Line 9...
9 
// #define COARSERESOLUTION 1
 9 
// #define COARSERESOLUTION 1
10 
 
 10 
 
11 
#ifdef COARSERESOLUTION
 11 
#ifdef COARSERESOLUTION
12 
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/32000*1.5f + 0.5f))
 12 
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/32000*1.5f + 0.5f))
13 
#define STABILIZATION_LOG_DIVIDER 6
 13 
#define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/32000*0.5f + 0.5f))
14 
#define SERVOLIMIT ((int16_t)(F_CPU/32000*0.8f + 0.5f))
 14 
#define SERVO_ABS_LIMIT ((int16_t)(F_CPU/32000*0.8f + 0.5f))
Line 15... Line 15...
15 
#define SCALE_FACTOR 4
 15 
//#define SCALE_FACTOR 4
16 
#define CS2 ((1<<CS21)|(1<<CS20))
 16 
#define CS2 ((1<<CS21)|(1<<CS20))
17 
 
 17 
 
18 
#else
 18 
#else
19 
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/8000.0f * 1.5f + 0.5f))
 19 
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/8000.0f * 1.5f + 0.5f))
20 
#define STABILIZATION_LOG_DIVIDER 4
 20 
#define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/8000.0f * 0.5f + 0.5f))
21 
#define SERVOLIMIT ((int16_t)(F_CPU/8000.0f * 0.8f + 0.5f))
 21 
#define SERVO_ABS_LIMIT ((int16_t)(F_CPU/8000.0f * 0.8f + 0.5f))
Line 22... Line 22...
22 
#define SCALE_FACTOR 16
 22 
//#define SCALE_FACTOR 16
23 
#define CS2 (1<<CS21)
 - 
 
24 
#endif
 - 
 
Line 25... Line -...
25 
 
 - 
 
26 
#define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVOLIMIT) * (uint16_t)staticParams.servoCount + 128)
 23 
#define CS2 (1<<CS21)
- 
 
 24 
#endif
27 
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
 25 
 
Line 28... Line 26...
28 
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
 26 
#define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVO_ABS_LIMIT) * (uint16_t)staticParams.servoCount + 128)
29 
 
 27 
 
Line 30... Line 28...
30 
volatile uint8_t recalculateServoTimes = 0;
 28 
volatile uint16_t servoValues[MAX_SERVOS];
Line 75... Line 73...
75 
    // Timer/Counter 2 Interrupt Mask Register
 73 
    // Timer/Counter 2 Interrupt Mask Register
76 
    // Enable timer output compare match A Interrupt only
 74 
    // Enable timer output compare match A Interrupt only
77 
    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
 75 
    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
78 
    TIMSK2 |= (1 << OCIE2A);
 76 
    TIMSK2 |= (1 << OCIE2A);
Line 79... Line 77...
79 
 
 77 
 
80 
    for (uint8_t axis=0; axis<2; axis++)
 78 
    for (uint8_t i=0; i<MAX_SERVOS; i++)
Line 81... Line 79...
81 
      previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR;
 79 
        servoValues[i] = NEUTRAL_PULSELENGTH;
82 
 
 80 
 
Line 83... Line 81...
83 
    SREG = sreg;
 81 
    SREG = sreg;
84 
}
 82 
}
85 
 
 83 
 
- 
 
 84 
/*****************************************************
86 
/*****************************************************
 85 
 * Control (camera gimbal etc.) servos
87 
 * Control (camera gimbal etc.) servos
 86 
 *****************************************************/
88 
 *****************************************************/
 87 
/*
89 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
 88 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
90 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
 89 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
Line 105... Line 104...
105 
  else if (diff < -maxSpeed) diff = -maxSpeed;
 104 
  else if (diff < -maxSpeed) diff = -maxSpeed;
106 
  manualValue = previousManualValues[axis] + diff;
 105 
  manualValue = previousManualValues[axis] + diff;
107 
  previousManualValues[axis] = manualValue;
 106 
  previousManualValues[axis] = manualValue;
108 
  return manualValue;
 107 
  return manualValue;
109 
}
 108 
}
- 
 
 109 
*/
Line -... Line 110...
- 
 
 110 
 
110 
 
 111 
/*
111 
// add stabilization and manual, apply soft position limits.
 112 
// add stabilization and manual, apply soft position limits.
112 
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
 113 
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
113 
int16_t featuredServoValue(uint8_t axis) {
 114 
int16_t featuredServoValue(uint8_t axis) {
114 
  int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR);
 115 
  int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR);
Line 121... Line 122...
121 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
 122 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
122 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
 123 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
123 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
 124 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
124 
  return value + NEUTRAL_PULSELENGTH;
 125 
  return value + NEUTRAL_PULSELENGTH;
125 
}
 126 
}
- 
 
 127 
*/
Line 126... Line 128...
126 
 
 128 
 
127 
void calculateControlServoValues(void) {
 129 
void calculateControlServoValues(void) {
128 
  int16_t value;
 - 
 
129 
  //int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR;
 - 
 
130 
  //int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR;
 130 
  int16_t value;
131 
  for (uint8_t axis=0; axis<4; axis++) {
 131 
  for (uint8_t axis=0; axis<4; axis++) {
- 
 
 132 
        value = controlServos[axis];
- 
 
 133 
 
- 
 
 134 
        // Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal.
- 
 
 135 
        int16_t min = (staticParams.servos[axis].minValue * SERVO_NORMAL_LIMIT) >> 6;
- 
 
 136 
        int16_t max = (staticParams.servos[axis].maxValue * SERVO_NORMAL_LIMIT) >> 6;
- 
 
 137 
 
- 
 
 138 
        if (value < min) value = min;
- 
 
 139 
        else if (value > max) value = max;
132 
        value = controlServos[axis];
 140 
 
133 
        if (value < -SERVOLIMIT) value = -SERVOLIMIT;
 141 
        if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT;
- 
 
 142 
        else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT;
134 
    else if (value > SERVOLIMIT) value = SERVOLIMIT;
 143 
 
135 
        servoValues[axis] = value + NEUTRAL_PULSELENGTH;
 144 
        servoValues[axis] = value + NEUTRAL_PULSELENGTH;
136 
  }
 145 
  }
Line -... Line 146...
- 
 
 146 
}
137 
}
 147 
 
138 
 
 148 
/*
139 
void calculateFeaturedServoValues(void) {
 149 
void calculateFeaturedServoValues(void) {
Line 140... Line 150...
140 
  int16_t value;
 150 
  int16_t value;
Line 152... Line 162...
152 
        servoValues[axis] = value;
 162 
        servoValues[axis] = value;
153 
  }
 163 
  }
Line 154... Line 164...
154 
 
 164 
 
155 
  recalculateServoTimes = 0;
 165 
  recalculateServoTimes = 0;
- 
 
 166 
}
Line 156... Line 167...
156 
}
 167 
*/
157 
 
 168 
 
158 
ISR(TIMER2_COMPA_vect) {
 169 
ISR(TIMER2_COMPA_vect) {
159 
  static uint16_t remainingPulseTime;
 170 
  static uint16_t remainingPulseTime;
Line 168... Line 179...
168 
      servoIndex++;
 179 
      servoIndex++;
169 
    } else {
 180 
    } else {
170 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
 181 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
171 
      remainingPulseTime = FRAMELENGTH - sumOfPulseTimes;
 182 
      remainingPulseTime = FRAMELENGTH - sumOfPulseTimes;
172 
      sumOfPulseTimes = servoIndex = 0;
 183 
      sumOfPulseTimes = servoIndex = 0;
173 
      recalculateServoTimes = 1;
 184 
      //recalculateServoTimes = 1;
174 
      HEF4017R_ON;
 185 
      HEF4017R_ON;
175 
    }
 186 
    }
176 
  }
 187 
  }
Line 177... Line 188...
177 
 
 188