canard!

fw/.gitignore

@@ -5,3 +5,4 @@
 include/nunavut/*
 include/reg/*
 include/uavcan/*
+compile_commands.json

fw/can_if_up.sh

@@ -6,5 +6,6 @@ rm ~/allocation_table.db
 export UAVCAN__CAN__IFACE="socketcan:can0"                                                         
 export UAVCAN__NODE__ID=127
 export UAVCAN__CAN__MTU=8
+export UAVCAN__CAN__BITRATE=500000
 
 # y mon --plug-and-play ~/allocation_table.db

fw/src/esp32twaican.cpp

@@ -24,8 +24,8 @@ int esp32twaicanOpen(const int pin_tx, const int pin_rx,
 
   twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(
       (gpio_num_t)pin_tx, (gpio_num_t)pin_rx, TWAI_MODE_NORMAL);
-  g_config.rx_queue_len = 30;
+  g_config.rx_queue_len = 100;
-  g_config.tx_queue_len = 30;
+  g_config.tx_queue_len = 100;
   twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
   twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
   if (filter_config) {
@@ -55,7 +55,7 @@ int esp32twaicanOpen(const int pin_tx, const int pin_rx,
     // Serial.println("Failed to reconfigure alerts");
     return ret;
   }
-  digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
+  //digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
 
   return 0;
 }
@@ -72,12 +72,11 @@ int16_t esp32twaicanPush(const struct CanardFrame *const frame,
   message.extd = 1; // extended can id
   message.identifier = frame->extended_can_id;
   message.rtr = 0;
+  message.self = 0;
   message.data_length_code = frame->payload.size;
   //printf(stderr, "size %d  data %d", frame->payload.size, (int)frame->payload.data);
   memcpy(&message.data[0], ((const uint8_t *)frame->payload.data), (int)frame->payload.size);
-    
+  int ret = twai_transmit(&message, (timeout_usec * (1e6 / configTICK_RATE_HZ)));
-  int ret = twai_transmit(&message, (timeout_usec * (1000000 / configTICK_RATE_HZ)));
-    
   if (ret == ESP_OK) {
     return 1;
     // Serial.println("CAN1: Message queued for transmission");
@@ -102,10 +101,10 @@ int16_t esp32twaicanPop(struct CanardFrame *const out_frame,
 
   uint32_t alerts_triggered;
   twai_read_alerts(&alerts_triggered,
-                   (timeout_usec * (1000000 / configTICK_RATE_HZ)));
+                   (timeout_usec * (1e6 / configTICK_RATE_HZ)));
   twai_status_info_t twaistatus;
   twai_get_status_info(&twaistatus);
-  if (alerts_triggered & TWAI_ALERT_RX_DATA) {
+  if (alerts_triggered & TWAI_ALERT_RX_DATA || true) {
     twai_message_t message;
     if (twai_receive(&message, 0) == ESP_OK) {
     //Serial.println("TWAI_ALERT_RX_DATA ESP_OK");
@@ -119,16 +118,22 @@ int16_t esp32twaicanPop(struct CanardFrame *const out_frame,
         return -1;
       }
 
+      uint8_t cmp[] = {0,1,2,3,4,5,6,7};
+      if(memcmp(message.data, cmp,sizeof(cmp))==0) {
+        Serial.printf("MSG!\n");
+      }
+
       const bool valid = (message.extd) &&         // Extended frame
                          (!message.rtr) &&          // Not RTR frame
                          (true /* error frame */); // Not error frame
       if (!valid) {
-
-    //Serial.println("invalid");
         return 0; // Not an extended data frame -- drop silently and return
                   // early.
       }
 
+
+
+
       // Handle the loopback frame logic.
       const bool loopback_frame = false;
       // ((uint32_t)msg.msg_flags & (uint32_t)MSG_CONFIRM) != 0;
@@ -139,22 +144,12 @@ int16_t esp32twaicanPop(struct CanardFrame *const out_frame,
       if (loopback != NULL) {
         *loopback = loopback_frame;
       }
-    //Serial.println("ok");
       // Obtain the CAN frame time stamp from the kernel.
       if (NULL != out_timestamp_usec) {
-        // struct timeval tv;
-        // gettimeofday(&tv, NULL);
-        // (void)memset(out_frame, 0, sizeof(CanardFrame));
-        // *out_timestamp_usec = (CanardMicrosecond)(((uint64_t)tv.tv_sec *
-        // MEGA)
-        // + (uint64_t)tv.tv_usec);
         *out_timestamp_usec =
             (CanardMicrosecond)(xTaskGetTickCount() *
-                                (1000000 / configTICK_RATE_HZ));
+                                (1e6 / configTICK_RATE_HZ));
       }
-
-    //Serial.print("can id ");
-    //Serial.println(message.identifier);
       out_frame->extended_can_id = message.identifier;
       out_frame->payload.size = message.data_length_code;
       out_frame->payload.data = payload_buffer;

fw/src/hash_preferences.hpp

@@ -0,0 +1,146 @@
+#ifndef PREFERENCES_HPP
+#define PREFERENCES_HPP
+
+#include "Arduino.h"
+#include <cstring>
+#include <mutex>
+#include <string>
+#include <type_traits>
+#include <unordered_map>
+
+class Preferences {
+public:
+  Preferences() = default;
+
+  bool begin(const char *name, bool readOnly = false) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    currentNamespace_ = name;
+    return true;
+  }
+
+  void end() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    currentNamespace_.clear();
+  }
+
+  bool clear() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty())
+      return false;
+    data_[currentNamespace_].clear();
+    return true;
+  }
+
+  bool remove(const char *key) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty())
+      return false;
+    return data_[currentNamespace_].erase(key) > 0;
+  }
+
+  bool isKey(const char* key) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty()) return false;
+    
+    auto nsIt = data_.find(currentNamespace_);
+    if (nsIt == data_.end()) return false;  // Namespace does not exist
+    
+    const auto& nsData = nsIt->second;
+    return nsData.find(key) != nsData.end();
+}
+
+  // ----- PUT -----
+  template <typename T> bool put(const char *key, const T &value) {
+    static_assert(std::is_trivially_copyable<T>::value,
+                  "Only trivially copyable types supported");
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty())
+      return false;
+    data_[currentNamespace_][key] =
+        std::string(reinterpret_cast<const char *>(&value), sizeof(T));
+    return true;
+  }
+
+  bool putBytes(const char *key, const void *bytes, size_t len) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty() || !bytes)
+      return false;
+    data_[currentNamespace_][key] =
+        std::string(static_cast<const char *>(bytes), len);
+    return true;
+  }
+
+  bool putString(const char *key, const char* value) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty())
+      return false;
+    data_[currentNamespace_][key] = value;
+    return true;
+  }  
+  
+  bool putString(const char *key, String value) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty())
+      return false;
+    data_[currentNamespace_][key] = value.c_str();
+    return true;
+  }
+
+  // ----- GET -----
+  template <typename T> bool get(const char *key, T &value) const {
+    static_assert(std::is_trivially_copyable<T>::value,
+                  "Only trivially copyable types supported");
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty())
+      return false;
+    const auto &nsData = data_.at(currentNamespace_);
+    auto it = nsData.find(key);
+    if (it == nsData.end() || it->second.size() != sizeof(T))
+      return false;
+    std::memcpy(&value, it->second.data(), sizeof(T));
+    return true;
+  }
+
+  size_t getBytes(const char *key, void *buf, size_t maxLen) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty() || !buf)
+      return 0;
+    const auto &nsData = data_.at(currentNamespace_);
+    auto it = nsData.find(key);
+    if (it == nsData.end())
+      return 0;
+    size_t copyLen = std::min(maxLen, it->second.size());
+    std::memcpy(buf, it->second.data(), copyLen);
+    return copyLen;
+  }
+  
+  size_t getString(const char* key, char* buf, size_t maxLen) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (currentNamespace_.empty() || !buf || maxLen == 0) return 0;
+    const auto& nsData = data_.at(currentNamespace_);
+    auto it = nsData.find(key);
+    if (it == nsData.end()) return 0;
+    size_t copyLen = std::min(maxLen - 1, it->second.size());
+    std::memcpy(buf, it->second.data(), copyLen);
+    buf[copyLen] = '\0'; // null-terminate
+    return copyLen;
+}
+
+String getString(const char *key, const char *defaultVal) const {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (currentNamespace_.empty())
+    return String(defaultVal);
+  const auto &nsData = data_.at(currentNamespace_);
+  auto it = nsData.find(key);
+  return String((it != nsData.end()) ? it->second.c_str() : defaultVal);
+}
+
+
+private:
+  mutable std::mutex mutex_;
+  std::string currentNamespace_;
+  std::unordered_map<std::string, std::unordered_map<std::string, std::string>>
+      data_;
+};
+
+#endif // PREFERENCES_HPP

fw/src/main.cpp

@@ -182,7 +182,7 @@ void setup() {
     delay(100);
     digitalWrite(LED_PIN, 1); // enable
     delay(2000);
-    Serial.println("Start");
+    //Serial.println("Start");
 
   //digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
   return;
@@ -400,6 +400,7 @@ void setup() {
 int i = 0;
 void loop() {
     mainloop();
+    return;
 #ifdef MOTOR
   // drv8323s.focdriver->voltage_power_supply = vdrive_read;
   // digitalWrite(LED_PIN, 0); // enable

fw/src/register.hpp

@@ -1,16 +1,17 @@
 #pragma once
 
+#ifdef RAM_PREFERENCES
+#include "hash_preferences.hpp"
+#else
 #include <Preferences.h>
+#endif
 #include <uavcan/_register/Value_1_0.h>
 #include <uavcan/_register/Name_1_0.h>
 #include <Arduino.h>
 #include <string.h>
 
-#include "USB.h"
+static Preferences preferences;
-extern USBCDC usbserial;
+static bool preferences_open = false;
-#define Serial usbserial
-
-Preferences preferences;
 
 #define REGISTER_LIST_KEY "__reglist"
 #define REGISTER_LIST_DELIM "|"
@@ -27,39 +28,46 @@ hash(const char *str)
     return String(hash, HEX);
 }
 
+void open_preferences_if_needed() {
+    if(!preferences_open) {
+        preferences.begin("uavcan");
+        preferences_open = true;
+    }
+}
+
+void close_preferences_if_needed() {
+    if(preferences_open) {
+        preferences.end();
+        preferences_open = false;
+    }
+}
+
 void registerWrite(const char *const register_name, const uavcan_register_Value_1_0 *const value)
 {
-    preferences.begin("uavcan");
+    open_preferences_if_needed();
     int bytes_read = preferences.putBytes(hash(register_name).c_str(), (uint8_t *)value, sizeof(*value));
-
-    Serial.printf("Register %s written (free %d)\n", register_name, preferences.freeEntries());
     String reglist = preferences.getString(REGISTER_LIST_KEY, "");
     String search = String(register_name) + REGISTER_LIST_DELIM;
     if (reglist.indexOf(search) == -1)
     {
         reglist += search;
         preferences.putString(REGISTER_LIST_KEY, reglist);
-        Serial.printf("Register list written: %s\n", reglist.c_str());
     }
-
+    close_preferences_if_needed();
-    preferences.end();
 }
 
 void registerRead(const char *const register_name, uavcan_register_Value_1_0 *const value)
 {
-    preferences.begin("uavcan", true);
+    open_preferences_if_needed();
     if (preferences.isKey(hash(register_name).c_str()))
     {
         preferences.getBytes(hash(register_name).c_str(), (uint8_t *)value, sizeof(*value));
-        preferences.end();
     }
     else
     {
-        preferences.end();
-        Serial.printf("Register %s not found\n", register_name);
         registerWrite(register_name, value);
     }
-    
+    close_preferences_if_needed();
 }
 
 uavcan_register_Name_1_0 registerGetNameByIndex(const uint16_t index)
@@ -68,32 +76,9 @@ uavcan_register_Name_1_0 registerGetNameByIndex(const uint16_t index)
     name.name.elements[0] = '\0';
     name.name.count = 0;
 
-    preferences.begin("uavcan", true);
+    open_preferences_if_needed();
     String reglist = preferences.getString(REGISTER_LIST_KEY, "");
-    //Serial.printf("Register list read: %s\n", reglist.c_str());
-    preferences.end();
-    /*
-        Register list written:
-        reg.udral.service.actuator.servo|
-        uavcan.pub.servo.feedback.type|
-        uavcan.pub.servo.status.type|
-        uavcan.pub.servo.power.type|
-        uavcan.pub.servo.dynamics.type|
-        uavcan.sub.servo.setpoint.type|
-        uavcan.sub.servo.readiness.type
-    */
 
-    /*
-    Register list read:
-    reg.udral.service.actuator.servo|
-    uavcan.pub.servo.feedback.type|
-    uavcan.pub.servo.status.type|
-    uavcan.pub.servo.power.type|
-    uavcan.pub.servo.dynamics.type|
-    uavcan.sub.servo.setpoint.type|
-    uavcan.sub.servo.readiness.type|
-    uavcan.node.id|
-    */
     int start = 0;
     int found = 0;
     while (found <= index)
@@ -113,9 +98,8 @@ uavcan_register_Name_1_0 registerGetNameByIndex(const uint16_t index)
         start = end + 1;
         found++;
     }
-
+    close_preferences_if_needed();
-    Serial.printf("Register '%s' at idx %d %s found\n", (char *)&name.name, index, (name.name.count ? "" : "NOT"));
+    return name;
-    return name; // Empty if not found
 }
 
 bool registerAssign(uavcan_register_Value_1_0 *const dst, const uavcan_register_Value_1_0 *const src)
@@ -128,10 +112,9 @@ bool registerAssign(uavcan_register_Value_1_0 *const dst, const uavcan_register_
     return false;
 }
 
-#include <nvs_flash.h>
-
 void registerDoFactoryReset(void)
 {
-    nvs_flash_erase(); // erase the NVS partition and...
+    open_preferences_if_needed();
-    nvs_flash_init();  // initialize the NVS partition.
+    preferences.clear();
+    close_preferences_if_needed();
 }

fw/src/udral_servo.hpp

@@ -13,6 +13,7 @@
 /// Copyright (C) 2021 OpenCyphal <maintainers@opencyphal.org>
 /// Author: Pavel Kirienko <pavel@opencyphal.org>
 
+#include "esp32-hal.h"
 #define NUNAVUT_ASSERT(x) assert(x)
 
 #include "pin_def.h"
@@ -30,8 +31,7 @@
 #include <uavcan/node/port/List_0_1.h>
 #include <uavcan/_register/Access_1_0.h>
 #include <uavcan/_register/List_1_0.h>
-#include "NodeIDAllocationData_1_0.h"
+#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
-// #include <uavcan/pnp/NodeIDAllocationData_1_0.h>
 
 #include <reg/udral/service/common/Readiness_0_1.h>
 #include <reg/udral/service/actuator/common/_0_1.h>
@@ -131,12 +131,7 @@ static volatile bool g_restart_required = false;
 /// it may change rate or make leap adjustments. The two kinds of time serve completely different purposes.
 static CanardMicrosecond getMonotonicMicroseconds()
 {
-    struct timespec ts;
+    return micros();
-    if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0)
-    {
-        abort();
-    }
-    return (uint64_t)(ts.tv_sec * 1000000 + ts.tv_nsec / 1000);
 }
 
 // Returns the 128-bit unique-ID of the local node. This value is used in uavcan.node.GetInfo.Response and during the
@@ -664,9 +659,9 @@ static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const ua
     name[req->name.name.count] = '\0';
 
     uavcan_register_Access_Response_1_0 resp = {0};
-    Serial.println("processRequestRegisterAccess");
+    // Serial.println("processRequestRegisterAccess");
-    Serial.print("name: ");
+    // Serial.print("name: ");
-    Serial.println((char *)&req->name.name);
+    // Serial.println((char *)&req->name.name);
 
     // If we're asked to write a new value, do it now:
     if (!uavcan_register_Value_1_0_is_empty_(&req->value))
@@ -810,6 +805,7 @@ static void processReceivedTransfer(State *const state,
                     sendResponse(state, transfer, serialized_size, &serialized[0], now_usec);
                 }
             }
+            Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_ send");
         }
         else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_)
         {
@@ -855,12 +851,11 @@ static void canardDeallocate(void *const user_reference, const size_t amount, vo
 int mainloop()
 {
     struct timespec ts;
-    (void)clock_gettime(CLOCK_REALTIME, &ts);
+    srand(micros());
-    srand((unsigned)ts.tv_nsec);
 
     State state = {0};
 
-    registerDoFactoryReset();
+    //registerDoFactoryReset();
     // A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack.
     // For the background and related theory refer to the following resources:
     // - https://github.com/OpenCyphal/libcanard/blob/master/README.md
@@ -1133,8 +1128,7 @@ int mainloop()
             // frames from any of the redundant interface in an arbitrary order.
             // The internal state machine will sort them out and remove duplicates automatically.
             struct CanardFrame frame = {0};
-            uint8_t buf[CANARD_MTU_CAN_FD] = {0};
+            uint8_t buf[CANARD_MTU_CAN_CLASSIC] = {0};
-            // const int16_t socketcan_result = socketcanPop(sock[ifidx], &frame, NULL, sizeof(buf), buf, 0, NULL);
             const int16_t recieve_result = esp32twaicanPop(&frame, NULL, sizeof(buf), buf, 0, NULL);
 
             if (recieve_result == 0) // The read operation has timed out with no frames, nothing to do here.