can.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #pragma once
00006 
00007 #include <cassert>
00008 #include <queue>
00009 #include <vector>
00010 #include <gtest/gtest.h>
00011 #include <uavcan/transport/can_io.hpp>
00012 #include <uavcan/transport/frame.hpp>
00013 #include <uavcan/driver/can.hpp>
00014 #include <uavcan/driver/system_clock.hpp>
00015 #include "../../clock.hpp"
00016 
00017 
00018 class CanIfaceMock : public uavcan::ICanIface
00019 {
00020 public:
00021     struct FrameWithTime
00022     {
00023         uavcan::CanFrame frame;
00024         uavcan::MonotonicTime time;
00025         uavcan::UtcTime time_utc;
00026         uavcan::CanIOFlags flags;
00027 
00028         FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time)
00029             : frame(frame)
00030             , time(time)
00031             , flags(0)
00032         { }
00033 
00034         FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time, uavcan::UtcTime time_utc)
00035             : frame(frame)
00036             , time(time)
00037             , time_utc(time_utc)
00038             , flags(0)
00039         { }
00040 
00041         FrameWithTime(const uavcan::CanFrame& frame, uint64_t time_usec)
00042             : frame(frame)
00043             , time(uavcan::MonotonicTime::fromUSec(time_usec))
00044             , flags(0)
00045         { }
00046     };
00047 
00048     std::queue<FrameWithTime> tx;       ///< Queue of outgoing frames (bus <-- library)
00049     std::queue<FrameWithTime> rx;       ///< Queue of incoming frames (bus --> library)
00050     std::queue<FrameWithTime> loopback; ///< Loopback
00051     bool writeable;
00052     bool tx_failure;
00053     bool rx_failure;
00054     uint64_t num_errors;
00055     uavcan::ISystemClock& iclock;
00056     bool enable_utc_timestamping;
00057     uavcan::CanFrame pending_tx;
00058 
00059     CanIfaceMock(uavcan::ISystemClock& iclock)
00060         : writeable(true)
00061         , tx_failure(false)
00062         , rx_failure(false)
00063         , num_errors(0)
00064         , iclock(iclock)
00065         , enable_utc_timestamping(false)
00066     { }
00067 
00068     void pushRx(const uavcan::CanFrame& frame)
00069     {
00070         rx.push(FrameWithTime(frame, iclock.getMonotonic()));
00071     }
00072 
00073     void pushRx(const uavcan::RxFrame& frame)
00074     {
00075         uavcan::CanFrame can_frame;
00076         EXPECT_TRUE(frame.compile(can_frame));
00077         rx.push(FrameWithTime(can_frame, frame.getMonotonicTimestamp(), frame.getUtcTimestamp()));
00078     }
00079 
00080     bool matchAndPopTx(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline)
00081     {
00082         if (tx.empty())
00083         {
00084             std::cout << "Tx buffer is empty" << std::endl;
00085             return false;
00086         }
00087         const FrameWithTime frame_time = tx.front();
00088         tx.pop();
00089         return (frame_time.frame == frame) && (frame_time.time == tx_deadline);
00090     }
00091 
00092     bool matchPendingTx(const uavcan::CanFrame& frame) const
00093     {
00094         if (pending_tx != frame)
00095         {
00096             std::cout << "Pending TX mismatch: \n"
00097                       << "    Expected: " << frame.toString(uavcan::CanFrame::StrAligned) << "\n"
00098                       << "    Actual:   " << pending_tx.toString(uavcan::CanFrame::StrAligned) << std::endl;
00099         }
00100         return pending_tx == frame;
00101     }
00102 
00103     bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline_usec)
00104     {
00105         return matchAndPopTx(frame, uavcan::MonotonicTime::fromUSec(tx_deadline_usec));
00106     }
00107 
00108     uavcan::CanFrame popTxFrame()
00109     {
00110         if (tx.empty())
00111         {
00112             std::cout << "Tx buffer is empty" << std::endl;
00113             std::abort();
00114         }
00115         const FrameWithTime frame_time = tx.front();
00116         tx.pop();
00117         return frame_time.frame;
00118     }
00119 
00120     virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
00121                                  uavcan::CanIOFlags flags)
00122     {
00123         assert(this);
00124         EXPECT_TRUE(writeable);        // Shall never be called when not writeable
00125         if (tx_failure)
00126         {
00127             return -1;
00128         }
00129         if (!writeable)
00130         {
00131             return 0;
00132         }
00133         tx.push(FrameWithTime(frame, tx_deadline));
00134         tx.back().flags = flags;
00135         if (flags & uavcan::CanIOFlagLoopback)
00136         {
00137             loopback.push(FrameWithTime(frame, iclock.getMonotonic()));
00138         }
00139         return 1;
00140     }
00141 
00142     virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
00143                                     uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
00144     {
00145         assert(this);
00146         if (loopback.empty())
00147         {
00148             EXPECT_TRUE(rx.size());        // Shall never be called when not readable
00149             if (rx_failure)
00150             {
00151                 return -1;
00152             }
00153             if (rx.empty())
00154             {
00155                 return 0;
00156             }
00157             const FrameWithTime frame = rx.front();
00158             rx.pop();
00159             out_frame = frame.frame;
00160             out_ts_monotonic = frame.time;
00161             out_ts_utc = frame.time_utc;
00162             out_flags = frame.flags;
00163         }
00164         else
00165         {
00166             out_flags |= uavcan::CanIOFlagLoopback;
00167             const FrameWithTime frame = loopback.front();
00168             loopback.pop();
00169             out_frame = frame.frame;
00170             out_ts_monotonic = frame.time;
00171             out_ts_utc = frame.time_utc;
00172         }
00173 
00174         // Let's just all pretend that this code is autogenerated, instead of being carefully designed by a human.
00175         if (out_ts_utc.isZero())
00176         {
00177             out_ts_utc = enable_utc_timestamping ? iclock.getUtc() : uavcan::UtcTime();
00178         }
00179         return 1;
00180     }
00181 
00182     // cppcheck-suppress unusedFunction
00183     // cppcheck-suppress functionConst
00184     virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return 0; }
00185     // cppcheck-suppress unusedFunction
00186     virtual uavcan::uint16_t getNumFilters() const { return 4; } // decrease number of HW_filters from 9 to 4
00187     virtual uavcan::uint64_t getErrorCount() const { return num_errors; }
00188 };
00189 
00190 class CanDriverMock : public uavcan::ICanDriver
00191 {
00192 public:
00193     std::vector<CanIfaceMock> ifaces;
00194     uavcan::ISystemClock& iclock;
00195     bool select_failure;
00196 
00197     CanDriverMock(unsigned num_ifaces, uavcan::ISystemClock& iclock)
00198         : ifaces(num_ifaces, CanIfaceMock(iclock))
00199         , iclock(iclock)
00200         , select_failure(false)
00201     { }
00202 
00203     void pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
00204     {
00205         for (uint8_t i = 0; i < getNumIfaces(); i++)
00206         {
00207             ifaces.at(i).pushRx(can_frame);
00208         }
00209     }
00210 
00211     virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
00212                                    const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
00213                                    uavcan::MonotonicTime deadline)
00214     {
00215         assert(this);
00216         //std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
00217 
00218         for (unsigned i = 0; i < ifaces.size(); i++)
00219         {
00220             ifaces.at(i).pending_tx = (pending_tx[i] == UAVCAN_NULLPTR) ? uavcan::CanFrame() : *pending_tx[i];
00221         }
00222 
00223         if (select_failure)
00224         {
00225             return -1;
00226         }
00227 
00228         const uavcan::uint8_t valid_iface_mask = uavcan::uint8_t((1 << getNumIfaces()) - 1);
00229         EXPECT_FALSE(inout_masks.write & ~valid_iface_mask);
00230         EXPECT_FALSE(inout_masks.read & ~valid_iface_mask);
00231 
00232         uavcan::uint8_t out_write_mask = 0;
00233         uavcan::uint8_t out_read_mask = 0;
00234         for (unsigned i = 0; i < getNumIfaces(); i++)
00235         {
00236             const uavcan::uint8_t mask = uavcan::uint8_t(1 << i);
00237             if ((inout_masks.write & mask) && ifaces.at(i).writeable)
00238             {
00239                 out_write_mask |= mask;
00240             }
00241             if ((inout_masks.read & mask) && (ifaces.at(i).rx.size() || ifaces.at(i).loopback.size()))
00242             {
00243                 out_read_mask |= mask;
00244             }
00245         }
00246         inout_masks.write = out_write_mask;
00247         inout_masks.read = out_read_mask;
00248         if ((out_write_mask | out_read_mask) == 0)
00249         {
00250             const uavcan::MonotonicTime ts = iclock.getMonotonic();
00251             const uavcan::MonotonicDuration diff = deadline - ts;
00252             SystemClockMock* const mock = dynamic_cast<SystemClockMock*>(&iclock);
00253             if (mock)
00254             {
00255                 if (diff.isPositive())
00256                 {
00257                     mock->advance(uint64_t(diff.toUSec()));   // Emulating timeout
00258                 }
00259             }
00260             else
00261             {
00262                 if (diff.isPositive())
00263                 {
00264                     usleep(unsigned(diff.toUSec()));
00265                 }
00266             }
00267             return 0;
00268         }
00269         return 1;  // This value is not being checked anyway, it just has to be greater than zero
00270     }
00271 
00272     virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) { return &ifaces.at(iface_index); }
00273     virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const { return &ifaces.at(iface_index); }
00274     virtual uavcan::uint8_t getNumIfaces() const { return uavcan::uint8_t(ifaces.size()); }
00275 };
00276 
00277 enum FrameType { STD, EXT };
00278 inline uavcan::CanFrame makeCanFrame(uint32_t id, const std::string& str_data, FrameType type)
00279 {
00280     id |= (type == EXT) ? uavcan::CanFrame::FlagEFF : 0;
00281     return uavcan::CanFrame(id, reinterpret_cast<const uint8_t*>(str_data.c_str()), uavcan::uint8_t(str_data.length()));
00282 }