add Peak Systems transport

lib/cpp/examples/bandwidth_test.cc

@@ -41,22 +41,24 @@ int main(int argc, char** argv) {
   using namespace mjbots;
 
   const std::vector<std::string> 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<moteus::PeakCan>(options);
 
   // Just for some kind of "--help".
   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;
-    }
+    // auto it = std::find(args.begin(), args.end(), "--minimal-format");
+    // if (it != args.end()) {
+    //   args.erase(it);
+    //   return true;
+    // }
     return false;
   }();
 
@@ -66,9 +68,9 @@ int main(int argc, char** argv) {
 
   // Populate our list of controllers with IDs from the command line.
   std::vector<std::shared_ptr<moteus::Controller>> controllers;
-  while (!args.empty()) {
-    auto id = std::stoul(args.front());
-    args.erase(args.begin());
+  // while (!args.empty()) {
+    auto id = 1; // std::stoul(args.front());
+    // args.erase(args.begin());
     controllers.push_back(
         std::make_shared<moteus::Controller>(
             [&]() {
@@ -84,7 +86,7 @@ int main(int argc, char** argv) {
               return options;
             }()));
     responses[id] = false;
-  }
+  // }
 
   std::vector<moteus::CanFdFrame> send_frames;
   std::vector<moteus::CanFdFrame> receive_frames;
@@ -136,4 +138,4 @@ int main(int argc, char** argv) {
 
     ::usleep(10);
   }
-}
+}

lib/cpp/examples/stop.cc

@@ -0,0 +1,46 @@
+// Copyright 2023 mjbots Robotic Systems, LLC.  [email protected]
+//
+// 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 <unistd.h>
+
+#include <iostream>
+
+#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();
+}

lib/cpp/mjbots/moteus/moteus_transport.h

@@ -44,6 +44,7 @@
 
 #include "moteus_protocol.h"
 #include "moteus_tokenizer.h"
+#include "PCANBasic.h"
 
 #ifdef CANFD_FDF
 #define MJBOTS_MOTEUS_ENABLE_SOCKETCAN 1
@@ -888,6 +889,170 @@ class Socketcan : public details::TimeoutTransport {
 };
 #endif  // MJBOTS_MOTEUS_ENABLE_SOCKETCAN
 
+class PeakCan : public details::TimeoutTransport
+{
+  public:
+    struct Options : details::TimeoutTransport::Options
+    {
+      TPCANHandle ifname = PCAN_PCIBUS3;
+
+      /// <summary>
+      /// 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"
+      /// </summary>
+      // TPCANBitrateFD BitrateFD = const_cast<LPSTR>("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<LPSTR>("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<LPSTR>("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<LPSTR>("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
+      }
+    }
+
+    virtual ~PeakCan()
+    {
+      std::atomic_store(&UNPROTECTED_event_loop_, {});
+      CAN_Uninitialize(options_.ifname); // Uninitialize PCAN handle
+    }
+
+  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");
+    }
+
+    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<struct sockaddr *>(&addr),
+                    sizeof(addr)) < 0,
+              "could not bind to CAN if");
+
+      return fd;
+    }
+
+    virtual int CHILD_GetReadFd() const override
+    {
+      return -1; // Not applicable for PCAN, return an invalid descriptor
+    }
+
+    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
+      }
+    }
+
+    virtual ConsumeCount CHILD_ConsumeData(
+        std::vector<CanFdFrame> *replies,
+        int /* expected_ok_count */,
+        std::vector<int> *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<int>(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));
+      }
+
+      ConsumeCount result;
+      result.ok = 1;
+      result.rcv = 1;
+      return result;
+    }
+
+    virtual void CHILD_FlushTransmit() override {}
+
+    const Options options_;
+};
+
 /// A factory which can create transports given an optional set of
 /// commandline arguments.
 class TransportFactory {
@@ -1125,4 +1290,4 @@ class TransportRegistry {
 };
 
 }
-}
+}