nils/N17BLDC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

canard!

Browse Source
This commit is contained in:
Nils Schulte 2025-06-14 19:32:51 +02:00
parent 67477dac47
commit dc902aa9b3
7 changed files with 206 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
fw/.gitignore vendored
View File

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

1
fw/can_if_up.sh
View File

@ -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

37
fw/src/esp32twaican.cpp
View File

@ -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;

146
fw/src/hash_preferences.hpp Normal file
View File

@ -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

3
fw/src/main.cpp
View File

@ -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

77
fw/src/register.hpp
View File

@ -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"
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 <nvs_flash.h>
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();
}

26
fw/src/udral_servo.hpp
View File

@ -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;
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.