msgbroker fifo

Author: qqqlab

src/imu/imu.h

@@ -90,7 +90,7 @@ class Imu : public ImuState {
   public:
     ImuConfig config;
     ImuGizmo *gizmo = nullptr;
-    MsgTopic<ImuState> topic = MsgTopic<ImuState>("imu");
+    MsgTopic<ImuState> topic = MsgTopic<ImuState>("imu", 10); //10-deep fifo
 
     int setup(); // Use config to setup gizmo, returns 0 on success, or error code
     bool update(); // Returns true if state was updated

src/tbx/MsgBroker.cpp

@@ -83,6 +83,11 @@ void MsgBroker::reset_stats() {
 //=============================================================================
 // MsgTopicBase
 //=============================================================================
+MsgTopicBase::MsgTopicBase(const char* name) {
+  strncpy(this->name, name, sizeof(this->name) - 1);
+  this->name[sizeof(this->name) - 1] = 0;
+}
+
 void MsgTopicBase::add_subscription(MsgSubscriptionBase *sub) {
   for(int i = 0; i < MF_MSGSUB_LIST_SIZE; i++) {
     if(!sub_list[i]) {
@@ -114,9 +119,6 @@ int MsgTopicBase::subscriber_count() {
 //=============================================================================
 // MsgSubscriptionBase
 //=============================================================================
-bool MsgSubscriptionBase::updated() {
-    return (generation != topic->generation);
-}
 
 MsgSubscriptionBase::MsgSubscriptionBase(const char *name, MsgTopicBase *topic) : topic{topic} {
     strncpy(this->name, name, sizeof(this->name) - 1);
@@ -128,16 +130,29 @@ MsgSubscriptionBase::~MsgSubscriptionBase() {
     topic->remove_subscription(this);
 }
 
+
+//returns true if new msg available
+bool MsgSubscriptionBase::updated() { 
+  return (generation != topic->generation);
+}
+
 //pull message: returns true if msg was pulled, returns false if no msg available
 bool MsgSubscriptionBase::pull(void *msg) {
-    if(!topic->pull(msg)) return false;
-    generation = topic->generation;
+    if(!topic->pull(msg, &generation)) return false;
     stat_pull_cnt++;
-    return true; //NOTE: retrieved msg might be older than this->generation
+    return true;
 }
 
 //pull updated message: returns true when updated msg available, else returns false and does not update msg 
 bool MsgSubscriptionBase::pull_updated(void *msg) {
     if(!updated()) return false;
     return pull(msg);
 }
+
+//pull last message: returns true if msg was pulled, returns false if no msg available
+bool MsgSubscriptionBase::pull_last(void *msg) {
+    generation = topic->generation - 1;
+    if(!topic->pull(msg, &generation)) return false;
+    stat_pull_cnt++;
+    return true;
+}

src/tbx/MsgBroker.h

@@ -74,12 +74,10 @@ class MsgTopicBase {
     MsgSubscriptionBase* sub_list[MF_MSGSUB_LIST_SIZE] = {};
 
     virtual ~MsgTopicBase() {}
-    MsgTopicBase(const char* name) {
-      strncpy(this->name, name, sizeof(this->name) - 1);
-      this->name[sizeof(this->name) - 1] = 0;
-    }
+    MsgTopicBase(const char* name);
 
-    virtual bool pull(void* msg) = 0;
+    //pull next message with gen
+    virtual bool pull(void* msg, uint32_t *subscriber_gen) = 0;
 
     //subscription
     void add_subscription(MsgSubscriptionBase *sub);
@@ -88,39 +86,62 @@ class MsgTopicBase {
 };
 
 //=============================================================================
-//single publisher, multi subscriber - double buffer for storage
+//single publisher, multi subscriber - lockless - uses FIFO buffer for storage
 template <class T>
 class MsgTopic : public MsgTopicBase {
+  private:
+    uint16_t msglen = 0; //message length
+    uint16_t buflen = 0; //message length rounded up to next 4 bytes for alignment
+    uint16_t bufdepth_1 = 0; //size of buffer in messages minus 1 (E.g. exclusive one message used for loading new data)
+    uint8_t *buf = nullptr; //buffer for bufdepth messages
+    uint8_t *buflast = nullptr; //pre-calculated pointer to last message
+    uint8_t *bufin = nullptr; //next publish message buffer 
+  
   public:
-    MsgTopic(const char* name) : MsgTopicBase(name) {
-      buflen = sizeof(T);
-      active_buf = aligned_alloc(4, buflen);
-      inactive_buf = aligned_alloc(4, buflen);
-      memset(active_buf, 0, buflen);
+    MsgTopic(const char* name, uint16_t fifo_depth = 1) : MsgTopicBase(name) {
+      bufdepth_1 = fifo_depth;
+      msglen = sizeof(T);
+      buflen = ((msglen + 3) / 4) * 4; //round up to the next 4 bytes
+      buf = (uint8_t*)aligned_alloc(4, buflen * (bufdepth_1 + 1)); //add one extra message for loading new data
+      memset(buf, 0, buflen * (bufdepth_1 + 1)); //clear buffer (not really needed)
+      bufin = buf + buflen;
+      buflast = buf + bufdepth_1 * buflen;
+
       MsgBroker::add_topic(this);
     }
 
     void publish(T *msg) { 
-      memcpy(inactive_buf, msg, buflen);
-      void *temp = active_buf;
-      active_buf = inactive_buf;
-      inactive_buf = temp;
-      generation++;
+      memcpy(bufin, msg, buflen); //use buflen for speed (reading potentially some garbage at the end of msg)
+      generation++; //update as soon as data is in buf
+      if(bufin < buflast) bufin += buflen; else bufin = buf; //calc next bufin
     }
 
-    bool pull(void* msg) override {
+    //pull message subscriber_gen+1 from fifo buffer, if not found then pull closest gen in fifo buffer
+    bool pull(void* msg, uint32_t *subscriber_gen) override {
       if(!generation) return false; //will miss a pull every 4,000,000,000 publishes...
-      memcpy(msg, active_buf, buflen);
-      return true;
+      uint8_t tries = 5;
+      do {
+        uint32_t topic_gen = generation; //copy the current topic generation (generation is volatile)
+        uint32_t depth = topic_gen - (*subscriber_gen + 1); //we're interested in the message at this depth
+        //limit depth to [0 .. bufdepth_1 - 1]
+        if(depth & 0x80000000) { //unsigned negative
+          depth = 0;
+        } else if(depth >= bufdepth_1) {
+          depth = bufdepth_1 - 1; 
+        }
+        uint8_t* bufout = buf + ((topic_gen - depth) % (bufdepth_1 + 1)) * buflen; //get pointer to message at this depth
+        memcpy(msg, bufout, msglen); //copy the message
+        if((generation - topic_gen) < (bufdepth_1 - depth)) { //if the message at depth did not get overwitten by publish(), then we have a consistent copy
+          *subscriber_gen = topic_gen - depth; //update the subscriber generation
+          return true;
+        }
+        //the message got updated while we were memcpy'ing it, try again...
+        //note: maybe increase subscriber_gen to look for next message and hopefully have a greater change of getting it
+      }while(--tries);
+      return false;
     }
-
-  private:
-    int buflen = 0;
-    void *active_buf = nullptr;
-    void *inactive_buf = nullptr;
 };
 
-
 //=============================================================================
 //multi publisher, multi subscriber - uses FreeRTOS Queue for storage
 template <class T>
@@ -143,8 +164,20 @@ class MsgTopicQueue : public MsgTopicBase {
       generation++;
     }
 
-    bool pull(void *msg) override {
-      return (xQueuePeek(queue, msg, 0) == pdPASS);
+    bool pull(void* msg, uint32_t *subscriber_gen) override {
+      uint8_t tries = 5;
+      uint32_t topic_gen;
+      do {
+        topic_gen = generation; //copy the current topic generation (generation is volatile)
+        if(xQueuePeek(queue, msg, 0) != pdPASS) return false; //exit if nothing in the queue
+        if(topic_gen == generation) { //if generation did not change, then we got the message for that generation
+          *subscriber_gen = topic_gen; //update the subscriber generation
+          return true;
+        }
+      }while(--tries);
+      //give up, return guess of retrieved generation
+      *subscriber_gen = topic_gen; //update the subscriber generation
+      return true;
     }
 
   private:
@@ -155,6 +188,7 @@ class MsgTopicQueue : public MsgTopicBase {
 class MsgSubscriptionBase {
   public:
     bool updated(); //returns true if new msg available
+
   protected:
     friend class MsgBroker;
     template <class T> friend class MsgSubscription;
@@ -168,6 +202,7 @@ class MsgSubscriptionBase {
 
     bool pull(void *msg); //pull message: returns true if msg was pulled, returns false if no msg available
     bool pull_updated(void *msg); //pull updated message: returns true when updated msg available, else returns false and does not update msg
+    bool pull_last(void *msg); //pull last message: returns true if msg was pulled, returns false if no msg available
 
   private:
     MsgSubscriptionBase() {}
@@ -180,5 +215,6 @@ class MsgSubscription : public MsgSubscriptionBase {
   public:
     MsgSubscription(const char* name, MsgTopicBase *topic) : MsgSubscriptionBase(name, topic) {}
     bool pull(T *msg) { return MsgSubscriptionBase::pull(msg); }
-    bool pull_updated(T *msg) { return MsgSubscriptionBase::pull_updated(msg); }  
+    bool pull_updated(T *msg) { return MsgSubscriptionBase::pull_updated(msg); }
+    bool pull_last(T *msg) { return MsgSubscriptionBase::pull_last(msg); }
 };