1152 lines
34 KiB
C++
1152 lines
34 KiB
C++
#include <QObject>
|
|
#include <QDebug>
|
|
#include <QCanBusFrame>
|
|
#include <QSerialPortInfo>
|
|
#include <QSettings>
|
|
#include <QStringBuilder>
|
|
#include <QtNetwork>
|
|
|
|
#include "gvretserial.h"
|
|
|
|
GVRetSerial::GVRetSerial(QString portName, bool useTcp) :
|
|
CANConnection(portName, "gvret", CANCon::GVRET_SERIAL, 0, 0, false, 0, 3, 4000, true),
|
|
mTimer(this), /*NB: set this as parent of timer to manage it from working thread */
|
|
useTcp(useTcp)
|
|
{
|
|
sendDebug("GVRetSerial()");
|
|
|
|
serial = nullptr;
|
|
tcpClient = nullptr;
|
|
udpClient = nullptr;
|
|
rx_state = IDLE;
|
|
rx_step = 0;
|
|
validationCounter = 10; //how many times we can miss validation before we die
|
|
isAutoRestart = false;
|
|
espSerialMode = true;
|
|
|
|
timeBasis = 0;
|
|
lastSystemTimeBasis = 0;
|
|
timeAtGVRETSync = 0;
|
|
|
|
readSettings();
|
|
}
|
|
|
|
|
|
GVRetSerial::~GVRetSerial()
|
|
{
|
|
stop();
|
|
sendDebug("~GVRetSerial()");
|
|
}
|
|
|
|
void GVRetSerial::sendDebug(const QString debugText)
|
|
{
|
|
qDebug() << debugText;
|
|
debugOutput(debugText);
|
|
}
|
|
|
|
void GVRetSerial::sendToSerial(const QByteArray &bytes)
|
|
{
|
|
if (serial == nullptr && tcpClient == nullptr && udpClient == nullptr)
|
|
{
|
|
sendDebug("Attempt to write to serial port when it has not been initialized!");
|
|
return;
|
|
}
|
|
|
|
if (serial && !serial->isOpen())
|
|
{
|
|
sendDebug("Attempt to write to serial port when it is not open!");
|
|
return;
|
|
}
|
|
|
|
if (tcpClient && !tcpClient->isOpen())
|
|
{
|
|
sendDebug("Attempt to write to TCP/IP port when it is not open!");
|
|
return;
|
|
}
|
|
|
|
if (udpClient && !udpClient->isOpen())
|
|
{
|
|
sendDebug("Attempt to write to UDP Socket when it is not open!");
|
|
return;
|
|
}
|
|
|
|
QString buildDebug;
|
|
buildDebug = "Write to serial -> ";
|
|
foreach (int byt, bytes) {
|
|
byt = (unsigned char)byt;
|
|
buildDebug = buildDebug % QString::number(byt, 16) % " ";
|
|
}
|
|
sendDebug(buildDebug);
|
|
|
|
if (serial) serial->write(bytes);
|
|
if (tcpClient) tcpClient->write(bytes);
|
|
if (udpClient) udpClient->write(bytes);
|
|
}
|
|
|
|
void GVRetSerial::piStarted()
|
|
{
|
|
connectDevice();
|
|
}
|
|
|
|
|
|
void GVRetSerial::piSuspend(bool pSuspend)
|
|
{
|
|
/* update capSuspended */
|
|
setCapSuspended(pSuspend);
|
|
|
|
/* flush queue if we are suspended */
|
|
if(isCapSuspended())
|
|
getQueue().flush();
|
|
}
|
|
|
|
|
|
void GVRetSerial::piStop()
|
|
{
|
|
mTimer.stop();
|
|
disconnectDevice();
|
|
}
|
|
|
|
|
|
bool GVRetSerial::piGetBusSettings(int pBusIdx, CANBus& pBus)
|
|
{
|
|
return getBusConfig(pBusIdx, pBus);
|
|
}
|
|
|
|
|
|
void GVRetSerial::piSetBusSettings(int pBusIdx, CANBus bus)
|
|
{
|
|
/* sanity checks */
|
|
if( (pBusIdx < 0) || pBusIdx >= getNumBuses())
|
|
return;
|
|
|
|
/* copy bus config */
|
|
setBusConfig(pBusIdx, bus);
|
|
|
|
qDebug() << "About to update bus " << pBusIdx << " on GVRET";
|
|
if (pBusIdx == 0)
|
|
{
|
|
can0Baud = bus.getSpeed();
|
|
can0Baud |= 0x80000000;
|
|
if (bus.isActive())
|
|
{
|
|
can0Baud |= 0x40000000;
|
|
can0Enabled = true;
|
|
}
|
|
else can0Enabled = false;
|
|
|
|
if (bus.isListenOnly())
|
|
{
|
|
can0Baud |= 0x20000000;
|
|
can0ListenOnly = true;
|
|
}
|
|
else can0ListenOnly = false;
|
|
}
|
|
else if (pBusIdx == 1)
|
|
{
|
|
can1Baud = bus.getSpeed();
|
|
can1Baud |= 0x80000000;
|
|
if (bus.isActive())
|
|
{
|
|
can1Baud |= 0x40000000;
|
|
can1Enabled = true;
|
|
}
|
|
else can1Enabled = false;
|
|
|
|
if (bus.isListenOnly())
|
|
{
|
|
can1Baud |= 0x20000000;
|
|
can1ListenOnly = true;
|
|
}
|
|
else can1ListenOnly = false;
|
|
|
|
if (bus.isSingleWire())
|
|
{
|
|
can1Baud |= 0x10000000;
|
|
deviceSingleWireMode = 1;
|
|
}
|
|
else deviceSingleWireMode = 0;
|
|
}
|
|
else if (pBusIdx == 2)
|
|
{
|
|
swcanBaud = bus.getSpeed();
|
|
swcanBaud |= 0x80000000;
|
|
if (bus.isActive())
|
|
{
|
|
swcanBaud |= 0x40000000;
|
|
swcanEnabled = true;
|
|
}
|
|
else swcanEnabled = false;
|
|
|
|
if (bus.isListenOnly())
|
|
{
|
|
swcanBaud |= 0x20000000;
|
|
swcanListenOnly = true;
|
|
}
|
|
else swcanListenOnly = false;
|
|
|
|
}
|
|
|
|
if (pBusIdx < 2) {
|
|
/* update baud rates */
|
|
QByteArray buffer;
|
|
sendDebug("Got signal to update bauds. 1: " + QString::number((can0Baud & 0xFFFFFFF)) + " 2: " + QString::number((can1Baud & 0xFFFFFFF)));
|
|
buffer[0] = (char)0xF1; //start of a command over serial
|
|
buffer[1] = 5; //setup canbus
|
|
buffer[2] = (char)(can0Baud & 0xFF); //four bytes of ID LSB first
|
|
buffer[3] = (char)(can0Baud >> 8);
|
|
buffer[4] = (char)(can0Baud >> 16);
|
|
buffer[5] = (char)(can0Baud >> 24);
|
|
buffer[6] = (char)(can1Baud & 0xFF); //four bytes of ID LSB first
|
|
buffer[7] = (char)(can1Baud >> 8);
|
|
buffer[8] = (char)(can1Baud >> 16);
|
|
buffer[9] = (char)(can1Baud >> 24);
|
|
buffer[10] = 0;
|
|
sendToSerial(buffer);
|
|
}
|
|
else
|
|
{
|
|
/* update baud rates */
|
|
QByteArray buffer;
|
|
sendDebug("Got signal to update extended bus speeds SWCAN: " + QString::number(swcanBaud) + " LIN1: " + QString::number(lin1Baud) + " LIN2: " + QString::number(lin2Baud));
|
|
buffer[0] = (char)0xF1; //start of a command over serial
|
|
buffer[1] = 14; //setup extended buses
|
|
buffer[2] = (char)(swcanBaud & 0xFF); //four bytes of ID LSB first
|
|
buffer[3] = (char)(swcanBaud >> 8);
|
|
buffer[4] = (char)(swcanBaud >> 16);
|
|
buffer[5] = (char)(swcanBaud >> 24);
|
|
buffer[6] = (char)(lin1Baud & 0xFF); //four bytes of ID LSB first
|
|
buffer[7] = (char)(lin1Baud >> 8);
|
|
buffer[8] = (char)(lin1Baud >> 16);
|
|
buffer[9] = (char)(lin1Baud >> 24);
|
|
buffer[10] = (char)(lin2Baud & 0xFF); //four bytes of ID LSB first
|
|
buffer[11] = (char)(lin2Baud >> 8);
|
|
buffer[12] = (char)(lin2Baud >> 16);
|
|
buffer[13] = (char)(lin2Baud >> 24);
|
|
buffer[14] = 0;
|
|
sendToSerial(buffer);
|
|
}
|
|
}
|
|
|
|
|
|
bool GVRetSerial::piSendFrame(const CANFrame& frame)
|
|
{
|
|
QByteArray buffer;
|
|
int c;
|
|
quint32 ID;
|
|
|
|
//qDebug() << "Sending out GVRET frame with id " << frame.ID << " on bus " << frame.bus;
|
|
|
|
framesRapid++;
|
|
|
|
if (serial == nullptr && tcpClient == nullptr && udpClient == nullptr) return false;
|
|
if (serial && !serial->isOpen()) return false;
|
|
if (tcpClient && !tcpClient->isOpen()) return false;
|
|
if (udpClient && !udpClient->isOpen()) return false;
|
|
//if (!isConnected) return false;
|
|
|
|
// Doesn't make sense to send an error frame
|
|
// to an adapter
|
|
if (frame.frameId() & 0x20000000) {
|
|
return true;
|
|
}
|
|
ID = frame.frameId();
|
|
if (frame.hasExtendedFrameFormat()) ID |= 1u << 31;
|
|
|
|
buffer[0] = (char)0xF1; //start of a command over serial
|
|
buffer[1] = 0; //command ID for sending a CANBUS frame
|
|
buffer[2] = (char)(ID & 0xFF); //four bytes of ID LSB first
|
|
buffer[3] = (char)(ID >> 8);
|
|
buffer[4] = (char)(ID >> 16);
|
|
buffer[5] = (char)(ID >> 24);
|
|
buffer[6] = (char)((frame.bus) & 3);
|
|
buffer[7] = (char)frame.payload().length();
|
|
for (c = 0; c < frame.payload().length(); c++)
|
|
{
|
|
buffer[8 + c] = frame.payload()[c];
|
|
}
|
|
buffer[8 + frame.payload().length()] = 0;
|
|
|
|
sendToSerial(buffer);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************/
|
|
|
|
void GVRetSerial::readSettings()
|
|
{
|
|
QSettings settings;
|
|
|
|
if (settings.value("Main/ValidateComm", true).toBool())
|
|
{
|
|
doValidation = true;
|
|
}
|
|
else doValidation = false;
|
|
}
|
|
|
|
|
|
void GVRetSerial::connectDevice()
|
|
{
|
|
QSettings settings;
|
|
|
|
/* disconnect device */
|
|
if(serial)
|
|
disconnectDevice();
|
|
if(tcpClient)
|
|
disconnectDevice();
|
|
if (udpClient)
|
|
disconnectDevice();
|
|
|
|
/* open new device */
|
|
|
|
if (useTcp)
|
|
{
|
|
// /*
|
|
sendDebug("TCP Connection to a GVRET device");
|
|
tcpClient = new QTcpSocket();
|
|
tcpClient->connectToHost(getPort(), 23);
|
|
connect(tcpClient, SIGNAL(readyRead()), this, SLOT(readSerialData()));
|
|
connect(tcpClient, SIGNAL(connected()), this, SLOT(deviceConnected()));
|
|
sendDebug("Created TCP Socket");
|
|
// */
|
|
/*
|
|
qDebug() << "UDP Connection to a GVRET device";
|
|
udpClient = new QUdpSocket();
|
|
udpClient->connectToHost(getPort(), 17222);
|
|
connect(udpClient, SIGNAL(readyRead()), this, SLOT(readSerialData()));
|
|
//connect(udpClient, SIGNAL(connected()), this, SLOT(tcpConnected()));
|
|
debugOutput("Created UDP Socket");
|
|
tcpConnected();
|
|
*/
|
|
}
|
|
else {
|
|
sendDebug("Serial connection to a GVRET device");
|
|
serial = new QSerialPort(QSerialPortInfo(getPort()));
|
|
if(!serial) {
|
|
sendDebug("can't open serial port " + getPort());
|
|
return;
|
|
}
|
|
sendDebug("Created Serial Port Object");
|
|
|
|
/* connect reading event */
|
|
connect(serial, SIGNAL(readyRead()), this, SLOT(readSerialData()));
|
|
connect(serial, SIGNAL(error(QSerialPort::SerialPortError)), this, SLOT(serialError(QSerialPort::SerialPortError)));
|
|
|
|
/* configure */
|
|
serial->setBaudRate(1000000); //most GVRET devices ignore baud, ESP32 needs it set explicitly to the proper value
|
|
serial->setDataBits(serial->Data8);
|
|
|
|
if (espSerialMode)
|
|
{
|
|
sendDebug("Trying ESP32 Serial Mode");
|
|
serial->setFlowControl(serial->NoFlowControl);
|
|
if (!serial->open(QIODevice::ReadWrite))
|
|
{
|
|
//sendDebug("Error returned during port opening: " + serial->errorString());
|
|
}
|
|
else
|
|
{
|
|
serial->setDataTerminalReady(false); //ESP32 uses these for bootloader selection and reset so turn them off
|
|
serial->setRequestToSend(false);
|
|
QTimer::singleShot(3000, this, SLOT(deviceConnected())); //give ESP32 some time as it could have rebooted
|
|
}
|
|
}
|
|
else
|
|
{
|
|
sendDebug("Trying Standard Serial Mode");
|
|
serial->setFlowControl(serial->HardwareControl); //Most GVRET style devices use hardware flow control
|
|
if (!serial->open(QIODevice::ReadWrite))
|
|
{
|
|
//sendDebug("Error returned during port opening: " + serial->errorString());
|
|
}
|
|
else
|
|
{
|
|
//serial->setDataTerminalReady(true); //Seemingly these two lines used to be needed
|
|
//serial->setRequestToSend(true); //But, really both ends should automatically handle these
|
|
deviceConnected();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void GVRetSerial::deviceConnected()
|
|
{
|
|
sendDebug("Connecting to GVRET Device!");
|
|
QByteArray output;
|
|
output.append((char)0xE7); //this puts the device into binary comm mode
|
|
output.append((char)0xE7);
|
|
|
|
output.append((char)0xF1);
|
|
output.append((char)0x0C); //get number of actually implemented buses. Not implemented except on M2RET
|
|
mNumBuses = 2; //the proper number if C/12 is not implemented
|
|
|
|
output.append((char)0xF1); //signal we want to issue a command
|
|
output.append((char)0x06); //request canbus stats from the board
|
|
|
|
output.append((char)0xF1); //another command to the GVRET
|
|
output.append((char)0x07); //request device information
|
|
|
|
/*output.append((char)0xF1);
|
|
output.append((char)0x08); //setting singlewire mode
|
|
if (settings.value("Main/SingleWireMode", false).toBool())
|
|
{
|
|
output.append((char)0x10); //signal that we do want single wire mode
|
|
}
|
|
else
|
|
{
|
|
output.append((char)0xFF); //signal we don't want single wire mode
|
|
}*/
|
|
|
|
output.append((char)0xF1); //and another command
|
|
output.append((char)0x01); //Time Sync - Not implemented until 333 but we can try
|
|
|
|
output.append((char)0xF1); //yet another command
|
|
output.append((char)0x09); //comm validation command
|
|
|
|
continuousTimeSync = true;
|
|
|
|
sendToSerial(output);
|
|
|
|
if(doValidation) {
|
|
QTimer::singleShot(5000, this, SLOT(connectionTimeout()));
|
|
}
|
|
else {
|
|
setStatus(CANCon::CONNECTED);
|
|
CANConStatus stats;
|
|
stats.conStatus = getStatus();
|
|
stats.numHardwareBuses = mNumBuses;
|
|
emit status(stats);
|
|
}
|
|
}
|
|
|
|
void GVRetSerial::disconnectDevice() {
|
|
if (serial != nullptr)
|
|
{
|
|
if (serial->isOpen())
|
|
{
|
|
//serial->clear();
|
|
serial->close();
|
|
|
|
}
|
|
serial->disconnect(); //disconnect all signals
|
|
delete serial;
|
|
serial = nullptr;
|
|
}
|
|
if (tcpClient != nullptr)
|
|
{
|
|
if (tcpClient->isOpen())
|
|
{
|
|
tcpClient->close();
|
|
}
|
|
tcpClient->disconnect();
|
|
delete tcpClient;
|
|
tcpClient = nullptr;
|
|
}
|
|
if (udpClient != nullptr)
|
|
{
|
|
if (udpClient->isOpen())
|
|
{
|
|
udpClient->close();
|
|
}
|
|
udpClient->disconnect();
|
|
delete udpClient;
|
|
udpClient = nullptr;
|
|
}
|
|
|
|
setStatus(CANCon::NOT_CONNECTED);
|
|
CANConStatus stats;
|
|
stats.conStatus = getStatus();
|
|
stats.numHardwareBuses = mNumBuses;
|
|
emit status(stats);
|
|
}
|
|
|
|
void GVRetSerial::serialError(QSerialPort::SerialPortError err)
|
|
{
|
|
QString errMessage;
|
|
bool killConnection = false;
|
|
switch (err)
|
|
{
|
|
case QSerialPort::NoError:
|
|
return;
|
|
case QSerialPort::DeviceNotFoundError:
|
|
errMessage = "Device not found error on serial";
|
|
killConnection = true;
|
|
piStop();
|
|
break;
|
|
case QSerialPort::PermissionError:
|
|
errMessage = "Permission error on serial port";
|
|
killConnection = true;
|
|
piStop();
|
|
break;
|
|
case QSerialPort::OpenError:
|
|
errMessage = "Open error on serial port";
|
|
killConnection = true;
|
|
piStop();
|
|
break;
|
|
#if QT_VERSION <= QT_VERSION_CHECK( 6, 0, 0 )
|
|
case QSerialPort::ParityError:
|
|
errMessage = "Parity error on serial port";
|
|
break;
|
|
case QSerialPort::FramingError:
|
|
errMessage = "Framing error on serial port";
|
|
break;
|
|
case QSerialPort::BreakConditionError:
|
|
errMessage = "Break error on serial port";
|
|
break;
|
|
#endif
|
|
case QSerialPort::WriteError:
|
|
errMessage = "Write error on serial port";
|
|
piStop();
|
|
break;
|
|
case QSerialPort::ReadError:
|
|
errMessage = "Read error on serial port";
|
|
piStop();
|
|
break;
|
|
case QSerialPort::ResourceError:
|
|
errMessage = "Serial port seems to have disappeared.";
|
|
killConnection = true;
|
|
piStop();
|
|
break;
|
|
case QSerialPort::UnsupportedOperationError:
|
|
errMessage = "Unsupported operation on serial port";
|
|
killConnection = true;
|
|
break;
|
|
case QSerialPort::UnknownError:
|
|
errMessage = "Beats me what happened to the serial port.";
|
|
killConnection = true;
|
|
piStop();
|
|
break;
|
|
case QSerialPort::TimeoutError:
|
|
errMessage = "Timeout error on serial port";
|
|
killConnection = true;
|
|
break;
|
|
case QSerialPort::NotOpenError:
|
|
errMessage = "The serial port isn't open";
|
|
killConnection = true;
|
|
piStop();
|
|
break;
|
|
}
|
|
/*
|
|
if (serial)
|
|
{
|
|
serial->clearError();
|
|
serial->flush();
|
|
serial->close();
|
|
}*/
|
|
if (errMessage.length() > 1)
|
|
{
|
|
sendDebug(errMessage);
|
|
}
|
|
if (killConnection)
|
|
{
|
|
qDebug() << "Shooting the serial object in the head. It deserves it.";
|
|
disconnectDevice();
|
|
}
|
|
}
|
|
|
|
|
|
void GVRetSerial::connectionTimeout()
|
|
{
|
|
//one second after trying to connect are we actually connected?
|
|
if (CANCon::NOT_CONNECTED==getStatus()) //no?
|
|
{
|
|
//then emit the the failure signal and see if anyone cares
|
|
sendDebug("Failed to connect to GVRET at that com port");
|
|
|
|
//toggle the serial mode and try again
|
|
espSerialMode = !espSerialMode;
|
|
disconnectDevice();
|
|
connectDevice();
|
|
}
|
|
else
|
|
{
|
|
/* start timer */
|
|
connect(&mTimer, SIGNAL(timeout()), this, SLOT(handleTick()));
|
|
mTimer.setInterval(250); //tick four times per second
|
|
mTimer.setSingleShot(false); //keep ticking
|
|
mTimer.start();
|
|
}
|
|
}
|
|
|
|
|
|
void GVRetSerial::readSerialData()
|
|
{
|
|
QByteArray data;
|
|
unsigned char c;
|
|
QString debugBuild;
|
|
|
|
if (serial) data = serial->readAll();
|
|
if (tcpClient) data = tcpClient->readAll();
|
|
if (udpClient) data = udpClient->readAll();
|
|
|
|
sendDebug("Got data from serial. Len = " % QString::number(data.length()));
|
|
for (int i = 0; i < data.length(); i++)
|
|
{
|
|
c = data.at(i);
|
|
//qDebug() << c << " " << QString::number(c, 16) << " " << QString(c);
|
|
debugBuild = debugBuild % QString::number(c, 16).rightJustified(2,'0') % " ";
|
|
procRXChar(c);
|
|
}
|
|
debugOutput(debugBuild);
|
|
//qDebug() << debugBuild;
|
|
}
|
|
|
|
//Debugging data sent from connection window. Inject it into Comm traffic.
|
|
void GVRetSerial::debugInput(QByteArray bytes) {
|
|
sendToSerial(bytes);
|
|
}
|
|
|
|
void GVRetSerial::procRXChar(unsigned char c)
|
|
{
|
|
CANConStatus stats;
|
|
int oldBuses;
|
|
QByteArray output;
|
|
|
|
switch (rx_state)
|
|
{
|
|
case IDLE:
|
|
if (c == 0xF1) rx_state = GET_COMMAND;
|
|
break;
|
|
case GET_COMMAND:
|
|
switch (c)
|
|
{
|
|
case 0: //receiving a can frame
|
|
rx_state = BUILD_CAN_FRAME;
|
|
rx_step = 0;
|
|
break;
|
|
case 1: //time sync
|
|
rx_state = TIME_SYNC;
|
|
rx_step = 0;
|
|
break;
|
|
case 2: //process a return reply for digital input states.
|
|
rx_state = GET_DIG_INPUTS;
|
|
rx_step = 0;
|
|
break;
|
|
case 3: //process a return reply for analog inputs
|
|
rx_state = GET_ANALOG_INPUTS;
|
|
break;
|
|
case 4: //we set digital outputs we don't accept replies so nothing here.
|
|
rx_state = IDLE;
|
|
break;
|
|
case 5: //we set canbus specs we don't accept replies.
|
|
rx_state = IDLE;
|
|
break;
|
|
case 6: //get canbus parameters from GVRET
|
|
rx_state = GET_CANBUS_PARAMS;
|
|
rx_step = 0;
|
|
break;
|
|
case 7: //get device info
|
|
rx_state = GET_DEVICE_INFO;
|
|
rx_step = 0;
|
|
break;
|
|
case 9:
|
|
validationCounter = 10;
|
|
qDebug() << "Got validated";
|
|
rx_state = IDLE;
|
|
break;
|
|
case 12:
|
|
rx_state = GET_NUM_BUSES;
|
|
qDebug() << "Got num buses reply";
|
|
rx_step = 0;
|
|
break;
|
|
case 13:
|
|
rx_state = GET_EXT_BUSES;
|
|
qDebug() << "Got extended buses info reply";
|
|
rx_step = 0;
|
|
break;
|
|
case 20:
|
|
rx_state = BUILD_FD_FRAME;
|
|
rx_step = 0;
|
|
break;
|
|
case 22:
|
|
rx_state = GET_FD_SETTINGS;
|
|
rx_step = 0;
|
|
qDebug() << "Got FD settings reply";
|
|
break;
|
|
}
|
|
break;
|
|
case BUILD_CAN_FRAME:
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
buildTimestamp = c;
|
|
break;
|
|
case 1:
|
|
buildTimestamp |= (uint)(c << 8);
|
|
break;
|
|
case 2:
|
|
buildTimestamp |= (uint)c << 16;
|
|
break;
|
|
case 3:
|
|
buildTimestamp |= (uint)c << 24;
|
|
|
|
buildTimestamp += timeBasis;
|
|
if (useSystemTime)
|
|
{
|
|
buildFrame.setTimeStamp(QCanBusFrame::TimeStamp::fromMicroSeconds(QDateTime::currentMSecsSinceEpoch() * 1000ul));
|
|
} else {
|
|
buildFrame.setTimeStamp(QCanBusFrame::TimeStamp(0, buildTimestamp));
|
|
}
|
|
break;
|
|
case 4:
|
|
buildId = c;
|
|
break;
|
|
case 5:
|
|
buildId |= c << 8;
|
|
break;
|
|
case 6:
|
|
buildId |= c << 16;
|
|
break;
|
|
case 7:
|
|
buildId |= c << 24;
|
|
if ((buildId & 1 << 31) == 1u << 31)
|
|
{
|
|
buildId &= 0x7FFFFFFF;
|
|
buildFrame.setExtendedFrameFormat(true);
|
|
}
|
|
else buildFrame.setExtendedFrameFormat(false);
|
|
buildFrame.setFrameId(buildId);
|
|
break;
|
|
case 8:
|
|
buildData.resize(c & 0xF);
|
|
buildFrame.bus = (c & 0xF0) >> 4;
|
|
break;
|
|
default:
|
|
if (rx_step < buildData.length() + 9)
|
|
{
|
|
buildData[rx_step - 9] = c;
|
|
if (rx_step == buildData.length() + 8) //it's the last data byte so immediately process the frame
|
|
{
|
|
rx_state = IDLE;
|
|
rx_step = 0;
|
|
buildFrame.isReceived = true;
|
|
buildFrame.setPayload(buildData);
|
|
buildFrame.setFrameType(QCanBusFrame::FrameType::DataFrame);
|
|
if (!isCapSuspended())
|
|
{
|
|
/* get frame from queue */
|
|
CANFrame* frame_p = getQueue().get();
|
|
if(frame_p) {
|
|
//qDebug() << "GVRET got frame on bus " << frame_p->bus;
|
|
/* copy frame */
|
|
*frame_p = buildFrame;
|
|
checkTargettedFrame(buildFrame);
|
|
/* enqueue frame */
|
|
getQueue().queue();
|
|
}
|
|
else
|
|
qDebug() << "can't get a frame, ERROR";
|
|
|
|
//take the time the frame came in and try to resync the time base.
|
|
//if (continuousTimeSync) txTimestampBasis = QDateTime::currentMSecsSinceEpoch() - (buildFrame.timestamp / 1000);
|
|
}
|
|
}
|
|
}
|
|
else //should never get here! But, just in case, reset the comm
|
|
{
|
|
rx_state = IDLE;
|
|
rx_step = 0;
|
|
}
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
case BUILD_FD_FRAME:
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
buildTimestamp = c;
|
|
break;
|
|
case 1:
|
|
buildTimestamp |= (uint)(c << 8);
|
|
break;
|
|
case 2:
|
|
buildTimestamp |= (uint)c << 16;
|
|
break;
|
|
case 3:
|
|
buildTimestamp |= (uint)c << 24;
|
|
|
|
buildTimestamp += timeBasis;
|
|
if (useSystemTime)
|
|
{
|
|
buildFrame.setTimeStamp(QCanBusFrame::TimeStamp::fromMicroSeconds(QDateTime::currentMSecsSinceEpoch() * 1000ul));
|
|
} else {
|
|
buildFrame.setTimeStamp(QCanBusFrame::TimeStamp(0, buildTimestamp));
|
|
}
|
|
break;
|
|
case 4:
|
|
buildId = c;
|
|
break;
|
|
case 5:
|
|
buildId |= c << 8;
|
|
break;
|
|
case 6:
|
|
buildId |= c << 16;
|
|
break;
|
|
case 7:
|
|
buildId |= c << 24;
|
|
if ((buildId & 1 << 31) == 1u << 31)
|
|
{
|
|
buildId &= 0x7FFFFFFF;
|
|
buildFrame.setExtendedFrameFormat(true);
|
|
}
|
|
else buildFrame.setExtendedFrameFormat(false);
|
|
buildFrame.setFrameId(buildId);
|
|
break;
|
|
case 8:
|
|
buildData.resize(c & 0x3F);
|
|
break;
|
|
case 9:
|
|
buildFrame.bus = c;
|
|
break;
|
|
default:
|
|
if (rx_step < buildData.length() + 10)
|
|
{
|
|
buildData[rx_step - 9] = c;
|
|
}
|
|
else
|
|
{
|
|
rx_state = IDLE;
|
|
rx_step = 0;
|
|
buildFrame.isReceived = true;
|
|
buildFrame.setPayload(buildData);
|
|
buildFrame.setFrameType(QCanBusFrame::FrameType::DataFrame);
|
|
if (!isCapSuspended())
|
|
{
|
|
/* get frame from queue */
|
|
CANFrame* frame_p = getQueue().get();
|
|
if(frame_p) {
|
|
//qDebug() << "GVRET got frame on bus " << frame_p->bus;
|
|
/* copy frame */
|
|
*frame_p = buildFrame;
|
|
checkTargettedFrame(buildFrame);
|
|
/* enqueue frame */
|
|
getQueue().queue();
|
|
}
|
|
else
|
|
qDebug() << "can't get a frame, ERROR";
|
|
|
|
//take the time the frame came in and try to resync the time base.
|
|
//if (continuousTimeSync) txTimestampBasis = QDateTime::currentMSecsSinceEpoch() - (buildFrame.timestamp / 1000);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
case TIME_SYNC: //gives a pretty good base guess for the proper timestamp. Can be refined when traffic starts to flow (if wanted)
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
buildTimeBasis = c;
|
|
break;
|
|
case 1:
|
|
buildTimeBasis += ((uint32_t)c << 8);
|
|
break;
|
|
case 2:
|
|
buildTimeBasis += ((uint32_t)c << 16);
|
|
break;
|
|
case 3:
|
|
buildTimeBasis += ((uint32_t)c << 24);
|
|
qDebug() << "GVRET firmware reports timestamp of " << buildTimeBasis;
|
|
timeAtGVRETSync = QDateTime::currentMSecsSinceEpoch() * 1000;
|
|
|
|
rebuildLocalTimeBasis();
|
|
|
|
continuousTimeSync = false;
|
|
rx_state = IDLE;
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
|
|
case GET_ANALOG_INPUTS: //get 9 bytes - 2 per analog input plus checksum
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
case GET_DIG_INPUTS: //get two bytes. One for digital in status and one for checksum.
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
break;
|
|
case 1:
|
|
rx_state = IDLE;
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
case GET_CANBUS_PARAMS:
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
can0Enabled = (c & 0xF);
|
|
can0ListenOnly = (c >> 4);
|
|
break;
|
|
case 1:
|
|
can0Baud = c;
|
|
break;
|
|
case 2:
|
|
can0Baud |= c << 8;
|
|
break;
|
|
case 3:
|
|
can0Baud |= c << 16;
|
|
break;
|
|
case 4:
|
|
can0Baud |= c << 24;
|
|
break;
|
|
case 5:
|
|
can1Enabled = (c & 0xF);
|
|
can1ListenOnly = (c >> 4);
|
|
deviceSingleWireMode = (c >> 6);
|
|
break;
|
|
case 6:
|
|
can1Baud = c;
|
|
break;
|
|
case 7:
|
|
can1Baud |= c << 8;
|
|
break;
|
|
case 8:
|
|
can1Baud |= c << 16;
|
|
break;
|
|
case 9:
|
|
can1Baud |= c << 24;
|
|
rx_state = IDLE;
|
|
qDebug() << "Baud 0 = " << can0Baud;
|
|
qDebug() << "Baud 1 = " << can1Baud;
|
|
mBusData[0].mBus.setSpeed(can0Baud);
|
|
mBusData[0].mBus.setActive(can0Enabled);
|
|
mBusData[0].mConfigured = true;
|
|
if (mBusData.count() > 1)
|
|
{
|
|
mBusData[1].mBus.setSpeed(can1Baud);
|
|
mBusData[1].mBus.setActive(can1Enabled);
|
|
mBusData[1].mConfigured = true;
|
|
}
|
|
|
|
can0Baud |= 0x80000000;
|
|
if (can0Enabled) can0Baud |= 0x40000000;
|
|
if (can0ListenOnly) can0Baud |= 0x20000000;
|
|
|
|
can1Baud |= 0x80000000;
|
|
if (can1Enabled) can1Baud |= 0x40000000;
|
|
if (can1ListenOnly) can1Baud |= 0x20000000;
|
|
if (deviceSingleWireMode > 0) can1Baud |= 0x10000000;
|
|
|
|
setStatus(CANCon::CONNECTED);
|
|
stats.conStatus = getStatus();
|
|
stats.numHardwareBuses = mNumBuses;
|
|
emit status(stats);
|
|
|
|
int can0Status = 0x78; //updating everything we can update
|
|
int can1Status = 0x78;
|
|
if (can0Enabled) can0Status +=1;
|
|
if (can0ListenOnly) can0Status += 4;
|
|
if (can1Enabled) can1Status += 1;
|
|
if (deviceSingleWireMode > 0) can1Status += 2;
|
|
if (can1ListenOnly) can1Status += 4;
|
|
//emit busStatus(busBase, can0Baud & 0xFFFFF, can0Status);
|
|
//emit busStatus(busBase + 1, can1Baud & 0xFFFFF, can1Status);
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
case GET_DEVICE_INFO:
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
deviceBuildNum = c;
|
|
break;
|
|
case 1:
|
|
deviceBuildNum |= c << 8;
|
|
break;
|
|
case 2:
|
|
break; //don't care about eeprom version
|
|
case 3:
|
|
break; //don't care about file type
|
|
case 4:
|
|
break; //don't care about whether it auto logs or not
|
|
case 5:
|
|
deviceSingleWireMode = c;
|
|
rx_state = IDLE;
|
|
qDebug() << "build num: " << deviceBuildNum;
|
|
qDebug() << "single wire can: " << deviceSingleWireMode;
|
|
emit deviceInfo(deviceBuildNum, deviceSingleWireMode);
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
case SET_DIG_OUTPUTS:
|
|
rx_state = IDLE;
|
|
break;
|
|
case SETUP_CANBUS:
|
|
rx_state = IDLE;
|
|
break;
|
|
case SET_SINGLEWIRE_MODE:
|
|
rx_state = IDLE;
|
|
break;
|
|
case GET_NUM_BUSES:
|
|
oldBuses = mNumBuses;
|
|
mNumBuses = c;
|
|
rx_state = IDLE;
|
|
qDebug() << "Get number of buses = " << mNumBuses;
|
|
stats.conStatus = getStatus();
|
|
stats.numHardwareBuses = mNumBuses;
|
|
mBusData.resize(mNumBuses);
|
|
if (mNumBuses > oldBuses)
|
|
{
|
|
for (int i = oldBuses; i < mNumBuses; i++)
|
|
{
|
|
mBusData[i].mConfigured = true;
|
|
mBusData[i].mBus = mBusData[0].mBus;
|
|
}
|
|
}
|
|
|
|
output.append((unsigned char)0xF1); //start a new command
|
|
output.append((unsigned char)13); //get extended buses
|
|
sendToSerial(output);
|
|
|
|
emit status(stats);
|
|
break;
|
|
case GET_FD_SETTINGS:
|
|
break;
|
|
case GET_EXT_BUSES:
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
swcanEnabled = (c & 0xF);
|
|
swcanListenOnly = (c >> 4);
|
|
break;
|
|
case 1:
|
|
swcanBaud = c;
|
|
break;
|
|
case 2:
|
|
swcanBaud |= c << 8;
|
|
break;
|
|
case 3:
|
|
swcanBaud |= c << 16;
|
|
break;
|
|
case 4:
|
|
swcanBaud |= c << 24;
|
|
break;
|
|
case 5:
|
|
lin1Enabled = (c & 0xF);
|
|
break;
|
|
case 6:
|
|
lin1Baud = c;
|
|
break;
|
|
case 7:
|
|
lin1Baud |= c << 8;
|
|
break;
|
|
case 8:
|
|
lin1Baud |= c << 16;
|
|
break;
|
|
case 9:
|
|
lin1Baud |= c << 24;
|
|
break;
|
|
case 10:
|
|
lin2Enabled = (c & 0xF);
|
|
break;
|
|
case 11:
|
|
lin2Baud = c;
|
|
break;
|
|
case 12:
|
|
lin2Baud |= c << 8;
|
|
break;
|
|
case 13:
|
|
lin2Baud |= c << 16;
|
|
break;
|
|
case 14:
|
|
lin2Baud |= c << 24;
|
|
rx_state = IDLE;
|
|
qDebug() << "SWCAN Baud = " << swcanBaud;
|
|
qDebug() << "LIN1 Baud = " << lin1Baud;
|
|
qDebug() << "LIN2 Baud = " << lin2Baud;
|
|
if (getNumBuses() > 2)
|
|
{
|
|
mBusData[2].mBus.setSpeed(swcanBaud);
|
|
mBusData[2].mBus.setActive(swcanEnabled);
|
|
}
|
|
|
|
setStatus(CANCon::CONNECTED);
|
|
stats.conStatus = getStatus();
|
|
stats.numHardwareBuses = mNumBuses;
|
|
emit status(stats);
|
|
break;
|
|
}
|
|
rx_step++;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void GVRetSerial::rebuildLocalTimeBasis()
|
|
{
|
|
qDebug() << "Rebuilding GVRET time base. GVRET local base = " << buildTimeBasis;
|
|
|
|
/*
|
|
our time basis is the value we have to modulate the main system basis by in order
|
|
to sync the GVRET timestamps to the rest of the system.
|
|
The rest of the system uses CANConManager::getInstance()->getTimeBasis as the basis.
|
|
GVRET returns to us the current time since boot up in microseconds.
|
|
timeAtGVRETSync stores the "system" timestamp when the GVRET timestamp was retrieved.
|
|
*/
|
|
lastSystemTimeBasis = CANConManager::getInstance()->getTimeBasis();
|
|
int64_t systemDelta = timeAtGVRETSync - lastSystemTimeBasis;
|
|
int32_t localDelta = buildTimeBasis - systemDelta;
|
|
timeBasis = -localDelta;
|
|
}
|
|
|
|
void GVRetSerial::handleTick()
|
|
{
|
|
if (lastSystemTimeBasis != CANConManager::getInstance()->getTimeBasis()) rebuildLocalTimeBasis();
|
|
//qDebug() << "Tick!";
|
|
|
|
if( CANCon::CONNECTED == getStatus() )
|
|
{
|
|
if (doValidation) validationCounter--;
|
|
//qDebug() << validationCounter;
|
|
if (validationCounter == 0 && doValidation)
|
|
{
|
|
if (serial == nullptr && tcpClient == nullptr) return;
|
|
if ( (serial && serial->isOpen()) || (tcpClient && tcpClient->isOpen()) || (udpClient && udpClient->isOpen())) //if it's still false we have a problem...
|
|
{
|
|
sendDebug("Comm validation failed.");
|
|
|
|
setStatus(CANCon::NOT_CONNECTED);
|
|
//emit status(getStatus());
|
|
|
|
disconnectDevice(); //start by stopping everything.
|
|
//Then wait 500ms and restart the connection automatically
|
|
//QTimer::singleShot(500, this, SLOT(connectDevice()));
|
|
return;
|
|
}
|
|
}
|
|
else if (doValidation)
|
|
{
|
|
//qDebug() << "Comm connection validated";
|
|
}
|
|
}
|
|
if (doValidation && serial && serial->isOpen()) sendCommValidation();
|
|
if (doValidation && tcpClient && tcpClient->isOpen()) sendCommValidation();
|
|
if (doValidation && udpClient && udpClient->isOpen()) sendCommValidation();
|
|
}
|
|
|
|
|
|
void GVRetSerial::sendCommValidation()
|
|
{
|
|
QByteArray output;
|
|
|
|
output.append((unsigned char)0xF1); //another command to the GVRET
|
|
output.append((unsigned char)0x09); //request a reply to get validation
|
|
|
|
sendToSerial(output);
|
|
}
|
|
|
|
|
|
|