Peripherals/Inc/peripherals_imu_icm.hpp

@@ -72,17 +72,17 @@ public:
                                   // rate = 1KHz, temp filter = 42
     hal::delay(10);
 
-    const std::uint8_t new_gyro_conf = (std::uint32_t(scale_gyro_) << 3);
+    const std::uint8_t new_gyro_conf =  (std::uint32_t(scale_gyro_) << 3);
     registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
-                            new_gyro_conf);
+        new_gyro_conf);
 
-    const std::uint8_t new_accel_config = (std::uint32_t(scale_accel_) << 3);
+    const std::uint8_t new_accel_config =  (std::uint32_t(scale_accel_) << 3);
     registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
-                            new_accel_config);
+        new_accel_config);
 
-    //    setGyroscopeScale(scale_gyro_);
+//    setGyroscopeScale(scale_gyro_);
-    //
+//
-    //    setAccelerometerScale(scale_accel_);
+//    setAccelerometerScale(scale_accel_);
 
     // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
     registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK,

Threads/Inc/threads_actor.hpp

@@ -15,15 +15,15 @@
 #include <type_traits>
 #include <variant>
 
-#include <threads_actor_thread.hpp>
+#include "threads_primitivethread.hpp"
-#include <threads_iactor.hpp>
+#include "threads_signal.hpp"
-#include <threads_signal.hpp>
+#include "threads_timer.hpp"
 
 namespace Threads
 {
 
 template<typename CRTP, typename... Ts>
-class Actor : public IActor
+class Actor
 {
 public:
   using value_type = std::variant<Ts...>;
@@ -31,11 +31,14 @@ public:
   static_assert(std::is_default_constructible_v<value_type>, "Ts must be default constructible.");
   static_assert(std::is_trivially_copyable_v<value_type>, "Ts must be trivially copyable.");
 
-  Actor() : IActor(sizeof(value_type)),
+  Actor(const std::uint32_t queue_size, const char* name, const osPriority_t priority, const std::uint32_t stack_size)
-            signal_(this)
+      : thread_(&Actor<CRTP, Ts...>::threadHandler_, this, name, priority, stack_size),
-  {}
+        msg_queue_(xQueueCreate(queue_size, sizeof(value_type))), signal_(this)
+  {
+    assert(msg_queue_);
+  }
 
-  ~Actor() override {}
+  virtual ~Actor() {}
 
   template<typename T>
   struct Signal : Signallable<T>
@@ -56,13 +59,17 @@ public:
     void emit(const T& data) override
     {
       parent::value_type to_send = data;
-      q_->dispatchEvent(to_send);
+      xQueueSend(q_->msg_queue_, &to_send, 0);
     }
 
     BaseType_t emitFromIsr(const T& data) override
     {
       parent::value_type to_send = data;
-      return q_->dispatchEventFromIsr(to_send);
+      BaseType_t was_awoken = pdFALSE;
+
+      xQueueSendFromISR(q_->msg_queue_, &to_send, &was_awoken);
+
+      return was_awoken;
     }
 
   private:
@@ -90,12 +97,13 @@ public:
     return &signal_;
   }
 
-  void dispatchSignal(const char* data) final
+protected:
-  {
+  virtual void init() = 0;
-    dispatchImpl_(data);
-  }
 
 private:
+  PrimitiveThread thread_;
+  QueueHandle_t msg_queue_;
+
   struct Visitor_
   {
     using parent = Actor<CRTP, Ts...>;
@@ -113,15 +121,113 @@ private:
     }
   };
 
-  void dispatchImpl_(const char* data)
+  void operator()()
   {
-    const value_type* signal_data = reinterpret_cast<const value_type*>(data);
+    init();
-    std::visit(Visitor_{this}, *signal_data);
+
+    value_type data{};
+
+    while (true)
+    {
+      const BaseType_t got = xQueueReceive(msg_queue_, &data, portMAX_DELAY);
+
+      if (got == pdTRUE)
+      {
+        std::visit(Visitor_{this}, data);
+      }
+    }
+  }
+
+  static void threadHandler_(void* d)
+  {
+    auto* dd = static_cast<Actor<CRTP, Ts...>*>(d);
+    (*dd)();
   }
 
   Signal<value_type> signal_;
 };
 
+template<typename CRTP>
+class Actor<CRTP, void>
+{
+public:
+  using value_type = void;
+
+  Actor(const std::uint32_t queue_size, const char* name, const osPriority_t priority, const std::uint32_t stack_size)
+      : thread_([](void* d) {
+          auto* dd = static_cast<Actor<CRTP, value_type>*>(d);
+          (*dd)();
+        },
+                this, name, priority, stack_size),
+        msg_queue_(xQueueCreate(queue_size, sizeof(int))), signal_(*this)
+  {
+    assert(msg_queue_);
+  }
+
+  virtual ~Actor() {}
+
+  Signallable<value_type>* getSignal()
+  {
+    return &signal_;
+  }
+
+protected:
+  virtual void init() = 0;
+
+private:
+  PrimitiveThread thread_;
+  QueueHandle_t msg_queue_;
+
+  void operator()()
+  {
+    init();
+
+    int data;
+
+    while (true)
+    {
+      const BaseType_t got = xQueueReceive(msg_queue_, &data, portMAX_DELAY);
+
+      if (got == pdTRUE)
+      {
+        auto* crtp = static_cast<CRTP*>(this);
+        crtp->onSignal();
+      }
+    }
+  }
+
+  friend struct Signal_;
+
+  struct Signal_ : Signallable<value_type>
+  {
+    using parent = Actor<CRTP, void>;
+    parent& q;
+
+    explicit Signal_(parent& q)
+        : q(q)
+    {}
+
+    void emit() override
+    {
+      const int data = 0;
+
+      xQueueSend(q.msg_queue_, &data, 0);
+    }
+
+    BaseType_t emitFromIsr() override
+    {
+      const int data = 0;
+
+      BaseType_t was_awoken = pdFALSE;
+      xQueueSendFromISR(q.msg_queue_, &data, &was_awoken);
+
+      return was_awoken;
+    }
+  };
+
+  Signal_ signal_;
+};
+
 }// namespace Threads
 
 

Threads/Inc/threads_actor_thread.hpp

@@ -1,50 +0,0 @@
-/*
- * threads_actor_thread.hpp
- *
- *  Created on: Nov 13, 2021
- *      Author: erki
- */
-
-#ifndef SKULLC_THREADS_ACTOR_THREAD_HPP_
-#define SKULLC_THREADS_ACTOR_THREAD_HPP_
-
-#include <array>
-#include <cstdint>
-
-#include <freertos_os2.h>
-#include <queue.h>
-
-#include <threads_thread.hpp>
-
-namespace Threads
-{
-
-class IActor;
-
-class ActorThread : public Thread<ActorThread>
-{
-public:
-  ActorThread(const std::size_t queue_length, const char* name, const osPriority_t priority, const std::uint32_t stack_size);
-
-  void operator()();
-
-private:
-  void init_();
-  void doMessageLoop_();
-
-  friend class IActor;
-  std::size_t acceptActor_(IActor* actor);
-
-  std::size_t queue_length_;
-  std::size_t queue_item_length_ = 0;
-
-  QueueHandle_t queue_ = nullptr;
-  char* rx_buffer_ = nullptr;
-  char* tx_buffer_ = nullptr;
-  std::array<IActor*, 32> actors_;
-};
-
-}// namespace Threads
-
-
-#endif /* SKULLC_THREADS_ACTOR_THREAD_HPP_ */

Threads/Inc/threads_iactor.hpp

@@ -1,68 +0,0 @@
-/*
- * threads_iactor.hpp
- *
- *  Created on: Nov 14, 2021
- *      Author: erki
- */
-
-#ifndef SKULLC_THREADS_IACTOR_HPP_
-#define SKULLC_THREADS_IACTOR_HPP_
-
-#include <cstdint>
-
-#include <freertos_os2.h>
-
-namespace Threads
-{
-
-class ActorThread;
-
-class IActor
-{
-public:
-  IActor(const std::size_t signal_size)
-      : signal_size_(signal_size)
-  {}
-
-  virtual ~IActor() {}
-
-  virtual void init() = 0;
-  virtual void dispatchSignal(const char* data) = 0;
-
-  void moveToThread(ActorThread* thread);
-
-  std::size_t actorIndex() const
-  {
-    return actor_index_;
-  }
-
-  std::size_t signalSize() const
-  {
-    return signal_size_;
-  }
-
-protected:
-  template<typename T>
-  void dispatchEvent(const T& data)
-  {
-    dispatchEventImpl_(&data, sizeof(T));
-  }
-
-  template<typename T>
-  BaseType_t dispatchEventFromIsr(const T& data)
-  {
-    return dispatchEventFromIsrImpl_(&data, sizeof(T));
-  }
-
-private:
-  ActorThread* parent_thread_ = nullptr;
-  std::size_t actor_index_ = 0;
-  std::size_t signal_size_;
-
-  void dispatchEventImpl_(const void* data, const std::size_t data_size);
-  BaseType_t dispatchEventFromIsrImpl_(const void* data, const std::size_t data_size);
-};
-
-}// namespace Threads
-
-#endif /* SKULLC_THREADS_IACTOR_HPP_ */

Threads/Src/threads_actor_thread.cpp

@@ -1,110 +0,0 @@
-/*
- * threads_actor_thread.cpp
- *
- *  Created on: Nov 13, 2021
- *      Author: erki
- */
-
-
-#include "threads_actor_thread.hpp"
-
-#include <algorithm>
-#include <cassert>
-#include <cstring>
-
-#include <threads_iactor.hpp>
-
-namespace
-{
-
-struct SignalMessage
-{
-  std::size_t actor_index;
-  const char* data;
-
-  static SignalMessage fromData(char* data)
-  {
-    SignalMessage message;
-    std::memcpy(&message.actor_index, data, sizeof(message.actor_index));
-    message.data = data + sizeof(message.actor_index);
-
-    return message;
-  }
-};
-
-}// namespace
-
-namespace Threads
-{
-
-ActorThread::ActorThread(const std::size_t queue_length, const char* name, const osPriority_t priority, const std::uint32_t stack_size)
-    : Thread<ActorThread>(name, priority, stack_size), queue_length_(queue_length), queue_(nullptr)
-{
-  actors_.fill(nullptr);
-}
-
-void ActorThread::operator()()
-{
-  init_();
-
-  osThreadYield();
-
-  while (true)
-  {
-    doMessageLoop_();
-
-    osThreadYield();
-  }
-}
-
-void ActorThread::init_()
-{
-  for (IActor* actor : actors_)
-  {
-    if (actor != nullptr)
-    {
-      queue_item_length_ = std::max(queue_item_length_, actor->signalSize());
-
-      actor->init();
-    }
-  }
-
-  queue_ = xQueueCreate(queue_length_, queue_item_length_ + sizeof(std::size_t));
-  assert(queue_ != nullptr);
-
-  rx_buffer_ = new char[queue_item_length_ + sizeof(std::size_t)];
-  tx_buffer_ = new char[queue_item_length_ + sizeof(std::size_t)];
-}
-
-void ActorThread::doMessageLoop_()
-{
-  const BaseType_t has_data = xQueueReceive(queue_, rx_buffer_, portMAX_DELAY);
-
-  if (has_data == pdTRUE)
-  {
-    const SignalMessage message = SignalMessage::fromData(rx_buffer_);
-    assert(message.actor_index <= actors_.size());
-
-    IActor* actor = actors_[message.actor_index];
-    assert(actor != nullptr);
-
-    actor->dispatchSignal(message.data);
-  }
-}
-
-std::size_t ActorThread::acceptActor_(IActor* actor)
-{
-  for (auto it = actors_.begin(); it != actors_.end(); ++it)
-  {
-    if (*it == nullptr)
-    {
-      *it = actor;
-      return std::distance(actors_.begin(), it);
-    }
-  }
-
-  assert(false);
-  return -1;
-}
-
-}// namespace Threads

Threads/Src/threads_iactor.cpp

@@ -1,55 +0,0 @@
-/*
- * threads_iactor.cpp
- *
- *  Created on: Nov 14, 2021
- *      Author: erki
- */
-
-
-#include "threads_iactor.hpp"
-
-#include <cstring>
-
-#include <queue.h>
-
-#include "threads_actor_thread.hpp"
-
-namespace Threads
-{
-
-void IActor::moveToThread(ActorThread* thread)
-{
-  actor_index_ = thread->acceptActor_(this);
-  parent_thread_ = thread;
-}
-
-void IActor::dispatchEventImpl_(const void* data, const std::size_t data_size)
-{
-  taskENTER_CRITICAL();
-
-  std::memcpy(parent_thread_->tx_buffer_, &actor_index_, sizeof(std::size_t));
-  char* data_dest = parent_thread_->tx_buffer_ + sizeof(std::size_t);
-  std::memcpy(data_dest, data, data_size);
-
-  xQueueSend(parent_thread_->queue_, parent_thread_->tx_buffer_, 0);
-
-  taskEXIT_CRITICAL();
-}
-
-BaseType_t IActor::dispatchEventFromIsrImpl_(const void* data, const std::size_t data_size)
-{
-  const auto isr_data = taskENTER_CRITICAL_FROM_ISR();
-
-  std::memcpy(parent_thread_->tx_buffer_, &actor_index_, sizeof(std::size_t));
-  char* data_dest = parent_thread_->tx_buffer_ + sizeof(std::size_t);
-  std::memcpy(data_dest, data, data_size);
-
-  BaseType_t task_awoken = pdFALSE;
-  xQueueSendFromISR(parent_thread_->queue_, parent_thread_->tx_buffer_, &task_awoken);
-
-  taskEXIT_CRITICAL_FROM_ISR(isr_data);
-
-  return task_awoken;
-}
-
-}// namespace Threads