Proper priority comparison for CAN frames of different types

libuavcan/src/driver/can.cpp

@@ -17,6 +17,45 @@ const uint32_t CanFrame::FlagRTR;
 const uint32_t CanFrame::FlagERR;
 const uint8_t CanFrame::MaxDataLen;
 
+bool CanFrame::priorityHigherThan(const CanFrame& rhs) const
+{
+    const uint32_t clean_id     = id     & MaskExtID;
+    const uint32_t rhs_clean_id = rhs.id & MaskExtID;
+
+    /*
+     * STD vs EXT - if 11 most significant bits are the same, EXT loses.
+     */
+    const bool ext     = id     & FlagEFF;
+    const bool rhs_ext = rhs.id & FlagEFF;
+    if (ext != rhs_ext)
+    {
+        const uint32_t arb11     = ext     ? (clean_id >> 18)     : clean_id;
+        const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id;
+        if (arb11 != rhs_arb11)
+        {
+            return arb11 < rhs_arb11;
+        }
+        else
+        {
+            return rhs_ext;
+        }
+    }
+
+    /*
+     * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses.
+     */
+    const bool rtr     = id     & FlagRTR;
+    const bool rhs_rtr = rhs.id & FlagRTR;
+    if (clean_id == rhs_clean_id && rtr != rhs_rtr)
+    {
+        return rhs_rtr;
+    }
+
+    /*
+     * Plain ID arbitration - greater value loses.
+     */
+    return clean_id < rhs_clean_id;
+}
 
 std::string CanFrame::toString(StringRepresentation mode) const
 {