From b1e2975ebe4837cbe79db9da58d201fca7c332d1 Mon Sep 17 00:00:00 2001 From: Airat Aminev Date: Sat, 8 Jun 2024 11:07:35 -0700 Subject: [PATCH] WIP attempt on adding Peak Systems transport --- lib/cpp/examples/bandwidth_test.cc | 118 +- lib/cpp/examples/stop.cc | 46 + lib/cpp/mjbots/moteus/moteus_transport.h | 2294 +++++++++++++--------- 3 files changed, 1490 insertions(+), 968 deletions(-) create mode 100644 lib/cpp/examples/stop.cc diff --git a/lib/cpp/examples/bandwidth_test.cc b/lib/cpp/examples/bandwidth_test.cc index 307bd414..d1bf573f 100644 --- a/lib/cpp/examples/bandwidth_test.cc +++ b/lib/cpp/examples/bandwidth_test.cc @@ -27,36 +27,43 @@ #include "moteus.h" -namespace { -// A simple way to get the current time accurately as a double. -static double GetNow() { - struct timespec ts = {}; - ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); - return static_cast(ts.tv_sec) + - static_cast(ts.tv_nsec) / 1e9; -} +namespace +{ + // A simple way to get the current time accurately as a double. + static double GetNow() + { + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) + + static_cast(ts.tv_nsec) / 1e9; + } } -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ using namespace mjbots; const std::vector args_in(argv, argv + argc); - auto args = moteus::Controller::ProcessTransportArgs(args_in); - auto transport = moteus::Controller::MakeSingletonTransport({}); + // auto args = moteus::Controller::ProcessTransportArgs(args_in); + // auto transport = moteus::Controller::MakeSingletonTransport({}); + moteus::PeakCan::Options options; + auto transport = std::make_shared(options); // Just for some kind of "--help". - moteus::Controller::DefaultArgProcess(argc, argv); + // moteus::Controller::DefaultArgProcess(argc, argv); - args.erase(args.begin()); // our name + // args.erase(args.begin()); // our name // Should use use int16 position/velocity command and query and // disable torque query? - const bool minimal_format = [&]() { - auto it = std::find(args.begin(), args.end(), "--minimal-format"); - if (it != args.end()) { - args.erase(it); - return true; - } + const bool minimal_format = [&]() + { + // auto it = std::find(args.begin(), args.end(), "--minimal-format"); + // if (it != args.end()) + // { + // args.erase(it); + // return true; + // } return false; }(); @@ -66,25 +73,34 @@ int main(int argc, char** argv) { // Populate our list of controllers with IDs from the command line. std::vector> controllers; - while (!args.empty()) { - auto id = std::stoul(args.front()); - args.erase(args.begin()); - controllers.push_back( - std::make_shared( - [&]() { - moteus::Controller::Options options; - options.id = id; - if (minimal_format) { - options.position_format.position = moteus::kInt16; - options.position_format.velocity = moteus::kInt16; - options.query_format.position = moteus::kInt16; - options.query_format.velocity = moteus::kInt16; - options.query_format.torque = moteus::kIgnore; - } - return options; - }())); - responses[id] = false; - } + // while (!args.empty()) + // { + auto id = 1; + // args.erase(args.begin()); + controllers.push_back( + std::make_shared( + [&]() + { + moteus::Controller::Options options; + options.transport = transport; + options.id = id; + if (minimal_format) + { + std::cout << "is minimal" << std::endl; + options.position_format.position = moteus::kInt16; + options.position_format.velocity = moteus::kInt16; + options.query_format.position = moteus::kInt16; + options.query_format.velocity = moteus::kInt16; + options.query_format.torque = moteus::kIgnore; + options.query_format.voltage = moteus::kIgnore; + options.query_format.mode = moteus::kIgnore; + options.query_format.temperature = moteus::kIgnore; + options.query_format.fault = moteus::kIgnore; + } + return options; + }())); + responses[id] = false; + // } std::vector send_frames; std::vector receive_frames; @@ -97,35 +113,45 @@ int main(int argc, char** argv) { double status_time = GetNow() + kStatusPeriodS; int hz_count = 0; - while (true) { + while (true) + { hz_count++; send_frames.clear(); receive_frames.clear(); - for (auto& c : controllers) { + for (auto &c : controllers) + { send_frames.push_back(c->MakePosition(cmd)); } - for (auto& pair : responses) { + for (auto &pair : responses) + { pair.second = false; } transport->BlockingCycle(&send_frames[0], send_frames.size(), &receive_frames); - for (const auto& f : receive_frames) { + for (const auto &f : receive_frames) + { responses[f.source] = true; } - const int count = [&]() { + const int count = [&]() + { int sum = 0; - for (const auto& pair : responses) { - if (pair.second) { sum++; } + for (const auto &pair : responses) + { + if (pair.second) + { + sum++; + } } return sum; }(); const auto now = GetNow(); - if (now > status_time) { + if (now > status_time) + { printf("%6.1fHz rx_count=%2d \r", hz_count / kStatusPeriodS, count); fflush(stdout); diff --git a/lib/cpp/examples/stop.cc b/lib/cpp/examples/stop.cc new file mode 100644 index 00000000..96ed7360 --- /dev/null +++ b/lib/cpp/examples/stop.cc @@ -0,0 +1,46 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file +/// +/// Show how to send Position mode commands at a regular interval and +/// intepret telemetry data from the servo. + +#include + +#include + +#include "moteus.h" + +int main(int argc, char **argv) +{ + using namespace mjbots; + + // The following DefaultArgProcess is an optional call. If made, + // then command line arguments will be handled which allow setting + // and configuring the default 'transport' to be used if none is + // specified in Controller::Options::transport. + moteus::Controller::DefaultArgProcess(argc, argv); + + // There are many possible options to set for each controller + // instance. Here we re-set the ID to the default (1), just to show + // how it is done. + moteus::Controller::Options options; + options.id = 1; + + moteus::Controller controller(options); + + // Command a stop to the controller. + controller.SetStop(); +} diff --git a/lib/cpp/mjbots/moteus/moteus_transport.h b/lib/cpp/mjbots/moteus/moteus_transport.h index 822c3c9d..7b2228e6 100644 --- a/lib/cpp/mjbots/moteus/moteus_transport.h +++ b/lib/cpp/mjbots/moteus/moteus_transport.h @@ -44,1085 +44,1535 @@ #include "moteus_protocol.h" #include "moteus_tokenizer.h" +#include "PCANBasic.h" #ifdef CANFD_FDF #define MJBOTS_MOTEUS_ENABLE_SOCKETCAN 1 #endif -namespace mjbots { -namespace moteus { - -using CompletionCallback = std::function; - -/// Turn async methods into synchronous ones. -class BlockingCallback { - public: - /// Pass the result of this to a singular async call. - CompletionCallback callback() { - return [&](int v) { - std::unique_lock lock(mutex_); - done_.store(true); - result_.store(v); - cv_.notify_one(); - }; - } - - /// Then call this to perform the blocking. - int Wait() { - cv_.wait(lock_, [&]() { return done_.load(); }); - return result_.load(); - } - - private: - std::atomic done_{false}; - std::atomic result_{0}; - std::recursive_mutex mutex_; - std::condition_variable_any cv_; - std::unique_lock lock_{mutex_}; -}; - -class Transport { - public: - virtual ~Transport() {} - - /// Start sending all the frames in the @p frames / @p size list. - /// - /// Any replies are collated in the @p replies result. - /// - /// Upon completion, invoke @p completed_callback from an arbitrary - /// thread that may or may not be the calling one and may or may not - /// be invoked rentrantly from within this call. - virtual void Cycle(const CanFdFrame* frames, - size_t size, - std::vector* replies, - CompletionCallback completed_callback) = 0; - - /// The same operation as above, but block until it is complete. - /// - /// A default implementation is provided which delegates to the - /// asynchronous version. - virtual void BlockingCycle(const CanFdFrame* frames, - size_t size, - std::vector* replies) { - BlockingCallback cbk; - - this->Cycle(frames, size, replies, cbk.callback()); - - cbk.Wait(); - } +namespace mjbots +{ + namespace moteus + { - /// Schedule the given callback to be invoked at a later time. This - /// will either be invoked from an arbitrary thread, or from within - /// another call to "Cycle or BlockingCycle". - virtual void Post(std::function callback) = 0; -}; - -namespace details { -/// This is just a simple RAII class for managing file descriptors. -class FileDescriptor { - public: - FileDescriptor() {} - FileDescriptor(int fd) { fd_ = fd; } - ~FileDescriptor() { - if (fd_ >= 0) { ::close(fd_); } - } + using CompletionCallback = std::function; - FileDescriptor& operator=(int fd) { - if (fd_ >= 0) { ::close(fd_); } - fd_ = fd; - return *this; - } - - bool operator==(const FileDescriptor& rhs) const { - return fd_ == rhs.fd_; - } - - operator int() const { - return fd_; - } - - int release() { - const auto result = fd_; - fd_ = -1; - return result; - } - - private: - int fd_ = -1; -}; - -/// A basic event loop implemented using C++11 primitives. -class ThreadedEventLoop { - public: - ThreadedEventLoop() { - thread_ = std::thread(std::bind(&ThreadedEventLoop::CHILD_Run, this)); - } - - ~ThreadedEventLoop() { + /// Turn async methods into synchronous ones. + class BlockingCallback { - std::unique_lock lock(mutex_); - done_ = true; - do_something_ = true; - something_cv_.notify_one(); - } - thread_.join(); - } - - // This is purely to catch out of control queues earlier, as - // typically there will just be 1 outstanding event at a time. - static constexpr int kMaxQueueSize = 2; - - void Post(std::function callback) { - std::unique_lock lock(mutex_); - event_queue_.push_back(std::move(callback)); - if (event_queue_.size() > kMaxQueueSize) { - throw std::runtime_error("There should never be more than one!"); - } - do_something_ = true; - something_cv_.notify_one(); - } - - private: - void CHILD_Run() { - std::unique_lock lock(mutex_); - - while (true) { - something_cv_.wait(lock, [&]() { - return do_something_ || !event_queue_.empty(); - }); - do_something_ = false; - - if (done_) { - return; + public: + /// Pass the result of this to a singular async call. + CompletionCallback callback() + { + return [&](int v) + { + std::unique_lock lock(mutex_); + done_.store(true); + result_.store(v); + cv_.notify_one(); + }; } - // Do at most one event. - if (!event_queue_.empty()) { - auto top = event_queue_.front(); - event_queue_.pop_front(); - top(); + /// Then call this to perform the blocking. + int Wait() + { + cv_.wait(lock_, [&]() + { return done_.load(); }); + return result_.load(); } - } - } - std::thread thread_; + private: + std::atomic done_{false}; + std::atomic result_{0}; + std::recursive_mutex mutex_; + std::condition_variable_any cv_; + std::unique_lock lock_{mutex_}; + }; - // The following variables are controlled by 'something_mutex'. - std::recursive_mutex mutex_; - std::condition_variable_any something_cv_; + class Transport + { + public: + virtual ~Transport() {} + + /// Start sending all the frames in the @p frames / @p size list. + /// + /// Any replies are collated in the @p replies result. + /// + /// Upon completion, invoke @p completed_callback from an arbitrary + /// thread that may or may not be the calling one and may or may not + /// be invoked rentrantly from within this call. + virtual void Cycle(const CanFdFrame *frames, + size_t size, + std::vector *replies, + CompletionCallback completed_callback) = 0; + + /// The same operation as above, but block until it is complete. + /// + /// A default implementation is provided which delegates to the + /// asynchronous version. + virtual void BlockingCycle(const CanFdFrame *frames, + size_t size, + std::vector *replies) + { + BlockingCallback cbk; + + this->Cycle(frames, size, replies, cbk.callback()); + + cbk.Wait(); + } - bool do_something_ = false; - bool done_ = false; + /// Schedule the given callback to be invoked at a later time. This + /// will either be invoked from an arbitrary thread, or from within + /// another call to "Cycle or BlockingCycle". + virtual void Post(std::function callback) = 0; + }; - std::deque> event_queue_; -}; + namespace details + { + /// This is just a simple RAII class for managing file descriptors. + class FileDescriptor + { + public: + FileDescriptor() {} + FileDescriptor(int fd) { fd_ = fd; } + ~FileDescriptor() + { + if (fd_ >= 0) + { + ::close(fd_); + } + } -/// A helper base class for transports that want to manage timeout -/// behavior in a similar manner. -class TimeoutTransport : public Transport { - public: - struct Options { - bool disable_brs = false; + FileDescriptor &operator=(int fd) + { + if (fd_ >= 0) + { + ::close(fd_); + } + fd_ = fd; + return *this; + } - uint32_t min_ok_wait_ns = 1000000; - uint32_t min_rcv_wait_ns = 5000000; + bool operator==(const FileDescriptor &rhs) const + { + return fd_ == rhs.fd_; + } - uint32_t rx_extra_wait_ns = 5000000; + operator int() const + { + return fd_; + } - // Send at most this many frames before waiting for responses. -1 - // means no limit. - int max_pipeline = -1; + int release() + { + const auto result = fd_; + fd_ = -1; + return result; + } - Options() {} - }; + private: + int fd_ = -1; + }; + + /// A basic event loop implemented using C++11 primitives. + class ThreadedEventLoop + { + public: + ThreadedEventLoop() + { + thread_ = std::thread(std::bind(&ThreadedEventLoop::CHILD_Run, this)); + } - TimeoutTransport(const Options& options) : t_options_(options) {} + ~ThreadedEventLoop() + { + { + std::unique_lock lock(mutex_); + done_ = true; + do_something_ = true; + something_cv_.notify_one(); + } + thread_.join(); + } - virtual void Cycle(const CanFdFrame* frames, - size_t size, - std::vector* replies, - CompletionCallback completed_callback) override { - // The event loop should never be empty here, but we make a copy - // just to assert that. - auto copy = std::atomic_load(&UNPROTECTED_event_loop_); - FailIf(!copy, "unexpected null event loop"); - copy->Post( - std::bind(&TimeoutTransport::CHILD_Cycle, - this, frames, size, replies, completed_callback)); - } + // This is purely to catch out of control queues earlier, as + // typically there will just be 1 outstanding event at a time. + static constexpr int kMaxQueueSize = 2; + + void Post(std::function callback) + { + std::unique_lock lock(mutex_); + event_queue_.push_back(std::move(callback)); + if (event_queue_.size() > kMaxQueueSize) + { + throw std::runtime_error("There should never be more than one!"); + } + do_something_ = true; + something_cv_.notify_one(); + } - virtual void Post(std::function callback) override { - // We might have an attempt to post an event while we are being - // destroyed. In that case, just ignore it. - auto copy = std::atomic_load(&UNPROTECTED_event_loop_); - if (copy) { - copy->Post(callback); - } - } + private: + void CHILD_Run() + { + std::unique_lock lock(mutex_); + + while (true) + { + something_cv_.wait(lock, [&]() + { return do_something_ || !event_queue_.empty(); }); + do_something_ = false; + + if (done_) + { + return; + } + + // Do at most one event. + if (!event_queue_.empty()) + { + auto top = event_queue_.front(); + event_queue_.pop_front(); + top(); + } + } + } - static int64_t GetNow() { - struct timespec ts = {}; - ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); - return static_cast(ts.tv_sec) * 1000000000ll + - static_cast(ts.tv_nsec); - } + std::thread thread_; + + // The following variables are controlled by 'something_mutex'. + std::recursive_mutex mutex_; + std::condition_variable_any something_cv_; + + bool do_something_ = false; + bool done_ = false; + + std::deque> event_queue_; + }; + + /// A helper base class for transports that want to manage timeout + /// behavior in a similar manner. + class TimeoutTransport : public Transport + { + public: + struct Options + { + bool disable_brs = false; + + uint32_t min_ok_wait_ns = 1000000; + uint32_t min_rcv_wait_ns = 5000000; + + uint32_t rx_extra_wait_ns = 5000000; + + // Send at most this many frames before waiting for responses. -1 + // means no limit. + int max_pipeline = -1; + + Options() {} + }; + + TimeoutTransport(const Options &options) : t_options_(options) {} + + virtual void Cycle(const CanFdFrame *frames, + size_t size, + std::vector *replies, + CompletionCallback completed_callback) override + { + // The event loop should never be empty here, but we make a copy + // just to assert that. + auto copy = std::atomic_load(&UNPROTECTED_event_loop_); + FailIf(!copy, "unexpected null event loop"); + copy->Post( + std::bind(&TimeoutTransport::CHILD_Cycle, + this, frames, size, replies, completed_callback)); + } - static void Fail(const std::string& message) { - throw std::runtime_error(message); - } + virtual void Post(std::function callback) override + { + // We might have an attempt to post an event while we are being + // destroyed. In that case, just ignore it. + auto copy = std::atomic_load(&UNPROTECTED_event_loop_); + if (copy) + { + copy->Post(callback); + } + } - static void FailIf(bool terminate, const std::string& message) { - if (terminate) { - Fail(message); - } - } + static int64_t GetNow() + { + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) * 1000000000ll + + static_cast(ts.tv_nsec); + } - static void FailIfErrno(bool terminate) { - if (terminate) { - Fail(::strerror(errno)); - } - } + static void Fail(const std::string &message) + { + throw std::runtime_error(message); + } + static void FailIf(bool terminate, const std::string &message) + { + if (terminate) + { + Fail(message); + } + } - protected: - virtual int CHILD_GetReadFd() const = 0; - virtual void CHILD_SendCanFdFrame(const CanFdFrame&) = 0; - - struct ConsumeCount { - int rcv = 0; - int ok = 0; - }; - - virtual ConsumeCount CHILD_ConsumeData( - std::vector* replies, - int expected_ok_count, - std::vector* expected_reply_count) = 0; - virtual void CHILD_FlushTransmit() = 0; - - void CHILD_Cycle(const CanFdFrame* frames, - size_t size, - std::vector* replies, - CompletionCallback completed_callback) { - if (replies) { replies->clear(); } - CHILD_CheckReplies(replies, kFlush, 0, nullptr); - - const auto advance = t_options_.max_pipeline < 0 ? - size : t_options_.max_pipeline; - - for (size_t start = 0; start < size; start += advance) { - int expected_ok_count = 0; - for (auto& v : expected_reply_count_) { v = 0; } - - for (size_t i = start; i < (start + advance) && i < size; i++) { - expected_ok_count++; - CHILD_SendCanFdFrame(frames[i]); - if (frames[i].reply_required) { - if ((frames[i].destination + 1) > - static_cast(expected_reply_count_.size())) { - expected_reply_count_.resize(frames[i].destination + 1); + static void FailIfErrno(bool terminate) + { + if (terminate) + { + Fail(::strerror(errno)); } - expected_reply_count_[frames[i].destination]++; } - } - CHILD_FlushTransmit(); + protected: + virtual int CHILD_GetReadFd() const = 0; + virtual void CHILD_SendCanFdFrame(const CanFdFrame &) = 0; + + struct ConsumeCount + { + int rcv = 0; + int ok = 0; + }; + + virtual ConsumeCount CHILD_ConsumeData( + std::vector *replies, + int expected_ok_count, + std::vector *expected_reply_count) = 0; + virtual void CHILD_FlushTransmit() = 0; + + void CHILD_Cycle(const CanFdFrame *frames, + size_t size, + std::vector *replies, + CompletionCallback completed_callback) + { + if (replies) + { + replies->clear(); + } + CHILD_CheckReplies(replies, kFlush, 0, nullptr); + + const auto advance = t_options_.max_pipeline < 0 ? size : t_options_.max_pipeline; + + for (size_t start = 0; start < size; start += advance) + { + int expected_ok_count = 0; + for (auto &v : expected_reply_count_) + { + v = 0; + } + + for (size_t i = start; i < (start + advance) && i < size; i++) + { + expected_ok_count++; + CHILD_SendCanFdFrame(frames[i]); + if (frames[i].reply_required) + { + if ((frames[i].destination + 1) > + static_cast(expected_reply_count_.size())) + { + expected_reply_count_.resize(frames[i].destination + 1); + } + expected_reply_count_[frames[i].destination]++; + } + } + + CHILD_FlushTransmit(); + + CHILD_CheckReplies(replies, + kWait, + expected_ok_count, + &expected_reply_count_); + } - CHILD_CheckReplies(replies, - kWait, - expected_ok_count, - &expected_reply_count_); - } + Post(std::bind(completed_callback, 0)); + } - Post(std::bind(completed_callback, 0)); - } + enum ReadDelay + { + kWait, + kFlush, + }; + + void CHILD_CheckReplies(std::vector *replies, + ReadDelay read_delay, + int expected_ok_count, + std::vector *expected_reply_count) + { + const auto start = GetNow(); + + const auto any_reply_checker = [&]() + { + if (!expected_reply_count) + { + return false; + } + for (auto v : *expected_reply_count) + { + if (v) + { + return true; + } + } + return false; + }; + auto end_time = + start + + (read_delay == kWait ? std::max(expected_ok_count != 0 ? t_options_.min_ok_wait_ns : 0, + any_reply_checker() ? t_options_.min_rcv_wait_ns : 0) + : 5000); + + struct pollfd fds[1] = {}; + fds[0].fd = CHILD_GetReadFd(); + fds[0].events = POLLIN; + + int ok_count = 0; + + while (true) + { + const auto now = GetNow(); + fds[0].revents = 0; + + struct timespec tmo = {}; + const auto to_sleep_ns = std::max(0, end_time - now); + tmo.tv_sec = to_sleep_ns / 1000000000; + tmo.tv_nsec = to_sleep_ns % 1000000000; + + const int poll_ret = ::ppoll(&fds[0], 1, &tmo, nullptr); + if (poll_ret < 0) + { + if (errno == EINTR) + { + // Go back and try again. + continue; + } + FailIfErrno(true); + } + if (poll_ret == 0) + { + return; + } + + const auto consume_count = CHILD_ConsumeData( + replies, expected_ok_count, expected_reply_count); + + ok_count += consume_count.ok; + + if (read_delay != kFlush && + !any_reply_checker() && ok_count >= expected_ok_count) + { + // Once we have the expected number of CAN replies and OKs, + // return immediately. + return; + } + + if (consume_count.rcv || consume_count.ok) + { + const auto finish_time = GetNow(); + end_time = finish_time + t_options_.rx_extra_wait_ns; + } + } + } - enum ReadDelay { - kWait, - kFlush, - }; - - void CHILD_CheckReplies(std::vector* replies, - ReadDelay read_delay, - int expected_ok_count, - std::vector* expected_reply_count) { - const auto start = GetNow(); - - const auto any_reply_checker = [&]() { - if (!expected_reply_count) { return false; } - for (auto v : *expected_reply_count) { - if (v) { return true; } - } - return false; - }; - auto end_time = - start + - (read_delay == kWait ? - std::max(expected_ok_count != 0 ? t_options_.min_ok_wait_ns : 0, - any_reply_checker() ? t_options_.min_rcv_wait_ns : 0) : - 5000); - - struct pollfd fds[1] = {}; - fds[0].fd = CHILD_GetReadFd(); - fds[0].events = POLLIN; - - int ok_count = 0; - - while (true) { - const auto now = GetNow(); - fds[0].revents = 0; - - struct timespec tmo = {}; - const auto to_sleep_ns = std::max(0, end_time - now); - tmo.tv_sec = to_sleep_ns / 1000000000; - tmo.tv_nsec = to_sleep_ns % 1000000000; - - const int poll_ret = ::ppoll(&fds[0], 1, &tmo, nullptr); - if (poll_ret < 0) { - if (errno == EINTR) { - // Go back and try again. - continue; - } - FailIfErrno(true); - } - if (poll_ret == 0) { return; } + // This is protected, because derived classes need to delete it + // before freeing any file descriptors. The public methods of the + // ThreadedEventLoop require no locking, but the shared_ptr itself + // requires either synchronization or access using the atomic std + // library methods. We'll exclusively use the atomic std library + // methods. + std::shared_ptr UNPROTECTED_event_loop_ = + std::make_shared(); - const auto consume_count = CHILD_ConsumeData( - replies, expected_ok_count, expected_reply_count); + private: + const Options t_options_; - ok_count += consume_count.ok; + std::vector expected_reply_count_; + }; + } - if (read_delay != kFlush && - !any_reply_checker() && ok_count >= expected_ok_count) { - // Once we have the expected number of CAN replies and OKs, - // return immediately. - return; + class Fdcanusb : public details::TimeoutTransport + { + public: + struct Options : details::TimeoutTransport::Options + { + Options() {} + }; + + // If @p device is empty, attempt to auto-detect a fdcanusb in the + // system. + Fdcanusb(const std::string &device_in, const Options &options = {}) + : details::TimeoutTransport(options), + options_(options) + { + Open(device_in); } - if (consume_count.rcv || consume_count.ok) { - const auto finish_time = GetNow(); - end_time = finish_time + t_options_.rx_extra_wait_ns; + // This constructor overload is intended for use in unit tests, + // where the file descriptors will likely be pipes. + Fdcanusb(int read_fd, int write_fd, const Options &options = {}) + : details::TimeoutTransport(options), + options_(options) + { + Open(read_fd, write_fd); } - } - } - - // This is protected, because derived classes need to delete it - // before freeing any file descriptors. The public methods of the - // ThreadedEventLoop require no locking, but the shared_ptr itself - // requires either synchronization or access using the atomic std - // library methods. We'll exclusively use the atomic std library - // methods. - std::shared_ptr UNPROTECTED_event_loop_ = - std::make_shared(); - - private: - const Options t_options_; - std::vector expected_reply_count_; -}; -} + virtual ~Fdcanusb() + { + std::atomic_store(&UNPROTECTED_event_loop_, {}); -class Fdcanusb : public details::TimeoutTransport { - public: - struct Options : details::TimeoutTransport::Options { - Options() {} - }; - - // If @p device is empty, attempt to auto-detect a fdcanusb in the - // system. - Fdcanusb(const std::string& device_in, const Options& options = {}) - : details::TimeoutTransport(options), - options_(options) { - Open(device_in); - } + if (read_fd_ == write_fd_) + { + write_fd_.release(); + } + } - // This constructor overload is intended for use in unit tests, - // where the file descriptors will likely be pipes. - Fdcanusb(int read_fd, int write_fd, const Options& options = {}) - : details::TimeoutTransport(options), - options_(options) { - Open(read_fd, write_fd); - } + static std::string DetectFdcanusb() + { + // For now, we'll only do linux like systems. + { + std::ifstream inf("/dev/fdcanusb"); + if (inf.is_open()) + { + return "/dev/fdcanusb"; + } + } - virtual ~Fdcanusb() { - std::atomic_store(&UNPROTECTED_event_loop_, {}); + { + glob_t glob_data = {}; + const int result = ::glob( + "/dev/serial/by-id/*fdcanusb*", 0, + nullptr, + &glob_data); - if (read_fd_ == write_fd_) { - write_fd_.release(); - } - } + std::string maybe_path; - static std::string DetectFdcanusb() { - // For now, we'll only do linux like systems. - { - std::ifstream inf("/dev/fdcanusb"); - if (inf.is_open()) { return "/dev/fdcanusb"; } - } + if (result == 0 && glob_data.gl_pathc > 0) + { + maybe_path = glob_data.gl_pathv[0]; + } - { - glob_t glob_data = {}; - const int result = ::glob( - "/dev/serial/by-id/*fdcanusb*", 0, - nullptr, - &glob_data); + globfree(&glob_data); - std::string maybe_path; + if (!maybe_path.empty()) + { + return maybe_path; + } + } - if (result == 0 && glob_data.gl_pathc > 0) { - maybe_path = glob_data.gl_pathv[0]; + return ""; } - globfree(&glob_data); + private: + void Open(const std::string &device_in) + { + std::string device = device_in; + if (device.empty()) + { + device = DetectFdcanusb(); + if (device.empty()) + { + throw std::runtime_error("Could not detect fdcanusb"); + } + } - if (!maybe_path.empty()) { return maybe_path; } - } + const int fd = ::open(device.c_str(), O_RDWR | O_NOCTTY); + FailIfErrno(fd == -1); - return ""; - } +#ifndef _WIN32 + { + struct serial_struct serial; + FailIfErrno(::ioctl(fd, TIOCGSERIAL, &serial) < 0); + serial.flags |= ASYNC_LOW_LATENCY; + FailIfErrno(::ioctl(fd, TIOCSSERIAL, &serial) < 0); + + struct termios toptions; + FailIfErrno(::tcgetattr(fd, &toptions) < 0); + + // Turn off things that could munge our byte stream to the + // device. + toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + toptions.c_oflag &= ~OPOST; + + FailIfErrno(::tcsetattr(fd, TCSANOW, &toptions) < 0); + FailIfErrno(::tcsetattr(fd, TCSAFLUSH, &toptions) < 0); + } +#else // _WIN32 + { + // Windows is likely broken for many other reasons, but if we do + // fix all the other problems, this will be necessary. + COMMTIMEOUTS new_timeouts = {MAXDWORD, 0, 0, 0, 0}; + SetCommTimeouts(fd, &new_timeouts); + } +#endif - private: - void Open(const std::string& device_in) { - std::string device = device_in; - if (device.empty()) { - device = DetectFdcanusb(); - if (device.empty()) { - throw std::runtime_error("Could not detect fdcanusb"); + Open(fd, fd); } - } - const int fd = ::open(device.c_str(), O_RDWR | O_NOCTTY); - FailIfErrno(fd == -1); - -#ifndef _WIN32 - { - struct serial_struct serial; - FailIfErrno(::ioctl(fd, TIOCGSERIAL, &serial) < 0); - serial.flags |= ASYNC_LOW_LATENCY; - FailIfErrno(::ioctl(fd, TIOCSSERIAL, &serial) < 0); + void Open(int read_fd, int write_fd) + { + read_fd_ = read_fd; + write_fd_ = write_fd; + } - struct termios toptions; - FailIfErrno(::tcgetattr(fd, &toptions) < 0); + virtual int CHILD_GetReadFd() const override + { + return read_fd_; + } - // Turn off things that could munge our byte stream to the - // device. - toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); - toptions.c_oflag &= ~OPOST; + virtual ConsumeCount CHILD_ConsumeData( + std::vector *replies, + int /* expected_ok_count */, + std::vector *expected_reply_count) override + { + // Read into our line buffer. + const int to_read = sizeof(line_buffer_) - line_buffer_pos_; + const int read_ret = ::read( + read_fd_, &line_buffer_[line_buffer_pos_], to_read); + if (read_ret < 0) + { + if (errno == EINTR || errno == EAGAIN) + { + return {}; + } + FailIfErrno(true); + } + line_buffer_pos_ += read_ret; + + const auto consume_count = CHILD_ConsumeLines( + replies, expected_reply_count); + if (line_buffer_pos_ >= sizeof(line_buffer_)) + { + // We overran our line buffer. For now, just drop everything + // and start from 0. + line_buffer_pos_ = 0; + } - FailIfErrno(::tcsetattr(fd, TCSANOW, &toptions) < 0); - FailIfErrno(::tcsetattr(fd, TCSAFLUSH, &toptions) < 0); - } -#else // _WIN32 - { - // Windows is likely broken for many other reasons, but if we do - // fix all the other problems, this will be necessary. - COMMTIMEOUTS new_timeouts = {MAXDWORD, 0, 0, 0, 0}; - SetCommTimeouts(fd, &new_timeouts); - } -#endif + return consume_count; + } - Open(fd, fd); - } + /// Return the number of CAN frames received. + ConsumeCount CHILD_ConsumeLines(std::vector *replies, + std::vector *expected_reply_count) + { + const auto start_size = replies ? replies->size() : 0; + ConsumeCount result; + while (CHILD_ConsumeLine(replies, &result.ok, expected_reply_count)) + { + } + result.rcv = replies ? (replies->size() - start_size) : 0; + return result; + } - void Open(int read_fd, int write_fd) { - read_fd_ = read_fd; - write_fd_ = write_fd; - } + bool CHILD_ConsumeLine(std::vector *replies, int *ok_count, + std::vector *expected_reply_count) + { + const auto line_end = [&]() -> int + { + for (size_t i = 0; i < line_buffer_pos_; i++) + { + if (line_buffer_[i] == '\r' || line_buffer_[i] == '\n') + { + return i; + } + } + return -1; + }(); + if (line_end < 0) + { + return false; + } - virtual int CHILD_GetReadFd() const override { - return read_fd_; - } + CHILD_ProcessLine(std::string(&line_buffer_[0], line_end), replies, + ok_count, expected_reply_count); - virtual ConsumeCount CHILD_ConsumeData( - std::vector* replies, - int /* expected_ok_count */, - std::vector* expected_reply_count) override { - // Read into our line buffer. - const int to_read = sizeof(line_buffer_) - line_buffer_pos_; - const int read_ret = ::read( - read_fd_, &line_buffer_[line_buffer_pos_], to_read); - if (read_ret < 0) { - if (errno == EINTR || errno == EAGAIN) { return {}; } - FailIfErrno(true); - } - line_buffer_pos_ += read_ret; - - const auto consume_count = CHILD_ConsumeLines( - replies, expected_reply_count); - if (line_buffer_pos_ >= sizeof(line_buffer_)) { - // We overran our line buffer. For now, just drop everything - // and start from 0. - line_buffer_pos_ = 0; - } + std::memmove(&line_buffer_[0], &line_buffer_[line_end + 1], + line_buffer_pos_ - line_end - 1); + line_buffer_pos_ -= (line_end + 1); - return consume_count; - } + return true; + } - /// Return the number of CAN frames received. - ConsumeCount CHILD_ConsumeLines(std::vector* replies, - std::vector* expected_reply_count) { - const auto start_size = replies ? replies->size() : 0; - ConsumeCount result; - while (CHILD_ConsumeLine(replies, &result.ok, expected_reply_count)) {} - result.rcv = replies ? (replies->size() - start_size) : 0; - return result; - } + void CHILD_ProcessLine(const std::string &line, + std::vector *replies, + int *ok_count, + std::vector *expected_reply_count) + { + if (line == "OK") + { + (*ok_count)++; + return; + } - bool CHILD_ConsumeLine(std::vector* replies, int* ok_count, - std::vector* expected_reply_count) { - const auto line_end = [&]() -> int { - for (size_t i = 0; i < line_buffer_pos_; i++) { - if (line_buffer_[i] == '\r' || line_buffer_[i] == '\n') { return i; } - } - return -1; - }(); - if (line_end < 0) { return false; } + // The only line we actually do anything with is a "rcv" line. + detail::Tokenizer tokenizer(line, " "); + const auto start = tokenizer.next(); + if (start != "rcv") + { + return; + } + const auto address = tokenizer.next(); + const auto data = tokenizer.next(); - CHILD_ProcessLine(std::string(&line_buffer_[0], line_end), replies, - ok_count, expected_reply_count); + if (address.empty() || data.empty()) + { + return; + } - std::memmove(&line_buffer_[0], &line_buffer_[line_end + 1], - line_buffer_pos_ - line_end - 1); - line_buffer_pos_ -= (line_end + 1); + CanFdFrame this_frame; + this_frame.arbitration_id = std::stoul(address, nullptr, 16); + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); - return true; - } + this_frame.size = ParseCanData(data, this_frame.data); - void CHILD_ProcessLine(const std::string& line, - std::vector* replies, - int* ok_count, - std::vector* expected_reply_count) { - if (line == "OK") { - (*ok_count)++; - return; - } + while (true) + { + const auto maybe_flags = tokenizer.next(); + if (maybe_flags.empty()) + { + break; + } + if (maybe_flags.size() != 1) + { + continue; + } + for (const char c : maybe_flags) + { + if (c == 'b') + { + this_frame.brs = CanFdFrame::kForceOff; + } + if (c == 'B') + { + this_frame.brs = CanFdFrame::kForceOn; + } + if (c == 'f') + { + this_frame.fdcan_frame = CanFdFrame::kForceOff; + } + if (c == 'F') + { + this_frame.fdcan_frame = CanFdFrame::kForceOn; + } + } + } - // The only line we actually do anything with is a "rcv" line. - detail::Tokenizer tokenizer(line, " "); - const auto start = tokenizer.next(); - if (start != "rcv") { return; } - const auto address = tokenizer.next(); - const auto data = tokenizer.next(); - - if (address.empty() || data.empty()) { return; } - - CanFdFrame this_frame; - this_frame.arbitration_id = std::stoul(address, nullptr, 16); - this_frame.destination = this_frame.arbitration_id & 0x7f; - this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; - this_frame.can_prefix = (this_frame.arbitration_id >> 16); - - this_frame.size = ParseCanData(data, this_frame.data); - - while (true) { - const auto maybe_flags = tokenizer.next(); - if (maybe_flags.empty()) { break; } - if (maybe_flags.size() != 1) { continue; } - for (const char c : maybe_flags) { - if (c == 'b') { this_frame.brs = CanFdFrame::kForceOff; } - if (c == 'B') { this_frame.brs = CanFdFrame::kForceOn; } - if (c == 'f') { this_frame.fdcan_frame = CanFdFrame::kForceOff; } - if (c == 'F') { this_frame.fdcan_frame = CanFdFrame::kForceOn; } - } - } + if (expected_reply_count) + { + if (this_frame.source < + static_cast(expected_reply_count->size())) + { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } - if (expected_reply_count) { - if (this_frame.source < - static_cast(expected_reply_count->size())) { - (*expected_reply_count)[this_frame.source] = std::max( - (*expected_reply_count)[this_frame.source] - 1, 0); + if (replies) + { + replies->emplace_back(std::move(this_frame)); + } } - } - if (replies) { - replies->emplace_back(std::move(this_frame)); - } - } + struct Printer + { + Printer(char *buf, size_t capacity) : buf_(buf), capacity_(capacity){}; + + const char *buf() { return buf_; } + size_t size() const { return pos_; } + size_t remaining() const { return capacity_ - pos_ - 1; } + + void operator()(const char *fmt, ...) + { + va_list ap; + va_start(ap, fmt); + auto n = ::vsnprintf(&buf_[pos_], remaining(), fmt, ap); + va_end(ap); + if (n < 0) + { + ::abort(); + } + pos_ += n; + }; - struct Printer { - Printer(char* buf, size_t capacity) : buf_(buf), capacity_(capacity) {}; + char *const buf_; + size_t pos_ = 0; + const size_t capacity_; + }; - const char* buf() { return buf_; } - size_t size() const { return pos_; } - size_t remaining() const { return capacity_ - pos_ - 1; } + virtual void CHILD_SendCanFdFrame(const CanFdFrame &frame) override + { + char buf[256] = {}; - void operator()(const char* fmt, ...) { - va_list ap; - va_start(ap, fmt); - auto n = ::vsnprintf(&buf_[pos_], remaining(), fmt, ap); - va_end(ap); - if (n < 0) { ::abort(); } - pos_ += n; - }; + Printer p(buf, sizeof(buf)); - char* const buf_; - size_t pos_ = 0; - const size_t capacity_; - }; + p("can send %04x ", frame.arbitration_id); - virtual void CHILD_SendCanFdFrame(const CanFdFrame& frame) override { - char buf[256] = {}; + const auto dlc = RoundUpDlc(frame.size); + for (size_t i = 0; i < frame.size; i++) + { + p("%02x", static_cast(frame.data[i])); + } + for (size_t i = frame.size; i < dlc; i++) + { + p("50"); + } - Printer p(buf, sizeof(buf)); + if (options_.disable_brs || frame.brs == CanFdFrame::kForceOff) + { + p(" b"); + } + else if (frame.brs == CanFdFrame::kForceOn) + { + p(" B"); + } + if (frame.fdcan_frame == CanFdFrame::kForceOff) + { + p(" f"); + } + else if (frame.fdcan_frame == CanFdFrame::kForceOn) + { + p(" F"); + } + p("\n"); - p("can send %04x ", frame.arbitration_id); + if (p.size() > (sizeof(tx_buffer_) - tx_buffer_size_)) + { + CHILD_FlushTransmit(); + } - const auto dlc = RoundUpDlc(frame.size); - for (size_t i = 0; i < frame.size; i++) { - p("%02x", static_cast(frame.data[i])); - } - for (size_t i = frame.size; i < dlc; i++) { - p("50"); - } + std::memcpy(&tx_buffer_[tx_buffer_size_], &buf[0], p.size()); + tx_buffer_size_ += p.size(); + } - if (options_.disable_brs || frame.brs == CanFdFrame::kForceOff) { - p(" b"); - } else if (frame.brs == CanFdFrame::kForceOn) { - p(" B"); - } - if (frame.fdcan_frame == CanFdFrame::kForceOff) { - p(" f"); - } else if (frame.fdcan_frame == CanFdFrame::kForceOn) { - p(" F"); - } - p("\n"); + virtual void CHILD_FlushTransmit() override + { + for (size_t n = 0; n < tx_buffer_size_;) + { + int ret = ::write(write_fd_, &tx_buffer_[n], tx_buffer_size_ - n); + if (ret < 0) + { + if (errno == EINTR || errno == EAGAIN) + { + continue; + } + + FailIfErrno(true); + } + else + { + n += ret; + } + } + tx_buffer_size_ = 0; + } - if (p.size() > (sizeof(tx_buffer_) - tx_buffer_size_)) { - CHILD_FlushTransmit(); - } + static size_t RoundUpDlc(size_t size) + { + if (size <= 8) + { + return size; + } + if (size <= 12) + { + return 12; + } + if (size <= 16) + { + return 16; + } + if (size <= 20) + { + return 20; + } + if (size <= 24) + { + return 24; + } + if (size <= 32) + { + return 32; + } + if (size <= 48) + { + return 48; + } + if (size <= 64) + { + return 64; + } + return size; + } - std::memcpy(&tx_buffer_[tx_buffer_size_], &buf[0], p.size()); - tx_buffer_size_ += p.size(); - } + static int ParseHexNybble(char c) + { + if (c >= '0' && c <= '9') + { + return c - '0'; + } + if (c >= 'a' && c <= 'f') + { + return c - 'a' + 10; + } + if (c >= 'A' && c <= 'F') + { + return c - 'A' + 10; + } + return -1; + } - virtual void CHILD_FlushTransmit() override { - for (size_t n = 0; n < tx_buffer_size_; ) { - int ret = ::write(write_fd_, &tx_buffer_[n], tx_buffer_size_ - n); - if (ret < 0) { - if (errno == EINTR || errno == EAGAIN) { continue; } + static int ParseHexByte(const char *value) + { + const int high = ParseHexNybble(value[0]); + if (high < 0) + { + return high; + } + const int low = ParseHexNybble(value[1]); + if (low < 0) + { + return low; + } + return (high << 4) | low; + } - FailIfErrno(true); - } else { - n += ret; + static int ParseCanData(const std::string &data, uint8_t *out) + { + size_t to_read = std::min(64 * 2, data.size()); + for (size_t i = 0; i < to_read; i += 2) + { + out[i / 2] = ParseHexByte(&data[i]); + } + return to_read / 2; } - } - tx_buffer_size_ = 0; - } - static size_t RoundUpDlc(size_t size) { - if (size <= 8) { return size; } - if (size <= 12) { return 12; } - if (size <= 16) { return 16; } - if (size <= 20) { return 20; } - if (size <= 24) { return 24; } - if (size <= 32) { return 32; } - if (size <= 48) { return 48; } - if (size <= 64) { return 64; } - return size; - } + // This is set in the parent, then used in the child. + const Options options_; - static int ParseHexNybble(char c) { - if (c >= '0' && c <= '9') { return c - '0'; } - if (c >= 'a' && c <= 'f') { return c - 'a' + 10; } - if (c >= 'A' && c <= 'F') { return c - 'A' + 10; } - return -1; - } + // We have these scoped file descriptors first in our member list, + // so they will only be closed after the threaded event loop has + // been destroyed during destruction. + details::FileDescriptor read_fd_; + details::FileDescriptor write_fd_; - static int ParseHexByte(const char* value) { - const int high = ParseHexNybble(value[0]); - if (high < 0) { return high; } - const int low = ParseHexNybble(value[1]); - if (low < 0) { return low; } - return (high << 4) | low; - } + // The following variables are only used in the child. + char line_buffer_[4096] = {}; + size_t line_buffer_pos_ = 0; - static int ParseCanData(const std::string& data, uint8_t* out) { - size_t to_read = std::min(64 * 2, data.size()); - for (size_t i = 0; i < to_read; i+= 2) { - out[i / 2] = ParseHexByte(&data[i]); - } - return to_read / 2; - } + char tx_buffer_[4096] = {}; + size_t tx_buffer_size_ = 0; + }; - // This is set in the parent, then used in the child. - const Options options_; +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN + class Socketcan : public details::TimeoutTransport + { + public: + struct Options : details::TimeoutTransport::Options + { + std::string ifname = "can0"; + + Options() {} + }; + + Socketcan(const Options &options) + : details::TimeoutTransport(options), + options_(options) + { + socket_ = Open(options_.ifname); + } - // We have these scoped file descriptors first in our member list, - // so they will only be closed after the threaded event loop has - // been destroyed during destruction. - details::FileDescriptor read_fd_; - details::FileDescriptor write_fd_; + virtual ~Socketcan() + { + std::atomic_store(&UNPROTECTED_event_loop_, {}); + } - // The following variables are only used in the child. - char line_buffer_[4096] = {}; - size_t line_buffer_pos_ = 0; + private: + static void SetNonblock(int fd) + { + int flags = ::fcntl(fd, F_GETFL, 0); + FailIf(flags < 0, "error getting flags"); + flags |= O_NONBLOCK; + FailIf(::fcntl(fd, F_SETFL, flags), "error setting flags"); + } - char tx_buffer_[4096] = {}; - size_t tx_buffer_size_ = 0; -}; + static int Open(const std::string &ifname) + { + const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); + FailIf(fd < 0, "error opening CAN socket"); + + SetNonblock(fd); + + struct ifreq ifr = {}; + std::strncpy(&ifr.ifr_name[0], ifname.c_str(), + sizeof(ifr.ifr_name) - 1); + FailIf(::ioctl(fd, SIOCGIFINDEX, &ifr) < 0, + "could not find CAN: " + ifname); + + const int enable_canfd = 1; + FailIf(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd)) != 0, + "could not set CAN-FD mode"); + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + FailIf(::bind(fd, + reinterpret_cast(&addr), + sizeof(addr)) < 0, + "could not bind to CAN if"); + + return fd; + } + virtual int CHILD_GetReadFd() const override + { + return socket_; + } -#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN -class Socketcan : public details::TimeoutTransport { - public: - struct Options : details::TimeoutTransport::Options { - std::string ifname = "can0"; - - Options() {} - }; - - Socketcan(const Options& options) - : details::TimeoutTransport(options), - options_(options) { - socket_ = Open(options_.ifname); - } + virtual void CHILD_SendCanFdFrame(const CanFdFrame &frame) override + { + struct canfd_frame send_frame = {}; + send_frame.can_id = frame.arbitration_id; + if (send_frame.can_id >= 0x7ff) + { + // Set the frame format flag if we need an extended ID. + send_frame.can_id |= (1 << 31); + } + send_frame.len = frame.size; + std::memcpy(send_frame.data, frame.data, frame.size); + + using F = CanFdFrame; + + send_frame.flags = + ((frame.fdcan_frame == F::kDefault || + frame.fdcan_frame == F::kForceOn) + ? CANFD_FDF + : 0) | + (((frame.brs == F::kDefault && !options_.disable_brs) || + frame.brs == F::kForceOn) + ? CANFD_BRS + : 0); + + FailIf(::write(socket_, &send_frame, sizeof(send_frame)) < 0, + "error writing CAN"); + } - virtual ~Socketcan() { - std::atomic_store(&UNPROTECTED_event_loop_, {}); - } + virtual ConsumeCount CHILD_ConsumeData( + std::vector *replies, + int /* expected_ok_count */, + std::vector *expected_reply_count) override + { + struct canfd_frame recv_frame = {}; + FailIf(::read(socket_, &recv_frame, sizeof(recv_frame)) < 0, + "error reading CAN frame"); + + CanFdFrame this_frame; + this_frame.arbitration_id = recv_frame.can_id & 0x1fffffff; + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); + + this_frame.brs = (recv_frame.flags & CANFD_BRS) ? CanFdFrame::kForceOn : CanFdFrame::kForceOff; + this_frame.fdcan_frame = (recv_frame.flags & CANFD_FDF) ? CanFdFrame::kForceOn : CanFdFrame::kForceOff; + + std::memcpy(this_frame.data, recv_frame.data, recv_frame.len); + this_frame.size = recv_frame.len; + + if (expected_reply_count) + { + if (this_frame.source < + static_cast(expected_reply_count->size())) + { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } - private: - static void SetNonblock(int fd) { - int flags = ::fcntl(fd, F_GETFL, 0); - FailIf(flags < 0, "error getting flags"); - flags |= O_NONBLOCK; - FailIf(::fcntl(fd, F_SETFL, flags), "error setting flags"); - } + if (replies) + { + replies->emplace_back(std::move(this_frame)); + } - static int Open(const std::string& ifname) { - const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); - FailIf(fd < 0, "error opening CAN socket"); + ConsumeCount result; + result.ok = 1; + result.rcv = 1; + return result; + } - SetNonblock(fd); + virtual void CHILD_FlushTransmit() override {} - struct ifreq ifr = {}; - std::strncpy(&ifr.ifr_name[0], ifname.c_str(), - sizeof(ifr.ifr_name) - 1); - FailIf(::ioctl(fd, SIOCGIFINDEX, &ifr) < 0, - "could not find CAN: " + ifname); + const Options options_; + details::FileDescriptor socket_; + }; +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN - const int enable_canfd = 1; - FailIf(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, - &enable_canfd, sizeof(enable_canfd)) != 0, - "could not set CAN-FD mode"); + class PeakCan : public details::TimeoutTransport + { + public: + struct Options : details::TimeoutTransport::Options + { + TPCANHandle ifname = PCAN_PCIBUS3; + + /// + /// Sets the bitrate for CAN FD devices. + /// Example - Bitrate Nom: 1Mbit/s Data: 2Mbit/s: + /// "f_clock_mhz=20, nom_brp=5, nom_tseg1=2, nom_tseg2=1, nom_sjw=1, data_brp=2, data_tseg1=3, data_tseg2=1, data_sjw=1" + /// + // TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=20, nom_brp=5, nom_tseg1=2, nom_tseg2=1, nom_sjw=1, data_brp=2, data_tseg1=3, data_tseg2=1, data_sjw=1"); + // TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=40, nom_brp=25, nom_tseg1=12, nom_tseg2=1, nom_sjw=1, data_brp=2, data_tseg1=2, data_tseg2=1, data_sjw=1"); + TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=80, nom_brp=10, nom_tseg1=12, nom_tseg2=1, nom_sjw=1, data_brp=4, data_tseg1=2, data_tseg2=1, data_sjw=1"); + // TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=80, nom_brp=10, nom_tseg1=51, nom_tseg2=28, nom_sjw=10, data_brp=1, data_tseg1=9, data_tseg2=6, data_sjw=5"); + + Options() {} + }; + + PeakCan(const Options &options) + : details::TimeoutTransport(options), + options_(options) + { + // socket_ = Open(options_.ifname); + TPCANStatus stsResult = CAN_InitializeFD(options_.ifname, options_.BitrateFD); + if (stsResult != PCAN_ERROR_OK) + { + char errorMsg[256]; + CAN_GetErrorText(stsResult, 0, errorMsg); + FailIf(true, errorMsg); // Replace with your error handling + } + } - struct sockaddr_can addr = {}; - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - FailIf(::bind(fd, - reinterpret_cast(&addr), - sizeof(addr)) < 0, - "could not bind to CAN if"); + virtual ~PeakCan() + { + std::atomic_store(&UNPROTECTED_event_loop_, {}); + CAN_Uninitialize(options_.ifname); // Uninitialize PCAN handle + } - return fd; - } + private: + static void SetNonblock(int fd) + { + int flags = ::fcntl(fd, F_GETFL, 0); + FailIf(flags < 0, "error getting flags"); + flags |= O_NONBLOCK; + FailIf(::fcntl(fd, F_SETFL, flags), "error setting flags"); + } - virtual int CHILD_GetReadFd() const override { - return socket_; - } + static int Open(const std::string &ifname) + { + const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); + FailIf(fd < 0, "error opening CAN socket"); + + SetNonblock(fd); + + struct ifreq ifr = {}; + std::strncpy(&ifr.ifr_name[0], ifname.c_str(), + sizeof(ifr.ifr_name) - 1); + FailIf(::ioctl(fd, SIOCGIFINDEX, &ifr) < 0, + "could not find CAN: " + ifname); + + const int enable_canfd = 1; + FailIf(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd)) != 0, + "could not set CAN-FD mode"); + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + FailIf(::bind(fd, + reinterpret_cast(&addr), + sizeof(addr)) < 0, + "could not bind to CAN if"); + + return fd; + } - virtual void CHILD_SendCanFdFrame(const CanFdFrame& frame) override { - struct canfd_frame send_frame = {}; - send_frame.can_id = frame.arbitration_id; - if (send_frame.can_id >= 0x7ff) { - // Set the frame format flag if we need an extended ID. - send_frame.can_id |= (1 << 31); - } - send_frame.len = frame.size; - std::memcpy(send_frame.data, frame.data, frame.size); + virtual int CHILD_GetReadFd() const override + { + return -1; // Not applicable for PCAN, return an invalid descriptor + } - using F = CanFdFrame; + virtual void CHILD_SendCanFdFrame(const CanFdFrame &frame) override + { + TPCANMsgFD msgCanMessageFD = {}; + msgCanMessageFD.ID = frame.arbitration_id; + if (msgCanMessageFD.ID >= 0x7ff) + { + // Set the frame format flag if we need an extended ID. + msgCanMessageFD.ID |= PCAN_MESSAGE_EXTENDED; + } + msgCanMessageFD.DLC = frame.size; // You might need to adjust DLC calculation + std::memcpy(msgCanMessageFD.DATA, frame.data, frame.size); + + using F = CanFdFrame; + + msgCanMessageFD.MSGTYPE = + ((frame.fdcan_frame == F::kDefault || frame.fdcan_frame == F::kForceOn) + ? PCAN_MESSAGE_FD + : 0) | + (((frame.brs == F::kDefault && !options_.disable_brs) || frame.brs == F::kForceOn) + ? PCAN_MESSAGE_BRS + : 0); + + TPCANStatus status = CAN_WriteFD(options_.ifname, &msgCanMessageFD); + if (status != PCAN_ERROR_OK) + { + // Handle the error + char errorMsg[256]; + CAN_GetErrorText(status, 0, errorMsg); + FailIf(true, errorMsg); // Replace with your error handling + } + } - send_frame.flags = - ((frame.fdcan_frame == F::kDefault || - frame.fdcan_frame == F::kForceOn) ? CANFD_FDF : 0) | - (((frame.brs == F::kDefault && !options_.disable_brs) || - frame.brs == F::kForceOn) ? CANFD_BRS : 0); + virtual ConsumeCount CHILD_ConsumeData( + std::vector *replies, + int /* expected_ok_count */, + std::vector *expected_reply_count) override + { + // Reading data from PCAN + TPCANMsgFD recv_frame = {}; + TPCANTimestampFD CANTimeStamp; + TPCANStatus status = CAN_ReadFD(options_.ifname, &recv_frame, &CANTimeStamp); + FailIf(status != PCAN_ERROR_OK, "error reading CAN frame"); + + CanFdFrame this_frame; + this_frame.arbitration_id = recv_frame.ID & 0x1fffffff; + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); + + this_frame.brs = (recv_frame.MSGTYPE & PCAN_MESSAGE_BRS) ? CanFdFrame::kForceOn : CanFdFrame::kForceOff; + this_frame.fdcan_frame = (recv_frame.MSGTYPE & PCAN_MESSAGE_FD) ? CanFdFrame::kForceOn : CanFdFrame::kForceOff; + + std::memcpy(this_frame.data, recv_frame.DATA, recv_frame.DLC); + this_frame.size = recv_frame.DLC; + + if (expected_reply_count) + { + if (this_frame.source < + static_cast(expected_reply_count->size())) + { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } - FailIf(::write(socket_, &send_frame, sizeof(send_frame)) < 0, - "error writing CAN"); - } + if (replies) + { + replies->emplace_back(std::move(this_frame)); + } - virtual ConsumeCount CHILD_ConsumeData( - std::vector* replies, - int /* expected_ok_count */, - std::vector* expected_reply_count) override { - struct canfd_frame recv_frame = {}; - FailIf(::read(socket_, &recv_frame, sizeof(recv_frame)) < 0, - "error reading CAN frame"); - - CanFdFrame this_frame; - this_frame.arbitration_id = recv_frame.can_id & 0x1fffffff; - this_frame.destination = this_frame.arbitration_id & 0x7f; - this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; - this_frame.can_prefix = (this_frame.arbitration_id >> 16); - - this_frame.brs = (recv_frame.flags & CANFD_BRS) ? - CanFdFrame::kForceOn : CanFdFrame::kForceOff; - this_frame.fdcan_frame = (recv_frame.flags & CANFD_FDF) ? - CanFdFrame::kForceOn : CanFdFrame::kForceOff; - - std::memcpy(this_frame.data, recv_frame.data, recv_frame.len); - this_frame.size = recv_frame.len; - - if (expected_reply_count) { - if (this_frame.source < - static_cast(expected_reply_count->size())) { - (*expected_reply_count)[this_frame.source] = std::max( - (*expected_reply_count)[this_frame.source] - 1, 0); + ConsumeCount result; + result.ok = 1; + result.rcv = 1; + return result; } - } - if (replies) { - replies->emplace_back(std::move(this_frame)); - } + virtual void CHILD_FlushTransmit() override {} - ConsumeCount result; - result.ok = 1; - result.rcv = 1; - return result; - } + const Options options_; + }; - virtual void CHILD_FlushTransmit() override {} - - const Options options_; - details::FileDescriptor socket_; -}; -#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN - -/// A factory which can create transports given an optional set of -/// commandline arguments. -class TransportFactory { - public: - virtual ~TransportFactory() {} - - virtual int priority() = 0; - virtual std::string name() = 0; - using TransportArgPair = std::pair, - std::vector>; - virtual TransportArgPair make(const std::vector&) = 0; - - struct Argument { - std::string name; - int nargs = 1; - std::string help; - - bool operator<(const Argument& rhs) const { - if (name < rhs.name) { return true; } - if (name > rhs.name) { return false; } - return help < rhs.help; - } + /// A factory which can create transports given an optional set of + /// commandline arguments. + class TransportFactory + { + public: + virtual ~TransportFactory() {} + + virtual int priority() = 0; + virtual std::string name() = 0; + using TransportArgPair = std::pair, + std::vector>; + virtual TransportArgPair make(const std::vector &) = 0; + + struct Argument + { + std::string name; + int nargs = 1; + std::string help; + + bool operator<(const Argument &rhs) const + { + if (name < rhs.name) + { + return true; + } + if (name > rhs.name) + { + return false; + } + return help < rhs.help; + } - Argument(const std::string& name_in, - int nargs_in, - const std::string& help_in) - : name(name_in), - nargs(nargs_in), - help(help_in) {} - }; + Argument(const std::string &name_in, + int nargs_in, + const std::string &help_in) + : name(name_in), + nargs(nargs_in), + help(help_in) {} + }; - virtual std::vector cmdline_arguments() = 0; + virtual std::vector cmdline_arguments() = 0; - virtual bool is_args_set(const std::vector&) = 0; -}; + virtual bool is_args_set(const std::vector &) = 0; + }; -class FdcanusbFactory : public TransportFactory { - public: - virtual ~FdcanusbFactory() {} + class FdcanusbFactory : public TransportFactory + { + public: + virtual ~FdcanusbFactory() {} - virtual int priority() override { return 10; } - virtual std::string name() override { return "fdcanusb"; } + virtual int priority() override { return 10; } + virtual std::string name() override { return "fdcanusb"; } - virtual TransportArgPair make(const std::vector& args_in) override { - auto args = args_in; + virtual TransportArgPair make(const std::vector &args_in) override + { + auto args = args_in; - Fdcanusb::Options options; - std::string device; + Fdcanusb::Options options; + std::string device; - { - auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); - if (it != args.end()) { - options.disable_brs = true; - args.erase(it); - } - } + { + auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); + if (it != args.end()) + { + options.disable_brs = true; + args.erase(it); + } + } - { - auto it = std::find(args.begin(), args.end(), "--fdcanusb"); - if (it != args.end()) { - if ((it + 1) != args.end()) { - device = *(it + 1); - args.erase(it, it + 2); - } else { - throw std::runtime_error("--fdcanusb requires a path"); + { + auto it = std::find(args.begin(), args.end(), "--fdcanusb"); + if (it != args.end()) + { + if ((it + 1) != args.end()) + { + device = *(it + 1); + args.erase(it, it + 2); + } + else + { + throw std::runtime_error("--fdcanusb requires a path"); + } + } } + + auto result = std::make_shared(device, options); + return TransportArgPair(result, args); } - } - auto result = std::make_shared(device, options); - return TransportArgPair(result, args); - } + virtual std::vector cmdline_arguments() override + { + return { + {"--fdcanusb", 1, "path to fdcanusb device"}, + {"--can-disable-brs", 0, "do not set BRS"}, + }; + } - virtual std::vector cmdline_arguments() override { - return { - { "--fdcanusb", 1, "path to fdcanusb device" }, - { "--can-disable-brs", 0, "do not set BRS" }, + virtual bool is_args_set(const std::vector &args) override + { + for (const auto &arg : args) + { + if (arg == "--fdcanusb") + { + return true; + } + } + return false; + } }; - } - - virtual bool is_args_set(const std::vector& args) override { - for (const auto& arg : args) { - if (arg == "--fdcanusb") { return true; } - } - return false; - } -}; #ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN -class SocketcanFactory : public TransportFactory { - public: - virtual ~SocketcanFactory() {} + class SocketcanFactory : public TransportFactory + { + public: + virtual ~SocketcanFactory() {} - virtual int priority() override { return 11; } - virtual std::string name() override { return "socketcan"; } + virtual int priority() override { return 11; } + virtual std::string name() override { return "socketcan"; } - virtual TransportArgPair make(const std::vector& args_in) override { - auto args = args_in; + virtual TransportArgPair make(const std::vector &args_in) override + { + auto args = args_in; - Socketcan::Options options; - std::string device; + Socketcan::Options options; + std::string device; - { - auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); - if (it != args.end()) { - options.disable_brs = true; - args.erase(it); - } - } + { + auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); + if (it != args.end()) + { + options.disable_brs = true; + args.erase(it); + } + } - { - auto it = std::find(args.begin(), args.end(), "--socketcan-iface"); - if (it != args.end()) { - if ((it + 1) != args.end()) { - options.ifname = *(it + 1); - args.erase(it, it + 2); - } else { - throw std::runtime_error("--socketcan-iface requires an interface name"); + { + auto it = std::find(args.begin(), args.end(), "--socketcan-iface"); + if (it != args.end()) + { + if ((it + 1) != args.end()) + { + options.ifname = *(it + 1); + args.erase(it, it + 2); + } + else + { + throw std::runtime_error("--socketcan-iface requires an interface name"); + } + } } + + auto result = std::make_shared(options); + return TransportArgPair(result, args); } - } - auto result = std::make_shared(options); - return TransportArgPair(result, args); - } + virtual std::vector cmdline_arguments() override + { + return { + {"--socketcan-iface", 1, "socketcan iface name"}, + {"--can-disable-brs", 0, "do not set BRS"}, + }; + } - virtual std::vector cmdline_arguments() override { - return { - { "--socketcan-iface", 1, "socketcan iface name" }, - { "--can-disable-brs", 0, "do not set BRS" }, + virtual bool is_args_set(const std::vector &args) override + { + for (const auto &arg : args) + { + if (arg == "--socketcan-iface") + { + return true; + } + } + return false; + } }; - } +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN - virtual bool is_args_set(const std::vector& args) override { - for (const auto& arg : args) { - if (arg == "--socketcan-iface") { return true; } - } - return false; - } -}; -#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN - -class TransportRegistry { - public: - template - void Register() { - items_.push_back(std::make_shared()); - } - - static TransportRegistry& singleton() { - static TransportRegistry reg; - return reg; - } - - std::vector cmdline_arguments() const { - std::vector result; - std::set uniqifier; - - result.push_back(TransportFactory::Argument( - "--force-transport", 1, - "force the given transport type to be used")); - uniqifier.insert(result.back()); + class TransportRegistry + { + public: + template + void Register() + { + items_.push_back(std::make_shared()); + } - for (const auto& item : items_) { - const auto item_args = item->cmdline_arguments(); - for (const auto& arg : item_args) { - if (uniqifier.count(arg) == 0) { - result.push_back(arg); - uniqifier.insert(arg); - } + static TransportRegistry &singleton() + { + static TransportRegistry reg; + return reg; } - } - return result; - } + std::vector cmdline_arguments() const + { + std::vector result; + std::set uniqifier; + + result.push_back(TransportFactory::Argument( + "--force-transport", 1, + "force the given transport type to be used")); + uniqifier.insert(result.back()); + + for (const auto &item : items_) + { + const auto item_args = item->cmdline_arguments(); + for (const auto &arg : item_args) + { + if (uniqifier.count(arg) == 0) + { + result.push_back(arg); + uniqifier.insert(arg); + } + } + } - TransportFactory::TransportArgPair make(const std::vector& args_in) const { - auto args = args_in; - auto to_try = items_; - - std::sort(to_try.begin(), to_try.end(), - [](const std::shared_ptr& lhs, - const std::shared_ptr& rhs) { - return lhs->priority() < rhs->priority(); - }); - - // Is the transport forced? - const auto it = std::find(args.begin(), args.end(), "--force-transport"); - if (it != args.end()) { - if ((it + 1) != args.end()) { - to_try = {}; - const auto name_to_find = *(it + 1); - for (auto item : items_) { - if (item->name() == name_to_find) { to_try.push_back(item); } - } - args.erase(it, it + 2); - } else { - throw std::runtime_error("--force-transport requires an argument"); + return result; } - } else { - std::vector> options_set; - for (auto item : items_) { - if (item->is_args_set(args)) { - options_set.push_back(item); + + TransportFactory::TransportArgPair make(const std::vector &args_in) const + { + auto args = args_in; + auto to_try = items_; + + std::sort(to_try.begin(), to_try.end(), + [](const std::shared_ptr &lhs, + const std::shared_ptr &rhs) + { + return lhs->priority() < rhs->priority(); + }); + + // Is the transport forced? + const auto it = std::find(args.begin(), args.end(), "--force-transport"); + if (it != args.end()) + { + if ((it + 1) != args.end()) + { + to_try = {}; + const auto name_to_find = *(it + 1); + for (auto item : items_) + { + if (item->name() == name_to_find) + { + to_try.push_back(item); + } + } + args.erase(it, it + 2); + } + else + { + throw std::runtime_error("--force-transport requires an argument"); + } } - } + else + { + std::vector> options_set; + for (auto item : items_) + { + if (item->is_args_set(args)) + { + options_set.push_back(item); + } + } - if (!options_set.empty()) { to_try = options_set; } - } + if (!options_set.empty()) + { + to_try = options_set; + } + } - std::string errors; - for (auto factory : to_try) { - try { - auto maybe_result = factory->make(args); - if (maybe_result.first) { - return maybe_result; + std::string errors; + for (auto factory : to_try) + { + try + { + auto maybe_result = factory->make(args); + if (maybe_result.first) + { + return maybe_result; + } + } + catch (std::runtime_error &re) + { + if (!errors.empty()) + { + errors += ", "; + } + errors += factory->name() + ": " + re.what(); + } } - } catch (std::runtime_error& re) { - if (!errors.empty()) { errors += ", "; } - errors += factory->name() + ": " + re.what(); + throw std::runtime_error("Unable to find a default transport: " + errors); } - } - throw std::runtime_error("Unable to find a default transport: " + errors); - } - private: - TransportRegistry() { - Register(); + private: + TransportRegistry() + { + Register(); #ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN - Register(); + Register(); #endif - } + } - std::vector> items_; -}; + std::vector> items_; + }; -} + } }