First implementation
This commit is contained in:
107
driver/SLCANDriver/SLCANDriver.cpp
Normal file
107
driver/SLCANDriver/SLCANDriver.cpp
Normal file
@@ -0,0 +1,107 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2022 Ethan Zonca
|
||||
|
||||
This file is part of cangaroo.
|
||||
|
||||
cangaroo 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 2 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
cangaroo 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 cangaroo. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include "SLCANDriver.h"
|
||||
#include "SLCANInterface.h"
|
||||
#include <core/Backend.h>
|
||||
#include <driver/GenericCanSetupPage.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <cstring>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
|
||||
//
|
||||
#include <QCoreApplication>
|
||||
#include <QDebug>
|
||||
#include <QtSerialPort/QSerialPort>
|
||||
#include <QtSerialPort/QSerialPortInfo>
|
||||
|
||||
SLCANDriver::SLCANDriver(Backend &backend)
|
||||
: CanDriver(backend),
|
||||
setupPage(new GenericCanSetupPage())
|
||||
{
|
||||
QObject::connect(&backend, SIGNAL(onSetupDialogCreated(SetupDialog&)), setupPage, SLOT(onSetupDialogCreated(SetupDialog&)));
|
||||
}
|
||||
|
||||
SLCANDriver::~SLCANDriver() {
|
||||
}
|
||||
|
||||
bool SLCANDriver::update() {
|
||||
|
||||
deleteAllInterfaces();
|
||||
|
||||
int interface_cnt = 0;
|
||||
|
||||
foreach (const QSerialPortInfo &info, QSerialPortInfo::availablePorts()) {
|
||||
fprintf(stderr, "Name : %s \r\n", info.portName().toStdString().c_str());
|
||||
fprintf(stderr, " Description : %s \r\n", info.description().toStdString().c_str());
|
||||
fprintf(stderr, " Manufacturer: %s \r\n", info.manufacturer().toStdString().c_str());
|
||||
|
||||
if(info.vendorIdentifier() == 0xad50 && info.productIdentifier() == 0x60C4)
|
||||
{
|
||||
|
||||
perror(" ++ CANable 1.0 or similar ST USB CDC device detected");
|
||||
|
||||
// Create new slcan interface without FD support
|
||||
SLCANInterface *intf = createOrUpdateInterface(interface_cnt, info.portName(), false);
|
||||
interface_cnt++;
|
||||
}
|
||||
else if(info.vendorIdentifier() == 0x16D0 && info.productIdentifier() == 0x117E)
|
||||
{
|
||||
perror(" ++ CANable 2.0 detected");
|
||||
|
||||
// Create new slcan interface without FD support
|
||||
SLCANInterface *intf = createOrUpdateInterface(interface_cnt, info.portName(), true);
|
||||
interface_cnt++;
|
||||
}
|
||||
else
|
||||
{
|
||||
perror(" !! This is not a CANable device!");
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
QString SLCANDriver::getName() {
|
||||
return "CANable SLCAN";
|
||||
}
|
||||
|
||||
|
||||
|
||||
SLCANInterface *SLCANDriver::createOrUpdateInterface(int index, QString name, bool fd_support) {
|
||||
|
||||
foreach (CanInterface *intf, getInterfaces()) {
|
||||
SLCANInterface *scif = dynamic_cast<SLCANInterface*>(intf);
|
||||
if (scif->getIfIndex() == index) {
|
||||
scif->setName(name);
|
||||
return scif;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
SLCANInterface *scif = new SLCANInterface(this, index, name, fd_support);
|
||||
addInterface(scif);
|
||||
return scif;
|
||||
}
|
||||
43
driver/SLCANDriver/SLCANDriver.h
Normal file
43
driver/SLCANDriver/SLCANDriver.h
Normal file
@@ -0,0 +1,43 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, 2016 Hubert Denkmair
|
||||
|
||||
This file is part of cangaroo.
|
||||
|
||||
cangaroo 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 2 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
cangaroo 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 cangaroo. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QString>
|
||||
#include <core/Backend.h>
|
||||
#include <driver/CanDriver.h>
|
||||
|
||||
class SLCANInterface;
|
||||
class SetupDialogInterfacePage;
|
||||
class GenericCanSetupPage;
|
||||
|
||||
class SLCANDriver: public CanDriver {
|
||||
public:
|
||||
SLCANDriver(Backend &backend);
|
||||
virtual ~SLCANDriver();
|
||||
|
||||
virtual QString getName();
|
||||
virtual bool update();
|
||||
|
||||
private:
|
||||
SLCANInterface *createOrUpdateInterface(int index, QString name, bool fd_support);
|
||||
GenericCanSetupPage *setupPage;
|
||||
};
|
||||
12
driver/SLCANDriver/SLCANDriver.pri
Normal file
12
driver/SLCANDriver/SLCANDriver.pri
Normal file
@@ -0,0 +1,12 @@
|
||||
CONFIG += c++11
|
||||
|
||||
SOURCES += \
|
||||
$$PWD/SLCANInterface.cpp \
|
||||
$$PWD/SLCANDriver.cpp
|
||||
|
||||
HEADERS += \
|
||||
$$PWD/SLCANInterface.h \
|
||||
$$PWD/SLCANDriver.h
|
||||
|
||||
FORMS +=
|
||||
|
||||
742
driver/SLCANDriver/SLCANInterface.cpp
Normal file
742
driver/SLCANDriver/SLCANInterface.cpp
Normal file
@@ -0,0 +1,742 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2022 Ethan Zonca
|
||||
|
||||
This file is part of cangaroo.
|
||||
|
||||
cangaroo 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 2 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
cangaroo 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 cangaroo. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
#include "SLCANInterface.h"
|
||||
|
||||
#include <core/Backend.h>
|
||||
#include <core/MeasurementInterface.h>
|
||||
#include <core/CanMessage.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <time.h>
|
||||
#include <fcntl.h>
|
||||
#include <QString>
|
||||
#include <QStringList>
|
||||
#include <QProcess>
|
||||
#include <QtSerialPort/QSerialPort>
|
||||
#include <QtSerialPort/QSerialPortInfo>
|
||||
#include <QThread>
|
||||
|
||||
SLCANInterface::SLCANInterface(SLCANDriver *driver, int index, QString name, bool fd_support)
|
||||
: CanInterface((CanDriver *)driver),
|
||||
_idx(index),
|
||||
_isOpen(false),
|
||||
_serport(NULL),
|
||||
_msg_queue(),
|
||||
_name(name),
|
||||
_rx_linbuf_ctr(0),
|
||||
_rxbuf_head(0),
|
||||
_rxbuf_tail(0),
|
||||
_ts_mode(ts_mode_SIOCSHWTSTAMP)
|
||||
{
|
||||
// Set defaults
|
||||
_settings.setBitrate(500000);
|
||||
_settings.setSamplePoint(875);
|
||||
|
||||
_config.supports_canfd = fd_support;
|
||||
}
|
||||
|
||||
SLCANInterface::~SLCANInterface() {
|
||||
}
|
||||
|
||||
QString SLCANInterface::getDetailsStr() const {
|
||||
if(_config.supports_canfd)
|
||||
{
|
||||
return "CANable with CANFD support";
|
||||
}
|
||||
else
|
||||
{
|
||||
return "CANable with standard CAN support";
|
||||
}
|
||||
}
|
||||
|
||||
QString SLCANInterface::getName() const {
|
||||
return _name;
|
||||
}
|
||||
|
||||
void SLCANInterface::setName(QString name) {
|
||||
_name = name;
|
||||
}
|
||||
|
||||
QList<CanTiming> SLCANInterface::getAvailableBitrates()
|
||||
{
|
||||
QList<CanTiming> retval;
|
||||
QList<unsigned> bitrates({10000, 20000, 50000, 83333, 100000, 125000, 250000, 500000, 800000, 1000000});
|
||||
QList<unsigned> bitrates_fd({0, 2000000, 5000000});
|
||||
|
||||
QList<unsigned> samplePoints({875});
|
||||
|
||||
unsigned i=0;
|
||||
foreach (unsigned br, bitrates) {
|
||||
foreach(unsigned br_fd, bitrates_fd) {
|
||||
foreach (unsigned sp, samplePoints) {
|
||||
retval << CanTiming(i++, br, br_fd, sp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return retval;
|
||||
}
|
||||
|
||||
|
||||
void SLCANInterface::applyConfig(const MeasurementInterface &mi)
|
||||
{
|
||||
// Save settings for port configuration
|
||||
_settings = mi;
|
||||
}
|
||||
|
||||
bool SLCANInterface::updateStatus()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SLCANInterface::readConfig()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SLCANInterface::readConfigFromLink(rtnl_link *link)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SLCANInterface::supportsTimingConfiguration()
|
||||
{
|
||||
return _config.supports_timing;
|
||||
}
|
||||
|
||||
bool SLCANInterface::supportsCanFD()
|
||||
{
|
||||
return _config.supports_canfd;
|
||||
}
|
||||
|
||||
bool SLCANInterface::supportsTripleSampling()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
unsigned SLCANInterface::getBitrate()
|
||||
{
|
||||
return _settings.bitrate();
|
||||
}
|
||||
|
||||
uint32_t SLCANInterface::getCapabilities()
|
||||
{
|
||||
uint32_t retval =
|
||||
CanInterface::capability_config_os |
|
||||
CanInterface::capability_listen_only |
|
||||
CanInterface::capability_auto_restart;
|
||||
|
||||
if (supportsCanFD()) {
|
||||
retval |= CanInterface::capability_canfd;
|
||||
}
|
||||
|
||||
if (supportsTripleSampling()) {
|
||||
retval |= CanInterface::capability_triple_sampling;
|
||||
}
|
||||
|
||||
return retval;
|
||||
}
|
||||
|
||||
bool SLCANInterface::updateStatistics()
|
||||
{
|
||||
return updateStatus();
|
||||
}
|
||||
|
||||
uint32_t SLCANInterface::getState()
|
||||
{
|
||||
if(_isOpen)
|
||||
return state_ok;
|
||||
else
|
||||
return state_bus_off;
|
||||
}
|
||||
|
||||
int SLCANInterface::getNumRxFrames()
|
||||
{
|
||||
return _status.rx_count;
|
||||
}
|
||||
|
||||
int SLCANInterface::getNumRxErrors()
|
||||
{
|
||||
return _status.rx_errors;
|
||||
}
|
||||
|
||||
int SLCANInterface::getNumTxFrames()
|
||||
{
|
||||
return _status.tx_count;
|
||||
}
|
||||
|
||||
int SLCANInterface::getNumTxErrors()
|
||||
{
|
||||
return _status.tx_errors;
|
||||
}
|
||||
|
||||
int SLCANInterface::getNumRxOverruns()
|
||||
{
|
||||
return _status.rx_overruns;
|
||||
}
|
||||
|
||||
int SLCANInterface::getNumTxDropped()
|
||||
{
|
||||
return _status.tx_dropped;
|
||||
}
|
||||
|
||||
int SLCANInterface::getIfIndex() {
|
||||
return _idx;
|
||||
}
|
||||
|
||||
void SLCANInterface::open()
|
||||
{
|
||||
if(_serport != NULL)
|
||||
{
|
||||
delete _serport;
|
||||
}
|
||||
|
||||
_serport = new QSerialPort();
|
||||
|
||||
_serport_mutex.lock();
|
||||
_serport->setPortName(_name);
|
||||
_serport->setBaudRate(1000000);
|
||||
_serport->setDataBits(QSerialPort::Data8);
|
||||
_serport->setParity(QSerialPort::NoParity);
|
||||
_serport->setStopBits(QSerialPort::OneStop);
|
||||
_serport->setFlowControl(QSerialPort::NoFlowControl);
|
||||
_serport->setReadBufferSize(2048);
|
||||
if (_serport->open(QIODevice::ReadWrite)) {
|
||||
//perror("Serport connected!");
|
||||
} else {
|
||||
perror("Serport connect failed!");
|
||||
_serport_mutex.unlock();
|
||||
_isOpen = false;
|
||||
return;
|
||||
}
|
||||
_serport->flush();
|
||||
_serport->clear();
|
||||
|
||||
// Set the classic CAN bitrate
|
||||
switch(_settings.bitrate())
|
||||
{
|
||||
case 1000000:
|
||||
_serport->write("S8\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 750000:
|
||||
_serport->write("S7\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 500000:
|
||||
_serport->write("S6\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 250000:
|
||||
_serport->write("S5\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 125000:
|
||||
_serport->write("S4\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 100000:
|
||||
_serport->write("S3\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 83333:
|
||||
_serport->write("S9\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 50000:
|
||||
_serport->write("S2\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 20000:
|
||||
_serport->write("S1\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 10000:
|
||||
_serport->write("S0\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
default:
|
||||
// Default to 10k
|
||||
_serport->write("S0\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
}
|
||||
|
||||
_serport->waitForBytesWritten(300);
|
||||
|
||||
|
||||
|
||||
// Set configured BRS rate
|
||||
if(_config.supports_canfd)
|
||||
{
|
||||
switch(_settings.fdBitrate())
|
||||
{
|
||||
case 2000000:
|
||||
_serport->write("Y2\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
case 5000000:
|
||||
_serport->write("Y5\r", 3);
|
||||
_serport->flush();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_serport->waitForBytesWritten(300);
|
||||
|
||||
|
||||
// Open the port
|
||||
_serport->write("O\r\n", 3);
|
||||
_serport->flush();
|
||||
|
||||
_isOpen = true;
|
||||
|
||||
// Release port mutex
|
||||
_serport_mutex.unlock();
|
||||
}
|
||||
|
||||
void SLCANInterface::close()
|
||||
{
|
||||
_serport_mutex.lock();
|
||||
|
||||
if (_serport->isOpen())
|
||||
{
|
||||
// Close CAN port
|
||||
_serport->write("C\r", 2);
|
||||
_serport->flush();
|
||||
_serport->waitForBytesWritten(300);
|
||||
_serport->clear();
|
||||
_serport->close();
|
||||
}
|
||||
|
||||
_isOpen = false;
|
||||
_serport_mutex.unlock();
|
||||
}
|
||||
|
||||
bool SLCANInterface::isOpen()
|
||||
{
|
||||
return _isOpen;
|
||||
}
|
||||
|
||||
void SLCANInterface::sendMessage(const CanMessage &msg) {
|
||||
|
||||
// SLCAN_MTU plus null terminator
|
||||
char buf[SLCAN_MTU+1] = {0};
|
||||
|
||||
uint8_t msg_idx = 0;
|
||||
|
||||
// Message is FD
|
||||
// Add character for frame type
|
||||
if(msg.isFD())
|
||||
{
|
||||
if(msg.isBRS())
|
||||
{
|
||||
buf[msg_idx] = 'b';
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
buf[msg_idx] = 'd';
|
||||
}
|
||||
}
|
||||
// Message is not FD
|
||||
// Add character for frame type
|
||||
else
|
||||
{
|
||||
if (msg.isRTR()) {
|
||||
buf[msg_idx] = 'r';
|
||||
}
|
||||
else
|
||||
{
|
||||
buf[msg_idx] = 't';
|
||||
}
|
||||
}
|
||||
|
||||
// Assume standard identifier
|
||||
uint8_t id_len = SLCAN_STD_ID_LEN;
|
||||
uint32_t tmp = msg.getId();
|
||||
|
||||
// Check if extended
|
||||
if (msg.isExtended())
|
||||
{
|
||||
// Convert first char to upper case for extended frame
|
||||
buf[msg_idx] -= 32;
|
||||
id_len = SLCAN_EXT_ID_LEN;
|
||||
}
|
||||
msg_idx++;
|
||||
|
||||
// Add identifier to buffer
|
||||
for(uint8_t j = id_len; j > 0; j--)
|
||||
{
|
||||
// Add nibble to buffer
|
||||
buf[j] = (tmp & 0xF);
|
||||
tmp = tmp >> 4;
|
||||
msg_idx++;
|
||||
}
|
||||
|
||||
// Sanity check length
|
||||
int8_t bytes = msg.getLength();
|
||||
|
||||
|
||||
if(bytes < 0)
|
||||
return;
|
||||
if(bytes > 64)
|
||||
return;
|
||||
|
||||
// If canfd
|
||||
if(bytes > 8)
|
||||
{
|
||||
switch(bytes)
|
||||
{
|
||||
case 12:
|
||||
bytes = 0x9;
|
||||
break;
|
||||
case 16:
|
||||
bytes = 0xA;
|
||||
break;
|
||||
case 20:
|
||||
bytes = 0xB;
|
||||
break;
|
||||
case 24:
|
||||
bytes = 0xC;
|
||||
break;
|
||||
case 32:
|
||||
bytes = 0xD;
|
||||
break;
|
||||
case 48:
|
||||
bytes = 0xE;
|
||||
break;
|
||||
case 64:
|
||||
bytes = 0xF;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Add DLC to buffer
|
||||
buf[msg_idx++] = bytes;
|
||||
|
||||
// Add data bytes
|
||||
for (uint8_t j = 0; j < msg.getLength(); j++)
|
||||
{
|
||||
buf[msg_idx++] = (msg.getByte(j) >> 4);
|
||||
buf[msg_idx++] = (msg.getByte(j) & 0x0F);
|
||||
}
|
||||
|
||||
// Convert to ASCII (2nd character to end)
|
||||
for (uint8_t j = 1; j < msg_idx; j++)
|
||||
{
|
||||
if (buf[j] < 0xA) {
|
||||
buf[j] += 0x30;
|
||||
} else {
|
||||
buf[j] += 0x37;
|
||||
}
|
||||
}
|
||||
|
||||
// Add CR for slcan EOL
|
||||
buf[msg_idx++] = '\r';
|
||||
|
||||
// Ensure null termination
|
||||
buf[msg_idx] = '\0';
|
||||
|
||||
_msg_queue.append(QString(buf));
|
||||
|
||||
}
|
||||
|
||||
bool SLCANInterface::readMessage(QList<CanMessage> &msglist, unsigned int timeout_ms)
|
||||
{
|
||||
// Don't saturate the thread. Read the buffer every 1ms.
|
||||
QThread().msleep(1);
|
||||
|
||||
// Transmit all items that are queued
|
||||
while(!_msg_queue.empty())
|
||||
{
|
||||
// Consume first item
|
||||
QString tmp = _msg_queue.front();
|
||||
_msg_queue.pop_front();
|
||||
|
||||
_serport_mutex.lock();
|
||||
// Write string to serial device
|
||||
_serport->write(tmp.toStdString().c_str(), tmp.length());
|
||||
_serport->flush();
|
||||
_serport->waitForBytesWritten(300);
|
||||
_serport_mutex.unlock();
|
||||
}
|
||||
|
||||
// RX doesn't work on windows unless we call this for some reason
|
||||
_serport->waitForReadyRead(1);
|
||||
|
||||
if(_serport->bytesAvailable())
|
||||
{
|
||||
// This is called when readyRead() is emitted
|
||||
QByteArray datas = _serport->readAll();
|
||||
_rxbuf_mutex.lock();
|
||||
for(int i=0; i<datas.count(); i++)
|
||||
{
|
||||
// If incrementing the head will hit the tail, we've filled the buffer. Reset and discard all data.
|
||||
if(((_rxbuf_head + 1) % RXCIRBUF_LEN) == _rxbuf_tail)
|
||||
{
|
||||
_rxbuf_head = 0;
|
||||
_rxbuf_tail = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Put inbound data at the head locatoin
|
||||
_rxbuf[_rxbuf_head] = datas.at(i);
|
||||
_rxbuf_head = (_rxbuf_head + 1) % RXCIRBUF_LEN; // Wrap at MTU
|
||||
}
|
||||
}
|
||||
_rxbuf_mutex.unlock();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//////////////////////////
|
||||
|
||||
bool ret = false;
|
||||
_rxbuf_mutex.lock();
|
||||
while(_rxbuf_tail != _rxbuf_head)
|
||||
{
|
||||
// Save data if room
|
||||
if(_rx_linbuf_ctr < SLCAN_MTU)
|
||||
{
|
||||
_rx_linbuf[_rx_linbuf_ctr++] = _rxbuf[_rxbuf_tail];
|
||||
|
||||
// If we have a newline, then we just finished parsing a CAN message.
|
||||
if(_rxbuf[_rxbuf_tail] == '\r')
|
||||
{
|
||||
CanMessage msg;
|
||||
ret = parseMessage(msg);
|
||||
msglist.append(msg);
|
||||
_rx_linbuf_ctr = 0;
|
||||
}
|
||||
}
|
||||
// Discard data if not
|
||||
else
|
||||
{
|
||||
perror("Linbuf full");
|
||||
_rx_linbuf_ctr = 0;
|
||||
}
|
||||
|
||||
_rxbuf_tail = (_rxbuf_tail + 1) % RXCIRBUF_LEN;
|
||||
}
|
||||
_rxbuf_mutex.unlock();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool SLCANInterface::parseMessage(CanMessage &msg)
|
||||
{
|
||||
// Set timestamp to current time
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv,NULL);
|
||||
msg.setTimestamp(tv);
|
||||
|
||||
// Defaults
|
||||
msg.setErrorFrame(0);
|
||||
msg.setInterfaceId(getId());
|
||||
msg.setId(0);
|
||||
|
||||
bool msg_is_fd = false;
|
||||
|
||||
// Convert from ASCII (2nd character to end)
|
||||
for (int i = 1; i < _rx_linbuf_ctr; i++)
|
||||
{
|
||||
// Lowercase letters
|
||||
if(_rx_linbuf[i] >= 'a')
|
||||
_rx_linbuf[i] = _rx_linbuf[i] - 'a' + 10;
|
||||
// Uppercase letters
|
||||
else if(_rx_linbuf[i] >= 'A')
|
||||
_rx_linbuf[i] = _rx_linbuf[i] - 'A' + 10;
|
||||
// Numbers
|
||||
else
|
||||
_rx_linbuf[i] = _rx_linbuf[i] - '0';
|
||||
}
|
||||
|
||||
|
||||
// Handle each incoming command
|
||||
switch(_rx_linbuf[0])
|
||||
{
|
||||
|
||||
// Transmit data frame command
|
||||
case 'T':
|
||||
msg.setExtended(1);
|
||||
break;
|
||||
case 't':
|
||||
msg.setExtended(0);
|
||||
break;
|
||||
|
||||
// Transmit remote frame command
|
||||
case 'r':
|
||||
msg.setExtended(0);
|
||||
msg.setRTR(1);
|
||||
break;
|
||||
case 'R':
|
||||
msg.setExtended(1);
|
||||
msg.setRTR(1);
|
||||
break;
|
||||
|
||||
// CANFD transmit - no BRS
|
||||
case 'd':
|
||||
msg.setExtended(0);
|
||||
msg_is_fd = true;
|
||||
break;
|
||||
case 'D':
|
||||
msg.setExtended(1);
|
||||
msg_is_fd = true;
|
||||
break;
|
||||
|
||||
// CANFD transmit - with BRS
|
||||
case 'b':
|
||||
msg.setExtended(0);
|
||||
msg_is_fd = true;
|
||||
break;
|
||||
case 'B':
|
||||
msg.setExtended(1);
|
||||
msg_is_fd = true;
|
||||
break;
|
||||
|
||||
|
||||
|
||||
// Invalid command
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
// Start parsing at second byte (skip command byte)
|
||||
uint8_t parse_loc = 1;
|
||||
|
||||
// Default to standard id len
|
||||
uint8_t id_len = SLCAN_STD_ID_LEN;
|
||||
|
||||
// Update length if message is extended ID
|
||||
if(msg.isExtended())
|
||||
id_len = SLCAN_EXT_ID_LEN;
|
||||
|
||||
uint32_t id_tmp = 0;
|
||||
|
||||
// Iterate through ID bytes
|
||||
while(parse_loc <= id_len)
|
||||
{
|
||||
id_tmp *= 16;
|
||||
id_tmp += _rx_linbuf[parse_loc++];
|
||||
}
|
||||
|
||||
|
||||
msg.setId(id_tmp);
|
||||
|
||||
// Attempt to parse DLC and check sanity
|
||||
uint8_t dlc_code_raw = _rx_linbuf[parse_loc++];
|
||||
|
||||
// If dlc is too long for an FD frame
|
||||
if(msg_is_fd && dlc_code_raw > 0xF)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(!msg_is_fd && dlc_code_raw > 0x8)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if(dlc_code_raw > 0x8)
|
||||
{
|
||||
switch(dlc_code_raw)
|
||||
{
|
||||
case 0x9:
|
||||
dlc_code_raw = 12;
|
||||
break;
|
||||
case 0xA:
|
||||
dlc_code_raw = 16;
|
||||
break;
|
||||
case 0xB:
|
||||
dlc_code_raw = 20;
|
||||
break;
|
||||
case 0xC:
|
||||
dlc_code_raw = 24;
|
||||
break;
|
||||
case 0xD:
|
||||
dlc_code_raw = 32;
|
||||
break;
|
||||
case 0xE:
|
||||
dlc_code_raw = 48;
|
||||
break;
|
||||
case 0xF:
|
||||
dlc_code_raw = 64;
|
||||
break;
|
||||
default:
|
||||
dlc_code_raw = 0;
|
||||
perror("Invalid length");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
msg.setLength(dlc_code_raw);
|
||||
|
||||
// Calculate number of bytes we expect in the message
|
||||
int8_t bytes_in_msg = dlc_code_raw;
|
||||
|
||||
if(bytes_in_msg < 0) {
|
||||
perror("Invalid length < 0");
|
||||
return false;
|
||||
}
|
||||
if(bytes_in_msg > 64) {
|
||||
perror("Invalid length > 64");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Parse data
|
||||
// TODO: Guard against walking off the end of the string!
|
||||
for (uint8_t i = 0; i < bytes_in_msg; i++)
|
||||
{
|
||||
msg.setByte(i, (_rx_linbuf[parse_loc] << 4) + _rx_linbuf[parse_loc+1]);
|
||||
parse_loc += 2;
|
||||
}
|
||||
|
||||
// Reset buffer
|
||||
_rx_linbuf_ctr = 0;
|
||||
_rx_linbuf[0] = '\0';
|
||||
return true;
|
||||
|
||||
|
||||
/*
|
||||
|
||||
// FIXME
|
||||
if (_ts_mode == ts_mode_SIOCSHWTSTAMP) {
|
||||
// TODO implement me
|
||||
_ts_mode = ts_mode_SIOCGSTAMPNS;
|
||||
}
|
||||
|
||||
if (_ts_mode==ts_mode_SIOCGSTAMPNS) {
|
||||
if (ioctl(_fd, SIOCGSTAMPNS, &ts_rcv) == 0) {
|
||||
msg.setTimestamp(ts_rcv.tv_sec, ts_rcv.tv_nsec/1000);
|
||||
} else {
|
||||
_ts_mode = ts_mode_SIOCGSTAMP;
|
||||
}
|
||||
}
|
||||
|
||||
if (_ts_mode==ts_mode_SIOCGSTAMP) {
|
||||
ioctl(_fd, SIOCGSTAMP, &tv_rcv);
|
||||
msg.setTimestamp(tv_rcv.tv_sec, tv_rcv.tv_usec);
|
||||
}*/
|
||||
|
||||
|
||||
}
|
||||
133
driver/SLCANDriver/SLCANInterface.h
Normal file
133
driver/SLCANDriver/SLCANInterface.h
Normal file
@@ -0,0 +1,133 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, 2016 Hubert Denkmair
|
||||
|
||||
This file is part of cangaroo.
|
||||
|
||||
cangaroo 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 2 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
cangaroo 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 cangaroo. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../CanInterface.h"
|
||||
#include <core/MeasurementInterface.h>
|
||||
#include <QtSerialPort/QSerialPort>
|
||||
#include <QtSerialPort/QSerialPortInfo>
|
||||
#include <QMutex>
|
||||
|
||||
// Maximum rx buffer len
|
||||
#define SLCAN_MTU 138 + 1 + 16 // canfd 64 frame plus \r plus some padding
|
||||
#define SLCAN_STD_ID_LEN 3
|
||||
#define SLCAN_EXT_ID_LEN 8
|
||||
|
||||
#define RXCIRBUF_LEN 8192 // Buffer for received serial data, serviced at 1ms intervals
|
||||
|
||||
class SLCANDriver;
|
||||
|
||||
typedef struct {
|
||||
bool supports_canfd;
|
||||
bool supports_timing;
|
||||
uint32_t state;
|
||||
uint32_t base_freq;
|
||||
uint32_t sample_point;
|
||||
uint32_t ctrl_mode;
|
||||
uint32_t restart_ms;
|
||||
} can_config_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t can_state;
|
||||
|
||||
uint64_t rx_count;
|
||||
int rx_errors;
|
||||
uint64_t rx_overruns;
|
||||
|
||||
uint64_t tx_count;
|
||||
int tx_errors;
|
||||
uint64_t tx_dropped;
|
||||
} can_status_t;
|
||||
|
||||
class SLCANInterface: public CanInterface {
|
||||
public:
|
||||
SLCANInterface(SLCANDriver *driver, int index, QString name, bool fd_support);
|
||||
virtual ~SLCANInterface();
|
||||
|
||||
QString getDetailsStr() const;
|
||||
virtual QString getName() const;
|
||||
void setName(QString name);
|
||||
|
||||
virtual QList<CanTiming> getAvailableBitrates();
|
||||
|
||||
virtual void applyConfig(const MeasurementInterface &mi);
|
||||
virtual bool readConfig();
|
||||
virtual bool readConfigFromLink(struct rtnl_link *link);
|
||||
|
||||
bool supportsTimingConfiguration();
|
||||
bool supportsCanFD();
|
||||
bool supportsTripleSampling();
|
||||
|
||||
virtual unsigned getBitrate();
|
||||
virtual uint32_t getCapabilities();
|
||||
|
||||
|
||||
virtual void open();
|
||||
virtual void close();
|
||||
virtual bool isOpen();
|
||||
|
||||
virtual void sendMessage(const CanMessage &msg);
|
||||
virtual bool readMessage(QList<CanMessage> &msglist, unsigned int timeout_ms);
|
||||
|
||||
virtual bool updateStatistics();
|
||||
virtual uint32_t getState();
|
||||
virtual int getNumRxFrames();
|
||||
virtual int getNumRxErrors();
|
||||
virtual int getNumRxOverruns();
|
||||
|
||||
virtual int getNumTxFrames();
|
||||
virtual int getNumTxErrors();
|
||||
virtual int getNumTxDropped();
|
||||
|
||||
int getIfIndex();
|
||||
|
||||
private:
|
||||
typedef enum {
|
||||
ts_mode_SIOCSHWTSTAMP,
|
||||
ts_mode_SIOCGSTAMPNS,
|
||||
ts_mode_SIOCGSTAMP
|
||||
} ts_mode_t;
|
||||
|
||||
int _idx;
|
||||
bool _isOpen;
|
||||
QSerialPort* _serport;
|
||||
QStringList _msg_queue;
|
||||
QMutex _serport_mutex;
|
||||
QString _name;
|
||||
char _rx_linbuf[SLCAN_MTU];
|
||||
int _rx_linbuf_ctr;
|
||||
|
||||
char _rxbuf[RXCIRBUF_LEN];
|
||||
uint32_t _rxbuf_head;
|
||||
uint32_t _rxbuf_tail;
|
||||
|
||||
QMutex _rxbuf_mutex;
|
||||
MeasurementInterface _settings;
|
||||
|
||||
can_config_t _config;
|
||||
can_status_t _status;
|
||||
ts_mode_t _ts_mode;
|
||||
|
||||
bool updateStatus();
|
||||
bool parseMessage(CanMessage &msg);
|
||||
|
||||
};
|
||||
Reference in New Issue
Block a user