Subversion Repositories FlightCtrl

Rev

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

Rev 2164 Rev 2189
Line 1... Line 1...
1 
#include <avr/io.h>
 1 
#include <avr/io.h>
2 
#include <avr/interrupt.h>
 2 
#include <avr/interrupt.h>
3 
#include "eeprom.h"
 - 
 
4 
#include "rc.h"
 3 
#include "timer2.h"
5 
#include "attitude.h"
 - 
 
6 
 
 - 
 
7 
#define COARSERESOLUTION 1
 - 
 
8 
 
 - 
 
9 
#ifdef COARSERESOLUTION
 - 
 
10 
#define NEUTRAL_PULSELENGTH 938
 - 
 
11 
#define STABILIZATION_LOG_DIVIDER 6
 - 
 
12 
#define SERVOLIMIT 500
 - 
 
13 
#define SCALE_FACTOR 4
 - 
 
14 
#define CS2 ((1<<CS21)|(1<<CS20))
 - 
 
Line -... Line 4...
- 
 
 4 
 
- 
 
 5 
#if (SERVO_RESOLUTION == COARSE)
15 
 
 6 
#define CS2 ((1<<CS21)|(1<<CS20))
16 
#else
 - 
 
17 
 
 - 
 
18 
#define NEUTRAL_PULSELENGTH 3750
 - 
 
19 
#define STABILIZATION_LOG_DIVIDER 4
 - 
 
20 
#define SERVOLIMIT 2000
 - 
 
21 
#define SCALE_FACTOR 16
 7 
#else
22 
#define CS2 (1<<CS21)
 8 
#define CS2 (1<<CS21)
Line 23... Line -...
23 
#endif
 - 
 
24 
 
 9 
#endif
25 
#define MAX_SERVOS 8
 - 
 
26 
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128)
 - 
 
27 
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
 10 
 
28 
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
 11 
#define FRAMELEN (PULSELENGTH_2200 * staticParams.servoCount + 128)
29 
 
 12 
 
30 
//volatile uint8_t servoActive = 0;
 13 
// volatile uint8_t servoActive = 0;
31 
volatile uint8_t recalculateServoTimes = 0;
 - 
 
Line 32... Line 14...
32 
volatile uint16_t servoValues[MAX_SERVOS];
 14 
// volatile uint8_t recalculateServoTimes = 0;
33 
volatile uint16_t previousManualValues[2];
 15 
volatile uint16_t pwmChannels[MAX_PWMCHANNELS];
Line 34... Line 16...
34 
 
 16 
 
Line 76... Line 58...
76 
    // Timer/Counter 2 Interrupt Mask Register
 58 
    // Timer/Counter 2 Interrupt Mask Register
77 
    // Enable timer output compare match A Interrupt only
 59 
    // Enable timer output compare match A Interrupt only
78 
    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
 60 
    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
79 
    TIMSK2 |= (1 << OCIE2A);
 61 
    TIMSK2 |= (1 << OCIE2A);
Line 80... Line -...
80 
 
 - 
 
81 
    for (uint8_t axis=0; axis<2; axis++)
 - 
 
82 
      previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
 - 
 
83 
 
 62 
 
84 
    SREG = sreg;
 63 
    SREG = sreg;
Line 85... Line 64...
85 
}
 64 
}
86 
 
 65 
 
Line 95... Line 74...
95 
*/
 74 
*/
Line 96... Line 75...
96 
 
 75 
 
97 
/*****************************************************
 76 
/*****************************************************
98 
 * Control Servo Position
 77 
 * Control Servo Position
99 
 *****************************************************/
 - 
 
100 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
 - 
 
101 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
 - 
 
102 
  // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
 - 
 
103 
  // That is a divisor of about 1<<14. Same conclusion as H&I.
 - 
 
104 
  value *= staticParams.servoConfigurations[axis].stabilizationFactor;
 - 
 
105 
  value = value >> 8;
 - 
 
106 
  if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
 - 
 
107 
    return -value;
 - 
 
108 
  return value;
 - 
 
109 
}
 - 
 
110 
 
 - 
 
111 
// With constant-speed limitation.
 - 
 
112 
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) {
 - 
 
113 
  int16_t diff = manualValue - previousManualValues[axis];
 - 
 
114 
  uint8_t maxSpeed = staticParams.servoManualMaxSpeed;
 - 
 
115 
  if (diff > maxSpeed) diff = maxSpeed;
 - 
 
116 
  else if (diff < -maxSpeed) diff = -maxSpeed;
 - 
 
117 
  manualValue = previousManualValues[axis] + diff;
 - 
 
118 
  previousManualValues[axis] = manualValue;
 - 
 
119 
  return manualValue;
 - 
 
120 
}
 - 
 
121 
 
 - 
 
122 
// add stabilization and manual, apply soft position limits.
 - 
 
123 
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
 - 
 
124 
int16_t featuredServoValue(uint8_t axis) {
 - 
 
125 
  int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR);
 - 
 
126 
  value += calculateStabilizedServoAxis(axis);
 - 
 
127 
  int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
 - 
 
128 
  if (value < limit) value = limit;
 - 
 
129 
  limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
 - 
 
130 
  if (value > limit) value = limit;
 - 
 
131 
  return value;
 - 
 
132 
}
 - 
 
133 
 
 - 
 
134 
uint16_t servoValue(uint8_t axis) {
 - 
 
135 
  int16_t value;
 - 
 
136 
  if (axis<2) value = featuredServoValue(axis);
 - 
 
137 
  else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8.
 - 
 
138 
  // Shift out of the [0..255*SCALE_FACTOR] space
 - 
 
139 
  value -= (128 * SCALE_FACTOR);
 - 
 
140 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
 - 
 
141 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
 - 
 
142 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
 - 
 
143 
  return value + NEUTRAL_PULSELENGTH;
 - 
 
Line -... Line 78...
- 
 
 78 
 *****************************************************/
144 
}
 79 
 
145 
 
 80 
/*
146 
void calculateServoValues(void) {
 81 
void calculateServoValues(void) {
147 
  if (!recalculateServoTimes) return;
 82 
  if (!recalculateServoTimes) return;
148 
  for (uint8_t axis=0; axis<MAX_SERVOS; axis++) {
 83 
  for (uint8_t axis=0; axis<MAX_SERVOS; axis++) {
149 
    servoValues[axis] = servoValue(axis);
 84 
    servoValues[axis] = servoValue(axis);
150 
  }
 85 
  }
151 
  recalculateServoTimes = 0;
 86 
  recalculateServoTimes = 0;
152 
}
 - 
 
Line 153... Line 87...
153 
 
 87 
}
154 
uint8_t servoMap[] = {2,3,0,1,4,5,6,7};
 88 
*/
155 
 
 89 
 
156 
ISR(TIMER2_COMPA_vect) {
 90 
ISR(TIMER2_COMPA_vect) {
Line 157... Line 91...
157 
  static uint16_t remainingPulseTime;
 91 
  static uint16_t remainingPulseTime;
158 
  static uint8_t servoIndex = 0;
 92 
  static uint8_t servoIndex = 0;
159 
  static uint16_t sumOfPulseTimes = 0;
 93 
  static uint16_t sumOfPulseTimes = 0;
160 
 
 94 
 
161 
  if (!remainingPulseTime) {
 95 
  if (!remainingPulseTime) {
162 
    // Pulse is over, and the next pulse has already just started. Calculate length of next pulse.
 96 
    // Pulse is over, and the next pulse has already just started. Calculate length of next pulse.
163 
    if (servoIndex < staticParams.servoCount) {
 97 
    if (servoIndex < staticParams.servoCount) {
164 
      // There are more signals to output.
 98 
      // There are more signals to output.
165 
      sumOfPulseTimes += (remainingPulseTime = servoValues[servoMap[servoIndex]]);
 99 
      sumOfPulseTimes += (remainingPulseTime = pwmChannels[servoIndex]); //pwmChannels[servoMap[servoIndex]]);
166 
      servoIndex++;
 100 
      servoIndex++;
167 
    } else {
 - 
 
168 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
 101 
    } else {
169 
      remainingPulseTime = FRAMELEN - sumOfPulseTimes;
 102 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
170 
      sumOfPulseTimes = servoIndex = 0;
 103 
      remainingPulseTime = FRAMELEN - sumOfPulseTimes;
171 
      recalculateServoTimes = 1;
 104 
      sumOfPulseTimes = servoIndex = 0;
172 
      HEF4017R_ON;
 105 
      HEF4017R_ON;
173 
    }
 106 
    }
174 
  }
 107 
  }
175 
 
 108 
 
176 
  // Schedule the next OCR2A event. The counter is already reset at this time.
 109 
  // Schedule the next OCR2A event. The counter is already reset at this time.
177 
  if (remainingPulseTime > 256+128) {
 110 
  if (remainingPulseTime > 256+128) {
178 
    // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again,
 111 
    // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again,
179 
    // as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
 112 
    // as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
180 
    OCR2A = 255;
 113 
    OCR2A = 255;
181 
    TCCR2A &= ~(1<<COM2A0);
 114 
    TCCR2A &= ~(1<<COM2A0);
182 
    remainingPulseTime-=256;
 115 
    remainingPulseTime -= 256;
183 
  } else if (remainingPulseTime > 256) {
 - 
 
184 
    // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard
 116 
  } else if (remainingPulseTime > 256) {
- 
 
 117 
    // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard
185 
    // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt
 118 
    // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt
186 
    // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
 119 
    // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
187 
    remainingPulseTime-=128;
 120 
    OCR2A=127;
188 
    OCR2A=127;
 121 
    remainingPulseTime -= 128;
189 
  } else {
 122 
  } else {
190 
    // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low
 123 
    // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low
191 
    TCCR2A |= (1<<COM2A0);
 124 
    TCCR2A |= (1<<COM2A0);
192 
    OCR2A = remainingPulseTime-1;
 125 
    OCR2A = remainingPulseTime-1;