mirror of https://github.com/ArduPilot/ardupilot
AP_UAVCAN: move all uavcan related methods to AP_UAVCAN
This commit is contained in:
Siddharth Purohit
Andrew Tridgell
parent
65ea8e1f13
commit
07b4708a36
|
|
@ -18,12 +18,12 @@
|
|||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_WITH_UAVCAN
|
||||
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
||||
#include "AP_UAVCAN.h"
|
||||
#include <GCS_MAVLink/GCS.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>
|
||||
|
||||
|
|
@ -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<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||
static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||
static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||
static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||
static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||
static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[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[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||
static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||
static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||
static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||
static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||
|
||||
// subscribers
|
||||
|
||||
// handler SafteyButton
|
||||
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
|
||||
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
|
||||
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
|
||||
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[];
|
||||
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_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)
|
||||
{
|
||||
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
|
||||
|
|
|
|||
|
|
@ -19,8 +19,9 @@
|
|||
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include "AP_UAVCAN_DNA_Server.h"
|
||||
|
||||
#include <AP_HAL/CAN.h>
|
||||
#include "AP_UAVCAN_IfaceMgr.h"
|
||||
#include "AP_UAVCAN_Clock.h"
|
||||
#include <AP_CANManager/AP_CANDriver.h>
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
|
|
@ -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 /////
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
}
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
|
||||
#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.h"
|
||||
|
|
@ -27,14 +27,14 @@
|
|||
#include <AP_Math/AP_Math.h>
|
||||
#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#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<uavcan::pr
|
|||
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 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
|
||||
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
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#pragma once
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_WITH_UAVCAN
|
||||
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <AP_Common/Bitmask.h>
|
||||
#include <StorageManager/StorageManager.h>
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
Loading…
Reference in New Issue