Current stage of the communication

The current version of the communication

Arduino/RPi-Communication/PBUtils.cpp

@@ -0,0 +1,185 @@
+/*=============================================================================================================================
+Author: lefake
+Date: April 2020
+==============================================================================================================================*/
+#include "PBUtils.h"
+
+/*
+ * Constructor of the class
+ * 
+ * @param topics: Array of all the topics containing their type
+ * @param nbs_topics: Number of topic in the array
+ */
+PBUtils::PBUtils(Topic *topics, int nbs_topics)
+{
+  _id_2_type = malloc(sizeof(pb_msgdesc_t *) * nbs_topics);
+  _id_2_msg = malloc(sizeof(void *) * nbs_topics);
+  
+  // Initializie array from the struct so it isn't dependant on array indexes
+  // Hence the id must be positive and can't appear more than once
+  for (int i = 0; i < nbs_topics; ++i)
+  {
+    _id_2_type[topics[i].id] = topics[i].type;
+    _id_2_msg[topics[i].id] = topics[i].msg;
+  }
+}
+
+PBUtils::~PBUtils() 
+{
+  free(_id_2_type);
+  free(_id_2_msg);
+}
+
+/*
+ * Convert a string to a list of PB messages
+ * 
+ * @param input_string: The input str to decode from
+ * @param sub_msg_id: An output list of all the messages id in the full message
+ * @param nbs_new_msgs: The number of sub messages in the fill message
+ * 
+ * @return: if the decode was successful
+ */
+bool PBUtils::decode_pb(char* input_string, int *sub_msg_id, int &nbs_new_msgs)
+{
+  char *sub_msgs[MAX_MSG_LEN];
+  bool success = true;
+  
+  nbs_new_msgs = parse_msg(input_string, sub_msg_id, sub_msgs);
+
+  // Add assert nbs_new_msgs < length of sub_msg_id
+
+  for (int i = 0; i < nbs_new_msgs; ++i)
+  {
+    uint8_t buffer_in[MAX_MSG_LEN];
+    chars2bytes(sub_msgs[i], buffer_in);
+
+    pb_istream_t stream_in = pb_istream_from_buffer(buffer_in, strlen(sub_msgs[i])/2);
+    success = success && pb_decode(&stream_in, _id_2_type[sub_msg_id[i]], _id_2_msg[sub_msg_id[i]]);
+  }
+  
+  return success;
+}
+
+/*
+ * Send protobufs messages to the serial port with format <id|msg;>
+ * 
+ * @param nbs: The numbers of id to send
+ * @param ...: List of all the ids to send 
+ *             ex : cant use it like this pb_send(3, POS, OBS_POS, DEBUG_ARDUINO);
+ * 
+ * @return: If the encode was successful  
+ */
+bool PBUtils::pb_send(int nbs, ...)
+{
+  bool success = true;
+  String to_send_builder = "<";
+  va_list ids_to_send;
+  va_start(ids_to_send, nbs);
+
+  for (int i = 0; i < nbs; ++i)
+  {
+    int id = va_arg ( ids_to_send, int );
+    uint8_t buffer_out[MAX_MSG_LEN];
+    char to_send[10];  // The biggest byte array in the nanopb encode seems to be 10 (should be coded to not depend on it tho...)
+    pb_ostream_t stream = pb_ostream_from_buffer(buffer_out, sizeof(buffer_out));
+    success = success && pb_encode(&stream, _id_2_type[id], _id_2_msg[id]);
+
+    if(success)
+    {
+      to_send_builder += String(id);
+      to_send_builder += "|";
+      
+      for(int j = 0; j < stream.bytes_written; j++)
+      {
+        sprintf (to_send, "%02X", buffer_out[j]);
+        to_send_builder += String(to_send);
+      }
+    }
+    else
+      break;
+    to_send_builder += ";";
+  }
+  to_send_builder += ">";
+  va_end(ids_to_send);
+
+  if (success)
+    Serial.print(to_send_builder);
+  
+  return success;
+}
+
+/*
+ * Convert an input string to a list of PB messages and ids
+ * 
+ * @param in_string: The input string to parse
+ * @param msg_ids: Output a list of id of all the sub messages in the full message
+ * @param out_strings: Output a list of str containting the messages (without id)
+ * 
+ * @return: The number of submessages
+ */
+int PBUtils::parse_msg(char in_string[], int *msg_ids, char **out_strings)
+{
+  // Split all messages
+  char *whole_msgs[MAX_MSG_LEN];
+  int ind = 0;
+  char* substr = strtok(in_string, ";");
+
+  while (substr != NULL)
+  {
+    whole_msgs[ind++] = substr;
+    substr = strtok(NULL, ";");
+  }
+
+  // Split (id, msg) pair
+  for (int i = 0; i < ind; i++)
+  {
+    char* id = strtok(whole_msgs[i], "|");
+    char* msg = strtok(NULL, "|");
+    
+    msg_ids[i] = atoi(id);
+    out_strings[i] = msg;
+  }
+
+  return ind;
+}
+
+/*
+ * Convert a string to a list of uint
+ *
+ * @param in_string: The input string to convert to hex
+ * @param string_values: The output of the conversion (size divided by 2 since 2 HEX is on uint8_t)
+ * 
+ * @return: 
+ */
+void PBUtils::chars2bytes(char* in_string, uint8_t* string_value)
+{
+  // Jai pt fucker dequoi, a retester ...
+  int len = strlen(in_string)/2;
+  for (int i = 0; i < len; ++i)
+    string_value[i] = char2hex(in_string[(i*2)], in_string[(i*2)+1]);
+}
+
+/*
+ * Convert two char (between 00 and FF) to an HEX value 
+ * 
+ * @param in: Array of two char to be converted
+ * 
+ * @return: The decimal value
+ */
+uint8_t PBUtils::char2hex(char in1, char in2)
+{
+  uint8_t val[2];
+  char in[2] = { in1, in2};
+
+  for (int i = 0; i < 2; i++)
+  {
+    if (in[i] >= 'A' && in[i] <= 'F')
+      val[i] = in[i] - 'A' + 10;
+    else if (in[i] >= 'a' && in[i] <= 'f')
+      val[i] = in[i] - 'a' + 10;
+    else
+      val[i] = in[i] - '0';
+  }
+ 
+  return val[0]*16 + val[1];
+}

Arduino/RPi-Communication/PBUtils.h

@@ -0,0 +1,43 @@
+/*=============================================================================================================================
+Author: lefake
+Date: April 2020
+Description: Header file of the nanopb communication protocol 
+             Encode and decode protobuf messages with an id on the serial port
+             The messages are of the form : <id|msg;id|msg;> (can have from one to MAX_NBS_MSG messages)
+==============================================================================================================================*/
+#ifndef PBUTILS_H
+#define PBUTILS_H
+
+#include <Arduino.h>
+#include <pb_common.h>
+#include <pb.h>
+#include <pb_encode.h>
+#include <pb_decode.h>
+
+#define MAX_MSG_LEN                         1000
+#define MAX_NBS_MSG                         10
+
+struct Topic {
+  int id;
+  pb_msgdesc_t* type;
+  void* msg;
+};
+
+class PBUtils
+{
+  public:
+    PBUtils(Topic*, int);
+    ~PBUtils();
+    bool decode_pb(char* , int *, int &);
+    bool pb_send(int, ...);
+
+  private:
+    int parse_msg(char[], int *, char **);
+    void chars2bytes(char* , uint8_t*);
+    uint8_t char2hex(char, char);
+    
+    pb_msgdesc_t** _id_2_type;
+    void** _id_2_msg;
+};
+
+#endif

Arduino/RPi-Communication/RPi-Communication.ino

@@ -0,0 +1,11 @@
+#include "PBUtils.h"
+
+void setup() {
+  // put your setup code here, to run once:
+
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+
+}

RPi/PBUtils.py

@@ -0,0 +1,121 @@
+from binascii import unhexlify
+import threading
+import time
+
+# Serialization utils
+class PBSerializationHandler:
+    def __init__(self, msg_obj):
+        self._msg_obj = msg_obj
+
+    def encode_msgs(self, ids, msgs):
+        msg = "<"
+
+        for id_msg, pb_msg in zip(ids, msgs):
+            msg += str(id_msg) + "|"
+            for byte in bytearray(pb_msg.SerializeToString()):
+                msg += str(hex(byte))[2:].zfill(2)  # Remove \x and fill with 0 in front to always takes 2 digits
+            msg += ";"
+
+        msg += ">"
+        return msg
+
+    def encode_msg(self, id, msg):
+        return self.encode_msgs([id], [msg])
+
+    def deserialize(self, messages):
+        messages = messages.decode("ascii")
+        msg_array = messages[1:-1].split(';')      # Remove < > characters and split sub-msgs
+
+        object_list = []
+
+        for msg in msg_array:
+            if len(msg) > 0:
+                msg_id, raw_msg = msg.split("|")    # Find the id of the message
+                msg_id = int(msg_id)
+                obj = self._msg_obj[msg_id]
+                obj.ParseFromString(unhexlify(raw_msg))
+                object_list.append([msg_id, obj])
+
+        return object_list
+
+
+
+# Serial communication utils
+
+class ArduinoReadHandler(threading.Thread):
+    def __init__(self, sleeptime, readfunc):
+        self._sleeptime = sleeptime
+        self._readfunc = readfunc
+        threading.Thread.__init__(self)
+        self._runflag = threading.Event()
+        self._runflag.clear()
+        self._run = True
+
+    def run(self):
+        self._runflag.set()
+        self.worker()
+
+    def worker(self):
+        while self._run:
+            if self._runflag.is_set():
+                self._readfunc()
+            time.sleep(self._sleeptime)
+
+    def pause(self):
+        self._runflag.clear()
+
+    def resume(self):
+        self._runflag.set()
+
+    def running(self):
+        return self._runflag.is_set()
+
+    def kill(self):
+        self._run = False
+
+
+class PBSerialHandler:
+    def __init__(self, serial, callback, msg_obj, sleeptime=0.01):
+        self._serial = serial
+        self._sleeptime = float(sleeptime)
+        self._callback = callback
+
+        self._interlock = False
+        self._response = None
+
+        self._serialization_handler = PBSerializationHandler(msg_obj)
+        self._worker = ArduinoReadHandler(self._sleeptime, self.read_callback)
+        self._worker.start()
+
+    def kill(self):
+        self._worker.kill()
+
+    def read_callback(self):
+        if not self._interlock:
+            self._interlock = True
+            try:
+                input = self._serial.read()
+                if input == b'<':
+                    buffer = self._serial.read_until(b'>')
+                    self._serial.flush()
+                    self._response = b'<' + buffer
+                    self._callback(self._response)
+            except Exception as e:
+                print("Read call back error " + str(e))
+
+            self._interlock = False
+
+    def write_pb_msg(self, id, msg):
+        self.write_pb_msgs([id], [msg])
+
+    def write_pb_msgs(self, ids, msgs):
+        encoded_msg = self._serialization_handler.encode_msgs(ids, msgs)
+
+        while self._interlock:
+            time.sleep(self._sleeptime)
+
+        self._interlock = True
+        self._serial.write(encoded_msg.encode("ascii"))
+        self._serial.flush()
+        self._interlock = False
+

RPi/Topics.py

@@ -0,0 +1,29 @@
+### Placeholder to store objects during communication
+
+class Topic:
+    def __init__(self, id, name, obj, converter, pub=None):
+        self._id = id
+        self._name = name
+        self._obj = obj
+        self._converter = converter
+        self._pub = pub
+
+    @property
+    def id(self):
+        return self._id
+
+    @property
+    def name(self):
+        return self._name
+
+    @property
+    def obj(self):
+        return self._obj
+
+    @property
+    def converter(self):
+        return self._converter
+
+    @property
+    def pub(self):
+        return self._pub