AP_PiccoloCAN: modify to use uavcan agnostic CAN drivers and manager

libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp

@@ -23,11 +23,8 @@
 
 #if HAL_PICCOLO_CAN_ENABLE
 
-#include <uavcan/uavcan.hpp>
-#include <uavcan/driver/can.hpp>
-
 #include <AP_BoardConfig/AP_BoardConfig.h>
-#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <AP_CANManager/AP_CANManager.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_HAL/utility/sparse-endian.h>
@@ -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_PiccoloCAN*>(AP::can().get_driver(driver_index));
 }
 
-// initialize PiccoloCAN bus
-void AP_PiccoloCAN::init(uint8_t driver_index, bool enable_filters)
-{
-    _driver_index = 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;
+    }
 
-    debug_can(2, "PiccoloCAN: starting init\n\r");
+    _can_iface = can_iface;
 
-    if (_initialized) {
-        debug_can(1, "PiccoloCAN: already initialized\n\r");
-        return;
+    if (_can_iface == nullptr) {
+        debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: CAN driver not found\n\r");
+        return false;
     }
 
-    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_iface->is_initialized()) {
+        debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Driver not initialized\n\r");
+        return false;
     }
 
-    if (!can_mgr->is_initialized()) {
-        debug_can(1, "PiccoloCAN: mgr not initialized\n\r");
-        return;
+    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;
 
-    _can_driver = can_mgr->get_driver();
+    debug_can(AP_CANManager::LOG_DEBUG, "PiccoloCAN: starting init\n\r");
 
-    if (_can_driver == nullptr) {
-        debug_can(1, "PiccoloCAN: no CAN driver\n\r");
+    if (_initialized) {
+        debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: already initialized\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);

libraries/AP_PiccoloCAN/AP_PiccoloCAN.h

@@ -18,7 +18,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
-#include <AP_HAL/CAN.h>
+#include <AP_CANManager/AP_CANDriver.h>
 #include <AP_HAL/Semaphores.h>
 
 #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 {