Rev 1910 | Rev 2102 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1910 | - | 1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
||
3 | #include "eeprom.h" |
||
4 | #include "rc.h" |
||
5 | #include "attitude.h" |
||
6 | |||
2099 | - | 7 | #define COARSERESOLUTION 1 |
1910 | - | 8 | |
2099 | - | 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)) |
||
1910 | - | 15 | |
2099 | - | 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 |
||
1910 | - | 23 | |
2099 | - | 24 | #define MAX_SERVOS 8 |
25 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
||
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]; |
||
32 | volatile uint16_t previousManualValues[2]; |
||
33 | |||
34 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
||
35 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
||
36 | |||
1910 | - | 37 | /***************************************************** |
2099 | - | 38 | * Initialize Timer 2 |
1910 | - | 39 | *****************************************************/ |
40 | void timer2_init(void) { |
||
2099 | - | 41 | uint8_t sreg = SREG; |
1910 | - | 42 | |
2099 | - | 43 | // disable all interrupts before reconfiguration |
44 | cli(); |
||
45 | |||
46 | // set PD7 as output of the PWM for pitch servo |
||
47 | DDRD |= (1 << DDD7); |
||
48 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
||
49 | |||
50 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
||
51 | HEF4017R_ON; // enable reset |
||
52 | |||
53 | // Timer/Counter 2 Control Register A |
||
54 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
||
55 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
||
56 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
||
57 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
||
58 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
||
59 | |||
60 | // Timer/Counter 2 Control Register B |
||
61 | |||
62 | // 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 |
||
64 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
||
65 | |||
66 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
||
67 | TCCR2B |= CS2; |
||
68 | |||
69 | // Initialize the Timer/Counter 2 Register |
||
70 | TCNT2 = 0; |
||
71 | |||
72 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
||
73 | OCR2A = 255; |
||
74 | |||
75 | // Timer/Counter 2 Interrupt Mask Register |
||
76 | // Enable timer output compare match A Interrupt only |
||
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; |
||
1910 | - | 84 | } |
85 | |||
2099 | - | 86 | /* |
87 | void servo_On(void) { |
||
88 | servoActive = 1; |
||
89 | } |
||
90 | void servo_Off(void) { |
||
91 | servoActive = 0; |
||
92 | HEF4017R_ON; // enable reset |
||
93 | } |
||
94 | */ |
||
95 | |||
1910 | - | 96 | /***************************************************** |
2099 | - | 97 | * Control Servo Position |
1910 | - | 98 | *****************************************************/ |
99 | |||
2099 | - | 100 | /*typedef struct { |
101 | uint8_t manualControl; |
||
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 |
||
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 | } |
||
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; |
||
136 | if (value < limit) value = limit; |
||
137 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
||
138 | if (value > limit) value = limit; |
||
139 | return value; |
||
140 | } |
||
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; |
||
152 | } |
||
153 | |||
154 | void calculateServoValues(void) { |
||
155 | if (!recalculateServoTimes) return; |
||
156 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
||
157 | servoValues[axis] = servoValue(axis); |
||
158 | } |
||
159 | recalculateServoTimes = 0; |
||
160 | } |
||
161 | |||
1910 | - | 162 | ISR(TIMER2_COMPA_vect) { |
2099 | - | 163 | static uint16_t remainingPulseTime; |
1910 | - | 164 | static uint8_t servoIndex = 0; |
165 | static uint16_t sumOfPulseTimes = 0; |
||
166 | |||
167 | if (!remainingPulseTime) { |
||
168 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
||
2099 | - | 169 | if (servoIndex < staticParams.servoCount) { |
1910 | - | 170 | // There are more signals to output. |
2099 | - | 171 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
1910 | - | 172 | servoIndex++; |
173 | } else { |
||
174 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
||
175 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
||
176 | sumOfPulseTimes = servoIndex = 0; |
||
2099 | - | 177 | recalculateServoTimes = 1; |
178 | HEF4017R_ON; |
||
1910 | - | 179 | } |
180 | } |
||
181 | |||
182 | // Schedule the next OCR2A event. The counter is already reset at this time. |
||
2099 | - | 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, |
||
1910 | - | 185 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
2099 | - | 186 | OCR2A = 255; |
1910 | - | 187 | TCCR2A &= ~(1<<COM2A0); |
2099 | - | 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 |
||
191 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
||
1910 | - | 192 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
2099 | - | 193 | remainingPulseTime-=128; |
194 | OCR2A=127; |
||
1910 | - | 195 | } else { |
196 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
||
197 | TCCR2A |= (1<<COM2A0); |
||
2099 | - | 198 | OCR2A = remainingPulseTime-1; |
199 | remainingPulseTime=0; |
||
200 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
||
1910 | - | 201 | } |
202 | } |