Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix service callback #284

Closed
wants to merge 6 commits into from
Closed

Fix service callback #284

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
a16ef09
Adapt socketcan for libcanard v3
skullbox305 May 13, 2024
3427855
Adapt kv_host
skullbox305 May 13, 2024
2209218
Remove GCC check
filippobrizzi Jan 7, 2025
0a0e714
Merge branch '107-systems:main' into main
filippobrizzi Jan 7, 2025
6428d8a
Fix: remove leftovers from refactor
filippobrizzi Jan 7, 2025
c381cfb
properly move on request callback through chain of calls
filippobrizzi Jan 21, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 25 additions & 21 deletions examples/CAN/host-example-01-opencyphal-basic-node/kv_host.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,67 +11,72 @@

#include "kv_host.h"

#if !defined(__GNUC__) || (__GNUC__ >= 11)

#include <cstdio>

#include <sstream>
#include <fstream>
#include <sstream>

#include <sys/stat.h>

/**************************************************************************************
* NAMESPACE
**************************************************************************************/

namespace cyphal::support::platform::storage::host
{
namespace cyphal::support::platform::storage::host {

/**************************************************************************************
* PRIVATE FREE FUNCTIONS
**************************************************************************************/

[[nodiscard]] static inline std::string toFilename(std::string_view const key)
{
[[nodiscard]] static inline std::string toFilename(std::string_view const key) {
auto const key_hash = std::hash<std::string_view>{}(key);
const std::string dir_path = "ros_nodes/firmware_bridge/cyphal_registry/";

// Check if the directory exists
struct stat info;
if (stat(dir_path.c_str(), &info) != 0 ||
!(info.st_mode & S_IFDIR)) { // Path does not exist or is not a directory
// Create the directory with read/write/search permissions for owner and group, and with read/search
// permissions for others.
mkdir(dir_path.c_str(), 0755);
}

std::stringstream key_filename;
key_filename << key_hash;
key_filename << dir_path << 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>
{
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);
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>
{
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.write(static_cast<const char*>(data), size);
out.close();

return std::nullopt;
}

auto KeyValueStorage::drop(const std::string_view key) -> std::optional<Error>
{
auto KeyValueStorage::drop(const std::string_view key) -> std::optional<Error> {
if (std::remove(toFilename(key).c_str()))
return Error::IO;

Expand All @@ -82,6 +87,5 @@ auto KeyValueStorage::drop(const std::string_view key) -> std::optional<Error>
* NAMESPACE
**************************************************************************************/

} /* cyphal::support::platform::storage::host */
} // namespace cyphal::support::platform::storage::host

#endif /* !defined(__GNUC__) || (__GNUC__ >= 11) */
3 changes: 1 addition & 2 deletions examples/CAN/host-example-01-opencyphal-basic-node/kv_host.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@

#include <107-Arduino-Cyphal.h>

#if !defined(__GNUC__) || (__GNUC__ >= 11)

/**************************************************************************************
* NAMESPACE
Expand Down Expand Up @@ -51,4 +50,4 @@ class KeyValueStorage final : public interface::KeyValueStorage

} /* cyphal::support::platform::storage::host */

#endif /* !defined(__GNUC__) || (__GNUC__ >= 11) */

73 changes: 41 additions & 32 deletions examples/CAN/host-example-01-opencyphal-basic-node/socketcan.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,13 @@
#include <string.h>
#include <time.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>

#define KILO 1000L
#define MEGA (KILO * KILO)

static int16_t getNegatedErrno()
static int16_t getNegatedErrno(void)
{
const int out = -abs(errno);
if (out < 0)
Expand Down Expand Up @@ -116,12 +118,12 @@ SocketCANFD socketcanOpen(const char* const iface_name, const bool can_fd)
ok = 0 == setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &en, sizeof(en));
}

// Enable outgoing-frame loop-back.
if (ok)
{
const int en = 1;
ok = 0 == setsockopt(fd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &en, sizeof(en));
}
// // Enable outgoing-frame loop-back.
// if (ok)
// {
// const int en = 1;
// ok = 0 == setsockopt(fd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &en, sizeof(en));
// }

if (ok)
{
Expand Down Expand Up @@ -164,12 +166,13 @@ int16_t socketcanPush(const SocketCANFD fd, const CanardFrame* const frame, cons
return poll_result;
}

int16_t socketcanPop(const SocketCANFD fd,
CanardFrame* const out_frame,
const size_t payload_buffer_size,
void* const payload_buffer,
const CanardMicrosecond timeout_usec,
bool* const loopback)
int16_t socketcanPop(const SocketCANFD fd,
CanardFrame* const out_frame,
CanardMicrosecond* const out_timestamp_usec,
const size_t payload_buffer_size,
void* const payload_buffer,
const CanardMicrosecond timeout_usec,
bool* const loopback)
{
if ((out_frame == NULL) || (payload_buffer == NULL))
{
Expand All @@ -185,8 +188,8 @@ int16_t socketcanPop(const SocketCANFD fd,
struct canfd_frame sockcan_frame = {0}; // CAN FD frame storage.
struct iovec iov = {
// Scatter/gather array items struct.
.iov_base = &sockcan_frame, // Starting address.
.iov_len = sizeof(sockcan_frame) // Number of bytes to transfer.
.iov_base = &sockcan_frame, // Starting address.
.iov_len = sizeof(sockcan_frame) // Number of bytes to transfer.

};

Expand Down Expand Up @@ -223,6 +226,8 @@ int16_t socketcanPop(const SocketCANFD fd,
return -EFBIG;
}

// printf("RX = CAN ID:0x%x, Length:%d, Flags:%d\n", sockcan_frame.can_id, sockcan_frame.len, sockcan_frame.flags);

const bool valid = ((sockcan_frame.can_id & CAN_EFF_FLAG) != 0) && // Extended frame
((sockcan_frame.can_id & CAN_ERR_FLAG) == 0) && // Not RTR frame
((sockcan_frame.can_id & CAN_RTR_FLAG) == 0); // Not error frame
Expand All @@ -244,21 +249,25 @@ int16_t socketcanPop(const SocketCANFD fd,

// Obtain the CAN frame time stamp from the kernel.
// This time stamp is from the CLOCK_REALTIME kernel source.
const struct cmsghdr* cmsg = CMSG_FIRSTHDR(&msg);
struct timeval tv = {0};
assert(cmsg != NULL);
if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP)
if (NULL != out_timestamp_usec)
{
(void) memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); // Copy to avoid alignment problems
assert(tv.tv_sec >= 0 && tv.tv_usec >= 0);
const struct cmsghdr* cmsg = CMSG_FIRSTHDR(&msg);
struct timeval tv = {0};
assert(cmsg != NULL);
if ((cmsg->cmsg_level == SOL_SOCKET) && (cmsg->cmsg_type == SO_TIMESTAMP))
{
(void) memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); // Copy to avoid alignment problems
assert(tv.tv_sec >= 0 && tv.tv_usec >= 0);
}
else
{
assert(0);
return -EIO;
}

(void) memset(out_frame, 0, sizeof(CanardFrame));
*out_timestamp_usec = (CanardMicrosecond) (((uint64_t) tv.tv_sec * MEGA) + (uint64_t) tv.tv_usec);
}
else
{
assert(0);
return -EIO;
}

(void) memset(out_frame, 0, sizeof(CanardFrame));
out_frame->extended_can_id = sockcan_frame.can_id & CAN_EFF_MASK;
out_frame->payload_size = sockcan_frame.len;
out_frame->payload = payload_buffer;
Expand All @@ -267,7 +276,7 @@ int16_t socketcanPop(const SocketCANFD fd,
return poll_result;
}

int16_t socketcanFilter(const SocketCANFD fd, const size_t num_configs, const SocketCANFilterConfig* const configs)
int16_t socketcanFilter(const SocketCANFD fd, const size_t num_configs, const CanardFilter* const configs)
{
if (configs == NULL)
{
Expand All @@ -281,12 +290,12 @@ int16_t socketcanFilter(const SocketCANFD fd, const size_t num_configs, const So
struct can_filter cfs[CAN_RAW_FILTER_MAX];
for (size_t i = 0; i < num_configs; i++)
{
cfs[i].can_id = (configs[i].extended_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
cfs[i].can_mask = (configs[i].mask & CAN_EFF_MASK) | CAN_EFF_FLAG | CAN_RTR_FLAG;
cfs[i].can_id = (configs[i].extended_can_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
cfs[i].can_mask = (configs[i].extended_mask & CAN_EFF_MASK) | CAN_EFF_FLAG | CAN_RTR_FLAG;
}

const int ret =
setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, cfs, (socklen_t)(sizeof(struct can_filter) * num_configs));
setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, cfs, (socklen_t) (sizeof(struct can_filter) * num_configs));

return (ret < 0) ? getNegatedErrno() : 0;
}
Loading
Loading