Subversion Repositories FlightCtrl

Rev

Rev 2135 | Rev 2142 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2135 Rev 2139
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 78... Line 76...
78 
    // Timer/Counter 2 Interrupt Mask Register
 76 
    // Timer/Counter 2 Interrupt Mask Register
79 
    // Enable timer output compare match A Interrupt only
 77 
    // Enable timer output compare match A Interrupt only
80 
    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
 78 
    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
81 
    TIMSK2 |= (1 << OCIE2A);
 79 
    TIMSK2 |= (1 << OCIE2A);
Line 82... Line 80...
82 
 
 80 
 
83 
    for (uint8_t axis=0; axis<2; axis++)
 81 
    //for (uint8_t axis=0; axis<2; axis++)
- 
 
 82 
    //  previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR;
- 
 
 83 
 
- 
 
 84 
    for (uint8_t i=0; i<MAX_SERVOS; i++)
Line 84... Line 85...
84 
      previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR;
 85 
        servoValues[i] = NEUTRAL_PULSELENGTH;
85 
 
 86 
 
Line 86... Line 87...
86 
    SREG = sreg;
 87 
    SREG = sreg;
87 
}
 88 
}
- 
 
 89 
 
88 
 
 90 
/*****************************************************
- 
 
 91 
 * Control (camera gimbal etc.) servos
89 
/*****************************************************
 92 
 * Makes no sense as long as we have no acc. reference.
90 
 * Control (camera gimbal etc.) servos
 93 
 *****************************************************/
91 
 *****************************************************/
 94 
/*
92 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
 95 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
93 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
 96 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
Line 108... Line 111...
108 
  else if (diff < -maxSpeed) diff = -maxSpeed;
 111 
  else if (diff < -maxSpeed) diff = -maxSpeed;
109 
  manualValue = previousManualValues[axis] + diff;
 112 
  manualValue = previousManualValues[axis] + diff;
110 
  previousManualValues[axis] = manualValue;
 113 
  previousManualValues[axis] = manualValue;
111 
  return manualValue;
 114 
  return manualValue;
112 
}
 115 
}
- 
 
 116 
*/
Line -... Line 117...
- 
 
 117 
 
113 
 
 118 
/*
114 
// add stabilization and manual, apply soft position limits.
 119 
// add stabilization and manual, apply soft position limits.
115 
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
 120 
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
116 
int16_t featuredServoValue(uint8_t axis) {
 121 
int16_t featuredServoValue(uint8_t axis) {
117 
  int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR);
 122 
  int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR);
Line 124... Line 129...
124 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
 129 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
125 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
 130 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
126 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
 131 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
127 
  return value + NEUTRAL_PULSELENGTH;
 132 
  return value + NEUTRAL_PULSELENGTH;
128 
}
 133 
}
- 
 
 134 
*/
Line 129... Line 135...
129 
 
 135 
 
130 
void calculateControlServoValues(void) {
 136 
void calculateControlServoValues(void) {
131 
  int16_t value;
 137 
  int16_t value;
132 
  for (uint8_t axis=0; axis<4; axis++) {
 138 
  for (uint8_t axis=0; axis<4; axis++) {
- 
 
 139 
        value = controlServos[axis];
- 
 
 140 
 
- 
 
 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;
- 
 
 143 
        int16_t max = (staticParams.servos[axis].maxValue * SERVO_NORMAL_LIMIT) >> 6;
- 
 
 144 
 
- 
 
 145 
        if (value < min) value = min;
- 
 
 146 
        else if (value > max) value = max;
133 
        value = controlServos[axis];
 147 
 
134 
        if (value < -SERVOLIMIT) value = -SERVOLIMIT;
 148 
        if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT;
- 
 
 149 
        else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT;
135 
    else if (value > SERVOLIMIT) value = SERVOLIMIT;
 150 
 
136 
        servoValues[axis] = value + NEUTRAL_PULSELENGTH;
 151 
        servoValues[axis] = value + NEUTRAL_PULSELENGTH;
137 
  }
 152 
  }
Line -... Line 153...
- 
 
 153 
}
138 
}
 154 
 
139 
 
 155 
/*
140 
void calculateFeaturedServoValues(void) {
 156 
void calculateFeaturedServoValues(void) {
Line 141... Line 157...
141 
  int16_t value;
 157 
  int16_t value;
Line 153... Line 169...
153 
        servoValues[axis] = value;
 169 
        servoValues[axis] = value;
154 
  }
 170 
  }
Line 155... Line 171...
155 
 
 171 
 
156 
  recalculateServoTimes = 0;
 172 
  recalculateServoTimes = 0;
- 
 
 173 
}
Line 157... Line 174...
157 
}
 174 
*/
158 
 
 175 
 
159 
ISR(TIMER2_COMPA_vect) {
 176 
ISR(TIMER2_COMPA_vect) {
160 
  static uint16_t remainingPulseTime;
 177 
  static uint16_t remainingPulseTime;
Line 169... Line 186...
169 
      servoIndex++;
 186 
      servoIndex++;
170 
    } else {
 187 
    } else {
171 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
 188 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
172 
      remainingPulseTime = FRAMELENGTH - sumOfPulseTimes;
 189 
      remainingPulseTime = FRAMELENGTH - sumOfPulseTimes;
173 
      sumOfPulseTimes = servoIndex = 0;
 190 
      sumOfPulseTimes = servoIndex = 0;
174 
      recalculateServoTimes = 1;
 191 
      //recalculateServoTimes = 1;
175 
      HEF4017R_ON;
 192 
      HEF4017R_ON;
176 
    }
 193 
    }
177 
  }
 194 
  }
Line 178... Line 195...
178 
 
 195