Rev 1646 | Rev 1796 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1646 | Rev 1775 | ||
---|---|---|---|
Line 62... | Line 62... | ||
62 | #include "timer0.h" |
62 | #include "timer0.h" |
63 | #include "uart0.h" |
63 | #include "uart0.h" |
64 | #include "attitude.h" |
64 | #include "attitude.h" |
65 | #include "rc.h" |
65 | #include "rc.h" |
66 | #include "externalControl.h" |
66 | #include "externalControl.h" |
- | 67 | #include "output.h" |
|
Line 67... | Line 68... | ||
67 | 68 | ||
68 | #if defined (USE_MK3MAG) |
69 | #if defined (USE_MK3MAG) |
69 | #include "ubx.h" |
70 | #include "ubx.h" |
70 | #endif |
71 | #endif |
Line 77... | Line 78... | ||
77 | #define MK3MAG_ADDRESS 3 |
78 | #define MK3MAG_ADDRESS 3 |
Line 78... | Line 79... | ||
78 | 79 | ||
79 | #define FALSE 0 |
80 | #define FALSE 0 |
80 | #define TRUE 1 |
81 | #define TRUE 1 |
81 | //int8_t test __attribute__ ((section (".noinit"))); |
82 | //int8_t test __attribute__ ((section (".noinit"))); |
82 | uint8_t Request_VerInfo = FALSE; |
83 | uint8_t request_VerInfo = FALSE; |
83 | uint8_t Request_ExternalControl = FALSE; |
84 | uint8_t request_ExternalControl = FALSE; |
84 | uint8_t Request_Display = FALSE; |
85 | uint8_t request_Display = FALSE; |
85 | uint8_t Request_Display1 = FALSE; |
86 | uint8_t request_Display1 = FALSE; |
86 | uint8_t Request_DebugData = FALSE; |
87 | uint8_t request_DebugData = FALSE; |
87 | uint8_t Request_Data3D = FALSE; |
88 | uint8_t request_Data3D = FALSE; |
88 | uint8_t Request_DebugLabel = 255; |
89 | uint8_t request_DebugLabel = 255; |
89 | uint8_t Request_PPMChannels = FALSE; |
90 | uint8_t request_PPMChannels = FALSE; |
- | 91 | uint8_t request_MotorTest = FALSE; |
|
- | 92 | uint8_t request_variables = FALSE; |
|
90 | uint8_t Request_MotorTest = FALSE; |
93 | |
Line 91... | Line 94... | ||
91 | uint8_t DisplayLine = 0; |
94 | uint8_t DisplayLine = 0; |
92 | 95 | ||
93 | volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
96 | volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
Line 129... | Line 132... | ||
129 | "GyroRoll(PID) ", |
132 | "GyroRoll(PID) ", |
130 | "GyroYaw ", //5 |
133 | "GyroYaw ", //5 |
131 | "GyroPitch(AC) ", |
134 | "GyroPitch(AC) ", |
132 | "GyroRoll(AC) ", |
135 | "GyroRoll(AC) ", |
133 | "GyroYaw(AC) ", |
136 | "GyroYaw(AC) ", |
134 | "AccPitch ", |
137 | "AccPitch (angle)", |
135 | "AccRoll ", //10 |
138 | "AccRoll (angle) ", //10 |
136 | "CorrSumPitch ", |
139 | "UBat ", |
137 | "CorrSumRoll ", |
140 | "Pitch Term ", |
138 | "Pressure range ", |
141 | "Roll Term ", |
139 | "Pressure A/D ", |
142 | "Yaw Term ", |
140 | "Pressure Value ", //15 |
143 | "Throttle Term ", //15 |
141 | "Press. rangewidz", |
144 | "0th O Corr pitch", |
142 | "Press. init A/D ", |
145 | "0th O Corr roll ", |
143 | "FACTOR ", |
146 | "DriftCompDelta P", |
144 | "throttleTerm ", |
147 | "DriftCompDelta R", |
145 | "pitchTerm ", //20 |
148 | "ADPitchGyroOffs ", //20 |
146 | "rollTerm ", |
149 | "ADRollGyroOffs ", |
147 | "yawTerm ", |
150 | "M1 ", |
148 | "P Pitch ", |
151 | "M2 ", |
149 | "I Pitch ", |
152 | "M3 ", |
150 | "P+D Pitch ", //25 |
153 | "M4 ", //25 |
151 | "Pitch acc noise ", |
154 | "ControlYaw ", |
152 | "Roll acc noise ", |
155 | "Acc Z ", |
153 | "DynamicPitch ", |
156 | "DriftCompPitch ", |
154 | "DynamicRoll ", |
157 | "DriftCompRoll ", |
155 | "Pitch Noise ", //30 |
158 | "Pitch Noise ", //30 |
156 | "Roll Noise " |
159 | "Roll Noise " |
157 | }; |
160 | }; |
Line 158... | Line 161... | ||
158 | 161 | ||
Line 331... | Line 334... | ||
331 | txd_complete = FALSE; |
334 | txd_complete = FALSE; |
332 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
335 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
333 | } |
336 | } |
Line 334... | Line 337... | ||
334 | 337 | ||
- | 338 | // -------------------------------------------------------------------------- |
|
- | 339 | // application example: |
|
- | 340 | // SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
|
- | 341 | /* |
|
- | 342 | void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
|
- | 343 | va_list ap; |
|
- | 344 | uint16_t txd_bufferIndex = 0; |
|
- | 345 | uint8_t *currentBuffer; |
|
- | 346 | uint8_t currentBufferIndex; |
|
- | 347 | uint16_t lengthOfCurrentBuffer; |
|
- | 348 | uint8_t shift = 0; |
|
- | 349 | |
|
- | 350 | txd_buffer[txd_bufferIndex++] = '#'; // Start character |
|
- | 351 | txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
|
- | 352 | txd_buffer[txd_bufferIndex++] = cmd; // Command |
|
- | 353 | |
|
- | 354 | va_start(ap, numofbuffers); |
|
- | 355 | ||
- | 356 | while(numofbuffers) { |
|
- | 357 | currentBuffer = va_arg(ap, uint8_t*); |
|
- | 358 | lengthOfCurrentBuffer = va_arg(ap, int); |
|
- | 359 | currentBufferIndex = 0; |
|
- | 360 | // Encode data: 3 bytes of data are encoded into 4 bytes, |
|
- | 361 | // where the 2 most significant bits are both 0. |
|
- | 362 | while(currentBufferIndex != lengthOfCurrentBuffer) { |
|
- | 363 | if (!shift) txd_buffer[txd_bufferIndex] = 0; |
|
- | 364 | txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
|
- | 365 | txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
|
- | 366 | shift += 2; |
|
- | 367 | if (shift == 6) { shift=0; txd_bufferIndex++; } |
|
- | 368 | currentBufferIndex++; |
|
- | 369 | } |
|
- | 370 | } |
|
- | 371 | // If the number of data bytes was not divisible by 3, stuff |
|
- | 372 | // with 0 pseudodata until length is again divisible by 3. |
|
- | 373 | if (shift == 2) { |
|
- | 374 | // We need to stuff with zero bytes at the end. |
|
- | 375 | txd_buffer[txd_bufferIndex] &= 0b00110000; |
|
- | 376 | txd_buffer[++txd_bufferIndex] = 0; |
|
- | 377 | shift = 4; |
|
- | 378 | } |
|
- | 379 | if (shift == 4) { |
|
- | 380 | // We need to stuff with zero bytes at the end. |
|
- | 381 | txd_buffer[txd_bufferIndex++] &= 0b00111100; |
|
- | 382 | txd_buffer[txd_bufferIndex] = 0; |
|
- | 383 | } |
|
- | 384 | va_end(ap); |
|
- | 385 | AddCRC(pt); // add checksum after data block and initates the transmission |
|
- | 386 | } |
|
- | 387 | */ |
|
335 | // -------------------------------------------------------------------------- |
388 | |
336 | void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
389 | void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
337 | va_list ap; |
390 | va_list ap; |
338 | uint16_t pt = 0; |
391 | uint16_t pt = 0; |
339 | uint8_t a,b,c; |
392 | uint8_t a,b,c; |
Line 424... | Line 477... | ||
424 | RxDataLen = ptrOut - 3; |
477 | RxDataLen = ptrOut - 3; |
425 | } |
478 | } |
Line 426... | Line 479... | ||
426 | 479 | ||
427 | // -------------------------------------------------------------------------- |
480 | // -------------------------------------------------------------------------- |
- | 481 | void usart0_ProcessRxData(void) { |
|
- | 482 | // We control the motorTestActive var from here: Count it down. |
|
428 | void usart0_ProcessRxData(void) { |
483 | if (motorTestActive) motorTestActive--; |
429 | // if data in the rxd buffer are not locked immediately return |
484 | // if data in the rxd buffer are not locked immediately return |
430 | if(!rxd_buffer_locked) return; |
- | |
431 | 485 | if(!rxd_buffer_locked) return; |
|
432 | uint8_t tempchar1, tempchar2; |
- | |
433 | 486 | uint8_t tempchar1, tempchar2; |
|
434 | Decode64(); // decode data block in rxd_buffer |
487 | Decode64(); // decode data block in rxd_buffer |
435 | 488 | ||
- | 489 | switch(rxd_buffer[1] - 'a') { |
|
436 | switch(rxd_buffer[1] - 'a') { |
490 | |
437 | case FC_ADDRESS: |
491 | case FC_ADDRESS: |
438 | switch(rxd_buffer[2]) { |
492 | switch(rxd_buffer[2]) { |
439 | #ifdef USE_MK3MAG |
493 | #ifdef USE_MK3MAG |
440 | case 'K':// compass value |
494 | case 'K':// compass value |
441 | CompassHeading = ((Heading_t *)pRxData)->Heading; |
495 | compassHeading = ((Heading_t *)pRxData)->Heading; |
442 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
496 | compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
443 | break; |
497 | break; |
444 | #endif |
- | |
445 | 498 | #endif |
|
446 | case 't':// motor test |
499 | case 't': // motor test |
447 | if(RxDataLen > 20) // |
- | |
448 | { |
500 | if(RxDataLen > 20) { |
449 | memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest)); |
- | |
450 | } |
501 | memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest)); |
451 | else |
- | |
452 | { |
502 | } else { |
453 | memcpy(&motorTest[0], (uint8_t*)pRxData, 4); |
503 | memcpy(&motorTest[0], (uint8_t*)pRxData, 4); |
454 | } |
- | |
455 | //Request_MotorTest = TRUE; |
504 | } |
456 | motorTestActive = 255; |
505 | motorTestActive = 255; |
457 | externalControlActive = 255; |
506 | externalControlActive = 255; |
Line 458... | Line 507... | ||
458 | break; |
507 | break; |
459 | 508 | ||
460 | case 'n':// "Get Mixer Table |
509 | case 'n':// "Get Mixer Table |
461 | while(!txd_complete); // wait for previous frame to be sent |
510 | while(!txd_complete); // wait for previous frame to be sent |
Line 462... | Line 511... | ||
462 | SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer)); |
511 | SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer)); |
463 | break; |
512 | break; |
464 | - | ||
465 | case 'm':// "Set Mixer Table |
513 | |
466 | if(pRxData[0] == EEMIXER_REVISION) |
514 | case 'm':// "Set Mixer Table |
467 | { |
515 | if(pRxData[0] == EEMIXER_REVISION) { |
468 | memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer)); |
516 | memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer)); |
469 | MixerTable_WriteToEEProm(); |
- | |
470 | while(!txd_complete); // wait for previous frame to be sent |
517 | MixerTable_WriteToEEProm(); |
471 | tempchar1 = 1; |
- | |
472 | } |
518 | while(!txd_complete); // wait for previous frame to be sent |
473 | else |
519 | tempchar1 = 1; |
474 | { |
520 | } else { |
475 | tempchar1 = 0; |
521 | tempchar1 = 0; |
Line 476... | Line 522... | ||
476 | } |
522 | } |
477 | SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1); |
523 | SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1); |
478 | break; |
524 | break; |
Line 479... | Line 525... | ||
479 | 525 | ||
480 | case 'p': // get PPM channels |
526 | case 'p': // get PPM channels |
481 | Request_PPMChannels = TRUE; |
- | |
482 | break; |
527 | request_PPMChannels = TRUE; |
483 | 528 | break; |
|
484 | case 'q':// request settings |
529 | |
485 | if(pRxData[0] == 0xFF) |
530 | case 'q':// request settings |
486 | { |
531 | if(pRxData[0] == 0xFF) { |
487 | pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
532 | pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
488 | } |
533 | } |
Line 525... | Line 570... | ||
525 | //unsupported command received |
570 | //unsupported command received |
526 | break; |
571 | break; |
527 | } // case FC_ADDRESS: |
572 | } // case FC_ADDRESS: |
Line 528... | Line 573... | ||
528 | 573 | ||
529 | default: // any Slave Address |
- | |
530 | 574 | default: // any Slave Address |
|
531 | switch(rxd_buffer[2]) |
- | |
532 | { |
575 | switch(rxd_buffer[2]) { |
533 | case 'a':// request for labels of the analog debug outputs |
576 | case 'a':// request for labels of the analog debug outputs |
534 | Request_DebugLabel = pRxData[0]; |
577 | request_DebugLabel = pRxData[0]; |
535 | if(Request_DebugLabel > 31) Request_DebugLabel = 31; |
578 | if(request_DebugLabel > 31) request_DebugLabel = 31; |
536 | externalControlActive = 255; |
579 | externalControlActive = 255; |
Line 537... | Line 580... | ||
537 | break; |
580 | break; |
538 | 581 | ||
Line 544... | Line 587... | ||
544 | 587 | ||
545 | case 'h':// request for display columns |
588 | case 'h':// request for display columns |
546 | externalControlActive = 255; |
589 | externalControlActive = 255; |
547 | RemoteKeys |= pRxData[0]; |
590 | RemoteKeys |= pRxData[0]; |
548 | if(RemoteKeys) DisplayLine = 0; |
591 | if(RemoteKeys) DisplayLine = 0; |
549 | Request_Display = TRUE; |
592 | request_Display = TRUE; |
Line 550... | Line 593... | ||
550 | break; |
593 | break; |
551 | 594 | ||
552 | case 'l':// request for display columns |
595 | case 'l':// request for display columns |
553 | externalControlActive = 255; |
596 | externalControlActive = 255; |
554 | MenuItem = pRxData[0]; |
597 | MenuItem = pRxData[0]; |
Line 555... | Line 598... | ||
555 | Request_Display1 = TRUE; |
598 | request_Display1 = TRUE; |
556 | break; |
599 | break; |
557 | 600 | ||
Line -... | Line 601... | ||
- | 601 | case 'v': // request for version and board release |
|
- | 602 | request_VerInfo = TRUE; |
|
- | 603 | break; |
|
- | 604 | ||
558 | case 'v': // request for version and board release |
605 | case 'x': |
559 | Request_VerInfo = TRUE; |
606 | request_variables = TRUE; |
560 | break; |
607 | break; |
Line 561... | Line 608... | ||
561 | 608 | ||
562 | case 'g':// get external control data |
609 | case 'g':// get external control data |
563 | Request_ExternalControl = TRUE; |
610 | request_ExternalControl = TRUE; |
564 | break; |
611 | break; |
Line 565... | Line 612... | ||
565 | 612 | ||
566 | case 'd': // request for the debug data |
613 | case 'd': // request for the debug data |
567 | DebugData_Interval = (uint16_t) pRxData[0] * 10; |
614 | DebugData_Interval = (uint16_t) pRxData[0] * 10; |
568 | if(DebugData_Interval > 0) Request_DebugData = TRUE; |
615 | if(DebugData_Interval > 0) request_DebugData = TRUE; |
Line 569... | Line 616... | ||
569 | break; |
616 | break; |
570 | 617 | ||
571 | case 'c': // request for the 3D data |
618 | case 'c': // request for the 3D data |
Line 600... | Line 647... | ||
600 | 647 | ||
601 | //--------------------------------------------------------------------------------------------- |
648 | //--------------------------------------------------------------------------------------------- |
602 | void usart0_TransmitTxData(void) { |
649 | void usart0_TransmitTxData(void) { |
Line 603... | Line 650... | ||
603 | if(!txd_complete) return; |
650 | if(!txd_complete) return; |
604 | 651 | ||
605 | if(Request_VerInfo && txd_complete) { |
652 | if(request_VerInfo && txd_complete) { |
606 | SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo)); |
653 | SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo)); |
Line 607... | Line 654... | ||
607 | Request_VerInfo = FALSE; |
654 | request_VerInfo = FALSE; |
608 | } |
655 | } |
609 | 656 | ||
610 | if(Request_Display && txd_complete) { |
657 | if(request_Display && txd_complete) { |
611 | LCD_PrintMenu(); |
658 | LCD_PrintMenu(); |
612 | SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
659 | SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
613 | DisplayLine++; |
660 | DisplayLine++; |
Line 614... | Line 661... | ||
614 | if(DisplayLine >= 4) DisplayLine = 0; |
661 | if(DisplayLine >= 4) DisplayLine = 0; |
615 | Request_Display = FALSE; |
662 | request_Display = FALSE; |
616 | } |
663 | } |
617 | 664 | ||
618 | if(Request_Display1 && txd_complete) { |
665 | if(request_Display1 && txd_complete) { |
Line 619... | Line 666... | ||
619 | LCD_PrintMenu(); |
666 | LCD_PrintMenu(); |
620 | SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
667 | SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
621 | Request_Display1 = FALSE; |
668 | request_Display1 = FALSE; |
622 | } |
669 | } |
623 | 670 | ||
624 | if(Request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
671 | if(request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
Line 625... | Line 672... | ||
625 | uint8_t label[16]; // local sram buffer |
672 | uint8_t label[16]; // local sram buffer |
626 | memcpy_P(label, ANALOG_LABEL[Request_DebugLabel], 16); // read lable from flash to sram buffer |
673 | memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer |
627 | SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), label, 16); |
674 | SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
628 | Request_DebugLabel = 0xFF; |
675 | request_DebugLabel = 0xFF; |
Line 629... | Line 676... | ||
629 | } |
676 | } |
630 | 677 | ||
631 | if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen |
678 | if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen |
632 | SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame)); |
679 | SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame)); |
633 | ConfirmFrame = 0; |
680 | ConfirmFrame = 0; |
Line 634... | Line 681... | ||
634 | } |
681 | } |
635 | 682 | ||
636 | if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete) { |
683 | if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) && txd_complete) { |
637 | SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut)); |
684 | SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut)); |
638 | DebugData_Timer = SetDelay(DebugData_Interval); |
685 | DebugData_Timer = SetDelay(DebugData_Interval); |
639 | Request_DebugData = FALSE; |
686 | request_DebugData = FALSE; |
640 | } |
687 | } |
641 | 688 | ||
Line 642... | Line 689... | ||
642 | if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete) { |
689 | if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) && txd_complete) { |
643 | SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D)); |
690 | SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D)); |
644 | Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
691 | Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
645 | Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
692 | Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Line 646... | Line 693... | ||
646 | Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
693 | Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
647 | Data3D_Timer = SetDelay(Data3D_Interval); |
694 | Data3D_Timer = SetDelay(Data3D_Interval); |
648 | Request_Data3D = FALSE; |
695 | request_Data3D = FALSE; |
649 | } |
696 | } |
650 | 697 | ||
651 | if(Request_ExternalControl && txd_complete) { |
698 | if(request_ExternalControl && txd_complete) { |
652 | SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &externalControl, sizeof(externalControl)); |
699 | SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &externalControl, sizeof(externalControl)); |
653 | Request_ExternalControl = FALSE; |
700 | request_ExternalControl = FALSE; |
654 | } |
701 | } |
655 | 702 | ||
656 | #ifdef USE_MK3MAG |
703 | #ifdef USE_MK3MAG |
657 | if((CheckDelay(Compass_Timer)) && txd_complete) { |
704 | if((CheckDelay(Compass_Timer)) && txd_complete) { |
658 | ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
705 | ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
Line 659... | Line 706... | ||
659 | ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
706 | ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
660 | ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
707 | ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
661 | ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
708 | ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
662 | ToMk3Mag.CalState = CompassCalState; |
709 | ToMk3Mag.CalState = compassCalState; |
Line 663... | Line 710... | ||
663 | SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
710 | SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
664 | // the last state is 5 and should be send only once to avoid multiple flash writing |
711 | // the last state is 5 and should be send only once to avoid multiple flash writing |
665 | if(CompassCalState > 4) CompassCalState = 0; |
712 | if(compassCalState > 4) compassCalState = 0; |
- | 713 | Compass_Timer = SetDelay(99); |
|
- | 714 | } |
|
- | 715 | #endif |
|
- | 716 | ||
- | 717 | if(request_MotorTest && txd_complete) { |
|
666 | Compass_Timer = SetDelay(99); |
718 | SendOutData('T', FC_ADDRESS, 0); |
667 | } |
719 | request_MotorTest = FALSE; |