Rev 1910 | Rev 2102 | Go to most recent revision | Show entire file | Regard 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 -... | Line 6... | ||
- | 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)) |
|
- | 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 |
|
8 | 25 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
|
9 | #define HEF4017_RESET_HIGH PORTC |= (1<<PORTC6) |
26 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
- | 27 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
|
- | 28 | ||
- | 29 | //volatile uint8_t servoActive = 0; |
|
- | 30 | volatile uint8_t recalculateServoTimes = 0; |
|
- | 31 | volatile uint16_t servoValues[MAX_SERVOS]; |
|
Line 10... | Line 32... | ||
10 | #define HEF4017_RESET_LOW PORTC &= ~(1<<PORTC6) |
32 | volatile uint16_t previousManualValues[2]; |
11 | 33 | ||
Line 12... | Line -... | ||
12 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
- | |
13 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
- | |
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 |
Line 26... | Line 46... | ||
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); |
47 | DDRD |= (1 << DDD7); |
28 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
48 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
Line 29... | Line 49... | ||
29 | 49 | ||
30 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
- | |
31 | //PORTC &= ~(1<<PORTC6); // set PC6 to low |
50 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
Line 32... | Line 51... | ||
32 | HEF4017_RESET_HIGH; // enable reset |
51 | HEF4017R_ON; // enable reset |
33 | - | ||
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) |
Line 39... | Line 57... | ||
39 | TCCR2A &= ~((1 << COM2A1) | (1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0)); |
57 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
Line 40... | Line 58... | ||
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 |
Line 47... | Line 64... | ||
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 | |
Line 49... | Line 66... | ||
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 | - | ||
Line 52... | Line 68... | ||
52 | // Initialize the Timer/Counter 2 Register |
68 | |
53 | TCNT2 = 0; |
69 | // Initialize the Timer/Counter 2 Register |
54 | 70 | TCNT2 = 0; |
|
55 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
71 | |
Line -... | Line 72... | ||
- | 72 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
|
- | 73 | OCR2A = 255; |
|
- | 74 | ||
56 | OCR2A = 255; |
75 | // Timer/Counter 2 Interrupt Mask Register |
57 | TCCR2A |= (1 << COM2A1); // set or clear at compare match depends on value of COM2A0 |
76 | // Enable timer output compare match A Interrupt only |
Line -... | Line 77... | ||
- | 77 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
|
- | 78 | TIMSK2 |= (1 << OCIE2A); |
|
- | 79 | ||
- | 80 | for (uint8_t axis=0; axis<2; axis++) |
|
- | 81 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
|
- | 82 | ||
- | 83 | SREG = sreg; |
|
- | 84 | } |
|
- | 85 | ||
- | 86 | /* |
|
58 | 87 | void servo_On(void) { |
|
59 | // Timer/Counter 2 Interrupt Mask Register |
88 | servoActive = 1; |
60 | // Enable timer output compare match A Interrupt only |
89 | } |
- | 90 | void servo_Off(void) { |
|
- | 91 | servoActive = 0; |
|
- | 92 | HEF4017R_ON; // enable reset |
|
- | 93 | } |
|
- | 94 | */ |
|
- | 95 | ||
- | 96 | /***************************************************** |
|
- | 97 | * Control Servo Position |
|
- | 98 | *****************************************************/ |
|
- | 99 | ||
- | 100 | /*typedef struct { |
|
- | 101 | uint8_t manualControl; |
|
61 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
102 | uint8_t compensationFactor; |
- | 103 | uint8_t minValue; |
|
- | 104 | uint8_t maxValue; |
|
- | 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 |
|
62 | TIMSK2 |= (1 << OCIE2A); |
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; |
|
- | 113 | value = value >> 8; |
|
- | 114 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
|
- | 115 | return -value; |
|
- | 116 | return value; |
|
- | 117 | } |
|
63 | 118 | ||
- | 119 | // With constant-speed limitation. |
|
- | 120 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
|
- | 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; |
|
64 | SREG = sreg; |
136 | if (value < limit) value = limit; |
- | 137 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
|
- | 138 | if (value > limit) value = limit; |
|
- | 139 | return value; |
|
65 | } |
140 | } |
66 | 141 | ||
- | 142 | uint16_t servoValue(uint8_t axis) { |
|
- | 143 | int16_t value; |
|
- | 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; |
|
Line 67... | Line 152... | ||
67 | /***************************************************** |
152 | } |
68 | * Control Servo Position |
153 | |
69 | *****************************************************/ |
154 | void calculateServoValues(void) { |
70 | const uint8_t SERVO_REMAPPING[MAX_OUTPUTS] = {0,0,1,2,3,4,5,6}; |
155 | if (!recalculateServoTimes) return; |
Line 71... | Line 156... | ||
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 | } |
74 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
159 | recalculateServoTimes = 0; |
75 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
- | |
76 | - | ||
77 | ISR(TIMER2_COMPA_vect) { |
- | |
78 | static uint16_t remainingPulseTime = 0; |
160 | } |
79 | static uint8_t servoIndex = 0; |
161 | |
80 | static uint16_t sumOfPulseTimes = 0; |
162 | ISR(TIMER2_COMPA_vect) { |
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; |
- | 166 | ||
84 | if (servoIndex < staticParams.ServoRefresh) { |
167 | if (!remainingPulseTime) { |
85 | // There are more signals to output. |
168 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
86 | remainingPulseTime = NEUTRAL_PULSELENGTH + outputs[SERVO_REMAPPING[servoIndex]].SetPoint; |
169 | if (servoIndex < staticParams.servoCount) { |
Line 87... | Line 170... | ||
87 | if (remainingPulseTime < MIN_PULSELENGTH) remainingPulseTime = MIN_PULSELENGTH; |
170 | // There are more signals to output. |
88 | else if (remainingPulseTime > MAX_PULSELENGTH) remainingPulseTime = MAX_PULSELENGTH; |
- | |
89 | sumOfPulseTimes += remainingPulseTime; |
171 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
90 | servoIndex++; |
- | |
91 | } else { |
172 | servoIndex++; |
92 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
173 | } else { |
- | 174 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
|
93 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
175 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
- | 176 | sumOfPulseTimes = servoIndex = 0; |
|
94 | sumOfPulseTimes = servoIndex = 0; |
177 | recalculateServoTimes = 1; |
95 | HEF4017_RESET_HIGH; |
178 | HEF4017R_ON; |
96 | } |
179 | } |
97 | } |
180 | } |
- | 181 | ||
98 | 182 | // Schedule the next OCR2A event. The counter is already reset at this time. |
|
99 | // Schedule the next OCR2A event. The counter is already reset at this time. |
183 | if (remainingPulseTime > 256+128) { |
100 | uint8_t delta; |
- | |
101 | if (remainingPulseTime > 255+128) { |
184 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
102 | delta = 255; |
185 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
- | 186 | OCR2A = 255; |
|
- | 187 | TCCR2A &= ~(1<<COM2A0); |
|
103 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
188 | remainingPulseTime-=256; |
104 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
189 | } else if (remainingPulseTime > 256) { |
105 | TCCR2A &= ~(1<<COM2A0); |
- | |
106 | } else if (remainingPulseTime > 255) { |
- | |
107 | // Remaining pulse lengths in the range [256..256+something small] might cause trouble if handled the standard |
190 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |