Go to most recent revision | 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 |