Rev 1986 | Rev 2019 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1986 | Rev 2018 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + Nur f�r den privaten Gebrauch |
3 | // + Nur für den privaten Gebrauch |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binürfiles, Sourcecode und Dokumentation), |
7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist. |
7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulüssig ist. |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausützen, |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder verüffentlicht, |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
17 | // + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
17 | // + auf anderen Webseiten oder Medien verüffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
20 | // + Keine Gewühr auf Fehlerfreiheit, Vollstündigkeit oder Funktion |
21 | // + Benutzung auf eigene Gefahr |
21 | // + Benutzung auf eigene Gefahr |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschüden |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
25 | // + mit unserer Zustimmung zul�ssig |
25 | // + mit unserer Zustimmung zulüssig |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
30 | // + this list of conditions and the following disclaimer. |
30 | // + this list of conditions and the following disclaimer. |
Line 75... | Line 75... | ||
75 | #define MK3MAG_ADDRESS 3 |
75 | #define MK3MAG_ADDRESS 3 |
Line 76... | Line 76... | ||
76 | 76 | ||
77 | #define FALSE 0 |
77 | #define FALSE 0 |
78 | #define TRUE 1 |
78 | #define TRUE 1 |
79 | //int8_t test __attribute__ ((section (".noinit"))); |
79 | //int8_t test __attribute__ ((section (".noinit"))); |
80 | uint8_t request_VerInfo = FALSE; |
80 | uint8_t request_verInfo = FALSE; |
81 | uint8_t request_ExternalControl = FALSE; |
81 | uint8_t request_externalControl = FALSE; |
82 | uint8_t request_Display = FALSE; |
82 | uint8_t request_display = FALSE; |
83 | uint8_t request_Display1 = FALSE; |
83 | uint8_t request_display1 = FALSE; |
84 | uint8_t request_DebugData = FALSE; |
84 | uint8_t request_debugData = FALSE; |
85 | uint8_t request_Data3D = FALSE; |
85 | uint8_t request_data3D = FALSE; |
86 | uint8_t request_DebugLabel = 255; |
86 | uint8_t request_debugLabel = 255; |
87 | uint8_t request_PPMChannels = FALSE; |
87 | uint8_t request_PPMChannels = FALSE; |
88 | uint8_t request_MotorTest = FALSE; |
88 | uint8_t request_motorTest = FALSE; |
Line 89... | Line 89... | ||
89 | uint8_t request_variables = FALSE; |
89 | uint8_t request_variables = FALSE; |
Line 90... | Line 90... | ||
90 | 90 | ||
91 | uint8_t DisplayLine = 0; |
91 | uint8_t displayLine = 0; |
92 | 92 | ||
93 | volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
93 | volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
94 | volatile uint8_t rxd_buffer_locked = FALSE; |
94 | volatile uint8_t rxd_buffer_locked = FALSE; |
95 | volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
95 | volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
96 | volatile uint8_t txd_complete = TRUE; |
96 | volatile uint8_t txd_complete = TRUE; |
Line 97... | Line 97... | ||
97 | volatile uint8_t ReceivedBytes = 0; |
97 | volatile uint8_t receivedBytes = 0; |
98 | volatile uint8_t *pRxData = 0; |
98 | volatile uint8_t *pRxData = 0; |
99 | volatile uint8_t RxDataLen = 0; |
99 | volatile uint8_t rxDataLen = 0; |
Line 100... | Line 100... | ||
100 | 100 | ||
101 | uint8_t motorTestActive = 0; |
101 | uint8_t motorTestActive = 0; |
102 | uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
102 | uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
Line 103... | Line 103... | ||
103 | uint8_t ConfirmFrame; |
103 | uint8_t confirmFrame; |
104 | 104 | ||
105 | typedef struct { |
- | |
Line 106... | Line 105... | ||
106 | int16_t Heading; |
105 | typedef struct { |
107 | }__attribute__((packed)) Heading_t; |
106 | int16_t Heading; |
108 | 107 | }__attribute__((packed)) Heading_t; |
|
109 | DebugOut_t debugOut; |
108 | |
Line 110... | Line 109... | ||
110 | Data3D_t Data3D; |
109 | DebugOut_t debugOut; |
111 | UART_VersionInfo_t UART_VersionInfo; |
110 | Data3D_t data3D; |
112 | 111 | ||
Line 113... | Line 112... | ||
113 | uint16_t DebugData_Timer; |
112 | uint16_t debugData_timer; |
114 | uint16_t Data3D_Timer; |
113 | uint16_t data3D_timer; |
115 | uint16_t DebugData_Interval = 0; // in 1ms |
114 | uint16_t debugData_interval = 0; // in 1ms |
Line 156... | Line 155... | ||
156 | "AirpressADC " }; |
155 | "AirpressADC " }; |
Line 157... | Line 156... | ||
157 | 156 | ||
158 | /****************************************************************/ |
157 | /****************************************************************/ |
159 | /* Initialization of the USART0 */ |
158 | /* Initialization of the USART0 */ |
160 | /****************************************************************/ |
159 | /****************************************************************/ |
161 | void usart0_Init(void) { |
160 | void usart0_init(void) { |
162 | uint8_t sreg = SREG; |
161 | uint8_t sreg = SREG; |
Line 163... | Line 162... | ||
163 | uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1); |
162 | uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1); |
164 | 163 | ||
Line 211... | Line 210... | ||
211 | UCSR0B |= (1 << RXCIE0); |
210 | UCSR0B |= (1 << RXCIE0); |
212 | // enable TX-Interrupt |
211 | // enable TX-Interrupt |
213 | UCSR0B |= (1 << TXCIE0); |
212 | UCSR0B |= (1 << TXCIE0); |
Line 214... | Line 213... | ||
214 | 213 | ||
215 | // initialize the debug timer |
214 | // initialize the debug timer |
Line 216... | Line 215... | ||
216 | DebugData_Timer = setDelay(DebugData_Interval); |
215 | debugData_timer = setDelay(debugData_interval); |
217 | 216 | ||
218 | // unlock rxd_buffer |
217 | // unlock rxd_buffer |
219 | rxd_buffer_locked = FALSE; |
218 | rxd_buffer_locked = FALSE; |
Line 220... | Line 219... | ||
220 | pRxData = 0; |
219 | pRxData = 0; |
221 | RxDataLen = 0; |
220 | rxDataLen = 0; |
Line 222... | Line 221... | ||
222 | 221 | ||
223 | // no bytes to send |
222 | // no bytes to send |
224 | txd_complete = TRUE; |
223 | txd_complete = TRUE; |
Line 225... | Line 224... | ||
225 | 224 | ||
226 | #ifdef USE_MK3MAG |
225 | #ifdef USE_MK3MAG |
227 | Compass_Timer = setDelay(220); |
226 | compass_timer = setDelay(220); |
228 | #endif |
227 | #endif |
229 | 228 | ||
Line 230... | Line 229... | ||
230 | UART_VersionInfo.SWMajor = VERSION_MAJOR; |
229 | versionInfo.SWMajor = VERSION_MAJOR; |
231 | UART_VersionInfo.SWMinor = VERSION_MINOR; |
230 | versionInfo.SWMinor = VERSION_MINOR; |
232 | UART_VersionInfo.SWPatch = VERSION_PATCH; |
231 | versionInfo.SWPatch = VERSION_PATCH; |
Line 233... | Line 232... | ||
233 | UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR; |
232 | versionInfo.protoMajor = VERSION_SERIAL_MAJOR; |
234 | UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR; |
233 | versionInfo.protoMinor = VERSION_SERIAL_MINOR; |
235 | 234 | ||
236 | // restore global interrupt flags |
235 | // restore global interrupt flags |
237 | SREG = sreg; |
- | |
238 | } |
236 | SREG = sreg; |
239 | 237 | } |
|
240 | /****************************************************************/ |
238 | |
241 | /* USART0 transmitter ISR */ |
239 | /****************************************************************/ |
242 | /****************************************************************/ |
240 | /* USART0 transmitter ISR */ |
Line 260... | Line 258... | ||
260 | } |
258 | } |
Line 261... | Line 259... | ||
261 | 259 | ||
262 | /****************************************************************/ |
260 | /****************************************************************/ |
263 | /* USART0 receiver ISR */ |
261 | /* USART0 receiver ISR */ |
264 | /****************************************************************/ |
262 | /****************************************************************/ |
265 | ISR(USART0_RX_vect) |
- | |
266 | { |
263 | ISR(USART0_RX_vect) { |
267 | static uint16_t checksum; |
264 | static uint16_t checksum; |
268 | static uint8_t ptr_rxd_buffer = 0; |
265 | static uint8_t ptr_rxd_buffer = 0; |
269 | uint8_t checksum1, checksum2; |
266 | uint8_t checksum1, checksum2; |
Line 301... | Line 298... | ||
301 | // compare checksum to transmitted checksum bytes |
298 | // compare checksum to transmitted checksum bytes |
302 | if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
299 | if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
303 | == rxd_buffer[ptr_rxd_buffer - 1])) { |
300 | == rxd_buffer[ptr_rxd_buffer - 1])) { |
304 | // checksum valid |
301 | // checksum valid |
305 | rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
302 | rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
306 | ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
303 | receivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
307 | rxd_buffer_locked = TRUE; // lock the rxd buffer |
304 | rxd_buffer_locked = TRUE; // lock the rxd buffer |
308 | // if 2nd byte is an 'R' enable watchdog that will result in an reset |
305 | // if 2nd byte is an 'R' enable watchdog that will result in an reset |
309 | if (rxd_buffer[2] == 'R') { |
306 | if (rxd_buffer[2] == 'R') { |
310 | wdt_enable(WDTO_250MS); |
307 | wdt_enable(WDTO_250MS); |
311 | } // Reset-Commando |
308 | } // Reset-Commando |
Line 334... | Line 331... | ||
334 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
331 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
335 | } |
332 | } |
Line 336... | Line 333... | ||
336 | 333 | ||
337 | // -------------------------------------------------------------------------- |
334 | // -------------------------------------------------------------------------- |
338 | // application example: |
335 | // application example: |
339 | // SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
336 | // sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
340 | /* |
337 | /* |
341 | void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
338 | void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
342 | va_list ap; |
339 | va_list ap; |
343 | uint16_t txd_bufferIndex = 0; |
340 | uint16_t txd_bufferIndex = 0; |
344 | uint8_t *currentBuffer; |
341 | uint8_t *currentBuffer; |
345 | uint8_t currentBufferIndex; |
342 | uint8_t currentBufferIndex; |
Line 383... | Line 380... | ||
383 | va_end(ap); |
380 | va_end(ap); |
384 | Addchecksum(pt); // add checksum after data block and initates the transmission |
381 | Addchecksum(pt); // add checksum after data block and initates the transmission |
385 | } |
382 | } |
386 | */ |
383 | */ |
Line 387... | Line 384... | ||
387 | 384 | ||
388 | void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
385 | void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
389 | va_list ap; |
386 | va_list ap; |
390 | uint16_t pt = 0; |
387 | uint16_t pt = 0; |
391 | uint8_t a, b, c; |
388 | uint8_t a, b, c; |
Line 454... | Line 451... | ||
454 | void Decode64(void) { |
451 | void Decode64(void) { |
455 | uint8_t a, b, c, d; |
452 | uint8_t a, b, c, d; |
456 | uint8_t x, y, z; |
453 | uint8_t x, y, z; |
457 | uint8_t ptrIn = 3; |
454 | uint8_t ptrIn = 3; |
458 | uint8_t ptrOut = 3; |
455 | uint8_t ptrOut = 3; |
459 | uint8_t len = ReceivedBytes - 6; |
456 | uint8_t len = receivedBytes - 6; |
Line 460... | Line 457... | ||
460 | 457 | ||
461 | while (len) { |
458 | while (len) { |
462 | a = rxd_buffer[ptrIn++] - '='; |
459 | a = rxd_buffer[ptrIn++] - '='; |
463 | b = rxd_buffer[ptrIn++] - '='; |
460 | b = rxd_buffer[ptrIn++] - '='; |
Line 481... | Line 478... | ||
481 | rxd_buffer[ptrOut++] = z; |
478 | rxd_buffer[ptrOut++] = z; |
482 | else |
479 | else |
483 | break; |
480 | break; |
484 | } |
481 | } |
485 | pRxData = &rxd_buffer[3]; |
482 | pRxData = &rxd_buffer[3]; |
486 | RxDataLen = ptrOut - 3; |
483 | rxDataLen = ptrOut - 3; |
487 | } |
484 | } |
Line 488... | Line 485... | ||
488 | 485 | ||
489 | // -------------------------------------------------------------------------- |
486 | // -------------------------------------------------------------------------- |
490 | void usart0_ProcessRxData(void) { |
487 | void usart0_processRxData(void) { |
491 | // We control the motorTestActive var from here: Count it down. |
488 | // We control the motorTestActive var from here: Count it down. |
492 | if (motorTestActive) |
489 | if (motorTestActive) |
493 | motorTestActive--; |
490 | motorTestActive--; |
494 | // if data in the rxd buffer are not locked immediately return |
491 | // if data in the rxd buffer are not locked immediately return |
Line 506... | Line 503... | ||
506 | compassHeading = ((Heading_t *)pRxData)->Heading; |
503 | compassHeading = ((Heading_t *)pRxData)->Heading; |
507 | // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
504 | // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
508 | break; |
505 | break; |
509 | #endif |
506 | #endif |
510 | case 't': // motor test |
507 | case 't': // motor test |
511 | if (RxDataLen > 20) { |
508 | if (rxDataLen > 20) { |
512 | memcpy(&motorTest[0], (uint8_t*) pRxData, sizeof(motorTest)); |
509 | memcpy(&motorTest[0], (uint8_t*) pRxData, sizeof(motorTest)); |
513 | } else { |
510 | } else { |
514 | memcpy(&motorTest[0], (uint8_t*) pRxData, 4); |
511 | memcpy(&motorTest[0], (uint8_t*) pRxData, 4); |
515 | } |
512 | } |
516 | motorTestActive = 255; |
513 | motorTestActive = 255; |
Line 518... | Line 515... | ||
518 | break; |
515 | break; |
Line 519... | Line 516... | ||
519 | 516 | ||
520 | case 'n':// "Get Mixer Table |
517 | case 'n':// "Get Mixer Table |
521 | while (!txd_complete) |
518 | while (!txd_complete) |
522 | ; // wait for previous frame to be sent |
519 | ; // wait for previous frame to be sent |
523 | SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &mixerMatrix, sizeof(mixerMatrix)); |
520 | sendOutData('N', FC_ADDRESS, 1, (uint8_t *) &mixerMatrix, sizeof(mixerMatrix)); |
Line 524... | Line 521... | ||
524 | break; |
521 | break; |
525 | 522 | ||
526 | case 'm':// "Set Mixer Table |
523 | case 'm':// "Set Mixer Table |
Line 531... | Line 528... | ||
531 | ; // wait for previous frame to be sent |
528 | ; // wait for previous frame to be sent |
532 | tempchar[0] = 1; |
529 | tempchar[0] = 1; |
533 | } else { |
530 | } else { |
534 | tempchar[0] = 0; |
531 | tempchar[0] = 0; |
535 | } |
532 | } |
536 | SendOutData('M', FC_ADDRESS, 1, &tempchar, 1); |
533 | sendOutData('M', FC_ADDRESS, 1, &tempchar, 1); |
537 | break; |
534 | break; |
Line 538... | Line 535... | ||
538 | 535 | ||
539 | case 'p': // get PPM channels |
536 | case 'p': // get PPM channels |
540 | request_PPMChannels = TRUE; |
537 | request_PPMChannels = TRUE; |
Line 554... | Line 551... | ||
554 | tempchar[0] = pRxData[0]; |
551 | tempchar[0] = pRxData[0]; |
555 | tempchar[1] = EEPARAM_REVISION; |
552 | tempchar[1] = EEPARAM_REVISION; |
556 | tempchar[2] = sizeof(staticParams); |
553 | tempchar[2] = sizeof(staticParams); |
557 | while (!txd_complete) |
554 | while (!txd_complete) |
558 | ; // wait for previous frame to be sent |
555 | ; // wait for previous frame to be sent |
559 | SendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams)); |
556 | sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams)); |
560 | break; |
557 | break; |
Line 561... | Line 558... | ||
561 | 558 | ||
562 | case 's': // save settings |
559 | case 's': // save settings |
563 | if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
560 | if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
Line 572... | Line 569... | ||
572 | } else { |
569 | } else { |
573 | tempchar[0] = 0; //indicate bad data |
570 | tempchar[0] = 0; //indicate bad data |
574 | } |
571 | } |
575 | while (!txd_complete) |
572 | while (!txd_complete) |
576 | ; // wait for previous frame to be sent |
573 | ; // wait for previous frame to be sent |
577 | SendOutData('S', FC_ADDRESS, 1, &tempchar, 1); |
574 | sendOutData('S', FC_ADDRESS, 1, &tempchar, 1); |
578 | } |
575 | } |
579 | break; |
576 | break; |
Line 580... | Line 577... | ||
580 | 577 | ||
581 | default: |
578 | default: |
Line 584... | Line 581... | ||
584 | } // case FC_ADDRESS: |
581 | } // case FC_ADDRESS: |
Line 585... | Line 582... | ||
585 | 582 | ||
586 | default: // any Slave Address |
583 | default: // any Slave Address |
587 | switch (rxd_buffer[2]) { |
584 | switch (rxd_buffer[2]) { |
588 | case 'a':// request for labels of the analog debug outputs |
585 | case 'a':// request for labels of the analog debug outputs |
589 | request_DebugLabel = pRxData[0]; |
586 | request_debugLabel = pRxData[0]; |
590 | if (request_DebugLabel > 31) |
587 | if (request_debugLabel > 31) |
591 | request_DebugLabel = 31; |
588 | request_debugLabel = 31; |
Line 592... | Line 589... | ||
592 | break; |
589 | break; |
593 | 590 | ||
594 | case 'b': // submit extern control |
591 | case 'b': // submit extern control |
595 | memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl)); |
592 | memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl)); |
596 | ConfirmFrame = externalControl.frame; |
593 | confirmFrame = externalControl.frame; |
Line 597... | Line 594... | ||
597 | externalControlActive = 255; |
594 | externalControlActive = 255; |
598 | break; |
595 | break; |
599 | 596 | ||
600 | case 'h':// request for display columns |
597 | case 'h':// request for display columns |
601 | RemoteKeys |= pRxData[0]; |
598 | remoteKeys |= pRxData[0]; |
602 | if (RemoteKeys) |
599 | if (remoteKeys) |
Line 603... | Line 600... | ||
603 | DisplayLine = 0; |
600 | displayLine = 0; |
604 | request_Display = TRUE; |
601 | request_display = TRUE; |
605 | break; |
602 | break; |
606 | 603 | ||
Line 607... | Line 604... | ||
607 | case 'l':// request for display columns |
604 | case 'l':// request for display columns |
608 | MenuItem = pRxData[0]; |
605 | menuItem = pRxData[0]; |
609 | request_Display1 = TRUE; |
606 | request_display1 = TRUE; |
Line 610... | Line 607... | ||
610 | break; |
607 | break; |
611 | 608 | ||
612 | case 'v': // request for version and board release |
609 | case 'v': // request for version and board release |
Line 613... | Line 610... | ||
613 | request_VerInfo = TRUE; |
610 | request_verInfo = TRUE; |
614 | break; |
611 | break; |
615 | 612 | ||
Line 616... | Line 613... | ||
616 | case 'x': |
613 | case 'x': |
617 | request_variables = TRUE; |
614 | request_variables = TRUE; |
618 | break; |
615 | break; |
619 | 616 | ||
620 | case 'g':// get external control data |
617 | case 'g':// get external control data |
Line 621... | Line 618... | ||
621 | request_ExternalControl = TRUE; |
618 | request_externalControl = TRUE; |
622 | break; |
619 | break; |
623 | 620 | ||
624 | case 'd': // request for the debug data |
621 | case 'd': // request for the debug data |
625 | DebugData_Interval = (uint16_t) pRxData[0] * 10; |
622 | debugData_interval = (uint16_t) pRxData[0] * 10; |
Line 626... | Line 623... | ||
626 | if (DebugData_Interval > 0) |
623 | if (debugData_interval > 0) |
627 | request_DebugData = TRUE; |
624 | request_debugData = TRUE; |
628 | break; |
625 | break; |
629 | 626 | ||
630 | case 'c': // request for the 3D data |
627 | case 'c': // request for the 3D data |
631 | Data3D_Interval = (uint16_t) pRxData[0] * 10; |
628 | data3D_interval = (uint16_t) pRxData[0] * 10; |
632 | if (Data3D_Interval > 0) |
629 | if (data3D_interval > 0) |
633 | request_Data3D = TRUE; |
630 | request_data3D = TRUE; |
634 | break; |
631 | break; |
635 | 632 | ||
636 | default: |
633 | default: |
Line 637... | Line 634... | ||
637 | //unsupported command received |
634 | //unsupported command received |
638 | break; |
635 | break; |
639 | } |
636 | } |
640 | break; // default: |
637 | break; // default: |
641 | } |
638 | } |
642 | // unlock the rxd buffer after processing |
639 | // unlock the rxd buffer after processing |
643 | pRxData = 0; |
640 | pRxData = 0; |
Line 657... | Line 654... | ||
657 | UDR0 = c; |
654 | UDR0 = c; |
658 | return (0); |
655 | return (0); |
659 | } |
656 | } |
Line 660... | Line 657... | ||
660 | 657 | ||
661 | //--------------------------------------------------------------------------------------------- |
658 | //--------------------------------------------------------------------------------------------- |
662 | void usart0_TransmitTxData(void) { |
659 | void usart0_transmitTxData(void) { |
663 | if (!txd_complete) |
660 | if (!txd_complete) |
Line 664... | Line 661... | ||
664 | return; |
661 | return; |
665 | 662 | ||
666 | if (request_VerInfo && txd_complete) { |
- | |
667 | SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, |
663 | if (request_verInfo && txd_complete) { |
668 | sizeof(UART_VersionInfo)); |
- | |
669 | request_VerInfo = FALSE; |
- | |
670 | } |
- | |
671 | - | ||
672 | if (request_Display && txd_complete) { |
- | |
673 | LCD_PrintMenu(); |
- | |
674 | SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), |
- | |
675 | &DisplayBuff[DisplayLine * 20], 20); |
- | |
676 | DisplayLine++; |
- | |
677 | if (DisplayLine >= 4) |
- | |
678 | DisplayLine = 0; |
- | |
679 | request_Display = FALSE; |
- | |
680 | } |
- | |
681 | - | ||
682 | if (request_Display1 && txd_complete) { |
- | |
683 | LCD_PrintMenu(); |
- | |
684 | SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, |
- | |
685 | sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
664 | sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo)); |
Line -... | Line 665... | ||
- | 665 | request_verInfo = FALSE; |
|
- | 666 | } |
|
- | 667 | ||
- | 668 | if (request_display && txd_complete) { |
|
- | 669 | LCD_printMenu(); |
|
- | 670 | sendOutData('H', FC_ADDRESS, 2, &displayLine, sizeof(displayLine), |
|
- | 671 | &displayBuff[displayLine * 20], 20); |
|
- | 672 | displayLine++; |
|
- | 673 | if (displayLine >= 4) |
|
- | 674 | displayLine = 0; |
|
- | 675 | request_display = FALSE; |
|
- | 676 | } |
|
- | 677 | ||
- | 678 | if (request_display1 && txd_complete) { |
|
- | 679 | LCD_printMenu(); |
|
- | 680 | sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem, |
|
- | 681 | sizeof(maxMenuItem), displayBuff, sizeof(displayBuff)); |
|
686 | request_Display1 = FALSE; |
682 | request_display1 = FALSE; |
687 | } |
683 | } |
688 | 684 | ||
689 | if (request_DebugLabel != 0xFF) { // Texte f�r die Analogdaten |
685 | if (request_debugLabel != 0xFF) { // Texte für die Analogdaten |
690 | uint8_t label[16]; // local sram buffer |
686 | uint8_t label[16]; // local sram buffer |
691 | memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer |
687 | memcpy_P(label, ANALOG_LABEL[request_debugLabel], 16); // read lable from flash to sram buffer |
692 | SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, |
688 | sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_debugLabel, |
Line 693... | Line 689... | ||
693 | sizeof(request_DebugLabel), label, 16); |
689 | sizeof(request_debugLabel), label, 16); |
694 | request_DebugLabel = 0xFF; |
690 | request_debugLabel = 0xFF; |
695 | } |
691 | } |
696 | 692 | ||
Line 697... | Line 693... | ||
697 | if (ConfirmFrame && txd_complete) { // Datensatz ohne checksum bestätigen |
693 | if (confirmFrame && txd_complete) { // Datensatz ohne checksum bestätigen |
698 | SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, sizeof(ConfirmFrame)); |
694 | sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame)); |
699 | ConfirmFrame = 0; |
695 | confirmFrame = 0; |
700 | } |
696 | } |
701 | 697 | ||
702 | if (((DebugData_Interval && checkDelay(DebugData_Timer)) || request_DebugData) |
698 | if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData) |
Line 703... | Line 699... | ||
703 | && txd_complete) { |
699 | && txd_complete) { |
704 | SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut)); |
700 | sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut)); |
705 | DebugData_Timer = setDelay(DebugData_Interval); |
701 | debugData_timer = setDelay(debugData_interval); |
706 | request_DebugData = FALSE; |
702 | request_debugData = FALSE; |
707 | } |
703 | } |
708 | 704 | ||
709 | if (((Data3D_Interval && checkDelay(Data3D_Timer)) || request_Data3D) |
705 | if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) |
710 | && txd_complete) { |
706 | && txd_complete) { |
711 | SendOutData('C', FC_ADDRESS, 1, (uint8_t *) &Data3D, sizeof(Data3D)); |
707 | sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
712 | Data3D.AngleNick = (int16_t) ((10 * angle[PITCH]) |
708 | data3D.anglePitch = (int16_t) ((10 * angle[PITCH]) |
713 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
709 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Line 714... | Line 710... | ||
714 | Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL]) |
710 | data3D.angleRoll = (int16_t) ((10 * angle[ROLL]) |
715 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
711 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
716 | Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
712 | data3D.heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
717 | Data3D_Timer = setDelay(Data3D_Interval); |
713 | data3D_timer = setDelay(data3D_interval); |
718 | request_Data3D = FALSE; |
714 | request_data3D = FALSE; |
Line 719... | Line 715... | ||
719 | } |
715 | } |
720 | 716 | ||
721 | if (request_ExternalControl && txd_complete) { |
717 | if (request_externalControl && txd_complete) { |
722 | SendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl, |
718 | sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl, |
723 | sizeof(externalControl)); |
719 | sizeof(externalControl)); |
724 | request_ExternalControl = FALSE; |
720 | request_externalControl = FALSE; |
725 | } |
721 | } |
726 | 722 | ||
727 | #ifdef USE_MK3MAG |
723 | #ifdef USE_MK3MAG |
728 | if((checkDelay(Compass_Timer)) && txd_complete) { |
724 | if((checkDelay(Compass_Timer)) && txd_complete) { |
729 | toMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
725 | toMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
730 | toMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
726 | toMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
731 | toMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
727 | toMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
Line 732... | Line 728... | ||
732 | toMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
728 | toMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
733 | toMk3Mag.CalState = compassCalState; |
729 | toMk3Mag.CalState = compassCalState; |
734 | SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |
730 | sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |
735 | // the last state is 5 and should be send only once to avoid multiple flash writing |
731 | // the last state is 5 and should be send only once to avoid multiple flash writing |
Line 736... | Line 732... | ||
736 | if(compassCalState > 4) compassCalState = 0; |
732 | if(compassCalState > 4) compassCalState = 0; |
737 | Compass_Timer = setDelay(99); |
733 | compass_timer = setDelay(99); |
738 | } |
734 | } |
739 | #endif |
735 | #endif |
Line 740... | Line 736... | ||
740 | 736 | ||
741 | if (request_MotorTest && txd_complete) { |
737 | if (request_motorTest && txd_complete) { |
742 | SendOutData('T', FC_ADDRESS, 0); |
738 | sendOutData('T', FC_ADDRESS, 0); |
743 | request_MotorTest = FALSE; |
739 | request_motorTest = FALSE; |
744 | } |
740 | } |