Rev 1910 | Rev 2102 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1910 | Rev 2099 | ||
---|---|---|---|
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 "timer2.h" |
- | |
4 | #include "eeprom.h" |
3 | #include "eeprom.h" |
5 | #include "uart0.h" |
- | |
6 | #include "rc.h" |
4 | #include "rc.h" |
7 | #include "attitude.h" |
5 | #include "attitude.h" |
Line 8... | Line -... | ||
8 | - | ||
9 | #define HEF4017_RESET_HIGH PORTC |= (1<<PORTC6) |
6 | |
Line -... | Line 7... | ||
- | 7 | #define COARSERESOLUTION 1 |
|
- | 8 | ||
10 | #define HEF4017_RESET_LOW PORTC &= ~(1<<PORTC6) |
9 | #ifdef COARSERESOLUTION |
- | 10 | #define NEUTRAL_PULSELENGTH 938 |
|
- | 11 | #define STABILIZATION_LOG_DIVIDER 6 |
|
11 | 12 | #define SERVOLIMIT 500 |
|
- | 13 | #define SCALE_FACTOR 4 |
|
- | 14 | #define CS2 ((1<<CS21)|(1<<CS20)) |
|
- | 15 | ||
- | 16 | #else |
|
- | 17 | #define NEUTRAL_PULSELENGTH 3750 |
|
- | 18 | #define STABILIZATION_LOG_DIVIDER 4 |
|
- | 19 | #define SERVOLIMIT 2000 |
|
- | 20 | #define SCALE_FACTOR 16 |
|
- | 21 | #define CS2 (1<<CS21) |
|
- | 22 | #endif |
|
- | 23 | ||
- | 24 | #define MAX_SERVOS 8 |
|
- | 25 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
|
Line -... | Line 26... | ||
- | 26 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
|
- | 27 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
|
12 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
28 | |
- | 29 | //volatile uint8_t servoActive = 0; |
|
- | 30 | volatile uint8_t recalculateServoTimes = 0; |
|
- | 31 | volatile uint16_t servoValues[MAX_SERVOS]; |
|
- | 32 | volatile uint16_t previousManualValues[2]; |
|
Line 13... | Line 33... | ||
13 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
33 | |
14 | 34 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
|
15 | OutputData_t outputs[MAX_OUTPUTS]; |
35 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
16 | 36 | ||
17 | /***************************************************** |
37 | /***************************************************** |
18 | * Initialize Timer 2 |
38 | * Initialize Timer 2 |
19 | *****************************************************/ |
39 | *****************************************************/ |
20 | void timer2_init(void) { |
40 | void timer2_init(void) { |
21 | uint8_t sreg = SREG; |
41 | uint8_t sreg = SREG; |
22 | 42 | ||
23 | // disable all interrupts before reconfiguration |
43 | // disable all interrupts before reconfiguration |
24 | cli(); |
44 | cli(); |
25 | 45 | ||
26 | // set PD7 as output of the PWM for pitch servo |
46 | // set PD7 as output of the PWM for pitch servo |
27 | DDRD |= (1 << DDD7); |
- | |
28 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
47 | DDRD |= (1 << DDD7); |
29 | 48 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
|
30 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
49 | |
31 | //PORTC &= ~(1<<PORTC6); // set PC6 to low |
- | |
32 | HEF4017_RESET_HIGH; // enable reset |
50 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
33 | 51 | HEF4017R_ON; // enable reset |
|
34 | // Timer/Counter 2 Control Register A |
52 | |
35 | 53 | // Timer/Counter 2 Control Register A |
|
36 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
54 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
37 | // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
55 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
38 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
56 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
39 | TCCR2A &= ~((1 << COM2A1) | (1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0)); |
57 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
40 | TCCR2A |= (1 << WGM21); |
58 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
41 | 59 | ||
42 | // Timer/Counter 2 Control Register B |
60 | // Timer/Counter 2 Control Register B |
43 | 61 | ||
44 | // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz |
- | |
45 | // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us |
62 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
46 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms |
63 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
47 | 64 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
|
48 | // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1) |
65 | |
49 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS22)); |
66 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
50 | TCCR2B |= (1 << CS21) | (1 << CS20); |
67 | TCCR2B |= CS2; |
51 | 68 | ||
52 | // Initialize the Timer/Counter 2 Register |
69 | // Initialize the Timer/Counter 2 Register |
53 | TCNT2 = 0; |
- | |
54 | 70 | TCNT2 = 0; |
|
55 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
71 | |
56 | OCR2A = 255; |
72 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
57 | TCCR2A |= (1 << COM2A1); // set or clear at compare match depends on value of COM2A0 |
73 | OCR2A = 255; |
58 | 74 | ||
- | 75 | // Timer/Counter 2 Interrupt Mask Register |
|
- | 76 | // Enable timer output compare match A Interrupt only |
|
- | 77 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
|
Line 59... | Line 78... | ||
59 | // Timer/Counter 2 Interrupt Mask Register |
78 | TIMSK2 |= (1 << OCIE2A); |
60 | // Enable timer output compare match A Interrupt only |
79 | |
Line -... | Line 80... | ||
- | 80 | for (uint8_t axis=0; axis<2; axis++) |
|
- | 81 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
|
- | 82 | ||
- | 83 | SREG = sreg; |
|
- | 84 | } |
|
- | 85 | ||
- | 86 | /* |
|
- | 87 | void servo_On(void) { |
|
- | 88 | servoActive = 1; |
|
- | 89 | } |
|
61 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
90 | void servo_Off(void) { |
62 | TIMSK2 |= (1 << OCIE2A); |
91 | servoActive = 0; |
63 | 92 | HEF4017R_ON; // enable reset |
|
- | 93 | } |
|
- | 94 | */ |
|
- | 95 | ||
- | 96 | /***************************************************** |
|
- | 97 | * Control Servo Position |
|
- | 98 | *****************************************************/ |
|
- | 99 | ||
- | 100 | /*typedef struct { |
|
- | 101 | uint8_t manualControl; |
|
- | 102 | uint8_t compensationFactor; |
|
- | 103 | uint8_t minValue; |
|
- | 104 | uint8_t maxValue; |
|
64 | SREG = sreg; |
105 | uint8_t flags; |
- | 106 | } servo_t;*/ |
|
- | 107 | ||
- | 108 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
|
- | 109 | int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
|
- | 110 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
|
- | 111 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
|
- | 112 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
|
65 | } |
113 | value = value >> 8; |
- | 114 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
|
- | 115 | return -value; |
|
- | 116 | return value; |
|
- | 117 | } |
|
- | 118 | ||
- | 119 | // With constant-speed limitation. |
|
- | 120 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
|
66 | 121 | int16_t diff = manualValue - previousManualValues[axis]; |
|
- | 122 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
|
- | 123 | if (diff > maxSpeed) diff = maxSpeed; |
|
- | 124 | else if (diff < -maxSpeed) diff = -maxSpeed; |
|
- | 125 | manualValue = previousManualValues[axis] + diff; |
|
- | 126 | previousManualValues[axis] = manualValue; |
|
- | 127 | return manualValue; |
|
- | 128 | } |
|
- | 129 | ||
- | 130 | // add stabilization and manual, apply soft position limits. |
|
- | 131 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
|
- | 132 | int16_t featuredServoValue(uint8_t axis) { |
|
- | 133 | int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
|
- | 134 | value += calculateStabilizedServoAxis(axis); |
|
- | 135 | int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
|
- | 136 | if (value < limit) value = limit; |
|
- | 137 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
|
- | 138 | if (value > limit) value = limit; |
|
67 | /***************************************************** |
139 | return value; |
- | 140 | } |
|
- | 141 | ||
- | 142 | uint16_t servoValue(uint8_t axis) { |
|
68 | * Control Servo Position |
143 | int16_t value; |
69 | *****************************************************/ |
144 | if (axis<2) value = featuredServoValue(axis); |
- | 145 | else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
|
- | 146 | // Shift out of the [0..255*SCALE_FACTOR] space |
|
- | 147 | value -= (128 * SCALE_FACTOR); |
|
- | 148 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
|
- | 149 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
|
- | 150 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
|
- | 151 | return value + NEUTRAL_PULSELENGTH; |
|
- | 152 | } |
|
- | 153 | ||
- | 154 | void calculateServoValues(void) { |
|
Line 70... | Line 155... | ||
70 | const uint8_t SERVO_REMAPPING[MAX_OUTPUTS] = {0,0,1,2,3,4,5,6}; |
155 | if (!recalculateServoTimes) return; |
71 | #define NEUTRAL_PULSELENGTH 937 |
156 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
72 | #define SERVOLIMIT 500 |
157 | servoValues[axis] = servoValue(axis); |
73 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.ServoRefresh + 128) |
158 | } |
Line 74... | Line 159... | ||
74 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
159 | recalculateServoTimes = 0; |
75 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
160 | } |
76 | 161 | ||
77 | ISR(TIMER2_COMPA_vect) { |
162 | ISR(TIMER2_COMPA_vect) { |
78 | static uint16_t remainingPulseTime = 0; |
- | |
79 | static uint8_t servoIndex = 0; |
- | |
80 | static uint16_t sumOfPulseTimes = 0; |
- | |
81 | 163 | static uint16_t remainingPulseTime; |
|
82 | if (!remainingPulseTime) { |
164 | static uint8_t servoIndex = 0; |
83 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
165 | static uint16_t sumOfPulseTimes = 0; |
84 | if (servoIndex < staticParams.ServoRefresh) { |
166 | |
85 | // There are more signals to output. |
167 | if (!remainingPulseTime) { |
86 | remainingPulseTime = NEUTRAL_PULSELENGTH + outputs[SERVO_REMAPPING[servoIndex]].SetPoint; |
168 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
- | 169 | if (servoIndex < staticParams.servoCount) { |
|
87 | if (remainingPulseTime < MIN_PULSELENGTH) remainingPulseTime = MIN_PULSELENGTH; |
170 | // There are more signals to output. |
88 | else if (remainingPulseTime > MAX_PULSELENGTH) remainingPulseTime = MAX_PULSELENGTH; |
171 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
89 | sumOfPulseTimes += remainingPulseTime; |
172 | servoIndex++; |
Line 90... | Line 173... | ||
90 | servoIndex++; |
173 | } else { |
91 | } else { |
- | |
92 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
174 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
93 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
- | |
94 | sumOfPulseTimes = servoIndex = 0; |
175 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
95 | HEF4017_RESET_HIGH; |
176 | sumOfPulseTimes = servoIndex = 0; |
- | 177 | recalculateServoTimes = 1; |
|
96 | } |
178 | HEF4017R_ON; |
- | 179 | } |
|
97 | } |
180 | } |
98 | 181 | ||
99 | // Schedule the next OCR2A event. The counter is already reset at this time. |
182 | // Schedule the next OCR2A event. The counter is already reset at this time. |
100 | uint8_t delta; |
183 | if (remainingPulseTime > 256+128) { |
- | 184 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
|
101 | if (remainingPulseTime > 255+128) { |
185 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
102 | delta = 255; |
186 | OCR2A = 255; |
103 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
- | |
104 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
187 | TCCR2A &= ~(1<<COM2A0); |
105 | TCCR2A &= ~(1<<COM2A0); |
188 | remainingPulseTime-=256; |
- | 189 | } else if (remainingPulseTime > 256) { |
|
- | 190 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
|
106 | } else if (remainingPulseTime > 255) { |
191 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
107 | // Remaining pulse lengths in the range [256..256+something small] might cause trouble if handled the standard |
192 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
108 | // way, which is in chunks of 255. The remainder would be very small, possibly causing an interrupt on interrupt |
- | |
109 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
- | |
110 | delta = 128; |
193 | remainingPulseTime-=128; |