Skip to content

Commit

Permalink
Merge pull request #229 from 107-systems/port-id-config-reg
Browse files Browse the repository at this point in the history
[example/host] Port ID shall be configurable via register.
  • Loading branch information
aentinger committed Aug 11, 2023
2 parents 09696be + 718dd4d commit 11aaeaf
Show file tree
Hide file tree
Showing 5 changed files with 243 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
27 changes: 27 additions & 0 deletions examples/host-example-01-opencyphal-basic-node/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
...
```
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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;

Expand All @@ -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++;
}

Expand Down Expand Up @@ -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;
}
87 changes: 87 additions & 0 deletions examples/host-example-01-opencyphal-basic-node/kv_host.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
/**
* This software is distributed under the terms of the MIT License.
* Copyright (c) 2023 LXRobotics.
* Author: Alexander Entinger <alexander.entinger@lxrobotics.com>
* 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) */
54 changes: 54 additions & 0 deletions examples/host-example-01-opencyphal-basic-node/kv_host.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
/**
* This software is distributed under the terms of the MIT License.
* Copyright (c) 2023 LXRobotics.
* Author: Alexander Entinger <alexander.entinger@lxrobotics.com>
* 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) */

0 comments on commit 11aaeaf

Please sign in to comment.