Rev 2142 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2142 | Rev 2152 | ||
---|---|---|---|
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 SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/32000*0.5f + 0.5f)) |
13 | #define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/32000*0.5f + 0.5f)) |
14 | #define SERVO_ABS_LIMIT ((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 SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/8000.0f * 0.5f + 0.5f)) |
20 | #define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/8000.0f * 0.5f + 0.5f)) |
21 | #define SERVO_ABS_LIMIT ((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 + SERVO_ABS_LIMIT) * (uint16_t)staticParams.servoCount + 128) |
26 | #define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVO_ABS_LIMIT) * (uint16_t)staticParams.servoCount + 128) |
27 | 27 | ||
28 | volatile uint16_t servoValues[MAX_SERVOS]; |
28 | volatile uint16_t servoValues[MAX_SERVOS]; |
29 | //volatile uint8_t recalculateServoTimes = 0; |
29 | //volatile uint8_t recalculateServoTimes = 0; |
30 | //volatile uint16_t previousManualValues[2]; |
30 | //volatile uint16_t previousManualValues[2]; |
31 | 31 | ||
32 | #define HEF4017R_ON PORTD |= (1<<PORTD3) |
32 | #define HEF4017R_ON PORTD |= (1<<PORTD3) |
33 | #define HEF4017R_OFF PORTD &= ~(1<<PORTD3) |
33 | #define HEF4017R_OFF PORTD &= ~(1<<PORTD3) |
34 | 34 | ||
35 | /***************************************************** |
35 | /***************************************************** |
36 | * Initialize Timer 2 |
36 | * Initialize Timer 2 |
37 | *****************************************************/ |
37 | *****************************************************/ |
38 | void timer2_init(void) { |
38 | void timer2_init(void) { |
39 | uint8_t sreg = SREG; |
39 | uint8_t sreg = SREG; |
40 | 40 | ||
41 | // disable all interrupts before reconfiguration |
41 | // disable all interrupts before reconfiguration |
42 | cli(); |
42 | cli(); |
43 | 43 | ||
44 | // set PD7 as output of the 4017 clk |
44 | // set PD7 as output of the 4017 clk |
45 | DDRB |= (1 << DDB3); |
45 | DDRB |= (1 << DDB3); |
46 | PORTB &= ~(1 << PORTB3); // set PD7 to low |
46 | PORTB &= ~(1 << PORTB3); // set PD7 to low |
47 | 47 | ||
48 | // oc2b DDRD |= (1 << DDD4); // set PC6 as output (Reset for HEF4017) |
48 | // oc2b DDRD |= (1 << DDD4); // set PC6 as output (Reset for HEF4017) |
49 | DDRD |= (1 << DDD3); // set PC6 as output (Reset for HEF4017) |
49 | DDRD |= (1 << DDD3); // set PC6 as output (Reset for HEF4017) |
50 | HEF4017R_ON; // reset |
50 | HEF4017R_ON; // reset |
51 | 51 | ||
52 | // Timer/Counter 2 Control Register A |
52 | // Timer/Counter 2 Control Register A |
53 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
53 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
54 | // PD3: Output OCR2 match, (Bits: COM2B1 = 1, COM2B0 = 0) |
54 | // PD3: Output OCR2 match, (Bits: COM2B1 = 1, COM2B0 = 0) |
55 | // PB3: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
55 | // PB3: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
56 | // ardu TCCR2A &= ~((1 << COM2B0) | (1 << COM2A1) | (1 << COM2A0) | (1 << WGM20) | (1 << WGM22)); |
56 | // ardu TCCR2A &= ~((1 << COM2B0) | (1 << COM2A1) | (1 << COM2A0) | (1 << WGM20) | (1 << WGM22)); |
57 | // ardu TCCR2A |= (1 << COM2B1) | (1 << WGM21); |
57 | // ardu TCCR2A |= (1 << COM2B1) | (1 << WGM21); |
58 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
58 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
59 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
59 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
60 | 60 | ||
61 | // Timer/Counter 2 Control Register B |
61 | // Timer/Counter 2 Control Register B |
62 | 62 | ||
63 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
63 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
64 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
64 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
65 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
65 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
66 | 66 | ||
67 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
67 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
68 | TCCR2B |= CS2; |
68 | TCCR2B |= CS2; |
69 | 69 | ||
70 | // Initialize the Timer/Counter 2 Register |
70 | // Initialize the Timer/Counter 2 Register |
71 | TCNT2 = 0; |
71 | TCNT2 = 0; |
72 | 72 | ||
73 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
73 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
74 | OCR2A = 255; |
74 | OCR2A = 255; |
75 | 75 | ||
76 | // Timer/Counter 2 Interrupt Mask Register |
76 | // Timer/Counter 2 Interrupt Mask Register |
77 | // Enable timer output compare match A Interrupt only |
77 | // Enable timer output compare match A Interrupt only |
78 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
78 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
79 | TIMSK2 |= (1 << OCIE2A); |
79 | TIMSK2 |= (1 << OCIE2A); |
80 | 80 | ||
81 | //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; |
82 | // previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR; |
83 | 83 | ||
84 | for (uint8_t i=0; i<MAX_SERVOS; i++) |
84 | for (uint8_t i=0; i<MAX_SERVOS; i++) |
85 | servoValues[i] = NEUTRAL_PULSELENGTH; |
85 | servoValues[i] = NEUTRAL_PULSELENGTH; |
86 | 86 | ||
87 | SREG = sreg; |
87 | SREG = sreg; |
88 | } |
88 | } |
89 | 89 | ||
90 | /***************************************************** |
90 | /***************************************************** |
91 | * Control (camera gimbal etc.) servos |
91 | * Control (camera gimbal etc.) servos |
92 | * Makes no sense as long as we have no acc. reference. |
92 | * Makes no sense as long as we have no acc. reference. |
93 | *****************************************************/ |
93 | *****************************************************/ |
94 | /* |
94 | /* |
95 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
95 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
96 | 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 |
97 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
97 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
98 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
98 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
99 | value *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor; |
99 | value *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor; |
100 | value = value >> 8; |
100 | value = value >> 8; |
101 | if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
101 | if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
102 | return -value; |
102 | return -value; |
103 | return value; |
103 | return value; |
104 | } |
104 | } |
105 | 105 | ||
106 | // With constant-speed limitation. |
106 | // With constant-speed limitation. |
107 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
107 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
108 | int16_t diff = manualValue - previousManualValues[axis]; |
108 | int16_t diff = manualValue - previousManualValues[axis]; |
109 | uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed; |
109 | uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed; |
110 | if (diff > maxSpeed) diff = maxSpeed; |
110 | if (diff > maxSpeed) diff = maxSpeed; |
111 | else if (diff < -maxSpeed) diff = -maxSpeed; |
111 | else if (diff < -maxSpeed) diff = -maxSpeed; |
112 | manualValue = previousManualValues[axis] + diff; |
112 | manualValue = previousManualValues[axis] + diff; |
113 | previousManualValues[axis] = manualValue; |
113 | previousManualValues[axis] = manualValue; |
114 | return manualValue; |
114 | return manualValue; |
115 | } |
115 | } |
116 | */ |
116 | */ |
117 | 117 | ||
118 | /* |
118 | /* |
119 | // add stabilization and manual, apply soft position limits. |
119 | // add stabilization and manual, apply soft position limits. |
120 | // 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) |
121 | int16_t featuredServoValue(uint8_t axis) { |
121 | int16_t featuredServoValue(uint8_t axis) { |
122 | int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR); |
122 | int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR); |
123 | value += calculateStabilizedServoAxis(axis); |
123 | value += calculateStabilizedServoAxis(axis); |
124 | int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR; |
124 | int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR; |
125 | if (value < limit) value = limit; |
125 | if (value < limit) value = limit; |
126 | limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR; |
126 | limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR; |
127 | if (value > limit) value = limit; |
127 | if (value > limit) value = limit; |
128 | value -= (128 * SCALE_FACTOR); |
128 | value -= (128 * SCALE_FACTOR); |
129 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
129 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
130 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
130 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
131 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
131 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
132 | return value + NEUTRAL_PULSELENGTH; |
132 | return value + NEUTRAL_PULSELENGTH; |
133 | } |
133 | } |
134 | */ |
134 | */ |
135 | 135 | ||
136 | void calculateControlServoValues(void) { |
136 | void calculateControlServoValues(void) { |
137 | int16_t value; |
137 | int16_t value; |
138 | for (uint8_t axis=0; axis<4; axis++) { |
138 | for (uint8_t axis=0; axis<4; axis++) { |
139 | value = controlServos[axis]; |
139 | value = controlServos[axis]; |
140 | 140 | ||
141 | // Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal. |
141 | // Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal. |
142 | int16_t min = (int16_t)staticParams.servos[axis].minValue * (SERVO_NORMAL_LIMIT >> 6); |
142 | int16_t min = (int16_t)staticParams.servos[axis].minValue * (SERVO_NORMAL_LIMIT >> 6); |
143 | int16_t max = (int16_t)staticParams.servos[axis].maxValue * (SERVO_NORMAL_LIMIT >> 6); |
143 | int16_t max = (int16_t)staticParams.servos[axis].maxValue * (SERVO_NORMAL_LIMIT >> 6); |
144 | 144 | ||
145 | if (value < -min) value = -min; |
145 | if (value < -min) value = -min; |
146 | else if (value > max) value = max; |
146 | else if (value > max) value = max; |
147 | 147 | ||
148 | if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT; |
148 | if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT; |
149 | else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT; |
149 | else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT; |
150 | 150 | ||
151 | debugOut.analog[24+axis] = value; |
151 | debugOut.analog[24+axis] = value; |
152 | 152 | ||
153 | servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
153 | servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
154 | } |
154 | } |
155 | } |
155 | } |
156 | 156 | ||
157 | /* |
157 | /* |
158 | void calculateFeaturedServoValues(void) { |
158 | void calculateFeaturedServoValues(void) { |
159 | int16_t value; |
159 | int16_t value; |
160 | uint8_t axis; |
160 | uint8_t axis; |
161 | 161 | ||
162 | // Save the computation cost of computing a new value before the old one is used. |
162 | // Save the computation cost of computing a new value before the old one is used. |
163 | if (!recalculateServoTimes) return; |
163 | if (!recalculateServoTimes) return; |
164 | 164 | ||
165 | for (axis= MAX_CONTROL_SERVOS; axis<MAX_CONTROL_SERVOS+2; axis++) { |
165 | for (axis= MAX_CONTROL_SERVOS; axis<MAX_CONTROL_SERVOS+2; axis++) { |
166 | value = featuredServoValue(axis-MAX_CONTROL_SERVOS); |
166 | value = featuredServoValue(axis-MAX_CONTROL_SERVOS); |
167 | servoValues[axis] = value; |
167 | servoValues[axis] = value; |
168 | } |
168 | } |
169 | for (axis=MAX_CONTROL_SERVOS+2; axis<MAX_SERVOS; axis++) { |
169 | for (axis=MAX_CONTROL_SERVOS+2; axis<MAX_SERVOS; axis++) { |
170 | value = 128 * SCALE_FACTOR; |
170 | value = 128 * SCALE_FACTOR; |
171 | servoValues[axis] = value; |
171 | servoValues[axis] = value; |
172 | } |
172 | } |
173 | 173 | ||
174 | recalculateServoTimes = 0; |
174 | recalculateServoTimes = 0; |
175 | } |
175 | } |
176 | */ |
176 | */ |
- | 177 | ||
177 | 178 | // Elevator, ailerons, ailerons, throttle, throttle. |
|
178 | uint8_t finalServoMap[] = {1,0,0,2,4,5,6,7}; |
179 | uint8_t finalServoMap[] = {1,0,0,2,2,5,6,7}; |
179 | 180 | ||
180 | ISR(TIMER2_COMPA_vect) { |
181 | ISR(TIMER2_COMPA_vect) { |
181 | static uint16_t remainingPulseTime; |
182 | static uint16_t remainingPulseTime; |
182 | static uint8_t servoIndex = 0; |
183 | static uint8_t servoIndex = 0; |
183 | static uint16_t sumOfPulseTimes = 0; |
184 | static uint16_t sumOfPulseTimes = 0; |
184 | 185 | ||
185 | if (!remainingPulseTime) { |
186 | if (!remainingPulseTime) { |
186 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
187 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
187 | if (servoIndex < staticParams.servoCount) { |
188 | if (servoIndex < staticParams.servoCount) { |
188 | // There are more signals to output. |
189 | // There are more signals to output. |
189 | sumOfPulseTimes += (remainingPulseTime = servoValues[finalServoMap[servoIndex]]); |
190 | sumOfPulseTimes += (remainingPulseTime = servoValues[finalServoMap[servoIndex]]); |
190 | servoIndex++; |
191 | servoIndex++; |
191 | } else { |
192 | } else { |
192 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
193 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
193 | remainingPulseTime = FRAMELENGTH - sumOfPulseTimes; |
194 | remainingPulseTime = FRAMELENGTH - sumOfPulseTimes; |
194 | sumOfPulseTimes = servoIndex = 0; |
195 | sumOfPulseTimes = servoIndex = 0; |
195 | //recalculateServoTimes = 1; |
196 | //recalculateServoTimes = 1; |
196 | HEF4017R_ON; |
197 | HEF4017R_ON; |
197 | } |
198 | } |
198 | } |
199 | } |
199 | 200 | ||
200 | // Schedule the next OCR2A event. The counter is already reset at this time. |
201 | // Schedule the next OCR2A event. The counter is already reset at this time. |
201 | if (remainingPulseTime > 256+128) { |
202 | if (remainingPulseTime > 256+128) { |
202 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
203 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
203 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
204 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
204 | OCR2A = 255; |
205 | OCR2A = 255; |
205 | TCCR2A &= ~(1<<COM2A0); |
206 | TCCR2A &= ~(1<<COM2A0); |
206 | remainingPulseTime-=256; |
207 | remainingPulseTime-=256; |
207 | } else if (remainingPulseTime > 256) { |
208 | } else if (remainingPulseTime > 256) { |
208 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
209 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
209 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
210 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
210 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
211 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
211 | remainingPulseTime-=128; |
212 | remainingPulseTime-=128; |
212 | OCR2A=127; |
213 | OCR2A=127; |
213 | } else { |
214 | } else { |
214 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
215 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
215 | TCCR2A |= (1<<COM2A0); |
216 | TCCR2A |= (1<<COM2A0); |
216 | OCR2A = remainingPulseTime-1; |
217 | OCR2A = remainingPulseTime-1; |
217 | remainingPulseTime=0; |
218 | remainingPulseTime=0; |
218 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
219 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
219 | } |
220 | } |
220 | } |
221 | } |
221 | 222 |