protocol.cpp

00001 #include "protocol.h"
00002 #include "toolbox.hpp"
00003 
00004 char newDataAvailable = 0;
00005 
00006 //Bytewise XOR checksum
00007 char calculateChecksum(char command, char* payload, char payloadLength) //Use only when receiving frames
00008 {
00009     char checksum = 0xff;
00010     checksum ^= command;
00011     for (int i = 0; i < payloadLength; i++)
00012         checksum ^= payload[i];
00013     return checksum;
00014 }
00015 
00016 char calculateChecksum(char *msg) //Use only when emitting speed frames
00017 {
00018     int i = 0;
00019     char checksum = msg[i++];
00020     for (; i < 5; i++)
00021         checksum ^= msg[i];
00022     return checksum;
00023 }
00024 
00025 void encodeMessage(char *msg, float vitesse)
00026 {
00027     msg[0] = 0xff;
00028     convertFloatToBytes(&vitesse, msg, 1);
00029     msg[5] = calculateChecksum(msg); 
00030 }
00031 
00032 char receiveState = STATE_START_OF_FRAME;
00033 char receiveIndex = 0;
00034 char receiveIndexLimit;
00035 char receivedCommand;
00036 char receivedPayload[PAYLOAD_MAX_SIZE] = {0};
00037 
00038 char verifiedPayload[PAYLOAD_MAX_SIZE] = {0};
00039 char verifiedIndexLimit;
00040 char verifiedCommand;
00041 
00042 //Decode the bytes received according to their order of reception.
00043 void decodeMessage(char c)
00044 {
00045     switch(receiveState)
00046     {
00047         case STATE_START_OF_FRAME:
00048             //Checking if we received the start Byte
00049             if (c == 0xff)
00050             {
00051                 receiveState = STATE_COMMAND_TYPE;
00052                 receiveIndex = 0;
00053             }
00054             break;
00055             
00056         case STATE_COMMAND_TYPE:
00057             receivedCommand = c;
00058             receiveState = STATE_PAYLOAD;
00059             switch(receivedCommand)
00060             {
00061                 case COMMAND_PWM:
00062                     receiveIndexLimit = 4;
00063                     break;
00064                     
00065                 case COMMAND_ASSERVISSEMENT:
00066                     receiveIndexLimit = 6;
00067                     break;
00068                     
00069                 case COMMAND_PARAMETRES:
00070                     receiveIndexLimit = 8;
00071                     break;
00072                 
00073                 default:
00074                     receiveState = STATE_START_OF_FRAME;
00075                     break;
00076             }
00077             break;
00078             
00079         case STATE_PAYLOAD:
00080             receivedPayload[receiveIndex++] = c;
00081             if (receiveIndex >= receiveIndexLimit)
00082                 receiveState = STATE_CHECKSUM;
00083             break;
00084                 
00085         case STATE_CHECKSUM:
00086             //Checksum
00087             if (c == calculateChecksum(receivedCommand, receivedPayload, receiveIndexLimit))
00088             {
00089                 //The frame is correct, we can use the values retrieved.
00090                 for (int i = 0; i < receiveIndexLimit; i++)
00091                     verifiedPayload[i] = receivedPayload[i];
00092                 verifiedIndexLimit = receiveIndexLimit;
00093                 verifiedCommand = receivedCommand;
00094                 newDataAvailable = 1;
00095             }
00096             receiveState = STATE_START_OF_FRAME;
00097             break;
00098             
00099         default:
00100             receiveState = STATE_START_OF_FRAME;
00101             break;
00102     }
00103 }
00104 
00105 //Return the values decoded
00106 char getVerifiedPayload(char *payloadOutput)
00107 {
00108     for (int i = 0; i < verifiedIndexLimit; i++)
00109         payloadOutput[i] = verifiedPayload[i];
00110     newDataAvailable = 0;
00111     return verifiedCommand;
00112 }
00113 
00114 //Return a value telling if new informations are available.
00115 char isDataAvailable()
00116 {
00117     return newDataAvailable;
00118 }