Diff: protocol.cpp

Revision:

110:a6d1d3525014

Child:

111:f11575e7c79b

diff -r 4ee7ffc8f175 -r a6d1d3525014 protocol.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/protocol.cpp	Thu Apr 07 12:13:04 2022 +0000
@@ -0,0 +1,88 @@
+#include "protocol.h"
+
+char newDataAvailable = 0;
+
+char calculateChecksum(uint32_t propulsion, uint32_t direction)
+{
+    char checksum = 0xff;
+    checksum ^= propulsion >> 8;
+    checksum ^= propulsion & (0b11111111);
+    checksum ^= direction >> 8;
+    checksum ^= direction & (0b11111111);
+    
+    return checksum;
+}
+
+char receiveState = STATE_START_OF_FRAME;
+char receiveIndex = 0;
+uint32_t receivedPropulsion;
+uint32_t receivedDirection;
+
+uint32_t verifiedPropulsion = 1500;
+uint32_t verifiedDirection = 1150;
+
+void decodeMessage(char c)
+{
+    switch(receiveState)
+    {
+        case STATE_START_OF_FRAME:
+            //Checking if we received the start Byte
+            if (c == 0xff)
+            {
+                receiveState = STATE_PROPULSION;
+                receiveIndex = 0;
+            }
+            break;
+            
+        case STATE_PROPULSION:
+            //Retrieving the propulsion PWM bytes (1. MSB; 2. LSB)
+            if (receiveIndex++ == 0)
+                receivedPropulsion = c << 8;
+            else
+            {
+                receivedPropulsion += c;
+                receiveState = STATE_DIRECTION;
+                receiveIndex = 0;
+            }
+            break;
+            
+        case STATE_DIRECTION:
+            //Retrieving the direction PWM bytes (1. MSB; 2. LSB)
+            if (receiveIndex++ == 0)
+                receivedDirection = c << 8;
+            else
+            {
+                receivedDirection += c;
+                receiveState = STATE_CHECKSUM;
+            }
+            break;
+            
+        case STATE_CHECKSUM:
+            //Checksum
+            if (c == calculateChecksum(receivedPropulsion, receivedDirection))
+            {
+                //Le message recu est correct, on peut exploiter ses données
+                verifiedPropulsion = receivedPropulsion;
+                verifiedDirection = receivedDirection;
+                newDataAvailable = 1;
+            }
+            receiveState = STATE_START_OF_FRAME;
+            break;
+            
+        default:
+            receiveState = STATE_START_OF_FRAME;
+            break;
+    }
+}
+
+void getVerifiedPWMValues(uint32_t *pwmPropulsion, uint32_t *pwmDirection)
+{
+    *pwmPropulsion = verifiedPropulsion;
+    *pwmDirection = verifiedDirection;
+    newDataAvailable = 0;
+}
+
+char isDataAvailable()
+{
+    return newDataAvailable;
+}