Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: move all uavcan related methods to AP_UAVCAN

This commit is contained in:
Siddharth Purohit 2020-06-24 17:37:28 +05:30 committed by Andrew Tridgell
parent 65ea8e1f13
commit 07b4708a36
9 changed files with 466 additions and 903 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -18,12 +18,12 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN #if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_UAVCAN.h" #include "AP_UAVCAN.h"
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h> #include <AP_CANManager/AP_CANManager.h>
#include <uavcan/transport/can_acceptance_filter_configurator.hpp> #include <uavcan/transport/can_acceptance_filter_configurator.hpp>
@ -58,7 +58,7 @@
extern const AP_HAL::HAL& hal; 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 // Translation of all messages from UAVCAN structures into AP structures is done
// in AP_UAVCAN and not in corresponding drivers. // 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 #define CAN_PERIODIC_TX_TIMEOUT_MS 2
// publisher interfaces // publisher interfaces
static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// subscribers // subscribers
// handler SafteyButton // handler SafteyButton
UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button); UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button);
static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_button_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// handler TrafficReport // handler TrafficReport
UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport); UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// handler actuator status // handler actuator status
UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status); UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status);
static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb> *actuator_status_listener[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb> *actuator_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// handler ESC status // handler ESC status
UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status); UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[]; AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[];
HAL_Semaphore AP_UAVCAN::_telem_sem; HAL_Semaphore AP_UAVCAN::_telem_sem;
@ -143,7 +143,7 @@ AP_UAVCAN::AP_UAVCAN() :
_SRV_conf[i].servo_pending = false; _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() AP_UAVCAN::~AP_UAVCAN()
@ -153,47 +153,53 @@ AP_UAVCAN::~AP_UAVCAN()
AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index) AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index)
{ {
if (driver_index >= AP::can().get_num_drivers() || 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 nullptr;
} }
return static_cast<AP_UAVCAN*>(AP::can().get_driver(driver_index)); return static_cast<AP_UAVCAN*>(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) 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; _driver_index = driver_index;
AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index]; if (_initialized) {
if (can_mgr == nullptr) { debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: init called more than once\n\r");
debug_uavcan(2, "UAVCAN: init called for inexisting CAN driver\n\r");
return; return;
} }
if (!can_mgr->is_initialized()) { if (_iface_mgr == nullptr) {
debug_uavcan(1, "UAVCAN: CAN driver not initialized\n\r"); debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't get UAVCAN interface driver\n\r");
return; return;
} }
uavcan::ICanDriver* driver = can_mgr->get_driver(); _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
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);
if (_node == nullptr) { 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; return;
} }
if (_node->isStarted()) { 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; return;
} }
@ -232,13 +238,13 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
int start_res = _node->start(); int start_res = _node->start();
if (start_res < 0) { 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; return;
} }
//Start Servers //Start Servers
if (!AP::uavcan_dna_server().init(this)) { 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; 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)) { if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
_node->setModeOfflineAndPublish(); _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; return;
} }
_initialized = true; _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 // send ESC telemetry messages over MAVLink
@ -413,7 +419,7 @@ void AP_UAVCAN::loop(void)
if (_servo_bm > 0) { if (_servo_bm > 0) {
// if we have any Servos in bitmask // 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()); const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
if (now - _SRV_last_send_us >= servo_period_us) { if (now - _SRV_last_send_us >= servo_period_us) {
_SRV_last_send_us = now; _SRV_last_send_us = now;
@ -560,7 +566,7 @@ void AP_UAVCAN::SRV_push_servos()
void AP_UAVCAN::led_out_send() 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) { if ((now - _led_conf.last_update) < LED_DELAY_US) {
return; return;
@ -655,7 +661,7 @@ void AP_UAVCAN::rtcm_stream_send()
// nothing to send // nothing to send
return; return;
} }
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::native_millis();
if (now - _rtcm_stream.last_send_ms < 20) { if (now - _rtcm_stream.last_send_ms < 20) {
// don't send more than 50 per second // don't send more than 50 per second
return; return;
@ -681,7 +687,7 @@ void AP_UAVCAN::rtcm_stream_send()
void AP_UAVCAN::safety_state_send() void AP_UAVCAN::safety_state_send()
{ {
ardupilot::indication::SafetyState msg; ardupilot::indication::SafetyState msg;
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::native_millis();
if (now - _last_safety_state_ms < 500) { if (now - _last_safety_state_ms < 500) {
// update at 2Hz // update at 2Hz
return; return;
@ -802,7 +808,7 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
pkt.flags |= 0x100; 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); 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) void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb)
{ {
// log as CSRV message // 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->actuator_id,
cb.msg->position, cb.msg->position,
cb.msg->force, 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; const uint8_t esc_index = cb.msg->esc_index;
// log as CESC message // log as CESC message
AP::logger().Write_ESCStatus(AP_HAL::micros64(), AP::logger().Write_ESCStatus(AP_HAL::native_micros64(),
esc_index, cb.msg->esc_index,
cb.msg->error_count, cb.msg->error_count,
cb.msg->voltage, cb.msg->voltage,
cb.msg->current, cb.msg->current,
@ -861,4 +867,4 @@ bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) {
return true; return true;
} }
#endif // HAL_WITH_UAVCAN #endif // HAL_NUM_CAN_IFACES

35
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -19,8 +19,9 @@
#include <uavcan/uavcan.hpp> #include <uavcan/uavcan.hpp>
#include "AP_UAVCAN_DNA_Server.h" #include "AP_UAVCAN_DNA_Server.h"
#include "AP_UAVCAN_IfaceMgr.h"
#include <AP_HAL/CAN.h> #include "AP_UAVCAN_Clock.h"
#include <AP_CANManager/AP_CANDriver.h>
#include <AP_HAL/Semaphores.h> #include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
@ -66,7 +67,7 @@ class ESCStatusCb;
RegistryBinder(uc, (Registry)ffunc) {} \ RegistryBinder(uc, (Registry)ffunc) {} \
} }
class AP_UAVCAN : public AP_HAL::CANProtocol { class AP_UAVCAN : public AP_CANDriver {
public: public:
AP_UAVCAN(); AP_UAVCAN();
~AP_UAVCAN(); ~AP_UAVCAN();
@ -77,6 +78,7 @@ public:
static AP_UAVCAN *get_uavcan(uint8_t driver_index); static AP_UAVCAN *get_uavcan(uint8_t driver_index);
void init(uint8_t driver_index, bool enable_filters) override; 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 // send ESC telemetry messages over MAVLink
void send_esc_telemetry_mavlink(uint8_t mav_chan); void send_esc_telemetry_mavlink(uint8_t mav_chan);
@ -124,31 +126,6 @@ public:
}; };
private: 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 // This will be needed to implement if UAVCAN is used with multithreading
// Such cases will be firmware update, etc. // Such cases will be firmware update, etc.
class RaiiSynchronizer {}; class RaiiSynchronizer {};
@ -182,6 +159,8 @@ private:
uavcan::Node<0> *_node; uavcan::Node<0> *_node;
uint8_t _driver_index; uint8_t _driver_index;
uavcan::CanIfaceMgr* _iface_mgr;
char _thread_name[13]; char _thread_name[13];
bool _initialized; bool _initialized;
///// SRV output ///// ///// SRV output /////

59
libraries/AP_UAVCAN/AP_UAVCAN_Clock.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
*/
#pragma once
#include <uavcan/uavcan.hpp>
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;
};
}

25
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
View File

@ -18,7 +18,7 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN #if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_UAVCAN_DNA_Server.h" #include "AP_UAVCAN_DNA_Server.h"
#include "AP_UAVCAN.h" #include "AP_UAVCAN.h"
@ -27,14 +27,14 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <uavcan/protocol/dynamic_node_id/Allocation.hpp> #include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include "AP_UAVCAN_Clock.h"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define NODEDATA_MAGIC 0xAC01 #define NODEDATA_MAGIC 0xAC01
#define NODEDATA_MAGIC_LEN 2 #define NODEDATA_MAGIC_LEN 2
#define MAX_NODE_ID 125 #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 //Callback Object Definitions
UC_REGISTRY_BINDER(AllocationCb, uavcan::protocol::dynamic_node_id::Allocation); UC_REGISTRY_BINDER(AllocationCb, uavcan::protocol::dynamic_node_id::Allocation);
@ -44,9 +44,9 @@ static void trampoline_handleNodeInfo(const uavcan::ServiceCallResult<uavcan::pr
static void trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb); static void trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb);
static void trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb); static void trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb);
static uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>* getNodeInfo_client[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>* getNodeInfo_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>* allocation_pub[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>* allocation_pub[HAL_MAX_CAN_PROTOCOL_DRIVERS];
/* Subscribe to all the messages we are going to handle for /* Subscribe to all the messages we are going to handle for
Server registry and Node allocation. */ Server registry and Node allocation. */
@ -422,14 +422,14 @@ void AP_UAVCAN_DNA_Server::verify_nodes(AP_UAVCAN *ap_uavcan)
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
uint32_t now = AP_HAL::millis(); uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec();
if ((now - last_verification_request) < 5000) { if ((now - last_verification_request) < 5000) {
return; return;
} }
//Check if we got acknowledgement from previous request //Check if we got acknowledgement from previous request
//except for requests using our own node_id //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]) { if (curr_verifying_node == self_node_id[i]) {
nodeInfo_resp_rcvd = true; nodeInfo_resp_rcvd = true;
} }
@ -455,7 +455,7 @@ void AP_UAVCAN_DNA_Server::verify_nodes(AP_UAVCAN *ap_uavcan)
break; 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)) { if (getNodeInfo_client[i] != nullptr && isNodeIDOccupied(curr_verifying_node)) {
uavcan::protocol::GetNodeInfo::Request request; uavcan::protocol::GetNodeInfo::Request request;
getNodeInfo_client[i]->call(curr_verifying_node, 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); WITH_SEMAPHORE(sem);
if (!isNodeIDVerified(node_id)) { if (!isNodeIDVerified(node_id)) {
//immediately begin verification of the 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) { if (getNodeInfo_client[i] != nullptr) {
uavcan::protocol::GetNodeInfo::Request request; uavcan::protocol::GetNodeInfo::Request request;
getNodeInfo_client[i]->call(node_id, 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 //Ignore Allocation messages that are not DNA requests
return; return;
} }
uint32_t now = AP_HAL::millis(); uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec();
if (driver_index == current_driver_index) { if (driver_index == current_driver_index) {
last_activity_ms = now; last_activity_ms = now;
} else if ((now - last_activity_ms) > 500) { } else if ((now - last_activity_ms) > 500) {
@ -670,8 +670,9 @@ namespace AP
{ {
AP_UAVCAN_DNA_Server& uavcan_dna_server() 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; return _server;
} }
} }
#endif //HAL_WITH_UAVCAN #endif //HAL_NUM_CAN_IFACES

4
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN #if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <uavcan/uavcan.hpp> #include <uavcan/uavcan.hpp>
#include <AP_Common/Bitmask.h> #include <AP_Common/Bitmask.h>
#include <StorageManager/StorageManager.h> #include <StorageManager/StorageManager.h>
@ -30,7 +30,7 @@ class AP_UAVCAN_DNA_Server
uint32_t last_verification_request; uint32_t last_verification_request;
uint8_t curr_verifying_node; 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; bool nodeInfo_resp_rcvd;
Bitmask<128> occupation_mask; Bitmask<128> occupation_mask;

259
libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
*/
#include "AP_UAVCAN_IfaceMgr.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_UAVCAN_Clock.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_CANManager/AP_CANManager.h>
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

73
libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <uavcan/uavcan.hpp>
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

574
libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
* Referenced from implementation by Pavel Kirienko <pavel.kirienko@zubax.com>
* for Zubax Babel
*/
#include <AP_HAL/AP_HAL.h>
#if AP_UAVCAN_SLCAN_ENABLED
#include "AP_UAVCAN_SLCAN.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_HAL_ChibiOS/CANSerialRouter.h>
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:
* <type> <id> <dlc> <data>
* 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:
* <type> <id> <dlc> <data> [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<char*>(&buf_[0]));
reset();
// Sending the response if provided
if (response != nullptr) {
_port->write_locked(reinterpret_cast<const uint8_t*>(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

240
libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h
View File

@ -1,240 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if AP_UAVCAN_SLCAN_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h>
#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<CanRxItem> 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<CANManager*>(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 <AP_HAL_ChibiOS/CANSerialRouter.h>
#endif // AP_UAVCAN_SLCAN_ENABLED