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 "."