Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2544 | holgerb | 1 | //############################################################################ |
2 | // MULTIPLEX Servo protocol SRXL16 & SRXL12 |
||
3 | //############################################################################ |
||
4 | |||
5 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
||
6 | |||
7 | #include "Spektrum.h" |
||
8 | #include "M-Link.h" |
||
9 | #include "main.h" |
||
10 | unsigned char NewMlinkData = 0; |
||
11 | unsigned char MlinkData[36]; |
||
12 | |||
13 | //############################################################################ |
||
14 | // Initializes the UART here |
||
15 | //############################################################################ |
||
16 | void MlinkUartInit(void) |
||
17 | { |
||
18 | SpektrumUartInit(); // same like Spektrum |
||
19 | }; |
||
20 | |||
21 | //############################################################################ |
||
22 | // Is called by the uart RX interrupt |
||
23 | // UDR contains the received byte |
||
24 | //############################################################################ |
||
25 | void MlinkParser(unsigned char udr) |
||
26 | { |
||
27 | static unsigned char state = 0, lenght = 0; |
||
28 | if(!SpektrumTimer) // Timeout-> block finished |
||
29 | { |
||
30 | if(state > 24 && state < 36) // udr = 0xA1 oder A2 |
||
31 | { |
||
32 | if(udr == 0xA1) lenght = 12 * 2 + 2; // 12 channels plus CRC |
||
33 | else |
||
34 | if(udr == 0xA2) lenght = 16 * 2 + 2; // 16 channels plus CRC |
||
35 | else lenght = 0; |
||
36 | } else lenght = 0; |
||
37 | state = 0; |
||
38 | } |
||
39 | else |
||
40 | { |
||
41 | if(state < sizeof(MlinkData)) |
||
42 | { |
||
43 | MlinkData[state++] = udr; |
||
44 | if(state == lenght) // last Byte received |
||
45 | { |
||
46 | NewMlinkData = lenght - 2; // without CRC |
||
47 | lenght = 0; |
||
48 | } |
||
49 | } |
||
50 | } |
||
51 | SpektrumTimer = 100; // 10ms Timeout |
||
52 | }; |
||
53 | |||
54 | void ProcessMlinkData(void) |
||
55 | { |
||
56 | unsigned char i = 0; |
||
57 | unsigned int tmp; |
||
58 | while(i < NewMlinkData) |
||
59 | { |
||
60 | tmp = (unsigned int) MlinkData[i] * 256 + MlinkData[i + 1]; |
||
61 | tmp /= 13; |
||
62 | i += 2; |
||
63 | s_update(i/2, (signed int) tmp - 156); // copies the values into the Channel-Data and calculates the PPM_Diff |
||
64 | SenderOkay = 220; |
||
65 | } |
||
66 | Channels = i/2 + 1; |
||
67 | NewPpmData = 0; // Null bedeutet: Neue Daten |
||
68 | NewMlinkData = 0; |
||
69 | } |
||
70 | #endif |