Branch data     Line data    Source code

       1                 :             : #include <linux/can.h>
       2                 :             : #include <net/if.h>
       3                 :             : #include <sys/ioctl.h>
       4                 :             : #include <sys/socket.h>
       5                 :             : #include <unistd.h>
       6                 :             : 
       7                 :             : #include <cerrno>
       8                 :             : #include <csignal>
       9                 :             : #include <cstdio>
      10                 :             : #include <cstring>
      11                 :             : #include <iostream>
      12                 :             : #include <memory>
      13                 :             : #include <string>
      14                 :             : #include <zmq.hpp>
      15                 :             : 
      16                 :             : #include "can/CanDriver.hpp"
      17                 :             : #include "mq/src/ZeroMQSocket.hpp"
      18                 :             : 
      19                 :             : namespace {
      20                 :             : // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
      21                 :             : volatile std::sig_atomic_t interrupted{0};
      22                 :             : }  // namespace
      23                 :             : 
      24                 :           0 : void signalHandler(int signum) {
      25                 :             :     (void)signum;  // ignore unused variable
      26                 :           0 :     interrupted = 1;
      27                 :           0 : }
      28                 :             : 
      29                 :           0 : void catchSignals() {
      30                 :           0 :     (void)std::signal(SIGINT, signalHandler);
      31                 :           0 :     (void)std::signal(SIGTERM, signalHandler);
      32                 :           0 :     (void)std::signal(SIGSEGV, signalHandler);
      33                 :           0 :     (void)std::signal(SIGABRT, signalHandler);
      34                 :           0 : }
      35                 :             : 
      36                 :           0 : auto main(int argc, char **argv) -> int {
      37                 :           0 :     int opt{};
      38                 :           0 :     std::string interface = "can0";
      39                 :             : 
      40                 :             :     // NOLINTNEXTLINE(concurrency-mt-unsafe)
      41                 :           0 :     while ((opt = getopt(argc, argv, "i:")) != -1) {  // "i:" means -i takes an argument
      42                 :           0 :         switch (opt) {
      43                 :           0 :             case 'i':
      44                 :           0 :                 interface = optarg;
      45                 :           0 :                 break;
      46                 :           0 :             case '?':  // Unknown option
      47                 :           0 :                 std::cerr << "Unknown option: -" << static_cast<char>(optopt) << "\n";
      48                 :           0 :                 return 1;
      49                 :           0 :             default:
      50                 :           0 :                 return 1;
      51                 :             :         }
      52                 :             :     }
      53                 :             : 
      54                 :           0 :     std::unique_ptr<candriver::CanDriver> can;
      55                 :             :     try {
      56                 :           0 :         can = std::make_unique<candriver::CanDriver>(interface, 5);
      57                 :           0 :     } catch (const std::exception &exception) {
      58                 :           0 :         std::cout << exception.what() << "\n";
      59                 :           0 :         return 1;
      60                 :           0 :     }
      61                 :             : 
      62                 :           0 :     struct can_frame frame {};  // Classical can frame
      63                 :           0 :     int32_t num_bytes{};
      64                 :             : 
      65                 :             :     // Connect to zmq
      66                 :           0 :     zmq::context_t context(1);
      67                 :           0 :     MQ::ZeroMQSocket publisher{context, zmq::socket_type::pub};
      68                 :           0 :     if (!publisher.connect("ipc:///tmp/speed.ipc")) {
      69                 :           0 :         return 1;
      70                 :             :     }
      71                 :             : 
      72                 :           0 :     catchSignals();
      73                 :             :     while (true) {
      74                 :           0 :         num_bytes = can->receive(&frame);
      75                 :             : 
      76                 :           0 :         if (num_bytes < 0) {
      77                 :           0 :             if (errno == EINTR) {
      78                 :           0 :                 std::cout << "interrupt received, exiting gracefully..." << "\n";
      79                 :           0 :                 break;
      80                 :             :             }
      81                 :           0 :             if (errno != EAGAIN) {
      82                 :           0 :                 perror("Error reading");
      83                 :             :             }
      84                 :           0 :             continue;
      85                 :             :         }
      86                 :             : 
      87                 :             :         // The len element contains the payload length in bytes and should be used instead of
      88                 :             :         // can_dlc
      89                 :           0 :         std::vector<uint8_t> message(frame.len + 1);
      90                 :           0 :         message[0] = static_cast<uint8_t>(frame.can_id);
      91                 :           0 :         memcpy(message.data() + 1, frame.data, static_cast<size_t>(num_bytes));
      92                 :             : 
      93                 :             :         try {
      94                 :           0 :             publisher.send(message);
      95                 :           0 :         } catch (zmq::error_t &e) {
      96                 :           0 :             if (ETERM == e.num()) {
      97                 :           0 :                 break;
      98                 :             :             }
      99                 :           0 :         }
     100                 :             : 
     101                 :           0 :         if (static_cast<bool>(interrupted)) {
     102                 :           0 :             std::cout << "interrupt received, killing program...\n";
     103                 :           0 :             break;
     104                 :             :         }
     105                 :           0 :     }
     106                 :             : 
     107                 :           0 :     return 0;
     108                 :           0 : }