diff --git a/examples/host-example-01-opencyphal-basic-node/CMakeLists.txt b/examples/host-example-01-opencyphal-basic-node/CMakeLists.txt index 0a19990d..df803092 100644 --- a/examples/host-example-01-opencyphal-basic-node/CMakeLists.txt +++ b/examples/host-example-01-opencyphal-basic-node/CMakeLists.txt @@ -6,6 +6,7 @@ set(EXAMPLE_01_TARGET host-example-01-basic-cyphal-node) add_executable(${EXAMPLE_01_TARGET} host-example-01-opencyphal-basic-node.cpp socketcan.c + kv_host.cpp ) ########################################################################## target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) diff --git a/examples/host-example-01-opencyphal-basic-node/README.md b/examples/host-example-01-opencyphal-basic-node/README.md index b99a6059..7e16eaf9 100644 --- a/examples/host-example-01-opencyphal-basic-node/README.md +++ b/examples/host-example-01-opencyphal-basic-node/README.md @@ -43,4 +43,31 @@ $ y r 42 cyphal.node.id $ y r 42 cyphal.node.id 12 12 + +$ y r 42 cyphal.pub.counter.id +65535 + +$ y r 42 cyphal.pub.counter.id 1001 +1001 +``` + +Store changed configuration and restart node: +```bash +$ y cmd 42 store +$ y cmd 42 restart +``` +**Note**: `restart` is implemented in this example as `exit(0)`, hence you need to restart the application yourself. + +Subscribe to counter subject: +```bash +y sub 1001:uavcan.primitive.scalar.Integer8.1.0 --with-metadata +--- +1001: + _meta_: {ts_system: 1691741133.026175, ts_monotonic: 25484.463219, source_node_id: 12, transfer_id: 1, priority: nominal, dtype: uavcan.primitive.scalar.Integer8.1.0} + value: 1 +--- +1001: + _meta_: {ts_system: 1691741138.027977, ts_monotonic: 25489.465034, source_node_id: 12, transfer_id: 2, priority: nominal, dtype: uavcan.primitive.scalar.Integer8.1.0} + value: 2 +... ``` diff --git a/examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp b/examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp index c70540cd..48b1a70c 100644 --- a/examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp +++ b/examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp @@ -20,14 +20,15 @@ #include +#include "kv_host.h" #include "socketcan.h" /************************************************************************************** * CONSTANT **************************************************************************************/ -static CanardPortID const DEFAULT_COUNTER_PORT_ID = 1001U; -static uint16_t const DEFAULT_COUNTER_UPDATE_PERIOD_ms = 5*1000UL; +static uint16_t const COUNTER_UPDATE_PERIOD_ms = 5*1000UL; +static uint16_t const HEARTBEAT_UPDATE_PERIOD_ms = 1000UL; /************************************************************************************** * TYPEDEF @@ -41,6 +42,14 @@ typedef uavcan::primitive::scalar::Integer8_1_0 CounterMsg; extern "C" CanardMicrosecond micros(); extern "C" unsigned long millis(); +uavcan::node::ExecuteCommand::Response_1_1 onExecuteCommand_1_1_Request_Received(uavcan::node::ExecuteCommand::Request_1_1 const &); + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +static cyphal::support::platform::storage::host::KeyValueStorage kv_storage; +static std::shared_ptr node_registry; /************************************************************************************** * MAIN @@ -58,25 +67,45 @@ int main(int argc, char ** argv) cyphal::Node node_hdl(node_heap.data(), node_heap.size(), micros, [socket_can_fd] (CanardFrame const & frame) { return (socketcanPush(socket_can_fd, &frame, 1000*1000UL) > 0); }); std::mutex node_mtx; - cyphal::Publisher heartbeat_pub = node_hdl.create_publisher - (1*1000*1000UL /* = 1 sec in usecs. */); + cyphal::Publisher heartbeat_pub = node_hdl.create_publisher(1*1000*1000UL /* = 1 sec in usecs. */); - cyphal::Publisher counter_pub = node_hdl.create_publisher - (DEFAULT_COUNTER_PORT_ID, 1*1000*1000UL /* = 1 sec in usecs. */); + cyphal::Publisher counter_pub; /* REGISTER ***************************************************************************/ CanardNodeID node_id = node_hdl.getNodeId(); - CanardPortID counter_port_id = DEFAULT_COUNTER_PORT_ID; - uint16_t counter_update_period_ms = DEFAULT_COUNTER_UPDATE_PERIOD_ms; + CanardPortID counter_port_id = std::numeric_limits::max(); - const auto node_registry = node_hdl.create_registry(); + node_registry = node_hdl.create_registry(); const auto reg_ro_node_description = node_registry->route ("cyphal.node.description", {true}, []() { return "basic-cyphal-node"; }); const auto reg_ro_pub_counter_type = node_registry->route ("cyphal.pub.counter.type", {true}, []() { return "uavcan.primitive.scalar.Integer8.1.0"; }); - const auto reg_rw_node_id = node_registry->expose("cyphal.node.id", {}, node_id); - const auto reg_rw_pub_counter_id = node_registry->expose("cyphal.pub.counter.id", {}, counter_port_id); - const auto reg_rw_pub_counter_update_period_ms = node_registry->expose("cyphal.pub.counter.update_period_ms", {}, counter_update_period_ms); + const auto reg_rw_node_id = node_registry->expose("cyphal.node.id", {true}, node_id); + const auto reg_rw_pub_counter_id = node_registry->expose("cyphal.pub.counter.id", {true}, counter_port_id); + + /* Configure service server for storing persistent + * states upon command request. + */ + auto const rc_load = cyphal::support::load(kv_storage, *node_registry); + if (rc_load.has_value()) { + std::cerr << "cyphal::support::load failed with " << static_cast(rc_load.value()) << std::endl; + return EXIT_FAILURE; + } + + cyphal::ServiceServer execute_command_srv = node_hdl.create_service_server< + uavcan::node::ExecuteCommand::Request_1_1, + uavcan::node::ExecuteCommand::Response_1_1>( + 2*1000*1000UL, + onExecuteCommand_1_1_Request_Received); + + /* Update node configuration from register value. + */ + node_hdl.setNodeId(node_id); + if (counter_port_id != std::numeric_limits::max()) + counter_pub = node_hdl.create_publisher(counter_port_id, 1*1000*1000UL /* = 1 sec in usecs. */); + + std::cout << "Node #" << static_cast(node_id) << std::endl + << "\tCounter Port ID: " << counter_port_id << std::endl; /* NODE INFO **************************************************************************/ const auto node_info = node_hdl.create_node_info @@ -131,9 +160,6 @@ int main(int argc, char ** argv) for (;;) { - /* Update node configuration from register values. */ - node_hdl.setNodeId(node_id); - { std::lock_guard lock(node_mtx); node_hdl.spinSome(); @@ -141,7 +167,7 @@ int main(int argc, char ** argv) auto const now = millis(); - if ((now - prev_heartbeat) > 1000UL) + if ((now - prev_heartbeat) > HEARTBEAT_UPDATE_PERIOD_ms) { prev_heartbeat = now; @@ -155,10 +181,13 @@ int main(int argc, char ** argv) heartbeat_pub->publish(msg); } - if ((now - prev_counter) > counter_update_period_ms) + if ((now - prev_counter) > COUNTER_UPDATE_PERIOD_ms) { prev_counter = now; - counter_pub->publish(counter_msg); + + if (counter_pub) + counter_pub->publish(counter_msg); + counter_msg.value++; } @@ -193,3 +222,30 @@ unsigned long millis() { return micros() / 1000; } + +uavcan::node::ExecuteCommand::Response_1_1 onExecuteCommand_1_1_Request_Received(uavcan::node::ExecuteCommand::Request_1_1 const & req) +{ + uavcan::node::ExecuteCommand::Response_1_1 rsp; + + if (req.command == uavcan::node::ExecuteCommand::Request_1_1::COMMAND_RESTART) + { + rsp.status = uavcan::node::ExecuteCommand::Response_1_1::STATUS_SUCCESS; + exit(0); + } + else if (req.command == uavcan::node::ExecuteCommand::Request_1_1::COMMAND_STORE_PERSISTENT_STATES) + { + auto const rc_save = cyphal::support::save(kv_storage, *node_registry, []() { /* watchdog function */ }); + if (rc_save.has_value()) + { + std::cerr << "cyphal::support::save failed with " << static_cast(rc_save.value()) << std::endl; + rsp.status = uavcan::node::ExecuteCommand::Response_1_1::STATUS_FAILURE; + return rsp; + } + rsp.status = uavcan::node::ExecuteCommand::Response_1_1::STATUS_SUCCESS; + } + else { + rsp.status = uavcan::node::ExecuteCommand::Response_1_1::STATUS_BAD_COMMAND; + } + + return rsp; +} diff --git a/examples/host-example-01-opencyphal-basic-node/kv_host.cpp b/examples/host-example-01-opencyphal-basic-node/kv_host.cpp new file mode 100644 index 00000000..d530667d --- /dev/null +++ b/examples/host-example-01-opencyphal-basic-node/kv_host.cpp @@ -0,0 +1,87 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2023 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal-Support/graphs/contributors. + */ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include "kv_host.h" + +#if !defined(__GNUC__) || (__GNUC__ >= 11) + +#include + +#include +#include + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +namespace cyphal::support::platform::storage::host +{ + +/************************************************************************************** + * PRIVATE FREE FUNCTIONS + **************************************************************************************/ + +[[nodiscard]] static inline std::string toFilename(std::string_view const key) +{ + auto const key_hash = std::hash{}(key); + std::stringstream key_filename; + key_filename << key_hash; + return key_filename.str(); +} + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +auto KeyValueStorage::get(const std::string_view key, const std::size_t size, void* const data) const + -> std::variant +{ + std::ifstream in(toFilename(key), std::ifstream::in | std::ifstream::binary); + + if (!in.good()) + return Error::Existence; + + in.read(static_cast(data), size); + size_t const bytes_read = in ? size : in.gcount(); + in.close(); + + return bytes_read; +} + +auto KeyValueStorage::put(const std::string_view key, const std::size_t size, const void* const data) + -> std::optional +{ + std::ofstream out(toFilename(key), std::ofstream::in | std::ofstream::binary | std::ofstream::trunc); + + if (!out.good()) + return Error::Existence; + + out.write(static_cast(data), size); + out.close(); + + return std::nullopt; +} + +auto KeyValueStorage::drop(const std::string_view key) -> std::optional +{ + if (std::remove(toFilename(key).c_str())) + return Error::IO; + + return std::nullopt; +} + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +} /* cyphal::support::platform::storage::host */ + +#endif /* !defined(__GNUC__) || (__GNUC__ >= 11) */ diff --git a/examples/host-example-01-opencyphal-basic-node/kv_host.h b/examples/host-example-01-opencyphal-basic-node/kv_host.h new file mode 100644 index 00000000..fcd6ed42 --- /dev/null +++ b/examples/host-example-01-opencyphal-basic-node/kv_host.h @@ -0,0 +1,54 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2023 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal-Support/graphs/contributors. + */ + +#pragma once + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include <107-Arduino-Cyphal.h> + +#if !defined(__GNUC__) || (__GNUC__ >= 11) + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +namespace cyphal::support::platform::storage::host +{ + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class KeyValueStorage final : public interface::KeyValueStorage +{ +public: + KeyValueStorage() { } + virtual ~KeyValueStorage() { } + + /// The return value is the number of bytes read into the buffer or the error. + [[nodiscard]] virtual auto get(const std::string_view key, const std::size_t size, void* const data) const + -> std::variant override; + + /// Existing data, if any, is replaced entirely. New file and its parent directories created implicitly. + /// Either all or none of the data bytes are written. + [[nodiscard]] virtual auto put(const std::string_view key, const std::size_t size, const void* const data) + -> std::optional override; + + /// Remove key. If the key does not exist, the existence error is returned. + [[nodiscard]] virtual auto drop(const std::string_view key) -> std::optional override; +}; + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +} /* cyphal::support::platform::storage::host */ + +#endif /* !defined(__GNUC__) || (__GNUC__ >= 11) */