Added I2C support for raspi & first rework of debugging

include/sta/bus/can/subscribable.tpp

@@ -0,0 +1,120 @@
+/**
+ * @brief Implementation of template class CanController<T>.
+ */
+#ifndef STA_CORE_CAN_SUBSCRIBABLE_TPP
+#define STA_CORE_CAN_SUBSCRIBABLE_TPP
+
+#ifndef STA_CORE_CAN_SUBSCRIBABLE_HPP
+#error "Direct use of internal header. Use <sta/can/subscribable.hpp> instead"
+#endif // !STA_CORE_CAN_SUBSCRIBABLE_HPP
+
+#ifndef STA_STDLIB_DISABLE
+#  include <algorithm> // fill_n
+#endif // !STA_STDLIB_DISABLE
+
+
+namespace sta
+{
+    template <typename T>
+    bool SubscribableCanController<T>::subscribe(const CanFilterConfig * subscriptions, uint8_t num)
+    {
+        // Check bounds
+        if (num > T::MAX_FILTER_COUNT)
+            return false;
+
+        // Clear previous subscriptions
+        unsubscribeAll();
+
+        for (uint8_t i = 0; i < num; ++i)
+        {
+            // Save handler callback
+            filterCallbacks_[i] = subscriptions[i].callback;
+
+            // Configure and enable filter
+            T::configureFilter(i, subscriptions[i].filter, true);
+        }
+
+        return true;
+    }
+
+    template <typename T>
+    void SubscribableCanController<T>::subscribeAll(CanRxCallback callback, uint8_t fifo)
+    {
+        uint8_t filterIdx = 0;
+
+        // Clear previous subscriptions
+        unsubscribeAll();
+
+        // Setup default filter
+        CanFilter filter{};
+        filter.type = CanFilterIdFormat::ANY;
+        filter.fifo = fifo;
+
+        // Store callback
+        filterCallbacks_[filterIdx] = callback;
+
+        // Configure and enable default filter
+        T::configureFilter(filterIdx, filter, true);
+    }
+
+    template <typename T>
+    void SubscribableCanController<T>::unsubscribeAll()
+    {
+        // Disable all filters
+        T::clearFilters();
+
+        // Clear filter callbacks
+#ifndef STA_STDLIB_DISABLE
+        std::fill_n(filterCallbacks_, T::MAX_FILTER_COUNT, nullptr);
+#else // STA_STDLIB_DISABLE
+        for (uint8_t i = 0; i < T::MAX_FILTER_COUNT; ++i)
+        {
+            filterCallbacks_[i] = nullptr;
+        }
+#endif // STA_STDLIB_DISABLE
+    }
+
+    template <typename T>
+    void SubscribableCanController<T>::receiveAndNotify(uint8_t fifo)
+    {
+        CanRxHeader header;
+        uint8_t payload[T::MAX_PAYLOAD_SIZE];
+
+        if (T::receiveFrame(fifo, &header, payload))
+        {
+            //displayFrameUART(frame);
+
+            // Forward frame to filter callback
+            if (fifo <= T::MAX_FILTER_COUNT && filterCallbacks_[header.filter])
+            {
+                filterCallbacks_[header.filter](header, payload);
+            }
+        }
+    }
+
+    template <typename T>
+    void SubscribableCanController<T>::processMessages()
+    {
+        // Read RX interrupt flags
+        uint32_t RFIF = T::getRxFifoFlags();
+
+        if (RFIF != 0)
+        {
+            // Check all flags
+            for (uint8_t fifo = 0; fifo < T::MAX_FIFO_COUNT; ++fifo)
+            {
+                // Process messages if flag is set
+                if (RFIF & 0x1)
+                {
+                    receiveAndNotify(fifo);
+                }
+
+                // Shift next RX flag to LSB
+                RFIF >>= 1;
+            }
+        }
+    }
+} // namespace sta
+
+
+#endif // STA_CORE_CAN_SUBSCRIBABLE_TPP