protocol.cpp

00001 #include "protocol.h"
00002 
00003 char newDataAvailable = 0;
00004 
00005 //Bytewise XOR
00006 char calculateChecksum(uint32_t propulsion, uint32_t direction)
00007 {
00008     char checksum = 0xff;
00009     checksum ^= propulsion >> 8;
00010     checksum ^= propulsion & (0b11111111);
00011     checksum ^= direction >> 8;
00012     checksum ^= direction & (0b11111111);
00013     
00014     return checksum;
00015 }
00016 
00017 char receiveState = STATE_START_OF_FRAME;
00018 char receiveIndex = 0;
00019 uint32_t receivedPropulsion;
00020 uint32_t receivedDirection;
00021 
00022 uint32_t verifiedPropulsion = 1500;
00023 uint32_t verifiedDirection = 1150;
00024 
00025 //Decode the bytes received according to their order of reception.
00026 void decodeMessage(char c)
00027 {
00028     switch(receiveState)
00029     {
00030         case STATE_START_OF_FRAME:
00031             //Checking if we received the start Byte
00032             if (c == 0xff)
00033             {
00034                 receiveState = STATE_PROPULSION;
00035                 receiveIndex = 0;
00036             }
00037             break;
00038             
00039         case STATE_PROPULSION:
00040             //Retrieving the propulsion PWM bytes (1. MSB; 2. LSB)
00041             if (receiveIndex++ == 0)
00042                 receivedPropulsion = c << 8;
00043             else
00044             {
00045                 receivedPropulsion += c;
00046                 receiveState = STATE_DIRECTION;
00047                 receiveIndex = 0;
00048             }
00049             break;
00050             
00051         case STATE_DIRECTION:
00052             //Retrieving the direction PWM bytes (1. MSB; 2. LSB)
00053             if (receiveIndex++ == 0)
00054                 receivedDirection = c << 8;
00055             else
00056             {
00057                 receivedDirection += c;
00058                 receiveState = STATE_CHECKSUM;
00059             }
00060             break;
00061             
00062         case STATE_CHECKSUM:
00063             //Checksum
00064             if (c == calculateChecksum(receivedPropulsion, receivedDirection))
00065             {
00066                 //The frame is correct, we can use the values retrieved.
00067                 verifiedPropulsion = receivedPropulsion;
00068                 verifiedDirection = receivedDirection;
00069                 newDataAvailable = 1;
00070             }
00071             receiveState = STATE_START_OF_FRAME;
00072             break;
00073             
00074         default:
00075             receiveState = STATE_START_OF_FRAME;
00076             break;
00077     }
00078 }
00079 
00080 //Return the values decoded
00081 void getVerifiedPWMValues(uint32_t *pwmPropulsion, uint32_t *pwmDirection)
00082 {
00083     *pwmPropulsion = verifiedPropulsion;
00084     *pwmDirection = verifiedDirection;
00085     newDataAvailable = 0;
00086 }
00087 
00088 //Return a value telling if new informations are available.
00089 char isDataAvailable()
00090 {
00091     return newDataAvailable;
00092 }