diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 33eb68b005..6d9d8b728d 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/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 (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, ...) diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index 46b8ec44b7..a328d3f96e 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/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 diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp new file mode 100644 index 0000000000..a20511e0f8 --- /dev/null +++ b/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 . + */ +/* + generic CAN sensor class, for easy creation of CAN sensors using prioprietary protocols + */ +#include + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS + +#include +#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 + diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h new file mode 100644 index 0000000000..8726d6a089 --- /dev/null +++ b/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 . + */ +/* + 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 +