.
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Jul 14 2022 22:49:30 by
1.7.2