API tools faq
paste
Advertisement
Back__Fire

Arduino Mavlink Decoder

Back__Fire
Aug 18th, 2014
466
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 8.39 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. ||                  Interceptor              ||
  3. ||                                           ||
  4. ||  Demonstraion of MavLink packet           ||
  5. ||  recognition.                             ||
  6.  
  7.  
  8.  
  9. //=============//
  10.       Notes
  11. //=============//
  12.  
  13.  - Kye Morton
  14. */
  15.  
  16. #define X25_INIT_CRC 0xffff
  17. #define X25_VALIDATE_CRC 0xf0b8
  18.  
  19. #define UINT8_MAX 0xFF
  20. #define UINT16_MAX 0xFFFF
  21. #define UINT32_MAX 0xFFFFFFFF
  22. #define UINT64_MAX 0xFFFFFFFFFFFFFFFFULL
  23.  
  24. #define CMD_ID_HEARTBEAT 0                                              //Define all the message IDs that you want to look for here
  25.  
  26. #define SERIAL_0_BAUD 57600
  27. #define SERIAL_1_BAUD 57600
  28.  
  29. #define MUTE_SYS_STATUS 0
  30. #define MUTE_DEBUG_STATUS 1
  31. #define MUTE_RAW_STATUS 1
  32.  
  33.  
  34. //=============//
  35. //  Variables  //
  36. //=============//
  37.  
  38. #include <SoftwareSerial.h>
  39.  
  40.  
  41. //=============//
  42. //  Variables  //
  43. //=============//
  44.  
  45. struct msgBuff_s                                                        //Temporary Storage While Reading Messages
  46. {
  47.     unsigned int byteCount;
  48.     unsigned char inputs[263];
  49.     unsigned char expByteLeft;
  50.     bool phraseInProgress;
  51.     uint16_t id;
  52. };
  53.  
  54.  
  55. //=============//
  56. // Func.Protos //
  57. //=============//
  58. unsigned char comm_receive();
  59. char phraseMessage(unsigned char c, msgBuff_s &inMsg);
  60. void clearMsgBuff(msgBuff_s &msg);
  61. void decodeMessage(msgBuff_s &msgIn);
  62.  
  63. static uint16_t crc_calculate(unsigned char *pBuffer, int length);
  64. static uint16_t crc_accumulate(unsigned char b, uint16_t crc);
  65.  
  66. static void writeMessage(msgBuff_s &msg, uint32_t port);
  67.  
  68. void updateHeartLED();
  69.  
  70.  
  71. //=============//
  72. //  Variables  //
  73. //=============//
  74. //Message CRC seeds. Each ID has a corresponding seed in this array. Heartbeat(ID:0)=50
  75. const unsigned char MAVLINK_MESSAGE_CRCS[] = {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 42, 21, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247};
  76.  
  77. uint16_t crcAccum = 0;
  78.  
  79. msgBuff_s inMsgSerial;                                                  //Message buffer for Serial1
  80.  
  81. unsigned long startTimeMilli = 0;
  82. unsigned long lastHeartMsg = 0;
  83.  
  84. bool LEDStatus = false;
  85. unsigned int LEDPin = 13;
  86.  
  87. bool MUTE = MUTE_SYS_STATUS;                                            //Set to '1' to disable debug serial output
  88. bool MUTE_DEBUG = MUTE_DEBUG_STATUS;                                    //Set to '1' to disable debug serial output
  89. bool MUTE_RAW = MUTE_RAW_STATUS;                                        //Set to '1' to disable debug serial output
  90.  
  91.  
  92. //=============//
  93. //    Main     //
  94. //=============//
  95. void setup()
  96. {  
  97.     startTimeMilli = millis();
  98.    
  99.     Serial.begin(SERIAL_0_BAUD);                                        //Monitoring Link
  100.  
  101.     if(!MUTE)
  102.     {
  103.         Serial.println("Opening communication serial read on Serial Port 0...");
  104.         Serial.println();
  105.     }
  106.  
  107.     Serial1.begin(SERIAL_1_BAUD);                                               //Monitoring Link
  108.  
  109.     if(!MUTE)
  110.     {
  111.         Serial.println("Opening communication serial read on Serial Port 1...");
  112.         Serial.println();
  113.     }
  114.    
  115.     pinMode(LEDPin, OUTPUT);
  116.     digitalWrite(LEDPin, LOW);
  117.        
  118.     clearMsgBuff(inMsgSerial);
  119.     inMsgSerial.id = 1;
  120. }
  121.  
  122. void loop()
  123. {
  124.     unsigned long startTimeLoop = millis();
  125.     unsigned long startTimeSecs = (startTimeLoop - startTimeMilli)/1000;
  126.  
  127.     if(!MUTE)
  128.     {
  129.         Serial.print("\nSystem has been online for ");
  130.         Serial.print(startTimeSecs);
  131.         Serial.println(" seconds.");
  132.     }
  133.    
  134.     while (millis() - startTimeLoop < 1000)                             //Holds the program for 1 second to catch new data from the serial buffer
  135.     {
  136.         switch(comm_receive())
  137.         {
  138.             case 0:                                                     //A message was read by Serial0
  139.             {
  140.                 break;
  141.             }
  142.             case 1:                                                     //A message was read by Serial1
  143.             {
  144.                 break;
  145.             }
  146.             default:                                                    //No Message was read
  147.             {
  148.                 break;
  149.             }
  150.         }
  151.     }
  152.    
  153.     if((lastHeartMsg + 2000)<millis())                                  //No heartbeat has come through in 2 seconds
  154.     {
  155.         digitalWrite(LEDPin, HIGH);
  156.     }
  157. }
  158.  
  159. unsigned char comm_receive()                                            //Reads in all available data in the serial buffer and passes it off to to be phrased
  160. {
  161.     while(Serial1.available() > 0)                                      //Duplicate this loop for multiple input serials
  162.     {
  163.         unsigned char c = Serial1.read();
  164.        
  165.         if(phraseMessage(c, inMsgSerial)>0)                             //Will return 1 (for Serial1) when a message has completed being read.
  166.         {      
  167.             return 1;
  168.         }
  169.     }
  170.        
  171.     return 0;
  172. }
  173.  
  174. char phraseMessage(unsigned char c, msgBuff_s &inMsg)
  175. {
  176.     bool messageComplete = 0;
  177.     uint16_t crcTemp = 0;
  178.     uint16_t crcCheck = 0;
  179.     int i = 0;
  180.    
  181.     if((c==0xFE)&&(!inMsg.phraseInProgress))                            //If it a header byte (0xFE) is detected, and there is no message currently being phrased
  182.     {
  183.         clearMsgBuff(inMsg);                                            //Reset message buffer and start recording message
  184.         inMsg.phraseInProgress = 1;
  185.     }
  186.    
  187.     if(inMsg.phraseInProgress==1)
  188.     {
  189.         inMsg.inputs[inMsg.byteCount] = c;                              //Appends the newest byte to the array
  190.        
  191.         inMsg.byteCount++;
  192.        
  193.         if(inMsg.byteCount>=(inMsg.inputs[1]+8))                        //The expected packet length has been collected. "inMsg.inputs[1]" is "len", +8 for rest of header
  194.         {  
  195.             inMsg.phraseInProgress = 0;
  196.             messageComplete = 1;
  197.         }
  198.     }
  199.    
  200.     if(messageComplete)
  201.     {
  202.         crcTemp = crc_calculate(inMsg.inputs, inMsg.byteCount-2);       //Calculate and CRC for the gathered messsage, excluding the sent CRC
  203.        
  204.         if (inMsg.byteCount > 5 && inMsg.inputs[0] == 254)              //Make sure there is at least a header, and the first byte is 0xFE
  205.         {
  206.             crcTemp = crc_accumulate(MAVLINK_MESSAGE_CRCS[inMsg.inputs[5]], crcTemp);   //Use magic to add to the crcTemp value
  207.         }
  208.  
  209.         crcCheck = (inMsg.inputs[inMsg.byteCount-1] << 8) | (inMsg.inputs[inMsg.byteCount-2]);  //Gather the value of the given CRC
  210.        
  211.         if(crcTemp==crcCheck)                                           //If the CRCs match, message is valid
  212.         {  
  213.             c = 0;
  214.            
  215.             decodeMessage(inMsg);
  216.             return 1;
  217.         }
  218.     }
  219.    
  220.     return 0;                                                           //Discard message, as it is not valid
  221. }
  222.  
  223. void decodeMessage(msgBuff_s &msgIn)
  224. {
  225.     unsigned char sys = msgIn.inputs[3];
  226.     unsigned char cmp = msgIn.inputs[4];
  227.     unsigned char id = msgIn.inputs[5];
  228.    
  229.     switch(id)                                                          //Add case's to this to give options for different id's to be recognised
  230.     {
  231.        
  232.         case CMD_ID_HEARTBEAT:                                          //Heartbeat message
  233.         {
  234.             if(!MUTE_DEBUG)
  235.             {
  236.                 Serial.println("Heartbeat message detected...");
  237.             }
  238.  
  239.             void updateHeartLED();
  240.            
  241.             break;
  242.         }
  243.         default:
  244.         {
  245.             if(!MUTE_DEBUG)
  246.             {
  247.                 Serial.println("Undefined MavLink message detected...");
  248.             }
  249.            
  250.             break;
  251.         }
  252.     }
  253. }
  254.  
  255. static uint16_t crc_accumulate(unsigned char b, uint16_t crc)           //More Magic
  256. {
  257.         unsigned char ch = (unsigned char)(b ^ (unsigned char)(crc & 0x00ff));
  258.         ch = (unsigned char)(ch ^ (ch << 4));
  259.         return (uint16_t)((crc >> 8) ^ (ch << 8) ^ (ch << 3) ^ (ch >> 4));
  260. }
  261.  
  262. static uint16_t crc_calculate(unsigned char* pBuffer, int length)       //Magic
  263. {
  264.     if (length < 1)
  265.     {
  266.         return 0xffff;
  267.     }
  268.  
  269.     uint16_t crcTmp;
  270.     int i;
  271.  
  272.     crcTmp = X25_INIT_CRC;
  273.  
  274.     for (i = 1; i < length; i++)                                        // skips header
  275.     {
  276.         crcTmp = crc_accumulate(pBuffer[i], crcTmp);
  277.     }
  278.  
  279.     return (crcTmp);
  280. }
  281.  
  282. void clearMsgBuff(msgBuff_s &msg)                                       //Resets the provided buffer struct
  283. {
  284.     msg.byteCount = 0;
  285.     msg.phraseInProgress = 0;
  286.     msg.expByteLeft = 0;
  287.    
  288.     for(int k=0;k<263;k++)
  289.     {
  290.         msg.inputs[k] = 0;
  291.     }
  292. }
  293.  
  294. static void writeMessage(msgBuff_s &msg, uint32_t port)                 //Pass a compiled msgBuff_s and the port you want msg.inputs to be writen to
  295. {                                                                       //msg.inputs will have msg.byteCount outputs written to the serial
  296.     switch(port)
  297.     {
  298.         case 0:
  299.             for(int i=0;i<msg.byteCount;i++)
  300.             {
  301.                 if(msg.inputs[i]<0x10)
  302.                 {
  303.                     Serial.print("0");
  304.                 }
  305.                 Serial.print(msg.inputs[i], HEX);
  306.                 Serial.print(" ");
  307.             }
  308.            
  309.             Serial.println();
  310.            
  311.             break;
  312.         case 1:
  313.             for(int i=0;i<msg.byteCount;i++)
  314.             {
  315.                 Serial1.write(msg.inputs[i]);
  316.             }
  317.            
  318.             break;
  319.         default:
  320.         {
  321.             break;
  322.         }
  323.     }  
  324. }
  325.  
  326. void updateHeartLED()
  327. {
  328.     lastHeartMsg = millis();
  329.    
  330.     digitalWrite(LEDPin, HIGH);
  331. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!