AP_CANManager: added CANSensor class

this makes creation of custom CAN protocol handlers much easier

libraries/AP_CANManager/AP_CANManager.cpp

@@ -110,6 +110,8 @@ AP_CANManager::AP_CANManager()
 
 void AP_CANManager::init()
 {
+    WITH_SEMAPHORE(_sem);
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     if (AP::sitl() != nullptr) {
         if (AP::sitl()->speedup > 1) {
@@ -274,6 +276,57 @@ void AP_CANManager::init()
     }
 }
 
+/*
+  register a new CAN driver
+ */
+bool AP_CANManager::register_driver(Driver_Type dtype, AP_CANDriver *driver)
+{
+    WITH_SEMAPHORE(_sem);
+
+    for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
+        uint8_t drv_num = _interfaces[i]._driver_number;
+        if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
+            continue;
+        }
+        // from 1 based to 0 based
+        drv_num--;
+
+        if (dtype != (Driver_Type)_drv_param[drv_num]._driver_type.get()) {
+            continue;
+        }
+        if (_drivers[drv_num] != nullptr) {
+            continue;
+        }
+        if (_num_drivers >= HAL_MAX_CAN_PROTOCOL_DRIVERS) {
+            continue;
+        }
+
+        if (hal.can[i] == nullptr) {
+            // if this interface is not allocated allocate it here,
+            // also pass the index of the CANBus
+            const_cast <AP_HAL::HAL&> (hal).can[i] = new HAL_CANIface(i);
+        }
+
+        // Initialise the interface we just allocated
+        if (hal.can[i] == nullptr) {
+            continue;
+        }
+        AP_HAL::CANIface* iface = hal.can[i];
+
+        _drivers[drv_num] = driver;
+        _drivers[drv_num]->add_interface(iface);
+        log_text(AP_CANManager::LOG_INFO, LOG_TAG, "Adding Interface %d to Driver %d", i + 1, drv_num + 1);
+
+        _drivers[drv_num]->init(drv_num, false);
+        _driver_type_cache[drv_num] = dtype;
+
+        _num_drivers++;
+
+        return true;
+    }
+    return false;
+}
+
 // Method used by CAN related library methods to report status and debug info
 // The result of this method can be accessed via ftp get @SYS/can_log.txt
 void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag, const char *fmt, ...)

libraries/AP_CANManager/AP_CANManager.h

@@ -61,6 +61,9 @@ public:
 
     void init(void);
 
+    // register a new driver
+    bool register_driver(Driver_Type dtype, AP_CANDriver *driver);
+
     // returns number of active CAN Drivers
     uint8_t get_num_drivers(void) const
     {
@@ -139,7 +142,7 @@ private:
     };
 
     CANIface_Params _interfaces[HAL_NUM_CAN_IFACES];
-    AP_CANDriver* _drivers[HAL_MAX_CAN_PROTOCOL_DRIVERS] {};
+    AP_CANDriver* _drivers[HAL_MAX_CAN_PROTOCOL_DRIVERS];
     CANDriver_Params _drv_param[HAL_MAX_CAN_PROTOCOL_DRIVERS];
     Driver_Type _driver_type_cache[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
@@ -150,6 +153,8 @@ private:
 
     char* _log_buf;
     uint32_t _log_pos;
+
+    HAL_Semaphore _sem;
 };
 
 namespace AP

libraries/AP_CANManager/AP_CANSensor.cpp

@@ -0,0 +1,127 @@
+/*
+   This program 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 program 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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  generic CAN sensor class, for easy creation of CAN sensors using prioprietary protocols
+ */
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_MAX_CAN_PROTOCOL_DRIVERS
+
+#include <AP_Scheduler/AP_Scheduler.h>
+#include "AP_CANSensor.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, _driver_name, fmt, ##args); } while (0)
+
+CANSensor::CANSensor(const char *driver_name, AP_CANManager::Driver_Type dtype, uint16_t stack_size) :
+    _driver_name(driver_name),
+    _stack_size(stack_size)
+{
+    if (!AP::can().register_driver(dtype, this)) {
+        debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", driver_name);
+    } else {
+        debug_can(AP_CANManager::LOG_INFO, "%s: constructed", driver_name);
+    }
+}
+
+void CANSensor::init(uint8_t driver_index, bool enable_filters)
+{
+    _driver_index = driver_index;
+
+    debug_can(AP_CANManager::LOG_INFO, "starting init");
+
+    if (_initialized) {
+        debug_can(AP_CANManager::LOG_ERROR, "already initialized");
+        return;
+    }
+
+    // get CAN manager instance
+    _can_driver = AP::can().get_driver(driver_index);
+
+    if (_can_driver == nullptr) {
+        debug_can(AP_CANManager::LOG_ERROR, "no CAN driver");
+        return;
+    }
+
+    // start thread for receiving and sending CAN frames
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&CANSensor::loop, void), _driver_name, _stack_size, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
+        debug_can(AP_CANManager::LOG_ERROR, "couldn't create thread");
+        return;
+    }
+
+    _initialized = true;
+
+    debug_can(AP_CANManager::LOG_INFO, "init done");
+
+    return;
+}
+
+bool CANSensor::add_interface(AP_HAL::CANIface* can_iface)
+{
+    if (_can_iface != nullptr) {
+        debug_can(AP_CANManager::LOG_ERROR, "Multiple Interface not supported");
+        return false;
+    }
+
+    _can_iface = can_iface;
+
+    if (_can_iface == nullptr) {
+        debug_can(AP_CANManager::LOG_ERROR, "CAN driver not found");
+        return false;
+    }
+
+    if (!_can_iface->is_initialized()) {
+        debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized");
+        return false;
+    }
+
+    if (!_can_iface->set_event_handle(&_event_handle)) {
+        debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle");
+        return false;
+    }
+    return true;
+}
+
+void CANSensor::loop()
+{
+    while (!hal.scheduler->is_system_initialized()) {
+        // don't process packets till startup complete
+        hal.scheduler->delay(1);
+    }
+    const uint32_t LOOP_INTERVAL_US = AP::scheduler().get_loop_period_us();
+    while (true) {
+        uint64_t timeout = AP_HAL::micros64() + LOOP_INTERVAL_US;
+
+        // wait to receive frame
+        bool read_select = true;
+        bool write_select = false;
+        bool ret = _can_iface->select(read_select, write_select, nullptr, timeout);
+        if (ret && read_select) {
+            uint64_t time;
+            AP_HAL::CANIface::CanIOFlags flags {};
+
+            AP_HAL::CANFrame frame;
+            int16_t res = _can_iface->receive(frame, time, flags);
+
+            if (res == 1) {
+                handle_frame(frame);
+            }
+        }
+    }
+}
+
+#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS
+

libraries/AP_CANManager/AP_CANSensor.h

@@ -0,0 +1,52 @@
+/*
+   This program 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 program 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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  CANSensor class, for easy creation of CAN sensors using custom CAN protocols
+ */
+ 
+#pragma once
+
+#include "AP_CANManager.h"
+
+#if HAL_MAX_CAN_PROTOCOL_DRIVERS
+
+class CANSensor : public AP_CANDriver {
+public:
+    CANSensor(const char *driver_name, AP_CANManager::Driver_Type dtype, uint16_t stack_size=2048);
+    
+    /* Do not allow copies */
+    CANSensor(const CANSensor &other) = delete;
+    CANSensor &operator=(const CANSensor&) = delete;
+
+    void init(uint8_t driver_index, bool enable_filters) override;
+    bool add_interface(AP_HAL::CANIface* can_iface) override;
+
+    // handler for incoming frames
+    virtual void handle_frame(AP_HAL::CANFrame &frame) = 0;
+
+private:
+    void loop();
+
+    const char *const _driver_name;
+    const uint16_t _stack_size;
+    bool _initialized;
+    uint8_t _driver_index;
+    AP_CANDriver *_can_driver;
+    HAL_EventHandle _event_handle;
+    AP_HAL::CANIface* _can_iface;
+};
+
+#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS
+