Rev 2052 | Rev 2057 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2052 | Rev 2055 | ||
---|---|---|---|
Line 23... | Line 23... | ||
23 | #define NC_ADDRESS 2 |
23 | #define NC_ADDRESS 2 |
24 | #define MK3MAG_ADDRESS 3 |
24 | #define MK3MAG_ADDRESS 3 |
Line 25... | Line 25... | ||
25 | 25 | ||
26 | #define FALSE 0 |
26 | #define FALSE 0 |
- | 27 | #define TRUE 1 |
|
27 | #define TRUE 1 |
28 | |
- | 29 | uint8_t requestedDebugLabel = 255; |
|
28 | //int8_t test __attribute__ ((section (".noinit"))); |
30 | |
29 | uint8_t request_verInfo = FALSE; |
31 | uint8_t request_verInfo = FALSE; |
30 | uint8_t request_externalControl = FALSE; |
32 | uint8_t request_externalControl = FALSE; |
31 | uint8_t request_display = FALSE; |
33 | uint8_t request_display = FALSE; |
32 | uint8_t request_display1 = FALSE; |
34 | uint8_t request_display1 = FALSE; |
33 | uint8_t request_debugData = FALSE; |
35 | uint8_t request_debugData = FALSE; |
34 | uint8_t request_data3D = FALSE; |
- | |
35 | uint8_t request_debugLabel = 255; |
36 | uint8_t request_data3D = FALSE; |
36 | uint8_t request_PPMChannels = FALSE; |
37 | uint8_t request_PPMChannels = FALSE; |
37 | uint8_t request_motorTest = FALSE; |
38 | uint8_t request_motorTest = FALSE; |
- | 39 | uint8_t request_variables = FALSE; |
|
- | 40 | uint8_t request_OSD = FALSE; |
|
- | 41 | ||
- | 42 | /* |
|
- | 43 | #define request_verInfo (1<<0) |
|
- | 44 | #define request_externalControl (1<<1) |
|
- | 45 | #define request_display (1<<3) |
|
- | 46 | #define request_display1 (1<<4) |
|
- | 47 | #define request_debugData (1<<5) |
|
- | 48 | #define request_data3D (1<<6) |
|
- | 49 | #define request_PPMChannels (1<<7) |
|
- | 50 | #define request_motorTest (1<<8) |
|
- | 51 | #define request_variables (1<<9) |
|
- | 52 | #define request_OSD (1<<10) |
|
- | 53 | */ |
|
- | 54 | ||
Line 38... | Line 55... | ||
38 | uint8_t request_variables = FALSE; |
55 | //uint16_t request = 0; |
Line 39... | Line 56... | ||
39 | 56 | ||
40 | uint8_t displayLine = 0; |
57 | uint8_t displayLine = 0; |
Line 57... | Line 74... | ||
57 | 74 | ||
Line 58... | Line 75... | ||
58 | Data3D_t data3D; |
75 | Data3D_t data3D; |
59 | 76 | ||
- | 77 | uint16_t debugData_timer; |
|
60 | uint16_t debugData_timer; |
78 | uint16_t data3D_timer; |
61 | uint16_t data3D_timer; |
79 | uint16_t OSD_timer; |
- | 80 | uint16_t debugData_interval = 0; // in 1ms |
|
Line 62... | Line 81... | ||
62 | uint16_t debugData_interval = 0; // in 1ms |
81 | uint16_t data3D_interval = 0; // in 1ms |
63 | uint16_t data3D_interval = 0; // in 1ms |
82 | uint16_t OSD_interval = 0; |
64 | 83 | ||
Line 73... | Line 92... | ||
73 | "AngleRoll ", |
92 | "AngleRoll ", |
74 | "AngleYaw ", |
93 | "AngleYaw ", |
75 | "GyroPitch ", |
94 | "GyroPitch ", |
76 | "GyroRoll ", |
95 | "GyroRoll ", |
77 | "GyroYaw ", //5 |
96 | "GyroYaw ", //5 |
78 | "AccPitch ", |
97 | "PitchTerm ", |
79 | "AccRoll ", |
98 | "RollTerm ", |
80 | "AccZ ", |
99 | "ThrottleTerm ", |
81 | "AccPitch (angle)", |
100 | "YawTerm ", |
82 | "AccRoll (angle) ", //10 |
101 | "M1 ", //10 |
83 | "GPS sat ", |
102 | "M2 ", |
84 | "mag hdng ", |
103 | "M3 ", |
85 | "Pitch Term ", |
104 | "M4 ", |
86 | "Roll Term ", |
105 | "pdpart ", |
87 | "Yaw Term ", //15 |
106 | "control ", //15 |
88 | "Throttle Term ", |
107 | "ipart ", |
89 | "ControlAct/10 ", |
108 | "ControlAct/10 ", |
90 | "RC Qual ", |
109 | "RC Qual ", |
91 | "heightTrottleIn ", |
110 | "heightTrottleIn ", |
92 | "HeightdThrottle ", //20 |
111 | "HeightdThrottle ", //20 |
93 | "Height ", |
112 | "Height ", |
94 | "TargetHeight ", |
113 | "TargetHeight ", |
95 | "heightError ", |
114 | "heightError ", |
96 | "HeightPdThrottle", |
115 | "HeightPdThrottle", |
97 | "HeightIdThrottle", //25 |
116 | "HeightIdThrottle", //25 |
98 | "HeightDdThrottle", |
117 | "HeightDdThrottle", |
99 | "HeadingInDegrees", |
118 | "Yaw Limit / 100 ", |
100 | "0 ", |
119 | "HeadingError/100", |
101 | "1 ", |
120 | "PD Yaw ", |
102 | "2 ", //30 |
121 | "2 ", //30 |
103 | "3 " |
122 | "3 " |
104 | }; |
123 | }; |
Line 105... | Line 124... | ||
105 | 124 | ||
Line 179... | Line 198... | ||
179 | versionInfo.SWMinor = VERSION_MINOR; |
198 | versionInfo.SWMinor = VERSION_MINOR; |
180 | versionInfo.SWPatch = VERSION_PATCH; |
199 | versionInfo.SWPatch = VERSION_PATCH; |
181 | versionInfo.protoMajor = VERSION_SERIAL_MAJOR; |
200 | versionInfo.protoMajor = VERSION_SERIAL_MAJOR; |
182 | versionInfo.protoMinor = VERSION_SERIAL_MINOR; |
201 | versionInfo.protoMinor = VERSION_SERIAL_MINOR; |
Line -... | Line 202... | ||
- | 202 | ||
- | 203 | for (uint8_t i=0; i<32; i++) { |
|
- | 204 | debugOut.analog[i] = i; |
|
- | 205 | } |
|
183 | 206 | ||
184 | // restore global interrupt flags |
207 | // restore global interrupt flags |
185 | SREG = sreg; |
208 | SREG = sreg; |
Line 186... | Line 209... | ||
186 | } |
209 | } |
Line 265... | Line 288... | ||
265 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
288 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
266 | } |
289 | } |
267 | } |
290 | } |
Line 268... | Line 291... | ||
268 | 291 | ||
269 | // -------------------------------------------------------------------------- |
292 | // -------------------------------------------------------------------------- |
270 | void Addchecksum(uint16_t datalen) { |
293 | void addChecksum(uint16_t datalen) { |
271 | uint16_t tmpchecksum = 0, i; |
294 | uint16_t tmpchecksum = 0, i; |
272 | for (i = 0; i < datalen; i++) { |
295 | for (i = 0; i < datalen; i++) { |
273 | tmpchecksum += txd_buffer[i]; |
296 | tmpchecksum += txd_buffer[i]; |
274 | } |
297 | } |
Line 391... | Line 414... | ||
391 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
414 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
392 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
415 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
393 | txd_buffer[pt++] = '=' + (c & 0x3f); |
416 | txd_buffer[pt++] = '=' + (c & 0x3f); |
394 | } |
417 | } |
395 | va_end(ap); |
418 | va_end(ap); |
396 | Addchecksum(pt); // add checksum after data block and initates the transmission |
419 | addChecksum(pt); // add checksum after data block and initates the transmission |
397 | } |
420 | } |
Line 398... | Line 421... | ||
398 | 421 | ||
399 | // -------------------------------------------------------------------------- |
422 | // -------------------------------------------------------------------------- |
400 | void Decode64(void) { |
423 | void decode64(void) { |
401 | uint8_t a, b, c, d; |
424 | uint8_t a, b, c, d; |
402 | uint8_t x, y, z; |
425 | uint8_t x, y, z; |
403 | uint8_t ptrIn = 3; |
426 | uint8_t ptrIn = 3; |
404 | uint8_t ptrOut = 3; |
427 | uint8_t ptrOut = 3; |
Line 439... | Line 462... | ||
439 | motorTestActive--; |
462 | motorTestActive--; |
440 | // if data in the rxd buffer are not locked immediately return |
463 | // if data in the rxd buffer are not locked immediately return |
441 | if (!rxd_buffer_locked) |
464 | if (!rxd_buffer_locked) |
442 | return; |
465 | return; |
443 | uint8_t tempchar[3]; |
466 | uint8_t tempchar[3]; |
444 | Decode64(); // decode data block in rxd_buffer |
467 | decode64(); // decode data block in rxd_buffer |
Line 445... | Line 468... | ||
445 | 468 | ||
Line 446... | Line 469... | ||
446 | switch (rxd_buffer[1] - 'a') { |
469 | switch (rxd_buffer[1] - 'a') { |
447 | 470 | ||
Line 530... | Line 553... | ||
530 | } // case FC_ADDRESS: |
553 | } // case FC_ADDRESS: |
Line 531... | Line 554... | ||
531 | 554 | ||
532 | default: // any Slave Address |
555 | default: // any Slave Address |
533 | switch (rxd_buffer[2]) { |
556 | switch (rxd_buffer[2]) { |
534 | case 'a':// request for labels of the analog debug outputs |
557 | case 'a':// request for labels of the analog debug outputs |
535 | request_debugLabel = pRxData[0]; |
558 | requestedDebugLabel = pRxData[0]; |
536 | if (request_debugLabel > 31) |
559 | if (requestedDebugLabel > 31) |
537 | request_debugLabel = 31; |
560 | requestedDebugLabel = 31; |
Line 538... | Line 561... | ||
538 | break; |
561 | break; |
539 | 562 | ||
540 | case 'b': // submit extern control |
563 | case 'b': // submit extern control |
Line 553... | Line 576... | ||
553 | case 'l':// request for display columns |
576 | case 'l':// request for display columns |
554 | menuItem = pRxData[0]; |
577 | menuItem = pRxData[0]; |
555 | request_display1 = TRUE; |
578 | request_display1 = TRUE; |
556 | break; |
579 | break; |
Line -... | Line 580... | ||
- | 580 | ||
- | 581 | case 'o':// request for OSD data (FC style) |
|
- | 582 | OSD_interval = (uint16_t) pRxData[0] * 10; |
|
- | 583 | if (OSD_interval > 0) |
|
- | 584 | request_OSD = TRUE; |
|
- | 585 | break; |
|
557 | 586 | ||
558 | case 'v': // request for version and board release |
587 | case 'v': // request for version and board release |
559 | request_verInfo = TRUE; |
588 | request_verInfo = TRUE; |
Line 560... | Line 589... | ||
560 | break; |
589 | break; |
Line 629... | Line 658... | ||
629 | sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem, |
658 | sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem, |
630 | sizeof(maxMenuItem), displayBuff, sizeof(displayBuff)); |
659 | sizeof(maxMenuItem), displayBuff, sizeof(displayBuff)); |
631 | request_display1 = FALSE; |
660 | request_display1 = FALSE; |
632 | } |
661 | } |
Line 633... | Line 662... | ||
633 | 662 | ||
634 | if (request_debugLabel != 0xFF) { // Texte f�r die Analogdaten |
663 | if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten |
635 | uint8_t label[16]; // local sram buffer |
664 | uint8_t label[16]; // local sram buffer |
636 | memcpy_P(label, ANALOG_LABEL[request_debugLabel], 16); // read lable from flash to sram buffer |
665 | memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer |
637 | sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_debugLabel, |
666 | sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel, |
638 | sizeof(request_debugLabel), label, 16); |
667 | sizeof(requestedDebugLabel), label, 16); |
639 | request_debugLabel = 0xFF; |
668 | requestedDebugLabel = 0xFF; |
Line 640... | Line 669... | ||
640 | } |
669 | } |
641 | 670 | ||
642 | if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen |
671 | if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen |
Line 649... | Line 678... | ||
649 | sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut)); |
678 | sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut)); |
650 | debugData_timer = setDelay(debugData_interval); |
679 | debugData_timer = setDelay(debugData_interval); |
651 | request_debugData = FALSE; |
680 | request_debugData = FALSE; |
652 | } |
681 | } |
Line 653... | Line 682... | ||
653 | 682 | ||
654 | if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) |
- | |
655 | && txd_complete) { |
683 | if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) { |
656 | sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
684 | sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
657 | data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
685 | data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg |
658 | data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
686 | data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg |
659 | data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1° |
687 | data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg |
660 | data3D_timer = setDelay(data3D_interval); |
688 | data3D_timer = setDelay(data3D_interval); |
661 | request_data3D = FALSE; |
689 | request_data3D = FALSE; |
Line 662... | Line 690... | ||
662 | } |
690 | } |
Line 693... | Line 721... | ||
693 | 721 | ||
694 | if (request_variables && txd_complete) { |
722 | if (request_variables && txd_complete) { |
695 | sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables)); |
723 | sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables)); |
696 | request_variables = FALSE; |
724 | request_variables = FALSE; |
- | 725 | } |
|
- | 726 | ||
- | 727 | if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) { |
|
- | 728 | int32_t height = analog_getHeight(); |
|
- | 729 | data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg |
|
- | 730 | data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg |
|
- | 731 | data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg |
|
- | 732 | sendOutData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat)); |
|
- | 733 | OSD_timer = setDelay(OSD_interval); |
|
- | 734 | request_OSD = FALSE; |
|
697 | } |
735 | } |