diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index d06f742428..7dbbd5b412 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -18,12 +18,12 @@ #include #include -#if HAL_WITH_UAVCAN +#if HAL_MAX_CAN_PROTOCOL_DRIVERS #include "AP_UAVCAN.h" #include #include -#include +#include #include @@ -58,7 +58,7 @@ extern const AP_HAL::HAL& hal; -#define debug_uavcan(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0) +#define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0) // Translation of all messages from UAVCAN structures into AP structures is done // in AP_UAVCAN and not in corresponding drivers. @@ -104,30 +104,30 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { #define CAN_PERIODIC_TX_TIMEOUT_MS 2 // publisher interfaces -static uavcan::Publisher* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS]; -static uavcan::Publisher* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS]; -static uavcan::Publisher* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS]; -static uavcan::Publisher* buzzer[MAX_NUMBER_OF_CAN_DRIVERS]; -static uavcan::Publisher* safety_state[MAX_NUMBER_OF_CAN_DRIVERS]; -static uavcan::Publisher* rtcm_stream[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Publisher* act_out_array[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* esc_raw[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS]; // subscribers // handler SafteyButton UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button); -static uavcan::Subscriber *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Subscriber *safety_button_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; // handler TrafficReport UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport); -static uavcan::Subscriber *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Subscriber *traffic_report_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; // handler actuator status UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status); -static uavcan::Subscriber *actuator_status_listener[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Subscriber *actuator_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; // handler ESC status UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status); -static uavcan::Subscriber *esc_status_listener[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Subscriber *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[]; HAL_Semaphore AP_UAVCAN::_telem_sem; @@ -143,7 +143,7 @@ AP_UAVCAN::AP_UAVCAN() : _SRV_conf[i].servo_pending = false; } - debug_uavcan(2, "AP_UAVCAN constructed\n\r"); + debug_uavcan(AP_CANManager::LOG_INFO, "AP_UAVCAN constructed\n\r"); } AP_UAVCAN::~AP_UAVCAN() @@ -153,47 +153,53 @@ AP_UAVCAN::~AP_UAVCAN() AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index) { if (driver_index >= AP::can().get_num_drivers() || - AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_UAVCAN) { + AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_UAVCAN) { return nullptr; } return static_cast(AP::can().get_driver(driver_index)); } +bool AP_UAVCAN::add_interface(AP_HAL::CANIface* can_iface) { + + if (_iface_mgr == nullptr) { + _iface_mgr = new uavcan::CanIfaceMgr(); + } + + if (_iface_mgr == nullptr) { + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't create UAVCAN interface manager\n\r"); + return false; + } + + if (!_iface_mgr->add_interface(can_iface)) { + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't add UAVCAN interface\n\r"); + return false; + } + return true; +} + void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) { - if (_initialized) { - debug_uavcan(2, "UAVCAN: init called more than once\n\r"); - return; - } - _driver_index = driver_index; - AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index]; - if (can_mgr == nullptr) { - debug_uavcan(2, "UAVCAN: init called for inexisting CAN driver\n\r"); + if (_initialized) { + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: init called more than once\n\r"); return; } - if (!can_mgr->is_initialized()) { - debug_uavcan(1, "UAVCAN: CAN driver not initialized\n\r"); + if (_iface_mgr == nullptr) { + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't get UAVCAN interface driver\n\r"); return; } - uavcan::ICanDriver* driver = can_mgr->get_driver(); - if (driver == nullptr) { - debug_uavcan(2, "UAVCAN: can't get UAVCAN interface driver\n\r"); - return; - } - - _node = new uavcan::Node<0>(*driver, SystemClock::instance(), _node_allocator); + _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), _node_allocator); if (_node == nullptr) { - debug_uavcan(1, "UAVCAN: couldn't allocate node\n\r"); + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node\n\r"); return; } if (_node->isStarted()) { - debug_uavcan(2, "UAVCAN: node was already started?\n\r"); + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node was already started?\n\r"); return; } @@ -232,13 +238,13 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) int start_res = _node->start(); if (start_res < 0) { - debug_uavcan(1, "UAVCAN: node start problem, error %d\n\r", start_res); + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node start problem, error %d\n\r", start_res); return; } //Start Servers if (!AP::uavcan_dna_server().init(this)) { - debug_uavcan(1, "UAVCAN: Failed to start DNA Server\n\r"); + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: Failed to start DNA Server\n\r"); return; } @@ -314,12 +320,12 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { _node->setModeOfflineAndPublish(); - debug_uavcan(1, "UAVCAN: couldn't create thread\n\r"); + debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't create thread\n\r"); return; } _initialized = true; - debug_uavcan(2, "UAVCAN: init done\n\r"); + debug_uavcan(AP_CANManager::LOG_INFO, "UAVCAN: init done\n\r"); } // send ESC telemetry messages over MAVLink @@ -413,7 +419,7 @@ void AP_UAVCAN::loop(void) if (_servo_bm > 0) { // if we have any Servos in bitmask - uint32_t now = AP_HAL::micros(); + uint32_t now = AP_HAL::native_micros(); const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); if (now - _SRV_last_send_us >= servo_period_us) { _SRV_last_send_us = now; @@ -560,7 +566,7 @@ void AP_UAVCAN::SRV_push_servos() void AP_UAVCAN::led_out_send() { - uint64_t now = AP_HAL::micros64(); + uint64_t now = AP_HAL::native_micros64(); if ((now - _led_conf.last_update) < LED_DELAY_US) { return; @@ -655,7 +661,7 @@ void AP_UAVCAN::rtcm_stream_send() // nothing to send return; } - uint32_t now = AP_HAL::millis(); + uint32_t now = AP_HAL::native_millis(); if (now - _rtcm_stream.last_send_ms < 20) { // don't send more than 50 per second return; @@ -681,7 +687,7 @@ void AP_UAVCAN::rtcm_stream_send() void AP_UAVCAN::safety_state_send() { ardupilot::indication::SafetyState msg; - uint32_t now = AP_HAL::millis(); + uint32_t now = AP_HAL::native_millis(); if (now - _last_safety_state_ms < 500) { // update at 2Hz return; @@ -802,7 +808,7 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con pkt.flags |= 0x100; } - vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000); + vehicle.last_update_ms = AP_HAL::native_millis() - (vehicle.info.tslc * 1000); adsb->handle_adsb_vehicle(vehicle); } @@ -812,7 +818,7 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb) { // log as CSRV message - AP::logger().Write_ServoStatus(AP_HAL::micros64(), + AP::logger().Write_ServoStatus(AP_HAL::native_micros64(), cb.msg->actuator_id, cb.msg->position, cb.msg->force, @@ -828,8 +834,8 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E const uint8_t esc_index = cb.msg->esc_index; // log as CESC message - AP::logger().Write_ESCStatus(AP_HAL::micros64(), - esc_index, + AP::logger().Write_ESCStatus(AP_HAL::native_micros64(), + cb.msg->esc_index, cb.msg->error_count, cb.msg->voltage, cb.msg->current, @@ -861,4 +867,4 @@ bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) { return true; } -#endif // HAL_WITH_UAVCAN +#endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index d6f1954fc1..4698cf1c1c 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -19,8 +19,9 @@ #include #include "AP_UAVCAN_DNA_Server.h" - -#include +#include "AP_UAVCAN_IfaceMgr.h" +#include "AP_UAVCAN_Clock.h" +#include #include #include @@ -66,7 +67,7 @@ class ESCStatusCb; RegistryBinder(uc, (Registry)ffunc) {} \ } -class AP_UAVCAN : public AP_HAL::CANProtocol { +class AP_UAVCAN : public AP_CANDriver { public: AP_UAVCAN(); ~AP_UAVCAN(); @@ -77,6 +78,7 @@ public: static AP_UAVCAN *get_uavcan(uint8_t driver_index); void init(uint8_t driver_index, bool enable_filters) override; + bool add_interface(AP_HAL::CANIface* can_iface) override; // send ESC telemetry messages over MAVLink void send_esc_telemetry_mavlink(uint8_t mav_chan); @@ -124,31 +126,6 @@ public: }; private: - class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable { - public: - SystemClock() = default; - - void adjustUtc(uavcan::UtcDuration adjustment) override { - utc_adjustment_usec = adjustment.toUSec(); - } - - uavcan::MonotonicTime getMonotonic() const override { - return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64()); - } - - uavcan::UtcTime getUtc() const override { - return uavcan::UtcTime::fromUSec(AP_HAL::micros64() + utc_adjustment_usec); - } - - static SystemClock& instance() { - static SystemClock inst; - return inst; - } - - private: - int64_t utc_adjustment_usec; - }; - // This will be needed to implement if UAVCAN is used with multithreading // Such cases will be firmware update, etc. class RaiiSynchronizer {}; @@ -182,6 +159,8 @@ private: uavcan::Node<0> *_node; uint8_t _driver_index; + + uavcan::CanIfaceMgr* _iface_mgr; char _thread_name[13]; bool _initialized; ///// SRV output ///// diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h b/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h new file mode 100644 index 0000000000..eba4923a3c --- /dev/null +++ b/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h @@ -0,0 +1,59 @@ +/* + * 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 . + * + * Author: Siddharth Bharat Purohit + */ + + +#pragma once + +#include + +namespace uavcan +{ +class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable +{ +public: + SystemClock() = default; + + void adjustUtc(uavcan::UtcDuration adjustment) override + { + utc_adjustment_usec = adjustment.toUSec(); + } + + uavcan::MonotonicTime getMonotonic() const override + { + return uavcan::MonotonicTime::fromUSec(AP_HAL::native_micros64()); + } + + uavcan::UtcTime getUtc() const override + { + return uavcan::UtcTime::fromUSec(AP_HAL::native_micros64() + utc_adjustment_usec); + } + + int64_t getAdjustUsec() const + { + return utc_adjustment_usec; + } + + static SystemClock& instance() + { + static SystemClock inst; + return inst; + } + +private: + int64_t utc_adjustment_usec; +}; +} \ No newline at end of file diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp index e722566b2a..be079a7b9e 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp @@ -18,7 +18,7 @@ #include -#if HAL_WITH_UAVCAN +#if HAL_MAX_CAN_PROTOCOL_DRIVERS #include "AP_UAVCAN_DNA_Server.h" #include "AP_UAVCAN.h" @@ -27,14 +27,14 @@ #include #include #include - +#include "AP_UAVCAN_Clock.h" extern const AP_HAL::HAL& hal; #define NODEDATA_MAGIC 0xAC01 #define NODEDATA_MAGIC_LEN 2 #define MAX_NODE_ID 125 -#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) +#define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0) //Callback Object Definitions UC_REGISTRY_BINDER(AllocationCb, uavcan::protocol::dynamic_node_id::Allocation); @@ -44,9 +44,9 @@ static void trampoline_handleNodeInfo(const uavcan::ServiceCallResult* getNodeInfo_client[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::ServiceClient* getNodeInfo_client[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* allocation_pub[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Publisher* allocation_pub[HAL_MAX_CAN_PROTOCOL_DRIVERS]; /* Subscribe to all the messages we are going to handle for Server registry and Node allocation. */ @@ -422,14 +422,14 @@ void AP_UAVCAN_DNA_Server::verify_nodes(AP_UAVCAN *ap_uavcan) { WITH_SEMAPHORE(sem); - uint32_t now = AP_HAL::millis(); + uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec(); if ((now - last_verification_request) < 5000) { return; } //Check if we got acknowledgement from previous request //except for requests using our own node_id - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + for (uint8_t i = 0; i < HAL_MAX_CAN_PROTOCOL_DRIVERS; i++) { if (curr_verifying_node == self_node_id[i]) { nodeInfo_resp_rcvd = true; } @@ -455,7 +455,7 @@ void AP_UAVCAN_DNA_Server::verify_nodes(AP_UAVCAN *ap_uavcan) break; } } - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + for (uint8_t i = 0; i < HAL_MAX_CAN_PROTOCOL_DRIVERS; i++) { if (getNodeInfo_client[i] != nullptr && isNodeIDOccupied(curr_verifying_node)) { uavcan::protocol::GetNodeInfo::Request request; getNodeInfo_client[i]->call(curr_verifying_node, request); @@ -475,7 +475,7 @@ void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb WITH_SEMAPHORE(sem); if (!isNodeIDVerified(node_id)) { //immediately begin verification of the node_id - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + for (uint8_t i = 0; i < HAL_MAX_CAN_PROTOCOL_DRIVERS; i++) { if (getNodeInfo_client[i] != nullptr) { uavcan::protocol::GetNodeInfo::Request request; getNodeInfo_client[i]->call(node_id, request); @@ -569,7 +569,7 @@ void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t driver_index, uint8_t node_i //Ignore Allocation messages that are not DNA requests return; } - uint32_t now = AP_HAL::millis(); + uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec(); if (driver_index == current_driver_index) { last_activity_ms = now; } else if ((now - last_activity_ms) > 500) { @@ -670,8 +670,9 @@ namespace AP { AP_UAVCAN_DNA_Server& uavcan_dna_server() { - static AP_UAVCAN_DNA_Server _server(StorageAccess(StorageManager::StorageCANDNA)); + // clang gets confused with only one pair of braces so we add one more pair + static AP_UAVCAN_DNA_Server _server((StorageAccess(StorageManager::StorageCANDNA))); return _server; } } -#endif //HAL_WITH_UAVCAN +#endif //HAL_NUM_CAN_IFACES diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h index a8ecd340a3..749b2cd2c1 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h @@ -1,7 +1,7 @@ #pragma once #include -#if HAL_WITH_UAVCAN +#if HAL_MAX_CAN_PROTOCOL_DRIVERS #include #include #include @@ -30,7 +30,7 @@ class AP_UAVCAN_DNA_Server uint32_t last_verification_request; uint8_t curr_verifying_node; - uint8_t self_node_id[MAX_NUMBER_OF_CAN_DRIVERS]; + uint8_t self_node_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; bool nodeInfo_resp_rcvd; Bitmask<128> occupation_mask; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp new file mode 100644 index 0000000000..f7f9b8cb57 --- /dev/null +++ b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp @@ -0,0 +1,259 @@ + +/* + * 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 . + * + * Author: Siddharth Bharat Purohit + */ + +#include "AP_UAVCAN_IfaceMgr.h" + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS +#include "AP_UAVCAN_Clock.h" +#include +#include +using namespace uavcan; +extern const AP_HAL::HAL& hal; +#define LOG_TAG "UAVCANIface" + +/***************************************************** + * * + * CAN Iface * + * * + * ***************************************************/ + +/** + * Non-blocking transmission. + * + * If the frame wasn't transmitted upon TX deadline, the driver should discard it. + * + * Note that it is LIKELY that the library will want to send the frames that were passed into the select() + * method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new + * frames between the calls. + * + * @return 1 = one frame transmitted, 0 = TX buffer full, negative for error. + */ +int16_t CanIface::send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) +{ + if (can_iface_ == UAVCAN_NULLPTR) { + return -1; + } + return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, frame.dlc), tx_deadline.toUSec(), flags); +} + +/** + * Non-blocking reception. + * + * Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller. + * + * Monotonic timestamp is required and can be not precise since it is needed only for + * protocol timing validation (transfer timeouts and inter-transfer intervals). + * + * UTC timestamp is optional, if available it will be used for precise time synchronization; + * must be set to zero if not available. + * + * Refer to @ref ISystemClock to learn more about timestamps. + * + * @param [out] out_ts_monotonic Monotonic timestamp, mandatory. + * @param [out] out_ts_utc UTC timestamp, optional, zero if unknown. + * @return 1 = one frame received, 0 = RX buffer empty, negative for error. + */ +int16_t CanIface::receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc, + CanIOFlags& out_flags) +{ + + if (can_iface_ == UAVCAN_NULLPTR) { + return -1; + } + AP_HAL::CANFrame frame; + uint64_t rx_timestamp; + uint16_t flags; + out_ts_monotonic = SystemClock::instance().getMonotonic(); + int16_t ret = can_iface_->receive(frame, rx_timestamp, flags); + if (ret < 0) { + return ret; + } + out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, frame.dlc); + out_flags = flags; + if (rx_timestamp != 0) { + out_ts_utc = uavcan::UtcTime::fromUSec(SystemClock::instance().getAdjustUsec() + rx_timestamp); + } else { + out_ts_utc = uavcan::UtcTime::fromUSec(0); + } + return ret; +} + +/** + * Configure the hardware CAN filters. @ref CanFilterConfig. + * + * @return 0 = success, negative for error. + */ +int16_t CanIface::configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs) +{ + if (can_iface_ == UAVCAN_NULLPTR) { + return -1; + } + AP_HAL::CANIface::CanFilterConfig* hal_filter_configs = new AP_HAL::CANIface::CanFilterConfig[num_configs]; + if (hal_filter_configs == nullptr) { + return -1; + } + for (uint16_t i = 0; i < num_configs; i++) { + hal_filter_configs[i].id = filter_configs[i].id; + hal_filter_configs[i].mask = filter_configs[i].mask; + } + return can_iface_->configureFilters(hal_filter_configs, num_configs); +} + +/** + * Number of available hardware filters. + */ +uint16_t CanIface::getNumFilters() const +{ + if (can_iface_ == UAVCAN_NULLPTR) { + return 0; + } + return can_iface_->getNumFilters(); +} + +/** + * Continuously incrementing counter of hardware errors. + * Arbitration lost should not be treated as a hardware error. + */ +uint64_t CanIface::getErrorCount() const +{ + if (can_iface_ == UAVCAN_NULLPTR) { + return 0; + } + return can_iface_->getErrorCount(); +} + +/***************************************************** + * * + * CAN Driver * + * * + * ***************************************************/ + +bool CanIfaceMgr::add_interface(AP_HAL::CANIface *can_iface) +{ + if (num_ifaces > HAL_NUM_CAN_IFACES) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Num Ifaces Exceeded\n"); + return false; + } + if (can_iface == nullptr) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface Null\n"); + return false; + } + if (ifaces[num_ifaces] != nullptr) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface already added\n"); + return false; + } + ifaces[num_ifaces] = new CanIface(can_iface); + if (ifaces[num_ifaces] == nullptr) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Can't alloc uavcan::iface\n"); + return false; + } + if (!ifaces[num_ifaces]->can_iface_->set_event_handle(&_event_handle)) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Setting event handle failed\n"); + return false; + } + AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "UAVCANIfaceMgr: Successfully added interface %d\n"); + num_ifaces++; + return true; +} +/** + * Returns an interface by index, or null pointer if the index is out of range. + */ +ICanIface* CanIfaceMgr::getIface(uint8_t iface_index) +{ + if (iface_index >= num_ifaces) { + return UAVCAN_NULLPTR; + } + return ifaces[iface_index]; +} + +/** + * Total number of available CAN interfaces. + * This value shall not change after initialization. + */ +uint8_t CanIfaceMgr::getNumIfaces() const +{ + return num_ifaces; +} + +CanSelectMasks CanIfaceMgr::makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const +{ + CanSelectMasks msk; + for (uint8_t i = 0; i < num_ifaces; i++) { + bool read = in_mask.read & (1 << i); + bool write = in_mask.write & (1 << i); + CanIface* iface = ifaces[i]; + if (iface == nullptr) { + continue; + } + if (pending_tx[i] == UAVCAN_NULLPTR) { + if (iface->can_iface_->select(read, write, nullptr, 0)) { + msk.read |= (read ? 1 : 0) << i; + msk.write |= (write ? 1 : 0) << i; + } + } else { + AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, pending_tx[i]->dlc}; + if (iface->can_iface_->select(read, write, &frame, 0)) { + msk.read |= (read ? 1 : 0) << i; + msk.write |= (write ? 1 : 0) << i; + } + } + } + + return msk; +} + +/** + * Block until the deadline, or one of the specified interfaces becomes available for read or write. + * + * Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO. + * + * Bit position in the masks defines interface index. + * + * Note that it is allowed to return from this method even if no requested events actually happened, or if + * there are events that were not requested by the library. + * + * The pending TX argument contains an array of pointers to CAN frames that the library wants to transmit + * next, per interface. This is intended to allow the driver to properly prioritize transmissions; many + * drivers will not need to use it. If a write flag for the given interface is set to one in the select mask + * structure, then the corresponding pointer is guaranteed to be valid (not UAVCAN_NULLPTR). + * + * @param [in,out] inout_masks Masks indicating which interfaces are needed/available for IO. + * @param [in] pending_tx Array of frames, per interface, that are likely to be transmitted next. + * @param [in] blocking_deadline Zero means non-blocking operation. + * @return Positive number of ready interfaces or negative error code. + */ +int16_t CanIfaceMgr::select(CanSelectMasks& inout_masks, + const CanFrame* (& pending_tx)[MaxCanIfaces], + const MonotonicTime blocking_deadline) +{ + const CanSelectMasks in_masks = inout_masks; + const uint64_t time = SystemClock::instance().getMonotonic().toUSec(); + + inout_masks = makeSelectMasks(in_masks, pending_tx); // Check if we already have some of the requested events + if ((inout_masks.read & in_masks.read) != 0 || + (inout_masks.write & in_masks.write) != 0) { + return 1; + } + if (time < blocking_deadline.toUSec()) { + _event_handle.wait(blocking_deadline.toUSec() - time); // Block until timeout expires or any iface updates + } + + inout_masks = makeSelectMasks(in_masks, pending_tx); // Return what we got even if none of the requested events are set + return 1; // Return value doesn't matter as long as it is non-negative +} +#endif diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h new file mode 100644 index 0000000000..5c693b97eb --- /dev/null +++ b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h @@ -0,0 +1,73 @@ +/* + * 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 . + * + * Author: Siddharth Bharat Purohit + */ +#pragma once + +#include +#if HAL_MAX_CAN_PROTOCOL_DRIVERS + +#include + +namespace uavcan +{ + +class CanDriver; + +class CanIface : public ICanIface, Noncopyable +{ + friend class CanIfaceMgr; + AP_HAL::CANIface* can_iface_; +public: + CanIface(AP_HAL::CANIface *can_iface) : can_iface_(can_iface) {} + + virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline, + CanIOFlags flags) override; + + virtual int16_t receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, + UtcTime& out_ts_utc, CanIOFlags& out_flags) override; + + int16_t configureFilters(const CanFilterConfig* filter_configs, + uint16_t num_configs) override; + + uint16_t getNumFilters() const override; + + uint64_t getErrorCount() const override; +}; + +/** + * Generic CAN driver. + */ +class CanIfaceMgr : public ICanDriver, Noncopyable +{ + CanIface* ifaces[HAL_NUM_CAN_IFACES]; + uint8_t num_ifaces; + HAL_EventHandle _event_handle; +public: + bool add_interface(AP_HAL::CANIface *can_drv); + + ICanIface* getIface(uint8_t iface_index) override; + + uint8_t getNumIfaces() const override; + + CanSelectMasks makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const; + + int16_t select(CanSelectMasks& inout_masks, + const CanFrame* (& pending_tx)[MaxCanIfaces], + const MonotonicTime blocking_deadline) override; +}; + +} +#endif diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp deleted file mode 100644 index 2fefbc870a..0000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp +++ /dev/null @@ -1,574 +0,0 @@ -/* - * 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 . - * - * Author: Siddharth Bharat Purohit - * Referenced from implementation by Pavel Kirienko - * for Zubax Babel - */ - -#include - -#if AP_UAVCAN_SLCAN_ENABLED - -#include "AP_UAVCAN_SLCAN.h" -#include - -#include - -extern const AP_HAL::HAL& hal; - -static uint8_t nibble2hex(uint8_t x) -{ - // Allocating in RAM because it's faster - static uint8_t ConversionTable[] = { - '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' - }; - return ConversionTable[x & 0x0F]; -} - -static bool hex2nibble_error; - -static uint8_t hex2nibble(char c) -{ - // Must go into RAM, not flash, because flash is slow - static uint8_t NumConversionTable[] = { - 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 - }; - - static uint8_t AlphaConversionTable[] = { - 10, 11, 12, 13, 14, 15 - }; - - uint8_t out = 255; - - if (c >= '0' && c <= '9') { - out = NumConversionTable[int(c) - int('0')]; - } - else if (c >= 'a' && c <= 'f') { - out = AlphaConversionTable[int(c) - int('a')]; - } - else if (c >= 'A' && c <= 'F') { - out = AlphaConversionTable[int(c) - int('A')]; - } - - if (out == 255) { - hex2nibble_error = true; - } - return out; -} - - -bool SLCAN::CAN::push_Frame(uavcan::CanFrame &frame) -{ - SLCAN::CanRxItem frm; - frm.frame = frame; - frm.flags = 0; - frm.utc_usec = AP_HAL::micros64(); - ChibiOS_CAN::CanIface::slcan_router().route_frame_to_can(frm.frame, frm.utc_usec); - return rx_queue_.push(frm); -} - -/** - * General frame format: - * - * The emitting functions below are highly optimized for speed. - */ -bool SLCAN::CAN::handle_FrameDataExt(const char* cmd) -{ - uavcan::CanFrame f; - hex2nibble_error = false; - f.id = f.FlagEFF | - (hex2nibble(cmd[1]) << 28) | - (hex2nibble(cmd[2]) << 24) | - (hex2nibble(cmd[3]) << 20) | - (hex2nibble(cmd[4]) << 16) | - (hex2nibble(cmd[5]) << 12) | - (hex2nibble(cmd[6]) << 8) | - (hex2nibble(cmd[7]) << 4) | - (hex2nibble(cmd[8]) << 0); - if (cmd[9] < '0' || cmd[9] > ('0' + uavcan::CanFrame::MaxDataLen)) { - return false; - } - f.dlc = cmd[9] - '0'; - if (f.dlc > uavcan::CanFrame::MaxDataLen) { - return false; - } - { - const char* p = &cmd[10]; - for (unsigned i = 0; i < f.dlc; i++) { - f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1)); - p += 2; - } - } - if (hex2nibble_error) { - return false; - } - return push_Frame(f); -} - -bool SLCAN::CAN::handle_FrameDataStd(const char* cmd) -{ - uavcan::CanFrame f; - hex2nibble_error = false; - f.id = (hex2nibble(cmd[1]) << 8) | - (hex2nibble(cmd[2]) << 4) | - (hex2nibble(cmd[3]) << 0); - if (cmd[4] < '0' || cmd[4] > ('0' + uavcan::CanFrame::MaxDataLen)) { - return false; - } - f.dlc = cmd[4] - '0'; - if (f.dlc > uavcan::CanFrame::MaxDataLen) { - return false; - } - { - const char* p = &cmd[5]; - for (unsigned i = 0; i < f.dlc; i++) { - f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1)); - p += 2; - } - } - if (hex2nibble_error) { - return false; - } - return push_Frame(f); -} - -bool SLCAN::CAN::handle_FrameRTRExt(const char* cmd) -{ - uavcan::CanFrame f; - hex2nibble_error = false; - f.id = f.FlagEFF | f.FlagRTR | - (hex2nibble(cmd[1]) << 28) | - (hex2nibble(cmd[2]) << 24) | - (hex2nibble(cmd[3]) << 20) | - (hex2nibble(cmd[4]) << 16) | - (hex2nibble(cmd[5]) << 12) | - (hex2nibble(cmd[6]) << 8) | - (hex2nibble(cmd[7]) << 4) | - (hex2nibble(cmd[8]) << 0); - if (cmd[9] < '0' || cmd[9] > ('0' + uavcan::CanFrame::MaxDataLen)) { - return false; - } - f.dlc = cmd[9] - '0'; - - if (f.dlc > uavcan::CanFrame::MaxDataLen) { - return false; - } - if (hex2nibble_error) { - return false; - } - return push_Frame(f); -} - -bool SLCAN::CAN::handle_FrameRTRStd(const char* cmd) -{ - uavcan::CanFrame f; - hex2nibble_error = false; - f.id = f.FlagRTR | - (hex2nibble(cmd[1]) << 8) | - (hex2nibble(cmd[2]) << 4) | - (hex2nibble(cmd[3]) << 0); - if (cmd[4] < '0' || cmd[4] > ('0' + uavcan::CanFrame::MaxDataLen)) { - return false; - } - f.dlc = cmd[4] - '0'; - if (f.dlc <= uavcan::CanFrame::MaxDataLen) { - return false; - } - if (hex2nibble_error) { - return false; - } - return push_Frame(f); -} - -static inline const char* getASCIIStatusCode(bool status) -{ - return status ? "\r" : "\a"; -} - - -bool SLCAN::CANManager::begin(uint32_t bitrate, uint8_t can_number) -{ - if (driver_.init(bitrate, SLCAN::CAN::NormalMode, nullptr) < 0) { - return false; - } - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&SLCAN::CANManager::reader_trampoline, void), "SLCAN", 4096, AP_HAL::Scheduler::PRIORITY_CAN, -1)) { - return false; - } - initialized(true); - return true; -} - -bool SLCAN::CANManager::is_initialized() -{ - return initialized_; -} - -void SLCAN::CANManager::initialized(bool val) -{ - initialized_ = val; -} - -int SLCAN::CAN::init(const uint32_t bitrate, const OperatingMode mode, AP_HAL::UARTDriver* port) -{ - if (port == nullptr) { - return -1; - } - _port = port; - initialized_ = true; - return 0; -} - -/** - * General frame format: - * [timestamp msec] [flags] - * Types: - * R - RTR extended - * r - RTR standard - * T - Data extended - * t - Data standard - * Flags: - * L - this frame is a loopback frame; timestamp field contains TX timestamp - */ -int16_t SLCAN::CAN::reportFrame(const uavcan::CanFrame& frame, bool loopback, uint64_t timestamp_usec) -{ - constexpr unsigned SLCANMaxFrameSize = 40; - uint8_t buffer[SLCANMaxFrameSize] = {'\0'}; - uint8_t* p = &buffer[0]; - /* - * Frame type - */ - if (frame.isRemoteTransmissionRequest()) { - *p++ = frame.isExtended() ? 'R' : 'r'; - } - else if (frame.isErrorFrame()) { - return -1; // Not supported - } - else { - *p++ = frame.isExtended() ? 'T' : 't'; - } - - /* - * ID - */ - { - const uint32_t id = frame.id & frame.MaskExtID; - if (frame.isExtended()) { - *p++ = nibble2hex(id >> 28); - *p++ = nibble2hex(id >> 24); - *p++ = nibble2hex(id >> 20); - *p++ = nibble2hex(id >> 16); - *p++ = nibble2hex(id >> 12); - } - *p++ = nibble2hex(id >> 8); - *p++ = nibble2hex(id >> 4); - *p++ = nibble2hex(id >> 0); - } - - /* - * DLC - */ - *p++ = char('0' + frame.dlc); - - /* - * Data - */ - for (unsigned i = 0; i < frame.dlc; i++) { - const uint8_t byte = frame.data[i]; - *p++ = nibble2hex(byte >> 4); - *p++ = nibble2hex(byte); - } - - /* - * Timestamp - */ - //if (param_cache.timestamping_on) - { - // SLCAN format - [0, 60000) milliseconds - const auto slcan_timestamp = uint16_t(timestamp_usec / 1000U); - *p++ = nibble2hex(slcan_timestamp >> 12); - *p++ = nibble2hex(slcan_timestamp >> 8); - *p++ = nibble2hex(slcan_timestamp >> 4); - *p++ = nibble2hex(slcan_timestamp >> 0); - } - - /* - * Flags - */ - //if (param_cache.flags_on) - { - if (loopback) { - *p++ = 'L'; - } - } - - /* - * Finalization - */ - *p++ = '\r'; - const auto frame_size = unsigned(p - &buffer[0]); - - if (_port->txspace() < _pending_frame_size) { - _pending_frame_size = frame_size; - return 0; - } - //Write to Serial - if (!_port->write_locked(&buffer[0], frame_size, _serial_lock_key)) { - return 0; - } - return 1; -} - - -/** - * Accepts command string, returns response string or nullptr if no response is needed. - */ -const char* SLCAN::CAN::processCommand(char* cmd) -{ - /* - * High-traffic SLCAN commands go first - */ - if (cmd[0] == 'T') { - return handle_FrameDataExt(cmd) ? "Z\r" : "\a"; - } - else if (cmd[0] == 't') { - return handle_FrameDataStd(cmd) ? "z\r" : "\a"; - } - else if (cmd[0] == 'R') { - return handle_FrameRTRExt(cmd) ? "Z\r" : "\a"; - } - else if (cmd[0] == 'r' && cmd[1] <= '9') { // The second condition is needed to avoid greedy matching - // See long commands below - return handle_FrameRTRStd(cmd) ? "z\r" : "\a"; - } - - /* - * Regular SLCAN commands - */ - switch (cmd[0]) { - case 'S': // Set CAN bitrate - case 'O': // Open CAN in normal mode - case 'L': // Open CAN in listen-only mode - case 'l': { // Open CAN with loopback enabled - _close = false; - return getASCIIStatusCode(true); // Returning success for compatibility reasons - } - case 'C': { // Close CAN - _close = true; - return getASCIIStatusCode(true); // Returning success for compatibility reasons - } - case 'M': // Set CAN acceptance filter ID - case 'm': // Set CAN acceptance filter mask - case 'U': // Set UART baud rate, see http://www.can232.com/docs/can232_v3.pdf - case 'Z': { // Enable/disable RX and loopback timestamping - return getASCIIStatusCode(true); // Returning success for compatibility reasons - } - case 'F': { // Get status flags - _port->printf("F%02X\r", unsigned(0)); // Returning success for compatibility reasons - return nullptr; - } - case 'V': { // HW/SW version - _port->printf("V%x%x%x%x\r", AP_UAVCAN_HW_VERS_MAJOR, AP_UAVCAN_HW_VERS_MINOR, AP_UAVCAN_SW_VERS_MAJOR, AP_UAVCAN_SW_VERS_MINOR); - return nullptr; - } - case 'N': { // Serial number - uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion - const uint8_t uid_buf_len = hw_version.unique_id.capacity(); - uint8_t uid_len = uid_buf_len; - uint8_t unique_id[uid_buf_len]; - char buf[uid_buf_len * 2 + 1] = {'\0'}; - char* pos = &buf[0]; - if (hal.util->get_system_id_unformatted(unique_id, uid_len)) { - for (uint8_t i = 0; i < uid_buf_len; i++) { - *pos++ = nibble2hex(unique_id[i] >> 4); - *pos++ = nibble2hex(unique_id[i]); - } - } - *pos++ = '\0'; - _port->printf("N%s\r", &buf[0]); - return nullptr; - } - default: { - break; - } - } - - return getASCIIStatusCode(false); -} - -/** - * Please keep in mind that this function is strongly optimized for speed. - */ -inline void SLCAN::CAN::addByte(const uint8_t byte) -{ - if ((byte >= 32 && byte <= 126)) { // Normal printable ASCII character - if (pos_ < SLCAN_BUFFER_SIZE) { - buf_[pos_] = char(byte); - pos_ += 1; - } - else { - reset(); // Buffer overrun; silently drop the data - } - } - else if (byte == '\r') { // End of command (SLCAN) - // Processing the command - buf_[pos_] = '\0'; - const char* const response = processCommand(reinterpret_cast(&buf_[0])); - reset(); - - // Sending the response if provided - if (response != nullptr) { - _port->write_locked(reinterpret_cast(response), - strlen(response), _serial_lock_key); - } - } - else if (byte == 8 || byte == 127) { // DEL or BS (backspace) - if (pos_ > 0) { - pos_ -= 1; - } - } - else { // This also includes Ctrl+C, Ctrl+D - reset(); // Invalid byte - drop the current command - } -} - -void SLCAN::CAN::reset() -{ - pos_ = 0; -} - - -void SLCAN::CAN::reader() -{ - if (_port == nullptr) { - return; - } - if (!_port_initialised) { - //_port->begin(bitrate_); - _port_initialised = true; - } - _port->lock_port(_serial_lock_key, _serial_lock_key); - if (!_port->wait_timeout(1,1)) { - int16_t data = _port->read_locked(_serial_lock_key); - while (data > 0) { - addByte(data); - data = _port->read_locked(_serial_lock_key); - } - } -} - -int16_t SLCAN::CAN::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) -{ - if (frame.isErrorFrame() || frame.dlc > 8) { - return -ErrUnsupportedFrame; - } - - return reportFrame(frame, flags & uavcan::CanIOFlagLoopback, AP_HAL::micros64()); -} - -int16_t SLCAN::CAN::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) -{ - out_ts_monotonic = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64());; // High precision is not required for monotonic timestamps - uint64_t utc_usec; - CanRxItem frm; - if (!rx_queue_.pop(frm)) { - return 0; - } - out_frame = frm.frame; - utc_usec = frm.utc_usec; - out_flags = frm.flags; - out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec); - return 1; -} - -bool SLCAN::CAN::pending_frame_sent() -{ - if (_pending_frame_size == 0) { - return false; - } - else if (_port->txspace() >= _pending_frame_size) { - _pending_frame_size = 0; - return true; - } - return false; -} - -bool SLCAN::CAN::isRxBufferEmpty() -{ - return rx_queue_.available() == 0; -} - -bool SLCAN::CAN::canAcceptNewTxFrame() const -{ - constexpr unsigned SLCANMaxFrameSize = 40; - if (_port->txspace() >= SLCANMaxFrameSize) { - return true; - } - return false; -} - -uavcan::CanSelectMasks SLCAN::CANManager::makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) -{ - uavcan::CanSelectMasks msk; - - for (uint8_t i = 0; i < _ifaces_num; i++) { - if (!driver_.is_initialized()) { - continue; - } - - if (!driver_.isRxBufferEmpty()) { - msk.read |= 1 << i; - } - - if (pending_tx[i] != nullptr) { - if (driver_.canAcceptNewTxFrame()) { - msk.write |= 1 << i; - } - } - } - - return msk; -} - -int16_t SLCAN::CANManager::select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) -{ - const uavcan::CanSelectMasks in_masks = inout_masks; - const uavcan::MonotonicTime time = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64()); - - inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events - if ((inout_masks.read & in_masks.read) != 0 || (inout_masks.write & in_masks.write) != 0) { - return 1; - } - _irq_handler_ctx = chThdGetSelfX(); - if (blocking_deadline.toUSec()) { - chEvtWaitAnyTimeout(ALL_EVENTS, chTimeUS2I((blocking_deadline - time).toUSec())); // Block until timeout expires or any iface updates - } - inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set - return 1; // Return value doesn't matter as long as it is non-negative -} - -void SLCAN::CANManager::reader_trampoline(void) -{ - while (true) { - driver_.reader(); - if ((driver_.pending_frame_sent() || !driver_.isRxBufferEmpty()) && _irq_handler_ctx) { - chEvtSignalI(_irq_handler_ctx, EVENT_MASK(0)); - } - } -} - -#endif // AP_UAVCAN_SLCAN_ENABLED - diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h deleted file mode 100644 index 7456ef34c0..0000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h +++ /dev/null @@ -1,240 +0,0 @@ - -#pragma once - -#include - -#if AP_UAVCAN_SLCAN_ENABLED - -#include -#include "AP_HAL/utility/RingBuffer.h" - - -#define SLCAN_BUFFER_SIZE 200 -#define SLCAN_RX_QUEUE_SIZE 64 -#define SLCAN_DRIVER_INDEX 2 - -class SLCANRouter; - -namespace SLCAN { -/** - * Driver error codes. - * These values can be returned from driver functions negated. - */ -static const int16_t ErrUnknown = 1000; ///< Reserved for future use -static const int16_t ErrNotImplemented = 1001; ///< Feature not implemented -static const int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported -static const int16_t ErrLogic = 1003; ///< Internal logic error -static const int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc) -static const int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1 -static const int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0 -static const int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished -static const int16_t ErrFilterNumConfigs = 1008; ///< Auto bit rate detection could not be finished -class CANManager; -/** - * RX queue item. - * The application shall not use this directly. - */ -struct CanRxItem { - uint64_t utc_usec; - uavcan::CanFrame frame; - uavcan::CanIOFlags flags; - CanRxItem() : - utc_usec(0), flags(0) - { - } -}; -class CAN: public AP_HAL::CANHal { - friend class CANManager; - friend class ::SLCANRouter; - struct TxItem { - uavcan::MonotonicTime deadline; - uavcan::CanFrame frame; - - bool pending; - - bool loopback; - - bool abort_on_error; - - TxItem() : - pending(false), loopback(false), abort_on_error(false) - { - } - }; - - enum { - NumTxMailboxes = 3 - }; - enum { - NumFilters = 14 - }; - - uint32_t bitrate_; - - virtual int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags) override; - - virtual int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override; - - int16_t reportFrame(const uavcan::CanFrame& frame, bool loopback, uint64_t timestamp_usec); - - virtual int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs) override - { - //TODO: possibly check at the first serial read - return 0; - } - - virtual uint16_t getNumFilters() const override - { - return NumFilters; - } - - /** - * Total number of hardware failures and other kinds of errors (e.g. queue overruns). - * May increase continuously if the interface is not connected to the bus. - */ - virtual uint64_t getErrorCount() const override - { - return 0; - } - - const char* processCommand(char* cmd); - - bool push_Frame(uavcan::CanFrame &frame); - - bool handle_FrameRTRStd(const char* cmd); - bool handle_FrameRTRExt(const char* cmd); - bool handle_FrameDataStd(const char* cmd); - bool handle_FrameDataExt(const char* cmd); - void reader(); - - inline void addByte(const uint8_t byte); - - bool initialized_; - bool _port_initialised; - char buf_[SLCAN_BUFFER_SIZE + 1]; - int16_t pos_ = 0; - AP_HAL::UARTDriver *_port = nullptr; - - ObjectBuffer rx_queue_; - uint8_t self_index_; - HAL_Semaphore rx_sem_; - unsigned _pending_frame_size = 0; - - const uint32_t _serial_lock_key = 0x53494442; - bool _close = true; -public: - - CAN(uint8_t self_index, uint8_t rx_queue_capacity): - self_index_(self_index), rx_queue_(rx_queue_capacity), _port_initialised(false) - { - UAVCAN_ASSERT(self_index_ < CAN_STM32_NUM_IFACES); - } - enum { - MaxRxQueueCapacity = 254 - }; - - enum OperatingMode { - NormalMode, SilentMode - }; - - int init(const uint32_t bitrate, const OperatingMode mode, AP_HAL::UARTDriver* port); - - bool begin(uint32_t bitrate) override - { - if (init(bitrate, OperatingMode::NormalMode, nullptr) == 0) { - bitrate_ = bitrate; - initialized_ = true; - } - else { - initialized_ = false; - } - return initialized_; - } - - void end() override - { - } - - void reset() override; - - int32_t tx_pending() override - { - return _port->tx_pending() ? 0:-1; - } - - int32_t available() override - { - return _port->available() ? 0:-1; - } - - bool is_initialized() override - { - return initialized_; - } - bool closed() - { - return _close; - } - bool pending_frame_sent(); - - bool isRxBufferEmpty(void); - bool canAcceptNewTxFrame() const; -}; - - -class CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver { - bool initialized_; - CAN driver_; - uint8_t _ifaces_num = 1; - - virtual int16_t select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) override; - - uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]); - thread_t *_irq_handler_ctx = nullptr; -public: - CANManager() - : AP_HAL::CANManager(this), initialized_(false), driver_(SLCAN_DRIVER_INDEX, SLCAN_RX_QUEUE_SIZE) - { } - - /** - * Whether at least one iface had at least one successful IO since previous call of this method. - * This is designed for use with iface activity LEDs. - */ - //bool hadActivity(); - - static CANManager *from(AP_HAL::CANManager *can) - { - return static_cast(can); - } - - bool begin(uint32_t bitrate, uint8_t can_number) override; - - /* - Test if CAN manager is ready and initialized - return false - CAN manager not initialized - true - CAN manager is initialized - */ - bool is_initialized() override; - void initialized(bool val) override; - - virtual CAN* getIface(uint8_t iface_index) override - { - return &driver_; - } - - virtual uint8_t getNumIfaces() const override - { - return _ifaces_num; - } - - void reader_trampoline(void); -}; - -} -#include - -#endif // AP_UAVCAN_SLCAN_ENABLED -