From dc902aa9b3e7dd768c57d05916fbfe1d8c5525d0 Mon Sep 17 00:00:00 2001 From: Nils Schulte Date: Sat, 14 Jun 2025 19:32:51 +0200 Subject: [PATCH] canard! --- fw/.gitignore | 1 + fw/can_if_up.sh | 1 + fw/src/esp32twaican.cpp | 37 ++++----- fw/src/hash_preferences.hpp | 146 ++++++++++++++++++++++++++++++++++++ fw/src/main.cpp | 3 +- fw/src/register.hpp | 77 ++++++++----------- fw/src/udral_servo.hpp | 26 +++---- 7 files changed, 206 insertions(+), 85 deletions(-) create mode 100644 fw/src/hash_preferences.hpp diff --git a/fw/.gitignore b/fw/.gitignore index 2705729..6affedd 100644 --- a/fw/.gitignore +++ b/fw/.gitignore @@ -5,3 +5,4 @@ include/nunavut/* include/reg/* include/uavcan/* +compile_commands.json diff --git a/fw/can_if_up.sh b/fw/can_if_up.sh index 5808652..331eec7 100755 --- a/fw/can_if_up.sh +++ b/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 diff --git a/fw/src/esp32twaican.cpp b/fw/src/esp32twaican.cpp index 3137fa5..ddf2acb 100644 --- a/fw/src/esp32twaican.cpp +++ b/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.tx_queue_len = 30; + g_config.rx_queue_len = 100; + 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 * (1000000 / configTICK_RATE_HZ))); - + int ret = twai_transmit(&message, (timeout_usec * (1e6 / 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; diff --git a/fw/src/hash_preferences.hpp b/fw/src/hash_preferences.hpp new file mode 100644 index 0000000..2d75235 --- /dev/null +++ b/fw/src/hash_preferences.hpp @@ -0,0 +1,146 @@ +#ifndef PREFERENCES_HPP +#define PREFERENCES_HPP + +#include "Arduino.h" +#include +#include +#include +#include +#include + +class Preferences { +public: + Preferences() = default; + + bool begin(const char *name, bool readOnly = false) { + std::lock_guard lock(mutex_); + currentNamespace_ = name; + return true; + } + + void end() { + std::lock_guard lock(mutex_); + currentNamespace_.clear(); + } + + bool clear() { + std::lock_guard lock(mutex_); + if (currentNamespace_.empty()) + return false; + data_[currentNamespace_].clear(); + return true; + } + + bool remove(const char *key) { + std::lock_guard lock(mutex_); + if (currentNamespace_.empty()) + return false; + return data_[currentNamespace_].erase(key) > 0; + } + + bool isKey(const char* key) const { + std::lock_guard 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 bool put(const char *key, const T &value) { + static_assert(std::is_trivially_copyable::value, + "Only trivially copyable types supported"); + std::lock_guard lock(mutex_); + if (currentNamespace_.empty()) + return false; + data_[currentNamespace_][key] = + std::string(reinterpret_cast(&value), sizeof(T)); + return true; + } + + bool putBytes(const char *key, const void *bytes, size_t len) { + std::lock_guard lock(mutex_); + if (currentNamespace_.empty() || !bytes) + return false; + data_[currentNamespace_][key] = + std::string(static_cast(bytes), len); + return true; + } + + bool putString(const char *key, const char* value) { + std::lock_guard lock(mutex_); + if (currentNamespace_.empty()) + return false; + data_[currentNamespace_][key] = value; + return true; + } + + bool putString(const char *key, String value) { + std::lock_guard lock(mutex_); + if (currentNamespace_.empty()) + return false; + data_[currentNamespace_][key] = value.c_str(); + return true; + } + + // ----- GET ----- + template bool get(const char *key, T &value) const { + static_assert(std::is_trivially_copyable::value, + "Only trivially copyable types supported"); + std::lock_guard 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 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 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 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> + data_; +}; + +#endif // PREFERENCES_HPP diff --git a/fw/src/main.cpp b/fw/src/main.cpp index 7d19e60..8e74ff8 100644 --- a/fw/src/main.cpp +++ b/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 diff --git a/fw/src/register.hpp b/fw/src/register.hpp index b568a1b..1a55f85 100644 --- a/fw/src/register.hpp +++ b/fw/src/register.hpp @@ -1,16 +1,17 @@ #pragma once +#ifdef RAM_PREFERENCES +#include "hash_preferences.hpp" +#else #include +#endif #include #include #include #include -#include "USB.h" -extern USBCDC usbserial; -#define Serial usbserial - -Preferences preferences; +static Preferences preferences; +static bool preferences_open = false; #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()); } - - preferences.end(); + close_preferences_if_needed(); } 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++; } - - Serial.printf("Register '%s' at idx %d %s found\n", (char *)&name.name, index, (name.name.count ? "" : "NOT")); - return name; // Empty if not found + close_preferences_if_needed(); + return name; } 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 - void registerDoFactoryReset(void) { - nvs_flash_erase(); // erase the NVS partition and... - nvs_flash_init(); // initialize the NVS partition. + open_preferences_if_needed(); + preferences.clear(); + close_preferences_if_needed(); } diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp index 1c0ea5d..1a3e5c1 100644 --- a/fw/src/udral_servo.hpp +++ b/fw/src/udral_servo.hpp @@ -13,6 +13,7 @@ /// Copyright (C) 2021 OpenCyphal /// Author: Pavel Kirienko +#include "esp32-hal.h" #define NUNAVUT_ASSERT(x) assert(x) #include "pin_def.h" @@ -30,8 +31,7 @@ #include #include #include -#include "NodeIDAllocationData_1_0.h" -// #include +#include #include #include @@ -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; - if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) - { - abort(); - } - return (uint64_t)(ts.tv_sec * 1000000 + ts.tv_nsec / 1000); + return micros(); } // 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.print("name: "); - Serial.println((char *)&req->name.name); + // Serial.println("processRequestRegisterAccess"); + // Serial.print("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((unsigned)ts.tv_nsec); + srand(micros()); 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}; - // const int16_t socketcan_result = socketcanPop(sock[ifidx], &frame, NULL, sizeof(buf), buf, 0, NULL); + uint8_t buf[CANARD_MTU_CAN_CLASSIC] = {0}; 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.