From 1f43c79bfd3bed98949f7061a504cfadd5e37efd Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 10 Feb 2022 13:10:44 +0530
Subject: [PATCH] AP_HAL: add support for taking in canfd bitrate in caniface

---
 libraries/AP_HAL/CANIface.cpp | 80 ++++++++++++++++++++++++++++++++++
 libraries/AP_HAL/CANIface.h   | 81 +++--------------------------------
 2 files changed, 87 insertions(+), 74 deletions(-)

diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp
index bce2ed6c1b..e5a712390d 100644
--- a/libraries/AP_HAL/CANIface.cpp
+++ b/libraries/AP_HAL/CANIface.cpp
@@ -93,3 +93,83 @@ bool AP_HAL::CANIface::register_frame_callback(FrameCb cb)
     frame_callback = cb;
     return true;
 }
+
+AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) :
+        id(can_id),
+        canfd(canfd_frame)
+{
+    if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) {
+        return;
+    }
+    memcpy(this->data, can_data, data_len);
+    if (data_len <= 8) {
+        dlc = data_len;
+    } else if (!canfd) {
+        dlc = 8;
+    } else {
+        /*
+        Data Length Code      9  10  11  12  13  14  15
+        Number of data bytes 12  16  20  24  32  48  64
+        */
+        if (data_len <= 12) {
+            dlc = 9;
+        } else if (data_len <= 16) {
+            dlc = 10;
+        } else if (data_len <= 20) {
+            dlc = 11;
+        } else if  (data_len <= 24) {
+            dlc = 12;
+        } else if (data_len <= 32) {
+            dlc = 13;
+        } else if (data_len <= 48) {
+            dlc = 14;
+        } else if (data_len <= 64) {
+            dlc = 15;
+        }
+    }
+}
+
+uint8_t AP_HAL::CANFrame::dataLengthToDlc(uint8_t data_length)
+{
+    if (data_length <= 8) {
+        return data_length;
+    } else if (data_length <= 12) {
+        return 9;
+    } else if (data_length <= 16) {
+        return 10;
+    } else if (data_length <= 20) {
+        return 11;
+    } else if (data_length <= 24) {
+        return 12;
+    } else if (data_length <= 32) {
+        return 13;
+    } else if (data_length <= 48) {
+        return 14;
+    }
+    return 15;
+}
+
+
+uint8_t AP_HAL::CANFrame::dlcToDataLength(uint8_t dlc)
+{
+    /*
+    Data Length Code      9  10  11  12  13  14  15
+    Number of data bytes 12  16  20  24  32  48  64
+    */
+    if (dlc <= 8) {
+        return dlc;
+    } else if (dlc == 9) {
+        return 12;
+    } else if (dlc == 10) {
+        return 16;
+    } else if (dlc == 11) {
+        return 20;
+    } else if (dlc == 12) {
+        return 24;
+    } else if (dlc == 13) {
+        return 32;
+    } else if (dlc == 14) {
+        return 48;
+    }
+    return 64;
+}
diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h
index 8246168da9..5abb17b1e3 100644
--- a/libraries/AP_HAL/CANIface.h
+++ b/libraries/AP_HAL/CANIface.h
@@ -56,40 +56,7 @@ struct AP_HAL::CANFrame {
         memset(data,0, MaxDataLen);
     }
 
-    CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false) :
-        id(can_id),
-        canfd(canfd_frame)
-    {
-        if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) {
-            return;
-        }
-        memcpy(this->data, can_data, data_len);
-        if (data_len <= 8) {
-            dlc = data_len;
-        } else if (!canfd) {
-            dlc = 8;
-        } else {
-            /*
-            Data Length Code      9  10  11  12  13  14  15
-            Number of data bytes 12  16  20  24  32  48  64
-            */
-           if (data_len <= 12) {
-               dlc = 9;
-           } else if (data_len <= 16) {
-               dlc = 10;
-           } else if (data_len <= 20) {
-               dlc = 11;
-           } else if  (data_len <= 24) {
-               dlc = 12;
-           } else if (data_len <= 32) {
-               dlc = 13;
-           } else if (data_len <= 48) {
-               dlc = 14;
-           } else if (data_len <= 64) {
-               dlc = 15;
-           }
-        }
-    }
+    CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false);
 
     bool operator!=(const CANFrame& rhs) const
     {
@@ -122,47 +89,9 @@ struct AP_HAL::CANFrame {
         return canfd;
     }
 
-    static uint8_t dlcToDataLength(uint8_t dlc) {
-        /*
-        Data Length Code      9  10  11  12  13  14  15
-        Number of data bytes 12  16  20  24  32  48  64
-        */
-        if (dlc <= 8) {
-            return dlc;
-        } else if (dlc == 9) {
-            return 12;
-        } else if (dlc == 10) {
-            return 16;
-        } else if (dlc == 11) {
-            return 20;
-        } else if (dlc == 12) {
-            return 24;
-        } else if (dlc == 13) {
-            return 32;
-        } else if (dlc == 14) {
-            return 48;
-        }
-        return 64;
-    }
+    static uint8_t dlcToDataLength(uint8_t dlc);
 
-    static uint8_t dataLengthToDlc(uint8_t data_length) {
-        if (data_length <= 8) {
-            return data_length;
-        } else if (data_length <= 12) {
-            return 9;
-        } else if (data_length <= 16) {
-            return 10;
-        } else if (data_length <= 20) {
-            return 11;
-        } else if (data_length <= 24) {
-            return 12;
-        } else if (data_length <= 32) {
-            return 13;
-        } else if (data_length <= 48) {
-            return 14;
-        }
-        return 15;
-    }
+    static uint8_t dataLengthToDlc(uint8_t data_length);
     /**
      * CAN frame arbitration rules, particularly STD vs EXT:
      *     Marco Di Natale - "Understanding and using the Controller Area Network"
@@ -234,6 +163,10 @@ public:
         }
     };
 
+    virtual bool init(const uint32_t bitrate, const OperatingMode mode, const uint32_t fdbitrate) {
+        return init(bitrate, mode);
+    }
+
     // Initialise the interface with hardware configuration required to start comms.
     virtual bool init(const uint32_t bitrate, const OperatingMode mode) = 0;