void GPRMC::load(const QString& bytes)
{
clear();
QStringList strList = bytes.trimmed().split(QChar(','));
quint8 refXor = (quint8)strList[12].split(QChar('*'))[1].toUInt(0,16);
dateTime.setTime(QTime::fromString(strList[1],"hhmmss.sss"));
dateTime.setDate(QDate::fromString(strList[9],"DDMMYY"));
if(refXor != checkXor(bytes.toUtf8()))
{
qDebug("Xor error \n");
return;
}
// qDebug("refXor %x,checkXor %x\n",refXor,checkXor(bytes.toUtf8()));
_isValid = true;
state = strList[2].toUtf8().data()[0];
latitude = strList[3].toDouble(); //北负南正
if(strList[4] == "N")
latitude *= -1;
longitude = strList[5].toDouble(); //西负东正
if(strList[6] == "W")
longitude *= -1;
latitude = (int)latitude/100 + fmod(latitude,100)/60;
longitude = (int)longitude/100 + fmod(longitude,100)/60;
// latitude += 22.5060938;
// longitude += -15.1757683;
speed = strList[7].toDouble()*1.852;
}
bool dataHandler(uint8_t *dataRecv, uint8_t *dataSend, uint8_t *dataSendLen)
{
TransPacket *dataPack = (TransPacket *)dataRecv;
TransPacket *dataHandle = (TransPacket *)dataSend;
if(dataPack->header != PC_HEADER)
return false;
if(checkXor(&dataPack->dataBuf[0], dataPack->len, &dataPack->dataBuf[dataPack->len]))
return false;
dataHandle->header = dataPack->header;
switch(dataPack->cmd)
{
case 0x01:
dataHandle->len = 0;
case 0x02:
commanderGetPacket(dataPack);
break;
case 0x03:
velocityGetPID(dataPack);
break;
case 0x04:
turnGetPID(dataPack);
break;
case 0x10:
accBack(dataHandle);
break;
case 0x11:
gyroBack(dataHandle);
break;
case 0x12:
magBack(dataHandle);
break;
case 0x13:
eulerBack(dataHandle);
break;
case 0x14:
motorBack(dataHandle);
break;
case 0x15:
// velocityPIDBack(dataHandle);
break;
case 0x16:
turnPIDBack(dataHandle);
break;
}
generateXor(&dataHandle->dataBuf[0], dataHandle->len, &dataHandle->dataBuf[dataHandle->len]);
*dataSendLen = dataHandle->len + 5;
return true;
}
void GPRMC::loadGGA(const QString& bytes)
{
QStringList strList = bytes.trimmed().split(QChar(','));
if(strList.length()<15)
return;
QStringList tmpList = strList[14].split(QChar('*'));
if(tmpList.length()<2)
return;
quint8 refXor = (quint8)tmpList[1].toUInt(0,16);
if(refXor != checkXor(bytes.toUtf8()))
{
qDebug("GPGGAXor error \n");
qDebug()<<bytes;
qDebug("sim:%x : %x",refXor, checkXor(bytes.toUtf8()));
return;
}
satelliteNr = strList[7].toInt();
// qDebug("refXor %x,checkXor %x\n",refXor,checkXor(bytes.toUtf8()));
_isValid = true;
}
void GPRMC::load(const QString& bytes)
{
QStringList strList = bytes.trimmed().split(QChar(','));
if(strList.length()<13)
return;
QStringList tmpList = strList[12].split(QChar('*'));
if(tmpList.length()<2)
return;
quint8 refXor = (quint8)tmpList[1].toUInt(0,16);
if(refXor != checkXor(bytes.toUtf8()))
{
qDebug("GPRMC Xor error \n");
qDebug()<<bytes;
qDebug("sim:%x : %x",refXor, checkXor(bytes.toUtf8()));
return;
}
dateTime.setTime(QTime::fromString(strList[1]+"0","hhmmss.zzz"));
// qDebug()<<"time : "<<strList[1];
dateTime.setDate(QDate::fromString(strList[9].mid(0,4)+"20"+strList[9].mid(4,2),"ddMMyyyy"));
dateTime = dateTime.addSecs(7*60*60);
// qDebug("refXor %x,checkXor %x\n",refXor,checkXor(bytes.toUtf8()));
_isValid = true;
state = strList[2].toUtf8().data()[0];
latitude = strList[3].toDouble(); //北负南正
if(strList[4] == "N")
latitude *= -1;
longitude = strList[5].toDouble(); //西负东正
if(strList[6] == "W")
longitude *= -1;
latitude = (int)latitude/100 + fmod(latitude,100)/60;
longitude = (int)longitude/100 + fmod(longitude,100)/60;
// latitude += 22.5060938;
// longitude += -15.1757683;
speed = strList[7].toDouble()*1.852;
}
int receiveFile(int sock){
fp = fopen(file_path,"w");
if(fp == NULL)
{
perror("open");
exit(EXIT_FAILURE);
}
int test =4;
while(1){
char buf[BUFFSIZE];
test--;
if(recvfrom(sock, &buf, BUFFSIZE, 0, (struct sockaddr*) &sin6_cli, &sin6len) == -1){
perror("Connection Lost");
exit(EXIT_FAILURE);
}
int type = readType(buf[0]);
int seq = readSeqNumber(buf[1]);
printf("Received test:%d seq:%d\n", test, seq);
if(type == PTYPE_DATA && test !=0){
window_start = (window_start+1)%MAXWINDOWSIZE;
if(seq == seq_number && xor_count !=0){
memcpy(&buffer[(XORFREQ-xor_count)*PAYLOADSIZE], &buf[4], PAYLOADSIZE);
msg_rvcd++;
xor_count--;
seq_number = (seq_number+1)%256;
window_end = (window_end+ 1)%MAXWINDOWSIZE;
sendMsg(sock, PTYPE_ACK, seq_number, window_end);
printf("Ack sent %d %d\n", seq_number, window_end);
last_length =readLength(buf);
if(last_length<512){
xor_count=0;
}
}else{// else discard packet, send last ack again.
sendMsg(sock, PTYPE_ACK, seq_number, window_end);
}
}else if(type == PTYPE_XOR){
computeXor(buffer);
int seq = readSeqNumber(buf[1]);
if(seq == seq_number && xor_count == 0){
if(checkXor(buf)){
xor_count = XORFREQ;
seq_number= (seq_number+1)%256;
window_end = (window_end+ 1)%MAXWINDOWSIZE;
sendMsg(sock, PTYPE_ACK, seq_number, window_end);
if(last_length<512){return 0;}
msg_rvcd=0;
}else{
// Make choice if XOR is wrong
/* No ack sent, all window will be retransmitted*/
}
}
}
if(test ==0){
test = 1000;
}
}
fclose(fp);
}
