677 lines
19 KiB
C++
677 lines
19 KiB
C++
#include <QObject>
|
|
#include <QDebug>
|
|
#include <QCanBusFrame>
|
|
#include <QSerialPortInfo>
|
|
#include <QSettings>
|
|
#include <QStringBuilder>
|
|
|
|
#include "gvretserial.h"
|
|
|
|
GVRetSerial::GVRetSerial(QString portName) :
|
|
CANConnection(portName, CANCon::GVRET_SERIAL, 3, 4000, true),
|
|
mTimer(this) /*NB: set this as parent of timer to manage it from working thread */
|
|
{
|
|
qDebug() << "GVRetSerial()";
|
|
debugOutput("GVRetSerial()");
|
|
|
|
serial = NULL;
|
|
rx_state = IDLE;
|
|
rx_step = 0;
|
|
gotValidated = true;
|
|
isAutoRestart = false;
|
|
|
|
timeBasis = 0;
|
|
lastSystemTimeBasis = 0;
|
|
timeAtGVRETSync = 0;
|
|
|
|
readSettings();
|
|
}
|
|
|
|
|
|
GVRetSerial::~GVRetSerial()
|
|
{
|
|
stop();
|
|
qDebug() << "~GVRetSerial()";
|
|
debugOutput("~GVRetSerial()");
|
|
}
|
|
|
|
|
|
void GVRetSerial::piStarted()
|
|
{
|
|
connectDevice();
|
|
|
|
/* 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::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;
|
|
}
|
|
|
|
/* update baud rates */
|
|
QByteArray buffer;
|
|
qDebug() << "Got signal to update bauds. 1: " << can0Baud <<" 2: " << can1Baud;
|
|
debugOutput("Got signal to update bauds. 1: " + QString::number(can0Baud) + " 2: " + QString::number(can1Baud));
|
|
buffer[0] = (char)0xF1; //start of a command over serial
|
|
buffer[1] = 5; //setup canbus
|
|
buffer[2] = (unsigned char)(can0Baud & 0xFF); //four bytes of ID LSB first
|
|
buffer[3] = (unsigned char)(can0Baud >> 8);
|
|
buffer[4] = (unsigned char)(can0Baud >> 16);
|
|
buffer[5] = (unsigned char)(can0Baud >> 24);
|
|
buffer[6] = (unsigned char)(can1Baud & 0xFF); //four bytes of ID LSB first
|
|
buffer[7] = (unsigned char)(can1Baud >> 8);
|
|
buffer[8] = (unsigned char)(can1Baud >> 16);
|
|
buffer[9] = (unsigned char)(can1Baud >> 24);
|
|
buffer[10] = 0;
|
|
if (serial == NULL) return;
|
|
if (!serial->isOpen()) return;
|
|
serial->write(buffer);
|
|
}
|
|
|
|
|
|
bool GVRetSerial::piSendFrame(const CANFrame& frame)
|
|
{
|
|
QByteArray buffer;
|
|
unsigned int c;
|
|
int ID;
|
|
|
|
//qDebug() << "Sending out GVRET frame with id " << frame.ID << " on bus " << frame.bus;
|
|
|
|
framesRapid++;
|
|
|
|
if (serial == NULL) return false;
|
|
if (!serial->isOpen()) return false;
|
|
//if (!isConnected) return false;
|
|
|
|
ID = frame.ID;
|
|
if (frame.extended) ID |= 1 << 31;
|
|
|
|
buffer[0] = (char)0xF1; //start of a command over serial
|
|
buffer[1] = 0; //command ID for sending a CANBUS frame
|
|
buffer[2] = (unsigned char)(ID & 0xFF); //four bytes of ID LSB first
|
|
buffer[3] = (unsigned char)(ID >> 8);
|
|
buffer[4] = (unsigned char)(ID >> 16);
|
|
buffer[5] = (unsigned char)(ID >> 24);
|
|
buffer[6] = (unsigned char)((frame.bus) & 3);
|
|
buffer[7] = (unsigned char)frame.len;
|
|
for (c = 0; c < frame.len; c++)
|
|
{
|
|
buffer[8 + c] = frame.data[c];
|
|
}
|
|
buffer[8 + frame.len] = 0;
|
|
|
|
//qDebug() << "writing " << buffer.length() << " bytes to serial port";
|
|
debugOutput("writing " + QString::number(buffer.length()) + " bytes to serial port");
|
|
serial->write(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();
|
|
|
|
/* open new device */
|
|
serial = new QSerialPort(QSerialPortInfo(getPort()));
|
|
if(!serial) {
|
|
qDebug() << "can't open serial port " << getPort();
|
|
debugOutput("can't open serial port " + getPort());
|
|
return;
|
|
}
|
|
|
|
/* configure */
|
|
serial->setDataBits(serial->Data8);
|
|
serial->setFlowControl(serial->HardwareControl); //this is important though
|
|
if (!serial->open(QIODevice::ReadWrite))
|
|
{
|
|
qDebug() << serial->errorString();
|
|
}
|
|
serial->setDataTerminalReady(true); //you do need to set these or the fan gets dirty
|
|
serial->setRequestToSend(true);
|
|
|
|
|
|
QByteArray output;
|
|
output.append((char)0xE7); //this puts the device into binary comm mode
|
|
output.append((char)0xE7);
|
|
|
|
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); //yet another command
|
|
output.append((char)0x09); //comm validation command
|
|
|
|
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);
|
|
output.append((char)0x12); //get number of actually implemented buses. Not implemented except on M2RET
|
|
mNumBuses = 2; //the proper number if 0x12 is not implemented
|
|
|
|
continuousTimeSync = true;
|
|
|
|
serial->write(output);
|
|
QString buildDebug;
|
|
buildDebug = "Write to serial -> ";
|
|
foreach (int byt, output) {
|
|
buildDebug = buildDebug % QString::number(byt, 16) % " ";
|
|
}
|
|
debugOutput(buildDebug);
|
|
|
|
if(doValidation) {
|
|
QTimer::singleShot(1000, this, SLOT(connectionTimeout()));
|
|
}
|
|
else {
|
|
setStatus(CANCon::CONNECTED);
|
|
emit status(getStatus());
|
|
}
|
|
|
|
/* connect reading event */
|
|
connect(serial, SIGNAL(readyRead()), this, SLOT(readSerialData()));
|
|
}
|
|
|
|
|
|
void GVRetSerial::disconnectDevice() {
|
|
if (serial != NULL)
|
|
{
|
|
if (serial->isOpen())
|
|
{
|
|
serial->clear();
|
|
serial->close();
|
|
|
|
}
|
|
serial->disconnect(); //disconnect all signals
|
|
delete serial;
|
|
serial = NULL;
|
|
}
|
|
}
|
|
|
|
|
|
void GVRetSerial::connectionTimeout()
|
|
{
|
|
//one second after trying to connect are we actually connected?
|
|
//if (CANCon::NOT_CONNECTED==getStatus()) //no?
|
|
if (!gotValidated)
|
|
{
|
|
//then emit the the failure signal and see if anyone cares
|
|
qDebug() << "Failed to connect to GVRET at that com port";
|
|
|
|
disconnectDevice();
|
|
}
|
|
}
|
|
|
|
|
|
void GVRetSerial::readSerialData()
|
|
{
|
|
QByteArray data = serial->readAll();
|
|
unsigned char c;
|
|
QString debugBuild;
|
|
debugOutput("Got data from serial. Len = " % QString::number(data.length()));
|
|
//qDebug() << (tr("Got data from serial. Len = %0").arg(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) % " ";
|
|
procRXChar(c);
|
|
}
|
|
debugOutput(debugBuild);
|
|
}
|
|
|
|
//Debugging data sent from connection window. Inject it into Comm traffic.
|
|
void GVRetSerial::debugInput(QByteArray bytes) {
|
|
serial->write(bytes);
|
|
}
|
|
|
|
void GVRetSerial::procRXChar(unsigned char c)
|
|
{
|
|
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:
|
|
gotValidated = true;
|
|
//qDebug() << "Got validated";
|
|
rx_state = IDLE;
|
|
break;
|
|
case 12:
|
|
rx_state = GET_NUM_BUSES;
|
|
rx_step = 0;
|
|
break;
|
|
}
|
|
break;
|
|
case BUILD_CAN_FRAME:
|
|
switch (rx_step)
|
|
{
|
|
case 0:
|
|
buildFrame.timestamp = c;
|
|
break;
|
|
case 1:
|
|
buildFrame.timestamp |= (uint)(c << 8);
|
|
break;
|
|
case 2:
|
|
buildFrame.timestamp |= (uint)c << 16;
|
|
break;
|
|
case 3:
|
|
buildFrame.timestamp |= (uint)c << 24;
|
|
|
|
buildFrame.timestamp += timeBasis;
|
|
if (useSystemTime)
|
|
{
|
|
buildFrame.timestamp = QDateTime::currentMSecsSinceEpoch() * 1000l;
|
|
}
|
|
break;
|
|
case 4:
|
|
buildFrame.ID = c;
|
|
break;
|
|
case 5:
|
|
buildFrame.ID |= c << 8;
|
|
break;
|
|
case 6:
|
|
buildFrame.ID |= c << 16;
|
|
break;
|
|
case 7:
|
|
buildFrame.ID |= c << 24;
|
|
if ((buildFrame.ID & 1 << 31) == 1u << 31)
|
|
{
|
|
buildFrame.ID &= 0x7FFFFFFF;
|
|
buildFrame.extended = true;
|
|
}
|
|
else buildFrame.extended = false;
|
|
break;
|
|
case 8:
|
|
buildFrame.len = c & 0xF;
|
|
if (buildFrame.len > 8) buildFrame.len = 8;
|
|
buildFrame.bus = (c & 0xF0) >> 4;
|
|
break;
|
|
default:
|
|
if (rx_step < buildFrame.len + 9)
|
|
{
|
|
buildFrame.data[rx_step - 9] = c;
|
|
}
|
|
else
|
|
{
|
|
rx_state = IDLE;
|
|
rx_step = 0;
|
|
buildFrame.isReceived = true;
|
|
|
|
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;
|
|
|
|
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);
|
|
emit status(getStatus());
|
|
|
|
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:
|
|
mNumBuses = c;
|
|
rx_state = IDLE;
|
|
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 (!gotValidated && doValidation)
|
|
{
|
|
if (serial == NULL) return;
|
|
if (serial->isOpen()) //if it's still false we have a problem...
|
|
{
|
|
qDebug() << "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;
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
if (doValidation && serial && serial->isOpen()) sendCommValidation();
|
|
}
|
|
|
|
|
|
void GVRetSerial::sendCommValidation()
|
|
{
|
|
QByteArray output;
|
|
|
|
gotValidated = false;
|
|
output.append((char)0xF1); //another command to the GVRET
|
|
output.append((char)0x09); //request a reply to get validation
|
|
//send it twice for good measure.
|
|
output.append((char)0xF1); //another command to the GVRET
|
|
output.append((char)0x09); //request a reply to get validation
|
|
|
|
serial->write(output);
|
|
}
|
|
|
|
|
|
|