uc_can.cpp

00001 /*
00002  * CAN bus driver interface.
00003  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00004  */
00005 
00006 #include <uavcan/driver/can.hpp>
00007 #include <cassert>
00008 
00009 namespace uavcan
00010 {
00011 
00012 const uint32_t CanFrame::MaskStdID;
00013 const uint32_t CanFrame::MaskExtID;
00014 const uint32_t CanFrame::FlagEFF;
00015 const uint32_t CanFrame::FlagRTR;
00016 const uint32_t CanFrame::FlagERR;
00017 const uint8_t CanFrame::MaxDataLen;
00018 
00019 bool CanFrame::priorityHigherThan(const CanFrame& rhs) const
00020 {
00021     const uint32_t clean_id     = id     & MaskExtID;
00022     const uint32_t rhs_clean_id = rhs.id & MaskExtID;
00023 
00024     /*
00025      * STD vs EXT - if 11 most significant bits are the same, EXT loses.
00026      */
00027     const bool ext     = id     & FlagEFF;
00028     const bool rhs_ext = rhs.id & FlagEFF;
00029     if (ext != rhs_ext)
00030     {
00031         const uint32_t arb11     = ext     ? (clean_id >> 18)     : clean_id;
00032         const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id;
00033         if (arb11 != rhs_arb11)
00034         {
00035             return arb11 < rhs_arb11;
00036         }
00037         else
00038         {
00039             return rhs_ext;
00040         }
00041     }
00042 
00043     /*
00044      * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses.
00045      */
00046     const bool rtr     = id     & FlagRTR;
00047     const bool rhs_rtr = rhs.id & FlagRTR;
00048     if (clean_id == rhs_clean_id && rtr != rhs_rtr)
00049     {
00050         return rhs_rtr;
00051     }
00052 
00053     /*
00054      * Plain ID arbitration - greater value loses.
00055      */
00056     return clean_id < rhs_clean_id;
00057 }
00058 
00059 #if UAVCAN_TOSTRING
00060 std::string CanFrame::toString(StringRepresentation mode) const
00061 {
00062     UAVCAN_ASSERT(mode == StrTight || mode == StrAligned);
00063 
00064     static const unsigned AsciiColumnOffset = 36U;
00065 
00066     char buf[50];
00067     char* wpos = buf;
00068     char* const epos = buf + sizeof(buf);
00069     fill(buf, buf + sizeof(buf), '\0');
00070 
00071     if (id & FlagEFF)
00072     {
00073         wpos += snprintf(wpos, unsigned(epos - wpos), "0x%08x  ", unsigned(id & MaskExtID));
00074     }
00075     else
00076     {
00077         const char* const fmt = (mode == StrAligned) ? "     0x%03x  " : "0x%03x  ";
00078         wpos += snprintf(wpos, unsigned(epos - wpos), fmt, unsigned(id & MaskStdID));
00079     }
00080 
00081     if (id & FlagRTR)
00082     {
00083         wpos += snprintf(wpos, unsigned(epos - wpos), " RTR");
00084     }
00085     else if (id & FlagERR)
00086     {
00087         wpos += snprintf(wpos, unsigned(epos - wpos), " ERR");
00088     }
00089     else
00090     {
00091         for (int dlen = 0; dlen < dlc; dlen++)                                 // hex bytes
00092         {
00093             wpos += snprintf(wpos, unsigned(epos - wpos), " %02x", unsigned(data[dlen]));
00094         }
00095 
00096         while ((mode == StrAligned) && (wpos < buf + AsciiColumnOffset))       // alignment
00097         {
00098             *wpos++ = ' ';
00099         }
00100 
00101         wpos += snprintf(wpos, unsigned(epos - wpos), "  \'");                 // ascii
00102         for (int dlen = 0; dlen < dlc; dlen++)
00103         {
00104             uint8_t ch = data[dlen];
00105             if (ch < 0x20 || ch > 0x7E)
00106             {
00107                 ch = '.';
00108             }
00109             wpos += snprintf(wpos, unsigned(epos - wpos), "%c", ch);
00110         }
00111         wpos += snprintf(wpos, unsigned(epos - wpos), "\'");
00112     }
00113     (void)wpos;
00114     return std::string(buf);
00115 }
00116 #endif
00117 
00118 }