Rev 2135 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2135 | Rev 2142 | ||
---|---|---|---|
1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
3 | #include "eeprom.h" |
3 | #include "eeprom.h" |
4 | #include "output.h" |
4 | #include "output.h" |
5 | #include "flight.h" |
5 | #include "flight.h" |
6 | #include "attitude.h" |
6 | #include "attitude.h" |
7 | #include "timer2.h" |
7 | #include "timer2.h" |
8 | 8 | ||
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)) |
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)) |
22 | #define SCALE_FACTOR 16 |
22 | //#define SCALE_FACTOR 16 |
23 | #define CS2 (1<<CS21) |
23 | #define CS2 (1<<CS21) |
24 | #endif |
24 | #endif |
25 | 25 | ||
26 | #define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVOLIMIT) * (uint16_t)staticParams.servoCount + 128) |
- | |
27 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
- | |
28 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
- | |
29 | 26 | #define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVO_ABS_LIMIT) * (uint16_t)staticParams.servoCount + 128) |
|
- | 27 | ||
30 | volatile uint8_t recalculateServoTimes = 0; |
28 | volatile uint16_t servoValues[MAX_SERVOS]; |
31 | volatile uint16_t servoValues[MAX_SERVOS]; |
29 | //volatile uint8_t recalculateServoTimes = 0; |
32 | volatile uint16_t previousManualValues[2]; |
30 | //volatile uint16_t previousManualValues[2]; |
33 | 31 | ||
34 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
32 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
35 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
33 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
36 | 34 | ||
37 | /***************************************************** |
35 | /***************************************************** |
38 | * Initialize Timer 2 |
36 | * Initialize Timer 2 |
39 | *****************************************************/ |
37 | *****************************************************/ |
40 | void timer2_init(void) { |
38 | void timer2_init(void) { |
41 | uint8_t sreg = SREG; |
39 | uint8_t sreg = SREG; |
42 | 40 | ||
43 | // disable all interrupts before reconfiguration |
41 | // disable all interrupts before reconfiguration |
44 | cli(); |
42 | cli(); |
45 | 43 | ||
46 | // set PD7 as output of the PWM for pitch servo |
44 | // set PD7 as output of the PWM for pitch servo |
47 | DDRD |= (1 << DDD7); |
45 | DDRD |= (1 << DDD7); |
48 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
46 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
49 | 47 | ||
50 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
48 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
51 | HEF4017R_ON; // reset |
49 | HEF4017R_ON; // reset |
52 | 50 | ||
53 | // Timer/Counter 2 Control Register A |
51 | // Timer/Counter 2 Control Register A |
54 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
52 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
55 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
53 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
56 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
54 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
57 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
55 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
58 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
56 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
59 | 57 | ||
60 | // Timer/Counter 2 Control Register B |
58 | // Timer/Counter 2 Control Register B |
61 | 59 | ||
62 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
60 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
63 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
61 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
64 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
62 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
65 | 63 | ||
66 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
64 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
67 | TCCR2B |= CS2; |
65 | TCCR2B |= CS2; |
68 | 66 | ||
69 | // Initialize the Timer/Counter 2 Register |
67 | // Initialize the Timer/Counter 2 Register |
70 | TCNT2 = 0; |
68 | TCNT2 = 0; |
71 | 69 | ||
72 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
70 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
73 | OCR2A = 255; |
71 | OCR2A = 255; |
74 | 72 | ||
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); |
79 | 77 | ||
80 | for (uint8_t axis=0; axis<2; axis++) |
78 | for (uint8_t i=0; i<MAX_SERVOS; i++) |
81 | previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR; |
79 | servoValues[i] = NEUTRAL_PULSELENGTH; |
82 | 80 | ||
83 | SREG = sreg; |
81 | SREG = sreg; |
84 | } |
82 | } |
85 | 83 | ||
86 | /***************************************************** |
84 | /***************************************************** |
87 | * Control (camera gimbal etc.) servos |
85 | * Control (camera gimbal etc.) servos |
88 | *****************************************************/ |
86 | *****************************************************/ |
- | 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 |
91 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
90 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
92 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
91 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
93 | value *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor; |
92 | value *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor; |
94 | value = value >> 8; |
93 | value = value >> 8; |
95 | if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
94 | if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
96 | return -value; |
95 | return -value; |
97 | return value; |
96 | return value; |
98 | } |
97 | } |
99 | 98 | ||
100 | // With constant-speed limitation. |
99 | // With constant-speed limitation. |
101 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
100 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
102 | int16_t diff = manualValue - previousManualValues[axis]; |
101 | int16_t diff = manualValue - previousManualValues[axis]; |
103 | uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed; |
102 | uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed; |
104 | if (diff > maxSpeed) diff = maxSpeed; |
103 | if (diff > maxSpeed) diff = maxSpeed; |
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 | */ |
|
- | 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); |
115 | value += calculateStabilizedServoAxis(axis); |
116 | value += calculateStabilizedServoAxis(axis); |
116 | int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR; |
117 | int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR; |
117 | if (value < limit) value = limit; |
118 | if (value < limit) value = limit; |
118 | limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR; |
119 | limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR; |
119 | if (value > limit) value = limit; |
120 | if (value > limit) value = limit; |
120 | value -= (128 * SCALE_FACTOR); |
121 | value -= (128 * SCALE_FACTOR); |
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 | */ |
|
126 | 128 | ||
127 | void calculateControlServoValues(void) { |
129 | void calculateControlServoValues(void) { |
128 | int16_t value; |
130 | int16_t value; |
129 | //int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR; |
- | |
130 | //int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR; |
- | |
131 | for (uint8_t axis=0; axis<4; axis++) { |
131 | for (uint8_t axis=0; axis<4; axis++) { |
132 | value = controlServos[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; |
|
- | 140 | ||
133 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
141 | if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT; |
134 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
142 | else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT; |
- | 143 | ||
135 | servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
144 | servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
136 | } |
145 | } |
137 | } |
146 | } |
- | 147 | ||
138 | 148 | /* |
|
139 | void calculateFeaturedServoValues(void) { |
149 | void calculateFeaturedServoValues(void) { |
140 | int16_t value; |
150 | int16_t value; |
141 | uint8_t axis; |
151 | uint8_t axis; |
142 | 152 | ||
143 | // Save the computation cost of computing a new value before the old one is used. |
153 | // Save the computation cost of computing a new value before the old one is used. |
144 | if (!recalculateServoTimes) return; |
154 | if (!recalculateServoTimes) return; |
145 | 155 | ||
146 | for (axis= MAX_CONTROL_SERVOS; axis<MAX_CONTROL_SERVOS+2; axis++) { |
156 | for (axis= MAX_CONTROL_SERVOS; axis<MAX_CONTROL_SERVOS+2; axis++) { |
147 | value = featuredServoValue(axis-MAX_CONTROL_SERVOS); |
157 | value = featuredServoValue(axis-MAX_CONTROL_SERVOS); |
148 | servoValues[axis] = value; |
158 | servoValues[axis] = value; |
149 | } |
159 | } |
150 | for (axis=MAX_CONTROL_SERVOS+2; axis<MAX_SERVOS; axis++) { |
160 | for (axis=MAX_CONTROL_SERVOS+2; axis<MAX_SERVOS; axis++) { |
151 | value = 128 * SCALE_FACTOR; |
161 | value = 128 * SCALE_FACTOR; |
152 | servoValues[axis] = value; |
162 | servoValues[axis] = value; |
153 | } |
163 | } |
154 | 164 | ||
155 | recalculateServoTimes = 0; |
165 | recalculateServoTimes = 0; |
156 | } |
166 | } |
- | 167 | */ |
|
157 | 168 | ||
158 | ISR(TIMER2_COMPA_vect) { |
169 | ISR(TIMER2_COMPA_vect) { |
159 | static uint16_t remainingPulseTime; |
170 | static uint16_t remainingPulseTime; |
160 | static uint8_t servoIndex = 0; |
171 | static uint8_t servoIndex = 0; |
161 | static uint16_t sumOfPulseTimes = 0; |
172 | static uint16_t sumOfPulseTimes = 0; |
162 | 173 | ||
163 | if (!remainingPulseTime) { |
174 | if (!remainingPulseTime) { |
164 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
175 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
165 | if (servoIndex < staticParams.servoCount) { |
176 | if (servoIndex < staticParams.servoCount) { |
166 | // There are more signals to output. |
177 | // There are more signals to output. |
167 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
178 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
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 | } |
177 | 188 | ||
178 | // Schedule the next OCR2A event. The counter is already reset at this time. |
189 | // Schedule the next OCR2A event. The counter is already reset at this time. |
179 | if (remainingPulseTime > 256+128) { |
190 | if (remainingPulseTime > 256+128) { |
180 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
191 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
181 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
192 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
182 | OCR2A = 255; |
193 | OCR2A = 255; |
183 | TCCR2A &= ~(1<<COM2A0); |
194 | TCCR2A &= ~(1<<COM2A0); |
184 | remainingPulseTime-=256; |
195 | remainingPulseTime-=256; |
185 | } else if (remainingPulseTime > 256) { |
196 | } else if (remainingPulseTime > 256) { |
186 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
197 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
187 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
198 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
188 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
199 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
189 | remainingPulseTime-=128; |
200 | remainingPulseTime-=128; |
190 | OCR2A=127; |
201 | OCR2A=127; |
191 | } else { |
202 | } else { |
192 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
203 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
193 | TCCR2A |= (1<<COM2A0); |
204 | TCCR2A |= (1<<COM2A0); |
194 | OCR2A = remainingPulseTime-1; |
205 | OCR2A = remainingPulseTime-1; |
195 | remainingPulseTime=0; |
206 | remainingPulseTime=0; |
196 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
207 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
197 | } |
208 | } |
198 | } |
209 | } |
199 | 210 |