From 791ec0b7b756aa79e0466f20c537b9819271ba14 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 14 Sep 2022 07:08:04 +0200 Subject: [PATCH 01/82] Providing basic register API. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 201 ++++++++++++------ 1 file changed, 138 insertions(+), 63 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index bcc43169..f01f24e3 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -30,13 +30,20 @@ #undef min #include +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +using namespace uavcan::node; +using namespace uavcan::_register; + /************************************************************************************** * TYPEDEF **************************************************************************************/ typedef struct { - uavcan::node::Heartbeat_1_0<>::Mode mode; + Heartbeat_1_0<>::Mode mode; } OpenCyphalNodeData; #warning "Run 'TMF8801-FactoryCalib' once in order to obtain sensor calibration data for node configuration 'calib_data'" @@ -53,14 +60,52 @@ typedef struct * CONSTANTS **************************************************************************************/ -static int const MKRCAN_MCP2515_CS_PIN = 3; -static int const MKRCAN_MCP2515_INT_PIN = 7; - +static int const MKRCAN_MCP2515_CS_PIN = 3; +static int const MKRCAN_MCP2515_INT_PIN = 7; +static SPISettings const MCP2515x_SPI_SETTING{1000000, MSBFIRST, SPI_MODE0}; +static CanardNodeID const CYPHAL_NODE_ID = 42; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; +static const uavcan_node_GetInfo_Response_1_0 NODE_INFO = { + /* uavcan.node.Version.1.0 protocol_version */ + {1, 0}, + /* uavcan.node.Version.1.0 hardware_version */ + {1, 0}, + /* uavcan.node.Version.1.0 software_version */ + {0, 1}, + /* saturated uint64 software_vcs_revision_id */ + NULL, + /* saturated uint8[16] unique_id */ + {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, + 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, + /* saturated uint8[<=50] name */ + { + "107-systems.tof-sensor-node", + strlen("107-systems.tof-sensor-node") + }, +}; + +static const uavcan_register_List_Response_1_0 register_list1 = { + { "uavcan.node.id", strlen("uavcan.node.id") }, +}; +static const uavcan_register_List_Response_1_0 register_list2 = { + { "uavcan.node.description", strlen("uavcan.node.description") }, +}; +static const uavcan_register_List_Response_1_0 register_list_last = { + { "", 0 }, +}; + +static const uavcan_register_List_Response_1_0 REGISTER_LIST_ARRAY[] = +{ + register_list1, + register_list2, + register_list_last +}; +static size_t const REGISTER_LIST_ARRAY_SIZE = sizeof(REGISTER_LIST_ARRAY) / sizeof(REGISTER_LIST_ARRAY[0]); + static OpenCyphalNodeData const OPEN_CYPHAL_NODE_INITIAL_DATA = { - uavcan::node::Heartbeat_1_0<>::Mode::INITIALIZATION, + Heartbeat_1_0<>::Mode::INITIALIZATION, }; static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION = { @@ -74,25 +119,21 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION * FUNCTION DECLARATION **************************************************************************************/ -void i2c_generic_write(uint8_t const i2c_slave_addr, uint8_t const reg_addr, uint8_t const * buf, uint8_t const num_bytes); -void i2c_generic_read (uint8_t const i2c_slave_addr, uint8_t const reg_addr, uint8_t * buf, uint8_t const num_bytes); - -namespace MCP2515 -{ -void onReceive(CanardFrame const &); -} +void mcp2515_onReceiveBufferFull(CanardFrame const &); +void onList_1_0_Request_Received(CanardRxTransfer const &, Node &); +void onGetInfo_1_0_Request_Received(CanardRxTransfer const &, Node &); namespace node { -uavcan::node::Heartbeat_1_0<>::Mode handle_INITIALIZATION(); -uavcan::node::Heartbeat_1_0<>::Mode handle_OPERATIONAL(); -uavcan::node::Heartbeat_1_0<>::Mode handle_MAINTENANCE(); -uavcan::node::Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE(); +Heartbeat_1_0<>::Mode handle_INITIALIZATION(); +Heartbeat_1_0<>::Mode handle_OPERATIONAL(); +Heartbeat_1_0<>::Mode handle_MAINTENANCE(); +Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE(); } namespace heartbeat { -void publish(Node &, uint32_t const, uavcan::node::Heartbeat_1_0<>::Mode const); +void publish(Node &, uint32_t const, Heartbeat_1_0<>::Mode const); } namespace tof @@ -104,20 +145,48 @@ void onTofDistanceUpdate(drone::unit::Length const l); * GLOBAL VARIABLES **************************************************************************************/ -ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, - []() { digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); }, - [](uint8_t const data) { return SPI.transfer(data); }, +ArduinoMCP2515 mcp2515([]() + { + noInterrupts(); + SPI.beginTransaction(MCP2515x_SPI_SETTING); + digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); + }, + []() + { + digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); + SPI.endTransaction(); + interrupts(); + }, + [](uint8_t const d) { return SPI.transfer(d); }, micros, - MCP2515::onReceive, + mcp2515_onReceiveBufferFull, nullptr); -Node node_hdl([](CanardFrame const & frame) { return mcp2515.transmit(frame); }); +Node node_hdl([](CanardFrame const & frame) { return mcp2515.transmit(frame); }, CYPHAL_NODE_ID); OpenCyphalNodeData node_data = OPEN_CYPHAL_NODE_INITIAL_DATA; OpenCyphalNodeConfiguration node_config = OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION; -drone::ArduinoTMF8801 tmf8801(i2c_generic_write, - i2c_generic_read, +drone::ArduinoTMF8801 tmf8801([](uint8_t const i2c_slave_addr, uint8_t const reg_addr, uint8_t const * buf, uint8_t const num_bytes) + { + Wire.beginTransmission(i2c_slave_addr); + Wire.write(reg_addr); + for(uint8_t bytes_written = 0; bytes_written < num_bytes; bytes_written++) { + Wire.write(buf[bytes_written]); + } + Wire.endTransmission(); + }, + [](uint8_t const i2c_slave_addr, uint8_t const reg_addr, uint8_t * buf, uint8_t const num_bytes) + { + Wire.beginTransmission(i2c_slave_addr); + Wire.write(reg_addr); + Wire.endTransmission(); + + Wire.requestFrom(i2c_slave_addr, num_bytes); + for(uint8_t bytes_read = 0; (bytes_read < num_bytes) && Wire.available(); bytes_read++) { + buf[bytes_read] = Wire.read(); + } + }, delay, TMF8801::DEFAULT_I2C_ADDR, node_config.calib_data, @@ -156,8 +225,8 @@ void setup() /* Configure MCP2515 */ mcp2515.begin(); - mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ); - mcp2515.setListenOnlyMode(); + mcp2515.setBitRate(CanBitRate::BR_1000kBPS_16MHZ); + mcp2515.setNormalMode(); /* Configure TMF8801 */ @@ -166,6 +235,10 @@ void setup() for(;;) { } } + /* Register callbacks for node info and register api. + */ + node_hdl.subscribe>(onList_1_0_Request_Received); + node_hdl.subscribe>(onGetInfo_1_0_Request_Received); } void loop() @@ -185,14 +258,14 @@ void loop() /* Handle state transitions and state specific action. */ - uavcan::node::Heartbeat_1_0<>::Mode next_mode = node_data.mode; + Heartbeat_1_0<>::Mode next_mode = node_data.mode; switch(node_data.mode) { - case uavcan::node::Heartbeat_1_0<>::Mode::INITIALIZATION: next_mode = node::handle_INITIALIZATION(); break; - case uavcan::node::Heartbeat_1_0<>::Mode::OPERATIONAL: next_mode = node::handle_OPERATIONAL(); break; - case uavcan::node::Heartbeat_1_0<>::Mode::MAINTENANCE: next_mode = node::handle_MAINTENANCE(); break; - case uavcan::node::Heartbeat_1_0<>::Mode::SOFTWARE_UPDATE: next_mode = node::handle_SOFTWARE_UPDATE(); break; + case Heartbeat_1_0<>::Mode::INITIALIZATION: next_mode = node::handle_INITIALIZATION(); break; + case Heartbeat_1_0<>::Mode::OPERATIONAL: next_mode = node::handle_OPERATIONAL(); break; + case Heartbeat_1_0<>::Mode::MAINTENANCE: next_mode = node::handle_MAINTENANCE(); break; + case Heartbeat_1_0<>::Mode::SOFTWARE_UPDATE: next_mode = node::handle_SOFTWARE_UPDATE(); break; } node_data.mode = next_mode; @@ -205,66 +278,68 @@ void loop() * FUNCTION DEFINITION **************************************************************************************/ -void i2c_generic_write(uint8_t const i2c_slave_addr, uint8_t const reg_addr, uint8_t const * buf, uint8_t const num_bytes) +void mcp2515_onReceiveBufferFull(CanardFrame const & frame) { - Wire.beginTransmission(i2c_slave_addr); - Wire.write(reg_addr); - for(uint8_t bytes_written = 0; bytes_written < num_bytes; bytes_written++) { - Wire.write(buf[bytes_written]); - } - Wire.endTransmission(); + node_hdl.onCanFrameReceived(frame, micros()); } -void i2c_generic_read(uint8_t const i2c_slave_addr, uint8_t const reg_addr, uint8_t * buf, uint8_t const num_bytes) +void onGetInfo_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_hdl) { - Wire.beginTransmission(i2c_slave_addr); - Wire.write(reg_addr); - Wire.endTransmission(); - - Wire.requestFrom(i2c_slave_addr, num_bytes); - for(uint8_t bytes_read = 0; (bytes_read < num_bytes) && Wire.available(); bytes_read++) { - buf[bytes_read] = Wire.read(); - } + DBG_INFO("onGetInfo_1_0_Request_Received"); + GetInfo_1_0::Response<> rsp = GetInfo_1_0::Response<>(); + memcpy(&rsp.data, &NODE_INFO, sizeof(uavcan_node_GetInfo_Response_1_0)); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } -namespace MCP2515 +void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_hdl) { + static int count = 0; + DBG_INFO("onList_1_0_Request_Received: count %d", count); -void onReceive(CanardFrame const & frame) { - node_hdl.onCanFrameReceived(frame, micros()); -} + List_1_0::Response<> rsp = List_1_0::Response<>(); + memcpy(&rsp.data, REGISTER_LIST_ARRAY + count, sizeof(uavcan_register_List_Response_1_0)); -} /* MCP2515 */ + if (!node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id)) + DBG_ERROR("respond() failed"); + + /* Reset counter after the last request, + * otherwise we risk an array overflow. + */ + if (count < (REGISTER_LIST_ARRAY_SIZE - 1)) + count++; + else + count = 0; +} namespace node { -uavcan::node::Heartbeat_1_0<>::Mode handle_INITIALIZATION() +Heartbeat_1_0<>::Mode handle_INITIALIZATION() { DBG_VERBOSE("INITIALIZATION"); - return uavcan::node::Heartbeat_1_0<>::Mode::OPERATIONAL; + return Heartbeat_1_0<>::Mode::OPERATIONAL; } -uavcan::node::Heartbeat_1_0<>::Mode handle_OPERATIONAL() +Heartbeat_1_0<>::Mode handle_OPERATIONAL() { DBG_VERBOSE("OPERATIONAL"); - return uavcan::node::Heartbeat_1_0<>::Mode::OPERATIONAL; + return Heartbeat_1_0<>::Mode::OPERATIONAL; } -uavcan::node::Heartbeat_1_0<>::Mode handle_MAINTENANCE() +Heartbeat_1_0<>::Mode handle_MAINTENANCE() { DBG_VERBOSE("MAINTENANCE"); - return uavcan::node::Heartbeat_1_0<>::Mode::INITIALIZATION; + return Heartbeat_1_0<>::Mode::INITIALIZATION; } -uavcan::node::Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE() +Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE() { DBG_VERBOSE("SOFTWARE_UPDATE"); - return uavcan::node::Heartbeat_1_0<>::Mode::INITIALIZATION; + return Heartbeat_1_0<>::Mode::INITIALIZATION; } } /* node */ @@ -272,12 +347,12 @@ uavcan::node::Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE() namespace heartbeat { -void publish(Node & u, uint32_t const uptime, uavcan::node::Heartbeat_1_0<>::Mode const mode) +void publish(Node & u, uint32_t const uptime, Heartbeat_1_0<>::Mode const mode) { - uavcan::node::Heartbeat_1_0<> hb; + Heartbeat_1_0<> hb; hb.data.uptime = uptime; - hb = uavcan::node::Heartbeat_1_0<>::Health::NOMINAL; + hb = Heartbeat_1_0<>::Health::NOMINAL; hb = mode; hb.data.vendor_specific_status_code = 0; From 23a5148c1df09aa4ea7aafb6e054081c56b1891f Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 14 Sep 2022 08:56:11 +0200 Subject: [PATCH 02/82] Use unqique ID from chip. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 37 ++++++++++++++++++- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index f01f24e3..dcdcae4c 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -30,6 +30,15 @@ #undef min #include +/************************************************************************************** + * DEFINES + **************************************************************************************/ + +#define ATSAMD21G18_SERIAL_NUMBER_WORD_0 *(volatile uint32_t*)(0x0080A00C) +#define ATSAMD21G18_SERIAL_NUMBER_WORD_1 *(volatile uint32_t*)(0x0080A040) +#define ATSAMD21G18_SERIAL_NUMBER_WORD_2 *(volatile uint32_t*)(0x0080A044) +#define ATSAMD21G18_SERIAL_NUMBER_WORD_3 *(volatile uint32_t*)(0x0080A048) + /************************************************************************************** * NAMESPACE **************************************************************************************/ @@ -41,6 +50,15 @@ using namespace uavcan::_register; * TYPEDEF **************************************************************************************/ +union UniqueId +{ + struct __attribute__((packed)) + { + uint32_t w0, w1, w2, w3; + } word_buf; + uint8_t byte_buf[16]; +}; + typedef struct { Heartbeat_1_0<>::Mode mode; @@ -60,9 +78,20 @@ typedef struct * CONSTANTS **************************************************************************************/ +UniqueId const UNIQUE_ID = []() +{ + UniqueId uid; + uid.word_buf.w0 = ATSAMD21G18_SERIAL_NUMBER_WORD_0; + uid.word_buf.w1 = ATSAMD21G18_SERIAL_NUMBER_WORD_1; + uid.word_buf.w2 = ATSAMD21G18_SERIAL_NUMBER_WORD_2; + uid.word_buf.w3 = ATSAMD21G18_SERIAL_NUMBER_WORD_3; + return uid; +} (); + static int const MKRCAN_MCP2515_CS_PIN = 3; static int const MKRCAN_MCP2515_INT_PIN = 7; static SPISettings const MCP2515x_SPI_SETTING{1000000, MSBFIRST, SPI_MODE0}; + static CanardNodeID const CYPHAL_NODE_ID = 42; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; @@ -76,8 +105,12 @@ static const uavcan_node_GetInfo_Response_1_0 NODE_INFO = { /* saturated uint64 software_vcs_revision_id */ NULL, /* saturated uint8[16] unique_id */ - {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, - 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, + { + UNIQUE_ID.byte_buf[ 0], UNIQUE_ID.byte_buf[ 1], UNIQUE_ID.byte_buf[ 2], UNIQUE_ID.byte_buf[ 3], + UNIQUE_ID.byte_buf[ 4], UNIQUE_ID.byte_buf[ 5], UNIQUE_ID.byte_buf[ 6], UNIQUE_ID.byte_buf[ 7], + UNIQUE_ID.byte_buf[ 8], UNIQUE_ID.byte_buf[ 9], UNIQUE_ID.byte_buf[10], UNIQUE_ID.byte_buf[11], + UNIQUE_ID.byte_buf[12], UNIQUE_ID.byte_buf[13], UNIQUE_ID.byte_buf[14], UNIQUE_ID.byte_buf[15] + }, /* saturated uint8[<=50] name */ { "107-systems.tof-sensor-node", From 48d4d740733c72f78aee2fa3d65971db871c4616 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 14 Sep 2022 11:11:18 +0200 Subject: [PATCH 03/82] Simplify sketch (remove all unnecessary namespaces). --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 111 +++++++----------- 1 file changed, 43 insertions(+), 68 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index dcdcae4c..174f4aa3 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -74,6 +74,22 @@ typedef struct uint8_t measurement_period_ms; } OpenCyphalNodeConfiguration; +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +void mcp2515_onReceiveBufferFull(CanardFrame const &); +void onList_1_0_Request_Received(CanardRxTransfer const &, Node &); +void onGetInfo_1_0_Request_Received(CanardRxTransfer const &, Node &); + +void publish_heartbeat(Node &, uint32_t const, Heartbeat_1_0<>::Mode const); +void publish_tofDistance(drone::unit::Length const l); + +Heartbeat_1_0<>::Mode handle_INITIALIZATION(); +Heartbeat_1_0<>::Mode handle_OPERATIONAL(); +Heartbeat_1_0<>::Mode handle_MAINTENANCE(); +Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE(); + /************************************************************************************** * CONSTANTS **************************************************************************************/ @@ -148,32 +164,6 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION 100 }; -/************************************************************************************** - * FUNCTION DECLARATION - **************************************************************************************/ - -void mcp2515_onReceiveBufferFull(CanardFrame const &); -void onList_1_0_Request_Received(CanardRxTransfer const &, Node &); -void onGetInfo_1_0_Request_Received(CanardRxTransfer const &, Node &); - -namespace node -{ -Heartbeat_1_0<>::Mode handle_INITIALIZATION(); -Heartbeat_1_0<>::Mode handle_OPERATIONAL(); -Heartbeat_1_0<>::Mode handle_MAINTENANCE(); -Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE(); -} - -namespace heartbeat -{ -void publish(Node &, uint32_t const, Heartbeat_1_0<>::Mode const); -} - -namespace tof -{ -void onTofDistanceUpdate(drone::unit::Length const l); -} - /************************************************************************************** * GLOBAL VARIABLES **************************************************************************************/ @@ -224,7 +214,7 @@ drone::ArduinoTMF8801 tmf8801([](uint8_t const i2c_slave_addr, uint8_t const reg TMF8801::DEFAULT_I2C_ADDR, node_config.calib_data, node_config.algo_state, - tof::onTofDistanceUpdate); + publish_tofDistance); DEBUG_INSTANCE(120, Serial); @@ -284,7 +274,7 @@ void loop() */ static unsigned long prev_heartbeat = 0; if ((now - prev_heartbeat) > node_config.heartbeat_period_ms) { - heartbeat::publish(node_hdl, now / 1000, node_data.mode); + publish_heartbeat(node_hdl, now / 1000, node_data.mode); prev_heartbeat = now; } @@ -295,10 +285,10 @@ void loop() switch(node_data.mode) { - case Heartbeat_1_0<>::Mode::INITIALIZATION: next_mode = node::handle_INITIALIZATION(); break; - case Heartbeat_1_0<>::Mode::OPERATIONAL: next_mode = node::handle_OPERATIONAL(); break; - case Heartbeat_1_0<>::Mode::MAINTENANCE: next_mode = node::handle_MAINTENANCE(); break; - case Heartbeat_1_0<>::Mode::SOFTWARE_UPDATE: next_mode = node::handle_SOFTWARE_UPDATE(); break; + case Heartbeat_1_0<>::Mode::INITIALIZATION: next_mode = handle_INITIALIZATION(); break; + case Heartbeat_1_0<>::Mode::OPERATIONAL: next_mode = handle_OPERATIONAL(); break; + case Heartbeat_1_0<>::Mode::MAINTENANCE: next_mode = handle_MAINTENANCE(); break; + case Heartbeat_1_0<>::Mode::SOFTWARE_UPDATE: next_mode = handle_SOFTWARE_UPDATE(); break; } node_data.mode = next_mode; @@ -344,8 +334,28 @@ void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_h count = 0; } -namespace node +void publish_heartbeat(Node & u, uint32_t const uptime, Heartbeat_1_0<>::Mode const mode) { + Heartbeat_1_0<> hb; + + hb.data.uptime = uptime; + hb = Heartbeat_1_0<>::Health::NOMINAL; + hb = mode; + hb.data.vendor_specific_status_code = 0; + + u.publish(hb); +} + +void publish_tofDistance(drone::unit::Length const l) +{ + DBG_INFO("[%05lu] Distance = %.3f m", millis(), l.value()); + + typedef uavcan::primitive::scalar::Real32_1_0 DistanceMessageType; + DistanceMessageType tof_distance_msg; + tof_distance_msg.data.value = l.value(); + + node_hdl.publish(tof_distance_msg); +} Heartbeat_1_0<>::Mode handle_INITIALIZATION() { @@ -374,38 +384,3 @@ Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE() return Heartbeat_1_0<>::Mode::INITIALIZATION; } - -} /* node */ - -namespace heartbeat -{ - -void publish(Node & u, uint32_t const uptime, Heartbeat_1_0<>::Mode const mode) -{ - Heartbeat_1_0<> hb; - - hb.data.uptime = uptime; - hb = Heartbeat_1_0<>::Health::NOMINAL; - hb = mode; - hb.data.vendor_specific_status_code = 0; - - u.publish(hb); -} - -} /* heartbeat */ - -namespace tof -{ - -void onTofDistanceUpdate(drone::unit::Length const l) -{ - DBG_INFO("[%05lu] Distance = %.3f m", millis(), l.value()); - - typedef uavcan::primitive::scalar::Real32_1_0 DistanceMessageType; - DistanceMessageType tof_distance_msg; - tof_distance_msg.data.value = l.value(); - - node_hdl.publish(tof_distance_msg); -} - -} From d5c6463df5d57c01a81a5b830ec84c90cad19fee Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 14 Sep 2022 11:25:07 +0200 Subject: [PATCH 04/82] Extract node info into separate header file in order to reduce code density in main file. --- .../NodeInfo.h | 76 +++++++++++++++++++ .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 51 +------------ 2 files changed, 77 insertions(+), 50 deletions(-) create mode 100644 examples/OpenCyphal-ToF-Distance-Sensor-Node/NodeInfo.h diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/NodeInfo.h b/examples/OpenCyphal-ToF-Distance-Sensor-Node/NodeInfo.h new file mode 100644 index 00000000..c8fc739e --- /dev/null +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/NodeInfo.h @@ -0,0 +1,76 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef NODE_INFO_H_ +#define NODE_INFO_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include <107-Arduino-Cyphal.h> + +/************************************************************************************** + * DEFINES + **************************************************************************************/ + +#define ATSAMD21G18_SERIAL_NUMBER_WORD_0 *(volatile uint32_t*)(0x0080A00C) +#define ATSAMD21G18_SERIAL_NUMBER_WORD_1 *(volatile uint32_t*)(0x0080A040) +#define ATSAMD21G18_SERIAL_NUMBER_WORD_2 *(volatile uint32_t*)(0x0080A044) +#define ATSAMD21G18_SERIAL_NUMBER_WORD_3 *(volatile uint32_t*)(0x0080A048) + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +union UniqueId +{ + struct __attribute__((packed)) + { + uint32_t w0, w1, w2, w3; + } word_buf; + uint8_t byte_buf[16]; +}; + +/************************************************************************************** + * CONSTANTS + **************************************************************************************/ + +UniqueId const UNIQUE_ID = []() +{ + UniqueId uid; + uid.word_buf.w0 = ATSAMD21G18_SERIAL_NUMBER_WORD_0; + uid.word_buf.w1 = ATSAMD21G18_SERIAL_NUMBER_WORD_1; + uid.word_buf.w2 = ATSAMD21G18_SERIAL_NUMBER_WORD_2; + uid.word_buf.w3 = ATSAMD21G18_SERIAL_NUMBER_WORD_3; + return uid; +} (); + +static const uavcan_node_GetInfo_Response_1_0 NODE_INFO = { + /* uavcan.node.Version.1.0 protocol_version */ + {1, 0}, + /* uavcan.node.Version.1.0 hardware_version */ + {1, 0}, + /* uavcan.node.Version.1.0 software_version */ + {0, 1}, + /* saturated uint64 software_vcs_revision_id */ + NULL, + /* saturated uint8[16] unique_id */ + { + UNIQUE_ID.byte_buf[ 0], UNIQUE_ID.byte_buf[ 1], UNIQUE_ID.byte_buf[ 2], UNIQUE_ID.byte_buf[ 3], + UNIQUE_ID.byte_buf[ 4], UNIQUE_ID.byte_buf[ 5], UNIQUE_ID.byte_buf[ 6], UNIQUE_ID.byte_buf[ 7], + UNIQUE_ID.byte_buf[ 8], UNIQUE_ID.byte_buf[ 9], UNIQUE_ID.byte_buf[10], UNIQUE_ID.byte_buf[11], + UNIQUE_ID.byte_buf[12], UNIQUE_ID.byte_buf[13], UNIQUE_ID.byte_buf[14], UNIQUE_ID.byte_buf[15] + }, + /* saturated uint8[<=50] name */ + { + "107-systems.tof-sensor-node", + strlen("107-systems.tof-sensor-node") + }, +}; + +#endif /* NODE_INFO_H_ */ diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 174f4aa3..c5cfce9e 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -30,14 +30,7 @@ #undef min #include -/************************************************************************************** - * DEFINES - **************************************************************************************/ - -#define ATSAMD21G18_SERIAL_NUMBER_WORD_0 *(volatile uint32_t*)(0x0080A00C) -#define ATSAMD21G18_SERIAL_NUMBER_WORD_1 *(volatile uint32_t*)(0x0080A040) -#define ATSAMD21G18_SERIAL_NUMBER_WORD_2 *(volatile uint32_t*)(0x0080A044) -#define ATSAMD21G18_SERIAL_NUMBER_WORD_3 *(volatile uint32_t*)(0x0080A048) +#include "NodeInfo.h" /************************************************************************************** * NAMESPACE @@ -50,15 +43,6 @@ using namespace uavcan::_register; * TYPEDEF **************************************************************************************/ -union UniqueId -{ - struct __attribute__((packed)) - { - uint32_t w0, w1, w2, w3; - } word_buf; - uint8_t byte_buf[16]; -}; - typedef struct { Heartbeat_1_0<>::Mode mode; @@ -94,16 +78,6 @@ Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE(); * CONSTANTS **************************************************************************************/ -UniqueId const UNIQUE_ID = []() -{ - UniqueId uid; - uid.word_buf.w0 = ATSAMD21G18_SERIAL_NUMBER_WORD_0; - uid.word_buf.w1 = ATSAMD21G18_SERIAL_NUMBER_WORD_1; - uid.word_buf.w2 = ATSAMD21G18_SERIAL_NUMBER_WORD_2; - uid.word_buf.w3 = ATSAMD21G18_SERIAL_NUMBER_WORD_3; - return uid; -} (); - static int const MKRCAN_MCP2515_CS_PIN = 3; static int const MKRCAN_MCP2515_INT_PIN = 7; static SPISettings const MCP2515x_SPI_SETTING{1000000, MSBFIRST, SPI_MODE0}; @@ -111,29 +85,6 @@ static SPISettings const MCP2515x_SPI_SETTING{1000000, MSBFIRST, SPI_MODE0}; static CanardNodeID const CYPHAL_NODE_ID = 42; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; -static const uavcan_node_GetInfo_Response_1_0 NODE_INFO = { - /* uavcan.node.Version.1.0 protocol_version */ - {1, 0}, - /* uavcan.node.Version.1.0 hardware_version */ - {1, 0}, - /* uavcan.node.Version.1.0 software_version */ - {0, 1}, - /* saturated uint64 software_vcs_revision_id */ - NULL, - /* saturated uint8[16] unique_id */ - { - UNIQUE_ID.byte_buf[ 0], UNIQUE_ID.byte_buf[ 1], UNIQUE_ID.byte_buf[ 2], UNIQUE_ID.byte_buf[ 3], - UNIQUE_ID.byte_buf[ 4], UNIQUE_ID.byte_buf[ 5], UNIQUE_ID.byte_buf[ 6], UNIQUE_ID.byte_buf[ 7], - UNIQUE_ID.byte_buf[ 8], UNIQUE_ID.byte_buf[ 9], UNIQUE_ID.byte_buf[10], UNIQUE_ID.byte_buf[11], - UNIQUE_ID.byte_buf[12], UNIQUE_ID.byte_buf[13], UNIQUE_ID.byte_buf[14], UNIQUE_ID.byte_buf[15] - }, - /* saturated uint8[<=50] name */ - { - "107-systems.tof-sensor-node", - strlen("107-systems.tof-sensor-node") - }, -}; - static const uavcan_register_List_Response_1_0 register_list1 = { { "uavcan.node.id", strlen("uavcan.node.id") }, }; From f2cd1030c7699588b7608d26d70273fba27d3f48 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 15 Sep 2022 10:48:23 +0200 Subject: [PATCH 05/82] Minimal implementation of reading a registers value - does not work. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index c5cfce9e..223c86f6 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -65,6 +65,7 @@ typedef struct void mcp2515_onReceiveBufferFull(CanardFrame const &); void onList_1_0_Request_Received(CanardRxTransfer const &, Node &); void onGetInfo_1_0_Request_Received(CanardRxTransfer const &, Node &); +void onAccess_1_0_Request_Received(CanardRxTransfer const &, Node &); void publish_heartbeat(Node &, uint32_t const, Heartbeat_1_0<>::Mode const); void publish_tofDistance(drone::unit::Length const l); @@ -211,6 +212,7 @@ void setup() /* Register callbacks for node info and register api. */ + node_hdl.subscribe>(onAccess_1_0_Request_Received); node_hdl.subscribe>(onList_1_0_Request_Received); node_hdl.subscribe>(onGetInfo_1_0_Request_Received); } @@ -285,6 +287,14 @@ void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_h count = 0; } +void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) +{ + Access_1_0::Request<> const req = Access_1_0::Request<>::deserialize(transfer); + + DBG_INFO("onAccess_1_0_Request_Received: reg %s", + req.data.name.name.elements); +} + void publish_heartbeat(Node & u, uint32_t const uptime, Heartbeat_1_0<>::Mode const mode) { Heartbeat_1_0<> hb; From ac1e38475792d9789588c1c624416d49e441e851 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 15 Sep 2022 10:55:09 +0200 Subject: [PATCH 06/82] Simplify register list reading. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 223c86f6..c02d30ec 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -269,22 +269,12 @@ void onGetInfo_1_0_Request_Received(CanardRxTransfer const &transfer, Node & nod void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_hdl) { - static int count = 0; - DBG_INFO("onList_1_0_Request_Received: count %d", count); + List_1_0::Request<> const req = List_1_0::Request<>::deserialize(transfer); + DBG_INFO("onList_1_0_Request_Received: index %d", req.data.index); List_1_0::Response<> rsp = List_1_0::Response<>(); - memcpy(&rsp.data, REGISTER_LIST_ARRAY + count, sizeof(uavcan_register_List_Response_1_0)); - - if (!node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id)) - DBG_ERROR("respond() failed"); - - /* Reset counter after the last request, - * otherwise we risk an array overflow. - */ - if (count < (REGISTER_LIST_ARRAY_SIZE - 1)) - count++; - else - count = 0; + memcpy(&rsp.data, REGISTER_LIST_ARRAY + req.data.index, sizeof(uavcan_register_List_Response_1_0)); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) From 56ede2e92a6b8ff50b1b54e965bd1682dde783d4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 15 Sep 2022 10:57:18 +0200 Subject: [PATCH 07/82] Extract registers into separate header file. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 19 +------- .../Register.h | 47 +++++++++++++++++++ 2 files changed, 48 insertions(+), 18 deletions(-) create mode 100644 examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index c02d30ec..dc3b2323 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -31,6 +31,7 @@ #include #include "NodeInfo.h" +#include "Register.h" /************************************************************************************** * NAMESPACE @@ -86,24 +87,6 @@ static SPISettings const MCP2515x_SPI_SETTING{1000000, MSBFIRST, SPI_MODE0}; static CanardNodeID const CYPHAL_NODE_ID = 42; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; -static const uavcan_register_List_Response_1_0 register_list1 = { - { "uavcan.node.id", strlen("uavcan.node.id") }, -}; -static const uavcan_register_List_Response_1_0 register_list2 = { - { "uavcan.node.description", strlen("uavcan.node.description") }, -}; -static const uavcan_register_List_Response_1_0 register_list_last = { - { "", 0 }, -}; - -static const uavcan_register_List_Response_1_0 REGISTER_LIST_ARRAY[] = -{ - register_list1, - register_list2, - register_list_last -}; -static size_t const REGISTER_LIST_ARRAY_SIZE = sizeof(REGISTER_LIST_ARRAY) / sizeof(REGISTER_LIST_ARRAY[0]); - static OpenCyphalNodeData const OPEN_CYPHAL_NODE_INITIAL_DATA = { Heartbeat_1_0<>::Mode::INITIALIZATION, diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h new file mode 100644 index 00000000..b1762698 --- /dev/null +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h @@ -0,0 +1,47 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_H_ +#define REGISTER_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include <107-Arduino-Cyphal.h> + +/************************************************************************************** + * DEFINES + **************************************************************************************/ + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +/************************************************************************************** + * CONSTANTS + **************************************************************************************/ + +static const uavcan_register_List_Response_1_0 register_list1 = { + { "uavcan.node.id", strlen("uavcan.node.id") }, +}; +static const uavcan_register_List_Response_1_0 register_list2 = { + { "uavcan.node.description", strlen("uavcan.node.description") }, +}; +static const uavcan_register_List_Response_1_0 register_list_last = { + { "", 0 }, +}; + +static const uavcan_register_List_Response_1_0 REGISTER_LIST_ARRAY[] = +{ + register_list1, + register_list2, + register_list_last +}; +static size_t const REGISTER_LIST_ARRAY_SIZE = sizeof(REGISTER_LIST_ARRAY) / sizeof(REGISTER_LIST_ARRAY[0]); + +#endif /* REGISTER_H_ */ From b09e21b24e36f628cbf3c326b08347219f230b38 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 08:09:37 +0200 Subject: [PATCH 08/82] Using a receive ringbuffer for async data processing. --- src/Node.cpp | 90 ++++++++++++++++++------- src/Node.h | 20 ++++-- src/Node.ipp | 10 --- src/utility/RingBuffer.hpp | 134 +++++++++++++++++++++++++++++++++++++ 4 files changed, 212 insertions(+), 42 deletions(-) create mode 100644 src/utility/RingBuffer.hpp diff --git a/src/Node.cpp b/src/Node.cpp index 4a101384..2bcc8b05 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -21,6 +21,7 @@ Node::Node(CanFrameTransmitFunc transmit_func, size_t const mtu_bytes) : _canard_hdl{canardInit(Node::o1heap_allocate, Node::o1heap_free)} , _canard_tx_queue{canardTxInit(tx_queue_capacity, mtu_bytes)} +, _canard_rx_queue{DEFAULT_RX_QUEUE_SIZE} , _transmit_func{transmit_func} { _canard_hdl.node_id = node_id; @@ -33,27 +34,76 @@ Node::Node(CanFrameTransmitFunc transmit_func, void Node::setNodeId(CanardNodeID const node_id) { - LockGuard lock; _canard_hdl.node_id = node_id; } CanardNodeID Node::getNodeId() const { - LockGuard lock; return _canard_hdl.node_id; } +#include + +void Node::spin() +{ + handle_receive(); + handle_transmit(); +} + void Node::onCanFrameReceived(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us) { - LockGuard lock; + uint32_t const extended_can_id = frame.extended_can_id; + size_t const payload_size = frame.payload_size; + + std::array payload; + payload.fill(0); + memcpy(payload.data(), frame.payload, std::min(payload_size, payload.size())); + + _canard_rx_queue.enqueue(std::make_tuple(extended_can_id, payload_size, payload, rx_timestamp_us)); +} + +/************************************************************************************** + * PRIVATE MEMBER FUNCTIONS + **************************************************************************************/ + +void * Node::o1heap_allocate(CanardInstance * const ins, size_t const amount) +{ + O1HeapLibcanard * o1heap = reinterpret_cast(ins->user_reference); + return o1heap->allocate(amount); +} + +void Node::o1heap_free(CanardInstance * const ins, void * const pointer) +{ + O1HeapLibcanard * o1heap = reinterpret_cast(ins->user_reference); + o1heap->free(pointer); +} + +void Node::handle_receive() +{ + while (!_canard_rx_queue.isEmpty()) + { + auto [extended_can_id, payload_size, payload, rx_timestamp_us] = _canard_rx_queue.dequeue(); + + CanardFrame frame; + frame.extended_can_id = extended_can_id; + frame.payload_size = payload_size; + frame.payload = reinterpret_cast(payload.data()); + + receiveOne(frame, micros()); + } +} + +void Node::receiveOne(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us) +{ + Serial.println(frame.extended_can_id, HEX); CanardRxTransfer transfer; int8_t const result = canardRxAccept(&_canard_hdl, - rx_timestamp_us, - &frame, - 0, /* redundant_transport_index */ - &transfer, - nullptr); + rx_timestamp_us, + &frame, + 0, /* redundant_transport_index */ + &transfer, + nullptr); if(result == 1) { @@ -76,10 +126,13 @@ void Node::onCanFrameReceived(CanardFrame const & frame, CanardMicrosecond const } } -bool Node::transmitCanFrame() +void Node::handle_transmit() { - LockGuard lock; + while (transmitOne()) { } +} +bool Node::transmitOne() +{ if (!_transmit_func) return false; @@ -95,22 +148,6 @@ bool Node::transmitCanFrame() return true; } -/************************************************************************************** - * PRIVATE MEMBER FUNCTIONS - **************************************************************************************/ - -void * Node::o1heap_allocate(CanardInstance * const ins, size_t const amount) -{ - O1HeapLibcanard * o1heap = reinterpret_cast(ins->user_reference); - return o1heap->allocate(amount); -} - -void Node::o1heap_free(CanardInstance * const ins, void * const pointer) -{ - O1HeapLibcanard * o1heap = reinterpret_cast(ins->user_reference); - o1heap->free(pointer); -} - CanardTransferID Node::getNextTransferId(CanardPortID const port_id) { CanardTransferID const next_transfer_id = (_tx_transfer_map.count(port_id) > 0) ? ((_tx_transfer_map[port_id] + 1) % CANARD_TRANSFER_ID_MAX) : 0; @@ -127,6 +164,7 @@ bool Node::subscribe(CanardTransferKind const transfer_kind, CanardPortID const payload_size_max, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &(_rx_transfer_map[port_id].canard_rx_sub)); + bool const success = (result >= 0); return success; } diff --git a/src/Node.h b/src/Node.h index 9fa6b239..1b0e7252 100644 --- a/src/Node.h +++ b/src/Node.h @@ -17,6 +17,7 @@ #undef max #undef min #include +#include #include #include @@ -27,6 +28,7 @@ #include "utility/convert.hpp" #include "utility/LockGuard.h" +#include "utility/RingBuffer.hpp" /************************************************************************************** * TYPEDEF @@ -46,6 +48,7 @@ class Node static size_t constexpr DEFAULT_O1HEAP_SIZE = 4096; static size_t constexpr DEFAULT_TX_QUEUE_SIZE = 100; + static size_t constexpr DEFAULT_RX_QUEUE_SIZE = 32; static size_t constexpr DEFAULT_MTU_SIZE = CANARD_MTU_CAN_CLASSIC; static CanardNodeID constexpr DEFAULT_NODE_ID = 42; @@ -65,16 +68,14 @@ class Node void setNodeId(CanardNodeID const node_id); CanardNodeID getNodeId() const; - + /* Must be called from the application to process + * all received CAN frames. + */ + void spin(); /* Must be called from the application upon the * reception of a can frame. */ void onCanFrameReceived(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us); - /* Must be called regularly from within the application - * in order to transmit all CAN pushed on the internal - * stack via publish/request. - */ - bool transmitCanFrame(); template bool subscribe (OnTransferReceivedFunc func); @@ -101,6 +102,7 @@ class Node O1HeapLibcanard _o1heap_hdl; CanardInstance _canard_hdl; CanardTxQueue _canard_tx_queue; + arduino::_107_::opencyphal::ThreadsafeRingBuffer, CanardMicrosecond>> _canard_rx_queue; CanFrameTransmitFunc _transmit_func; std::map _rx_transfer_map; std::map _tx_transfer_map; @@ -108,6 +110,12 @@ class Node static void * o1heap_allocate(CanardInstance * const ins, size_t const amount); static void o1heap_free (CanardInstance * const ins, void * const pointer); + void handle_receive(); + void receiveOne(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us); + void handle_transmit(); + bool transmitOne(); + + CanardTransferID getNextTransferId(CanardPortID const port_id); bool subscribe (CanardTransferKind const transfer_kind, CanardPortID const port_id, size_t const payload_size_max, OnTransferReceivedFunc func); bool unsubscribe (CanardTransferKind const transfer_kind, CanardPortID const port_id); diff --git a/src/Node.ipp b/src/Node.ipp index 10fa4fb8..5068e73f 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -12,24 +12,18 @@ template bool Node::subscribe(OnTransferReceivedFunc func) { - LockGuard lock; - return subscribe(T::TRANSFER_KIND, T::PORT_ID, T::MAX_PAYLOAD_SIZE, func); } template bool Node::unsubscribe() { - LockGuard lock; - return unsubscribe(T::TRANSFER_KIND, T::PORT_ID); } template bool Node::publish(T_MSG const & msg) { - LockGuard lock; - static_assert(T_MSG::TRANSFER_KIND == CanardTransferKindMessage, "Node::publish API only works with CanardTransferKindMessage"); std::array payload_buf; @@ -43,8 +37,6 @@ bool Node::publish(T_MSG const & msg) template bool Node::respond(T_RSP const & rsp, CanardNodeID const remote_node_id, CanardTransferID const transfer_id) { - LockGuard lock; - static_assert(T_RSP::TRANSFER_KIND == CanardTransferKindResponse, "Node::respond API only works with CanardTransferKindResponse"); std::array payload_buf; @@ -57,8 +49,6 @@ bool Node::respond(T_RSP const & rsp, CanardNodeID const remote_node_id, CanardT template bool Node::request(T_REQ const & req, CanardNodeID const remote_node_id, OnTransferReceivedFunc func) { - LockGuard lock; - static_assert(T_REQ::TRANSFER_KIND == CanardTransferKindRequest, "Node::request API - T_REQ != CanardTransferKindRequest"); static_assert(T_RSP::TRANSFER_KIND == CanardTransferKindResponse, "Node::request API - T_RSP != CanardTransferKindResponse"); diff --git a/src/utility/RingBuffer.hpp b/src/utility/RingBuffer.hpp new file mode 100644 index 00000000..d6a8007d --- /dev/null +++ b/src/utility/RingBuffer.hpp @@ -0,0 +1,134 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef ARDUINO_CYPHAL_UTILITY_RINGBUFFER_H_ +#define ARDUINO_CYPHAL_UTILITY_RINGBUFFER_H_ + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +namespace arduino +{ +namespace _107_ +{ +namespace opencyphal +{ + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +template +class Ringbuffer +{ +public: + Ringbuffer(size_t const size) + : _buffer{new T[size]} + , _size{size} + , _head{0} + , _tail{0} + , _num_elems{0} + { } + + ~Ringbuffer() + { + delete[] _buffer; + _size = 0; + _head = 0; + _tail = 0; + _num_elems = 0; + } + + void enqueue(T const & val) + { + if (isFull()) return; + _buffer[_head] = val; + _head = nextIndex(_head); + _num_elems++; + } + + T dequeue() + { + if (isEmpty()) return T{}; + T const val = _buffer[_tail]; + _tail = nextIndex(_tail); + _num_elems--; + return val; + } + + size_t available() const + { + return _num_elems; + } + + bool isFull() const + { + return (_num_elems == _size); + } + + bool isEmpty() const + { + return (_num_elems == 0); + } + +private: + T * _buffer; + size_t _size, _head, _tail, _num_elems; + + size_t nextIndex(size_t const index) + { + return ((index + 1) % _size); + } +}; + +template +class ThreadsafeRingBuffer : private Ringbuffer +{ +public: + ThreadsafeRingBuffer(size_t const size) : Ringbuffer(size) { } + + void enqueue(T const & val) + { + LockGuard lock; + Ringbuffer::enqueue(val); + } + + T dequeue() + { + LockGuard lock; + return Ringbuffer::dequeue(); + } + + size_t available() const + { + LockGuard lock; + return Ringbuffer::available(); + } + + bool isFull() const + { + LockGuard lock; + return Ringbuffer::isFull(); + } + + bool isEmpty() const + { + LockGuard lock; + return Ringbuffer::isEmpty(); + } +}; + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +} /* arduino */ +} /* _107_ */ +} /* opencyphal */ + +#endif /* ARDUINO_CYPHAL_UTILITY_RINGBUFFER_H_ */ From 35289950c052b93f55cdc0b65e29ec982767d7d4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 08:09:52 +0200 Subject: [PATCH 09/82] Minimal register list read example now existing. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index dc3b2323..2c8e7e5e 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -82,9 +82,9 @@ Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE(); static int const MKRCAN_MCP2515_CS_PIN = 3; static int const MKRCAN_MCP2515_INT_PIN = 7; -static SPISettings const MCP2515x_SPI_SETTING{1000000, MSBFIRST, SPI_MODE0}; +static SPISettings const MCP2515x_SPI_SETTING{10000000, MSBFIRST, SPI_MODE0}; -static CanardNodeID const CYPHAL_NODE_ID = 42; +static CanardNodeID const OPEN_CYPHAL_NODE_ID = 42; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; static OpenCyphalNodeData const OPEN_CYPHAL_NODE_INITIAL_DATA = @@ -120,7 +120,7 @@ ArduinoMCP2515 mcp2515([]() mcp2515_onReceiveBufferFull, nullptr); -Node node_hdl([](CanardFrame const & frame) { return mcp2515.transmit(frame); }, CYPHAL_NODE_ID); +Node node_hdl([](CanardFrame const & frame) { return mcp2515.transmit(frame); }, OPEN_CYPHAL_NODE_ID); OpenCyphalNodeData node_data = OPEN_CYPHAL_NODE_INITIAL_DATA; OpenCyphalNodeConfiguration node_config = OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION; @@ -183,7 +183,7 @@ void setup() /* Configure MCP2515 */ mcp2515.begin(); - mcp2515.setBitRate(CanBitRate::BR_1000kBPS_16MHZ); + mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ); mcp2515.setNormalMode(); /* Configure TMF8801 @@ -195,13 +195,17 @@ void setup() /* Register callbacks for node info and register api. */ - node_hdl.subscribe>(onAccess_1_0_Request_Received); node_hdl.subscribe>(onList_1_0_Request_Received); node_hdl.subscribe>(onGetInfo_1_0_Request_Received); + node_hdl.subscribe>(onAccess_1_0_Request_Received); } void loop() { + /* Process all pending OpenCyphal actions. + */ + node_hdl.spin(); + /* Handle actions common to all states. */ unsigned long const now = millis(); @@ -228,9 +232,6 @@ void loop() } node_data.mode = next_mode; - - /* Transmit all enqeued CAN frames */ - while(node_hdl.transmitCanFrame()) { } } /************************************************************************************** @@ -263,9 +264,20 @@ void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_h void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) { Access_1_0::Request<> const req = Access_1_0::Request<>::deserialize(transfer); + const char * reg_name = reinterpret_cast(req.data.name.name.elements); + DBG_INFO("onAccess_1_0_Request_Received: reg: %s", reg_name); - DBG_INFO("onAccess_1_0_Request_Received: reg %s", - req.data.name.name.elements); + if (!strncmp(reg_name, reinterpret_cast(register_list1.name.name.elements), register_list1.name.name.count)) + { + Access_1_0::Response<> rsp; + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = false; + rsp.data.persistent = true; + rsp.data.value.natural8.value.elements[0] = OPEN_CYPHAL_NODE_ID; + rsp.data.value.natural8.value.count = 1; + DBG_INFO("remote_node_id: %d, transfer_id: %d", transfer.metadata.remote_node_id, transfer.metadata.transfer_id); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); + } } void publish_heartbeat(Node & u, uint32_t const uptime, Heartbeat_1_0<>::Mode const mode) From 27e89da350bc35d07a2d25fa1335b02e575b1120 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 08:30:06 +0200 Subject: [PATCH 10/82] Move check for a registered transmit function to a different space in code. --- src/Node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Node.cpp b/src/Node.cpp index 2bcc8b05..4465eb08 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -133,9 +133,6 @@ void Node::handle_transmit() bool Node::transmitOne() { - if (!_transmit_func) - return false; - CanardTxQueueItem const * tx_queue_item = canardTxPeek(&_canard_tx_queue); if (tx_queue_item == nullptr) @@ -186,6 +183,9 @@ bool Node::unsubscribe(CanardTransferKind const transfer_kind, CanardPortID cons bool Node::enqeueTransfer(CanardNodeID const remote_node_id, CanardTransferKind const transfer_kind, CanardPortID const port_id, size_t const payload_size, void * payload, CanardTransferID const transfer_id) { + if (!_transmit_func) + return false; + CanardTransferMetadata const transfer_metadata = { .priority = CanardPriorityNominal, From 1789705616fccb95d2a66a7d263cf6a6945946f4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 08:35:13 +0200 Subject: [PATCH 11/82] Simplify transmit queue processing. --- src/Node.cpp | 26 ++++++++++---------------- src/Node.h | 3 +-- 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/src/Node.cpp b/src/Node.cpp index 4465eb08..52062c0b 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -47,7 +47,7 @@ CanardNodeID Node::getNodeId() const void Node::spin() { handle_receive(); - handle_transmit(); + processTxQueue(); } void Node::onCanFrameReceived(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us) @@ -126,23 +126,17 @@ void Node::receiveOne(CanardFrame const & frame, CanardMicrosecond const & rx_ti } } -void Node::handle_transmit() +void Node::processTxQueue() { - while (transmitOne()) { } -} - -bool Node::transmitOne() -{ - CanardTxQueueItem const * tx_queue_item = canardTxPeek(&_canard_tx_queue); - - if (tx_queue_item == nullptr) - return false; - - if (!_transmit_func(tx_queue_item->frame)) - return false; + for(CanardTxQueueItem * tx_queue_item = const_cast(canardTxPeek(&_canard_tx_queue)); + tx_queue_item != nullptr; + tx_queue_item = const_cast(canardTxPeek(&_canard_tx_queue))) + { + if (!_transmit_func(tx_queue_item->frame)) + return; - _canard_hdl.memory_free(&_canard_hdl, canardTxPop(&_canard_tx_queue, tx_queue_item)); - return true; + _canard_hdl.memory_free(&_canard_hdl, canardTxPop(&_canard_tx_queue, tx_queue_item)); + } } CanardTransferID Node::getNextTransferId(CanardPortID const port_id) diff --git a/src/Node.h b/src/Node.h index 1b0e7252..bfe83ed0 100644 --- a/src/Node.h +++ b/src/Node.h @@ -112,8 +112,7 @@ class Node void handle_receive(); void receiveOne(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us); - void handle_transmit(); - bool transmitOne(); + void processTxQueue(); CanardTransferID getNextTransferId(CanardPortID const port_id); From 59dcd04a4af34c5c2aa846658c58f37ff2cf5678 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 08:38:31 +0200 Subject: [PATCH 12/82] Simplify receive queue processing. --- src/Node.cpp | 57 ++++++++++++++++++++++------------------------------ src/Node.h | 4 +--- 2 files changed, 25 insertions(+), 36 deletions(-) diff --git a/src/Node.cpp b/src/Node.cpp index 52062c0b..57b27b5f 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -42,11 +42,9 @@ CanardNodeID Node::getNodeId() const return _canard_hdl.node_id; } -#include - void Node::spin() { - handle_receive(); + processRxQueue(); processTxQueue(); } @@ -78,7 +76,7 @@ void Node::o1heap_free(CanardInstance * const ins, void * const pointer) o1heap->free(pointer); } -void Node::handle_receive() +void Node::processRxQueue() { while (!_canard_rx_queue.isEmpty()) { @@ -89,40 +87,33 @@ void Node::handle_receive() frame.payload_size = payload_size; frame.payload = reinterpret_cast(payload.data()); - receiveOne(frame, micros()); - } -} - -void Node::receiveOne(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us) -{ - Serial.println(frame.extended_can_id, HEX); - - CanardRxTransfer transfer; - int8_t const result = canardRxAccept(&_canard_hdl, - rx_timestamp_us, - &frame, - 0, /* redundant_transport_index */ - &transfer, - nullptr); + CanardRxTransfer transfer; + int8_t const result = canardRxAccept(&_canard_hdl, + rx_timestamp_us, + &frame, + 0, /* redundant_transport_index */ + &transfer, + nullptr); - if(result == 1) - { - if (_rx_transfer_map.count(transfer.metadata.port_id) > 0) + if(result == 1) { - OnTransferReceivedFunc transfer_received_func = _rx_transfer_map[transfer.metadata.port_id].transfer_complete_callback; - - if (transfer.metadata.transfer_kind == CanardTransferKindResponse) { - if ((_tx_transfer_map.count(transfer.metadata.port_id) > 0) && (_tx_transfer_map[transfer.metadata.port_id] == transfer.metadata.transfer_id)) - { - transfer_received_func(transfer, *this); - unsubscribe(CanardTransferKindResponse, transfer.metadata.port_id); + if (_rx_transfer_map.count(transfer.metadata.port_id) > 0) + { + OnTransferReceivedFunc transfer_received_func = _rx_transfer_map[transfer.metadata.port_id].transfer_complete_callback; + + if (transfer.metadata.transfer_kind == CanardTransferKindResponse) { + if ((_tx_transfer_map.count(transfer.metadata.port_id) > 0) && (_tx_transfer_map[transfer.metadata.port_id] == transfer.metadata.transfer_id)) + { + transfer_received_func(transfer, *this); + unsubscribe(CanardTransferKindResponse, transfer.metadata.port_id); + } } + else + transfer_received_func(transfer, *this); } - else - transfer_received_func(transfer, *this); + /* Free dynamically allocated memory after processing. */ + _canard_hdl.memory_free(&_canard_hdl, transfer.payload); } - /* Free dynamically allocated memory after processing. */ - _canard_hdl.memory_free(&_canard_hdl, transfer.payload); } } diff --git a/src/Node.h b/src/Node.h index bfe83ed0..9201e5de 100644 --- a/src/Node.h +++ b/src/Node.h @@ -110,11 +110,9 @@ class Node static void * o1heap_allocate(CanardInstance * const ins, size_t const amount); static void o1heap_free (CanardInstance * const ins, void * const pointer); - void handle_receive(); - void receiveOne(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us); + void processRxQueue(); void processTxQueue(); - CanardTransferID getNextTransferId(CanardPortID const port_id); bool subscribe (CanardTransferKind const transfer_kind, CanardPortID const port_id, size_t const payload_size_max, OnTransferReceivedFunc func); bool unsubscribe (CanardTransferKind const transfer_kind, CanardPortID const port_id); From 52276372bdf68bd011a394da31d1ba73887b3c29 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 11:17:51 +0200 Subject: [PATCH 13/82] Update now async processing API across all examples. --- examples/OpenCyphal-Blink/OpenCyphal-Blink.ino | 7 ++++--- examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino | 7 ++++--- .../OpenCyphal-Heartbeat-Publish.ino | 7 ++++--- .../OpenCyphal-Heartbeat-Subscribe.ino | 4 +++- .../OpenCyphal-Service-Client.ino | 5 +++-- .../OpenCyphal-Service-Server.ino | 5 +++-- 6 files changed, 21 insertions(+), 14 deletions(-) diff --git a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino index 1a81ec9a..b4d2a1b7 100644 --- a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino +++ b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino @@ -116,6 +116,10 @@ void setup() void loop() { + /* Process all pending OpenCyphal actions. + */ + node_hdl.spin(); + /* Update the heartbeat object */ hb.data.uptime = millis() / 1000; hb = Heartbeat_1_0<>::Mode::OPERATIONAL; @@ -127,9 +131,6 @@ void loop() node_hdl.publish(hb); prev = now; } - - /* Transmit all enqeued CAN frames */ - while(node_hdl.transmitCanFrame()) { } } /************************************************************************************** diff --git a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino index 8ce21660..41d10863 100644 --- a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino +++ b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino @@ -143,6 +143,10 @@ void setup() void loop() { + /* Process all pending OpenCyphal actions. + */ + node_hdl.spin(); + /* Handle actions common to all states. */ unsigned long const now = millis(); @@ -169,9 +173,6 @@ void loop() } node_data.mode = next_mode; - - /* Transmit all enqeued CAN frames */ - while(node_hdl.transmitCanFrame()) { } } /************************************************************************************** diff --git a/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino b/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino index ebf69ce3..67daf99e 100644 --- a/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino +++ b/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino @@ -75,6 +75,10 @@ void setup() void loop() { + /* Process all pending OpenCyphal actions. + */ + node_hdl.spin(); + /* Update the heartbeat object */ hb.data.uptime = millis() / 1000; hb = Heartbeat_1_0<>::Mode::OPERATIONAL; @@ -86,7 +90,4 @@ void loop() node_hdl.publish(hb); prev = now; } - - /* Transmit all enqeued CAN frames */ - while(node_hdl.transmitCanFrame()) { } } diff --git a/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino b/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino index 3ffb3cfb..7c648d48 100644 --- a/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino +++ b/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino @@ -77,7 +77,9 @@ void setup() void loop() { - + /* Process all pending OpenCyphal actions. + */ + node_hdl.spin(); } /************************************************************************************** diff --git a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino index de809384..bd848c0f 100644 --- a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino +++ b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino @@ -86,8 +86,9 @@ void setup() void loop() { - /* Transmit all enqeued CAN frames */ - while(node_hdl.transmitCanFrame()) { } + /* Process all pending OpenCyphal actions. + */ + node_hdl.spin(); } /************************************************************************************** diff --git a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino index bbad8e3b..7ea235b5 100644 --- a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino +++ b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino @@ -78,8 +78,9 @@ void setup() void loop() { - /* Transmit all enqeued CAN frames */ - while(node_hdl.transmitCanFrame()) { } + /* Process all pending OpenCyphal actions. + */ + node_hdl.spin(); } /************************************************************************************** From 956f553dd8c7b4feeae0df91aef3777c6d77bb12 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 11:19:12 +0200 Subject: [PATCH 14/82] Rename API spin to spinSome. This indicates that are couple of loops are run (vs i.e. spinOnce), but that the function ultimately returns. --- examples/OpenCyphal-Blink/OpenCyphal-Blink.ino | 2 +- examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino | 2 +- .../OpenCyphal-Heartbeat-Publish.ino | 2 +- .../OpenCyphal-Heartbeat-Subscribe.ino | 2 +- .../OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino | 2 +- .../OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino | 2 +- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 2 +- src/Node.cpp | 2 +- src/Node.h | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino index b4d2a1b7..4dc9b4f7 100644 --- a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino +++ b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino @@ -118,7 +118,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spin(); + node_hdl.spinSome(); /* Update the heartbeat object */ hb.data.uptime = millis() / 1000; diff --git a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino index 41d10863..48b4be8c 100644 --- a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino +++ b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino @@ -145,7 +145,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spin(); + node_hdl.spinSome(); /* Handle actions common to all states. */ diff --git a/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino b/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino index 67daf99e..a0913393 100644 --- a/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino +++ b/examples/OpenCyphal-Heartbeat-Publish/OpenCyphal-Heartbeat-Publish.ino @@ -77,7 +77,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spin(); + node_hdl.spinSome(); /* Update the heartbeat object */ hb.data.uptime = millis() / 1000; diff --git a/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino b/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino index 7c648d48..a602c244 100644 --- a/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino +++ b/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino @@ -79,7 +79,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spin(); + node_hdl.spinSome(); } /************************************************************************************** diff --git a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino index bd848c0f..2b18f888 100644 --- a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino +++ b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino @@ -88,7 +88,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spin(); + node_hdl.spinSome(); } /************************************************************************************** diff --git a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino index 7ea235b5..0c75de64 100644 --- a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino +++ b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino @@ -80,7 +80,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spin(); + node_hdl.spinSome(); } /************************************************************************************** diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 2c8e7e5e..4d4a31a5 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -204,7 +204,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spin(); + node_hdl.spinSome(); /* Handle actions common to all states. */ diff --git a/src/Node.cpp b/src/Node.cpp index 57b27b5f..9786abb2 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -42,7 +42,7 @@ CanardNodeID Node::getNodeId() const return _canard_hdl.node_id; } -void Node::spin() +void Node::spinSome() { processRxQueue(); processTxQueue(); diff --git a/src/Node.h b/src/Node.h index 9201e5de..d621e0da 100644 --- a/src/Node.h +++ b/src/Node.h @@ -71,7 +71,7 @@ class Node /* Must be called from the application to process * all received CAN frames. */ - void spin(); + void spinSome(); /* Must be called from the application upon the * reception of a can frame. */ From 64f72080db8ce0e73532ddf153f282d00ec39fc3 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 11:19:52 +0200 Subject: [PATCH 15/82] Replace obsolete keyword transmitCanFrame with spinSome. --- keywords.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/keywords.txt b/keywords.txt index 01e5ee53..0272ce45 100644 --- a/keywords.txt +++ b/keywords.txt @@ -15,7 +15,7 @@ Response KEYWORD1 ####################################### onCanFrameReceived KEYWORD2 -transmitCanFrame KEYWORD2 +spinSome KEYWORD2 subscribe KEYWORD2 publish KEYWORD2 respond KEYWORD2 From 343d1b755a75ef6b2009c6fb470f1c3b81a4590c Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 12:23:24 +0200 Subject: [PATCH 16/82] Fix: Select the correct type of the access response. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 4d4a31a5..d1038b8b 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -270,12 +270,14 @@ void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & nod if (!strncmp(reg_name, reinterpret_cast(register_list1.name.name.elements), register_list1.name.name.count)) { Access_1_0::Response<> rsp; + rsp.data.timestamp.microsecond = micros(); rsp.data._mutable = false; rsp.data.persistent = true; rsp.data.value.natural8.value.elements[0] = OPEN_CYPHAL_NODE_ID; rsp.data.value.natural8.value.count = 1; - DBG_INFO("remote_node_id: %d, transfer_id: %d", transfer.metadata.remote_node_id, transfer.metadata.transfer_id); + uavcan_register_Value_1_0_select_natural8_(&rsp.data.value); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } } From 752953aa1e205626c6ea108af121c7b8188dacd1 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 16 Sep 2022 13:09:25 +0200 Subject: [PATCH 17/82] Update README. --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index f9cdf3c4..6edc2565 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,9 @@ Node node_hdl([](CanardFrame const & frame) { /* ... */ }); Heartbeat_1_0 hb; /* ... */ void loop() { + /* Process all pending OpenCyphal actions. */ + node_hdl.spinSome(); + /* Update the heartbeat object */ hb.uptime(millis() / 1000); hb.mode = Heartbeat_1_0::Mode::OPERATIONAL; @@ -49,9 +52,6 @@ void loop() { node_hdl.publish(hb); prev = now; } - - /* Transmit all enqeued CAN frames */ - while(node_hdl.transmitCanFrame()) { } } ``` From a91fc446b2ad16e1138f7bc762e1fac57ebba6db Mon Sep 17 00:00:00 2001 From: generationmake Date: Sun, 18 Sep 2022 08:54:06 +0200 Subject: [PATCH 18/82] made uavcan.node.id and uavcan.node.description writeable --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 35 +++++++++++++++++-- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index d1038b8b..e6f06f42 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -85,6 +85,7 @@ static int const MKRCAN_MCP2515_INT_PIN = 7; static SPISettings const MCP2515x_SPI_SETTING{10000000, MSBFIRST, SPI_MODE0}; static CanardNodeID const OPEN_CYPHAL_NODE_ID = 42; +static CanardNodeID OPEN_CYPHAL_NODE_ID_volatile = OPEN_CYPHAL_NODE_ID; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; static OpenCyphalNodeData const OPEN_CYPHAL_NODE_INITIAL_DATA = @@ -153,6 +154,8 @@ drone::ArduinoTMF8801 tmf8801([](uint8_t const i2c_slave_addr, uint8_t const reg DEBUG_INSTANCE(120, Serial); +uint8_t node_description_str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_]="My litte ToF-sensor-node"; + /************************************************************************************** * SETUP/LOOP **************************************************************************************/ @@ -269,15 +272,41 @@ void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & nod if (!strncmp(reg_name, reinterpret_cast(register_list1.name.name.elements), register_list1.name.name.count)) { + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) + { + DBG_INFO("writing uavcan.node.id"); + OPEN_CYPHAL_NODE_ID_volatile=req.data.value.natural8.value.elements[0]; + } + Access_1_0::Response<> rsp; rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = false; - rsp.data.persistent = true; - rsp.data.value.natural8.value.elements[0] = OPEN_CYPHAL_NODE_ID; + rsp.data._mutable = true; + rsp.data.persistent = false; + rsp.data.value.natural8.value.elements[0] = OPEN_CYPHAL_NODE_ID_volatile; rsp.data.value.natural8.value.count = 1; uavcan_register_Value_1_0_select_natural8_(&rsp.data.value); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); + } + else if (!strncmp(reg_name, reinterpret_cast(register_list2.name.name.elements), register_list2.name.name.count)) + { + if(uavcan_register_Value_1_0_is_string_(&req.data.value)) + { + DBG_INFO("writing uavcan.node.description"); + strncpy((char *)node_description_str, (const char *)req.data.value._string.value.elements, req.data.value._string.value.count); + node_description_str[req.data.value._string.value.count]=0; + } + + Access_1_0::Response<> rsp; + + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = true; + rsp.data.persistent = false; + strncpy((char *)rsp.data.value._string.value.elements, (const char *)node_description_str, strlen((const char *)node_description_str)); + rsp.data.value._string.value.count = strlen((const char *)node_description_str); + uavcan_register_Value_1_0_select_string_(&rsp.data.value); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } } From 35e8fb70719d1c25b8c0c8bb29391dce901c9aec Mon Sep 17 00:00:00 2001 From: generationmake Date: Sun, 18 Sep 2022 09:05:53 +0200 Subject: [PATCH 19/82] corrected spelling --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index e6f06f42..8c2c7ca2 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -154,7 +154,7 @@ drone::ArduinoTMF8801 tmf8801([](uint8_t const i2c_slave_addr, uint8_t const reg DEBUG_INSTANCE(120, Serial); -uint8_t node_description_str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_]="My litte ToF-sensor-node"; +uint8_t node_description_str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_]="My little ToF-sensor-node"; /************************************************************************************** * SETUP/LOOP From a419c0e8786e80664f58325ae7959afa5944f548 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 19 Sep 2022 14:08:32 +0200 Subject: [PATCH 20/82] Fix: Connect TMF8801 INT pin to D6 and capture events via attachInterrupt. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 8c2c7ca2..8d68da19 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -80,7 +80,8 @@ Heartbeat_1_0<>::Mode handle_SOFTWARE_UPDATE(); * CONSTANTS **************************************************************************************/ -static int const MKRCAN_MCP2515_CS_PIN = 3; +static int const MKRCAN_MCP2515_CS_PIN = 3; +static int const TMF8801_INT_PIN = 6; static int const MKRCAN_MCP2515_INT_PIN = 7; static SPISettings const MCP2515x_SPI_SETTING{10000000, MSBFIRST, SPI_MODE0}; @@ -171,6 +172,12 @@ void setup() */ Wire.begin(); + /* Attach interrupt handler to obtain events + * signalled by the TMF8801. + */ + pinMode(TMF8801_INT_PIN, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(TMF8801_INT_PIN), [](){ tmf8801.onExternalEventHandler(); }, FALLING); + /* Setup SPI access */ SPI.begin(); From 4c2a3e8687f119e85ba7923c0f4f3c31a06532a4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 20 Sep 2022 13:27:07 +0200 Subject: [PATCH 21/82] Very basic idea off template-class based register api. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 6 +- .../Register.h | 86 ++++++++++++++++--- 2 files changed, 79 insertions(+), 13 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 8d68da19..baebb304 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -267,7 +267,7 @@ void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_h DBG_INFO("onList_1_0_Request_Received: index %d", req.data.index); List_1_0::Response<> rsp = List_1_0::Response<>(); - memcpy(&rsp.data, REGISTER_LIST_ARRAY + req.data.index, sizeof(uavcan_register_List_Response_1_0)); + REGISTER_LIST_ARRAY[req.data.index]->toListResponse(&rsp.data); node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } @@ -277,7 +277,7 @@ void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & nod const char * reg_name = reinterpret_cast(req.data.name.name.elements); DBG_INFO("onAccess_1_0_Request_Received: reg: %s", reg_name); - if (!strncmp(reg_name, reinterpret_cast(register_list1.name.name.elements), register_list1.name.name.count)) + if (*REGISTER_LIST_ARRAY[0] == req.data.name) { if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { @@ -296,7 +296,7 @@ void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & nod node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } - else if (!strncmp(reg_name, reinterpret_cast(register_list2.name.name.elements), register_list2.name.name.count)) + if (*REGISTER_LIST_ARRAY[1] == req.data.name) { if(uavcan_register_Value_1_0_is_string_(&req.data.value)) { diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h index b1762698..61eead18 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h @@ -26,22 +26,88 @@ * CONSTANTS **************************************************************************************/ -static const uavcan_register_List_Response_1_0 register_list1 = { - { "uavcan.node.id", strlen("uavcan.node.id") }, +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class RegisterBase +{ +public: + RegisterBase(std::string const & name) + : _name{name} + { } + + void toListResponse(uavcan_register_List_Response_1_0 * rsp_ptr) + { + size_t const bytes_to_copy = std::min(_name.size(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(&(rsp_ptr->name.name.elements), _name.c_str(), bytes_to_copy); + rsp_ptr->name.name.count = bytes_to_copy; + } + + bool operator == (uavcan_register_Name_1_0 const & reg_name) + { + return (strncmp(_name.c_str(), reinterpret_cast(reg_name.name.elements), reg_name.name.count) == 0); + } + +private: + std::string const _name; }; -static const uavcan_register_List_Response_1_0 register_list2 = { - { "uavcan.node.description", strlen("uavcan.node.description") }, + +template +class RegisterReadOnly : RegisterBase +{ +public: + + RegisterReadOnly(std::string const & name, + T const & initial_val) + : RegisterBase{name} + , _val{initial_val} + { } + + inline T get() const { return _val; } + +protected: + inline void set(T const & val) { _val = val; } + +private: + T _val; }; -static const uavcan_register_List_Response_1_0 register_list_last = { - { "", 0 }, + +template +class RegisterReadWrite : public RegisterReadOnly +{ +public: + typedef std::function const &)> OnRegisterValueChangeFunc; + + RegisterReadWrite(std::string const & name, + T const & initial_val, + OnRegisterValueChangeFunc func) + : RegisterReadOnly{name, initial_val} + , _func{func} + { } + + inline void set(T const & val) + { + RegisterReadOnly::set(val); + if (_func) + _func(*this); + } + +private: + OnRegisterValueChangeFunc _func; }; -static const uavcan_register_List_Response_1_0 REGISTER_LIST_ARRAY[] = +static RegisterReadWrite reg_rw_uavcan_node_id ("uavcan.node.id", 42, nullptr); +static RegisterReadOnly reg_ro_uavcan_node_description("uavcan.node.description", "OpenCyphal-ToF-Distance-Sensor-Node"); +static RegisterBase reg_last (""); + +static RegisterBase * REGISTER_LIST_ARRAY[] = { - register_list1, - register_list2, - register_list_last + reinterpret_cast(®_rw_uavcan_node_id), + reinterpret_cast(®_ro_uavcan_node_description), + reinterpret_cast(®_last) }; static size_t const REGISTER_LIST_ARRAY_SIZE = sizeof(REGISTER_LIST_ARRAY) / sizeof(REGISTER_LIST_ARRAY[0]); + #endif /* REGISTER_H_ */ From 19c6405a7a4b5f495ea58ad8b29136a1f4c392cd Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 20 Sep 2022 13:46:01 +0200 Subject: [PATCH 22/82] Replace std::string with C-String eases comparison and reduces comparison overhead. --- .../Register.h | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h index 61eead18..ec1caf11 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h @@ -33,24 +33,30 @@ class RegisterBase { public: - RegisterBase(std::string const & name) - : _name{name} - { } + RegisterBase(char const * name) + { + _name.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(_name.name.elements, name, _name.name.count); + } void toListResponse(uavcan_register_List_Response_1_0 * rsp_ptr) { - size_t const bytes_to_copy = std::min(_name.size(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(&(rsp_ptr->name.name.elements), _name.c_str(), bytes_to_copy); - rsp_ptr->name.name.count = bytes_to_copy; + memcpy(&rsp_ptr->name.name.elements, _name.name.elements, _name.name.count); + rsp_ptr->name.name.count = _name.name.count; } bool operator == (uavcan_register_Name_1_0 const & reg_name) { - return (strncmp(_name.c_str(), reinterpret_cast(reg_name.name.elements), reg_name.name.count) == 0); + if (reg_name.name.count != _name.name.count) + return false; + else + return (strncmp(reinterpret_cast(_name.name.elements), + reinterpret_cast(reg_name.name.elements), + reg_name.name.count) == 0); } private: - std::string const _name; + uavcan_register_Name_1_0 _name; }; template @@ -58,7 +64,7 @@ class RegisterReadOnly : RegisterBase { public: - RegisterReadOnly(std::string const & name, + RegisterReadOnly(char const * name, T const & initial_val) : RegisterBase{name} , _val{initial_val} @@ -79,7 +85,7 @@ class RegisterReadWrite : public RegisterReadOnly public: typedef std::function const &)> OnRegisterValueChangeFunc; - RegisterReadWrite(std::string const & name, + RegisterReadWrite(char const * name, T const & initial_val, OnRegisterValueChangeFunc func) : RegisterReadOnly{name, initial_val} From d29aaa3bb08ebaffe78ffaba227e65cf77063b24 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 20 Sep 2022 13:52:03 +0200 Subject: [PATCH 23/82] Directly create a List response. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 3 +-- examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h | 8 +++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index baebb304..cb4bc9b4 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -266,8 +266,7 @@ void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_h List_1_0::Request<> const req = List_1_0::Request<>::deserialize(transfer); DBG_INFO("onList_1_0_Request_Received: index %d", req.data.index); - List_1_0::Response<> rsp = List_1_0::Response<>(); - REGISTER_LIST_ARRAY[req.data.index]->toListResponse(&rsp.data); + List_1_0::Response<> const rsp = REGISTER_LIST_ARRAY[req.data.index]->toListResponse(); node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h index ec1caf11..67e14d92 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h @@ -39,10 +39,12 @@ class RegisterBase memcpy(_name.name.elements, name, _name.name.count); } - void toListResponse(uavcan_register_List_Response_1_0 * rsp_ptr) + uavcan::_register::List_1_0::Response<> toListResponse() const { - memcpy(&rsp_ptr->name.name.elements, _name.name.elements, _name.name.count); - rsp_ptr->name.name.count = _name.name.count; + uavcan::_register::List_1_0::Response<> rsp; + memcpy(&rsp.data.name.name.elements, _name.name.elements, _name.name.count); + rsp.data.name.name.count = _name.name.count; + return rsp; } bool operator == (uavcan_register_Name_1_0 const & reg_name) From f27a69f90c4a1e030bd7252c8f68c2bf2d264014 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 20 Sep 2022 14:55:47 +0200 Subject: [PATCH 24/82] Bare-minimum very fugly register API. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 31 +-------- .../Register.h | 67 ++++++++++++++++++- 2 files changed, 68 insertions(+), 30 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index cb4bc9b4..c089216f 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,8 +155,6 @@ drone::ArduinoTMF8801 tmf8801([](uint8_t const i2c_slave_addr, uint8_t const reg DEBUG_INSTANCE(120, Serial); -uint8_t node_description_str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_]="My little ToF-sensor-node"; - /************************************************************************************** * SETUP/LOOP **************************************************************************************/ @@ -281,38 +279,15 @@ void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & nod if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { DBG_INFO("writing uavcan.node.id"); - OPEN_CYPHAL_NODE_ID_volatile=req.data.value.natural8.value.elements[0]; + reinterpret_cast *>(REGISTER_LIST_ARRAY[0])->set(req.data.value); } - Access_1_0::Response<> rsp; - - rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = true; - rsp.data.persistent = false; - rsp.data.value.natural8.value.elements[0] = OPEN_CYPHAL_NODE_ID_volatile; - rsp.data.value.natural8.value.count = 1; - uavcan_register_Value_1_0_select_natural8_(&rsp.data.value); - + Access_1_0::Response<> const rsp = REGISTER_LIST_ARRAY[0]->toAccessResponse(); node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } if (*REGISTER_LIST_ARRAY[1] == req.data.name) { - if(uavcan_register_Value_1_0_is_string_(&req.data.value)) - { - DBG_INFO("writing uavcan.node.description"); - strncpy((char *)node_description_str, (const char *)req.data.value._string.value.elements, req.data.value._string.value.count); - node_description_str[req.data.value._string.value.count]=0; - } - - Access_1_0::Response<> rsp; - - rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = true; - rsp.data.persistent = false; - strncpy((char *)rsp.data.value._string.value.elements, (const char *)node_description_str, strlen((const char *)node_description_str)); - rsp.data.value._string.value.count = strlen((const char *)node_description_str); - uavcan_register_Value_1_0_select_string_(&rsp.data.value); - + Access_1_0::Response<> const rsp = REGISTER_LIST_ARRAY[1]->toAccessResponse(); node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } } diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h index 67e14d92..8a2a705e 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h @@ -38,6 +38,7 @@ class RegisterBase _name.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); memcpy(_name.name.elements, name, _name.name.count); } + virtual ~RegisterBase() { } uavcan::_register::List_1_0::Response<> toListResponse() const { @@ -47,6 +48,12 @@ class RegisterBase return rsp; } + virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() + { + uavcan::_register::Access_1_0::Response<> rsp; + return rsp; + } + bool operator == (uavcan_register_Name_1_0 const & reg_name) { if (reg_name.name.count != _name.name.count) @@ -61,6 +68,41 @@ class RegisterBase uavcan_register_Name_1_0 _name; }; +template uavcan_register_Value_1_0 toRegisterValue(T const & val); + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural8.value.elements[0] = val; + reg_val.natural8.value.count = 1; + uavcan_register_Value_1_0_select_natural8_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); + uavcan_register_Value_1_0_select_string_(®_val); + return reg_val; +} + +template T fromRegisterValue(uavcan_register_Value_1_0 const & val); + +template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural8.value.elements[0]; +} + +template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; + memcpy(str, val._string.value.elements, val._string.value.count); + str[val._string.value.count + 1] = '\0'; + return std::string(str); +} + template class RegisterReadOnly : RegisterBase { @@ -71,9 +113,22 @@ class RegisterReadOnly : RegisterBase : RegisterBase{name} , _val{initial_val} { } + virtual ~RegisterReadOnly() { } inline T get() const { return _val; } + virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() + { + uavcan::_register::Access_1_0::Response<> rsp; + + rsp.data.value = toRegisterValue(_val); + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = false; + rsp.data.persistent = false; + + return rsp; + } + protected: inline void set(T const & val) { _val = val; } @@ -93,10 +148,18 @@ class RegisterReadWrite : public RegisterReadOnly : RegisterReadOnly{name, initial_val} , _func{func} { } + virtual ~RegisterReadWrite() { } + + virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() + { + uavcan::_register::Access_1_0::Response<> rsp = RegisterReadOnly::toAccessResponse(); + rsp.data._mutable = true; + return rsp; + } - inline void set(T const & val) + inline void set(uavcan_register_Value_1_0 const & val) { - RegisterReadOnly::set(val); + RegisterReadOnly::set(fromRegisterValue(val)); if (_func) _func(*this); } From 85d0caf296ae799033fc547f429084a57c710da3 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 20 Sep 2022 15:14:17 +0200 Subject: [PATCH 25/82] Moving register base classes over to library and cleaning up structure. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 17 +- .../Register.h | 184 ------------------ src/107-Arduino-Cyphal.h | 2 + src/register/RegisterBase.h | 61 ++++++ src/register/RegisterReadOnly.hpp | 56 ++++++ src/register/RegisterReadWrite.hpp | 56 ++++++ src/register/util/register_util.hpp | 62 ++++++ 7 files changed, 253 insertions(+), 185 deletions(-) delete mode 100644 examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h create mode 100644 src/register/RegisterBase.h create mode 100644 src/register/RegisterReadOnly.hpp create mode 100644 src/register/RegisterReadWrite.hpp create mode 100644 src/register/util/register_util.hpp diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index c089216f..d852c9eb 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -31,7 +31,6 @@ #include #include "NodeInfo.h" -#include "Register.h" /************************************************************************************** * NAMESPACE @@ -105,6 +104,22 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION * GLOBAL VARIABLES **************************************************************************************/ +/* REGISTER ***************************************************************************/ + +static RegisterReadWrite reg_rw_uavcan_node_id ("uavcan.node.id", 42, nullptr); +static RegisterReadOnly reg_ro_uavcan_node_description("uavcan.node.description", "OpenCyphal-ToF-Distance-Sensor-Node"); +static RegisterBase reg_last (""); + +static RegisterBase * REGISTER_LIST_ARRAY[] = +{ + reinterpret_cast(®_rw_uavcan_node_id), + reinterpret_cast(®_ro_uavcan_node_description), + reinterpret_cast(®_last) +}; +static size_t const REGISTER_LIST_ARRAY_SIZE = sizeof(REGISTER_LIST_ARRAY) / sizeof(REGISTER_LIST_ARRAY[0]); + +/* DRIVER *****************************************************************************/ + ArduinoMCP2515 mcp2515([]() { noInterrupts(); diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h b/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h deleted file mode 100644 index 8a2a705e..00000000 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/Register.h +++ /dev/null @@ -1,184 +0,0 @@ -/** - * This software is distributed under the terms of the MIT License. - * Copyright (c) 2020 LXRobotics. - * Author: Alexander Entinger - * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. - */ - -#ifndef REGISTER_H_ -#define REGISTER_H_ - -/************************************************************************************** - * INCLUDES - **************************************************************************************/ - -#include <107-Arduino-Cyphal.h> - -/************************************************************************************** - * DEFINES - **************************************************************************************/ - -/************************************************************************************** - * TYPEDEF - **************************************************************************************/ - -/************************************************************************************** - * CONSTANTS - **************************************************************************************/ - -/************************************************************************************** - * CLASS DECLARATION - **************************************************************************************/ - -class RegisterBase -{ -public: - RegisterBase(char const * name) - { - _name.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(_name.name.elements, name, _name.name.count); - } - virtual ~RegisterBase() { } - - uavcan::_register::List_1_0::Response<> toListResponse() const - { - uavcan::_register::List_1_0::Response<> rsp; - memcpy(&rsp.data.name.name.elements, _name.name.elements, _name.name.count); - rsp.data.name.name.count = _name.name.count; - return rsp; - } - - virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() - { - uavcan::_register::Access_1_0::Response<> rsp; - return rsp; - } - - bool operator == (uavcan_register_Name_1_0 const & reg_name) - { - if (reg_name.name.count != _name.name.count) - return false; - else - return (strncmp(reinterpret_cast(_name.name.elements), - reinterpret_cast(reg_name.name.elements), - reg_name.name.count) == 0); - } - -private: - uavcan_register_Name_1_0 _name; -}; - -template uavcan_register_Value_1_0 toRegisterValue(T const & val); - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural8.value.elements[0] = val; - reg_val.natural8.value.count = 1; - uavcan_register_Value_1_0_select_natural8_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); - uavcan_register_Value_1_0_select_string_(®_val); - return reg_val; -} - -template T fromRegisterValue(uavcan_register_Value_1_0 const & val); - -template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural8.value.elements[0]; -} - -template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; - memcpy(str, val._string.value.elements, val._string.value.count); - str[val._string.value.count + 1] = '\0'; - return std::string(str); -} - -template -class RegisterReadOnly : RegisterBase -{ -public: - - RegisterReadOnly(char const * name, - T const & initial_val) - : RegisterBase{name} - , _val{initial_val} - { } - virtual ~RegisterReadOnly() { } - - inline T get() const { return _val; } - - virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() - { - uavcan::_register::Access_1_0::Response<> rsp; - - rsp.data.value = toRegisterValue(_val); - rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = false; - rsp.data.persistent = false; - - return rsp; - } - -protected: - inline void set(T const & val) { _val = val; } - -private: - T _val; -}; - -template -class RegisterReadWrite : public RegisterReadOnly -{ -public: - typedef std::function const &)> OnRegisterValueChangeFunc; - - RegisterReadWrite(char const * name, - T const & initial_val, - OnRegisterValueChangeFunc func) - : RegisterReadOnly{name, initial_val} - , _func{func} - { } - virtual ~RegisterReadWrite() { } - - virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() - { - uavcan::_register::Access_1_0::Response<> rsp = RegisterReadOnly::toAccessResponse(); - rsp.data._mutable = true; - return rsp; - } - - inline void set(uavcan_register_Value_1_0 const & val) - { - RegisterReadOnly::set(fromRegisterValue(val)); - if (_func) - _func(*this); - } - -private: - OnRegisterValueChangeFunc _func; -}; - -static RegisterReadWrite reg_rw_uavcan_node_id ("uavcan.node.id", 42, nullptr); -static RegisterReadOnly reg_ro_uavcan_node_description("uavcan.node.description", "OpenCyphal-ToF-Distance-Sensor-Node"); -static RegisterBase reg_last (""); - -static RegisterBase * REGISTER_LIST_ARRAY[] = -{ - reinterpret_cast(®_rw_uavcan_node_id), - reinterpret_cast(®_ro_uavcan_node_description), - reinterpret_cast(®_last) -}; -static size_t const REGISTER_LIST_ARRAY_SIZE = sizeof(REGISTER_LIST_ARRAY) / sizeof(REGISTER_LIST_ARRAY[0]); - - -#endif /* REGISTER_H_ */ diff --git a/src/107-Arduino-Cyphal.h b/src/107-Arduino-Cyphal.h index a57ec607..45c457c6 100644 --- a/src/107-Arduino-Cyphal.h +++ b/src/107-Arduino-Cyphal.h @@ -14,5 +14,7 @@ #include "Node.h" #include "Types.h" +#include "register/RegisterReadOnly.hpp" +#include "register/RegisterReadWrite.hpp" #endif /* _107_ARDUINO_CYPHAL_H_ */ diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h new file mode 100644 index 00000000..c0fc766b --- /dev/null +++ b/src/register/RegisterBase.h @@ -0,0 +1,61 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_BASE_H_ +#define REGISTER_BASE_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "../Types.h" + +#include + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class RegisterBase +{ +public: + RegisterBase(char const * name) + { + _name.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(_name.name.elements, name, _name.name.count); + } + virtual ~RegisterBase() { } + + uavcan::_register::List_1_0::Response<> toListResponse() const + { + uavcan::_register::List_1_0::Response<> rsp; + memcpy(&rsp.data.name.name.elements, _name.name.elements, _name.name.count); + rsp.data.name.name.count = _name.name.count; + return rsp; + } + + virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() + { + uavcan::_register::Access_1_0::Response<> rsp; + return rsp; + } + + bool operator == (uavcan_register_Name_1_0 const & reg_name) + { + if (reg_name.name.count != _name.name.count) + return false; + else + return (strncmp(reinterpret_cast(_name.name.elements), + reinterpret_cast(reg_name.name.elements), + reg_name.name.count) == 0); + } + +private: + uavcan_register_Name_1_0 _name; +}; + +#endif /* REGISTER_BASE_H_ */ diff --git a/src/register/RegisterReadOnly.hpp b/src/register/RegisterReadOnly.hpp new file mode 100644 index 00000000..a8daf85d --- /dev/null +++ b/src/register/RegisterReadOnly.hpp @@ -0,0 +1,56 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_READ_ONLY_H_ +#define REGISTER_READ_ONLY_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "RegisterBase.h" + +#include "util/register_util.hpp" + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +template +class RegisterReadOnly : RegisterBase +{ +public: + + RegisterReadOnly(char const * name, + T const & initial_val) + : RegisterBase{name} + , _val{initial_val} + { } + virtual ~RegisterReadOnly() { } + + inline T get() const { return _val; } + + virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() + { + uavcan::_register::Access_1_0::Response<> rsp; + + rsp.data.value = toRegisterValue(_val); + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = false; + rsp.data.persistent = false; + + return rsp; + } + +protected: + inline void set(T const & val) { _val = val; } + +private: + T _val; +}; + +#endif /* REGISTER_READ_ONLY_H_ */ diff --git a/src/register/RegisterReadWrite.hpp b/src/register/RegisterReadWrite.hpp new file mode 100644 index 00000000..a2fa2b1b --- /dev/null +++ b/src/register/RegisterReadWrite.hpp @@ -0,0 +1,56 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_READ_WRITE_H_ +#define REGISTER_READ_WRITE_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "RegisterReadOnly.hpp" + +#include "util/register_util.hpp" + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +template +class RegisterReadWrite : public RegisterReadOnly +{ +public: + typedef std::function const &)> OnRegisterValueChangeFunc; + + RegisterReadWrite(char const * name, + T const & initial_val, + OnRegisterValueChangeFunc func) + : RegisterReadOnly{name, initial_val} + , _func{func} + { } + virtual ~RegisterReadWrite() { } + + virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() + { + uavcan::_register::Access_1_0::Response<> rsp = RegisterReadOnly::toAccessResponse(); + rsp.data._mutable = true; + return rsp; + } + + inline void set(uavcan_register_Value_1_0 const & val) + { + RegisterReadOnly::set(fromRegisterValue(val)); + if (_func) + _func(*this); + } + +private: + OnRegisterValueChangeFunc _func; +}; + +#endif /* REGISTER_READ_WRITE_H_ */ + diff --git a/src/register/util/register_util.hpp b/src/register/util/register_util.hpp new file mode 100644 index 00000000..2ce7ad25 --- /dev/null +++ b/src/register/util/register_util.hpp @@ -0,0 +1,62 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_UTIL_HPP_ +#define REGISTER_UTIL_HPP_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "../../Types.h" + +#include +#include + +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +template uavcan_register_Value_1_0 toRegisterValue(T const & val); +template T fromRegisterValue(uavcan_register_Value_1_0 const & val); + +/************************************************************************************** + * FUNCTION DEFINITION + **************************************************************************************/ + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural8.value.elements[0] = val; + reg_val.natural8.value.count = 1; + uavcan_register_Value_1_0_select_natural8_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); + uavcan_register_Value_1_0_select_string_(®_val); + return reg_val; +} + +template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural8.value.elements[0]; +} + +template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; + memcpy(str, val._string.value.elements, val._string.value.count); + str[val._string.value.count + 1] = '\0'; + return std::string(str); +} + +#endif /* REGISTER_UTIL_HPP_ */ From 10d9d2c1987b9ad637cf85909a7b826886a00aa5 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 07:04:55 +0200 Subject: [PATCH 26/82] Works, but not really. Fugly hack. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 52 +------- src/107-Arduino-Cyphal.h | 3 +- src/register/RegisterBase.h | 18 ++- src/register/RegisterList.h | 117 ++++++++++++++++++ src/register/RegisterReadOnly.hpp | 8 +- src/register/RegisterReadWrite.hpp | 2 +- 6 files changed, 149 insertions(+), 51 deletions(-) create mode 100644 src/register/RegisterList.h diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index d852c9eb..691532b7 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -63,9 +63,7 @@ typedef struct **************************************************************************************/ void mcp2515_onReceiveBufferFull(CanardFrame const &); -void onList_1_0_Request_Received(CanardRxTransfer const &, Node &); void onGetInfo_1_0_Request_Received(CanardRxTransfer const &, Node &); -void onAccess_1_0_Request_Received(CanardRxTransfer const &, Node &); void publish_heartbeat(Node &, uint32_t const, Heartbeat_1_0<>::Mode const); void publish_tofDistance(drone::unit::Length const l); @@ -108,15 +106,8 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION static RegisterReadWrite reg_rw_uavcan_node_id ("uavcan.node.id", 42, nullptr); static RegisterReadOnly reg_ro_uavcan_node_description("uavcan.node.description", "OpenCyphal-ToF-Distance-Sensor-Node"); -static RegisterBase reg_last (""); - -static RegisterBase * REGISTER_LIST_ARRAY[] = -{ - reinterpret_cast(®_rw_uavcan_node_id), - reinterpret_cast(®_ro_uavcan_node_description), - reinterpret_cast(®_last) -}; -static size_t const REGISTER_LIST_ARRAY_SIZE = sizeof(REGISTER_LIST_ARRAY) / sizeof(REGISTER_LIST_ARRAY[0]); +static RegisterBase reg_last ("", RegisterBase::AccessType::ReadOnly); +static RegisterList reg_list; /* DRIVER *****************************************************************************/ @@ -218,9 +209,11 @@ void setup() /* Register callbacks for node info and register api. */ - node_hdl.subscribe>(onList_1_0_Request_Received); node_hdl.subscribe>(onGetInfo_1_0_Request_Received); - node_hdl.subscribe>(onAccess_1_0_Request_Received); + reg_list.subscribe(node_hdl); + reg_list.add(reinterpret_cast(®_rw_uavcan_node_id)); + reg_list.add(reinterpret_cast(®_ro_uavcan_node_description)); + reg_list.add(reinterpret_cast(®_last)); } void loop() @@ -274,39 +267,6 @@ void onGetInfo_1_0_Request_Received(CanardRxTransfer const &transfer, Node & nod node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } -void onList_1_0_Request_Received(CanardRxTransfer const &transfer, Node & node_hdl) -{ - List_1_0::Request<> const req = List_1_0::Request<>::deserialize(transfer); - DBG_INFO("onList_1_0_Request_Received: index %d", req.data.index); - - List_1_0::Response<> const rsp = REGISTER_LIST_ARRAY[req.data.index]->toListResponse(); - node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); -} - -void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) -{ - Access_1_0::Request<> const req = Access_1_0::Request<>::deserialize(transfer); - const char * reg_name = reinterpret_cast(req.data.name.name.elements); - DBG_INFO("onAccess_1_0_Request_Received: reg: %s", reg_name); - - if (*REGISTER_LIST_ARRAY[0] == req.data.name) - { - if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) - { - DBG_INFO("writing uavcan.node.id"); - reinterpret_cast *>(REGISTER_LIST_ARRAY[0])->set(req.data.value); - } - - Access_1_0::Response<> const rsp = REGISTER_LIST_ARRAY[0]->toAccessResponse(); - node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); - } - if (*REGISTER_LIST_ARRAY[1] == req.data.name) - { - Access_1_0::Response<> const rsp = REGISTER_LIST_ARRAY[1]->toAccessResponse(); - node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); - } -} - void publish_heartbeat(Node & u, uint32_t const uptime, Heartbeat_1_0<>::Mode const mode) { Heartbeat_1_0<> hb; diff --git a/src/107-Arduino-Cyphal.h b/src/107-Arduino-Cyphal.h index 45c457c6..ea780b93 100644 --- a/src/107-Arduino-Cyphal.h +++ b/src/107-Arduino-Cyphal.h @@ -14,7 +14,6 @@ #include "Node.h" #include "Types.h" -#include "register/RegisterReadOnly.hpp" -#include "register/RegisterReadWrite.hpp" +#include "register/RegisterList.h" #endif /* _107_ARDUINO_CYPHAL_H_ */ diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index c0fc766b..2c65176c 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -23,13 +23,22 @@ class RegisterBase { public: - RegisterBase(char const * name) + enum class AccessType + { + ReadWrite, ReadOnly + }; + + RegisterBase(char const * name, AccessType const access_type) + : _access_type{access_type} { _name.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); memcpy(_name.name.elements, name, _name.name.count); } virtual ~RegisterBase() { } + + inline AccessType type() const { return _access_type; } + uavcan::_register::List_1_0::Response<> toListResponse() const { uavcan::_register::List_1_0::Response<> rsp; @@ -41,6 +50,12 @@ class RegisterBase virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() { uavcan::_register::Access_1_0::Response<> rsp; + + uavcan_register_Value_1_0_select_empty_(&rsp.data.value); + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = false; + rsp.data.persistent = false; + return rsp; } @@ -56,6 +71,7 @@ class RegisterBase private: uavcan_register_Name_1_0 _name; + AccessType const _access_type; }; #endif /* REGISTER_BASE_H_ */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h new file mode 100644 index 00000000..84785981 --- /dev/null +++ b/src/register/RegisterList.h @@ -0,0 +1,117 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_LIST_H_ +#define REGISTER_LIST_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include + +#include "Node.h" +#include "RegisterBase.h" +#include "RegisterReadOnly.hpp" +#include "RegisterReadWrite.hpp" + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class RegisterList +{ +public: + void subscribe(Node & node_hdl) + { + node_hdl.subscribe> + ([this](CanardRxTransfer const & transfer, Node & node_hdl) { this->onList_1_0_Request_Received(transfer, node_hdl); }); + node_hdl.subscribe> + ([this](CanardRxTransfer const & transfer, Node & node_hdl) { this->onAccess_1_0_Request_Received(transfer, node_hdl); }); + } + + void add(RegisterBase * reg_ptr) + { + _reg_list.push_back(reg_ptr); + } + + +private: + std::vector _reg_list; + + void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) + { + uavcan::_register::List_1_0::Request<> const req = uavcan::_register::List_1_0::Request<>::deserialize(transfer); + uavcan::_register::List_1_0::Response<> const rsp = _reg_list[req.data.index]->toListResponse(); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); + } + + void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) + { + uavcan::_register::Access_1_0::Request<> const req = uavcan::_register::Access_1_0::Request<>::deserialize(transfer); + + /* Find a register with the same register name within + * the register list. + */ + auto iter = std::find_if(std::begin(_reg_list), + std::end (_reg_list), + [req](RegisterBase * reg_ptr) + { + return ((*reg_ptr) == req.data.name); + }); + + uavcan::_register::Access_1_0::Response<> rsp; + + /* If not register can be found return + * an empty response. + */ + if (iter == std::end(_reg_list)) + { + uavcan_register_Value_1_0_select_empty_(&rsp.data.value); + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = false; + rsp.data.persistent = false; + Serial.println("no find entry"); + } + + /* Retrieve the content of the iterator for better + * understanding of the underlying code operation. + */ + RegisterBase * reg_ptr = *iter; + + /* If its a RO register, directly form the response + * and send it back. + */ + if (reg_ptr->type() == RegisterBase::AccessType::ReadOnly) + { + Serial.println("RO"); + } + + /* Perform a write operation if the value sent in + * the request is not empty. + */ + if (reg_ptr->type() == RegisterBase::AccessType::ReadWrite) + { + Serial.println("RW"); + + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) + { + RegisterReadWrite * rw_reg_ptr = reinterpret_cast *>(reg_ptr); + rw_reg_ptr->set(req.data.value); + rsp = rw_reg_ptr->toAccessResponse(); + Serial.println("RW uint8_t"); + } + /* TODO: Implement for all the other types ... */ + } + + /* Send the actual response. */ + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); + } + +}; + +#endif /* REGISTER_LIST_H_ */ \ No newline at end of file diff --git a/src/register/RegisterReadOnly.hpp b/src/register/RegisterReadOnly.hpp index a8daf85d..4a41f579 100644 --- a/src/register/RegisterReadOnly.hpp +++ b/src/register/RegisterReadOnly.hpp @@ -26,10 +26,16 @@ class RegisterReadOnly : RegisterBase public: RegisterReadOnly(char const * name, + AccessType const access_type, T const & initial_val) - : RegisterBase{name} + : RegisterBase{name, access_type} , _val{initial_val} { } + RegisterReadOnly(char const * name, + T const & initial_val) + : RegisterReadOnly{name, RegisterBase::AccessType::ReadOnly, initial_val} + { } + virtual ~RegisterReadOnly() { } inline T get() const { return _val; } diff --git a/src/register/RegisterReadWrite.hpp b/src/register/RegisterReadWrite.hpp index a2fa2b1b..e7fa79a6 100644 --- a/src/register/RegisterReadWrite.hpp +++ b/src/register/RegisterReadWrite.hpp @@ -29,7 +29,7 @@ class RegisterReadWrite : public RegisterReadOnly RegisterReadWrite(char const * name, T const & initial_val, OnRegisterValueChangeFunc func) - : RegisterReadOnly{name, initial_val} + : RegisterReadOnly{name, RegisterBase::AccessType::ReadWrite, initial_val} , _func{func} { } virtual ~RegisterReadWrite() { } From 79378a068a696597f30cb9acdbba5ee7b0046a9e Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 07:23:06 +0200 Subject: [PATCH 27/82] Eliminate the need to dedicatedly store the last register. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 2 -- src/register/RegisterList.h | 14 ++++++++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 691532b7..075eccde 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -106,7 +106,6 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION static RegisterReadWrite reg_rw_uavcan_node_id ("uavcan.node.id", 42, nullptr); static RegisterReadOnly reg_ro_uavcan_node_description("uavcan.node.description", "OpenCyphal-ToF-Distance-Sensor-Node"); -static RegisterBase reg_last ("", RegisterBase::AccessType::ReadOnly); static RegisterList reg_list; /* DRIVER *****************************************************************************/ @@ -213,7 +212,6 @@ void setup() reg_list.subscribe(node_hdl); reg_list.add(reinterpret_cast(®_rw_uavcan_node_id)); reg_list.add(reinterpret_cast(®_ro_uavcan_node_description)); - reg_list.add(reinterpret_cast(®_last)); } void loop() diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 84785981..ea831bd2 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -26,6 +26,10 @@ class RegisterList { public: + RegisterList() + : _reg_last{"", RegisterBase::AccessType::ReadOnly} + { } + void subscribe(Node & node_hdl) { node_hdl.subscribe> @@ -42,11 +46,14 @@ class RegisterList private: std::vector _reg_list; + RegisterBase const _reg_last; + void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) { uavcan::_register::List_1_0::Request<> const req = uavcan::_register::List_1_0::Request<>::deserialize(transfer); - uavcan::_register::List_1_0::Response<> const rsp = _reg_list[req.data.index]->toListResponse(); + uavcan::_register::List_1_0::Response<> const rsp = + (req.data.index < _reg_list.size()) ? _reg_list[req.data.index]->toListResponse() : _reg_last.toListResponse(); node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } @@ -76,6 +83,9 @@ class RegisterList rsp.data._mutable = false; rsp.data.persistent = false; Serial.println("no find entry"); + + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); + return; } /* Retrieve the content of the iterator for better @@ -100,10 +110,10 @@ class RegisterList if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { + Serial.println("RW uint8_t"); RegisterReadWrite * rw_reg_ptr = reinterpret_cast *>(reg_ptr); rw_reg_ptr->set(req.data.value); rsp = rw_reg_ptr->toAccessResponse(); - Serial.println("RW uint8_t"); } /* TODO: Implement for all the other types ... */ } From b4595b8a1d22e6e356efe8297cd689043332b5ca Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 07:34:24 +0200 Subject: [PATCH 28/82] Only one templated register type. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 6 +- .../{RegisterReadOnly.hpp => Register.hpp} | 35 +++++++----- src/register/RegisterList.h | 6 +- src/register/RegisterReadWrite.hpp | 56 ------------------- 4 files changed, 25 insertions(+), 78 deletions(-) rename src/register/{RegisterReadOnly.hpp => Register.hpp} (65%) delete mode 100644 src/register/RegisterReadWrite.hpp diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 075eccde..541d4a2a 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -104,9 +104,9 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION /* REGISTER ***************************************************************************/ -static RegisterReadWrite reg_rw_uavcan_node_id ("uavcan.node.id", 42, nullptr); -static RegisterReadOnly reg_ro_uavcan_node_description("uavcan.node.description", "OpenCyphal-ToF-Distance-Sensor-Node"); -static RegisterList reg_list; +static Register reg_rw_uavcan_node_id ("uavcan.node.id", RegisterBase::AccessType::ReadWrite, 42, nullptr); +static Register reg_ro_uavcan_node_description("uavcan.node.description", RegisterBase::AccessType::ReadOnly, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterList reg_list; /* DRIVER *****************************************************************************/ diff --git a/src/register/RegisterReadOnly.hpp b/src/register/Register.hpp similarity index 65% rename from src/register/RegisterReadOnly.hpp rename to src/register/Register.hpp index 4a41f579..8eda4302 100644 --- a/src/register/RegisterReadOnly.hpp +++ b/src/register/Register.hpp @@ -21,24 +21,31 @@ **************************************************************************************/ template -class RegisterReadOnly : RegisterBase +class Register : RegisterBase { public: + typedef std::function const &)> OnRegisterValueChangeFunc; - RegisterReadOnly(char const * name, - AccessType const access_type, - T const & initial_val) + Register(char const * name, + AccessType const access_type, + T const & initial_val, + OnRegisterValueChangeFunc func) : RegisterBase{name, access_type} , _val{initial_val} + , _func{func} { } - RegisterReadOnly(char const * name, - T const & initial_val) - : RegisterReadOnly{name, RegisterBase::AccessType::ReadOnly, initial_val} - { } - - virtual ~RegisterReadOnly() { } + virtual ~Register() { } - inline T get() const { return _val; } + T get() const { return _val; } + void set(uavcan_register_Value_1_0 const & val) + { + if (type() == AccessType::ReadOnly) + return; + + _val = fromRegisterValue(val); + if (_func) + _func(*this); + } virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() { @@ -46,17 +53,15 @@ class RegisterReadOnly : RegisterBase rsp.data.value = toRegisterValue(_val); rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = false; + rsp.data._mutable = (type() == AccessType::ReadOnly) ? false : true; rsp.data.persistent = false; return rsp; } -protected: - inline void set(T const & val) { _val = val; } - private: T _val; + OnRegisterValueChangeFunc _func; }; #endif /* REGISTER_READ_ONLY_H_ */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index ea831bd2..83a28ce5 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -15,9 +15,7 @@ #include #include "Node.h" -#include "RegisterBase.h" -#include "RegisterReadOnly.hpp" -#include "RegisterReadWrite.hpp" +#include "Register.hpp" /************************************************************************************** * CLASS DECLARATION @@ -111,7 +109,7 @@ class RegisterList if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { Serial.println("RW uint8_t"); - RegisterReadWrite * rw_reg_ptr = reinterpret_cast *>(reg_ptr); + Register * rw_reg_ptr = reinterpret_cast *>(reg_ptr); rw_reg_ptr->set(req.data.value); rsp = rw_reg_ptr->toAccessResponse(); } diff --git a/src/register/RegisterReadWrite.hpp b/src/register/RegisterReadWrite.hpp deleted file mode 100644 index e7fa79a6..00000000 --- a/src/register/RegisterReadWrite.hpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * This software is distributed under the terms of the MIT License. - * Copyright (c) 2020 LXRobotics. - * Author: Alexander Entinger - * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. - */ - -#ifndef REGISTER_READ_WRITE_H_ -#define REGISTER_READ_WRITE_H_ - -/************************************************************************************** - * INCLUDES - **************************************************************************************/ - -#include "RegisterReadOnly.hpp" - -#include "util/register_util.hpp" - -/************************************************************************************** - * CLASS DECLARATION - **************************************************************************************/ - -template -class RegisterReadWrite : public RegisterReadOnly -{ -public: - typedef std::function const &)> OnRegisterValueChangeFunc; - - RegisterReadWrite(char const * name, - T const & initial_val, - OnRegisterValueChangeFunc func) - : RegisterReadOnly{name, RegisterBase::AccessType::ReadWrite, initial_val} - , _func{func} - { } - virtual ~RegisterReadWrite() { } - - virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() - { - uavcan::_register::Access_1_0::Response<> rsp = RegisterReadOnly::toAccessResponse(); - rsp.data._mutable = true; - return rsp; - } - - inline void set(uavcan_register_Value_1_0 const & val) - { - RegisterReadOnly::set(fromRegisterValue(val)); - if (_func) - _func(*this); - } - -private: - OnRegisterValueChangeFunc _func; -}; - -#endif /* REGISTER_READ_WRITE_H_ */ - From 7fe4a98f000d79168703a46f99ab9781575f7922 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 09:35:38 +0200 Subject: [PATCH 29/82] A bit of refactoring ... --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 4 +- src/register/Register.hpp | 18 +++++---- src/register/RegisterBase.h | 36 +++++------------ src/register/RegisterList.h | 40 +++++++------------ 4 files changed, 37 insertions(+), 61 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 541d4a2a..8ab5f78c 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -104,8 +104,8 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION /* REGISTER ***************************************************************************/ -static Register reg_rw_uavcan_node_id ("uavcan.node.id", RegisterBase::AccessType::ReadWrite, 42, nullptr); -static Register reg_ro_uavcan_node_description("uavcan.node.description", RegisterBase::AccessType::ReadOnly, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, 42, nullptr); +static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadOnly, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); static RegisterList reg_list; /* DRIVER *****************************************************************************/ diff --git a/src/register/Register.hpp b/src/register/Register.hpp index 8eda4302..02d63729 100644 --- a/src/register/Register.hpp +++ b/src/register/Register.hpp @@ -21,25 +21,28 @@ **************************************************************************************/ template -class Register : RegisterBase +class Register : public RegisterBase { public: typedef std::function const &)> OnRegisterValueChangeFunc; + enum class Access { ReadWrite, ReadOnly }; + Register(char const * name, - AccessType const access_type, + Access const access, T const & initial_val, OnRegisterValueChangeFunc func) - : RegisterBase{name, access_type} + : RegisterBase{name} + , _access{access} , _val{initial_val} , _func{func} { } - virtual ~Register() { } + T get() const { return _val; } void set(uavcan_register_Value_1_0 const & val) { - if (type() == AccessType::ReadOnly) + if (_access == Access::ReadOnly) return; _val = fromRegisterValue(val); @@ -47,13 +50,13 @@ class Register : RegisterBase _func(*this); } - virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() + uavcan::_register::Access_1_0::Response<> toAccessResponse() { uavcan::_register::Access_1_0::Response<> rsp; rsp.data.value = toRegisterValue(_val); rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = (type() == AccessType::ReadOnly) ? false : true; + rsp.data._mutable = (_access == Access::ReadOnly) ? false : true; rsp.data.persistent = false; return rsp; @@ -61,6 +64,7 @@ class Register : RegisterBase private: T _val; + Access const _access; OnRegisterValueChangeFunc _func; }; diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index 2c65176c..669ac558 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -23,21 +23,18 @@ class RegisterBase { public: - enum class AccessType + RegisterBase(char const * name) + : _name { - ReadWrite, ReadOnly - }; - - RegisterBase(char const * name, AccessType const access_type) - : _access_type{access_type} - { - _name.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(_name.name.elements, name, _name.name.count); + [name]() -> uavcan_register_Name_1_0 + { + uavcan_register_Name_1_0 n; + n.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(n.name.elements, name, n.name.count); + return n; + } () } - virtual ~RegisterBase() { } - - - inline AccessType type() const { return _access_type; } + { } uavcan::_register::List_1_0::Response<> toListResponse() const { @@ -47,18 +44,6 @@ class RegisterBase return rsp; } - virtual uavcan::_register::Access_1_0::Response<> toAccessResponse() - { - uavcan::_register::Access_1_0::Response<> rsp; - - uavcan_register_Value_1_0_select_empty_(&rsp.data.value); - rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = false; - rsp.data.persistent = false; - - return rsp; - } - bool operator == (uavcan_register_Name_1_0 const & reg_name) { if (reg_name.name.count != _name.name.count) @@ -71,7 +56,6 @@ class RegisterBase private: uavcan_register_Name_1_0 _name; - AccessType const _access_type; }; #endif /* REGISTER_BASE_H_ */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 83a28ce5..5b51543b 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -25,7 +25,7 @@ class RegisterList { public: RegisterList() - : _reg_last{"", RegisterBase::AccessType::ReadOnly} + : _reg_last{""} { } void subscribe(Node & node_hdl) @@ -39,6 +39,7 @@ class RegisterList void add(RegisterBase * reg_ptr) { _reg_list.push_back(reg_ptr); + Serial.println(_reg_list.size()); } @@ -76,36 +77,23 @@ class RegisterList */ if (iter == std::end(_reg_list)) { + Serial.println("no find entry"); + uavcan_register_Value_1_0_select_empty_(&rsp.data.value); - rsp.data.timestamp.microsecond = micros(); + rsp.data.timestamp.microsecond = 0; rsp.data._mutable = false; rsp.data.persistent = false; - Serial.println("no find entry"); - - node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); - return; } - - /* Retrieve the content of the iterator for better - * understanding of the underlying code operation. - */ - RegisterBase * reg_ptr = *iter; - - /* If its a RO register, directly form the response - * and send it back. - */ - if (reg_ptr->type() == RegisterBase::AccessType::ReadOnly) - { - Serial.println("RO"); - } - - /* Perform a write operation if the value sent in - * the request is not empty. - */ - if (reg_ptr->type() == RegisterBase::AccessType::ReadWrite) + else { - Serial.println("RW"); - + /* Retrieve the content of the iterator for better + * understanding of the underlying code operation. + */ + RegisterBase * reg_ptr = *iter; + + /* Perform a write operation if the value sent in + * the request is not empty. + */ if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { Serial.println("RW uint8_t"); From 2ee458f875414e6565c33e008da57c2e56aaae91 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 09:43:32 +0200 Subject: [PATCH 30/82] Simplify control flow by initialising with an empty response. --- src/register/RegisterList.h | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 5b51543b..e8e77170 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -39,7 +39,6 @@ class RegisterList void add(RegisterBase * reg_ptr) { _reg_list.push_back(reg_ptr); - Serial.println(_reg_list.size()); } @@ -70,21 +69,22 @@ class RegisterList return ((*reg_ptr) == req.data.name); }); - uavcan::_register::Access_1_0::Response<> rsp; - - /* If not register can be found return - * an empty response. + /* Initialise with an empty response in case we + * can't find a matching register. */ - if (iter == std::end(_reg_list)) + uavcan::_register::Access_1_0::Response<> rsp = []() { - Serial.println("no find entry"); + uavcan::_register::Access_1_0::Response<> r; - uavcan_register_Value_1_0_select_empty_(&rsp.data.value); - rsp.data.timestamp.microsecond = 0; - rsp.data._mutable = false; - rsp.data.persistent = false; - } - else + uavcan_register_Value_1_0_select_empty_(&r.data.value); + r.data.timestamp.microsecond = 0; + r.data._mutable = false; + r.data.persistent = false; + + return r; + } (); + + if (iter != std::end(_reg_list)) { /* Retrieve the content of the iterator for better * understanding of the underlying code operation. @@ -94,6 +94,9 @@ class RegisterList /* Perform a write operation if the value sent in * the request is not empty. */ + Serial.print("tag = "); + Serial.println(req.data.value._tag_); + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { Serial.println("RW uint8_t"); From b1232bda2aa96491d8758d738f6f1ba6da3e60ef Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 09:45:41 +0200 Subject: [PATCH 31/82] Works, but only for uint8_t right now. --- src/register/RegisterList.h | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index e8e77170..178de3a6 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -86,24 +86,22 @@ class RegisterList if (iter != std::end(_reg_list)) { - /* Retrieve the content of the iterator for better - * understanding of the underlying code operation. - */ - RegisterBase * reg_ptr = *iter; - /* Perform a write operation if the value sent in * the request is not empty. */ Serial.print("tag = "); Serial.println(req.data.value._tag_); + Register * rw_reg_ptr = reinterpret_cast *>(*iter); + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { Serial.println("RW uint8_t"); - Register * rw_reg_ptr = reinterpret_cast *>(reg_ptr); rw_reg_ptr->set(req.data.value); - rsp = rw_reg_ptr->toAccessResponse(); } + + rsp = rw_reg_ptr->toAccessResponse(); + /* TODO: Implement for all the other types ... */ } From eaf4c8271a3280f64bf5daa3928ca82a428bbdb4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 10:04:18 +0200 Subject: [PATCH 32/82] Move response initialisation up. --- src/register/RegisterList.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 178de3a6..4db98384 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -59,16 +59,6 @@ class RegisterList { uavcan::_register::Access_1_0::Request<> const req = uavcan::_register::Access_1_0::Request<>::deserialize(transfer); - /* Find a register with the same register name within - * the register list. - */ - auto iter = std::find_if(std::begin(_reg_list), - std::end (_reg_list), - [req](RegisterBase * reg_ptr) - { - return ((*reg_ptr) == req.data.name); - }); - /* Initialise with an empty response in case we * can't find a matching register. */ @@ -84,6 +74,16 @@ class RegisterList return r; } (); + /* Find a register with the same register name within + * the register list. + */ + auto iter = std::find_if(std::begin(_reg_list), + std::end (_reg_list), + [req](RegisterBase * reg_ptr) + { + return ((*reg_ptr) == req.data.name); + }); + if (iter != std::end(_reg_list)) { /* Perform a write operation if the value sent in From baf8dd8d76a2a6049bb11e320fdfd03e369bd642 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 10:05:51 +0200 Subject: [PATCH 33/82] Refactor a bit for better readability. --- src/register/RegisterList.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 4db98384..56902b2f 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -92,17 +92,16 @@ class RegisterList Serial.print("tag = "); Serial.println(req.data.value._tag_); - Register * rw_reg_ptr = reinterpret_cast *>(*iter); + /* TODO: How to know to which type one should cast?! */ + Register * reg_ptr = reinterpret_cast *>(*iter); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) { Serial.println("RW uint8_t"); - rw_reg_ptr->set(req.data.value); + reg_ptr->set(req.data.value); } - rsp = rw_reg_ptr->toAccessResponse(); - - /* TODO: Implement for all the other types ... */ + rsp = reg_ptr->toAccessResponse(); } /* Send the actual response. */ From 5ae20314996ad66461e2109bd8c363cc5e425286 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 10:31:38 +0200 Subject: [PATCH 34/82] Very crude DIY type_info. --- src/register/Register.hpp | 3 +-- src/register/RegisterBase.h | 6 +++++- src/register/RegisterList.h | 23 +++++++++++++++-------- src/register/util/register_util.hpp | 11 +++++++++++ 4 files changed, 32 insertions(+), 11 deletions(-) diff --git a/src/register/Register.hpp b/src/register/Register.hpp index 02d63729..ed934a49 100644 --- a/src/register/Register.hpp +++ b/src/register/Register.hpp @@ -32,13 +32,12 @@ class Register : public RegisterBase Access const access, T const & initial_val, OnRegisterValueChangeFunc func) - : RegisterBase{name} + : RegisterBase{name, toTypeTag(initial_val)} , _access{access} , _val{initial_val} , _func{func} { } - T get() const { return _val; } void set(uavcan_register_Value_1_0 const & val) { diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index 669ac558..ba4b758a 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -23,7 +23,7 @@ class RegisterBase { public: - RegisterBase(char const * name) + RegisterBase(char const * name, uint8_t const type_tag) : _name { [name]() -> uavcan_register_Name_1_0 @@ -34,8 +34,11 @@ class RegisterBase return n; } () } + , _type_tag{type_tag} { } + inline uint8_t type_tag() const { return _type_tag; } + uavcan::_register::List_1_0::Response<> toListResponse() const { uavcan::_register::List_1_0::Response<> rsp; @@ -56,6 +59,7 @@ class RegisterBase private: uavcan_register_Name_1_0 _name; + uint8_t _type_tag; }; #endif /* REGISTER_BASE_H_ */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 56902b2f..6a4b5afa 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -25,7 +25,7 @@ class RegisterList { public: RegisterList() - : _reg_last{""} + : _reg_last{"", 0} { } void subscribe(Node & node_hdl) @@ -92,16 +92,23 @@ class RegisterList Serial.print("tag = "); Serial.println(req.data.value._tag_); - /* TODO: How to know to which type one should cast?! */ - Register * reg_ptr = reinterpret_cast *>(*iter); + uint8_t const type_tag = (*iter)->type_tag(); - if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) + if (type_tag == 0) { - Serial.println("RW uint8_t"); - reg_ptr->set(req.data.value); + Register * reg_ptr = reinterpret_cast *>(*iter); + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); } - - rsp = reg_ptr->toAccessResponse(); + if (type_tag == 1) + { + Register * reg_ptr = reinterpret_cast *>(*iter); + if(uavcan_register_Value_1_0_is_string_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + /* TODO: Do for other types ... */ } /* Send the actual response. */ diff --git a/src/register/util/register_util.hpp b/src/register/util/register_util.hpp index 2ce7ad25..620b98ea 100644 --- a/src/register/util/register_util.hpp +++ b/src/register/util/register_util.hpp @@ -21,6 +21,7 @@ * FUNCTION DECLARATION **************************************************************************************/ +template uint8_t toTypeTag(T const); template uavcan_register_Value_1_0 toRegisterValue(T const & val); template T fromRegisterValue(uavcan_register_Value_1_0 const & val); @@ -28,6 +29,16 @@ template T fromRegisterValue(uavcan_register_Value_1_0 const & val) * FUNCTION DEFINITION **************************************************************************************/ +template<> inline uint8_t toTypeTag(uint8_t const) +{ + return 0; +} + +template<> inline uint8_t toTypeTag(std::string const) +{ + return 1; +} + template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) { uavcan_register_Value_1_0 reg_val; From 9fd592e98b7eea9cff17a9830d719c0648636989 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 13:34:17 +0200 Subject: [PATCH 35/82] Change node id on the fly. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 8ab5f78c..0baeab8f 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -102,12 +102,6 @@ static OpenCyphalNodeConfiguration const OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION * GLOBAL VARIABLES **************************************************************************************/ -/* REGISTER ***************************************************************************/ - -static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, 42, nullptr); -static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadOnly, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterList reg_list; - /* DRIVER *****************************************************************************/ ArduinoMCP2515 mcp2515([]() @@ -160,6 +154,12 @@ drone::ArduinoTMF8801 tmf8801([](uint8_t const i2c_slave_addr, uint8_t const reg DEBUG_INSTANCE(120, Serial); +/* REGISTER ***************************************************************************/ + +static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, 42, [&node_hdl](Register const & reg) { node_hdl.setNodeId(reg.get()); }); +static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadOnly, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterList reg_list; + /************************************************************************************** * SETUP/LOOP **************************************************************************************/ From b928748f9169e9f9dde612b75274c5e73f9dd29e Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 13:35:35 +0200 Subject: [PATCH 36/82] Cleanup unused constants. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 0baeab8f..d0d42662 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -83,7 +83,6 @@ static int const MKRCAN_MCP2515_INT_PIN = 7; static SPISettings const MCP2515x_SPI_SETTING{10000000, MSBFIRST, SPI_MODE0}; static CanardNodeID const OPEN_CYPHAL_NODE_ID = 42; -static CanardNodeID OPEN_CYPHAL_NODE_ID_volatile = OPEN_CYPHAL_NODE_ID; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; static OpenCyphalNodeData const OPEN_CYPHAL_NODE_INITIAL_DATA = @@ -156,7 +155,7 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, 42, [&node_hdl](Register const & reg) { node_hdl.setNodeId(reg.get()); }); +static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](Register const & reg) { node_hdl.setNodeId(reg.get()); }); static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadOnly, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); static RegisterList reg_list; From b3c1e3e187645b478bb92d1e20fb679ade02f3f6 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 21 Sep 2022 13:36:05 +0200 Subject: [PATCH 37/82] Make description R/W. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index d0d42662..2340d08c 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -156,7 +156,7 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](Register const & reg) { node_hdl.setNodeId(reg.get()); }); -static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadOnly, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); static RegisterList reg_list; /************************************************************************************** From 29a66834837a8386fcf5e8c709d63d0e5259d60c Mon Sep 17 00:00:00 2001 From: generationmake Date: Thu, 22 Sep 2022 03:10:26 +0200 Subject: [PATCH 38/82] added Natural16 to Register API, added distance Subject-ID to example --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 2 ++ src/register/RegisterList.h | 9 ++++++++- src/register/util/register_util.hpp | 19 +++++++++++++++++++ 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 2340d08c..17ab6740 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -157,6 +157,7 @@ DEBUG_INSTANCE(120, Serial); static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](Register const & reg) { node_hdl.setNodeId(reg.get()); }); static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static Register reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); static RegisterList reg_list; /************************************************************************************** @@ -211,6 +212,7 @@ void setup() reg_list.subscribe(node_hdl); reg_list.add(reinterpret_cast(®_rw_uavcan_node_id)); reg_list.add(reinterpret_cast(®_ro_uavcan_node_description)); + reg_list.add(reinterpret_cast(®_ro_uavcan_pub_distance_id)); } void loop() diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 6a4b5afa..f1514b9c 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -108,6 +108,13 @@ class RegisterList reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } + if (type_tag == 2) + { + Register * reg_ptr = reinterpret_cast *>(*iter); + if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } /* TODO: Do for other types ... */ } @@ -117,4 +124,4 @@ class RegisterList }; -#endif /* REGISTER_LIST_H_ */ \ No newline at end of file +#endif /* REGISTER_LIST_H_ */ diff --git a/src/register/util/register_util.hpp b/src/register/util/register_util.hpp index 620b98ea..568832d6 100644 --- a/src/register/util/register_util.hpp +++ b/src/register/util/register_util.hpp @@ -39,6 +39,11 @@ template<> inline uint8_t toTypeTag(std::string const) return 1; } +template<> inline uint8_t toTypeTag(uint16_t const) +{ + return 2; +} + template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) { uavcan_register_Value_1_0 reg_val; @@ -48,6 +53,15 @@ template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t con return reg_val; } +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint16_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural16.value.elements[0] = val; + reg_val.natural16.value.count = 1; + uavcan_register_Value_1_0_select_natural16_(®_val); + return reg_val; +} + template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) { uavcan_register_Value_1_0 reg_val; @@ -62,6 +76,11 @@ template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 c return val.natural8.value.elements[0]; } +template<> inline uint16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural16.value.elements[0]; +} + template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) { char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; From 3e65d87787dfa8c9baa939ab56fce657faa7e6b8 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 07:32:06 +0200 Subject: [PATCH 39/82] Rename class Register to RegisterDerived. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 ++++---- src/register/{Register.hpp => RegisterDerived.hpp} | 12 ++++++------ src/register/RegisterList.h | 8 ++++---- 3 files changed, 14 insertions(+), 14 deletions(-) rename src/register/{Register.hpp => RegisterDerived.hpp} (84%) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 17ab6740..0b31c12f 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static Register reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](Register const & reg) { node_hdl.setNodeId(reg.get()); }); -static Register reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static Register reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); -static RegisterList reg_list; +static RegisterDerived reg_rw_uavcan_node_id ("uavcan.node.id", RegisterDerived::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterDerived const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterDerived reg_ro_uavcan_node_description("uavcan.node.description", RegisterDerived::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterDerived reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", RegisterDerived::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterList reg_list; /************************************************************************************** * SETUP/LOOP diff --git a/src/register/Register.hpp b/src/register/RegisterDerived.hpp similarity index 84% rename from src/register/Register.hpp rename to src/register/RegisterDerived.hpp index ed934a49..d9a9071c 100644 --- a/src/register/Register.hpp +++ b/src/register/RegisterDerived.hpp @@ -21,17 +21,17 @@ **************************************************************************************/ template -class Register : public RegisterBase +class RegisterDerived : public RegisterBase { public: - typedef std::function const &)> OnRegisterValueChangeFunc; + typedef std::function const &)> OnRegisterValueChangeFunc; enum class Access { ReadWrite, ReadOnly }; - Register(char const * name, - Access const access, - T const & initial_val, - OnRegisterValueChangeFunc func) + RegisterDerived(char const * name, + Access const access, + T const & initial_val, + OnRegisterValueChangeFunc func) : RegisterBase{name, toTypeTag(initial_val)} , _access{access} , _val{initial_val} diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index f1514b9c..af2cfaa4 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -15,7 +15,7 @@ #include #include "Node.h" -#include "Register.hpp" +#include "RegisterDerived.hpp" /************************************************************************************** * CLASS DECLARATION @@ -96,21 +96,21 @@ class RegisterList if (type_tag == 0) { - Register * reg_ptr = reinterpret_cast *>(*iter); + RegisterDerived * reg_ptr = reinterpret_cast *>(*iter); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } if (type_tag == 1) { - Register * reg_ptr = reinterpret_cast *>(*iter); + RegisterDerived * reg_ptr = reinterpret_cast *>(*iter); if(uavcan_register_Value_1_0_is_string_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } if (type_tag == 2) { - Register * reg_ptr = reinterpret_cast *>(*iter); + RegisterDerived * reg_ptr = reinterpret_cast *>(*iter); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); From a728fce8e69f617063650fe12667090be76c48af Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 07:35:13 +0200 Subject: [PATCH 40/82] Use typedefs for less semantic friction. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 ++++---- src/register/RegisterDerived.hpp | 8 ++++++++ src/register/RegisterList.h | 6 +++--- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 0b31c12f..57da098b 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterDerived reg_rw_uavcan_node_id ("uavcan.node.id", RegisterDerived::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterDerived const & reg) { node_hdl.setNodeId(reg.get()); }); -static RegisterDerived reg_ro_uavcan_node_description("uavcan.node.description", RegisterDerived::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterDerived reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", RegisterDerived::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); -static RegisterList reg_list; +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", RegisterNatural8::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterString reg_ro_uavcan_node_description("uavcan.node.description", RegisterString::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", RegisterNatural16::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterList reg_list; /************************************************************************************** * SETUP/LOOP diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index d9a9071c..2a1ad56b 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -67,4 +67,12 @@ class RegisterDerived : public RegisterBase OnRegisterValueChangeFunc _func; }; +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +typedef RegisterDerived RegisterNatural8; +typedef RegisterDerived RegisterNatural16; +typedef RegisterDerived RegisterString; + #endif /* REGISTER_READ_ONLY_H_ */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index af2cfaa4..5df2ce22 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -96,21 +96,21 @@ class RegisterList if (type_tag == 0) { - RegisterDerived * reg_ptr = reinterpret_cast *>(*iter); + RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } if (type_tag == 1) { - RegisterDerived * reg_ptr = reinterpret_cast *>(*iter); + RegisterString * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_string_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } if (type_tag == 2) { - RegisterDerived * reg_ptr = reinterpret_cast *>(*iter); + RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); From 342228bd40fe66690497fb9120ba2b5b712dbcee Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 07:38:25 +0200 Subject: [PATCH 41/82] Pull enum class Access out of templated class RegisterDerived<>. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 6 ++-- src/register/RegisterAccess.h | 30 +++++++++++++++++++ src/register/RegisterDerived.hpp | 11 ++++--- 3 files changed, 38 insertions(+), 9 deletions(-) create mode 100644 src/register/RegisterAccess.h diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 57da098b..ecc42c8f 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,9 +155,9 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", RegisterNatural8::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); -static RegisterString reg_ro_uavcan_node_description("uavcan.node.description", RegisterString::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", RegisterNatural16::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterString reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); static RegisterList reg_list; /************************************************************************************** diff --git a/src/register/RegisterAccess.h b/src/register/RegisterAccess.h new file mode 100644 index 00000000..12ba7aaa --- /dev/null +++ b/src/register/RegisterAccess.h @@ -0,0 +1,30 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_ACCESS_H_ +#define REGISTER_ACCESS_H_ + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +namespace Register +{ + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +enum class Access { ReadWrite, ReadOnly }; + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +} /* Register */ + +#endif /* REGISTER_ACCESS_H_ */ \ No newline at end of file diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 2a1ad56b..d69d9f79 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -13,6 +13,7 @@ **************************************************************************************/ #include "RegisterBase.h" +#include "RegisterAccess.h" #include "util/register_util.hpp" @@ -26,10 +27,8 @@ class RegisterDerived : public RegisterBase public: typedef std::function const &)> OnRegisterValueChangeFunc; - enum class Access { ReadWrite, ReadOnly }; - RegisterDerived(char const * name, - Access const access, + Register::Access const access, T const & initial_val, OnRegisterValueChangeFunc func) : RegisterBase{name, toTypeTag(initial_val)} @@ -41,7 +40,7 @@ class RegisterDerived : public RegisterBase T get() const { return _val; } void set(uavcan_register_Value_1_0 const & val) { - if (_access == Access::ReadOnly) + if (_access == Register::Access::ReadOnly) return; _val = fromRegisterValue(val); @@ -55,7 +54,7 @@ class RegisterDerived : public RegisterBase rsp.data.value = toRegisterValue(_val); rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = (_access == Access::ReadOnly) ? false : true; + rsp.data._mutable = (_access == Register::Access::ReadOnly) ? false : true; rsp.data.persistent = false; return rsp; @@ -63,7 +62,7 @@ class RegisterDerived : public RegisterBase private: T _val; - Access const _access; + Register::Access const _access; OnRegisterValueChangeFunc _func; }; From 125e9c1ff8ee71da0c0aed6f99cb60261528c617 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 07:50:00 +0200 Subject: [PATCH 42/82] Use enum class makes code considerably more readable. --- src/register/RegisterBase.h | 8 +-- src/register/RegisterDerived.hpp | 2 +- src/register/RegisterList.h | 10 ++-- src/register/types/TypeTag.hpp | 77 +++++++++++++++++++++++++++++ src/register/util/register_util.hpp | 16 ------ 5 files changed, 88 insertions(+), 25 deletions(-) create mode 100644 src/register/types/TypeTag.hpp diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index ba4b758a..53b35998 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -16,6 +16,8 @@ #include +#include "types/TypeTag.hpp" + /************************************************************************************** * CLASS DECLARATION **************************************************************************************/ @@ -23,7 +25,7 @@ class RegisterBase { public: - RegisterBase(char const * name, uint8_t const type_tag) + RegisterBase(char const * name, Register::TypeTag const type_tag) : _name { [name]() -> uavcan_register_Name_1_0 @@ -37,7 +39,7 @@ class RegisterBase , _type_tag{type_tag} { } - inline uint8_t type_tag() const { return _type_tag; } + inline Register::TypeTag type_tag() const { return _type_tag; } uavcan::_register::List_1_0::Response<> toListResponse() const { @@ -59,7 +61,7 @@ class RegisterBase private: uavcan_register_Name_1_0 _name; - uint8_t _type_tag; + Register::TypeTag const _type_tag; }; #endif /* REGISTER_BASE_H_ */ diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index d69d9f79..3d7668a6 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -31,7 +31,7 @@ class RegisterDerived : public RegisterBase Register::Access const access, T const & initial_val, OnRegisterValueChangeFunc func) - : RegisterBase{name, toTypeTag(initial_val)} + : RegisterBase{name, Register::toTypeTag(initial_val)} , _access{access} , _val{initial_val} , _func{func} diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 5df2ce22..f21fbda8 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -25,7 +25,7 @@ class RegisterList { public: RegisterList() - : _reg_last{"", 0} + : _reg_last{"", Register::TypeTag::Empty} { } void subscribe(Node & node_hdl) @@ -92,23 +92,23 @@ class RegisterList Serial.print("tag = "); Serial.println(req.data.value._tag_); - uint8_t const type_tag = (*iter)->type_tag(); + Register::TypeTag const type_tag = (*iter)->type_tag(); - if (type_tag == 0) + if (type_tag == Register::TypeTag::Natural8) { RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } - if (type_tag == 1) + if (type_tag == Register::TypeTag::String) { RegisterString * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_string_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } - if (type_tag == 2) + if (type_tag == Register::TypeTag::Natural16) { RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp new file mode 100644 index 00000000..4ee15ab4 --- /dev/null +++ b/src/register/types/TypeTag.hpp @@ -0,0 +1,77 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_TAG_H_ +#define REGISTER_TAG_H_ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +namespace Register +{ + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +enum class TypeTag +{ + Empty, + String, + Bit, + Integer8, + Integer16, + Integer32, + Integer64, + Natural8, + Natural16, + Natural32, + Natural64, + Real16, + Real32, + Real64, +}; + +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +template TypeTag toTypeTag(T const); + +/************************************************************************************** + * FUNCTION DEFINITION + **************************************************************************************/ + +template<> inline TypeTag toTypeTag(std::string const) +{ + return TypeTag::String; +} + +template<> inline TypeTag toTypeTag(uint8_t const) +{ + return TypeTag::Natural8; +} + +template<> inline TypeTag toTypeTag(uint16_t const) +{ + return TypeTag::Natural16; +} + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +} /* Register */ + +#endif /* REGISTER_TAG_H_ */ diff --git a/src/register/util/register_util.hpp b/src/register/util/register_util.hpp index 568832d6..b1a9cd8f 100644 --- a/src/register/util/register_util.hpp +++ b/src/register/util/register_util.hpp @@ -21,7 +21,6 @@ * FUNCTION DECLARATION **************************************************************************************/ -template uint8_t toTypeTag(T const); template uavcan_register_Value_1_0 toRegisterValue(T const & val); template T fromRegisterValue(uavcan_register_Value_1_0 const & val); @@ -29,21 +28,6 @@ template T fromRegisterValue(uavcan_register_Value_1_0 const & val) * FUNCTION DEFINITION **************************************************************************************/ -template<> inline uint8_t toTypeTag(uint8_t const) -{ - return 0; -} - -template<> inline uint8_t toTypeTag(std::string const) -{ - return 1; -} - -template<> inline uint8_t toTypeTag(uint16_t const) -{ - return 2; -} - template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) { uavcan_register_Value_1_0 reg_val; From 5a6cdb3d91de838fac6bed34bbe53a388f145faa Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 07:50:43 +0200 Subject: [PATCH 43/82] Reorder if-statements to concur with enum class order. --- src/register/RegisterList.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index f21fbda8..b3568a4e 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -101,17 +101,17 @@ class RegisterList reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } - if (type_tag == Register::TypeTag::String) + else if (type_tag == Register::TypeTag::Natural16) { - RegisterString * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_string_(&req.data.value)) + RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } - if (type_tag == Register::TypeTag::Natural16) + else if (type_tag == Register::TypeTag::String) { - RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) + RegisterString * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_string_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } From a7d82eda1d6e3e35b4622898ece6548731d1e6d5 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 07:51:58 +0200 Subject: [PATCH 44/82] Moving Access enum class into types subfolder. --- src/register/RegisterDerived.hpp | 3 +-- src/register/{RegisterAccess.h => types/Access.h} | 0 2 files changed, 1 insertion(+), 2 deletions(-) rename src/register/{RegisterAccess.h => types/Access.h} (100%) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 3d7668a6..b8b7a3eb 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -13,8 +13,7 @@ **************************************************************************************/ #include "RegisterBase.h" -#include "RegisterAccess.h" - +#include "types/Access.h" #include "util/register_util.hpp" /************************************************************************************** diff --git a/src/register/RegisterAccess.h b/src/register/types/Access.h similarity index 100% rename from src/register/RegisterAccess.h rename to src/register/types/Access.h From f75070e3950e10b13be8982339f4224b91310fa1 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 08:12:18 +0200 Subject: [PATCH 45/82] Templated method allows for more convenient public API. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 6 +++--- src/register/RegisterList.h | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index ecc42c8f..5e5db829 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -210,9 +210,9 @@ void setup() */ node_hdl.subscribe>(onGetInfo_1_0_Request_Received); reg_list.subscribe(node_hdl); - reg_list.add(reinterpret_cast(®_rw_uavcan_node_id)); - reg_list.add(reinterpret_cast(®_ro_uavcan_node_description)); - reg_list.add(reinterpret_cast(®_ro_uavcan_pub_distance_id)); + reg_list.add(reg_rw_uavcan_node_id); + reg_list.add(reg_ro_uavcan_node_description); + reg_list.add(reg_ro_uavcan_pub_distance_id); } void loop() diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index b3568a4e..93f0a954 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -36,9 +36,10 @@ class RegisterList ([this](CanardRxTransfer const & transfer, Node & node_hdl) { this->onAccess_1_0_Request_Received(transfer, node_hdl); }); } - void add(RegisterBase * reg_ptr) + template + void add(RegisterDerived & reg_ptr) { - _reg_list.push_back(reg_ptr); + _reg_list.push_back(reinterpret_cast(®_ptr)); } From f4791b1cc6bdf22cdd065dc4928d11a5c208ed17 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 15:07:08 +0200 Subject: [PATCH 46/82] Fix: Eliminate trailing whitespace. --- src/register/RegisterDerived.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index b8b7a3eb..85f65846 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -41,7 +41,7 @@ class RegisterDerived : public RegisterBase { if (_access == Register::Access::ReadOnly) return; - + _val = fromRegisterValue(val); if (_func) _func(*this); From 66d2786078e88c80296840c6e65f0a9f7fc837e9 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Sep 2022 15:08:30 +0200 Subject: [PATCH 47/82] Fix: add missing new-line. --- src/register/types/Access.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/register/types/Access.h b/src/register/types/Access.h index 12ba7aaa..23e9e120 100644 --- a/src/register/types/Access.h +++ b/src/register/types/Access.h @@ -27,4 +27,4 @@ enum class Access { ReadWrite, ReadOnly }; } /* Register */ -#endif /* REGISTER_ACCESS_H_ */ \ No newline at end of file +#endif /* REGISTER_ACCESS_H_ */ From 47c0929a097a2773bb2ad01396d9f43f38de6bf1 Mon Sep 17 00:00:00 2001 From: generationmake Date: Fri, 23 Sep 2022 01:09:36 +0200 Subject: [PATCH 48/82] added missing register types --- src/register/RegisterDerived.hpp | 11 ++- src/register/RegisterList.h | 73 +++++++++++++- src/register/types/TypeTag.hpp | 45 +++++++++ src/register/util/register_util.hpp | 142 ++++++++++++++++++++++++++-- 4 files changed, 257 insertions(+), 14 deletions(-) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 85f65846..702c2350 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -69,8 +69,17 @@ class RegisterDerived : public RegisterBase * TYPEDEF **************************************************************************************/ +typedef RegisterDerived RegisterString; +typedef RegisterDerived RegisterBit; +typedef RegisterDerived RegisterInteger8; +typedef RegisterDerived RegisterInteger16; +typedef RegisterDerived RegisterInteger32; +typedef RegisterDerived RegisterInteger64; typedef RegisterDerived RegisterNatural8; typedef RegisterDerived RegisterNatural16; -typedef RegisterDerived RegisterString; +typedef RegisterDerived RegisterNatural32; +typedef RegisterDerived RegisterNatural64; +typedef RegisterDerived RegisterReal32; +typedef RegisterDerived RegisterReal64; #endif /* REGISTER_READ_ONLY_H_ */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 93f0a954..808c093f 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -95,7 +95,49 @@ class RegisterList Register::TypeTag const type_tag = (*iter)->type_tag(); - if (type_tag == Register::TypeTag::Natural8) + if (type_tag == Register::TypeTag::String) + { + RegisterString * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_string_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Bit) + { + RegisterBit * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_bit_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer8) + { + RegisterInteger8 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer8_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer16) + { + RegisterInteger16 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer16_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer32) + { + RegisterInteger32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer32_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer64) + { + RegisterInteger64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer64_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Natural8) { RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) @@ -109,14 +151,35 @@ class RegisterList reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } - else if (type_tag == Register::TypeTag::String) + else if (type_tag == Register::TypeTag::Natural32) { - RegisterString * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_string_(&req.data.value)) + RegisterNatural32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_natural32_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Natural64) + { + RegisterNatural64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_natural64_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Real32) + { + RegisterReal32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_real32_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Real64) + { + RegisterReal64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_real64_(&req.data.value)) reg_ptr->set(req.data.value); rsp = reg_ptr->toAccessResponse(); } - /* TODO: Do for other types ... */ + /* TODO: Do for other types ... Real16 */ } /* Send the actual response. */ diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp index 4ee15ab4..2f4b8f23 100644 --- a/src/register/types/TypeTag.hpp +++ b/src/register/types/TypeTag.hpp @@ -58,6 +58,31 @@ template<> inline TypeTag toTypeTag(std::string const) return TypeTag::String; } +template<> inline TypeTag toTypeTag(bool const) +{ + return TypeTag::Bit; +} + +template<> inline TypeTag toTypeTag(int8_t const) +{ + return TypeTag::Integer8; +} + +template<> inline TypeTag toTypeTag(int16_t const) +{ + return TypeTag::Integer16; +} + +template<> inline TypeTag toTypeTag(int32_t const) +{ + return TypeTag::Integer32; +} + +template<> inline TypeTag toTypeTag(int64_t const) +{ + return TypeTag::Integer64; +} + template<> inline TypeTag toTypeTag(uint8_t const) { return TypeTag::Natural8; @@ -68,6 +93,26 @@ template<> inline TypeTag toTypeTag(uint16_t const) return TypeTag::Natural16; } +template<> inline TypeTag toTypeTag(uint32_t const) +{ + return TypeTag::Natural32; +} + +template<> inline TypeTag toTypeTag(uint64_t const) +{ + return TypeTag::Natural64; +} + +template<> inline TypeTag toTypeTag(float const) +{ + return TypeTag::Real32; +} + +template<> inline TypeTag toTypeTag(double const) +{ + return TypeTag::Real64; +} + /************************************************************************************** * NAMESPACE **************************************************************************************/ diff --git a/src/register/util/register_util.hpp b/src/register/util/register_util.hpp index b1a9cd8f..ba19836e 100644 --- a/src/register/util/register_util.hpp +++ b/src/register/util/register_util.hpp @@ -28,6 +28,60 @@ template T fromRegisterValue(uavcan_register_Value_1_0 const & val) * FUNCTION DEFINITION **************************************************************************************/ +template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); + uavcan_register_Value_1_0_select_string_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(bool const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.bit.value.bitpacked[0] = (uint8_t)val; + reg_val.bit.value.count = 1; + uavcan_register_Value_1_0_select_bit_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int8_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer8.value.elements[0] = val; + reg_val.integer8.value.count = 1; + uavcan_register_Value_1_0_select_integer8_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int16_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer16.value.elements[0] = val; + reg_val.integer16.value.count = 1; + uavcan_register_Value_1_0_select_integer16_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int32_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer32.value.elements[0] = val; + reg_val.integer32.value.count = 1; + uavcan_register_Value_1_0_select_integer32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int64_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer64.value.elements[0] = val; + reg_val.integer64.value.count = 1; + uavcan_register_Value_1_0_select_integer64_(®_val); + return reg_val; +} + template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) { uavcan_register_Value_1_0 reg_val; @@ -46,23 +100,40 @@ template<> inline uavcan_register_Value_1_0 toRegisterValue(uint16_t c return reg_val; } -template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint32_t const & val) { uavcan_register_Value_1_0 reg_val; - reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); - uavcan_register_Value_1_0_select_string_(®_val); + reg_val.natural32.value.elements[0] = val; + reg_val.natural32.value.count = 1; + uavcan_register_Value_1_0_select_natural32_(®_val); return reg_val; } -template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint64_t const & val) { - return val.natural8.value.elements[0]; + uavcan_register_Value_1_0 reg_val; + reg_val.natural64.value.elements[0] = val; + reg_val.natural64.value.count = 1; + uavcan_register_Value_1_0_select_natural64_(®_val); + return reg_val; } -template<> inline uint16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +template<> inline uavcan_register_Value_1_0 toRegisterValue(float const & val) { - return val.natural16.value.elements[0]; + uavcan_register_Value_1_0 reg_val; + reg_val.real32.value.elements[0] = val; + reg_val.real32.value.count = 1; + uavcan_register_Value_1_0_select_real32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(double const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.real64.value.elements[0] = val; + reg_val.real64.value.count = 1; + uavcan_register_Value_1_0_select_real64_(®_val); + return reg_val; } template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) @@ -73,4 +144,59 @@ template<> inline std::string fromRegisterValue(uavcan_register_Val return std::string(str); } +template<> inline bool fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.bit.value.bitpacked[0]; +} + +template<> inline int8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer8.value.elements[0]; +} + +template<> inline int16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer16.value.elements[0]; +} + +template<> inline int32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer32.value.elements[0]; +} + +template<> inline int64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer64.value.elements[0]; +} + +template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural8.value.elements[0]; +} + +template<> inline uint16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural16.value.elements[0]; +} + +template<> inline uint32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural32.value.elements[0]; +} + +template<> inline uint64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural64.value.elements[0]; +} + +template<> inline float fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.real32.value.elements[0]; +} + +template<> inline double fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.real64.value.elements[0]; +} + #endif /* REGISTER_UTIL_HPP_ */ From aa63e00e9debac779a2a0d51cc1d38e656c49538 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 06:36:07 +0200 Subject: [PATCH 49/82] Split RegisterBase in .h/.cpp. --- src/register/RegisterBase.cpp | 54 +++++++++++++++++++++++++++++++++++ src/register/RegisterBase.h | 36 ++--------------------- 2 files changed, 57 insertions(+), 33 deletions(-) create mode 100644 src/register/RegisterBase.cpp diff --git a/src/register/RegisterBase.cpp b/src/register/RegisterBase.cpp new file mode 100644 index 00000000..9317a96a --- /dev/null +++ b/src/register/RegisterBase.cpp @@ -0,0 +1,54 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "RegisterBase.h" + +#include + +/************************************************************************************** + * CTOR/DTOR + **************************************************************************************/ + +RegisterBase::RegisterBase(char const * name, Register::TypeTag const type_tag) +: _name +{ + [name]() -> uavcan_register_Name_1_0 + { + uavcan_register_Name_1_0 n; + n.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(n.name.elements, name, n.name.count); + return n; + } () +} +, _type_tag{type_tag} +{ } + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +uavcan::_register::List_1_0::Response<> RegisterBase::toListResponse() const +{ + uavcan::_register::List_1_0::Response<> rsp; + memcpy(&rsp.data.name.name.elements, _name.name.elements, _name.name.count); + rsp.data.name.name.count = _name.name.count; + return rsp; +} + +bool RegisterBase::operator == (uavcan_register_Name_1_0 const & reg_name) +{ + if (reg_name.name.count != _name.name.count) + return false; + else + return (strncmp(reinterpret_cast(_name.name.elements), + reinterpret_cast(reg_name.name.elements), + reg_name.name.count) == 0); +} diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index 53b35998..741cbe6d 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -13,9 +13,6 @@ **************************************************************************************/ #include "../Types.h" - -#include - #include "types/TypeTag.hpp" /************************************************************************************** @@ -25,39 +22,12 @@ class RegisterBase { public: - RegisterBase(char const * name, Register::TypeTag const type_tag) - : _name - { - [name]() -> uavcan_register_Name_1_0 - { - uavcan_register_Name_1_0 n; - n.name.count = std::min(strlen(name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(n.name.elements, name, n.name.count); - return n; - } () - } - , _type_tag{type_tag} - { } + RegisterBase(char const * name, Register::TypeTag const type_tag); inline Register::TypeTag type_tag() const { return _type_tag; } + uavcan::_register::List_1_0::Response<> toListResponse() const; + bool operator == (uavcan_register_Name_1_0 const & reg_name); - uavcan::_register::List_1_0::Response<> toListResponse() const - { - uavcan::_register::List_1_0::Response<> rsp; - memcpy(&rsp.data.name.name.elements, _name.name.elements, _name.name.count); - rsp.data.name.name.count = _name.name.count; - return rsp; - } - - bool operator == (uavcan_register_Name_1_0 const & reg_name) - { - if (reg_name.name.count != _name.name.count) - return false; - else - return (strncmp(reinterpret_cast(_name.name.elements), - reinterpret_cast(reg_name.name.elements), - reg_name.name.count) == 0); - } private: uavcan_register_Name_1_0 _name; From ad284f9c0c5d4148af8c62f21549a80093625d4a Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 06:44:31 +0200 Subject: [PATCH 50/82] Splitting RegisterList in .h/.cpp. --- src/register/RegisterDerived.hpp | 2 + src/register/RegisterList.cpp | 169 +++++++++++++++++++++++++++++++ src/register/RegisterList.h | 157 +--------------------------- 3 files changed, 176 insertions(+), 152 deletions(-) create mode 100644 src/register/RegisterList.cpp diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 702c2350..c8abc593 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -16,6 +16,8 @@ #include "types/Access.h" #include "util/register_util.hpp" +extern unsigned long micros(void); + /************************************************************************************** * CLASS DECLARATION **************************************************************************************/ diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp new file mode 100644 index 00000000..986f36e7 --- /dev/null +++ b/src/register/RegisterList.cpp @@ -0,0 +1,169 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "RegisterList.h" + +/************************************************************************************** + * CTOR/DTOR + **************************************************************************************/ + +RegisterList::RegisterList() +: _reg_last{"", Register::TypeTag::Empty} +{ } + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +void RegisterList::subscribe(Node & node_hdl) +{ + node_hdl.subscribe> + ([this](CanardRxTransfer const & transfer, Node & node_hdl) { this->onList_1_0_Request_Received(transfer, node_hdl); }); + node_hdl.subscribe> + ([this](CanardRxTransfer const & transfer, Node & node_hdl) { this->onAccess_1_0_Request_Received(transfer, node_hdl); }); +} + +/************************************************************************************** + * PRIVATE MEMBER FUNCTIONS + **************************************************************************************/ + +void RegisterList::onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) +{ + uavcan::_register::List_1_0::Request<> const req = uavcan::_register::List_1_0::Request<>::deserialize(transfer); + uavcan::_register::List_1_0::Response<> const rsp = + (req.data.index < _reg_list.size()) ? _reg_list[req.data.index]->toListResponse() : _reg_last.toListResponse(); + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); +} + +void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) +{ + uavcan::_register::Access_1_0::Request<> const req = uavcan::_register::Access_1_0::Request<>::deserialize(transfer); + + /* Initialise with an empty response in case we + * can't find a matching register. + */ + uavcan::_register::Access_1_0::Response<> rsp = []() + { + uavcan::_register::Access_1_0::Response<> r; + + uavcan_register_Value_1_0_select_empty_(&r.data.value); + r.data.timestamp.microsecond = 0; + r.data._mutable = false; + r.data.persistent = false; + + return r; + } (); + + /* Find a register with the same register name within + * the register list. + */ + auto iter = std::find_if(std::begin(_reg_list), + std::end (_reg_list), + [req](RegisterBase * reg_ptr) + { + return ((*reg_ptr) == req.data.name); + }); + + if (iter != std::end(_reg_list)) + { + Register::TypeTag const type_tag = (*iter)->type_tag(); + + if (type_tag == Register::TypeTag::String) + { + RegisterString * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_string_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Bit) + { + RegisterBit * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_bit_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer8) + { + RegisterInteger8 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer8_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer16) + { + RegisterInteger16 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer16_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer32) + { + RegisterInteger32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer32_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Integer64) + { + RegisterInteger64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_integer64_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Natural8) + { + RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Natural16) + { + RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Natural32) + { + RegisterNatural32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_natural32_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Natural64) + { + RegisterNatural64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_natural64_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Real32) + { + RegisterReal32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_real32_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + else if (type_tag == Register::TypeTag::Real64) + { + RegisterReal64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_real64_(&req.data.value)) + reg_ptr->set(req.data.value); + rsp = reg_ptr->toAccessResponse(); + } + /* TODO: Do for other types ... Real16 */ + } + + /* Send the actual response. */ + node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); +} diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 808c093f..ece38c19 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -24,21 +24,11 @@ class RegisterList { public: - RegisterList() - : _reg_last{"", Register::TypeTag::Empty} - { } + RegisterList(); - void subscribe(Node & node_hdl) - { - node_hdl.subscribe> - ([this](CanardRxTransfer const & transfer, Node & node_hdl) { this->onList_1_0_Request_Received(transfer, node_hdl); }); - node_hdl.subscribe> - ([this](CanardRxTransfer const & transfer, Node & node_hdl) { this->onAccess_1_0_Request_Received(transfer, node_hdl); }); - } + void subscribe(Node & node_hdl); - template - void add(RegisterDerived & reg_ptr) - { + template inline void add(RegisterDerived & reg_ptr) { _reg_list.push_back(reinterpret_cast(®_ptr)); } @@ -47,145 +37,8 @@ class RegisterList std::vector _reg_list; RegisterBase const _reg_last; - - void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) - { - uavcan::_register::List_1_0::Request<> const req = uavcan::_register::List_1_0::Request<>::deserialize(transfer); - uavcan::_register::List_1_0::Response<> const rsp = - (req.data.index < _reg_list.size()) ? _reg_list[req.data.index]->toListResponse() : _reg_last.toListResponse(); - node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); - } - - void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl) - { - uavcan::_register::Access_1_0::Request<> const req = uavcan::_register::Access_1_0::Request<>::deserialize(transfer); - - /* Initialise with an empty response in case we - * can't find a matching register. - */ - uavcan::_register::Access_1_0::Response<> rsp = []() - { - uavcan::_register::Access_1_0::Response<> r; - - uavcan_register_Value_1_0_select_empty_(&r.data.value); - r.data.timestamp.microsecond = 0; - r.data._mutable = false; - r.data.persistent = false; - - return r; - } (); - - /* Find a register with the same register name within - * the register list. - */ - auto iter = std::find_if(std::begin(_reg_list), - std::end (_reg_list), - [req](RegisterBase * reg_ptr) - { - return ((*reg_ptr) == req.data.name); - }); - - if (iter != std::end(_reg_list)) - { - /* Perform a write operation if the value sent in - * the request is not empty. - */ - Serial.print("tag = "); - Serial.println(req.data.value._tag_); - - Register::TypeTag const type_tag = (*iter)->type_tag(); - - if (type_tag == Register::TypeTag::String) - { - RegisterString * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_string_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Bit) - { - RegisterBit * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_bit_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Integer8) - { - RegisterInteger8 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_integer8_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Integer16) - { - RegisterInteger16 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_integer16_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Integer32) - { - RegisterInteger32 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_integer32_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Integer64) - { - RegisterInteger64 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_integer64_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Natural8) - { - RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Natural16) - { - RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Natural32) - { - RegisterNatural32 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_natural32_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Natural64) - { - RegisterNatural64 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_natural64_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Real32) - { - RegisterReal32 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_real32_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - else if (type_tag == Register::TypeTag::Real64) - { - RegisterReal64 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_real64_(&req.data.value)) - reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); - } - /* TODO: Do for other types ... Real16 */ - } - - /* Send the actual response. */ - node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); - } - + void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); + void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); }; #endif /* REGISTER_LIST_H_ */ From 7c7b2190b035c02b6c188f845c16dce701454411 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 06:48:20 +0200 Subject: [PATCH 51/82] Split RegisterDerived in .hpp/.ipp. --- src/register/RegisterDerived.hpp | 37 +++++++---------------- src/register/RegisterDerived.ipp | 51 ++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+), 27 deletions(-) create mode 100644 src/register/RegisterDerived.ipp diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index c8abc593..6a03f313 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -31,35 +31,12 @@ class RegisterDerived : public RegisterBase RegisterDerived(char const * name, Register::Access const access, T const & initial_val, - OnRegisterValueChangeFunc func) - : RegisterBase{name, Register::toTypeTag(initial_val)} - , _access{access} - , _val{initial_val} - , _func{func} - { } + OnRegisterValueChangeFunc func); - T get() const { return _val; } - void set(uavcan_register_Value_1_0 const & val) - { - if (_access == Register::Access::ReadOnly) - return; + inline T get() const { return _val; } + void set(uavcan_register_Value_1_0 const & val); + uavcan::_register::Access_1_0::Response<> toAccessResponse(); - _val = fromRegisterValue(val); - if (_func) - _func(*this); - } - - uavcan::_register::Access_1_0::Response<> toAccessResponse() - { - uavcan::_register::Access_1_0::Response<> rsp; - - rsp.data.value = toRegisterValue(_val); - rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = (_access == Register::Access::ReadOnly) ? false : true; - rsp.data.persistent = false; - - return rsp; - } private: T _val; @@ -84,4 +61,10 @@ typedef RegisterDerived RegisterNatural64; typedef RegisterDerived RegisterReal32; typedef RegisterDerived RegisterReal64; +/************************************************************************************** + * TEMPLATE IMPLEMENTATION + **************************************************************************************/ + +#include "RegisterDerived.ipp" + #endif /* REGISTER_READ_ONLY_H_ */ diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp new file mode 100644 index 00000000..d0694065 --- /dev/null +++ b/src/register/RegisterDerived.ipp @@ -0,0 +1,51 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +/************************************************************************************** + * EXTERN DECLARATION + **************************************************************************************/ + +extern unsigned long micros(void); + +/************************************************************************************** + * CTOR/DTOR + **************************************************************************************/ + +template +RegisterDerived::RegisterDerived(char const * name, + Register::Access const access, + T const & initial_val, + OnRegisterValueChangeFunc func) +: RegisterBase{name, Register::toTypeTag(initial_val)} +, _access{access} +, _val{initial_val} +, _func{func} +{ } + +template +void RegisterDerived::set(uavcan_register_Value_1_0 const & val) +{ + if (_access == Register::Access::ReadOnly) + return; + + _val = fromRegisterValue(val); + if (_func) + _func(*this); +} + +template +uavcan::_register::Access_1_0::Response<> RegisterDerived::toAccessResponse() +{ + uavcan::_register::Access_1_0::Response<> rsp; + + rsp.data.value = toRegisterValue(_val); + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = (_access == Register::Access::ReadOnly) ? false : true; + rsp.data.persistent = false; + + return rsp; +} From 6fa587af7793f873fbf00e7a4916408510806cc9 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 06:49:47 +0200 Subject: [PATCH 52/82] Fix include guards (naming outdated with file name). --- src/register/RegisterDerived.hpp | 6 +++--- src/register/types/TypeTag.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 6a03f313..2d328317 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -5,8 +5,8 @@ * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. */ -#ifndef REGISTER_READ_ONLY_H_ -#define REGISTER_READ_ONLY_H_ +#ifndef REGISTER_DERIVED_HPP_ +#define REGISTER_DERIVED_HPP_ /************************************************************************************** * INCLUDES @@ -67,4 +67,4 @@ typedef RegisterDerived RegisterReal64; #include "RegisterDerived.ipp" -#endif /* REGISTER_READ_ONLY_H_ */ +#endif /* REGISTER_DERIVED_HPP_ */ diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp index 2f4b8f23..44d7e05f 100644 --- a/src/register/types/TypeTag.hpp +++ b/src/register/types/TypeTag.hpp @@ -5,8 +5,8 @@ * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. */ -#ifndef REGISTER_TAG_H_ -#define REGISTER_TAG_H_ +#ifndef REGISTER_TYPE_TAG_HPP_ +#define REGISTER_TYPE_TAG_HPP_ /************************************************************************************** * INCLUDE @@ -119,4 +119,4 @@ template<> inline TypeTag toTypeTag(double const) } /* Register */ -#endif /* REGISTER_TAG_H_ */ +#endif /* REGISTER_TYPE_TAG_HPP_ */ From b2a4f75ccdf9935b81121584b2ec6f6c330be665 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 06:54:19 +0200 Subject: [PATCH 53/82] Move code from register_util within RegisterDerived, where it belongs in any case. --- src/register/RegisterDerived.hpp | 3 - src/register/RegisterDerived.ipp | 186 +++++++++++++++++++++++++ src/register/RegisterList.cpp | 2 + src/register/util/register_util.hpp | 202 ---------------------------- 4 files changed, 188 insertions(+), 205 deletions(-) delete mode 100644 src/register/util/register_util.hpp diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 2d328317..6a697320 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -14,9 +14,6 @@ #include "RegisterBase.h" #include "types/Access.h" -#include "util/register_util.hpp" - -extern unsigned long micros(void); /************************************************************************************** * CLASS DECLARATION diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index d0694065..83dc971b 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -11,6 +11,13 @@ extern unsigned long micros(void); +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +template uavcan_register_Value_1_0 toRegisterValue(T const & val); +template T fromRegisterValue(uavcan_register_Value_1_0 const & val); + /************************************************************************************** * CTOR/DTOR **************************************************************************************/ @@ -26,6 +33,10 @@ RegisterDerived::RegisterDerived(char const * name, , _func{func} { } +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + template void RegisterDerived::set(uavcan_register_Value_1_0 const & val) { @@ -49,3 +60,178 @@ uavcan::_register::Access_1_0::Response<> RegisterDerived::toAccessResponse() return rsp; } + +/************************************************************************************** + * FUNCTION DEFINITION + **************************************************************************************/ + +template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); + uavcan_register_Value_1_0_select_string_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(bool const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.bit.value.bitpacked[0] = (uint8_t)val; + reg_val.bit.value.count = 1; + uavcan_register_Value_1_0_select_bit_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int8_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer8.value.elements[0] = val; + reg_val.integer8.value.count = 1; + uavcan_register_Value_1_0_select_integer8_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int16_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer16.value.elements[0] = val; + reg_val.integer16.value.count = 1; + uavcan_register_Value_1_0_select_integer16_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int32_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer32.value.elements[0] = val; + reg_val.integer32.value.count = 1; + uavcan_register_Value_1_0_select_integer32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int64_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer64.value.elements[0] = val; + reg_val.integer64.value.count = 1; + uavcan_register_Value_1_0_select_integer64_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural8.value.elements[0] = val; + reg_val.natural8.value.count = 1; + uavcan_register_Value_1_0_select_natural8_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint16_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural16.value.elements[0] = val; + reg_val.natural16.value.count = 1; + uavcan_register_Value_1_0_select_natural16_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint32_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural32.value.elements[0] = val; + reg_val.natural32.value.count = 1; + uavcan_register_Value_1_0_select_natural32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint64_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural64.value.elements[0] = val; + reg_val.natural64.value.count = 1; + uavcan_register_Value_1_0_select_natural64_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(float const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.real32.value.elements[0] = val; + reg_val.real32.value.count = 1; + uavcan_register_Value_1_0_select_real32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(double const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.real64.value.elements[0] = val; + reg_val.real64.value.count = 1; + uavcan_register_Value_1_0_select_real64_(®_val); + return reg_val; +} + +template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; + memcpy(str, val._string.value.elements, val._string.value.count); + str[val._string.value.count + 1] = '\0'; + return std::string(str); +} + +template<> inline bool fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.bit.value.bitpacked[0]; +} + +template<> inline int8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer8.value.elements[0]; +} + +template<> inline int16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer16.value.elements[0]; +} + +template<> inline int32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer32.value.elements[0]; +} + +template<> inline int64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer64.value.elements[0]; +} + +template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural8.value.elements[0]; +} + +template<> inline uint16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural16.value.elements[0]; +} + +template<> inline uint32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural32.value.elements[0]; +} + +template<> inline uint64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural64.value.elements[0]; +} + +template<> inline float fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.real32.value.elements[0]; +} + +template<> inline double fromRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.real64.value.elements[0]; +} diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 986f36e7..1a8bc17c 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -12,6 +12,8 @@ #include "RegisterList.h" +#include + /************************************************************************************** * CTOR/DTOR **************************************************************************************/ diff --git a/src/register/util/register_util.hpp b/src/register/util/register_util.hpp deleted file mode 100644 index ba19836e..00000000 --- a/src/register/util/register_util.hpp +++ /dev/null @@ -1,202 +0,0 @@ -/** - * This software is distributed under the terms of the MIT License. - * Copyright (c) 2020 LXRobotics. - * Author: Alexander Entinger - * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. - */ - -#ifndef REGISTER_UTIL_HPP_ -#define REGISTER_UTIL_HPP_ - -/************************************************************************************** - * INCLUDES - **************************************************************************************/ - -#include "../../Types.h" - -#include -#include - -/************************************************************************************** - * FUNCTION DECLARATION - **************************************************************************************/ - -template uavcan_register_Value_1_0 toRegisterValue(T const & val); -template T fromRegisterValue(uavcan_register_Value_1_0 const & val); - -/************************************************************************************** - * FUNCTION DEFINITION - **************************************************************************************/ - -template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); - uavcan_register_Value_1_0_select_string_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(bool const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.bit.value.bitpacked[0] = (uint8_t)val; - reg_val.bit.value.count = 1; - uavcan_register_Value_1_0_select_bit_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int8_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer8.value.elements[0] = val; - reg_val.integer8.value.count = 1; - uavcan_register_Value_1_0_select_integer8_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int16_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer16.value.elements[0] = val; - reg_val.integer16.value.count = 1; - uavcan_register_Value_1_0_select_integer16_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int32_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer32.value.elements[0] = val; - reg_val.integer32.value.count = 1; - uavcan_register_Value_1_0_select_integer32_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int64_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer64.value.elements[0] = val; - reg_val.integer64.value.count = 1; - uavcan_register_Value_1_0_select_integer64_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural8.value.elements[0] = val; - reg_val.natural8.value.count = 1; - uavcan_register_Value_1_0_select_natural8_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint16_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural16.value.elements[0] = val; - reg_val.natural16.value.count = 1; - uavcan_register_Value_1_0_select_natural16_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint32_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural32.value.elements[0] = val; - reg_val.natural32.value.count = 1; - uavcan_register_Value_1_0_select_natural32_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint64_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural64.value.elements[0] = val; - reg_val.natural64.value.count = 1; - uavcan_register_Value_1_0_select_natural64_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(float const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.real32.value.elements[0] = val; - reg_val.real32.value.count = 1; - uavcan_register_Value_1_0_select_real32_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(double const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.real64.value.elements[0] = val; - reg_val.real64.value.count = 1; - uavcan_register_Value_1_0_select_real64_(®_val); - return reg_val; -} - -template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; - memcpy(str, val._string.value.elements, val._string.value.count); - str[val._string.value.count + 1] = '\0'; - return std::string(str); -} - -template<> inline bool fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.bit.value.bitpacked[0]; -} - -template<> inline int8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer8.value.elements[0]; -} - -template<> inline int16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer16.value.elements[0]; -} - -template<> inline int32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer32.value.elements[0]; -} - -template<> inline int64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer64.value.elements[0]; -} - -template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural8.value.elements[0]; -} - -template<> inline uint16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural16.value.elements[0]; -} - -template<> inline uint32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural32.value.elements[0]; -} - -template<> inline uint64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural64.value.elements[0]; -} - -template<> inline float fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.real32.value.elements[0]; -} - -template<> inline double fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.real64.value.elements[0]; -} - -#endif /* REGISTER_UTIL_HPP_ */ From eb3faea00464591c41a460d49053ecae9c220174 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 07:00:43 +0200 Subject: [PATCH 54/82] Fix: enforce C calling convention to allow the linker to find the right function. --- src/register/RegisterDerived.ipp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 83dc971b..068a4df4 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -9,7 +9,7 @@ * EXTERN DECLARATION **************************************************************************************/ -extern unsigned long micros(void); +extern "C" unsigned long micros(void); /************************************************************************************** * FUNCTION DECLARATION From c16962db70ec304bd4bee4034e0f47dac37796f7 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 07:05:04 +0200 Subject: [PATCH 55/82] Move call to subscribe to end of reg-list-adding. Works the same, but is semantically more correct. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 5e5db829..29eeba56 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -209,10 +209,11 @@ void setup() /* Register callbacks for node info and register api. */ node_hdl.subscribe>(onGetInfo_1_0_Request_Received); - reg_list.subscribe(node_hdl); + reg_list.add(reg_rw_uavcan_node_id); reg_list.add(reg_ro_uavcan_node_description); reg_list.add(reg_ro_uavcan_pub_distance_id); + reg_list.subscribe(node_hdl); } void loop() From 0abfc2edc10b650a1f61c054562583f7222f7ebd Mon Sep 17 00:00:00 2001 From: generationmake Date: Fri, 23 Sep 2022 09:06:35 +0200 Subject: [PATCH 56/82] added uavcan.pub.distance.type register --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 29eeba56..36b7faea 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,9 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); -static RegisterString reg_ro_uavcan_node_description("uavcan.node.description", Register::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, "uavcan.primitive.scalar.Real32.1.0", nullptr); static RegisterList reg_list; /************************************************************************************** @@ -213,6 +214,7 @@ void setup() reg_list.add(reg_rw_uavcan_node_id); reg_list.add(reg_ro_uavcan_node_description); reg_list.add(reg_ro_uavcan_pub_distance_id); + reg_list.add(reg_ro_uavcan_pub_distance_type); reg_list.subscribe(node_hdl); } From d3fad1a2b56907de5112f19eef5a8d654f88dcd5 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Fri, 23 Sep 2022 09:37:51 +0200 Subject: [PATCH 57/82] Fixing member initialisatio order fixes -Wreorder problem on ESP32. --- src/register/RegisterDerived.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 6a697320..3e908341 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -36,8 +36,8 @@ class RegisterDerived : public RegisterBase private: - T _val; Register::Access const _access; + T _val; OnRegisterValueChangeFunc _func; }; From 37e23035559b5e158dfd8d14520e6dc539da80b5 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 06:16:22 +0200 Subject: [PATCH 58/82] Move creation of ListResponse into RegisterList class. --- src/register/RegisterBase.cpp | 8 -------- src/register/RegisterBase.h | 2 +- src/register/RegisterList.cpp | 10 +++++++++- src/register/RegisterList.h | 2 ++ 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/register/RegisterBase.cpp b/src/register/RegisterBase.cpp index 9317a96a..4f644720 100644 --- a/src/register/RegisterBase.cpp +++ b/src/register/RegisterBase.cpp @@ -35,14 +35,6 @@ RegisterBase::RegisterBase(char const * name, Register::TypeTag const type_tag) * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ -uavcan::_register::List_1_0::Response<> RegisterBase::toListResponse() const -{ - uavcan::_register::List_1_0::Response<> rsp; - memcpy(&rsp.data.name.name.elements, _name.name.elements, _name.name.count); - rsp.data.name.name.count = _name.name.count; - return rsp; -} - bool RegisterBase::operator == (uavcan_register_Name_1_0 const & reg_name) { if (reg_name.name.count != _name.name.count) diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index 741cbe6d..90da5487 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -24,8 +24,8 @@ class RegisterBase public: RegisterBase(char const * name, Register::TypeTag const type_tag); + inline uavcan_register_Name_1_0 const & name() const { return _name; } inline Register::TypeTag type_tag() const { return _type_tag; } - uavcan::_register::List_1_0::Response<> toListResponse() const; bool operator == (uavcan_register_Name_1_0 const & reg_name); diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 1a8bc17c..789e10eb 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -42,7 +42,7 @@ void RegisterList::onList_1_0_Request_Received(CanardRxTransfer const & transfer { uavcan::_register::List_1_0::Request<> const req = uavcan::_register::List_1_0::Request<>::deserialize(transfer); uavcan::_register::List_1_0::Response<> const rsp = - (req.data.index < _reg_list.size()) ? _reg_list[req.data.index]->toListResponse() : _reg_last.toListResponse(); + (req.data.index < _reg_list.size()) ? toListResponse(_reg_list[req.data.index]->name()) : toListResponse(_reg_last.name()); node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } @@ -169,3 +169,11 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf /* Send the actual response. */ node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } + +uavcan::_register::List_1_0::Response<> RegisterList::toListResponse(uavcan_register_Name_1_0 const & name) +{ + uavcan::_register::List_1_0::Response<> rsp; + memcpy(&rsp.data.name.name.elements, name.name.elements, name.name.count); + rsp.data.name.name.count = name.name.count; + return rsp; +} \ No newline at end of file diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index ece38c19..964b9c14 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -39,6 +39,8 @@ class RegisterList void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); + + static uavcan::_register::List_1_0::Response<> toListResponse(uavcan_register_Name_1_0 const & name); }; #endif /* REGISTER_LIST_H_ */ From b4893cb138ad3e9f0d9ba64da0a94a86854756cc Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 06:23:40 +0200 Subject: [PATCH 59/82] Eliminate comparison operator and move functionality to RegisterList class. --- src/register/RegisterBase.cpp | 14 -------------- src/register/RegisterBase.h | 1 - src/register/RegisterList.cpp | 7 ++++++- 3 files changed, 6 insertions(+), 16 deletions(-) diff --git a/src/register/RegisterBase.cpp b/src/register/RegisterBase.cpp index 4f644720..0dafa4aa 100644 --- a/src/register/RegisterBase.cpp +++ b/src/register/RegisterBase.cpp @@ -30,17 +30,3 @@ RegisterBase::RegisterBase(char const * name, Register::TypeTag const type_tag) } , _type_tag{type_tag} { } - -/************************************************************************************** - * PUBLIC MEMBER FUNCTIONS - **************************************************************************************/ - -bool RegisterBase::operator == (uavcan_register_Name_1_0 const & reg_name) -{ - if (reg_name.name.count != _name.name.count) - return false; - else - return (strncmp(reinterpret_cast(_name.name.elements), - reinterpret_cast(reg_name.name.elements), - reg_name.name.count) == 0); -} diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index 90da5487..c52acda8 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -26,7 +26,6 @@ class RegisterBase inline uavcan_register_Name_1_0 const & name() const { return _name; } inline Register::TypeTag type_tag() const { return _type_tag; } - bool operator == (uavcan_register_Name_1_0 const & reg_name); private: diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 789e10eb..0d42165d 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -72,7 +72,12 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf std::end (_reg_list), [req](RegisterBase * reg_ptr) { - return ((*reg_ptr) == req.data.name); + if (reg_ptr->name().name.count != req.data.name.name.count) + return false; + + return (strncmp(reinterpret_cast(reg_ptr->name().name.elements), + reinterpret_cast(req.data.name.name.elements), + reg_ptr->name().name.count) == 0); }); if (iter != std::end(_reg_list)) From bed18f0ddc43a8cb9986c4430b0df7252de96841 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 06:28:15 +0200 Subject: [PATCH 60/82] Move ListResponse creation into separate class ListResponse. --- src/register/ListResponse.cpp | 26 ++++++++++++++++++++++++++ src/register/ListResponse.h | 30 ++++++++++++++++++++++++++++++ src/register/RegisterList.cpp | 12 +++--------- src/register/RegisterList.h | 2 -- 4 files changed, 59 insertions(+), 11 deletions(-) create mode 100644 src/register/ListResponse.cpp create mode 100644 src/register/ListResponse.h diff --git a/src/register/ListResponse.cpp b/src/register/ListResponse.cpp new file mode 100644 index 00000000..5d7ab260 --- /dev/null +++ b/src/register/ListResponse.cpp @@ -0,0 +1,26 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "ListResponse.h" + +#include + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +uavcan::_register::List_1_0::Response<> ListResponse::create(uavcan_register_Name_1_0 const & name) +{ + uavcan::_register::List_1_0::Response<> rsp; + memcpy(&rsp.data.name.name.elements, name.name.elements, name.name.count); + rsp.data.name.name.count = name.name.count; + return rsp; +} diff --git a/src/register/ListResponse.h b/src/register/ListResponse.h new file mode 100644 index 00000000..0216d0b8 --- /dev/null +++ b/src/register/ListResponse.h @@ -0,0 +1,30 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef LIST_RESPONSE_H_ +#define LIST_RESPONSE_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "../Types.h" + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class ListResponse +{ +public: + ListResponse() = delete; + ListResponse(ListResponse const & other) = delete; + + static uavcan::_register::List_1_0::Response<> create(uavcan_register_Name_1_0 const & name); +}; + +#endif /* LIST_RESPONSE_H_ */ diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 0d42165d..4c5e38fe 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -14,6 +14,8 @@ #include +#include "ListResponse.h" + /************************************************************************************** * CTOR/DTOR **************************************************************************************/ @@ -42,7 +44,7 @@ void RegisterList::onList_1_0_Request_Received(CanardRxTransfer const & transfer { uavcan::_register::List_1_0::Request<> const req = uavcan::_register::List_1_0::Request<>::deserialize(transfer); uavcan::_register::List_1_0::Response<> const rsp = - (req.data.index < _reg_list.size()) ? toListResponse(_reg_list[req.data.index]->name()) : toListResponse(_reg_last.name()); + (req.data.index < _reg_list.size()) ? ListResponse::create(_reg_list[req.data.index]->name()) : ListResponse::create(_reg_last.name()); node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } @@ -174,11 +176,3 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf /* Send the actual response. */ node_hdl.respond(rsp, transfer.metadata.remote_node_id, transfer.metadata.transfer_id); } - -uavcan::_register::List_1_0::Response<> RegisterList::toListResponse(uavcan_register_Name_1_0 const & name) -{ - uavcan::_register::List_1_0::Response<> rsp; - memcpy(&rsp.data.name.name.elements, name.name.elements, name.name.count); - rsp.data.name.name.count = name.name.count; - return rsp; -} \ No newline at end of file diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 964b9c14..ece38c19 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -39,8 +39,6 @@ class RegisterList void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); - - static uavcan::_register::List_1_0::Response<> toListResponse(uavcan_register_Name_1_0 const & name); }; #endif /* REGISTER_LIST_H_ */ From 3821f04bc498fc3c37bac61a99e89a5c4880d5eb Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 06:44:33 +0200 Subject: [PATCH 61/82] Extract Access Response generation into dedicated class. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 +- src/register/AccessResponse.hpp | 38 +++++ src/register/AccessResponse.ipp | 147 ++++++++++++++++++ src/register/RegisterBase.cpp | 11 +- src/register/RegisterBase.h | 10 +- src/register/RegisterDerived.hpp | 2 +- src/register/RegisterDerived.ipp | 128 +-------------- src/register/RegisterList.cpp | 27 ++-- 8 files changed, 227 insertions(+), 144 deletions(-) create mode 100644 src/register/AccessResponse.hpp create mode 100644 src/register/AccessResponse.ipp diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 36b7faea..c52a735a 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); -static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); -static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, "uavcan.primitive.scalar.Real32.1.0", nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, false, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, false, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, false, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, false, "uavcan.primitive.scalar.Real32.1.0", nullptr); static RegisterList reg_list; /************************************************************************************** diff --git a/src/register/AccessResponse.hpp b/src/register/AccessResponse.hpp new file mode 100644 index 00000000..44facf4c --- /dev/null +++ b/src/register/AccessResponse.hpp @@ -0,0 +1,38 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef ACCESS_RESPONSE_H_ +#define ACCESS_RESPONSE_H_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "../Types.h" +#include "RegisterDerived.hpp" + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class AccessResponse +{ +public: + AccessResponse() = delete; + AccessResponse(AccessResponse const & other) = delete; + + template + static uavcan::_register::Access_1_0::Response<> create(RegisterDerived const & reg); +}; + +/************************************************************************************** + * TEMPLATE IMPLEMEMTATION + **************************************************************************************/ + +#include "AccessResponse.ipp" + +#endif /* ACCESS_RESPONSE_H_ */ diff --git a/src/register/AccessResponse.ipp b/src/register/AccessResponse.ipp new file mode 100644 index 00000000..5b0715d9 --- /dev/null +++ b/src/register/AccessResponse.ipp @@ -0,0 +1,147 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +template uavcan_register_Value_1_0 toRegisterValue(T const & val); + +/************************************************************************************** + * EXTERN DECLARATION + **************************************************************************************/ + +extern "C" unsigned long micros(void); + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +template +uavcan::_register::Access_1_0::Response<> AccessResponse::create(RegisterDerived const & reg) +{ + uavcan::_register::Access_1_0::Response<> rsp; + + rsp.data.value = toRegisterValue(reg.get()); + rsp.data.timestamp.microsecond = micros(); + rsp.data._mutable = reg.isMutable(); + rsp.data.persistent = reg.isPersistent(); + + return rsp; +} + +/************************************************************************************** + * FUNCTION DEFINITION + **************************************************************************************/ + +template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); + uavcan_register_Value_1_0_select_string_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(bool const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.bit.value.bitpacked[0] = (uint8_t)val; + reg_val.bit.value.count = 1; + uavcan_register_Value_1_0_select_bit_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int8_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer8.value.elements[0] = val; + reg_val.integer8.value.count = 1; + uavcan_register_Value_1_0_select_integer8_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int16_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer16.value.elements[0] = val; + reg_val.integer16.value.count = 1; + uavcan_register_Value_1_0_select_integer16_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int32_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer32.value.elements[0] = val; + reg_val.integer32.value.count = 1; + uavcan_register_Value_1_0_select_integer32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(int64_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.integer64.value.elements[0] = val; + reg_val.integer64.value.count = 1; + uavcan_register_Value_1_0_select_integer64_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural8.value.elements[0] = val; + reg_val.natural8.value.count = 1; + uavcan_register_Value_1_0_select_natural8_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint16_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural16.value.elements[0] = val; + reg_val.natural16.value.count = 1; + uavcan_register_Value_1_0_select_natural16_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint32_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural32.value.elements[0] = val; + reg_val.natural32.value.count = 1; + uavcan_register_Value_1_0_select_natural32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(uint64_t const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.natural64.value.elements[0] = val; + reg_val.natural64.value.count = 1; + uavcan_register_Value_1_0_select_natural64_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(float const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.real32.value.elements[0] = val; + reg_val.real32.value.count = 1; + uavcan_register_Value_1_0_select_real32_(®_val); + return reg_val; +} + +template<> inline uavcan_register_Value_1_0 toRegisterValue(double const & val) +{ + uavcan_register_Value_1_0 reg_val; + reg_val.real64.value.elements[0] = val; + reg_val.real64.value.count = 1; + uavcan_register_Value_1_0_select_real64_(®_val); + return reg_val; +} diff --git a/src/register/RegisterBase.cpp b/src/register/RegisterBase.cpp index 0dafa4aa..0a570777 100644 --- a/src/register/RegisterBase.cpp +++ b/src/register/RegisterBase.cpp @@ -17,7 +17,10 @@ * CTOR/DTOR **************************************************************************************/ -RegisterBase::RegisterBase(char const * name, Register::TypeTag const type_tag) +RegisterBase::RegisterBase(char const * name, + Register::TypeTag const type_tag, + bool const is_mutable, + bool const is_persistent) : _name { [name]() -> uavcan_register_Name_1_0 @@ -29,4 +32,8 @@ RegisterBase::RegisterBase(char const * name, Register::TypeTag const type_tag) } () } , _type_tag{type_tag} -{ } +, _is_mutable{is_mutable} +, _is_persistent{is_persistent} +{ + +} diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index c52acda8..22befeac 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -22,15 +22,23 @@ class RegisterBase { public: - RegisterBase(char const * name, Register::TypeTag const type_tag); + RegisterBase(char const * name, + Register::TypeTag const type_tag, + bool const is_mutable, + bool const is_persistent); + inline uavcan_register_Name_1_0 const & name() const { return _name; } inline Register::TypeTag type_tag() const { return _type_tag; } + inline bool isMutable() const { return _is_mutable; } + inline bool isPersistent() const { return _is_persistent; } private: uavcan_register_Name_1_0 _name; Register::TypeTag const _type_tag; + bool const _is_mutable; + bool const _is_persistent; }; #endif /* REGISTER_BASE_H_ */ diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 3e908341..f6db1d8e 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -27,12 +27,12 @@ class RegisterDerived : public RegisterBase RegisterDerived(char const * name, Register::Access const access, + bool const is_persistent, T const & initial_val, OnRegisterValueChangeFunc func); inline T get() const { return _val; } void set(uavcan_register_Value_1_0 const & val); - uavcan::_register::Access_1_0::Response<> toAccessResponse(); private: diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 068a4df4..d75206a5 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -15,7 +15,6 @@ extern "C" unsigned long micros(void); * FUNCTION DECLARATION **************************************************************************************/ -template uavcan_register_Value_1_0 toRegisterValue(T const & val); template T fromRegisterValue(uavcan_register_Value_1_0 const & val); /************************************************************************************** @@ -25,9 +24,13 @@ template T fromRegisterValue(uavcan_register_Value_1_0 const & val) template RegisterDerived::RegisterDerived(char const * name, Register::Access const access, + bool const is_persistent, T const & initial_val, OnRegisterValueChangeFunc func) -: RegisterBase{name, Register::toTypeTag(initial_val)} +: RegisterBase{name, + Register::toTypeTag(initial_val), + (_access == Register::Access::ReadOnly), + is_persistent} , _access{access} , _val{initial_val} , _func{func} @@ -48,131 +51,10 @@ void RegisterDerived::set(uavcan_register_Value_1_0 const & val) _func(*this); } -template -uavcan::_register::Access_1_0::Response<> RegisterDerived::toAccessResponse() -{ - uavcan::_register::Access_1_0::Response<> rsp; - - rsp.data.value = toRegisterValue(_val); - rsp.data.timestamp.microsecond = micros(); - rsp.data._mutable = (_access == Register::Access::ReadOnly) ? false : true; - rsp.data.persistent = false; - - return rsp; -} - /************************************************************************************** * FUNCTION DEFINITION **************************************************************************************/ -template<> inline uavcan_register_Value_1_0 toRegisterValue(std::string const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val._string.value.count = std::min(val.length(), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - memcpy(reg_val._string.value.elements, val.c_str(), reg_val._string.value.count); - uavcan_register_Value_1_0_select_string_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(bool const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.bit.value.bitpacked[0] = (uint8_t)val; - reg_val.bit.value.count = 1; - uavcan_register_Value_1_0_select_bit_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int8_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer8.value.elements[0] = val; - reg_val.integer8.value.count = 1; - uavcan_register_Value_1_0_select_integer8_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int16_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer16.value.elements[0] = val; - reg_val.integer16.value.count = 1; - uavcan_register_Value_1_0_select_integer16_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int32_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer32.value.elements[0] = val; - reg_val.integer32.value.count = 1; - uavcan_register_Value_1_0_select_integer32_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(int64_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.integer64.value.elements[0] = val; - reg_val.integer64.value.count = 1; - uavcan_register_Value_1_0_select_integer64_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint8_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural8.value.elements[0] = val; - reg_val.natural8.value.count = 1; - uavcan_register_Value_1_0_select_natural8_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint16_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural16.value.elements[0] = val; - reg_val.natural16.value.count = 1; - uavcan_register_Value_1_0_select_natural16_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint32_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural32.value.elements[0] = val; - reg_val.natural32.value.count = 1; - uavcan_register_Value_1_0_select_natural32_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(uint64_t const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.natural64.value.elements[0] = val; - reg_val.natural64.value.count = 1; - uavcan_register_Value_1_0_select_natural64_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(float const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.real32.value.elements[0] = val; - reg_val.real32.value.count = 1; - uavcan_register_Value_1_0_select_real32_(®_val); - return reg_val; -} - -template<> inline uavcan_register_Value_1_0 toRegisterValue(double const & val) -{ - uavcan_register_Value_1_0 reg_val; - reg_val.real64.value.elements[0] = val; - reg_val.real64.value.count = 1; - uavcan_register_Value_1_0_select_real64_(®_val); - return reg_val; -} - template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) { char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 4c5e38fe..5d4f9b44 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -15,13 +15,14 @@ #include #include "ListResponse.h" +#include "AccessResponse.hpp" /************************************************************************************** * CTOR/DTOR **************************************************************************************/ RegisterList::RegisterList() -: _reg_last{"", Register::TypeTag::Empty} +: _reg_last{"", Register::TypeTag::Empty, false, false} { } /************************************************************************************** @@ -91,84 +92,84 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterString * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_string_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Bit) { RegisterBit * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_bit_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer8) { RegisterInteger8 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_integer8_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer16) { RegisterInteger16 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_integer16_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer32) { RegisterInteger32 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_integer32_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer64) { RegisterInteger64 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_integer64_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural8) { RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural16) { RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural32) { RegisterNatural32 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural32_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural64) { RegisterNatural64 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_natural64_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Real32) { RegisterReal32 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_real32_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Real64) { RegisterReal64 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_real64_(&req.data.value)) reg_ptr->set(req.data.value); - rsp = reg_ptr->toAccessResponse(); + rsp = AccessResponse::create(*reg_ptr); } /* TODO: Do for other types ... Real16 */ } From 7b294c4647afa309cb0f17792f0e0358fc8e075e Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 06:47:01 +0200 Subject: [PATCH 62/82] Remove no-longer necessary member variable _access. --- src/register/RegisterDerived.hpp | 1 - src/register/RegisterDerived.ipp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index f6db1d8e..07e2ce6f 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -36,7 +36,6 @@ class RegisterDerived : public RegisterBase private: - Register::Access const _access; T _val; OnRegisterValueChangeFunc _func; }; diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index d75206a5..c336859a 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -29,9 +29,8 @@ RegisterDerived::RegisterDerived(char const * name, OnRegisterValueChangeFunc func) : RegisterBase{name, Register::toTypeTag(initial_val), - (_access == Register::Access::ReadOnly), + (access == Register::Access::ReadOnly), is_persistent} -, _access{access} , _val{initial_val} , _func{func} { } @@ -43,7 +42,7 @@ RegisterDerived::RegisterDerived(char const * name, template void RegisterDerived::set(uavcan_register_Value_1_0 const & val) { - if (_access == Register::Access::ReadOnly) + if (!isMutable()) return; _val = fromRegisterValue(val); From 7491bc0a27adb8c2094d5a8f2d0703640fc84bef Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 06:49:32 +0200 Subject: [PATCH 63/82] Create enum class for type-safe persistence configuration. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 ++--- src/register/RegisterDerived.hpp | 3 +- src/register/RegisterDerived.ipp | 4 +-- src/register/types/Persistence.h | 30 +++++++++++++++++++ 4 files changed, 38 insertions(+), 7 deletions(-) create mode 100644 src/register/types/Persistence.h diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index c52a735a..dcdf1018 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, false, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); -static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, false, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, false, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); -static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, false, "uavcan.primitive.scalar.Real32.1.0", nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistence::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistence::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistence::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistence::No, "uavcan.primitive.scalar.Real32.1.0", nullptr); static RegisterList reg_list; /************************************************************************************** diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 07e2ce6f..f4b2558b 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -14,6 +14,7 @@ #include "RegisterBase.h" #include "types/Access.h" +#include "types/Persistence.h" /************************************************************************************** * CLASS DECLARATION @@ -27,7 +28,7 @@ class RegisterDerived : public RegisterBase RegisterDerived(char const * name, Register::Access const access, - bool const is_persistent, + Register::Persistence const persistence, T const & initial_val, OnRegisterValueChangeFunc func); diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index c336859a..cd90a814 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -24,13 +24,13 @@ template T fromRegisterValue(uavcan_register_Value_1_0 const & val) template RegisterDerived::RegisterDerived(char const * name, Register::Access const access, - bool const is_persistent, + Register::Persistence const persistence, T const & initial_val, OnRegisterValueChangeFunc func) : RegisterBase{name, Register::toTypeTag(initial_val), (access == Register::Access::ReadOnly), - is_persistent} + (persistence == Register::Persistence::Yes)} , _val{initial_val} , _func{func} { } diff --git a/src/register/types/Persistence.h b/src/register/types/Persistence.h new file mode 100644 index 00000000..2317a311 --- /dev/null +++ b/src/register/types/Persistence.h @@ -0,0 +1,30 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef REGISTER_PERSISTENCE_H_ +#define REGISTER_PERSISTENCE_H_ + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +namespace Register +{ + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +enum class Persistence { Yes, No }; + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +} /* Register */ + +#endif /* REGISTER_PERSISTENCE_H_ */ From f53029ad66c06802400cf08f427eedc1eb08018c Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 06:51:27 +0200 Subject: [PATCH 64/82] Moving ListResponse/AccessReponse into subfolder util. --- src/register/RegisterList.cpp | 4 ++-- src/register/{ => util}/AccessResponse.hpp | 3 +-- src/register/{ => util}/AccessResponse.ipp | 0 src/register/{ => util}/ListResponse.cpp | 0 src/register/{ => util}/ListResponse.h | 2 +- 5 files changed, 4 insertions(+), 5 deletions(-) rename src/register/{ => util}/AccessResponse.hpp (95%) rename src/register/{ => util}/AccessResponse.ipp (100%) rename src/register/{ => util}/ListResponse.cpp (100%) rename src/register/{ => util}/ListResponse.h (97%) diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 5d4f9b44..aaaa2cd4 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -14,8 +14,8 @@ #include -#include "ListResponse.h" -#include "AccessResponse.hpp" +#include "util/ListResponse.h" +#include "util/AccessResponse.hpp" /************************************************************************************** * CTOR/DTOR diff --git a/src/register/AccessResponse.hpp b/src/register/util/AccessResponse.hpp similarity index 95% rename from src/register/AccessResponse.hpp rename to src/register/util/AccessResponse.hpp index 44facf4c..bff3687f 100644 --- a/src/register/AccessResponse.hpp +++ b/src/register/util/AccessResponse.hpp @@ -12,8 +12,7 @@ * INCLUDES **************************************************************************************/ -#include "../Types.h" -#include "RegisterDerived.hpp" +#include "../RegisterDerived.hpp" /************************************************************************************** * CLASS DECLARATION diff --git a/src/register/AccessResponse.ipp b/src/register/util/AccessResponse.ipp similarity index 100% rename from src/register/AccessResponse.ipp rename to src/register/util/AccessResponse.ipp diff --git a/src/register/ListResponse.cpp b/src/register/util/ListResponse.cpp similarity index 100% rename from src/register/ListResponse.cpp rename to src/register/util/ListResponse.cpp diff --git a/src/register/ListResponse.h b/src/register/util/ListResponse.h similarity index 97% rename from src/register/ListResponse.h rename to src/register/util/ListResponse.h index 0216d0b8..e8fe9e3e 100644 --- a/src/register/ListResponse.h +++ b/src/register/util/ListResponse.h @@ -12,7 +12,7 @@ * INCLUDES **************************************************************************************/ -#include "../Types.h" +#include "../../Types.h" /************************************************************************************** * CLASS DECLARATION From 5e61fe2e37640a13219b731a8fc625cd710396d2 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:12:32 +0200 Subject: [PATCH 65/82] Rename enum class Persistence to Persistent, as it makes more sense semantically. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 ++++---- src/register/RegisterBase.h | 2 +- src/register/RegisterDerived.hpp | 4 ++-- src/register/RegisterDerived.ipp | 4 ++-- src/register/types/{Persistence.h => Persistent.h} | 8 ++++---- 5 files changed, 13 insertions(+), 13 deletions(-) rename src/register/types/{Persistence.h => Persistent.h} (86%) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index dcdf1018..eece55dc 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistence::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); -static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistence::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistence::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); -static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistence::No, "uavcan.primitive.scalar.Real32.1.0", nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); +static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0", nullptr); static RegisterList reg_list; /************************************************************************************** diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index 22befeac..4d9d7793 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -28,7 +28,7 @@ class RegisterBase bool const is_persistent); - inline uavcan_register_Name_1_0 const & name() const { return _name; } + inline uavcan_register_Name_1_0 const & name() const { return _name; } inline Register::TypeTag type_tag() const { return _type_tag; } inline bool isMutable() const { return _is_mutable; } inline bool isPersistent() const { return _is_persistent; } diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index f4b2558b..87fe292a 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -14,7 +14,7 @@ #include "RegisterBase.h" #include "types/Access.h" -#include "types/Persistence.h" +#include "types/Persistent.h" /************************************************************************************** * CLASS DECLARATION @@ -28,7 +28,7 @@ class RegisterDerived : public RegisterBase RegisterDerived(char const * name, Register::Access const access, - Register::Persistence const persistence, + Register::Persistent const is_persistent, T const & initial_val, OnRegisterValueChangeFunc func); diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index cd90a814..14a381b6 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -24,13 +24,13 @@ template T fromRegisterValue(uavcan_register_Value_1_0 const & val) template RegisterDerived::RegisterDerived(char const * name, Register::Access const access, - Register::Persistence const persistence, + Register::Persistent const is_persistent, T const & initial_val, OnRegisterValueChangeFunc func) : RegisterBase{name, Register::toTypeTag(initial_val), (access == Register::Access::ReadOnly), - (persistence == Register::Persistence::Yes)} + (is_persistent == Register::Persistent::Yes)} , _val{initial_val} , _func{func} { } diff --git a/src/register/types/Persistence.h b/src/register/types/Persistent.h similarity index 86% rename from src/register/types/Persistence.h rename to src/register/types/Persistent.h index 2317a311..b9e949ee 100644 --- a/src/register/types/Persistence.h +++ b/src/register/types/Persistent.h @@ -5,8 +5,8 @@ * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. */ -#ifndef REGISTER_PERSISTENCE_H_ -#define REGISTER_PERSISTENCE_H_ +#ifndef REGISTER_PERSISTENT_H_ +#define REGISTER_PERSISTENT_H_ /************************************************************************************** * NAMESPACE @@ -19,7 +19,7 @@ namespace Register * TYPEDEF **************************************************************************************/ -enum class Persistence { Yes, No }; +enum class Persistent { Yes, No }; /************************************************************************************** * NAMESPACE @@ -27,4 +27,4 @@ enum class Persistence { Yes, No }; } /* Register */ -#endif /* REGISTER_PERSISTENCE_H_ */ +#endif /* REGISTER_PERSISTENT_H_ */ From db3e5ab54d44a88d5f07e314585272598328af17 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:16:10 +0200 Subject: [PATCH 66/82] Change semantics, do only provide new value in OnWrite callback. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 2 +- src/register/RegisterDerived.hpp | 8 +++++--- src/register/RegisterDerived.ipp | 8 ++++---- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index eece55dc..2cd83832 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,7 +155,7 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](RegisterNatural8 const & reg) { node_hdl.setNodeId(reg.get()); }); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](uint8_t const & val) { node_hdl.setNodeId(val); }); static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0", nullptr); diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 87fe292a..9ffe20d7 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -24,13 +24,13 @@ template class RegisterDerived : public RegisterBase { public: - typedef std::function const &)> OnRegisterValueChangeFunc; + typedef std::function OnWriteRequestFunc; RegisterDerived(char const * name, Register::Access const access, Register::Persistent const is_persistent, T const & initial_val, - OnRegisterValueChangeFunc func); + OnWriteRequestFunc on_write_func); inline T get() const { return _val; } void set(uavcan_register_Value_1_0 const & val); @@ -38,7 +38,9 @@ class RegisterDerived : public RegisterBase private: T _val; - OnRegisterValueChangeFunc _func; + OnWriteRequestFunc _on_write_func; + + inline void onWriteFunc() const { if (_on_write_func) _on_write_func(_val); } }; /************************************************************************************** diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 14a381b6..01f0895d 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -26,13 +26,13 @@ RegisterDerived::RegisterDerived(char const * name, Register::Access const access, Register::Persistent const is_persistent, T const & initial_val, - OnRegisterValueChangeFunc func) + OnWriteRequestFunc on_write_func) : RegisterBase{name, Register::toTypeTag(initial_val), (access == Register::Access::ReadOnly), (is_persistent == Register::Persistent::Yes)} , _val{initial_val} -, _func{func} +, _on_write_func{on_write_func} { } /************************************************************************************** @@ -46,8 +46,8 @@ void RegisterDerived::set(uavcan_register_Value_1_0 const & val) return; _val = fromRegisterValue(val); - if (_func) - _func(*this); + + onWriteFunc(); } /************************************************************************************** From bb801206338be4c8eebc2183b5ca2b7896ec7497 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:23:53 +0200 Subject: [PATCH 67/82] Provide callback to be called in case of a read request being sent (and before that request is fulfilled. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 +++---- src/register/RegisterDerived.hpp | 11 +++++---- src/register/RegisterDerived.ipp | 8 ++++--- src/register/RegisterList.cpp | 24 +++++++++++++++++++ 4 files changed, 40 insertions(+), 11 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 2cd83832..23c419f4 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](uint8_t const & val) { node_hdl.setNodeId(val); }); -static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr); -static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0", nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](uint8_t const & val) { node_hdl.setNodeId(val); }, nullptr); +static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr, nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr, nullptr); +static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0", nullptr, nullptr); static RegisterList reg_list; /************************************************************************************** diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 9ffe20d7..6c376d31 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -25,22 +25,25 @@ class RegisterDerived : public RegisterBase { public: typedef std::function OnWriteRequestFunc; + typedef std::function &)> onReadRequestFunc; RegisterDerived(char const * name, Register::Access const access, Register::Persistent const is_persistent, T const & initial_val, - OnWriteRequestFunc on_write_func); + OnWriteRequestFunc on_write_request_func, + onReadRequestFunc on_read_request_func); inline T get() const { return _val; } void set(uavcan_register_Value_1_0 const & val); - + inline void onReadRequest() { if (_on_read_request_func) _on_read_request_func(*this); } private: T _val; - OnWriteRequestFunc _on_write_func; + OnWriteRequestFunc _on_write_request_func; + onReadRequestFunc _on_read_request_func; - inline void onWriteFunc() const { if (_on_write_func) _on_write_func(_val); } + inline void onWriteRequest() const { if (_on_write_request_func) _on_write_request_func(_val); } }; /************************************************************************************** diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 01f0895d..2d96377d 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -26,13 +26,15 @@ RegisterDerived::RegisterDerived(char const * name, Register::Access const access, Register::Persistent const is_persistent, T const & initial_val, - OnWriteRequestFunc on_write_func) + OnWriteRequestFunc on_write_request_func, + onReadRequestFunc on_read_request_func) : RegisterBase{name, Register::toTypeTag(initial_val), (access == Register::Access::ReadOnly), (is_persistent == Register::Persistent::Yes)} , _val{initial_val} -, _on_write_func{on_write_func} +, _on_write_request_func{on_write_request_func} +, _on_read_request_func{on_read_request_func} { } /************************************************************************************** @@ -47,7 +49,7 @@ void RegisterDerived::set(uavcan_register_Value_1_0 const & val) _val = fromRegisterValue(val); - onWriteFunc(); + onWriteRequest(); } /************************************************************************************** diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index aaaa2cd4..b632bcf2 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -90,6 +90,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if (type_tag == Register::TypeTag::String) { RegisterString * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_string_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -97,6 +99,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Bit) { RegisterBit * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_bit_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -104,6 +108,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Integer8) { RegisterInteger8 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer8_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -111,6 +117,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Integer16) { RegisterInteger16 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer16_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -118,6 +126,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Integer32) { RegisterInteger32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer32_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -125,6 +135,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Integer64) { RegisterInteger64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer64_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -132,6 +144,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Natural8) { RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -139,6 +153,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Natural16) { RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -146,6 +162,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Natural32) { RegisterNatural32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural32_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -153,6 +171,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Natural64) { RegisterNatural64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural64_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -160,6 +180,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Real32) { RegisterReal32 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real32_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); @@ -167,6 +189,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf else if (type_tag == Register::TypeTag::Real64) { RegisterReal64 * reg_ptr = reinterpret_cast(*iter); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real64_(&req.data.value)) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); From 22e785a24edceded643346b559c35c49e51155ad Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:27:25 +0200 Subject: [PATCH 68/82] Fix: It should be allowed to set a value from the user-side, RO refers to the perspective of the external communication partner. --- src/register/RegisterDerived.ipp | 3 --- src/register/RegisterList.cpp | 24 ++++++++++++------------ 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 2d96377d..ed8e8e6a 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -44,9 +44,6 @@ RegisterDerived::RegisterDerived(char const * name, template void RegisterDerived::set(uavcan_register_Value_1_0 const & val) { - if (!isMutable()) - return; - _val = fromRegisterValue(val); onWriteRequest(); diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index b632bcf2..cba566d3 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -92,7 +92,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterString * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_string_(&req.data.value)) + if(uavcan_register_Value_1_0_is_string_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -101,7 +101,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterBit * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_bit_(&req.data.value)) + if(uavcan_register_Value_1_0_is_bit_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -110,7 +110,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterInteger8 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer8_(&req.data.value)) + if(uavcan_register_Value_1_0_is_integer8_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -119,7 +119,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterInteger16 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer16_(&req.data.value)) + if(uavcan_register_Value_1_0_is_integer16_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -128,7 +128,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterInteger32 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer32_(&req.data.value)) + if(uavcan_register_Value_1_0_is_integer32_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -137,7 +137,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterInteger64 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer64_(&req.data.value)) + if(uavcan_register_Value_1_0_is_integer64_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -146,7 +146,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural8_(&req.data.value)) + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -155,7 +155,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural16_(&req.data.value)) + if(uavcan_register_Value_1_0_is_natural16_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -164,7 +164,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterNatural32 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural32_(&req.data.value)) + if(uavcan_register_Value_1_0_is_natural32_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -173,7 +173,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterNatural64 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural64_(&req.data.value)) + if(uavcan_register_Value_1_0_is_natural64_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -182,7 +182,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterReal32 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_real32_(&req.data.value)) + if(uavcan_register_Value_1_0_is_real32_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } @@ -191,7 +191,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf RegisterReal64 * reg_ptr = reinterpret_cast(*iter); if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_real64_(&req.data.value)) + if(uavcan_register_Value_1_0_is_real64_(&req.data.value) && reg_ptr->isMutable()) reg_ptr->set(req.data.value); rsp = AccessResponse::create(*reg_ptr); } From 1a26ee28e0dd15514012468e7b94866f10586efa Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:30:22 +0200 Subject: [PATCH 69/82] Move calling of onWriteRequestFunc inside RegisterList. --- src/register/RegisterDerived.hpp | 6 +++++- src/register/RegisterDerived.ipp | 2 -- src/register/RegisterList.cpp | 36 ++++++++++++++++++++++++++++++++ 3 files changed, 41 insertions(+), 3 deletions(-) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 6c376d31..9234a8e5 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -27,6 +27,7 @@ class RegisterDerived : public RegisterBase typedef std::function OnWriteRequestFunc; typedef std::function &)> onReadRequestFunc; + RegisterDerived(char const * name, Register::Access const access, Register::Persistent const is_persistent, @@ -34,16 +35,19 @@ class RegisterDerived : public RegisterBase OnWriteRequestFunc on_write_request_func, onReadRequestFunc on_read_request_func); + inline T get() const { return _val; } void set(uavcan_register_Value_1_0 const & val); + inline void onReadRequest() { if (_on_read_request_func) _on_read_request_func(*this); } + inline void onWriteRequest() const { if (_on_write_request_func) _on_write_request_func(_val); } + private: T _val; OnWriteRequestFunc _on_write_request_func; onReadRequestFunc _on_read_request_func; - inline void onWriteRequest() const { if (_on_write_request_func) _on_write_request_func(_val); } }; /************************************************************************************** diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index ed8e8e6a..4a8cd499 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -45,8 +45,6 @@ template void RegisterDerived::set(uavcan_register_Value_1_0 const & val) { _val = fromRegisterValue(val); - - onWriteRequest(); } /************************************************************************************** diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index cba566d3..7f791afd 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -93,7 +93,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_string_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Bit) @@ -102,7 +105,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_bit_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer8) @@ -111,7 +117,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer8_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer16) @@ -120,7 +129,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer16_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer32) @@ -129,7 +141,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer32_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Integer64) @@ -138,7 +153,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer64_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural8) @@ -147,7 +165,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural16) @@ -156,7 +177,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural32) @@ -165,7 +189,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural32_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Natural64) @@ -174,7 +201,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural64_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Real32) @@ -183,7 +213,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real32_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } else if (type_tag == Register::TypeTag::Real64) @@ -192,7 +225,10 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real64_(&req.data.value) && reg_ptr->isMutable()) + { reg_ptr->set(req.data.value); + reg_ptr->onWriteRequest(); + } rsp = AccessResponse::create(*reg_ptr); } /* TODO: Do for other types ... Real16 */ From a35c640ddc225a840d0669cf788c51715da68a20 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:39:23 +0200 Subject: [PATCH 70/82] Fix typo: CamelCase not fully consistent. --- src/register/RegisterDerived.hpp | 7 +++---- src/register/RegisterDerived.ipp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 9234a8e5..f26845c0 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -25,7 +25,7 @@ class RegisterDerived : public RegisterBase { public: typedef std::function OnWriteRequestFunc; - typedef std::function &)> onReadRequestFunc; + typedef std::function &)> OnReadRequestFunc; RegisterDerived(char const * name, @@ -33,7 +33,7 @@ class RegisterDerived : public RegisterBase Register::Persistent const is_persistent, T const & initial_val, OnWriteRequestFunc on_write_request_func, - onReadRequestFunc on_read_request_func); + OnReadRequestFunc on_read_request_func); inline T get() const { return _val; } @@ -46,8 +46,7 @@ class RegisterDerived : public RegisterBase private: T _val; OnWriteRequestFunc _on_write_request_func; - onReadRequestFunc _on_read_request_func; - + OnReadRequestFunc _on_read_request_func; }; /************************************************************************************** diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 4a8cd499..60731048 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -27,7 +27,7 @@ RegisterDerived::RegisterDerived(char const * name, Register::Persistent const is_persistent, T const & initial_val, OnWriteRequestFunc on_write_request_func, - onReadRequestFunc on_read_request_func) + OnReadRequestFunc on_read_request_func) : RegisterBase{name, Register::toTypeTag(initial_val), (access == Register::Access::ReadOnly), From 688cadde10fa02b8e37aab30276bb2e18a79720a Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:50:36 +0200 Subject: [PATCH 71/82] Extract union to type conversion into module AccessRequest. Also fully transfer value setting into register class module. --- src/register/RegisterDerived.hpp | 2 +- src/register/RegisterDerived.ipp | 83 ------------------------ src/register/RegisterList.cpp | 25 ++++---- src/register/util/AccessRequest.hpp | 37 +++++++++++ src/register/util/AccessRequest.ipp | 97 +++++++++++++++++++++++++++++ 5 files changed, 148 insertions(+), 96 deletions(-) create mode 100644 src/register/util/AccessRequest.hpp create mode 100644 src/register/util/AccessRequest.ipp diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index f26845c0..584fd483 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -37,7 +37,7 @@ class RegisterDerived : public RegisterBase inline T get() const { return _val; } - void set(uavcan_register_Value_1_0 const & val); + inline void set(T const & val) { _val = val; } inline void onReadRequest() { if (_on_read_request_func) _on_read_request_func(*this); } inline void onWriteRequest() const { if (_on_write_request_func) _on_write_request_func(_val); } diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 60731048..3a46c3d9 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -11,12 +11,6 @@ extern "C" unsigned long micros(void); -/************************************************************************************** - * FUNCTION DECLARATION - **************************************************************************************/ - -template T fromRegisterValue(uavcan_register_Value_1_0 const & val); - /************************************************************************************** * CTOR/DTOR **************************************************************************************/ @@ -36,80 +30,3 @@ RegisterDerived::RegisterDerived(char const * name, , _on_write_request_func{on_write_request_func} , _on_read_request_func{on_read_request_func} { } - -/************************************************************************************** - * PUBLIC MEMBER FUNCTIONS - **************************************************************************************/ - -template -void RegisterDerived::set(uavcan_register_Value_1_0 const & val) -{ - _val = fromRegisterValue(val); -} - -/************************************************************************************** - * FUNCTION DEFINITION - **************************************************************************************/ - -template<> inline std::string fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; - memcpy(str, val._string.value.elements, val._string.value.count); - str[val._string.value.count + 1] = '\0'; - return std::string(str); -} - -template<> inline bool fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.bit.value.bitpacked[0]; -} - -template<> inline int8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer8.value.elements[0]; -} - -template<> inline int16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer16.value.elements[0]; -} - -template<> inline int32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer32.value.elements[0]; -} - -template<> inline int64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.integer64.value.elements[0]; -} - -template<> inline uint8_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural8.value.elements[0]; -} - -template<> inline uint16_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural16.value.elements[0]; -} - -template<> inline uint32_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural32.value.elements[0]; -} - -template<> inline uint64_t fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.natural64.value.elements[0]; -} - -template<> inline float fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.real32.value.elements[0]; -} - -template<> inline double fromRegisterValue(uavcan_register_Value_1_0 const & val) -{ - return val.real64.value.elements[0]; -} diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 7f791afd..a704b896 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -15,6 +15,7 @@ #include #include "util/ListResponse.h" +#include "util/AccessRequest.hpp" #include "util/AccessResponse.hpp" /************************************************************************************** @@ -94,7 +95,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_string_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -106,7 +107,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_bit_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -118,7 +119,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer8_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -130,7 +131,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer16_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -142,7 +143,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer32_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -154,7 +155,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer64_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -166,7 +167,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -178,7 +179,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -190,7 +191,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural32_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -202,7 +203,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural64_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -214,7 +215,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real32_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); @@ -226,7 +227,7 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real64_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(req.data.value); + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } rsp = AccessResponse::create(*reg_ptr); diff --git a/src/register/util/AccessRequest.hpp b/src/register/util/AccessRequest.hpp new file mode 100644 index 00000000..21103742 --- /dev/null +++ b/src/register/util/AccessRequest.hpp @@ -0,0 +1,37 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +#ifndef ACCESS_REQUEST_HPP_ +#define ACCESS_REQUEST_HPP_ + +/************************************************************************************** + * INCLUDES + **************************************************************************************/ + +#include "../../Types.h" + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class AccessRequest +{ +public: + AccessRequest() = delete; + AccessRequest(AccessRequest const & other) = delete; + + template + static T toRegisterValue(uavcan_register_Value_1_0 const & val); +}; + +/************************************************************************************** + * TEMPLATE IMPLEMEMTATION + **************************************************************************************/ + +#include "AccessRequest.ipp" + +#endif /* ACCESS_REQUEST_HPP_ */ diff --git a/src/register/util/AccessRequest.ipp b/src/register/util/AccessRequest.ipp new file mode 100644 index 00000000..aebc796e --- /dev/null +++ b/src/register/util/AccessRequest.ipp @@ -0,0 +1,97 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ + +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +namespace impl +{ +template T toRegisterValue(uavcan_register_Value_1_0 const & val); +} /* impl */ + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +template +T AccessRequest::toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return impl::toRegisterValue(val); +} + +/************************************************************************************** + * FUNCTION DEFINITION + **************************************************************************************/ + +namespace impl +{ + +template<> inline std::string toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + char str[uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_] = {0}; + memcpy(str, val._string.value.elements, val._string.value.count); + str[val._string.value.count + 1] = '\0'; + return std::string(str); +} + +template<> inline bool toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.bit.value.bitpacked[0]; +} + +template<> inline int8_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer8.value.elements[0]; +} + +template<> inline int16_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer16.value.elements[0]; +} + +template<> inline int32_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer32.value.elements[0]; +} + +template<> inline int64_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.integer64.value.elements[0]; +} + +template<> inline uint8_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural8.value.elements[0]; +} + +template<> inline uint16_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural16.value.elements[0]; +} + +template<> inline uint32_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural32.value.elements[0]; +} + +template<> inline uint64_t toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.natural64.value.elements[0]; +} + +template<> inline float toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.real32.value.elements[0]; +} + +template<> inline double toRegisterValue(uavcan_register_Value_1_0 const & val) +{ + return val.real64.value.elements[0]; +} + +} /* impl */ From 7352e4bf0828cd0f827054e0d79127ddd09dbaa6 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:53:10 +0200 Subject: [PATCH 72/82] Specifying type of enum class TypeTag to be of underlying type size_t. --- src/register/types/TypeTag.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp index 44d7e05f..2e8a6b11 100644 --- a/src/register/types/TypeTag.hpp +++ b/src/register/types/TypeTag.hpp @@ -25,9 +25,9 @@ namespace Register * TYPEDEF **************************************************************************************/ -enum class TypeTag +enum class TypeTag : size_t { - Empty, + Empty = 0, String, Bit, Integer8, From 5df0594bba2eb43f6f90626bf4304e6d27a22e79 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:53:44 +0200 Subject: [PATCH 73/82] Drop enum value Real16, as it is not supported by this platform. --- src/register/types/TypeTag.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp index 2e8a6b11..24b30f1d 100644 --- a/src/register/types/TypeTag.hpp +++ b/src/register/types/TypeTag.hpp @@ -38,7 +38,6 @@ enum class TypeTag : size_t Natural16, Natural32, Natural64, - Real16, Real32, Real64, }; From 1b2b266814e527b8feb7f4b5ca91eca77124aea8 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 08:55:54 +0200 Subject: [PATCH 74/82] Adding constant defining the number of entries within enum class TypeTag. --- src/register/types/TypeTag.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp index 24b30f1d..5c329233 100644 --- a/src/register/types/TypeTag.hpp +++ b/src/register/types/TypeTag.hpp @@ -14,6 +14,8 @@ #include +#include "../../utility/convert.hpp" + /************************************************************************************** * NAMESPACE **************************************************************************************/ @@ -42,6 +44,12 @@ enum class TypeTag : size_t Real64, }; +/************************************************************************************** + * CONSTANT + **************************************************************************************/ + +static size_t constexpr TYPE_TAG_SIZE = arduino::_107_::opencyphal::to_integer(TypeTag::Real64); + /************************************************************************************** * FUNCTION DECLARATION **************************************************************************************/ From e7a909e9a14699db533671aa8ae40e0bc61c9b2c Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 09:13:34 +0200 Subject: [PATCH 75/82] Refactor read/write request handling by storing event handlers in function callback array. --- src/register/RegisterList.cpp | 336 +++++++++++++++++++--------------- src/register/RegisterList.h | 3 + 2 files changed, 191 insertions(+), 148 deletions(-) diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index a704b896..542cad85 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -24,7 +24,188 @@ RegisterList::RegisterList() : _reg_last{"", Register::TypeTag::Empty, false, false} -{ } +{ + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Empty)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + uavcan::_register::Access_1_0::Response<> r; + + uavcan_register_Value_1_0_select_empty_(&r.data.value); + r.data.timestamp.microsecond = 0; + r.data._mutable = false; + r.data.persistent = false; + + return r; + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::String)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterString * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_string_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Bit)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterBit * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_bit_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer8)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterInteger8 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_integer8_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer16)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterInteger16 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_integer16_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer32)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterInteger32 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_integer32_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer64)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterInteger64 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_integer64_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural8)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterNatural8 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_natural8_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural16)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterNatural16 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_natural16_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural32)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterNatural32 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_natural32_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural64)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterNatural64 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_natural64_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Real32)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterReal32 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_real32_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; + + _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Real64)] = + [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) + { + RegisterReal64 * reg_ptr = reinterpret_cast(reg_base_ptr); + if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) + reg_ptr->onReadRequest(); + if(uavcan_register_Value_1_0_is_real64_(&req.data.value) && reg_ptr->isMutable()) + { + reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + reg_ptr->onWriteRequest(); + } + return AccessResponse::create(*reg_ptr); + }; +} /************************************************************************************** * PUBLIC MEMBER FUNCTIONS @@ -86,153 +267,12 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf if (iter != std::end(_reg_list)) { - Register::TypeTag const type_tag = (*iter)->type_tag(); - - if (type_tag == Register::TypeTag::String) - { - RegisterString * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_string_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Bit) - { - RegisterBit * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_bit_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Integer8) - { - RegisterInteger8 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer8_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Integer16) - { - RegisterInteger16 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer16_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Integer32) - { - RegisterInteger32 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer32_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Integer64) - { - RegisterInteger64 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_integer64_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Natural8) - { - RegisterNatural8 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural8_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Natural16) - { - RegisterNatural16 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural16_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Natural32) - { - RegisterNatural32 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural32_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Natural64) - { - RegisterNatural64 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_natural64_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Real32) - { - RegisterReal32 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_real32_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - else if (type_tag == Register::TypeTag::Real64) - { - RegisterReal64 * reg_ptr = reinterpret_cast(*iter); - if(uavcan_register_Value_1_0_is_empty_(&req.data.value)) - reg_ptr->onReadRequest(); - if(uavcan_register_Value_1_0_is_real64_(&req.data.value) && reg_ptr->isMutable()) - { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); - reg_ptr->onWriteRequest(); - } - rsp = AccessResponse::create(*reg_ptr); - } - /* TODO: Do for other types ... Real16 */ + /* Store in separate variable for better clarity. */ + RegisterBase * reg_base_ptr = *iter; + /* Determine the actual type of the register. */ + Register::TypeTag const type_tag = reg_base_ptr->type_tag(); + /* Call the approbriate callback handler. */ + rsp = _on_access_request_handler[arduino::_107_::opencyphal::to_integer(type_tag)](req, reg_base_ptr); } /* Send the actual response. */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index ece38c19..7e34f7eb 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -36,6 +36,9 @@ class RegisterList private: std::vector _reg_list; RegisterBase const _reg_last; + typedef std::function(uavcan::_register::Access_1_0::Request<> const &, RegisterBase *)> OnAccessRequestHandlerFunc; + OnAccessRequestHandlerFunc _on_access_request_handler[Register::TYPE_TAG_SIZE]; + void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); void onAccess_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); From bc9eeb69066a97db38dda59c75485998a3c65020 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 09:42:19 +0200 Subject: [PATCH 76/82] Add limit-api which allows to limit the value which can be set via remote call. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 ++-- src/register/RegisterDerived.hpp | 7 +++- src/register/RegisterDerived.ipp | 17 +++++++- src/register/RegisterList.cpp | 42 ++++++++++++++----- 4 files changed, 57 insertions(+), 17 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 23c419f4..a597edf7 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](uint8_t const & val) { node_hdl.setNodeId(val); }, nullptr); -static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr, nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr, nullptr); -static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0", nullptr, nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](uint8_t const & val) { node_hdl.setNodeId(val); }, nullptr, nullptr); +static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr, nullptr, nullptr); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr, nullptr, nullptr); +static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0", nullptr, nullptr, nullptr); static RegisterList reg_list; /************************************************************************************** diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index 584fd483..e2b0e0d3 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -26,6 +26,7 @@ class RegisterDerived : public RegisterBase public: typedef std::function OnWriteRequestFunc; typedef std::function &)> OnReadRequestFunc; + typedef std::function ValueLimiterFunc; RegisterDerived(char const * name, @@ -33,20 +34,22 @@ class RegisterDerived : public RegisterBase Register::Persistent const is_persistent, T const & initial_val, OnWriteRequestFunc on_write_request_func, - OnReadRequestFunc on_read_request_func); + OnReadRequestFunc on_read_request_func, + ValueLimiterFunc value_limiter_func); inline T get() const { return _val; } inline void set(T const & val) { _val = val; } + T limitValue(T const & val) const; inline void onReadRequest() { if (_on_read_request_func) _on_read_request_func(*this); } inline void onWriteRequest() const { if (_on_write_request_func) _on_write_request_func(_val); } - private: T _val; OnWriteRequestFunc _on_write_request_func; OnReadRequestFunc _on_read_request_func; + ValueLimiterFunc _value_limiter_func; }; /************************************************************************************** diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 3a46c3d9..d77127b9 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -21,7 +21,8 @@ RegisterDerived::RegisterDerived(char const * name, Register::Persistent const is_persistent, T const & initial_val, OnWriteRequestFunc on_write_request_func, - OnReadRequestFunc on_read_request_func) + OnReadRequestFunc on_read_request_func, + ValueLimiterFunc value_limiter_func) : RegisterBase{name, Register::toTypeTag(initial_val), (access == Register::Access::ReadOnly), @@ -29,4 +30,18 @@ RegisterDerived::RegisterDerived(char const * name, , _val{initial_val} , _on_write_request_func{on_write_request_func} , _on_read_request_func{on_read_request_func} +, _value_limiter_func{value_limiter_func} { } + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +template +T RegisterDerived::limitValue(T const & val) const +{ + if (_value_limiter_func) + return _value_limiter_func(val); + else + return val; +} diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 542cad85..452f5e4e 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -46,6 +46,7 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_string_(&req.data.value) && reg_ptr->isMutable()) { + /* Limitation makes no sense for 'std::string' */ reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } @@ -60,6 +61,7 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_bit_(&req.data.value) && reg_ptr->isMutable()) { + /* Limitation makes no sense for 'bool' */ reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); reg_ptr->onWriteRequest(); } @@ -74,7 +76,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer8_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + int8_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + int8_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -88,7 +92,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer16_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + int16_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + int16_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -102,7 +108,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer32_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + int32_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + int32_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -116,7 +124,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_integer64_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + int64_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + int64_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -130,7 +140,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural8_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + uint8_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + uint8_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -144,7 +156,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural16_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + uint16_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + uint16_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -158,7 +172,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural32_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + uint32_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + uint32_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -172,7 +188,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_natural64_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + uint64_t const reg_val = AccessRequest::toRegisterValue(req.data.value); + uint64_t const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -186,7 +204,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real32_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + float const reg_val = AccessRequest::toRegisterValue(req.data.value); + float const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); @@ -200,7 +220,9 @@ RegisterList::RegisterList() reg_ptr->onReadRequest(); if(uavcan_register_Value_1_0_is_real64_(&req.data.value) && reg_ptr->isMutable()) { - reg_ptr->set(AccessRequest::toRegisterValue(req.data.value)); + double const reg_val = AccessRequest::toRegisterValue(req.data.value); + double const limited_reg_val = reg_ptr->limitValue(reg_val); + reg_ptr->set(limited_reg_val); reg_ptr->onWriteRequest(); } return AccessResponse::create(*reg_ptr); From f4896ad52e2acbcac3092d7bbbc908e84c50a223 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 09:45:28 +0200 Subject: [PATCH 77/82] CI spellcheck: fix typo. --- src/register/util/AccessRequest.hpp | 2 +- src/register/util/AccessResponse.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/register/util/AccessRequest.hpp b/src/register/util/AccessRequest.hpp index 21103742..631b6601 100644 --- a/src/register/util/AccessRequest.hpp +++ b/src/register/util/AccessRequest.hpp @@ -29,7 +29,7 @@ class AccessRequest }; /************************************************************************************** - * TEMPLATE IMPLEMEMTATION + * TEMPLATE IMPLEMENTATION **************************************************************************************/ #include "AccessRequest.ipp" diff --git a/src/register/util/AccessResponse.hpp b/src/register/util/AccessResponse.hpp index bff3687f..3b14b049 100644 --- a/src/register/util/AccessResponse.hpp +++ b/src/register/util/AccessResponse.hpp @@ -29,7 +29,7 @@ class AccessResponse }; /************************************************************************************** - * TEMPLATE IMPLEMEMTATION + * TEMPLATE IMPLEMENTATION **************************************************************************************/ #include "AccessResponse.ipp" From 8c8cdcaca6db7781346c264d395aca361ae6cd36 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 10:51:09 +0200 Subject: [PATCH 78/82] Fix: only update the timestamp when the value is actually changed, not when it is retrieved. --- src/register/RegisterBase.cpp | 9 +++++++++ src/register/RegisterBase.h | 6 ++++++ src/register/RegisterDerived.hpp | 2 +- src/register/RegisterDerived.ipp | 7 +++++++ src/register/util/AccessResponse.ipp | 8 +------- 5 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/register/RegisterBase.cpp b/src/register/RegisterBase.cpp index 0a570777..9ada5f8b 100644 --- a/src/register/RegisterBase.cpp +++ b/src/register/RegisterBase.cpp @@ -34,6 +34,15 @@ RegisterBase::RegisterBase(char const * name, , _type_tag{type_tag} , _is_mutable{is_mutable} , _is_persistent{is_persistent} +, _timestamp +{ + []() -> uavcan_time_SynchronizedTimestamp_1_0 + { + uavcan_time_SynchronizedTimestamp_1_0 ts; + ts.microsecond = 0; + return ts; + } () +} { } diff --git a/src/register/RegisterBase.h b/src/register/RegisterBase.h index 4d9d7793..9bcc1c5b 100644 --- a/src/register/RegisterBase.h +++ b/src/register/RegisterBase.h @@ -32,6 +32,11 @@ class RegisterBase inline Register::TypeTag type_tag() const { return _type_tag; } inline bool isMutable() const { return _is_mutable; } inline bool isPersistent() const { return _is_persistent; } + inline uavcan_time_SynchronizedTimestamp_1_0 timestamp() const { return _timestamp; } + + +protected: + inline void setTimestamp(uint64_t const microsecond) { _timestamp.microsecond = microsecond; } private: @@ -39,6 +44,7 @@ class RegisterBase Register::TypeTag const _type_tag; bool const _is_mutable; bool const _is_persistent; + uavcan_time_SynchronizedTimestamp_1_0 _timestamp; }; #endif /* REGISTER_BASE_H_ */ diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index e2b0e0d3..c272f441 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -39,7 +39,7 @@ class RegisterDerived : public RegisterBase inline T get() const { return _val; } - inline void set(T const & val) { _val = val; } + void set(T const & val); T limitValue(T const & val) const; inline void onReadRequest() { if (_on_read_request_func) _on_read_request_func(*this); } diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index d77127b9..8bbc2e9e 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -37,6 +37,13 @@ RegisterDerived::RegisterDerived(char const * name, * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ +template +void RegisterDerived::set(T const & val) +{ + _val = val; + setTimestamp(micros()); +} + template T RegisterDerived::limitValue(T const & val) const { diff --git a/src/register/util/AccessResponse.ipp b/src/register/util/AccessResponse.ipp index 5b0715d9..c64450a0 100644 --- a/src/register/util/AccessResponse.ipp +++ b/src/register/util/AccessResponse.ipp @@ -11,12 +11,6 @@ template uavcan_register_Value_1_0 toRegisterValue(T const & val); -/************************************************************************************** - * EXTERN DECLARATION - **************************************************************************************/ - -extern "C" unsigned long micros(void); - /************************************************************************************** * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ @@ -27,7 +21,7 @@ uavcan::_register::Access_1_0::Response<> AccessResponse::create(RegisterDerived uavcan::_register::Access_1_0::Response<> rsp; rsp.data.value = toRegisterValue(reg.get()); - rsp.data.timestamp.microsecond = micros(); + rsp.data.timestamp = reg.timestamp(); rsp.data._mutable = reg.isMutable(); rsp.data.persistent = reg.isPersistent(); From 4a01f8d407b19aa990b490efc6366006263006b4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 27 Sep 2022 13:21:58 +0200 Subject: [PATCH 79/82] Overloaded constructors ease the burden for configuration. --- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 8 ++++---- src/register/RegisterDerived.hpp | 15 +++++++++++++++ 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index a597edf7..b9e8f86d 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -155,10 +155,10 @@ DEBUG_INSTANCE(120, Serial); /* REGISTER ***************************************************************************/ -static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](uint8_t const & val) { node_hdl.setNodeId(val); }, nullptr, nullptr); -static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node", nullptr, nullptr, nullptr); -static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA, nullptr, nullptr, nullptr); -static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0", nullptr, nullptr, nullptr); +static RegisterNatural8 reg_rw_uavcan_node_id ("uavcan.node.id", Register::Access::ReadWrite, Register::Persistent::No, OPEN_CYPHAL_NODE_ID, [&node_hdl](uint8_t const & val) { node_hdl.setNodeId(val); }); +static RegisterString reg_ro_uavcan_node_description ("uavcan.node.description", Register::Access::ReadWrite, Register::Persistent::No, "OpenCyphal-ToF-Distance-Sensor-Node"); +static RegisterNatural16 reg_ro_uavcan_pub_distance_id ("uavcan.pub.distance.id", Register::Access::ReadOnly, Register::Persistent::No, OPEN_CYPHAL_ID_DISTANCE_DATA); +static RegisterString reg_ro_uavcan_pub_distance_type("uavcan.pub.distance.type", Register::Access::ReadOnly, Register::Persistent::No, "uavcan.primitive.scalar.Real32.1.0"); static RegisterList reg_list; /************************************************************************************** diff --git a/src/register/RegisterDerived.hpp b/src/register/RegisterDerived.hpp index c272f441..326af175 100644 --- a/src/register/RegisterDerived.hpp +++ b/src/register/RegisterDerived.hpp @@ -37,6 +37,21 @@ class RegisterDerived : public RegisterBase OnReadRequestFunc on_read_request_func, ValueLimiterFunc value_limiter_func); + RegisterDerived(char const * name, + Register::Access const access, + Register::Persistent const is_persistent, + T const & initial_val) + : RegisterDerived(name, access, is_persistent, initial_val, nullptr, nullptr, nullptr) + { } + + RegisterDerived(char const * name, + Register::Access const access, + Register::Persistent const is_persistent, + T const & initial_val, + OnWriteRequestFunc on_write_request_func) + : RegisterDerived(name, access, is_persistent, initial_val, on_write_request_func, nullptr, nullptr) + { } + inline T get() const { return _val; } void set(T const & val); From d5077647639e7883be4ef38ab2d045039f946144 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 28 Sep 2022 10:17:06 +0200 Subject: [PATCH 80/82] Fix: Using a std::map and checking for existence of a key rules out memory access violations due to an unexpected type tag. --- src/register/RegisterList.cpp | 29 +++++++++++++++-------------- src/register/RegisterList.h | 3 ++- src/register/types/TypeTag.hpp | 8 -------- 3 files changed, 17 insertions(+), 23 deletions(-) diff --git a/src/register/RegisterList.cpp b/src/register/RegisterList.cpp index 452f5e4e..96bce04e 100644 --- a/src/register/RegisterList.cpp +++ b/src/register/RegisterList.cpp @@ -25,7 +25,7 @@ RegisterList::RegisterList() : _reg_last{"", Register::TypeTag::Empty, false, false} { - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Empty)] = + _on_access_request_handler_map[Register::TypeTag::Empty] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { uavcan::_register::Access_1_0::Response<> r; @@ -38,7 +38,7 @@ RegisterList::RegisterList() return r; }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::String)] = + _on_access_request_handler_map[Register::TypeTag::String] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterString * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -53,7 +53,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Bit)] = + _on_access_request_handler_map[Register::TypeTag::Bit] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterBit * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -68,7 +68,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer8)] = + _on_access_request_handler_map[Register::TypeTag::Integer8] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterInteger8 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -84,7 +84,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer16)] = + _on_access_request_handler_map[Register::TypeTag::Integer16] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterInteger16 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -100,7 +100,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer32)] = + _on_access_request_handler_map[Register::TypeTag::Integer32] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterInteger32 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -116,7 +116,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Integer64)] = + _on_access_request_handler_map[Register::TypeTag::Integer64] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterInteger64 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -132,7 +132,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural8)] = + _on_access_request_handler_map[Register::TypeTag::Natural8] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterNatural8 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -148,7 +148,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural16)] = + _on_access_request_handler_map[Register::TypeTag::Natural16] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterNatural16 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -164,7 +164,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural32)] = + _on_access_request_handler_map[Register::TypeTag::Natural32] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterNatural32 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -180,7 +180,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Natural64)] = + _on_access_request_handler_map[Register::TypeTag::Natural64] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterNatural64 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -196,7 +196,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Real32)] = + _on_access_request_handler_map[Register::TypeTag::Real32] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterReal32 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -212,7 +212,7 @@ RegisterList::RegisterList() return AccessResponse::create(*reg_ptr); }; - _on_access_request_handler[arduino::_107_::opencyphal::to_integer(Register::TypeTag::Real64)] = + _on_access_request_handler_map[Register::TypeTag::Real64] = [](uavcan::_register::Access_1_0::Request<> const & req, RegisterBase * reg_base_ptr) { RegisterReal64 * reg_ptr = reinterpret_cast(reg_base_ptr); @@ -294,7 +294,8 @@ void RegisterList::onAccess_1_0_Request_Received(CanardRxTransfer const & transf /* Determine the actual type of the register. */ Register::TypeTag const type_tag = reg_base_ptr->type_tag(); /* Call the approbriate callback handler. */ - rsp = _on_access_request_handler[arduino::_107_::opencyphal::to_integer(type_tag)](req, reg_base_ptr); + if (_on_access_request_handler_map.count(type_tag) > 0) + rsp = _on_access_request_handler_map.at(type_tag)(req, reg_base_ptr); } /* Send the actual response. */ diff --git a/src/register/RegisterList.h b/src/register/RegisterList.h index 7e34f7eb..135b0044 100644 --- a/src/register/RegisterList.h +++ b/src/register/RegisterList.h @@ -12,6 +12,7 @@ * INCLUDES **************************************************************************************/ +#include #include #include "Node.h" @@ -37,7 +38,7 @@ class RegisterList std::vector _reg_list; RegisterBase const _reg_last; typedef std::function(uavcan::_register::Access_1_0::Request<> const &, RegisterBase *)> OnAccessRequestHandlerFunc; - OnAccessRequestHandlerFunc _on_access_request_handler[Register::TYPE_TAG_SIZE]; + std::map _on_access_request_handler_map; void onList_1_0_Request_Received(CanardRxTransfer const & transfer, Node & node_hdl); diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp index 5c329233..24b30f1d 100644 --- a/src/register/types/TypeTag.hpp +++ b/src/register/types/TypeTag.hpp @@ -14,8 +14,6 @@ #include -#include "../../utility/convert.hpp" - /************************************************************************************** * NAMESPACE **************************************************************************************/ @@ -44,12 +42,6 @@ enum class TypeTag : size_t Real64, }; -/************************************************************************************** - * CONSTANT - **************************************************************************************/ - -static size_t constexpr TYPE_TAG_SIZE = arduino::_107_::opencyphal::to_integer(TypeTag::Real64); - /************************************************************************************** * FUNCTION DECLARATION **************************************************************************************/ From 3d5d1b13d7ee3934dc2015c50c90f806db228fb5 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 28 Sep 2022 10:17:37 +0200 Subject: [PATCH 81/82] Revert "Specifying type of enum class TypeTag to be of underlying type size_t." This reverts commit 7352e4bf0828cd0f827054e0d79127ddd09dbaa6. --- src/register/types/TypeTag.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/register/types/TypeTag.hpp b/src/register/types/TypeTag.hpp index 24b30f1d..a442a79b 100644 --- a/src/register/types/TypeTag.hpp +++ b/src/register/types/TypeTag.hpp @@ -25,9 +25,9 @@ namespace Register * TYPEDEF **************************************************************************************/ -enum class TypeTag : size_t +enum class TypeTag { - Empty = 0, + Empty, String, Bit, Integer8, From b5b7bc462f1ac2a01d2e34ac65f20556b7c57eb7 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 28 Sep 2022 10:22:55 +0200 Subject: [PATCH 82/82] Fix: ReadWrite regs should be writable. --- src/register/RegisterDerived.ipp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/register/RegisterDerived.ipp b/src/register/RegisterDerived.ipp index 8bbc2e9e..0fcf5d35 100644 --- a/src/register/RegisterDerived.ipp +++ b/src/register/RegisterDerived.ipp @@ -25,7 +25,7 @@ RegisterDerived::RegisterDerived(char const * name, ValueLimiterFunc value_limiter_func) : RegisterBase{name, Register::toTypeTag(initial_val), - (access == Register::Access::ReadOnly), + (access == Register::Access::ReadWrite), (is_persistent == Register::Persistent::Yes)} , _val{initial_val} , _on_write_request_func{on_write_request_func}