Rev 2051 | Rev 2055 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
||
3 | #include <avr/wdt.h> |
||
4 | #include <avr/pgmspace.h> |
||
5 | #include <stdarg.h> |
||
6 | #include <string.h> |
||
7 | |||
8 | #include "eeprom.h" |
||
9 | #include "menu.h" |
||
10 | #include "timer0.h" |
||
11 | #include "uart0.h" |
||
12 | #include "rc.h" |
||
13 | #include "externalControl.h" |
||
1775 | - | 14 | #include "output.h" |
1864 | - | 15 | #include "attitude.h" |
2048 | - | 16 | #include "commands.h" |
1612 | dongfang | 17 | |
2039 | - | 18 | #ifdef USE_DIRECT_GPS |
1612 | dongfang | 19 | #include "mk3mag.h" |
20 | #endif |
||
21 | |||
22 | #define FC_ADDRESS 1 |
||
23 | #define NC_ADDRESS 2 |
||
24 | #define MK3MAG_ADDRESS 3 |
||
25 | |||
26 | #define FALSE 0 |
||
27 | #define TRUE 1 |
||
28 | //int8_t test __attribute__ ((section (".noinit"))); |
||
2018 | - | 29 | uint8_t request_verInfo = FALSE; |
30 | uint8_t request_externalControl = FALSE; |
||
31 | uint8_t request_display = FALSE; |
||
32 | uint8_t request_display1 = FALSE; |
||
33 | uint8_t request_debugData = FALSE; |
||
34 | uint8_t request_data3D = FALSE; |
||
35 | uint8_t request_debugLabel = 255; |
||
1821 | - | 36 | uint8_t request_PPMChannels = FALSE; |
2018 | - | 37 | uint8_t request_motorTest = FALSE; |
1821 | - | 38 | uint8_t request_variables = FALSE; |
1775 | - | 39 | |
2018 | - | 40 | uint8_t displayLine = 0; |
1612 | dongfang | 41 | |
42 | volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
||
43 | volatile uint8_t rxd_buffer_locked = FALSE; |
||
44 | volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
||
45 | volatile uint8_t txd_complete = TRUE; |
||
2018 | - | 46 | volatile uint8_t receivedBytes = 0; |
1612 | dongfang | 47 | volatile uint8_t *pRxData = 0; |
2018 | - | 48 | volatile uint8_t rxDataLen = 0; |
1612 | dongfang | 49 | |
1821 | - | 50 | uint8_t motorTestActive = 0; |
51 | uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
||
2018 | - | 52 | uint8_t confirmFrame; |
1612 | dongfang | 53 | |
54 | typedef struct { |
||
2039 | - | 55 | int16_t heading; |
1821 | - | 56 | }__attribute__((packed)) Heading_t; |
1612 | dongfang | 57 | |
2018 | - | 58 | Data3D_t data3D; |
1612 | dongfang | 59 | |
2018 | - | 60 | uint16_t debugData_timer; |
61 | uint16_t data3D_timer; |
||
62 | uint16_t debugData_interval = 0; // in 1ms |
||
63 | uint16_t data3D_interval = 0; // in 1ms |
||
1612 | dongfang | 64 | |
2039 | - | 65 | #ifdef USE_DIRECT_GPS |
66 | int16_t toMk3MagTimer; |
||
1612 | dongfang | 67 | #endif |
68 | |||
69 | // keep lables in flash to save 512 bytes of sram space |
||
70 | const prog_uint8_t ANALOG_LABEL[32][16] = { |
||
1821 | - | 71 | //1234567890123456 |
72 | "AnglePitch ", //0 |
||
73 | "AngleRoll ", |
||
74 | "AngleYaw ", |
||
1974 | - | 75 | "GyroPitch ", |
76 | "GyroRoll ", |
||
77 | "GyroYaw ", //5 |
||
78 | "AccPitch ", |
||
79 | "AccRoll ", |
||
80 | "AccZ ", |
||
1821 | - | 81 | "AccPitch (angle)", |
82 | "AccRoll (angle) ", //10 |
||
2044 | - | 83 | "GPS sat ", |
84 | "mag hdng ", |
||
1821 | - | 85 | "Pitch Term ", |
86 | "Roll Term ", |
||
2044 | - | 87 | "Yaw Term ", //15 |
88 | "Throttle Term ", |
||
1986 | - | 89 | "ControlAct/10 ", |
2045 | - | 90 | "RC Qual ", |
2035 | - | 91 | "heightTrottleIn ", |
92 | "HeightdThrottle ", //20 |
||
2026 | - | 93 | "Height ", |
2035 | - | 94 | "TargetHeight ", |
95 | "heightError ", |
||
96 | "HeightPdThrottle", |
||
97 | "HeightIdThrottle", //25 |
||
98 | "HeightDdThrottle", |
||
2048 | - | 99 | "HeadingInDegrees", |
2051 | - | 100 | "0 ", |
101 | "1 ", |
||
102 | "2 ", //30 |
||
103 | "3 " |
||
2045 | - | 104 | }; |
1612 | dongfang | 105 | |
106 | /****************************************************************/ |
||
107 | /* Initialization of the USART0 */ |
||
108 | /****************************************************************/ |
||
2018 | - | 109 | void usart0_init(void) { |
1821 | - | 110 | uint8_t sreg = SREG; |
111 | uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1); |
||
112 | |||
113 | // disable all interrupts before configuration |
||
114 | cli(); |
||
115 | |||
116 | // disable RX-Interrupt |
||
117 | UCSR0B &= ~(1 << RXCIE0); |
||
118 | // disable TX-Interrupt |
||
119 | UCSR0B &= ~(1 << TXCIE0); |
||
120 | |||
121 | // set direction of RXD0 and TXD0 pins |
||
122 | // set RXD0 (PD0) as an input pin |
||
123 | PORTD |= (1 << PORTD0); |
||
124 | DDRD &= ~(1 << DDD0); |
||
125 | // set TXD0 (PD1) as an output pin |
||
126 | PORTD |= (1 << PORTD1); |
||
127 | DDRD |= (1 << DDD1); |
||
128 | |||
129 | // USART0 Baud Rate Register |
||
130 | // set clock divider |
||
131 | UBRR0H = (uint8_t) (ubrr >> 8); |
||
132 | UBRR0L = (uint8_t) ubrr; |
||
133 | |||
134 | // USART0 Control and Status Register A, B, C |
||
135 | |||
136 | // enable double speed operation in |
||
137 | UCSR0A |= (1 << U2X0); |
||
138 | // enable receiver and transmitter in |
||
139 | UCSR0B = (1 << TXEN0) | (1 << RXEN0); |
||
140 | // set asynchronous mode |
||
141 | UCSR0C &= ~(1 << UMSEL01); |
||
142 | UCSR0C &= ~(1 << UMSEL00); |
||
143 | // no parity |
||
144 | UCSR0C &= ~(1 << UPM01); |
||
145 | UCSR0C &= ~(1 << UPM00); |
||
146 | // 1 stop bit |
||
147 | UCSR0C &= ~(1 << USBS0); |
||
148 | // 8-bit |
||
149 | UCSR0B &= ~(1 << UCSZ02); |
||
150 | UCSR0C |= (1 << UCSZ01); |
||
151 | UCSR0C |= (1 << UCSZ00); |
||
152 | |||
153 | // flush receive buffer |
||
154 | while (UCSR0A & (1 << RXC0)) |
||
155 | UDR0; |
||
156 | |||
157 | // enable interrupts at the end |
||
158 | // enable RX-Interrupt |
||
159 | UCSR0B |= (1 << RXCIE0); |
||
160 | // enable TX-Interrupt |
||
161 | UCSR0B |= (1 << TXCIE0); |
||
162 | |||
163 | // initialize the debug timer |
||
2018 | - | 164 | debugData_timer = setDelay(debugData_interval); |
1821 | - | 165 | |
166 | // unlock rxd_buffer |
||
167 | rxd_buffer_locked = FALSE; |
||
168 | pRxData = 0; |
||
2018 | - | 169 | rxDataLen = 0; |
1821 | - | 170 | |
171 | // no bytes to send |
||
172 | txd_complete = TRUE; |
||
173 | |||
2039 | - | 174 | #ifdef USE_DIRECT_GPS |
175 | toMk3MagTimer = setDelay(220); |
||
1612 | dongfang | 176 | #endif |
1821 | - | 177 | |
2018 | - | 178 | versionInfo.SWMajor = VERSION_MAJOR; |
179 | versionInfo.SWMinor = VERSION_MINOR; |
||
180 | versionInfo.SWPatch = VERSION_PATCH; |
||
181 | versionInfo.protoMajor = VERSION_SERIAL_MAJOR; |
||
182 | versionInfo.protoMinor = VERSION_SERIAL_MINOR; |
||
1821 | - | 183 | |
184 | // restore global interrupt flags |
||
185 | SREG = sreg; |
||
1612 | dongfang | 186 | } |
187 | |||
188 | /****************************************************************/ |
||
189 | /* USART0 transmitter ISR */ |
||
190 | /****************************************************************/ |
||
2018 | - | 191 | ISR(USART0_TX_vect) { |
1821 | - | 192 | static uint16_t ptr_txd_buffer = 0; |
193 | uint8_t tmp_tx; |
||
194 | if (!txd_complete) { // transmission not completed |
||
195 | ptr_txd_buffer++; // die [0] wurde schon gesendet |
||
196 | tmp_tx = txd_buffer[ptr_txd_buffer]; |
||
197 | // if terminating character or end of txd buffer was reached |
||
198 | if ((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) { |
||
199 | ptr_txd_buffer = 0; // reset txd pointer |
||
200 | txd_complete = 1; // stop transmission |
||
201 | } |
||
202 | UDR0 = tmp_tx; // send current byte will trigger this ISR again |
||
203 | } |
||
204 | // transmission completed |
||
205 | else |
||
206 | ptr_txd_buffer = 0; |
||
1612 | dongfang | 207 | } |
208 | |||
209 | /****************************************************************/ |
||
210 | /* USART0 receiver ISR */ |
||
211 | /****************************************************************/ |
||
2018 | - | 212 | ISR(USART0_RX_vect) { |
1969 | - | 213 | static uint16_t checksum; |
1821 | - | 214 | static uint8_t ptr_rxd_buffer = 0; |
1969 | - | 215 | uint8_t checksum1, checksum2; |
1821 | - | 216 | uint8_t c; |
1612 | dongfang | 217 | |
1821 | - | 218 | c = UDR0; // catch the received byte |
1612 | dongfang | 219 | |
1821 | - | 220 | if (rxd_buffer_locked) |
221 | return; // if rxd buffer is locked immediately return |
||
1612 | dongfang | 222 | |
1821 | - | 223 | // the rxd buffer is unlocked |
224 | if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
||
225 | rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
||
1969 | - | 226 | checksum = c; // init checksum |
1821 | - | 227 | } |
1612 | dongfang | 228 | #if 0 |
1821 | - | 229 | else if (ptr_rxd_buffer == 1) { // handle address |
230 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
||
1969 | - | 231 | checksum += c; // update checksum |
1821 | - | 232 | } |
1612 | dongfang | 233 | #endif |
1821 | - | 234 | else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
235 | if (c != '\r') { // no termination character |
||
236 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
||
1969 | - | 237 | checksum += c; // update checksum |
1821 | - | 238 | } else { // termination character was received |
239 | // the last 2 bytes are no subject for checksum calculation |
||
240 | // they are the checksum itself |
||
1969 | - | 241 | checksum -= rxd_buffer[ptr_rxd_buffer - 2]; |
242 | checksum -= rxd_buffer[ptr_rxd_buffer - 1]; |
||
1821 | - | 243 | // calculate checksum from transmitted data |
1969 | - | 244 | checksum %= 4096; |
245 | checksum1 = '=' + checksum / 64; |
||
246 | checksum2 = '=' + checksum % 64; |
||
1821 | - | 247 | // compare checksum to transmitted checksum bytes |
1969 | - | 248 | if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
1821 | - | 249 | == rxd_buffer[ptr_rxd_buffer - 1])) { |
250 | // checksum valid |
||
251 | rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
||
2018 | - | 252 | receivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
1821 | - | 253 | rxd_buffer_locked = TRUE; // lock the rxd buffer |
254 | // if 2nd byte is an 'R' enable watchdog that will result in an reset |
||
255 | if (rxd_buffer[2] == 'R') { |
||
256 | wdt_enable(WDTO_250MS); |
||
257 | } // Reset-Commando |
||
258 | } else { // checksum invalid |
||
259 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
||
260 | } |
||
261 | ptr_rxd_buffer = 0; // reset rxd buffer pointer |
||
262 | } |
||
263 | } else { // rxd buffer overrun |
||
264 | ptr_rxd_buffer = 0; // reset rxd buffer |
||
265 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
||
266 | } |
||
1612 | dongfang | 267 | } |
268 | |||
269 | // -------------------------------------------------------------------------- |
||
1969 | - | 270 | void Addchecksum(uint16_t datalen) { |
271 | uint16_t tmpchecksum = 0, i; |
||
1821 | - | 272 | for (i = 0; i < datalen; i++) { |
1969 | - | 273 | tmpchecksum += txd_buffer[i]; |
1821 | - | 274 | } |
1969 | - | 275 | tmpchecksum %= 4096; |
276 | txd_buffer[i++] = '=' + tmpchecksum / 64; |
||
277 | txd_buffer[i++] = '=' + tmpchecksum % 64; |
||
1821 | - | 278 | txd_buffer[i++] = '\r'; |
279 | txd_complete = FALSE; |
||
280 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
||
1612 | dongfang | 281 | } |
282 | |||
283 | // -------------------------------------------------------------------------- |
||
1775 | - | 284 | // application example: |
2018 | - | 285 | // sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
1775 | - | 286 | /* |
2018 | - | 287 | void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
1821 | - | 288 | va_list ap; |
289 | uint16_t txd_bufferIndex = 0; |
||
290 | uint8_t *currentBuffer; |
||
291 | uint8_t currentBufferIndex; |
||
292 | uint16_t lengthOfCurrentBuffer; |
||
293 | uint8_t shift = 0; |
||
1775 | - | 294 | |
1821 | - | 295 | txd_buffer[txd_bufferIndex++] = '#'; // Start character |
296 | txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
||
297 | txd_buffer[txd_bufferIndex++] = cmd; // Command |
||
1775 | - | 298 | |
1821 | - | 299 | va_start(ap, numofbuffers); |
300 | |||
301 | while(numofbuffers) { |
||
302 | currentBuffer = va_arg(ap, uint8_t*); |
||
303 | lengthOfCurrentBuffer = va_arg(ap, int); |
||
304 | currentBufferIndex = 0; |
||
305 | // Encode data: 3 bytes of data are encoded into 4 bytes, |
||
306 | // where the 2 most significant bits are both 0. |
||
307 | while(currentBufferIndex != lengthOfCurrentBuffer) { |
||
308 | if (!shift) txd_buffer[txd_bufferIndex] = 0; |
||
309 | txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
||
310 | txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
||
311 | shift += 2; |
||
312 | if (shift == 6) { shift=0; txd_bufferIndex++; } |
||
313 | currentBufferIndex++; |
||
314 | } |
||
315 | } |
||
316 | // If the number of data bytes was not divisible by 3, stuff |
||
317 | // with 0 pseudodata until length is again divisible by 3. |
||
318 | if (shift == 2) { |
||
319 | // We need to stuff with zero bytes at the end. |
||
320 | txd_buffer[txd_bufferIndex] &= 0b00110000; |
||
321 | txd_buffer[++txd_bufferIndex] = 0; |
||
322 | shift = 4; |
||
323 | } |
||
324 | if (shift == 4) { |
||
325 | // We need to stuff with zero bytes at the end. |
||
326 | txd_buffer[txd_bufferIndex++] &= 0b00111100; |
||
327 | txd_buffer[txd_bufferIndex] = 0; |
||
328 | } |
||
329 | va_end(ap); |
||
1969 | - | 330 | Addchecksum(pt); // add checksum after data block and initates the transmission |
1821 | - | 331 | } |
332 | */ |
||
333 | |||
2018 | - | 334 | void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
1821 | - | 335 | va_list ap; |
336 | uint16_t pt = 0; |
||
337 | uint8_t a, b, c; |
||
338 | uint8_t ptr = 0; |
||
1612 | dongfang | 339 | |
1821 | - | 340 | uint8_t *pdata = 0; |
341 | int len = 0; |
||
1612 | dongfang | 342 | |
1821 | - | 343 | txd_buffer[pt++] = '#'; // Start character |
344 | txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...) |
||
345 | txd_buffer[pt++] = cmd; // Command |
||
346 | |||
347 | va_start(ap, numofbuffers); |
||
348 | |||
349 | if (numofbuffers) { |
||
350 | pdata = va_arg(ap, uint8_t*); |
||
351 | len = va_arg(ap, int); |
||
352 | ptr = 0; |
||
353 | numofbuffers--; |
||
354 | } |
||
355 | |||
356 | while (len) { |
||
357 | if (len) { |
||
358 | a = pdata[ptr++]; |
||
359 | len--; |
||
360 | if ((!len) && numofbuffers) { |
||
361 | pdata = va_arg(ap, uint8_t*); |
||
362 | len = va_arg(ap, int); |
||
363 | ptr = 0; |
||
364 | numofbuffers--; |
||
365 | } |
||
366 | } else |
||
367 | a = 0; |
||
368 | if (len) { |
||
369 | b = pdata[ptr++]; |
||
370 | len--; |
||
371 | if ((!len) && numofbuffers) { |
||
372 | pdata = va_arg(ap, uint8_t*); |
||
373 | len = va_arg(ap, int); |
||
374 | ptr = 0; |
||
375 | numofbuffers--; |
||
376 | } |
||
377 | } else |
||
378 | b = 0; |
||
379 | if (len) { |
||
380 | c = pdata[ptr++]; |
||
381 | len--; |
||
382 | if ((!len) && numofbuffers) { |
||
383 | pdata = va_arg(ap, uint8_t*); |
||
384 | len = va_arg(ap, int); |
||
385 | ptr = 0; |
||
386 | numofbuffers--; |
||
387 | } |
||
388 | } else |
||
389 | c = 0; |
||
390 | txd_buffer[pt++] = '=' + (a >> 2); |
||
391 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
||
392 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
||
393 | txd_buffer[pt++] = '=' + (c & 0x3f); |
||
394 | } |
||
395 | va_end(ap); |
||
1969 | - | 396 | Addchecksum(pt); // add checksum after data block and initates the transmission |
1612 | dongfang | 397 | } |
398 | |||
399 | // -------------------------------------------------------------------------- |
||
400 | void Decode64(void) { |
||
1821 | - | 401 | uint8_t a, b, c, d; |
402 | uint8_t x, y, z; |
||
403 | uint8_t ptrIn = 3; |
||
404 | uint8_t ptrOut = 3; |
||
2018 | - | 405 | uint8_t len = receivedBytes - 6; |
1821 | - | 406 | |
407 | while (len) { |
||
408 | a = rxd_buffer[ptrIn++] - '='; |
||
409 | b = rxd_buffer[ptrIn++] - '='; |
||
410 | c = rxd_buffer[ptrIn++] - '='; |
||
411 | d = rxd_buffer[ptrIn++] - '='; |
||
412 | //if(ptrIn > ReceivedBytes - 3) break; |
||
413 | |||
414 | x = (a << 2) | (b >> 4); |
||
415 | y = ((b & 0x0f) << 4) | (c >> 2); |
||
416 | z = ((c & 0x03) << 6) | d; |
||
417 | |||
418 | if (len--) |
||
419 | rxd_buffer[ptrOut++] = x; |
||
420 | else |
||
421 | break; |
||
422 | if (len--) |
||
423 | rxd_buffer[ptrOut++] = y; |
||
424 | else |
||
425 | break; |
||
426 | if (len--) |
||
427 | rxd_buffer[ptrOut++] = z; |
||
428 | else |
||
429 | break; |
||
430 | } |
||
431 | pRxData = &rxd_buffer[3]; |
||
2018 | - | 432 | rxDataLen = ptrOut - 3; |
1612 | dongfang | 433 | } |
434 | |||
435 | // -------------------------------------------------------------------------- |
||
2018 | - | 436 | void usart0_processRxData(void) { |
1821 | - | 437 | // We control the motorTestActive var from here: Count it down. |
438 | if (motorTestActive) |
||
439 | motorTestActive--; |
||
440 | // if data in the rxd buffer are not locked immediately return |
||
441 | if (!rxd_buffer_locked) |
||
442 | return; |
||
1980 | - | 443 | uint8_t tempchar[3]; |
1821 | - | 444 | Decode64(); // decode data block in rxd_buffer |
1775 | - | 445 | |
1821 | - | 446 | switch (rxd_buffer[1] - 'a') { |
1775 | - | 447 | |
1821 | - | 448 | case FC_ADDRESS: |
449 | switch (rxd_buffer[2]) { |
||
2039 | - | 450 | #ifdef USE_DIRECT_GPS |
1821 | - | 451 | case 'K':// compass value |
2041 | - | 452 | // What is the point of this - the compass will overwrite this soon? |
453 | magneticHeading = ((Heading_t *)pRxData)->heading; |
||
1821 | - | 454 | // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
455 | break; |
||
1612 | dongfang | 456 | #endif |
1821 | - | 457 | case 't': // motor test |
2018 | - | 458 | if (rxDataLen > 20) { |
1821 | - | 459 | memcpy(&motorTest[0], (uint8_t*) pRxData, sizeof(motorTest)); |
460 | } else { |
||
461 | memcpy(&motorTest[0], (uint8_t*) pRxData, 4); |
||
462 | } |
||
463 | motorTestActive = 255; |
||
464 | externalControlActive = 255; |
||
465 | break; |
||
1612 | dongfang | 466 | |
1821 | - | 467 | case 'n':// "Get Mixer Table |
468 | while (!txd_complete) |
||
469 | ; // wait for previous frame to be sent |
||
2018 | - | 470 | sendOutData('N', FC_ADDRESS, 1, (uint8_t *) &mixerMatrix, sizeof(mixerMatrix)); |
1821 | - | 471 | break; |
1612 | dongfang | 472 | |
1821 | - | 473 | case 'm':// "Set Mixer Table |
474 | if (pRxData[0] == EEMIXER_REVISION) { |
||
1960 | - | 475 | memcpy(&mixerMatrix, (uint8_t*) pRxData, sizeof(mixerMatrix)); |
476 | mixerMatrix_writeToEEProm(); |
||
1821 | - | 477 | while (!txd_complete) |
478 | ; // wait for previous frame to be sent |
||
1980 | - | 479 | tempchar[0] = 1; |
1821 | - | 480 | } else { |
1980 | - | 481 | tempchar[0] = 0; |
1821 | - | 482 | } |
2018 | - | 483 | sendOutData('M', FC_ADDRESS, 1, &tempchar, 1); |
1821 | - | 484 | break; |
1612 | dongfang | 485 | |
1821 | - | 486 | case 'p': // get PPM channels |
487 | request_PPMChannels = TRUE; |
||
488 | break; |
||
1612 | dongfang | 489 | |
1821 | - | 490 | case 'q':// request settings |
491 | if (pRxData[0] == 0xFF) { |
||
1960 | - | 492 | pRxData[0] = getParamByte(PID_ACTIVE_SET); |
1821 | - | 493 | } |
494 | // limit settings range |
||
495 | if (pRxData[0] < 1) |
||
496 | pRxData[0] = 1; // limit to 1 |
||
497 | else if (pRxData[0] > 5) |
||
498 | pRxData[0] = 5; // limit to 5 |
||
499 | // load requested parameter set |
||
1960 | - | 500 | paramSet_readFromEEProm(pRxData[0]); |
1980 | - | 501 | tempchar[0] = pRxData[0]; |
502 | tempchar[1] = EEPARAM_REVISION; |
||
503 | tempchar[2] = sizeof(staticParams); |
||
1821 | - | 504 | while (!txd_complete) |
505 | ; // wait for previous frame to be sent |
||
2018 | - | 506 | sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams)); |
1821 | - | 507 | break; |
1612 | dongfang | 508 | |
1821 | - | 509 | case 's': // save settings |
510 | if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
||
511 | { |
||
2051 | - | 512 | if ((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION) && (pRxData[2] == sizeof(staticParams))) // check for setting to be in range and version of settings |
1821 | - | 513 | { |
2051 | - | 514 | memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams)); |
1960 | - | 515 | paramSet_writeToEEProm(pRxData[0]); |
1980 | - | 516 | tempchar[0] = getActiveParamSet(); |
517 | beepNumber(tempchar[0]); |
||
1821 | - | 518 | } else { |
1980 | - | 519 | tempchar[0] = 0; //indicate bad data |
1821 | - | 520 | } |
521 | while (!txd_complete) |
||
522 | ; // wait for previous frame to be sent |
||
2018 | - | 523 | sendOutData('S', FC_ADDRESS, 1, &tempchar, 1); |
1821 | - | 524 | } |
525 | break; |
||
1612 | dongfang | 526 | |
1821 | - | 527 | default: |
528 | //unsupported command received |
||
529 | break; |
||
530 | } // case FC_ADDRESS: |
||
1612 | dongfang | 531 | |
1821 | - | 532 | default: // any Slave Address |
533 | switch (rxd_buffer[2]) { |
||
534 | case 'a':// request for labels of the analog debug outputs |
||
2018 | - | 535 | request_debugLabel = pRxData[0]; |
536 | if (request_debugLabel > 31) |
||
537 | request_debugLabel = 31; |
||
1821 | - | 538 | break; |
1612 | dongfang | 539 | |
1821 | - | 540 | case 'b': // submit extern control |
541 | memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl)); |
||
2018 | - | 542 | confirmFrame = externalControl.frame; |
1821 | - | 543 | externalControlActive = 255; |
544 | break; |
||
1612 | dongfang | 545 | |
1821 | - | 546 | case 'h':// request for display columns |
2018 | - | 547 | remoteKeys |= pRxData[0]; |
548 | if (remoteKeys) |
||
549 | displayLine = 0; |
||
550 | request_display = TRUE; |
||
1821 | - | 551 | break; |
1612 | dongfang | 552 | |
1821 | - | 553 | case 'l':// request for display columns |
2018 | - | 554 | menuItem = pRxData[0]; |
555 | request_display1 = TRUE; |
||
1821 | - | 556 | break; |
1612 | dongfang | 557 | |
1821 | - | 558 | case 'v': // request for version and board release |
2018 | - | 559 | request_verInfo = TRUE; |
1821 | - | 560 | break; |
1775 | - | 561 | |
1821 | - | 562 | case 'x': |
563 | request_variables = TRUE; |
||
564 | break; |
||
1612 | dongfang | 565 | |
1821 | - | 566 | case 'g':// get external control data |
2018 | - | 567 | request_externalControl = TRUE; |
1821 | - | 568 | break; |
1612 | dongfang | 569 | |
1821 | - | 570 | case 'd': // request for the debug data |
2018 | - | 571 | debugData_interval = (uint16_t) pRxData[0] * 10; |
572 | if (debugData_interval > 0) |
||
573 | request_debugData = TRUE; |
||
1821 | - | 574 | break; |
1612 | dongfang | 575 | |
1821 | - | 576 | case 'c': // request for the 3D data |
2018 | - | 577 | data3D_interval = (uint16_t) pRxData[0] * 10; |
578 | if (data3D_interval > 0) |
||
579 | request_data3D = TRUE; |
||
1821 | - | 580 | break; |
581 | |||
582 | default: |
||
583 | //unsupported command received |
||
584 | break; |
||
585 | } |
||
586 | break; // default: |
||
587 | } |
||
588 | // unlock the rxd buffer after processing |
||
589 | pRxData = 0; |
||
2018 | - | 590 | rxDataLen = 0; |
1821 | - | 591 | rxd_buffer_locked = FALSE; |
1612 | dongfang | 592 | } |
593 | |||
1645 | - | 594 | /************************************************************************/ |
2035 | - | 595 | /* Routine f�r die Serielle Ausgabe */ |
1645 | - | 596 | /************************************************************************/ |
1821 | - | 597 | int16_t uart_putchar(int8_t c) { |
598 | if (c == '\n') |
||
599 | uart_putchar('\r'); |
||
600 | // wait until previous character was send |
||
601 | loop_until_bit_is_set(UCSR0A, UDRE0); |
||
602 | // send character |
||
603 | UDR0 = c; |
||
604 | return (0); |
||
1612 | dongfang | 605 | } |
606 | |||
607 | //--------------------------------------------------------------------------------------------- |
||
2018 | - | 608 | void usart0_transmitTxData(void) { |
1821 | - | 609 | if (!txd_complete) |
610 | return; |
||
1612 | dongfang | 611 | |
2018 | - | 612 | if (request_verInfo && txd_complete) { |
613 | sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo)); |
||
614 | request_verInfo = FALSE; |
||
1821 | - | 615 | } |
1612 | dongfang | 616 | |
2018 | - | 617 | if (request_display && txd_complete) { |
618 | LCD_printMenu(); |
||
619 | sendOutData('H', FC_ADDRESS, 2, &displayLine, sizeof(displayLine), |
||
620 | &displayBuff[displayLine * 20], 20); |
||
621 | displayLine++; |
||
622 | if (displayLine >= 4) |
||
623 | displayLine = 0; |
||
624 | request_display = FALSE; |
||
1821 | - | 625 | } |
1612 | dongfang | 626 | |
2018 | - | 627 | if (request_display1 && txd_complete) { |
628 | LCD_printMenu(); |
||
629 | sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem, |
||
630 | sizeof(maxMenuItem), displayBuff, sizeof(displayBuff)); |
||
631 | request_display1 = FALSE; |
||
1821 | - | 632 | } |
633 | |||
2035 | - | 634 | if (request_debugLabel != 0xFF) { // Texte f�r die Analogdaten |
1821 | - | 635 | uint8_t label[16]; // local sram buffer |
2018 | - | 636 | memcpy_P(label, ANALOG_LABEL[request_debugLabel], 16); // read lable from flash to sram buffer |
637 | sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_debugLabel, |
||
638 | sizeof(request_debugLabel), label, 16); |
||
639 | request_debugLabel = 0xFF; |
||
1821 | - | 640 | } |
641 | |||
2035 | - | 642 | if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen |
2018 | - | 643 | sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame)); |
644 | confirmFrame = 0; |
||
1821 | - | 645 | } |
646 | |||
2018 | - | 647 | if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData) |
1821 | - | 648 | && txd_complete) { |
2018 | - | 649 | sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut)); |
650 | debugData_timer = setDelay(debugData_interval); |
||
651 | request_debugData = FALSE; |
||
1821 | - | 652 | } |
653 | |||
2018 | - | 654 | if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) |
1821 | - | 655 | && txd_complete) { |
2018 | - | 656 | sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
2048 | - | 657 | data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
658 | data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
||
659 | data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1° |
||
2018 | - | 660 | data3D_timer = setDelay(data3D_interval); |
661 | request_data3D = FALSE; |
||
1821 | - | 662 | } |
663 | |||
2018 | - | 664 | if (request_externalControl && txd_complete) { |
665 | sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl, |
||
1821 | - | 666 | sizeof(externalControl)); |
2018 | - | 667 | request_externalControl = FALSE; |
1821 | - | 668 | } |
669 | |||
2039 | - | 670 | #ifdef USE_DIRECT_GPS |
671 | if((checkDelay(toMk3MagTimer)) && txd_complete) { |
||
2048 | - | 672 | toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg |
673 | toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg |
||
2039 | - | 674 | toMk3Mag.userParam[0] = dynamicParams.userParams[0]; |
675 | toMk3Mag.userParam[1] = dynamicParams.userParams[1]; |
||
676 | toMk3Mag.calState = compassCalState; |
||
2018 | - | 677 | sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |
1821 | - | 678 | // the last state is 5 and should be send only once to avoid multiple flash writing |
679 | if(compassCalState > 4) compassCalState = 0; |
||
2039 | - | 680 | toMk3MagTimer = setDelay(99); |
1821 | - | 681 | } |
1612 | dongfang | 682 | #endif |
683 | |||
2018 | - | 684 | if (request_motorTest && txd_complete) { |
685 | sendOutData('T', FC_ADDRESS, 0); |
||
686 | request_motorTest = FALSE; |
||
1821 | - | 687 | } |
1775 | - | 688 | |
1821 | - | 689 | if (request_PPMChannels && txd_complete) { |
2018 | - | 690 | sendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in)); |
1821 | - | 691 | request_PPMChannels = FALSE; |
692 | } |
||
693 | |||
694 | if (request_variables && txd_complete) { |
||
2018 | - | 695 | sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables)); |
1821 | - | 696 | request_variables = FALSE; |
697 | } |
||
1612 | dongfang | 698 | } |