Merge pull request #229 from 107-systems/port-id-config-reg

[example/host] Port ID shall be configurable via register.

Author: aentinger

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)

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
+...
 ```

examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp

@@ -20,14 +20,15 @@
 
 #include <cyphal++/cyphal++.h>
 
+#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<cyphal::registry::Registry> 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<uavcan::node::Heartbeat_1_0> heartbeat_pub = node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>
-    (1*1000*1000UL /* = 1 sec in usecs. */);
+  cyphal::Publisher<uavcan::node::Heartbeat_1_0> heartbeat_pub = node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>(1*1000*1000UL /* = 1 sec in usecs. */);
 
-  cyphal::Publisher<CounterMsg> counter_pub = node_hdl.create_publisher<CounterMsg>
-    (DEFAULT_COUNTER_PORT_ID, 1*1000*1000UL /* = 1 sec in usecs. */);
+  cyphal::Publisher<CounterMsg> 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<CanardPortID>::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<int>(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<CanardPortID>::max())
+    counter_pub = node_hdl.create_publisher<CounterMsg>(counter_port_id, 1*1000*1000UL /* = 1 sec in usecs. */);
+
+  std::cout << "Node #" << static_cast<int>(node_id) << std::endl
+            << "\tCounter Port ID: " << counter_port_id << std::endl;
 
   /* NODE INFO **************************************************************************/
   const auto node_info = node_hdl.create_node_info
@@ -131,17 +160,14 @@ int main(int argc, char ** argv)
 
   for (;;)
   {
-    /* Update node configuration from register values. */
-    node_hdl.setNodeId(node_id);
-
     {
       std::lock_guard<std::mutex> lock(node_mtx);
       node_hdl.spinSome();
     }
 
     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<int>(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;
+}

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 <[email protected]>
+ * Contributors: https://github.com/107-systems/107-Arduino-Cyphal-Support/graphs/contributors.
+ */
+
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
+#include "kv_host.h"
+
+#if !defined(__GNUC__) || (__GNUC__ >= 11)
+
+#include <cstdio>
+
+#include <sstream>
+#include <fstream>
+
+/**************************************************************************************
+ * 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<std::string_view>{}(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<Error, std::size_t>
+{
+  std::ifstream in(toFilename(key), std::ifstream::in | std::ifstream::binary);
+
+  if (!in.good())
+    return Error::Existence;
+
+  in.read(static_cast<char *>(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<Error>
+{
+  std::ofstream out(toFilename(key), std::ofstream::in | std::ofstream::binary | std::ofstream::trunc);
+
+  if (!out.good())
+    return Error::Existence;
+
+  out.write(static_cast<const char *>(data), size);
+  out.close();
+
+  return std::nullopt;
+}
+
+auto KeyValueStorage::drop(const std::string_view key) -> std::optional<Error>
+{
+  if (std::remove(toFilename(key).c_str()))
+    return Error::IO;
+
+  return std::nullopt;
+}
+
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
+} /* cyphal::support::platform::storage::host */
+
+#endif /* !defined(__GNUC__) || (__GNUC__ >= 11) */

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 <[email protected]>
+ * 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<Error, std::size_t> 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<Error> 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<Error> override;
+};
+
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
+} /* cyphal::support::platform::storage::host */
+
+#endif /* !defined(__GNUC__) || (__GNUC__ >= 11) */