diff --git a/libraries/AP_HAL/AP_HAL.h b/libraries/AP_HAL/AP_HAL.h
index 90adf1f28a..64198b63d0 100644
--- a/libraries/AP_HAL/AP_HAL.h
+++ b/libraries/AP_HAL/AP_HAL.h
@@ -22,9 +22,7 @@
#include "Flash.h"
#include "DSP.h"
-#if HAL_WITH_UAVCAN
-#include "CAN.h"
-#endif
+#include "CANIface.h"
#include "utility/BetterStream.h"
diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h
index 9942dd9c43..d8967f13de 100644
--- a/libraries/AP_HAL/AP_HAL_Boards.h
+++ b/libraries/AP_HAL/AP_HAL_Boards.h
@@ -156,8 +156,8 @@
#define HAL_COMPASS_HMC5843_I2C_ADDR 0x1E
#endif
-#ifndef HAL_WITH_UAVCAN
-#define HAL_WITH_UAVCAN 0
+#ifndef HAL_NUM_CAN_IFACES
+#define HAL_NUM_CAN_IFACES 0
#endif
#ifndef HAL_RCINPUT_WITH_AP_RADIO
@@ -215,6 +215,14 @@
#define HAL_CAN_DRIVER_DEFAULT 0
#endif
+#ifndef HAL_MAX_CAN_PROTOCOL_DRIVERS
+#define HAL_MAX_CAN_PROTOCOL_DRIVERS HAL_NUM_CAN_IFACES
+#endif
+
+#ifndef HAL_ENABLE_LIBUAVCAN_DRIVERS
+#define HAL_ENABLE_LIBUAVCAN_DRIVERS (HAL_MAX_CAN_PROTOCOL_DRIVERS > 0)
+#endif
+
#ifdef HAVE_LIBDL
#define AP_MODULE_SUPPORTED 1
#else
@@ -230,7 +238,7 @@
#define HAL_HAVE_DUAL_USB_CDC 0
#endif
-#if HAL_WITH_UAVCAN && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#if HAL_NUM_CAN_IFACES && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#define AP_UAVCAN_SLCAN_ENABLED 1
#else
#define AP_UAVCAN_SLCAN_ENABLED 0
diff --git a/libraries/AP_HAL/AP_HAL_Macros.h b/libraries/AP_HAL/AP_HAL_Macros.h
index 99bd7c042c..e2bc758722 100644
--- a/libraries/AP_HAL/AP_HAL_Macros.h
+++ b/libraries/AP_HAL/AP_HAL_Macros.h
@@ -40,7 +40,7 @@
#define floor(x) DO_NOT_USE_DOUBLE_MATHS()
#define round(x) DO_NOT_USE_DOUBLE_MATHS()
#define fmax(x,y) DO_NOT_USE_DOUBLE_MATHS()
-#if !HAL_WITH_UAVCAN
+#if !HAL_NUM_CAN_IFACES
// we should do log() and fabs() as well, but can't because of a conflict in uavcan
#define log(x) DO_NOT_USE_DOUBLE_MATHS()
#define fabs(x) DO_NOT_USE_DOUBLE_MATHS()
diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h
index 2b03dc7f1d..5ec50d621b 100644
--- a/libraries/AP_HAL/AP_HAL_Namespace.h
+++ b/libraries/AP_HAL/AP_HAL_Namespace.h
@@ -32,9 +32,8 @@ namespace AP_HAL {
class OpticalFlow;
class DSP;
- class CANProtocol;
- class CANManager;
- class CANHal;
+ class CANIface;
+ class CANFrame;
class Util;
class Flash;
diff --git a/libraries/AP_HAL/CAN.h b/libraries/AP_HAL/CAN.h
deleted file mode 100644
index f9c7fc855e..0000000000
--- a/libraries/AP_HAL/CAN.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * Copyright (C) 2017 Eugene Shamaev
- *
- * This file is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program. If not, see .
- */
-
-#pragma once
-
-#include
-#include
-#include
-#include
-
-#if HAL_WITH_UAVCAN
-
-#include "AP_HAL_Namespace.h"
-#include "utility/functor.h"
-#include
-#include
-
-#define MAX_NUMBER_OF_CAN_INTERFACES 2
-#define MAX_NUMBER_OF_CAN_DRIVERS 2
-
-/**
- * Interface that CAN protocols need to implement
- */
-class AP_HAL::CANProtocol {
-public:
- /* method called when initializing the CAN interfaces
- *
- * if initialization doesn't have errors, protocol class
- * should create a thread to do send and receive operations
- */
- virtual void init(uint8_t driver_index, bool enable_filters) = 0;
-};
-
-/**
- * Single non-blocking CAN interface.
- */
-class AP_HAL::CANHal: public uavcan::ICanIface {
-public:
- /* CAN port open method
-
- bitrate Selects the speed that the port will be configured to. If zero, the port speed is left unchanged.
-
- return false - CAN port open failed
- true - CAN port open succeeded
- */
- virtual bool begin(uint32_t bitrate) = 0;
-
- /*
- CAN port close
- */
- virtual void end() = 0;
-
- /*
- Reset opened CAN port
-
- Pending messages to be transmitted are deleted and receive state and FIFO also reset.
- All pending errors are cleared.
- */
- virtual void reset() = 0;
-
- /*
- Test if CAN port is opened and initialized
-
- return false - CAN port not initialized
- true - CAN port is initialized
- */
- virtual bool is_initialized() = 0;
-
- /*
- Return if CAN port has some untransmitted pending messages
-
- return -1 - CAN port is not opened or initialized
- 0 - no messages are pending
- positive - number of pending messages
- */
- virtual int32_t tx_pending() = 0;
-
- /*
- Return if CAN port has received but not yet read messages
-
- return -1 - CAN port is not opened or initialized
- 0 - no messages are pending for read
- positive - number of pending messages for read
- */
- virtual int32_t available() = 0;
-
-};
-
-/**
- * Generic CAN driver.
- */
-class AP_HAL::CANManager {
-public:
- CANManager(uavcan::ICanDriver* driver) : _driver(driver) {}
-
- /* CAN port open method
- Opens port with specified bit rate
- bitrate - selects the speed that the port will be configured to. If zero, the port speed is left
- unchanged.
- can_number - the index of can interface to be opened
-
- return false - CAN port open failed
- true - CAN port open succeeded
- */
- virtual bool begin(uint32_t bitrate, uint8_t can_number) = 0;
-
- /*
- Test if CAN manager is ready and initialized
- return false - CAN manager not initialized
- true - CAN manager is initialized
- */
- virtual bool is_initialized() = 0;
- virtual void initialized(bool val) = 0;
-
- uavcan::ICanDriver* get_driver() { return _driver; }
-private:
- uavcan::ICanDriver* _driver;
-};
-
-#endif
diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp
new file mode 100644
index 0000000000..ef2e65b375
--- /dev/null
+++ b/libraries/AP_HAL/CANIface.cpp
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2020 Siddharth B Purohit
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ */
+
+#include "CANIface.h"
+
+bool AP_HAL::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;
+}
diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h
new file mode 100644
index 0000000000..77fda62e52
--- /dev/null
+++ b/libraries/AP_HAL/CANIface.h
@@ -0,0 +1,209 @@
+/*
+ * Copyright (C) 2020 Siddharth B Purohit
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include "AP_HAL_Namespace.h"
+
+/**
+ * Raw CAN frame, as passed to/from the CAN driver.
+ */
+struct AP_HAL::CANFrame {
+ static const uint32_t MaskStdID = 0x000007FFU;
+ static const uint32_t MaskExtID = 0x1FFFFFFFU;
+ static const uint32_t FlagEFF = 1U << 31; ///< Extended frame format
+ static const uint32_t FlagRTR = 1U << 30; ///< Remote transmission request
+ static const uint32_t FlagERR = 1U << 29; ///< Error frame
+
+ static const uint8_t MaxDataLen = 8;
+
+ uint32_t id; ///< CAN ID with flags (above)
+ uint8_t data[MaxDataLen];
+ uint8_t dlc; ///< Data Length Code
+
+ CANFrame() :
+ id(0),
+ dlc(0)
+ {
+ memset(data,0, MaxDataLen);
+ }
+
+ CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) :
+ id(can_id),
+ dlc((data_len > MaxDataLen) ? MaxDataLen : data_len)
+ {
+ if ((can_data == nullptr) || (data_len != dlc) || (dlc == 0)) {
+ return;
+ }
+ memcpy(this->data, can_data, dlc);
+ }
+
+ bool operator!=(const CANFrame& rhs) const
+ {
+ return !operator==(rhs);
+ }
+ bool operator==(const CANFrame& rhs) const
+ {
+ return (id == rhs.id) && (dlc == rhs.dlc) && (memcmp(data, rhs.data, dlc) == 0);
+ }
+
+ bool isExtended() const
+ {
+ return id & FlagEFF;
+ }
+ bool isRemoteTransmissionRequest() const
+ {
+ return id & FlagRTR;
+ }
+ bool isErrorFrame() const
+ {
+ return id & FlagERR;
+ }
+
+ /**
+ * CAN frame arbitration rules, particularly STD vs EXT:
+ * Marco Di Natale - "Understanding and using the Controller Area Network"
+ * http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf
+ */
+ bool priorityHigherThan(const CANFrame& rhs) const;
+ bool priorityLowerThan(const CANFrame& rhs) const
+ {
+ return rhs.priorityHigherThan(*this);
+ }
+};
+
+class AP_HAL::CANIface
+{
+public:
+
+ enum OperatingMode {
+ PassThroughMode,
+ NormalMode,
+ SilentMode
+ };
+
+ typedef uint16_t CanIOFlags;
+ static const CanIOFlags Loopback = 1;
+ static const CanIOFlags AbortOnError = 2;
+
+ // Single Rx Frame with related info
+ struct CanRxItem {
+ uint64_t timestamp_us = 0;
+ CanIOFlags flags = 0;
+ CANFrame frame;
+ };
+
+ // Single Tx Frame with related info
+ struct CanTxItem {
+ uint64_t deadline = 0;
+ CANFrame frame;
+ uint32_t index = 0;
+ bool loopback = false;
+ bool abort_on_error = false;
+ bool aborted = false;
+ bool pushed = false;
+ bool setup = false;
+
+ bool operator<(const CanTxItem& rhs) const
+ {
+ if (frame.priorityLowerThan(rhs.frame)) {
+ return true;
+ }
+ if (frame.priorityHigherThan(rhs.frame)) {
+ return false;
+ }
+ return index > rhs.index;
+ }
+ };
+
+ struct CanFilterConfig {
+ uint32_t id = 0;
+ uint32_t mask = 0;
+
+ bool operator==(const CanFilterConfig& rhs) const
+ {
+ return rhs.id == id && rhs.mask == mask;
+ }
+ };
+
+ // Initialise the interface with hardware configuration required to start comms.
+ virtual bool init(const uint32_t bitrate, const OperatingMode mode) = 0;
+
+ // Select method to notify user of Rx and Tx buffer state.
+ // fill read select with true if a frame is available in Rx buffer
+ // fill write select with true if space is available in Tx buffer
+ // Also waits for Rx or Tx event depending on read_select and write_select values
+ // passed to the method until timeout. Returns true if the Rx/Tx even occured
+ // while waiting, false if timedout
+ virtual bool select(bool &read_select, bool &write_select,
+ const CANFrame* const pending_tx, uint64_t timeout)
+ {
+ return false;
+ }
+
+ virtual bool set_event_handle(EventHandle* evt_handle)
+ {
+ return true;
+ }
+
+ // Put frame in queue to be sent, return negative if error occured, 0 if no space, and 1 if successful
+ virtual int16_t send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) = 0;
+
+ // Non blocking receive frame that pops the frames received inside the buffer, return negative if error occured,
+ // 0 if no frame available, 1 if successful
+ virtual int16_t receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags) = 0;
+
+ //Configure filters so as to reject frames that are not going to be handled by us
+ virtual bool configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs)
+ {
+ return 0;
+ }
+
+
+ //Number of available hardware filters.
+ virtual uint16_t getNumFilters() const
+ {
+ return 0;
+ }
+
+ //Return Total Error Count generated so far
+ virtual uint32_t getErrorCount() const
+ {
+ return 0;
+ }
+
+ //Get status info of the interface
+ virtual uint32_t get_stats(char* data, uint32_t max_size)
+ {
+ return 0;
+ }
+
+ // return true if busoff was detected and not cleared
+ virtual bool is_busoff() const
+ {
+ return false;
+ }
+
+ // Methods to be used only while testing CANBus
+ // Not for normal operation or use case
+ virtual void flush_tx() {}
+ virtual void clear_rx() {}
+
+ // return true if init was called and successful
+ virtual bool is_initialized() const = 0;
+};
diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h
index 55d3c6e11a..f512936326 100644
--- a/libraries/AP_HAL/HAL.h
+++ b/libraries/AP_HAL/HAL.h
@@ -14,9 +14,7 @@ class AP_Param;
#include "system.h"
#include "OpticalFlow.h"
#include "DSP.h"
-#if HAL_WITH_UAVCAN
-#include "CAN.h"
-#endif
+#include "CANIface.h"
class AP_HAL::HAL {
@@ -42,10 +40,10 @@ public:
AP_HAL::OpticalFlow*_opticalflow,
AP_HAL::Flash* _flash,
AP_HAL::DSP* _dsp,
-#if HAL_WITH_UAVCAN
- AP_HAL::CANManager* _can_mgr[MAX_NUMBER_OF_CAN_DRIVERS])
+#if HAL_NUM_CAN_IFACES > 0
+ AP_HAL::CANIface* _can_ifaces[HAL_NUM_CAN_IFACES])
#else
- AP_HAL::CANManager** _can_mgr)
+ AP_HAL::CANIface** _can_ifaces)
#endif
:
uartA(_uartA),
@@ -70,13 +68,13 @@ public:
flash(_flash),
dsp(_dsp)
{
-#if HAL_WITH_UAVCAN
- if (_can_mgr == nullptr) {
- for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
- can_mgr[i] = nullptr;
+#if HAL_NUM_CAN_IFACES > 0
+ if (_can_ifaces == nullptr) {
+ for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++)
+ can[i] = nullptr;
} else {
- for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
- can_mgr[i] = _can_mgr[i];
+ for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++)
+ can[i] = _can_ifaces[i];
}
#endif
@@ -122,9 +120,9 @@ public:
AP_HAL::OpticalFlow *opticalflow;
AP_HAL::Flash *flash;
AP_HAL::DSP *dsp;
-#if HAL_WITH_UAVCAN
- AP_HAL::CANManager* can_mgr[MAX_NUMBER_OF_CAN_DRIVERS];
+#if HAL_NUM_CAN_IFACES > 0
+ AP_HAL::CANIface* can[HAL_NUM_CAN_IFACES];
#else
- AP_HAL::CANManager** can_mgr;
+ AP_HAL::CANIface** can;
#endif
};
diff --git a/libraries/AP_HAL/board/chibios.h b/libraries/AP_HAL/board/chibios.h
index aed3e263d7..cb545ee7bf 100644
--- a/libraries/AP_HAL/board/chibios.h
+++ b/libraries/AP_HAL/board/chibios.h
@@ -26,8 +26,8 @@
#define HAL_GPIO_LED_OFF 1
#endif
-#ifndef HAL_WITH_UAVCAN
-#define HAL_WITH_UAVCAN 0
+#ifndef HAL_NUM_CAN_IFACES
+#define HAL_NUM_CAN_IFACES 0
#endif
#ifndef HAL_HAVE_BOARD_VOLTAGE
diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h
index 2af296af15..8da77249af 100644
--- a/libraries/AP_HAL/board/linux.h
+++ b/libraries/AP_HAL/board/linux.h
@@ -175,7 +175,7 @@
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_OPTFLOW_PX4FLOW_I2C_BUS 2
#define HAL_RANGEFINDER_LIGHTWARE_I2C_BUS 2
- #define HAL_WITH_UAVCAN 1
+ #define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(LSM9DS1, "lsm9ds1_ag", ROTATION_PITCH_180)
#define HAL_MAG_PROBE_LIST PROBE_MAG_SPI(LSM9DS1, "lsm9ds1_m", ROTATION_NONE)
@@ -199,7 +199,7 @@
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_OPTFLOW_PX4FLOW_I2C_BUS 1
#define HAL_RANGEFINDER_LIGHTWARE_I2C_BUS 1
- #define HAL_WITH_UAVCAN 1
+ #define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
#define HAL_GPIO_A_LED_PIN 59
#define HAL_GPIO_B_LED_PIN 58
@@ -213,7 +213,7 @@
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_OPTFLOW_PX4FLOW_I2C_BUS 2
#define HAL_RANGEFINDER_LIGHTWARE_I2C_BUS 2
- #define HAL_WITH_UAVCAN 1
+ #define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77)
#define HAL_INS_PROBE1 PROBE_IMU_I2C(Invensense, 1, 0x69, ROTATION_NONE)
@@ -247,7 +247,7 @@
#define HAL_MAG_PROBE3 PROBE_MAG_I2C(IST8310, 4, 0x0e, true, ROTATION_PITCH_180_YAW_90)
#define HAL_MAG_PROBE_LIST HAL_MAG_PROBE1; HAL_MAG_PROBE2; HAL_MAG_PROBE3
#define HAL_RCOUTPUT_TAP_DEVICE "/dev/ttyS1"
- #define HAL_WITH_UAVCAN 1
+ #define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK
#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE)
#define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE)
@@ -268,7 +268,7 @@
// only external compasses
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE
- #define HAL_WITH_UAVCAN 1
+ #define HAL_NUM_CAN_IFACES 1
#define HAL_IMU_TEMP_DEFAULT 55
#define HAL_HAVE_IMU_HEATER 1
#define HAL_UTILS_HEAT HAL_LINUX_HEAT_PWM
diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h
index 1ca5a2fe1e..3b1adbb661 100644
--- a/libraries/AP_HAL/board/sitl.h
+++ b/libraries/AP_HAL/board/sitl.h
@@ -61,7 +61,9 @@
#include
#define HAL_EventHandle AP_HAL::EventHandle
-#define HAL_NUM_CAN_IFACES 2
+#ifndef HAL_NUM_CAN_IFACES
+#define HAL_NUM_CAN_IFACES 0
+#endif
#ifndef HAL_BOARD_STORAGE_DIRECTORY
#define HAL_BOARD_STORAGE_DIRECTORY "."