mirror of https://github.com/ArduPilot/ardupilot
AP_UAVCAN: add source for SLCAN interface
This commit is contained in:
Siddharth Purohit
Andrew Tridgell
parent
18e97bd895
commit
eb29a7aa44
|
|
@ -0,0 +1,592 @@
|
|||
/*
|
||||
* 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 HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES
|
||||
#include "AP_UAVCAN_SLCAN.h"
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_Common/Semaphore.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 ch)
|
||||
{
|
||||
// Must go into RAM, not flash, because flash is slow
|
||||
static uint8_t ConversionTable[] =
|
||||
{
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255,
|
||||
0, 1, 2, 3, 4, 5, 6, 7, 8, 9, // 0..9
|
||||
255, 255, 255, 255, 255, 255, 255,
|
||||
10, 11, 12, 13, 14, 15, // A..F
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255,
|
||||
10, 11, 12, 13, 14, 15, // a..f
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255
|
||||
};
|
||||
|
||||
const uint8_t out = ConversionTable[int(ch)];
|
||||
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();
|
||||
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';
|
||||
assert(f.dlc <= uavcan::CanFrame::MaxDataLen);
|
||||
{
|
||||
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';
|
||||
assert(f.dlc <= uavcan::CanFrame::MaxDataLen);
|
||||
{
|
||||
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(1000000, SLCAN::CAN::NormalMode) < 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)
|
||||
{
|
||||
_port = AP_SerialManager::get_instance()->find_serial(AP_SerialManager::SerialProtocol_SLCAN, 0);
|
||||
if (_port == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
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(&buffer[0], frame_size)) {
|
||||
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
|
||||
case 'C': // Close CAN
|
||||
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(reinterpret_cast<const uint8_t*>(response),
|
||||
strlen(response));
|
||||
}
|
||||
}
|
||||
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(921600);
|
||||
_port_initialised = true;
|
||||
|
||||
}
|
||||
_port->wait_timeout(1, 1);
|
||||
size_t nread = _port->available();
|
||||
if (nread > 0) {
|
||||
int16_t data = _port->read();
|
||||
while (data != -1) {
|
||||
addByte(data);
|
||||
data = _port->read();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
rx_queue_.pop(frm);
|
||||
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 //HAL_WITH_UAVCAN
|
||||
|
|
@ -0,0 +1,227 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES
|
||||
|
||||
#include <AP_HAL/CAN.h>
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
#include "AP_HAL/utility/RingBuffer.h"
|
||||
|
||||
|
||||
|
||||
#define SLCAN_BUFFER_SIZE 200
|
||||
#define SLCAN_STM32_RX_QUEUE_SIZE 64
|
||||
#define SLCAN_DRIVER_INDEX 2
|
||||
|
||||
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::CAN {
|
||||
friend class CANManager;
|
||||
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);
|
||||
|
||||
inline void addByte(const uint8_t byte);
|
||||
|
||||
void reader(void);
|
||||
|
||||
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;
|
||||
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);
|
||||
|
||||
bool begin(uint32_t bitrate) override
|
||||
{
|
||||
if (init(bitrate, OperatingMode::NormalMode) == 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 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_STM32_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;
|
||||
}
|
||||
|
||||
static void reader(void);
|
||||
void reader_trampoline(void);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif //#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES
|
||||
|
||||
|
||||
Loading…
Reference in New Issue