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 <stdlib.h> |
1 | #include <stdlib.h> |
2 | #include <avr/io.h> |
2 | #include <avr/io.h> |
3 | #include <avr/interrupt.h> |
3 | #include <avr/interrupt.h> |
Line 4... | Line 4... | ||
4 | 4 | ||
5 | #include "rc.h" |
- | |
6 | #include "uart0.h" |
5 | #include "rc.h" |
7 | #include "controlMixer.h" |
6 | #include "controlMixer.h" |
8 | #include "configuration.h" |
7 | #include "configuration.h" |
- | 8 | #include "commands.h" |
|
Line 9... | Line 9... | ||
9 | #include "commands.h" |
9 | #include "output.h" |
10 | 10 | ||
11 | // The channel array is 1-based. The 0th entry is not used. |
- | |
12 | volatile int16_t PPM_in[MAX_CHANNELS]; |
11 | // The channel array is 0-based! |
13 | volatile uint8_t NewPpmData = 1; |
- | |
14 | volatile int16_t RC_Quality = 0; |
12 | volatile int16_t PPM_in[MAX_CHANNELS]; |
15 | int16_t RC_PRTY[4]; |
13 | volatile uint8_t RCQuality; |
Line 16... | Line -... | ||
16 | uint8_t lastRCCommand = COMMAND_NONE; |
- | |
17 | uint8_t commandTimer = 0; |
- | |
18 | - | ||
19 | // Useless. Just trim on the R/C instead. |
14 | uint8_t lastRCCommand = COMMAND_NONE; |
20 | // int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
15 | uint8_t commandTimer = 0; |
21 | 16 | ||
22 | /*************************************************************** |
17 | /*************************************************************** |
23 | * 16bit timer 1 is used to decode the PPM-Signal |
18 | * 16bit timer 1 is used to decode the PPM-Signal |
Line 24... | Line 19... | ||
24 | ***************************************************************/ |
19 | ***************************************************************/ |
25 | void RC_Init(void) { |
20 | void RC_Init(void) { |
Line 26... | Line 21... | ||
26 | uint8_t sreg = SREG; |
21 | uint8_t sreg = SREG; |
27 | 22 | ||
28 | // disable all interrupts before reconfiguration |
23 | // disable all interrupts before reconfiguration |
Line 29... | Line 24... | ||
29 | cli(); |
24 | cli(); |
30 | 25 | ||
31 | // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
26 | // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
32 | DDRD &= ~(1 << DDD6); |
27 | DDRD &= ~(1<<6); |
33 | PORTD |= (1 << PORTD6); |
28 | PORTD |= (1<<PORTD6); |
Line 34... | Line 29... | ||
34 | 29 | ||
35 | // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
30 | // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
36 | // set as output |
31 | // set as output |
37 | DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3); |
32 | DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3); |
38 | // low level |
33 | // low level |
39 | PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3)); |
34 | PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
Line 40... | Line 35... | ||
40 | 35 | ||
Line 41... | Line 36... | ||
41 | // PD3 can't be used if 2nd UART is activated |
36 | // PD3 can't be used if 2nd UART is activated |
42 | // because TXD1 is at that port |
37 | // because TXD1 is at that port |
43 | if (CPUType != ATMEGA644P) { |
38 | if (CPUType != ATMEGA644P) { |
44 | DDRD |= (1 << PORTD3); |
39 | DDRD |= (1<<PORTD3); |
45 | PORTD &= ~(1 << PORTD3); |
40 | PORTD &= ~(1<<PORTD3); |
46 | } |
41 | } |
47 | 42 | ||
48 | // Timer/Counter1 Control Register A, B, C |
43 | // Timer/Counter1 Control Register A, B, C |
49 | - | ||
50 | // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
44 | |
51 | // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
45 | // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
52 | // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
46 | // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
Line 53... | Line 47... | ||
53 | // Enable input capture noise cancler (bit: ICNC1=1) |
47 | // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
54 | // Trigger on positive edge of the input capture pin (bit: ICES1=1), |
- | |
55 | // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
48 | // Enable input capture noise cancler (bit: ICNC1=1) |
56 | // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
49 | // Trigger on positive edge of the input capture pin (bit: ICES1=1), |
57 | TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) |
50 | // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s |
58 | | (1 << WGM11) | (1 << WGM10)); |
51 | // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
59 | TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
52 | TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10)); |
Line 60... | Line 53... | ||
60 | TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
53 | TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
Line 61... | Line 54... | ||
61 | TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
54 | TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
62 | 55 | TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
|
Line 63... | Line 56... | ||
63 | // Timer/Counter1 Interrupt Mask Register |
56 | |
64 | 57 | // Timer/Counter1 Interrupt Mask Register |
|
65 | // Enable Input Capture Interrupt (bit: ICIE1=1) |
58 | // Enable Input Capture Interrupt (bit: ICIE1=1) |
66 | // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
59 | // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
67 | // Enable Overflow Interrupt (bit: TOIE1=0) |
60 | // Enable Overflow Interrupt (bit: TOIE1=0) |
68 | TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1)); |
61 | TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
69 | TIMSK1 |= (1 << ICIE1); |
62 | TIMSK1 |= (1<<ICIE1); |
70 | 63 | ||
71 | RC_Quality = 0; |
64 | RCQuality = 0; |
72 | 65 | ||
73 | SREG = sreg; |
66 | SREG = sreg; |
Line 74... | Line 67... | ||
74 | } |
67 | } |
75 | 68 | ||
76 | /********************************************************************/ |
69 | /********************************************************************/ |
77 | /* Every time a positive edge is detected at PD6 */ |
70 | /* Every time a positive edge is detected at PD6 */ |
78 | /********************************************************************/ |
71 | /********************************************************************/ |
79 | /* t-Frame |
72 | /* t-Frame |
80 | <-----------------------------------------------------------------------> |
73 | <-----------------------------------------------------------------------> |
81 | ____ ______ _____ ________ ______ sync gap ____ |
74 | ____ ______ _____ ________ ______ sync gap ____ |
82 | | | | | | | | | | | | |
75 | | | | | | | | | | | | |
83 | | | | | | | | | | | | |
- | |
84 | ___| |_| |_| |_| |_.............| |________________| |
76 | | | | | | | | | | | | |
85 | <-----><-------><------><--------> <------> <--- |
77 | ___| |_| |_| |_| |_.............| |________________| |
86 | t0 t1 t2 t4 tn t0 |
78 | <-----><-------><------><----------- <------> <--- |
87 | 79 | t0 t1 t2 t4 tn t0 |
|
Line 88... | Line 80... | ||
88 | The PPM-Frame length is 22.5 ms. |
80 | |
89 | Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse. |
81 | The PPM-Frame length is 22.5 ms. |
Line 109... | Line 101... | ||
109 | signal = (uint16_t) ICR1 - oldICR1; |
101 | signal = (uint16_t) ICR1 - oldICR1; |
110 | oldICR1 = ICR1; |
102 | oldICR1 = ICR1; |
Line 111... | Line 103... | ||
111 | 103 | ||
112 | //sync gap? (3.52 ms < signal < 25.6 ms) |
104 | //sync gap? (3.52 ms < signal < 25.6 ms) |
113 | if ((signal > 1100) && (signal < 8000)) { |
- | |
114 | // if a sync gap happens and there where at least 4 channels decoded before |
- | |
115 | // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
- | |
116 | if (index >= 4) { |
- | |
117 | NewPpmData = 0; // Null means NewData for the first 4 channels |
- | |
118 | } |
- | |
119 | // synchronize channel index |
105 | if ((signal > 1100) && (signal < 8000)) { |
120 | index = 1; |
106 | index = 0; |
121 | } else { // within the PPM frame |
107 | } else { // within the PPM frame |
122 | if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels |
108 | if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
123 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
109 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
124 | // signal range is from 1.0ms/3.2us = 312.5 to 2.0ms/3.2us = 625 |
110 | // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
125 | if ((signal > 250) && (signal < 687)) { |
111 | if ((signal > 250) && (signal < 687)) { |
126 | // shift signal to zero symmetric range -154 to 159 |
112 | // shift signal to zero symmetric range -154 to 159 |
127 | signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
113 | signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
128 | // Signal is now in the +/- 156 range (nominally). |
114 | // check for stable signal |
129 | if (abs(signal - PPM_in[index]) < 6) { |
115 | if (abs(signal - PPM_in[index]) < 6) { |
130 | if (RC_Quality < 200) |
116 | if (RCQuality < 200) |
131 | RC_Quality += 10; |
117 | RCQuality += 10; |
132 | else |
118 | else |
133 | RC_Quality = 200; |
119 | RCQuality = 200; |
- | 120 | } |
|
- | 121 | // If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff. |
|
- | 122 | // if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
|
- | 123 | // In addition, if the signal is very close to 0, just set it to 0. |
|
- | 124 | if (signal >= -1 && signal <= 1) { |
|
- | 125 | tmp = 0; |
|
- | 126 | //} else { |
|
- | 127 | // tmp = PPM_in[index]; |
|
- | 128 | // } |
|
- | 129 | } else |
|
134 | } |
130 | tmp = signal; |
135 | PPM_in[index] = signal; // update channel value |
131 | PPM_in[index] = tmp; // update channel value |
136 | } |
132 | } |
- | 133 | index++; // next channel |
|
- | 134 | // demux sum signal for channels 5 to 7 to J3, J4, J5 |
|
- | 135 | // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
|
- | 136 | // channels are usually available at the receiver anyway. |
|
- | 137 | // if(index == 5) J3HIGH; else J3LOW; |
|
- | 138 | // if(index == 6) J4HIGH; else J4LOW; |
|
- | 139 | // if(CPUType != ATMEGA644P) // not used as TXD1 |
|
- | 140 | // { |
|
- | 141 | // if(index == 7) J5HIGH; else J5LOW; |
|
137 | index++; // next channel |
142 | // } |
138 | } |
143 | } |
139 | } |
144 | } |
Line 140... | Line 145... | ||
140 | } |
145 | } |
141 | - | ||
142 | #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
- | |
143 | - | ||
144 | /* |
- | |
145 | * This must be called (as the only thing) for each control loop cycle (488 Hz). |
- | |
146 | */ |
- | |
147 | void RC_update() { |
- | |
148 | if (RC_Quality) { |
146 | |
149 | RC_Quality--; |
- | |
150 | if (NewPpmData-- == 0) { |
- | |
151 | RC_PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * staticParams.StickElevatorP * 2/ 10; |
147 | #define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
152 | RC_PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * staticParams.StickAileronsP * 2 / 10; |
148 | #define COMMAND_THRESHOLD 85 |
153 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * 2 + 310; |
- | |
154 | if (RC_PRTY[CONTROL_THROTTLE] < 0) |
- | |
155 | RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
- | |
156 | RC_PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * staticParams.StickRudderP * 2 / 10; |
- | |
157 | } |
- | |
158 | } else { // Bad signal |
- | |
159 | RC_PRTY[CONTROL_ELEVATOR] = RC_PRTY[CONTROL_AILERONS] = RC_PRTY[CONTROL_THROTTLE] |
- | |
160 | = RC_PRTY[CONTROL_RUDDER] = 0; |
- | |
Line 161... | Line 149... | ||
161 | } |
149 | #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
162 | } |
150 | #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
163 | 151 | ||
- | 152 | /* |
|
164 | /* |
153 | * Get Pitch, Roll, Throttle, Yaw values |
165 | * Get Pitch, Roll, Throttle, Yaw values |
154 | */ |
- | 155 | void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
|
- | 156 | if (RCQuality) { |
|
- | 157 | RCQuality--; |
|
- | 158 | PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR); |
|
- | 159 | PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS); |
|
- | 160 | PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); |
|
- | 161 | PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER); |
|
- | 162 | ||
- | 163 | uint8_t command = COMMAND_NONE; //RC_getStickCommand(); |
|
- | 164 | if (lastRCCommand == command) { |
|
- | 165 | // Keep timer from overrunning. |
|
- | 166 | if (commandTimer < COMMAND_TIMER) |
|
- | 167 | commandTimer++; |
|
- | 168 | } else { |
|
- | 169 | // There was a change. |
|
- | 170 | lastRCCommand = command; |
|
166 | */ |
171 | commandTimer = 0; |
Line 167... | Line 172... | ||
167 | int16_t* RC_getEATR(void) { |
172 | } |
168 | return RC_PRTY; |
173 | } // if RCQuality is no good, we just do nothing. |
169 | } |
174 | } |
170 | 175 | ||
171 | /* |
176 | /* |
172 | * Get other channel value |
177 | * Get other channel value |
173 | */ |
178 | */ |
174 | int16_t RC_getVariable(uint8_t varNum) { |
179 | int16_t RC_getVariable(uint8_t varNum) { |
175 | if (varNum < 4) |
180 | if (varNum < 4) |
176 | // 0th variable is 5th channel (1-based) etc. |
- | |
177 | return RCChannel(varNum + 4) + POT_OFFSET; |
181 | // 0th variable is 5th channel (1-based) etc. |
178 | /* |
- | |
179 | * Let's just say: |
- | |
180 | * The RC variable 4 is hardwired to channel 5 |
- | |
181 | * The RC variable 5 is hardwired to channel 6 |
182 | return RCChannel(varNum + CH_POTS) + POT_OFFSET; |
182 | * The RC variable 6 is hardwired to channel 7 |
183 | /* |
183 | * The RC variable 7 is hardwired to channel 8 |
184 | * Let's just say: |
Line 184... | Line 185... | ||
184 | * Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
185 | * The RC variable i is hardwired to channel i, i>=4 |
185 | */ |
186 | */ |
186 | return PPM_in[varNum + 1] + POT_OFFSET; |
187 | return PPM_in[varNum] + POT_OFFSET; |
187 | } |
188 | } |
188 | 189 | ||
189 | uint8_t RC_getSignalQuality(void) { |
190 | uint8_t RC_getSignalQuality(void) { |
190 | if (RC_Quality >= 160) |
191 | if (RCQuality >= 160) |
191 | return SIGNAL_GOOD; |
192 | return SIGNAL_GOOD; |
192 | if (RC_Quality >= 140) |
193 | if (RCQuality >= 140) |
Line 193... | Line 194... | ||
193 | return SIGNAL_OK; |
194 | return SIGNAL_OK; |
Line 207... | Line 208... | ||
207 | */ |
208 | */ |
208 | void RC_calibrate(void) { |
209 | void RC_calibrate(void) { |
209 | // Do nothing. |
210 | // Do nothing. |
210 | } |
211 | } |
Line 211... | Line -... | ||
211 | - | ||
212 | /* |
- | |
213 | if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
- | |
214 | // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
- | |
215 | stickOffsetPitch = stickOffsetRoll = 0; |
- | |
216 | } else { |
- | |
217 | stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
- | |
218 | stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
- | |
219 | } |
- | |
220 | } |
- | |
221 | */ |
- | |
222 | 212 | ||
- | 213 | uint8_t RC_getCommand(void) { |
|
- | 214 | if (commandTimer == COMMAND_TIMER) { |
|
- | 215 | // Stick has been held long enough; command committed. |
|
- | 216 | return lastRCCommand; |
|
223 | uint8_t RC_getCommand(void) { |
217 | } |
224 | // Noy impplemented - not from RC at least. |
218 | // Not yet sure what the command is. |
225 | return COMMAND_NONE; |
219 | return COMMAND_NONE; |
Line 226... | Line 220... | ||
226 | } |
220 | } |
227 | 221 | ||
Line 236... | Line 230... | ||
236 | * + <-- |
230 | * + <-- |
237 | * 0 |
231 | * 0 |
238 | * |
232 | * |
239 | * Not in any of these positions: 0 |
233 | * Not in any of these positions: 0 |
240 | */ |
234 | */ |
241 | uint8_t RC_getArgument(void) { |
- | |
242 | return 0; |
- | |
243 | } |
- | |
Line -... | Line 235... | ||
- | 235 | ||
- | 236 | #define ARGUMENT_THRESHOLD 70 |
|
- | 237 | #define ARGUMENT_CHANNEL_VERTICAL CH_ELEVATOR |
|
- | 238 | #define ARGUMENT_CHANNEL_HORIZONTAL CH_AILERONS |
|
244 | 239 | ||
- | 240 | uint8_t RC_getArgument(void) { |
|
- | 241 | if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
|
- | 242 | // vertical is up |
|
- | 243 | if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
|
- | 244 | return 2; |
|
- | 245 | if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
|
- | 246 | return 4; |
|
- | 247 | return 3; |
|
- | 248 | } else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
|
- | 249 | // vertical is down |
|
- | 250 | if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
|
- | 251 | return 8; |
|
- | 252 | if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
|
- | 253 | return 6; |
|
- | 254 | return 7; |
|
- | 255 | } else { |
|
- | 256 | // vertical is around center |
|
- | 257 | if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
|
- | 258 | return 1; |
|
- | 259 | if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
|
245 | uint8_t RC_testCompassCalState(void) { |
260 | return 5; |
- | 261 | return 0; |
|
246 | return 0; |
262 | } |
247 | } |
- | |
248 | /* |
- | |
249 | * Abstract controls are not used at the moment. |
- | |
250 | t_control rc_control = { |
- | |
251 | RC_getPitch, |
- | |
252 | RC_getRoll, |
- | |
253 | RC_getYaw, |
- | |
254 | RC_getThrottle, |
- | |
255 | RC_getSignalQuality, |
- | |
256 | RC_calibrate |
- | |
257 | }; |
- |