From 0fcf0b4564239e607eb324c4af0df4551f989428 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sun, 31 May 2020 18:06:45 +0530 Subject: [PATCH] AP_PiccoloCAN: modify to use uavcan agnostic CAN drivers and manager --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 147 ++++++++++------------ libraries/AP_PiccoloCAN/AP_PiccoloCAN.h | 18 +-- 2 files changed, 77 insertions(+), 88 deletions(-) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 8d6bad46dc..96edffe8f3 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -23,11 +23,8 @@ #if HAL_PICCOLO_CAN_ENABLE -#include -#include - #include -#include +#include #include #include #include @@ -43,59 +40,62 @@ extern const AP_HAL::HAL& hal; -static const uint8_t CAN_IFACE_INDEX = 0; - -#define debug_can(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0) +#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "PiccoloCAN", fmt, ##args); } while (0) AP_PiccoloCAN::AP_PiccoloCAN() { - debug_can(2, "PiccoloCAN: constructed\n\r"); + debug_can(AP_CANManager::LOG_INFO, "PiccoloCAN: constructed\n\r"); } AP_PiccoloCAN *AP_PiccoloCAN::get_pcan(uint8_t driver_index) { if (driver_index >= AP::can().get_num_drivers() || - AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_PiccoloCAN) { + AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_PiccoloCAN) { return nullptr; } return static_cast(AP::can().get_driver(driver_index)); } +bool AP_PiccoloCAN::add_interface(AP_HAL::CANIface* can_iface) { + if (_can_iface != nullptr) { + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Multiple Interface not supported\n\r"); + return false; + } + + _can_iface = can_iface; + + if (_can_iface == nullptr) { + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: CAN driver not found\n\r"); + return false; + } + + if (!_can_iface->is_initialized()) { + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Driver not initialized\n\r"); + return false; + } + + if (!_can_iface->set_event_handle(&_event_handle)) { + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Cannot add event handle\n\r"); + return false; + } + return true; +} + // initialize PiccoloCAN bus void AP_PiccoloCAN::init(uint8_t driver_index, bool enable_filters) { _driver_index = driver_index; - debug_can(2, "PiccoloCAN: starting init\n\r"); + debug_can(AP_CANManager::LOG_DEBUG, "PiccoloCAN: starting init\n\r"); if (_initialized) { - debug_can(1, "PiccoloCAN: already initialized\n\r"); + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: already initialized\n\r"); return; } - - AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index]; - - if (can_mgr == nullptr) { - debug_can(1, "PiccoloCAN: no mgr for this driver\n\r"); - return; - } - - if (!can_mgr->is_initialized()) { - debug_can(1, "PiccoloCAN: mgr not initialized\n\r"); - return; - } - - _can_driver = can_mgr->get_driver(); - - if (_can_driver == nullptr) { - debug_can(1, "PiccoloCAN: no CAN driver\n\r"); - return; - } - // start calls to loop in separate thread if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_PiccoloCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_MAIN, 1)) { - debug_can(1, "PiccoloCAN: couldn't create thread\n\r"); + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: couldn't create thread\n\r"); return; } @@ -103,14 +103,14 @@ void AP_PiccoloCAN::init(uint8_t driver_index, bool enable_filters) snprintf(_thread_name, sizeof(_thread_name), "PiccoloCAN_%u", driver_index); - debug_can(2, "PiccoloCAN: init done\n\r"); + debug_can(AP_CANManager::LOG_DEBUG, "PiccoloCAN: init done\n\r"); } // loop to send output to CAN devices in background thread void AP_PiccoloCAN::loop() { - uavcan::CanFrame txFrame; - uavcan::CanFrame rxFrame; + AP_HAL::CANFrame txFrame; + AP_HAL::CANFrame rxFrame; // How often to transmit CAN messages (milliseconds) #define CMD_TX_PERIOD 10 @@ -121,17 +121,17 @@ void AP_PiccoloCAN::loop() uint8_t frame_id_group; // Piccolo message group uint16_t frame_id_device; // Device identifier - uavcan::MonotonicTime timeout; + uint64_t timeout; while (true) { if (!_initialized) { - debug_can(2, "PiccoloCAN: not initialized\n\r"); + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: not initialized\n\r"); hal.scheduler->delay_microseconds(10000); continue; } - timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 250); + timeout = AP_HAL::micros64() + 250; // 1ms loop delay hal.scheduler->delay_microseconds(1 * 1000); @@ -151,7 +151,7 @@ void AP_PiccoloCAN::loop() frame_id_device = (rxFrame.id >> 8) & 0xFF; // Only accept extended messages - if ((rxFrame.id & uavcan::CanFrame::FlagEFF) == 0) { + if ((rxFrame.id & AP_HAL::CANFrame::FlagEFF) == 0) { continue; } @@ -179,55 +179,45 @@ void AP_PiccoloCAN::loop() } // write frame on CAN bus, returns true on success -bool AP_PiccoloCAN::write_frame(uavcan::CanFrame &out_frame, uavcan::MonotonicTime timeout) +bool AP_PiccoloCAN::write_frame(AP_HAL::CANFrame &out_frame, uint64_t timeout) { if (!_initialized) { - debug_can(1, "PiccoloCAN: Driver not initialized for write_frame\n\r"); + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Driver not initialized for write_frame\n\r"); return false; } - // wait for space in buffer to send command - uavcan::CanSelectMasks inout_mask; - + bool read_select = false; + bool write_select = true; + bool ret; do { - inout_mask.read = 0; - inout_mask.write = (1 << CAN_IFACE_INDEX); - _select_frames[CAN_IFACE_INDEX] = &out_frame; - _can_driver->select(inout_mask, _select_frames, timeout); - - if (!inout_mask.write) { + ret = _can_iface->select(read_select, write_select, &out_frame, timeout); + if (!ret || !write_select) { hal.scheduler->delay_microseconds(50); } - } while (!inout_mask.write); + } while (!ret || !write_select); - return (_can_driver->getIface(CAN_IFACE_INDEX)->send(out_frame, timeout, uavcan::CanIOFlagAbortOnError) == 1); + return (_can_iface->send(out_frame, timeout, AP_HAL::CANIface::AbortOnError) == 1); } // read frame on CAN bus, returns true on succses -bool AP_PiccoloCAN::read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout) +bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint64_t timeout) { if (!_initialized) { - debug_can(1, "PiccoloCAN: Driver not initialized for read_frame\n\r"); + debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Driver not initialized for read_frame\n\r"); return false; } - - uavcan::CanSelectMasks inout_mask; - inout_mask.read = 1 << CAN_IFACE_INDEX; - inout_mask.write = 0; - - _select_frames[CAN_IFACE_INDEX] = &recv_frame; - _can_driver->select(inout_mask, _select_frames, timeout); - - if (!inout_mask.read) { + bool read_select = true; + bool write_select = false; + bool ret = _can_iface->select(read_select, write_select, nullptr, timeout); + if (!ret || !read_select) { // No frame available return false; } - uavcan::MonotonicTime time; - uavcan::UtcTime utc_time; - uavcan::CanIOFlags flags {}; + uint64_t time; + AP_HAL::CANIface::CanIOFlags flags {}; - return (_can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags) == 1); + return (_can_iface->receive(recv_frame, time, flags) == 1); } // called from SRV_Channels @@ -355,9 +345,9 @@ void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan) // send ESC messages over CAN void AP_PiccoloCAN::send_esc_messages(void) { - uavcan::CanFrame txFrame; + AP_HAL::CANFrame txFrame; - uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 250); + uint64_t timeout = AP_HAL::micros64() + 250; // TODO - How to buffer CAN messages properly? // Sending more than 2 messages at each loop instance means that sometimes messages are dropped @@ -427,7 +417,7 @@ void AP_PiccoloCAN::send_esc_messages(void) // interpret an ESC message received over CAN -bool AP_PiccoloCAN::handle_esc_message(uavcan::CanFrame &frame) +bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame) { uint64_t timestamp = AP_HAL::micros64(); @@ -455,7 +445,6 @@ bool AP_PiccoloCAN::handle_esc_message(uavcan::CanFrame &frame) if (decodeESC_StatusAPacketStructure(&frame, &esc.statusA)) { esc.newTelemetry = true; } else if (decodeESC_StatusBPacketStructure(&frame, &esc.statusB)) { - esc.newTelemetry = true; } else if (decodeESC_FirmwarePacketStructure(&frame, &esc.firmware)) { // TODO @@ -560,7 +549,7 @@ bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len) //! \return the packet data pointer from the packet uint8_t* getESCVelocityPacketData(void* pkt) { - uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt; + AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt; return (uint8_t*) frame->data; } @@ -568,7 +557,7 @@ uint8_t* getESCVelocityPacketData(void* pkt) //! \return the packet data pointer from the packet, const const uint8_t* getESCVelocityPacketDataConst(const void* pkt) { - uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt; + AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt; return (const uint8_t*) frame->data; } @@ -576,10 +565,10 @@ const uint8_t* getESCVelocityPacketDataConst(const void* pkt) //! Complete a packet after the data have been encoded void finishESCVelocityPacket(void* pkt, int size, uint32_t packetID) { - uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt; + AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt; - if (size > uavcan::CanFrame::MaxDataLen) { - size = uavcan::CanFrame::MaxDataLen; + if (size > AP_HAL::CANFrame::MaxDataLen) { + size = AP_HAL::CANFrame::MaxDataLen; } frame->dlc = size; @@ -599,7 +588,7 @@ void finishESCVelocityPacket(void* pkt, int size, uint32_t packetID) (((uint8_t) AP_PiccoloCAN::ActuatorType::ESC) << 8); // Actuator type // Extended frame format - id |= uavcan::CanFrame::FlagEFF; + id |= AP_HAL::CANFrame::FlagEFF; frame->id = id; } @@ -607,7 +596,7 @@ void finishESCVelocityPacket(void* pkt, int size, uint32_t packetID) //! \return the size of a packet from the packet header int getESCVelocityPacketSize(const void* pkt) { - uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt; + AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt; return (int) frame->dlc; } @@ -615,7 +604,7 @@ int getESCVelocityPacketSize(const void* pkt) //! \return the ID of a packet from the packet header uint32_t getESCVelocityPacketID(const void* pkt) { - uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt; + AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt; // Extract the message ID field from the 29-bit ID return (uint32_t) ((frame->id >> 16) & 0xFF); diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 022cf33751..5daacada72 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include "piccolo_protocol/ESCPackets.h" @@ -28,12 +28,12 @@ #define PICCOLO_CAN_MAX_GROUP_ESC (PICCOLO_CAN_MAX_NUM_ESC / 4) #ifndef HAL_PICCOLO_CAN_ENABLE -#define HAL_PICCOLO_CAN_ENABLE (HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES) +#define HAL_PICCOLO_CAN_ENABLE (HAL_NUM_CAN_IFACES && !HAL_MINIMIZE_FEATURES) #endif #if HAL_PICCOLO_CAN_ENABLE -class AP_PiccoloCAN : public AP_HAL::CANProtocol +class AP_PiccoloCAN : public AP_CANDriver { public: AP_PiccoloCAN(); @@ -65,6 +65,7 @@ public: // initialize PiccoloCAN bus void init(uint8_t driver_index, bool enable_filters) override; + bool add_interface(AP_HAL::CANIface* can_iface) override; // called from SRV_Channels void update(); @@ -87,23 +88,22 @@ private: void loop(); // write frame on CAN bus, returns true on success - bool write_frame(uavcan::CanFrame &out_frame, uavcan::MonotonicTime timeout); + bool write_frame(AP_HAL::CANFrame &out_frame, uint64_t timeout); // read frame on CAN bus, returns true on succses - bool read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout); + bool read_frame(AP_HAL::CANFrame &recv_frame, uint64_t timeout); // send ESC commands over CAN void send_esc_messages(void); // interpret an ESC message received over CAN - bool handle_esc_message(uavcan::CanFrame &frame); + bool handle_esc_message(AP_HAL::CANFrame &frame); bool _initialized; char _thread_name[16]; uint8_t _driver_index; - uavcan::ICanDriver* _can_driver; - const uavcan::CanFrame* _select_frames[uavcan::MaxCanIfaces] { }; - + AP_HAL::CANIface* _can_iface; + HAL_EventHandle _event_handle; HAL_Semaphore _telem_sem; struct PiccoloESC_Info_t {