Rev 1796 | Rev 1864 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1796 | Rev 1821 | ||
|---|---|---|---|
| Line 136... | Line 136... | ||
| 136 | "UBat ", |
136 | "UBat ", |
| 137 | "Pitch Term ", |
137 | "Pitch Term ", |
| 138 | "Roll Term ", |
138 | "Roll Term ", |
| 139 | "Yaw Term ", |
139 | "Yaw Term ", |
| 140 | "Throttle Term ", //15 |
140 | "Throttle Term ", //15 |
| 141 | "0th O Corr pitch", |
- | |
| 142 | "0th O Corr roll ", |
141 | "0th O Corr pitch", "0th O Corr roll ", |
| 143 | "DriftCompDelta P", |
142 | "DriftCompDelta P", |
| 144 | "DriftCompDelta R", |
143 | "DriftCompDelta R", |
| 145 | "ADPitchGyroOffs ", //20 |
144 | "ADPitchGyroOffs ", //20 |
| 146 | "ADRollGyroOffs ", |
- | |
| 147 | "M1 ", |
- | |
| 148 | "M2 ", |
145 | "ADRollGyroOffs ", "M1 ", "M2 ", |
| 149 | "M3 ", |
146 | "M3 ", |
| 150 | "M4 ", //25 |
147 | "M4 ", //25 |
| 151 | "ControlYaw ", |
- | |
| 152 | "Airpress. Range ", |
- | |
| 153 | "DriftCompPitch ", |
148 | "ControlYaw ", "Airpress. Range ", "DriftCompPitch ", |
| 154 | "DriftCompRoll ", |
- | |
| 155 | "AirpressFiltered", //30 |
149 | "DriftCompRoll ", "AirpressFiltered", //30 |
| 156 | "AirpressADC " |
150 | "AirpressADC " }; |
| 157 | }; |
- | |
| Line 158... | Line 151... | ||
| 158 | 151 | ||
| 159 | /****************************************************************/ |
152 | /****************************************************************/ |
| 160 | /* Initialization of the USART0 */ |
153 | /* Initialization of the USART0 */ |
| 161 | /****************************************************************/ |
154 | /****************************************************************/ |
| Line 202... | Line 195... | ||
| 202 | UCSR0B &= ~(1 << UCSZ02); |
195 | UCSR0B &= ~(1 << UCSZ02); |
| 203 | UCSR0C |= (1 << UCSZ01); |
196 | UCSR0C |= (1 << UCSZ01); |
| 204 | UCSR0C |= (1 << UCSZ00); |
197 | UCSR0C |= (1 << UCSZ00); |
| Line 205... | Line 198... | ||
| 205 | 198 | ||
| 206 | // flush receive buffer |
199 | // flush receive buffer |
| - | 200 | while (UCSR0A & (1 << RXC0)) |
|
| Line 207... | Line 201... | ||
| 207 | while ( UCSR0A & (1<<RXC0) ) UDR0; |
201 | UDR0; |
| 208 | 202 | ||
| 209 | // enable interrupts at the end |
203 | // enable interrupts at the end |
| 210 | // enable RX-Interrupt |
204 | // enable RX-Interrupt |
| Line 238... | Line 232... | ||
| 238 | } |
232 | } |
| Line 239... | Line 233... | ||
| 239 | 233 | ||
| 240 | /****************************************************************/ |
234 | /****************************************************************/ |
| 241 | /* USART0 transmitter ISR */ |
235 | /* USART0 transmitter ISR */ |
| 242 | /****************************************************************/ |
236 | /****************************************************************/ |
| - | 237 | ISR(USART0_TX_vect) |
|
| 243 | ISR(USART0_TX_vect) { |
238 | { |
| 244 | static uint16_t ptr_txd_buffer = 0; |
239 | static uint16_t ptr_txd_buffer = 0; |
| 245 | uint8_t tmp_tx; |
240 | uint8_t tmp_tx; |
| 246 | if(!txd_complete) { // transmission not completed |
241 | if (!txd_complete) { // transmission not completed |
| 247 | ptr_txd_buffer++; // die [0] wurde schon gesendet |
242 | ptr_txd_buffer++; // die [0] wurde schon gesendet |
| Line 252... | Line 247... | ||
| 252 | txd_complete = 1; // stop transmission |
247 | txd_complete = 1; // stop transmission |
| 253 | } |
248 | } |
| 254 | UDR0 = tmp_tx; // send current byte will trigger this ISR again |
249 | UDR0 = tmp_tx; // send current byte will trigger this ISR again |
| 255 | } |
250 | } |
| 256 | // transmission completed |
251 | // transmission completed |
| - | 252 | else |
|
| 257 | else ptr_txd_buffer = 0; |
253 | ptr_txd_buffer = 0; |
| 258 | } |
254 | } |
| Line 259... | Line 255... | ||
| 259 | 255 | ||
| 260 | /****************************************************************/ |
256 | /****************************************************************/ |
| 261 | /* USART0 receiver ISR */ |
257 | /* USART0 receiver ISR */ |
| 262 | /****************************************************************/ |
258 | /****************************************************************/ |
| - | 259 | ISR(USART0_RX_vect) |
|
| 263 | ISR(USART0_RX_vect) { |
260 | { |
| 264 | static uint16_t crc; |
261 | static uint16_t crc; |
| 265 | static uint8_t ptr_rxd_buffer = 0; |
262 | static uint8_t ptr_rxd_buffer = 0; |
| 266 | uint8_t crc1, crc2; |
263 | uint8_t crc1, crc2; |
| Line 267... | Line 264... | ||
| 267 | uint8_t c; |
264 | uint8_t c; |
| Line -... | Line 265... | ||
| - | 265 | ||
| 268 | 266 | c = UDR0; // catch the received byte |
|
| Line 269... | Line 267... | ||
| 269 | c = UDR0; // catch the received byte |
267 | |
| 270 | 268 | if (rxd_buffer_locked) |
|
| 271 | if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return |
269 | return; // if rxd buffer is locked immediately return |
| 272 | 270 | ||
| Line 293... | Line 291... | ||
| 293 | // calculate checksum from transmitted data |
291 | // calculate checksum from transmitted data |
| 294 | crc %= 4096; |
292 | crc %= 4096; |
| 295 | crc1 = '=' + crc / 64; |
293 | crc1 = '=' + crc / 64; |
| 296 | crc2 = '=' + crc % 64; |
294 | crc2 = '=' + crc % 64; |
| 297 | // compare checksum to transmitted checksum bytes |
295 | // compare checksum to transmitted checksum bytes |
| 298 | if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) { |
296 | if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2 |
| - | 297 | == rxd_buffer[ptr_rxd_buffer - 1])) { |
|
| 299 | // checksum valid |
298 | // checksum valid |
| 300 | rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
299 | rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
| 301 | ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
300 | ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
| 302 | rxd_buffer_locked = TRUE; // lock the rxd buffer |
301 | rxd_buffer_locked = TRUE; // lock the rxd buffer |
| 303 | // if 2nd byte is an 'R' enable watchdog that will result in an reset |
302 | // if 2nd byte is an 'R' enable watchdog that will result in an reset |
| 304 | if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
303 | if (rxd_buffer[2] == 'R') { |
| - | 304 | wdt_enable(WDTO_250MS); |
|
| - | 305 | } // Reset-Commando |
|
| 305 | } else { // checksum invalid |
306 | } else { // checksum invalid |
| 306 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
307 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
| 307 | } |
308 | } |
| 308 | ptr_rxd_buffer = 0; // reset rxd buffer pointer |
309 | ptr_rxd_buffer = 0; // reset rxd buffer pointer |
| 309 | } |
310 | } |
| Line 408... | Line 409... | ||
| 408 | pdata = va_arg(ap, uint8_t*); |
409 | pdata = va_arg(ap, uint8_t*); |
| 409 | len = va_arg(ap, int); |
410 | len = va_arg(ap, int); |
| 410 | ptr = 0; |
411 | ptr = 0; |
| 411 | numofbuffers--; |
412 | numofbuffers--; |
| 412 | } |
413 | } |
| 413 | } |
414 | } else |
| 414 | else a = 0; |
415 | a = 0; |
| 415 | if(len) { |
416 | if (len) { |
| 416 | b = pdata[ptr++]; |
417 | b = pdata[ptr++]; |
| 417 | len--; |
418 | len--; |
| 418 | if((!len) && numofbuffers) { |
419 | if ((!len) && numofbuffers) { |
| 419 | pdata = va_arg(ap, uint8_t*); |
420 | pdata = va_arg(ap, uint8_t*); |
| 420 | len = va_arg(ap, int); |
421 | len = va_arg(ap, int); |
| 421 | ptr = 0; |
422 | ptr = 0; |
| 422 | numofbuffers--; |
423 | numofbuffers--; |
| 423 | } |
424 | } |
| - | 425 | } else |
|
| 424 | } else b = 0; |
426 | b = 0; |
| 425 | if(len) { |
427 | if (len) { |
| 426 | c = pdata[ptr++]; |
428 | c = pdata[ptr++]; |
| 427 | len--; |
429 | len--; |
| 428 | if((!len) && numofbuffers) { |
430 | if ((!len) && numofbuffers) { |
| 429 | pdata = va_arg(ap, uint8_t*); |
431 | pdata = va_arg(ap, uint8_t*); |
| 430 | len = va_arg(ap, int); |
432 | len = va_arg(ap, int); |
| 431 | ptr = 0; |
433 | ptr = 0; |
| 432 | numofbuffers--; |
434 | numofbuffers--; |
| 433 | } |
435 | } |
| 434 | } |
436 | } else |
| 435 | else c = 0; |
437 | c = 0; |
| 436 | txd_buffer[pt++] = '=' + (a >> 2); |
438 | txd_buffer[pt++] = '=' + (a >> 2); |
| 437 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
439 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
| 438 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
440 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
| 439 | txd_buffer[pt++] = '=' + ( c & 0x3f); |
441 | txd_buffer[pt++] = '=' + (c & 0x3f); |
| 440 | } |
442 | } |
| Line 459... | Line 461... | ||
| 459 | 461 | ||
| 460 | x = (a << 2) | (b >> 4); |
462 | x = (a << 2) | (b >> 4); |
| 461 | y = ((b & 0x0f) << 4) | (c >> 2); |
463 | y = ((b & 0x0f) << 4) | (c >> 2); |
| Line -... | Line 464... | ||
| - | 464 | z = ((c & 0x03) << 6) | d; |
|
| 462 | z = ((c & 0x03) << 6) | d; |
465 | |
| - | 466 | if (len--) |
|
| - | 467 | rxd_buffer[ptrOut++] = x; |
|
| - | 468 | else |
|
| 463 | 469 | break; |
|
| - | 470 | if (len--) |
|
| - | 471 | rxd_buffer[ptrOut++] = y; |
|
| - | 472 | else |
|
| 464 | if(len--) rxd_buffer[ptrOut++] = x; else break; |
473 | break; |
| - | 474 | if (len--) |
|
| - | 475 | rxd_buffer[ptrOut++] = z; |
|
| 465 | if(len--) rxd_buffer[ptrOut++] = y; else break; |
476 | else |
| 466 | if(len--) rxd_buffer[ptrOut++] = z; else break; |
477 | break; |
| 467 | } |
478 | } |
| 468 | pRxData = &rxd_buffer[3]; |
479 | pRxData = &rxd_buffer[3]; |
| Line 469... | Line 480... | ||
| 469 | RxDataLen = ptrOut - 3; |
480 | RxDataLen = ptrOut - 3; |
| 470 | } |
481 | } |
| 471 | 482 | ||
| 472 | // -------------------------------------------------------------------------- |
483 | // -------------------------------------------------------------------------- |
| - | 484 | void usart0_ProcessRxData(void) { |
|
| 473 | void usart0_ProcessRxData(void) { |
485 | // We control the motorTestActive var from here: Count it down. |
| 474 | // We control the motorTestActive var from here: Count it down. |
486 | if (motorTestActive) |
| - | 487 | motorTestActive--; |
|
| 475 | if (motorTestActive) motorTestActive--; |
488 | // if data in the rxd buffer are not locked immediately return |
| 476 | // if data in the rxd buffer are not locked immediately return |
489 | if (!rxd_buffer_locked) |
| Line 477... | Line 490... | ||
| 477 | if(!rxd_buffer_locked) return; |
490 | return; |
| Line 478... | Line 491... | ||
| 478 | uint8_t tempchar1, tempchar2; |
491 | uint8_t tempchar1, tempchar2; |
| 479 | Decode64(); // decode data block in rxd_buffer |
492 | Decode64(); // decode data block in rxd_buffer |
| 480 | 493 | ||
| 481 | switch(rxd_buffer[1] - 'a') { |
494 | switch (rxd_buffer[1] - 'a') { |
| 482 | 495 | ||
| 483 | case FC_ADDRESS: |
496 | case FC_ADDRESS: |
| 484 | switch(rxd_buffer[2]) { |
497 | switch (rxd_buffer[2]) { |
| 485 | #ifdef USE_MK3MAG |
498 | #ifdef USE_MK3MAG |
| 486 | case 'K':// compass value |
499 | case 'K':// compass value |
| 487 | compassHeading = ((Heading_t *)pRxData)->Heading; |
500 | compassHeading = ((Heading_t *)pRxData)->Heading; |
| 488 | compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
501 | // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| Line 497... | Line 510... | ||
| 497 | motorTestActive = 255; |
510 | motorTestActive = 255; |
| 498 | externalControlActive = 255; |
511 | externalControlActive = 255; |
| 499 | break; |
512 | break; |
| Line 500... | Line 513... | ||
| 500 | 513 | ||
| - | 514 | case 'n':// "Get Mixer Table |
|
| 501 | case 'n':// "Get Mixer Table |
515 | while (!txd_complete) |
| 502 | while(!txd_complete); // wait for previous frame to be sent |
516 | ; // wait for previous frame to be sent |
| 503 | SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer)); |
517 | SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer)); |
| Line 504... | Line 518... | ||
| 504 | break; |
518 | break; |
| 505 | 519 | ||
| 506 | case 'm':// "Set Mixer Table |
520 | case 'm':// "Set Mixer Table |
| 507 | if(pRxData[0] == EEMIXER_REVISION) { |
521 | if (pRxData[0] == EEMIXER_REVISION) { |
| - | 522 | memcpy(&Mixer, (uint8_t*) pRxData, sizeof(Mixer)); |
|
| 508 | memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer)); |
523 | MixerTable_WriteToEEProm(); |
| 509 | MixerTable_WriteToEEProm(); |
524 | while (!txd_complete) |
| 510 | while(!txd_complete); // wait for previous frame to be sent |
525 | ; // wait for previous frame to be sent |
| 511 | tempchar1 = 1; |
526 | tempchar1 = 1; |
| 512 | } else { |
527 | } else { |
| 513 | tempchar1 = 0; |
528 | tempchar1 = 0; |
| Line 522... | Line 537... | ||
| 522 | case 'q':// request settings |
537 | case 'q':// request settings |
| 523 | if(pRxData[0] == 0xFF) { |
538 | if (pRxData[0] == 0xFF) { |
| 524 | pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
539 | pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
| 525 | } |
540 | } |
| 526 | // limit settings range |
541 | // limit settings range |
| - | 542 | if (pRxData[0] < 1) |
|
| 527 | if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1 |
543 | pRxData[0] = 1; // limit to 1 |
| - | 544 | else if (pRxData[0] > 5) |
|
| 528 | else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5 |
545 | pRxData[0] = 5; // limit to 5 |
| 529 | // load requested parameter set |
546 | // load requested parameter set |
| 530 | ParamSet_ReadFromEEProm(pRxData[0]); |
547 | ParamSet_ReadFromEEProm(pRxData[0]); |
| 531 | tempchar1 = pRxData[0]; |
548 | tempchar1 = pRxData[0]; |
| 532 | tempchar2 = EEPARAM_REVISION; |
549 | tempchar2 = EEPARAM_REVISION; |
| - | 550 | while (!txd_complete) |
|
| 533 | while(!txd_complete); // wait for previous frame to be sent |
551 | ; // wait for previous frame to be sent |
| 534 | SendOutData('Q', FC_ADDRESS,3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (uint8_t *) &staticParams, sizeof(staticParams)); |
552 | SendOutData('Q', FC_ADDRESS, 3, &tempchar1, sizeof(tempchar1), |
| - | 553 | &tempchar2, sizeof(tempchar2), (uint8_t *) &staticParams, |
|
| - | 554 | sizeof(staticParams)); |
|
| 535 | break; |
555 | break; |
| Line 536... | Line 556... | ||
| 536 | 556 | ||
| 537 | case 's': // save settings |
557 | case 's': // save settings |
| 538 | if(!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors ar off |
558 | if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
| - | 559 | { |
|
| 539 | { |
560 | if ((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] |
| 540 | if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings |
561 | == EEPARAM_REVISION)) // check for setting to be in range and version of settings |
| 541 | { |
562 | { |
| 542 | memcpy(&staticParams, (uint8_t*)&pRxData[2], sizeof(staticParams)); |
563 | memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams)); |
| 543 | ParamSet_WriteToEEProm(pRxData[0]); |
564 | ParamSet_WriteToEEProm(pRxData[0]); |
| 544 | /* |
565 | /* |
| 545 | TODO: Remove this encapsulation breach |
566 | TODO: Remove this encapsulation breach |
| 546 | turnOver180Pitch = (int32_t) staticParams.AngleTurnOverPitch * 2500L; |
567 | turnOver180Pitch = (int32_t) staticParams.AngleTurnOverPitch * 2500L; |
| 547 | turnOver180Roll = (int32_t) staticParams.AngleTurnOverRoll * 2500L; |
568 | turnOver180Roll = (int32_t) staticParams.AngleTurnOverRoll * 2500L; |
| 548 | */ |
569 | */ |
| 549 | tempchar1 = getActiveParamSet(); |
570 | tempchar1 = getActiveParamSet(); |
| 550 | beepNumber(tempchar1); |
- | |
| 551 | } |
571 | beepNumber(tempchar1); |
| 552 | else |
- | |
| 553 | { |
572 | } else { |
| 554 | tempchar1 = 0; //indicate bad data |
573 | tempchar1 = 0; //indicate bad data |
| - | 574 | } |
|
| 555 | } |
575 | while (!txd_complete) |
| 556 | while(!txd_complete); // wait for previous frame to be sent |
576 | ; // wait for previous frame to be sent |
| 557 | SendOutData('S', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1)); |
577 | SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1)); |
| 558 | } |
578 | } |
| Line 559... | Line 579... | ||
| 559 | break; |
579 | break; |
| Line 565... | Line 585... | ||
| 565 | 585 | ||
| 566 | default: // any Slave Address |
586 | default: // any Slave Address |
| 567 | switch(rxd_buffer[2]) { |
587 | switch (rxd_buffer[2]) { |
| 568 | case 'a':// request for labels of the analog debug outputs |
588 | case 'a':// request for labels of the analog debug outputs |
| 569 | request_DebugLabel = pRxData[0]; |
589 | request_DebugLabel = pRxData[0]; |
| - | 590 | if (request_DebugLabel > 31) |
|
| 570 | if(request_DebugLabel > 31) request_DebugLabel = 31; |
591 | request_DebugLabel = 31; |
| 571 | externalControlActive = 255; |
592 | externalControlActive = 255; |
| Line 572... | Line 593... | ||
| 572 | break; |
593 | break; |
| 573 | 594 | ||
| Line 578... | Line 599... | ||
| 578 | break; |
599 | break; |
| Line 579... | Line 600... | ||
| 579 | 600 | ||
| 580 | case 'h':// request for display columns |
601 | case 'h':// request for display columns |
| 581 | externalControlActive = 255; |
602 | externalControlActive = 255; |
| - | 603 | RemoteKeys |= pRxData[0]; |
|
| 582 | RemoteKeys |= pRxData[0]; |
604 | if (RemoteKeys) |
| 583 | if(RemoteKeys) DisplayLine = 0; |
605 | DisplayLine = 0; |
| 584 | request_Display = TRUE; |
606 | request_Display = TRUE; |
| Line 585... | Line 607... | ||
| 585 | break; |
607 | break; |
| 586 | 608 | ||
| Line 602... | Line 624... | ||
| 602 | request_ExternalControl = TRUE; |
624 | request_ExternalControl = TRUE; |
| 603 | break; |
625 | break; |
| Line 604... | Line 626... | ||
| 604 | 626 | ||
| 605 | case 'd': // request for the debug data |
627 | case 'd': // request for the debug data |
| - | 628 | DebugData_Interval = (uint16_t) pRxData[0] * 10; |
|
| 606 | DebugData_Interval = (uint16_t) pRxData[0] * 10; |
629 | if (DebugData_Interval > 0) |
| 607 | if(DebugData_Interval > 0) request_DebugData = TRUE; |
630 | request_DebugData = TRUE; |
| Line 608... | Line 631... | ||
| 608 | break; |
631 | break; |
| 609 | 632 | ||
| - | 633 | case 'c': // request for the 3D data |
|
| 610 | case 'c': // request for the 3D data |
634 | Data3D_Interval = (uint16_t) pRxData[0] * 10; |
| 611 | Data3D_Interval = (uint16_t) pRxData[0] * 10; |
635 | if (Data3D_Interval > 0) |
| Line 612... | Line 636... | ||
| 612 | if(Data3D_Interval > 0) request_Data3D = TRUE; |
636 | request_Data3D = TRUE; |
| 613 | break; |
637 | break; |
| 614 | 638 | ||
| Line 637... | Line 661... | ||
| 637 | return (0); |
661 | return (0); |
| 638 | } |
662 | } |
| Line 639... | Line 663... | ||
| 639 | 663 | ||
| 640 | //--------------------------------------------------------------------------------------------- |
664 | //--------------------------------------------------------------------------------------------- |
| 641 | void usart0_TransmitTxData(void) { |
665 | void usart0_TransmitTxData(void) { |
| - | 666 | if (!txd_complete) |
|
| Line 642... | Line 667... | ||
| 642 | if(!txd_complete) return; |
667 | return; |
| 643 | 668 | ||
| - | 669 | if (request_VerInfo && txd_complete) { |
|
| 644 | if(request_VerInfo && txd_complete) { |
670 | SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, |
| 645 | SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo)); |
671 | sizeof(UART_VersionInfo)); |
| Line 646... | Line 672... | ||
| 646 | request_VerInfo = FALSE; |
672 | request_VerInfo = FALSE; |
| 647 | } |
673 | } |
| 648 | 674 | ||
| - | 675 | if (request_Display && txd_complete) { |
|
| 649 | if(request_Display && txd_complete) { |
676 | LCD_PrintMenu(); |
| 650 | LCD_PrintMenu(); |
677 | SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), |
| - | 678 | &DisplayBuff[DisplayLine * 20], 20); |
|
| 651 | SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
679 | DisplayLine++; |
| 652 | DisplayLine++; |
680 | if (DisplayLine >= 4) |
| Line 653... | Line 681... | ||
| 653 | if(DisplayLine >= 4) DisplayLine = 0; |
681 | DisplayLine = 0; |
| 654 | request_Display = FALSE; |
682 | request_Display = FALSE; |
| 655 | } |
683 | } |
| - | 684 | ||
| 656 | 685 | if (request_Display1 && txd_complete) { |
|
| 657 | if(request_Display1 && txd_complete) { |
686 | LCD_PrintMenu(); |
| Line 658... | Line 687... | ||
| 658 | LCD_PrintMenu(); |
687 | SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, |
| 659 | SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
688 | sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
| 660 | request_Display1 = FALSE; |
689 | request_Display1 = FALSE; |
| 661 | } |
690 | } |
| - | 691 | ||
| 662 | 692 | if (request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
|
| 663 | if(request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
693 | uint8_t label[16]; // local sram buffer |
| Line 664... | Line 694... | ||
| 664 | uint8_t label[16]; // local sram buffer |
694 | memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer |
| 665 | memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer |
695 | SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, |
| - | 696 | sizeof(request_DebugLabel), label, 16); |
|
| 666 | SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
697 | request_DebugLabel = 0xFF; |
| 667 | request_DebugLabel = 0xFF; |
698 | } |
| Line 668... | Line 699... | ||
| 668 | } |
699 | |
| - | 700 | if (ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen |
|
| 669 | 701 | SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, |
|
| 670 | if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen |
702 | sizeof(ConfirmFrame)); |
| 671 | SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame)); |
703 | ConfirmFrame = 0; |
| 672 | ConfirmFrame = 0; |
704 | } |
| Line 673... | Line 705... | ||
| 673 | } |
705 | |
| - | 706 | if (((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) |
|
| 674 | 707 | && txd_complete) { |
|
| - | 708 | SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut)); |
|
| 675 | if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) && txd_complete) { |
709 | DebugData_Timer = SetDelay(DebugData_Interval); |
| - | 710 | request_DebugData = FALSE; |
|
| 676 | SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut)); |
711 | } |
| 677 | DebugData_Timer = SetDelay(DebugData_Interval); |
712 | |
| 678 | request_DebugData = FALSE; |
713 | if (((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) |
| 679 | } |
714 | && txd_complete) { |
| 680 | 715 | SendOutData('C', FC_ADDRESS, 1, (uint8_t *) &Data3D, sizeof(Data3D)); |
|
| Line 681... | Line 716... | ||
| 681 | if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) && txd_complete) { |
716 | Data3D.AngleNick = (int16_t) ((10 * angle[PITCH]) |
| 682 | SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D)); |
717 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| - | 718 | Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL]) |
|
| 683 | Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
719 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| 684 | Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
720 | Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| Line 685... | Line 721... | ||
| 685 | Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
721 | Data3D_Timer = SetDelay(Data3D_Interval); |
| 686 | Data3D_Timer = SetDelay(Data3D_Interval); |
722 | request_Data3D = FALSE; |