/** Send content of the local sendBuffer to ALL destination peers.
* It can happen that this function immediatly fills the local receive-buffer.
* At the moment there are no more effects on the internal buffers.
* Make sure that all IM messages are sent before this method is called.
* However, the internal send buffers should be cleared after this call. */
static F2FError f2fSend(F2FPeer * srcPeer, F2FPeer * dstPeer, F2FGroup * group)
{
F2FError error;
F2FMessageHeader * hdr;
while(sendBuffer.peercount > 0)
{
/* Take the first peer and process message to it */
F2FPeer * peer = sendBuffer.peerList[sendBuffer.peercount - 1];
/* Find the communication provider, which helps
* us to reach the destination */
switch( peer->activeprovider )
{
case F2FProviderMyself:
/* here we can just evaluate the received data */
hdr = (F2FMessageHeader *) & sendBuffer.buffer;
error = parseBuffer( srcPeer, dstPeer, group,
hdr->messagetype, sendBuffer.buffer + sizeof(F2FMessageHeader),
ntohl(hdr->len) );
if(error!=F2FErrOK) return error;
break;
case F2FProviderTCPIPV4:
break;
/* TODO: implement */
case F2FProviderUDPHolePunch:
/* TODO: implement */
break;
case F2FProviderIM:
break;
}
sendBuffer.peercount --;
}
// should be done automatically f2fSetSentBufferEmpty(); /* Release the SentBuffer again */
return F2FErrOK;
}
/** Send data to specific peer in a group */
F2FError f2fPeerSendData( F2FGroup *group, F2FPeer *peer,
F2FMessageType type,
const char *data, const F2FWord32 dataLen )
{
if( peer == myself)
{
return parseBuffer(myself,myself,group,type,data,dataLen);
}
if(! f2fTestSentBufferEmpty())
return F2FErrBufferFull;
/* TODO: make sure, bigger blocks can be sent !!! */
F2FError error = prepareBufferWithData( sendBuffer.buffer, group, peer, type, data, dataLen );
if (error != F2FErrOK ) return error;
/* prepare the right destinations for the buffer
* in terms of provider */
if (peer->activeprovider == F2FProviderIM )
{
F2FAdapterReceiveMessage * freebuffer = f2fAdapterReceiveBufferReserve();
if( freebuffer == NULL )
return F2FErrBufferFull;
/* fill the IM sendbuffer */
error = encodeIMMessage( sendBuffer.buffer, sendBuffer.size, freebuffer );
if ( error!= F2FErrOK ) return error;
freebuffer->localPeerIDlist[0] = peer->localPeerId;
freebuffer->peercount = 1;
freebuffer->buffertype = F2FAdapterReceiveMessageTypeIMForward;
}
else
{
sendBuffer.peerList[0] = peer;
sendBuffer.peercount = 1;
return f2fSend(myself, peer, group);
}
return F2FErrOK;
}
/**
* This checks the Serial stream for characters, and assembles them into a buffer.
* When the terminator character (default '\n') is seen, it starts parsing the
* buffer for a prefix command, and calls handlers setup by addCommand() member
*/
void SerialCommand::update(char inChar) {
#ifdef SERIALCOMMAND_DEBUG
Serial.print(inChar); // Echo back to serial stream
#endif
if (inChar == '\r' || inChar == '\n') { // Check for the terminator (default '\r') meaning end of command
#ifdef SERIALCOMMAND_DEBUG
Serial.print("Received: ");
Serial.println(buffer);
#endif
parseBuffer();
clearBuffer();
} else if (isprint(inChar)) { // Only printable characters into the buffer
if (bufPos < SERIALCOMMAND_BUFFER) {
buffer[bufPos++] = inChar; // Put character into buffer
buffer[bufPos] = '\0'; // Null terminate
} else {
#ifdef SERIALCOMMAND_DEBUG
Serial.println(
"Line buffer is full - increase SERIALCOMMAND_BUFFER");
#endif
}
} else if (inChar == '\b' && bufPos > 0) {
// backspace support
bufPos--;
buffer[bufPos] = '\0';
}
}
void MClient::rxHandler(){
/* accodo quanto ricevuto al buffer */
m_buffer += m_sock->readAll();
/* per trace */
qDebug() << QString("MClient::rxHandler -> ") + m_buffer;
/* se il buffer contiene la keyword */
if(m_buffer.contains(_MTEMP_BOARD_OK)){
/* processo il comando */
parseBuffer();
/* se invece contiene la stringa che indica il fallimento dell'operazione */
}else if(m_buffer.contains(_MTEMP_BOARD_FAIL)){
/* pulisco il buffer */
m_buffer.clear();
/* emetto il segnale */
emit boardFailure();
/* se invece contiene la stringa di errore del login */
}else if(m_buffer.contains(_MTEMP_BOARD_ERROR)){
/* pulisco il buffer */
m_buffer.clear();
/* emetto il segnale */
emit boardError();
}
}
//protected Slots:
void sJarvisNode::data_rx(QByteArray data)
{
m_rxBuffer.append(data);
m_commLog.append(data);
parseBuffer(m_rxBuffer);
emit rawInput(data);
}
void* ReadBuffer(Client* client){
if (client->outputnetworkBuf->recived > 0){
//************************************
//CRITICAL
pthread_mutex_lock(&client->networkBufLock);
client->parsingoutput = true;
pthread_mutex_unlock(&client->networkBufLock);
//CRITICAL
//************************************
//if(printbufferbool)
uint32_t delta = parseBuffer(client,client->outputnetworkBuf->recived);
memmove(client->outputnetworkBuf->networkBuf, client->outputnetworkBuf->networkBuf+ delta, client->outputnetworkBuf->recived- delta);
client->outputnetworkBuf->recived -= delta;
//************************************
//CRITICAL
pthread_mutex_lock(&client->networkBufLock);
client->parsingoutput = false;
pthread_mutex_unlock(&client->networkBufLock);
//CRITICAL
//************************************
}
}
/// Receive and parse messages
///
/// This will attempt to receive JSON-encoded messages from the associated
/// socket. Note that this method doesn't call the message handlers
/// immediately. Instead they are enqueued for deferred dispatching via
/// `dispatch`.
///
/// The order the messages are received is the same order they'll be
/// dispatched.
bool process() {
// TODO: Propagation of errors
auto free_buffer = m_buffer.capacity() - m_buffer.size();
if (free_buffer == 0) {
// What do?
fmt::print("No free buffer.\n");
}
int len = strlen(&m_buffer[0]);
m_buffer.resize(free_buffer);
ssize_t data_or_error = recv(m_socket, &m_buffer[len], free_buffer, 0);
len = strlen(&m_buffer[0]);
if (data_or_error == 0) {
m_buffer.resize(0);
return false;
} else if (data_or_error == -1) {
// Error, need to check errno, may be EAGAIN/EWOULDBLOCK
if (errno != EAGAIN) {
fmt::print("(MessageProcessor) Error receiving: {}\n",
strerror(errno));
return false;
}
m_buffer.resize(len);
return true;
}
m_buffer.resize(len);
parseBuffer();
return true;
}
//--------------------------------------------------------------
// parse the file
void BVHFile::parseFile(
const char* fileName)
{
FILE* bvhFile = fopen(fileName, "rt");
if (bvhFile == NULL)
throw new mgException("cannot open %s", (const char*) fileName);
m_state = STATE_START;
m_linenum = 1;
char buffer[4096];
while (true)
{
size_t readLen = fread(buffer, 1, sizeof(buffer), bvhFile);
if (readLen == 0)
break;
parseBuffer(buffer, (int) readLen);
}
if (!m_token.isEmpty())
processToken(m_token);
fclose(bvhFile);
if (m_root != NULL)
mgDebug("read %d nodes", m_root->nodeCount());
mgDebug("frameCount: %d, frameTime: %g, totalChannels: %d", m_frameCount, m_frameTime, m_totalChannels);
if (m_state != STATE_DONE)
mgDebug("did not read all samples. count = %d, expected %d", m_count, m_totalChannels*m_frameCount);
}
void Tracker::onSocketEvent(TcpSocket *sock, SocketEvent evt) {
CHECK_RET(sock == m_socket.get());
if (evt == SOCK_CONNECTED) {
sendGetRequest();
} else if (evt == SOCK_READ) {
*m_socket >> m_inBuffer;
parseBuffer();
} else if (
void PosterPreview::slotProcessExited(KProcess *)
{
if(m_process->normalExit() && m_process->exitStatus() == 0)
parseBuffer();
else
m_rows = m_cols = 0;
m_dirty = false;
update();
}
//------------------------------------------------------------------------------
///
/// opens the config.cfg file
///
/// @param config_file Getting the whole file in one string with whitespaces
///
/// @return ERROR_OUT_OF_MEMORY means that there is no RAM available anymore.
//
int openConfig(char config_file[], float *nr_gravity, unsigned int *res_width,
unsigned int *res_height, unsigned int *nr_pps, float *wind_angle,
float *wind_force)
{
FILE *file_descriptor;
char *buffer = NULL;
int file_size = 0;
file_descriptor = fopen(config_file, "r");
if(file_descriptor == NULL)
{
printf(ERROR_MSG_NO_CONFIG);
return SUCCESS;
}
else
{
//getting file size
fseek(file_descriptor, 0, SEEK_END);
file_size = ftell(file_descriptor);
rewind(file_descriptor);
//allocate memory for the whole file:
buffer = (char*) malloc (sizeof(char)*file_size);
if(buffer == NULL)
{
free(buffer);
return ERROR_OUT_OF_MEMORY;
}
while(!feof(file_descriptor))
{
fread(buffer, 1, file_size, file_descriptor);
if(ferror(file_descriptor))
{
fclose(file_descriptor);
free(buffer);
return ERROR_ARGUMENTS;
}
}
buffer = (char *)realloc(buffer, file_size + 2);
if (buffer == NULL)
{
return ERROR_OUT_OF_MEMORY;
}
buffer[file_size] = '\n';
buffer[file_size + 1] = '\0';
parseBuffer(buffer, nr_gravity, res_width, res_height, nr_pps, wind_angle,
wind_force);
}
free(buffer);
fclose(file_descriptor);
return SUCCESS;
}
void ofxFlashConnection::onReadable(const AutoPtr<ReadableNotification>& pNotif) {
int n = socket.receiveBytes(raw, BUFFER_SIZE);
if(n > 0) {
for(int i = 0; i < n; ++i) {
buffer.push_back(raw[i]);
}
parseBuffer();
}
else {
delete this;
}
}
void ONBinaryProtocol::Attach(QIODevice *device)
{
if (_device != nullptr) {
throw Exception("Already attached");
}
if (device == nullptr) {
throw Exception("Can't attach to a nullptr");
}
_device = device;
connect(_device, SIGNAL(readyRead()), this, SLOT(dataAvailable()));
_buffer.append(_device->readAll());
parseBuffer();
}
/** Fill send buffer with data for all group members */
static F2FError groupSend( F2FGroup *group,
F2FMessageType type,
const char * message, F2FSize len)
{
/* Check if send-buffers are empty */
if(! f2fTestSentBufferEmpty())
return F2FErrBufferFull;
/* fill the lists of peers to send data to */
/* go through all peers in this group and add these to the
* respective sendlists */
// obsolet - already asked sendBuffer.peercount = 0;
// obsolet - already asked sendIMBuffer.peercount = 0;
// Get a receive buffer for the adapter
F2FAdapterReceiveMessage * freebuffer = f2fAdapterReceiveBufferReserve();
if( freebuffer == NULL )
return F2FErrBufferFull;
freebuffer->peercount = 0;
int peerindex;
for( peerindex = 0; peerindex < group->listSize; peerindex ++)
{
F2FPeer *peer = group->sortedPeerList[peerindex].peer;
if( peer != myself && peer->status == F2FPeerActive ) // Make sure, peer is active and not myself
{
if ( peer->activeprovider == F2FProviderIM )
freebuffer->localPeerIDlist[freebuffer->peercount++] =
peer->localPeerId;
else
sendBuffer.peerList[sendBuffer.peercount++] = peer;
}
}
char buffer[F2FMaxMessageSize];
F2FError error = prepareBufferWithData( buffer, group, NULL, type, message, len );
if (error != F2FErrOK ) return error;
/* and the IM endbuffer */
if( freebuffer->peercount > 0 )
{
error = encodeIMMessage( sendBuffer.buffer, sendBuffer.size, freebuffer );
if (error != F2FErrOK ) return error;
freebuffer->buffertype = F2FAdapterReceiveMessageTypeIMForward;
}
else f2fAdapterReceiveBufferRelease( freebuffer ); /* Was not necessary */
error = f2fSend(myself, NULL, group); /* send it out */
if (error != F2FErrOK ) return error;
// Send the message to myself
return parseBuffer(myself,myself,group,type,message,len);
}
/**
* Lit dans le fichier fifo qui permet de faire la communication avec le serveur
* le message de l'action à faire (de la forme "DO 0021CBE5 1")
* et envoie la trame correspondante à la station
* pipe - le fichier fifo
* sock - la socket pour l'envoie des messages
*/
void pipeReceiveSocketSend (FILE* pipe, SOCKET sock) {
char* bufferPipe = malloc(BUFFER_RECEIVE_SIZE);
char* bufferFrame = malloc(BUFFER_RECEIVE_SIZE);
idValue envoi;
while ( ! feof(pipe) && !ferror(pipe) && fgets(bufferPipe, BUFFER_RECEIVE_SIZE,pipe)!=NULL ) {
envoi = parseBuffer(bufferPipe);
convertToFrame(envoi,bufferFrame);
if(send(sock, bufferFrame, strlen(bufferFrame), 0) < 0)
{
printf("Erreur d'envoi de la trame d'actionneur");
}
}
free(bufferPipe);
free(bufferFrame);
return;
}
//-----------------------------------------------------------------------------
// Receive a packet from the IO board. Returns TRUE in case of success and
// FALSE in case of any error.
//-----------------------------------------------------------------------------
bool receivePacket()
{
uint8_t receiveBuffer[100];
int response = read(serial_fd, receiveBuffer, 100);
if (response == -1)
{
printf("Couldn't read from IO\n");
return 0;
}
else if (response == 0)
{
printf("No response from IO\n");
return 0;
}
else
{
/*
printf("Read %d bytes\n", response);
for (int i = 0; i < response; i++)
{
printf("0x%02x ", receiveBuffer[i]);
receiveBuffer[i] = 0;
}
printf("\n");
*/
if (parseBuffer(receiveBuffer, response))
{
struct OPLC_input *dataPointer;
dataPointer = &input_data;
pthread_mutex_lock(&ioLock);
memcpy(dataPointer, receiveBuffer, sizeof(struct OPLC_input));
pthread_mutex_unlock(&ioLock);
return 1;
}
}
return 0;
}
void* thread_Recive(){
connection.messagebuffer = (char*)malloc(2048);
memset(connection.messagebuffer,0,2048);
printf ("recv test....\n");
int recsize;
uint32_t time= getTime();
int recived = 0;
uint32_t delta = 0;
while(connection.connected){
recsize = recv(connection.SocketFD, connection.messagebuffer + recived, 1024,0);
if (recsize < 0)
fprintf(stderr, "error\n");
recived +=recsize;
if (recived >= 1){
delta = 1;
while (delta){
pthread_mutex_lock(&lockInput);
delta = parseBuffer(connection.messagebuffer,recived);
pthread_mutex_unlock(&lockInput);
memmove(connection.messagebuffer, connection.messagebuffer+ delta, 2048 - delta);
recived -= delta;
}
}
usleep(40);
}
shutdown(connection.SocketFD, SHUT_RDWR);
close(connection.SocketFD);
pthread_exit(0);
}
bool CDataSocket::read(std::vector<CData> &dataBuffer)
{
if (!m_socket)
{
CLog::instance()->log(CLog::msgFlagNetwork, CLog::msgLvlError, _("Attempt to read from not initialized data socket.\n"));
return false;
}
if( !SDLNet_SocketReady(m_socket) )
return true;
m_lastPong = time(NULL);
if( m_wrtPtr >= m_buf + sizeof(m_buf) )
{
CLog::instance()->log(CLog::msgFlagNetwork,CLog::msgLvlInfo,_("(%s:%d) : Data socket input buffer overflow.\n"), m_remoteHost.c_str(), m_remotePort);
return false;
}
// res - number of received bytes
int res = SDLNet_TCP_Recv(m_socket, m_wrtPtr, (int) (m_buf + sizeof(m_buf) - m_wrtPtr) );
if( res <= 0 )
{
m_sockErrorOccured = true;
CLog::instance()->log(CLog::msgFlagNetwork,CLog::msgLvlInfo,_("(%s:%d) : Error occured while reading from data soket.\n"), m_remoteHost.c_str(), m_remotePort);
return false;
}
else
m_wrtPtr += res; // Move write pointer to the end of data
byte *newBufStartPtr = parseBuffer(m_buf, m_wrtPtr, dataBuffer);
if( newBufStartPtr != m_buf )
{
if( m_wrtPtr > newBufStartPtr )
memcpy(m_buf, newBufStartPtr, m_wrtPtr - newBufStartPtr);
m_wrtPtr -= (newBufStartPtr - m_buf);
};
return true;
};
void LogLoader::run()
{
QFile logfile(m_filename);
logfile.open(QIODevice::ReadOnly);
//QByteArray arr = logfile.readAll();
//logfile.close();
int curr = 0;
//bool escape = false;
bool inpacket = false;
QByteArray currPacket;
//while (!escape || curr >= arr.size())
QByteArray retval;
while (!logfile.atEnd())
{
//logfile.read(&retval,1);
emit logProgress(logfile.pos(),logfile.size());
retval = logfile.read(1);
if (retval[0] == (char)0xAA)
{
if (inpacket)
{
//in the middle of a packet
currPacket.clear();
}
currPacket.append(retval[0]);
//Start byte
//qDebug() << "Start byte";
inpacket = true;
}
else if (retval[0] == (char)0xCC)
{
//currPacket
currPacket.append(retval[0]);
QString output;
for (int i=0;i<currPacket.size();i++)
{
int num = (unsigned char)currPacket[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
qDebug() << "Full packet:";
//qDebug() << output;
parseBuffer(currPacket);
currPacket.clear();
//qDebug() << "loop";
msleep(25);
}
else if (inpacket)
{
if (retval[0] == (char)0xBB)
{
//Need to escape the next byte
retval = logfile.read(1);
if (retval[0] == (char)0x55)
{
currPacket.append((char)0xAA);
}
else if (retval[0] == (char)0x44)
{
currPacket.append((char)0xBB);
}
else if (retval[0] == (char)0x33)
{
currPacket.append((char)0xCC);
}
}
else
{
currPacket.append(retval[0]);
}
}
curr++;
}
emit endOfLog();
logfile.close();
return;
/*for (int i=0;i<arr.size();i++)
{
//i++;
//qDebug() << QString::number(arr[i],16);
curr = i;
curr+=3;
curr += 1;
i += curr-1;
//i++;
}*/
}
// Apply the SPRMs for the Paragraph Properties, as documented in the Word file specifications.
void WrdPapParser::applySprm (
WrdParagraphProperties& io_obj,
const WrdProperty::SPRM& in_sprm,
const ChBYTE* in_pData,
ChUINT2& out_unDataBytesUsed,
ChSINT4& out_lHugePapxFc)
{
out_lHugePapxFc = -1;
if (in_sprm.m_sprmType != WrdProperty::PAP)
{
ChTHROW(WRD_WRONG_SPRM_TYPE);
}
if (in_pData == NULL)
{
ChTHROW(CS_INVALID_ARGUMENT);
}
ChBYTE bTemp = '\0';
ChUINT2 unTemp1 = 0;
ChUINT2 unTemp2 = 0;
ChSINT2 nTemp = 0;
ChSINT4 lTemp = 0;
// Determine the amount of data bytes that will be used, based on the SPRM
out_unDataBytesUsed = WrdProperty::getSizeOfSPRMParameter(in_sprm);
switch (in_sprm.m_unOpCode)
{
case 0x4600:
io_obj.setParagraphStyleIndex (CsLeReadUnsignedShort(in_pData));
break;
case 0xC601:
if (in_sprm.m_bVariableSize > 6)
{
// First two bytes are always set to 0
in_pData += 2 * sizeof(ChBYTE);
// Index of first style in range to which permutation stored in rgistd applies
unTemp1 = CsLeReadUnsignedShort(in_pData);
// Index of last style in range to which permutation stored in rgistd applies
unTemp2 = CsLeReadUnsignedShort(in_pData);
if ((io_obj.getParagraphStyleIndex() > unTemp1) && (io_obj.getParagraphStyleIndex() <= unTemp2))
{
// Grab the style ID from rgistd[], using the index of current styleID - Index of first style
in_pData += sizeof(ChUINT2) * (io_obj.getParagraphStyleIndex() - unTemp1);
io_obj.setParagraphStyleIndex(CsLeReadUnsignedShort(in_pData));
}
}
break;
case 0x2602:
// Applied to pieces in piece table that contain paragrphs with style codes >= 1 and <= 9. These style codes
// identify heading levels in a Word outline structure. Causes a set of paragraphs to be changed to a different
// heading level.
// Only applies in grpprls linked to a piece table.
if ((io_obj.getParagraphStyleIndex() >= 1) && (io_obj.getParagraphStyleIndex() <= 9))
{
bTemp = in_pData[0];
nTemp = 0;
if (bTemp & 80)
{
// Documentation says to sign extend to a word (which will make it negative)
nTemp = bTemp | 0xFF00;
}
else
{
nTemp = bTemp;
}
io_obj.setParagraphStyleIndex(io_obj.getParagraphStyleIndex() + nTemp);
io_obj.setOutlineLevel(io_obj.getOutlineLevel() + nTemp);
}
break;
case 0x2403:
io_obj.setJustificationV8 ((JustificationEnums)in_pData[0]);
break;
case 0x2461:
io_obj.setJustification((JustificationEnums)in_pData[0]);
break;
case 0x2404:
io_obj.setSideBySide ((in_pData[0] == 1) ? 1 : 0);
break;
case 0x2405:
io_obj.setKeepOnOnePage ((in_pData[0] == 1) ? 1 : 0);
break;
case 0x2406:
io_obj.setKeepFollowingParagraph ((in_pData[0] >= 1) ? 1 : 0);
break;
case 0x2407:
io_obj.setPageBreakBefore ((in_pData[0] == 1) ? 1 : 0);
break;
case 0x2408:
io_obj.setBorderLineStyle ((BorderLineStyleEnums)in_pData[0]);
break;
case 0x2409:
io_obj.setBorderCode ((BorderCodeEnums)in_pData[0]);
break;
case 0x260A:
io_obj.setListLevel (in_pData[0]);
break;
case 0x460B:
io_obj.setIndexToList (CsLeReadUnsignedShort(in_pData));
break;
case 0x240C:
io_obj.setNoLineNumbering ((in_pData[0] == 1) ? 1 : 0);
break;
case 0xC60D:
applyTabSPRM(io_obj, in_sprm, in_pData, out_unDataBytesUsed);
break;
case 0x840E:
io_obj.setRightIndent (CsLeReadShort(in_pData));
break;
case 0x845D:
io_obj.setTrailingIndent (CsLeReadShort(in_pData));
break;
case 0x840F:
io_obj.setLeftIndent(CsLeReadShort(in_pData));
break;
case 0x845E:
io_obj.setLeadingIndent (CsLeReadShort(in_pData));
break;
case 0x8411:
io_obj.setFirstLineLeftIndent(CsLeReadShort(in_pData));
break;
case 0x8460:
io_obj.setFirstLineLeadingIndent (CsLeReadShort(in_pData));
break;
case 0x4456:
io_obj.setLeftIndentCharacterUnits (CsLeReadShort(in_pData));
break;
case 0x4457:
io_obj.setFirstLineIndentCharacterUnits (CsLeReadShort(in_pData));
break;
case 0x4455:
io_obj.setRightIndentCharacterUnits (CsLeReadShort(in_pData));
break;
case 0x4610:
{
// According to the documentation, the sprm is a nesting sprm, which
// indicates that the indentation (in the parameter) is to be added to
// the existing indentation. If it is negative, clamp it to 0.
// Note, if it is 0, then it actually has no impact on the left indent,
// therefore do nothing.
nTemp = CsLeReadShort(in_pData);
if (nTemp != 0)
{
io_obj.setLeftIndent (nTemp + io_obj.getLeftIndent());
if (io_obj.getLeftIndent() < 0)
{
io_obj.setLeftIndent(0);
}
}
}
break;
case 0x465F:
// According to the documentation, the sprm is a nesting sprm, which
// indicates that the indentation (in the parameter) is to be added to
// the existing indentation. If it is negative, clamp it to 0.
io_obj.setLeadingIndent (CsLeReadShort(in_pData) + io_obj.getLeadingIndent());
if (io_obj.getLeadingIndent() < 0)
{
io_obj.setLeadingIndent (0);
}
break;
case 0x6412:
parseBuffer(io_obj.getLineSpaceReference(), in_pData);
break;
case 0xA413:
io_obj.setSpaceBefore (CsLeReadUnsignedShort(in_pData));
break;
case 0xA414:
io_obj.setSpaceAfter (CsLeReadUnsignedShort(in_pData));
break;
case 0x245C:
io_obj.setVerticalSpacingAfterAutomatic ((in_pData[0] == 1) ? 1 : 0);
break;
case 0x245B:
io_obj.setVerticalSpacingBeforeAutomatic ((in_pData[0] == 1) ? 1 : 0);
break;
case 0x4459:
io_obj.setLineSpaceAfter(CsLeReadShort(in_pData));
break;
case 0x4458:
io_obj.setLineSpaceBefore (CsLeReadShort(in_pData));
break;
case 0xC615:
// The parameter size needs to be overridden here, because it is possible that the size is greater than 255,
// which is the maximum value that can be in in_sprm.m_bVariableSize.
applyTabSPRM(io_obj, in_sprm, in_pData, out_unDataBytesUsed);
break;
case 0x2416:
io_obj.setParagraphInTable ((in_pData[0] == 1) ? 1 : 0);
break;
case 0x2417:
io_obj.setTableTrailerParagraph ((in_pData[0] == 1) ? 1 : 0);
break;
case 0x8418:
io_obj.setHorizontalPlacement (CsLeReadShort(in_pData));
break;
case 0x8419:
io_obj.setVerticalPlacement (CsLeReadShort(in_pData));
break;
case 0x841A:
io_obj.setWidth (CsLeReadUnsignedShort(in_pData));
break;
case 0x261B:
bTemp = in_pData[0];
if ((bTemp & 0x30) != 0x30)
{
io_obj.setVerticalPosition((VerticalPositionEnums)((bTemp & 0x30) >> 4));
}
if ((bTemp & 0xC0) != 0xC0)
{
io_obj.setHorizontalPosition((HorizontalPositionEnums)((bTemp & 0xC0) >> 6));
}
void SerialThread::run()
{
if (openPort(m_portName,m_baud))
{
qDebug() << "Error opening com port";
return;
}
//m_logFile = new QFile(m_logFileName);
//m_logFile->open(QIODevice::ReadWrite | QIODevice::Truncate);
unsigned char buffer[1024];
int readlen = 0;
QByteArray qbuffer;
bool inpacket= false;
bool inescape=true;
while (true)
{
#ifdef Q_OS_WIN32
if (!ReadFile(m_portHandle,(LPVOID)buffer,1024,(LPDWORD)&readlen,NULL))
{
//Serial error here
qDebug() << "Serial Read error";
}
#else
readlen = read(m_portHandle,buffer,1024);
#endif //Q_OS_WIN32
if (readlen == 0)
{
msleep(10);
}
else if (readlen == -1)
{
qDebug() << "Serial Read error";
}
for (int i=0;i<readlen;i++)
{
if (buffer[i] == 0xAA)
{
if (inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
inescape = false;
qbuffer.clear();
}
qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
inpacket = true;
}
else if (buffer[i] == 0xCC && inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
inpacket = false;
qbuffer.append(buffer[i]);
//m_logFile->write(qbuffer);
//m_logFile->flush();
emit parseBuffer(qbuffer);
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (inpacket && !inescape)
{
if (buffer[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
inescape = true;
}
else
{
qbuffer.append(buffer[i]);
}
}
else if (inpacket && inescape)
{
if (buffer[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if (buffer[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if (buffer[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
inescape = false;
}
}
}
}
}
void WrdPicParser::applySprm (
WrdPictureProperties& io_obj,
const WrdProperty::SPRM& in_sprm,
const ChBYTE* in_pData,
ChUINT2& out_unDataBytesUsed)
{
ChASSERT( in_pData != NULL && in_sprm.m_sprmType == WrdProperty::PIC);
// Determine the amount of data bytes that will be used, based on the SPRM
out_unDataBytesUsed = WrdProperty::getSizeOfSPRMParameter(in_sprm);
switch (in_sprm.m_unOpCode)
{
case 0x2E00: // sprmPicBrcl
// Obsolte as it applies to the brcl attribute.
break;
case 0xCE01: // sprmPicScale
ChASSERT(in_sprm.m_bVariableSize == 12);
io_obj.setScaleFactor(coordX, CsLeReadShort(in_pData));
in_pData += sizeof(ChSINT2);
io_obj.setScaleFactor(coordY, CsLeReadShort(in_pData));
in_pData += sizeof(ChSINT2);
io_obj.setCrop(sideLeft, CsLeReadShort(in_pData));
in_pData += sizeof(ChSINT2);
io_obj.setCrop(sideTop, CsLeReadShort(in_pData));
in_pData += sizeof(ChSINT2);
io_obj.setCrop(sideRight, CsLeReadShort(in_pData));
in_pData += sizeof(ChSINT2);
io_obj.setCrop(sideBottom, CsLeReadShort(in_pData));
in_pData += sizeof(ChSINT2);
break;
case 0x6C02: // sprmPicBrcTop80
parseBRC80(io_obj.getBorderReference(sideTop), in_pData);
break;
case 0xCE0A: // sprmPicBrcBottom
parseBuffer(io_obj.getBorderReference(sideBottom), in_pData);
break;
case 0x4C04: // sprmPicBrcBottom70
parseBRC70(io_obj.getBorderReference(sideBottom), in_pData);
break;
case 0x6C03: // sprmPicBrcLeft80
parseBRC80(io_obj.getBorderReference(sideLeft), in_pData);
break;
case 0xCE09: // sprmPicBrcLeft
parseBuffer(io_obj.getBorderReference(sideLeft), in_pData);
break;
case 0x4C03: // sprmPicBrcLeft70
parseBRC70(io_obj.getBorderReference(sideLeft), in_pData);
break;
case 0x6C04: // sprmPicBrcBottom80
parseBRC80(io_obj.getBorderReference(sideBottom), in_pData);
break;
case 0xCE0B: // sprmPicBrcRight
parseBuffer(io_obj.getBorderReference(sideRight), in_pData);
break;
case 0x4C05: // sprmPicBrcRight70
parseBRC70(io_obj.getBorderReference(sideRight), in_pData);
break;
case 0x6C05: // sprmPicBrcRight80
parseBRC80(io_obj.getBorderReference(sideRight), in_pData);
break;
case 0xCE08: // sprmPicBrcTop
parseBuffer(io_obj.getBorderReference(sideTop), in_pData);
break;
case 0x4C02: // sprmPicBrcTop70
parseBRC70(io_obj.getBorderReference(sideTop), in_pData);
break;
default:
{
#ifdef _DEBUG
ChCHAR1 strmsg[64];
ChSprintf(strmsg, "Unknown sprm code 0x%X\n", in_sprm.m_unOpCode);
ChLOG_WARNING( strmsg, LOG_WRD );
#endif
}
break;
}
}
/** Forward messages from the IM program to the core.
* To avoid letting the receivebuffer get too full
* f2fReceive should be called to be able to clear
* the receive buffers.
* The messages have to start with the right header and must be base64 encoded to be detectable.
* If any other message is passed here, the function will return F2FErrNotF2FMessage.
* We send here the local peerid and the local identifier as this peer might not
* be in our peer list.
*
* @param F2FString identifier - display name of contact in list
* @param F2FWord32 localPeerId - local reference for Peer
*
* */
F2FError f2fForward( const F2FWord32 localPeerId,
const F2FString identifier, const F2FString message,
const F2FSize size )
{
F2FError error;
char tmpbuffer[F2FMaxMessageSize];
/* TODO: check reasonable? if( size < (sizeof( F2FMessageHeader )+2)/3 * 4)
return F2FErrMessageTooShort; */
error = f2fIMDecode(message, size, tmpbuffer, F2FMaxMessageSize);
if( error != F2FErrOK ) return error;
F2FMessageHeader *hdr = (F2FMessageHeader *) tmpbuffer;
F2FGroup * group = f2fGroupListFindGroup(
ntohl(hdr->groupID.hi), ntohl(hdr->groupID.lo) );
F2FPeer * srcPeer = f2fPeerListFindPeer(
ntohl(hdr->sourcePeerID.hi), ntohl(hdr->sourcePeerID.lo) );
F2FPeer * dstPeer;
if(ntohl(hdr->destPeerID.hi) == 0 && ntohl(hdr->destPeerID.lo) == 0)
dstPeer = myself;
else dstPeer = f2fPeerListFindPeer(
ntohl(hdr->destPeerID.hi), ntohl(hdr->destPeerID.lo) );
// Special treatment for invitemessages
if( hdr->messagetype == F2FMessageTypeInvite )
{
if( group == NULL ) /* the group does not exist, normal, when I am invited */
{
/* TODO: process the actual invite string and ask myself, if I want to be part
* of this computation group */
F2FMessageInvite * msgiv =
(F2FMessageInvite *) (tmpbuffer + sizeof (F2FMessageHeader));
F2FSize namelen = ntohl(msgiv->groupNameLength);
if (namelen>F2FMaxNameLength) namelen=F2FMaxNameLength;
char groupname[namelen+1];
groupname[namelen] = 0;
memcpy(groupname, msgiv->nameAndInvite, namelen);
group = f2fGroupListAdd( groupname,
ntohl(hdr->groupID.hi), ntohl(hdr->groupID.lo) );
if( group == NULL ) return F2FErrListFull;
}
/* TODO: is the else branch here important for security? */
if( srcPeer == NULL ) /* not in the local peer list, normal, when I am invited */
{
/* TODO: verify that this is really my friend contacting me */
/* add to my peer list */
srcPeer = f2fPeerListNew( ntohl(hdr->sourcePeerID.hi), ntohl(hdr->sourcePeerID.lo) );
if( srcPeer == NULL ) return F2FErrListFull;
srcPeer->localPeerId = localPeerId;
srcPeer->activeprovider = F2FProviderIM; // Only IM at this time
srcPeer->identifier[F2FMaxNameLength] = 0;
strncpy( srcPeer->identifier, identifier, F2FMaxNameLength );
}
if( dstPeer == NULL ) /* also very likely as this was a fake temporary number, which will be used
* as a challenge */
{
dstPeer = myself; /* Assume invitation was for me */
}
/* TODO: is the else branch here important for security? */
}
if( group == NULL ) return F2FErrNotFound;
/*if( srcPeer == NULL ) return F2FErrNotFound; can be NULL if InviteMessageAnswer */
if( dstPeer == NULL ) return F2FErrNotFound;
/* maybe check the src for Invitemessageanswers here */
if( dstPeer == myself )
return parseBuffer( srcPeer, dstPeer, group,
hdr->messagetype, tmpbuffer + sizeof(F2FMessageHeader), ntohl(hdr->len) );
// Message is not for myself, so send it either internally or give it back to send via IM
if( dstPeer->activeprovider == F2FProviderIM )
return fillAdapterReceiveBuffer( F2FAdapterReceiveMessageTypeIMForward,
group->id.hi, group->id.lo,
ntohl(hdr->sourcePeerID.hi), ntohl(hdr->sourcePeerID.lo),
dstPeer->id.hi, dstPeer->id.lo,
hdr->messagetype,
message, size ); // This implements a simple routing
printf("f2fForward: Provider not supported.\n"); // TODO: other providers
return F2FErrOK;
}
void ServiceClient::readFromSocket()
{
try
{
//qDebug() << "Recieved! new data! size=" << m_qTcpSocket->bytesAvailable();
if (m_flagForWaitBytes == WAIT_ID)
{
//ждем ID-сообщения
if (m_qTcpSocket->bytesAvailable() < (qint64)sizeof(quint16))
return;
quint16 _id;
memcpy(&_id, m_qTcpSocket->read(sizeof(quint16)).data(), sizeof(quint16));
_id = m_bIsLittleEndian ? qFromBigEndian(_id) : _id;
if(_id != ID_Message)
return;
m_flagForWaitBytes = WAIT_SIZE;
}
if (m_flagForWaitBytes == WAIT_SIZE)
{
//ждем размер сообщения
if (m_qTcpSocket->bytesAvailable() < (qint64)sizeof(quint32))
return;
memcpy(&m_uiBlockSize, m_qTcpSocket->read(sizeof(quint32)).data(), sizeof(quint32));
m_uiBlockSize = m_bIsLittleEndian ? qFromBigEndian(m_uiBlockSize) : m_uiBlockSize;
m_flagForWaitBytes = WAIT_MESSAGE;
}
//ждем пока блок прийдет полностью
//m_flagForWaitBytes = WAIT_MESSAGE
if (m_qTcpSocket->bytesAvailable() < m_uiBlockSize)
return;
else
{
//можно принимать новый блок
qint64 bytesAvailable = m_qTcpSocket->bytesAvailable();
qint64 bytesCurrent = 0;//количество прочитанный байт
//полная обработка сообщения
while(true)
{
if(m_flagForWaitBytes != WAIT_MESSAGE)
{
if(bytesAvailable - bytesCurrent < sizeof(quint16) + sizeof(quint32))
{
m_flagForWaitBytes = WAIT_ID;
m_uiBlockSize = 0;
return;
}
//id
quint16 _id;
memcpy(&_id, m_qTcpSocket->read(sizeof(quint16)).data(), sizeof(quint16));
_id = m_bIsLittleEndian ? qFromBigEndian(_id) : _id;
if(_id != ID_Message)
{
bytesCurrent += sizeof(quint16);
continue;
}
memcpy(&m_uiBlockSize, m_qTcpSocket->read(sizeof(quint32)).data(), sizeof(quint32));
m_uiBlockSize = m_bIsLittleEndian ? qFromBigEndian(m_uiBlockSize) : m_uiBlockSize;
bytesCurrent += sizeof(quint16) + sizeof(quint32);
m_flagForWaitBytes = WAIT_MESSAGE;
//если недостаточная длина буфера
if(bytesAvailable - bytesCurrent < m_uiBlockSize)
return;
else
continue;
}
if(m_flagForWaitBytes == WAIT_MESSAGE)
{
QByteArray buff = m_qTcpSocket->read(m_uiBlockSize);
bytesCurrent += m_uiBlockSize;
m_flagForWaitBytes = WAIT_ID;
parseBuffer(buff);
}
}//while
}
}
catch(...)
{
qDebug() << "Error: read socket! Function: readFromSocket()";
QMessageBox::information(0, "Error", "Error: read socket!");
}
}
/* This function does prescanning as well, to determine number of events,
* number of CPUs, max/min CPU frequency etc */
void TraceParser::threadParser()
{
unsigned int i = 0;
bool eof;
prepareParse();
while(true) {
eof = parseBuffer(i);
determineTraceType();
if (eof)
break;
eventsWatcher->sendNextIndex(events->size());
i++;
if (i == NR_TBUFFERS)
i = 0;
if (traceType == TRACE_TYPE_FTRACE)
goto ftrace;
if (traceType == TRACE_TYPE_PERF)
goto perf;
}
/* Must have been a short trace or a lot of unknown garbage in the
* trace if we end up here */
goto out;
/* The purpose of jumping to these loops is to be able to use the
* (hopefully faster) specialized parse functions */
ftrace:
while(true) {
if (parseFtraceBuffer(i))
break;
eventsWatcher->sendNextIndex(events->size());
i++;
if (i == NR_TBUFFERS)
i = 0;
}
goto out;
perf:
while(true) {
if (parsePerfBuffer(i))
break;
eventsWatcher->sendNextIndex(events->size());
i++;
if (i == NR_TBUFFERS)
i = 0;
}
out:
/* Make sure that the processing thread can continue even if no trace
* type was detected, otherwise it would wait forever in
* waitForTraceType() */
sendTraceType();
/* It's probable that at this point, one of the sendNextIndex() calls
* above has already been issued with and index value that corresponds
* to the last event but that is OK because the fixLastEvent() does
* not touch data that would be read by the code that waits for
* the sendNextIndex, i.e. the analyzer thread, so it's OK to call
* fixLastEvent while the analyzer thread is reading the last event. */
fixLastEvent();
eventsWatcher->sendNextIndex(events->size());
eventsWatcher->sendEOF();
}
am_status_t Properties::load(const std::string& fileName)
{
am_status_t status = AM_SUCCESS;
PRFileDesc *propFile;
PRErrorCode pr_errorCode;
propFile = PR_Open(fileName.c_str(), PR_RDONLY, 0);
if (NULL != propFile) {
PRFileInfo fileInfo;
if (PR_GetOpenFileInfo(propFile, &fileInfo) == PR_SUCCESS) {
try {
PRInt32 readCount;
char *buffer = new char[fileInfo.size + 1];
readCount = PR_Read(propFile, buffer, fileInfo.size);
if (readCount == fileInfo.size) {
buffer[readCount] = '\0';
status = parseBuffer(buffer);
} else {
Log::log(Log::ALL_MODULES,
Log::LOG_ERROR,
"am_properties_load(): "
"Could not load properties file %s: "
"Number of bytes read (%d) "
"did not match expected file size (%d).\n",
fileName.c_str(), readCount, fileInfo.size);
status = AM_NSPR_ERROR;
}
delete[] buffer;
} catch (const std::bad_alloc&) {
status = AM_NO_MEMORY;
}
} else {
pr_errorCode = PR_GetError();
Log::log(Log::ALL_MODULES, Log::LOG_ERROR,
"am_properties_load(): "
"Error getting info for properties file %s: %s\n",
fileName.c_str(),
PR_ErrorToString(pr_errorCode, PR_LANGUAGE_I_DEFAULT));
status = AM_NSPR_ERROR;
}
PR_Close(propFile);
} else {
pr_errorCode = PR_GetError();
if (PR_FILE_NOT_FOUND_ERROR == pr_errorCode) {
status = AM_NOT_FOUND;
} else if (PR_NO_ACCESS_RIGHTS_ERROR == pr_errorCode) {
status = AM_ACCESS_DENIED;
} else {
Log::log(Log::ALL_MODULES, Log::LOG_ERROR,
"am_properties_load(): "
"Error opening properties file '%s': %s\n",
fileName.c_str(),
PR_ErrorToString(pr_errorCode, PR_LANGUAGE_I_DEFAULT));
status = AM_NSPR_ERROR;
}
}
return status;
}
/****************************************************************************
Desc: Read the ini file and parse its contents
****************************************************************************/
RCODE FTKAPI F_IniFile::read(
const char * pszFileName)
{
RCODE rc = NE_FLM_OK;
FLMBOOL bMore = FALSE;
FLMBOOL bEOF = FALSE;
#define INITIAL_READ_BUF_SIZE 100
FLMUINT uiReadBufSize = 0;
FLMUINT uiBytesAvail = 0;
FLMUINT uiBytesInLine = 0;
char * pszReadBuf = NULL;
FLMUINT uiLineNum = 0;
IF_FileSystem * pFileSystem = f_getFileSysPtr();
f_assert( m_bReady);
f_assert( !m_pFileHdl);
// Open the file
if (RC_BAD( rc = f_alloc( f_strlen( pszFileName) + 1, &m_pszFileName)))
{
goto Exit;
}
f_strcpy( m_pszFileName, pszFileName);
// It's not an error if the file doesn't exist. If it does exist,
// we'll read in its data.
if( RC_BAD( pFileSystem->openFile( pszFileName,
FLM_IO_RDONLY, &m_pFileHdl)))
{
goto Exit;
}
m_uiFileOffset = 0;
// Read in and parse the file
uiReadBufSize = INITIAL_READ_BUF_SIZE;
if (RC_BAD( rc = f_alloc( uiReadBufSize, &pszReadBuf)))
{
goto Exit;
}
// Read in and parse each line in the file...
while (!bEOF)
{
uiLineNum++;
uiBytesAvail = uiReadBufSize;
if( RC_BAD( rc = readLine( pszReadBuf, &uiBytesAvail, &bMore)) &&
rc != NE_FLM_IO_END_OF_FILE)
{
goto Exit;
}
if (rc == NE_FLM_IO_END_OF_FILE)
{
bEOF = TRUE;
}
// While there are more bytes in the line, re-alloc the buffer, and do
// another read.
uiBytesInLine = uiBytesAvail;
while( bMore)
{
uiBytesAvail = uiReadBufSize;
uiReadBufSize *= 2;
if (RC_BAD( rc = f_realloc( uiReadBufSize, &pszReadBuf)))
{
goto Exit;
}
if (RC_BAD( rc = readLine( pszReadBuf+uiBytesAvail,
&uiBytesAvail, &bMore)) &&
(rc != NE_FLM_IO_END_OF_FILE) )
{
goto Exit;
}
if( rc == NE_FLM_IO_END_OF_FILE)
{
bEOF = TRUE;
}
uiBytesInLine += uiBytesAvail;
}
if ( (RC_OK( rc) || (rc == NE_FLM_IO_END_OF_FILE)) &&
(uiBytesInLine > 0) )
{
// NumBytes will be 0 if the line was blank. No need
// to call parseBuffer in this case
if (RC_BAD( rc = parseBuffer( pszReadBuf, uiBytesInLine)))
{
if (rc == NE_FLM_SYNTAX)
{
rc = NE_FLM_OK;
}
else
{
goto Exit;
}
}
}
}
Exit:
// Close the file
if( m_pFileHdl)
{
m_pFileHdl->closeFile();
m_pFileHdl->Release();
m_pFileHdl = NULL;
}
// Free the buffer
if (pszReadBuf)
{
f_free( &pszReadBuf);
}
if (rc == NE_FLM_IO_END_OF_FILE)
{
rc = NE_FLM_OK;
}
return rc;
}
int main(int argc, char **argv){ //we need argc and argv for the rosInit function
ros::init(argc, argv, "compass"); //inits the driver
ros::NodeHandle n; //this is what ROS uses to connect to a node
/*Advertises our various messages*/
ros::Publisher compassHeadingMsg = n.advertise<std_msgs::Float32>("compassHeading", 100);
ros::Publisher compassPitchMsg = n.advertise<std_msgs::Float32>("compassPitch", 100);
ros::Publisher compassRollMsg = n.advertise<std_msgs::Float32>("compassRoll", 100);
/*Sets up the message structures*/
std_msgs::Float32 compassHeading;
std_msgs::Float32 compassPitch;
std_msgs::Float32 compassRoll;
ros::Rate loop_rate(10); //how many times a second (i.e. Hz) the code should run
if(!open_port()){
return 0; //we failed to open the port so end
}
config_port();
ROS_INFO("Compass Driver Online");
while (ros::ok()){
if(write_port()){ //if we send correctly
if(read_port() != 0){ //if we read correctly
parseBuffer(); //parse the buffer
//printf("H: %f P: %f R: %f\n",heading,pitch,roll);
/* Below here sets up the messages ready for transmission*/
compassHeading.data = heading;
compassPitch.data = pitch;
compassRoll.data = roll;
ROS_DEBUG("H: %.3f P: %.3f R: %.3f",heading,pitch,roll);
}
else{
ROS_ERROR("Read no data");
}
}
else{
ROS_ERROR("Failed to write");
}
/*Below here we publish our readings*/
compassHeadingMsg.publish(compassHeading);
compassRollMsg.publish(compassRoll);
compassPitchMsg.publish(compassPitch);
/*Have a snooze*/
loop_rate.sleep();
}
ros::spin();
close(fd);
ROS_INFO("Shutting Down");
printf("Shutting Down\n");
return 0;
}
void SimThread::run()
{
init = true;
currentLocationPacket=0;
currentLocationInfoPacket=0;
QFile logfile(m_filename);
if (!logfile.open(QIODevice::ReadOnly))
{
qDebug() << "Unable to open file. Exiting sim thread";
return;
}
QByteArray logbytes = logfile.readAll();
nextPacketIsLocation = false;
QByteArray qbuffer;
QList<QByteArray> m_queuedMessages;
QString byteoutofpacket;
bool m_inpacket = false;
bool m_inescape = false;
for (int i=0;i<logbytes.size();i++)
{
if ((unsigned char)logbytes[i] == 0xAA)
{
if (m_inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
m_inescape = false;
qbuffer.clear();
}
//qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
m_inpacket = true;
}
else if ((unsigned char)logbytes[i] == 0xCC && m_inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
m_inpacket = false;
//qbuffer.append(buffer[i]);
//m_logFile->flush();
//emit parseBuffer(qbuffer);
//New Location of checksum
unsigned char sum = 0;
for (int i=0;i<qbuffer.size()-1;i++)
{
sum += qbuffer[i];
}
//qDebug() << "Payload sum:" << QString::number(sum);
//qDebug() << "Checksum sum:" << QString::number((unsigned char)currPacket[currPacket.length()-1]);
if (sum != (unsigned char)qbuffer[qbuffer.size()-1])
{
qDebug() << "BAD CHECKSUM!";
//return QPair<QByteArray,QByteArray>();
}
else
{
m_queuedMessages.append(qbuffer.mid(0,qbuffer.length()-1));
}
//return qbuffer;
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (m_inpacket && !m_inescape)
{
if ((unsigned char)logbytes[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
m_inescape = true;
}
else
{
qbuffer.append(logbytes[i]);
}
}
else if (m_inpacket && m_inescape)
{
if ((unsigned char)logbytes[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if ((unsigned char)logbytes[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if ((unsigned char)logbytes[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
else
{
qDebug() << "Error, escaped character is not valid!:" << QString::number(logbytes[i],16);
}
m_inescape = false;
}
else
{
//qDebug() << "Byte out of a packet:" << QString::number(buffer[i],16);
byteoutofpacket += QString::number(logbytes[i],16) + " ";
}
}
}
//qDebug() << "Bytes out of a packet:" << byteoutofpacket;
//Done.
qDebug() << m_queuedMessages.size() << "Messages read in";
for (int i=0;i<m_queuedMessages.size();i++)
{
Packet parsedPacket = parseBuffer(m_queuedMessages[i]);
packetList.append(parsedPacket);
//parsePacket(parsedPacket);
}
int fd=0;
#ifdef HAVE_POSIX_OPENPT
fd = posix_openpt(O_RDWR | O_NOCTTY);
#else
fd = open("/dev/ptmx",O_RDWR | O_NOCTTY);
#endif //HAVE_POSIX_OPENPT
if(-1 == fd) {
#ifdef HAVE_POSIX_OPENPT
perror("Error in posix_openpt");
#else
perror("Error opening /dev/ptmx");
#endif //HAVE_POSIX_OPENPT
return;
}
grantpt(fd);
unlockpt(fd);
struct termios oldtio;
if(0 != tcgetattr(fd,&oldtio)) {
perror("tcgetattr openpt warning");
}
//bzero(&newtio,sizeof(newtio));
oldtio.c_cflag = CS8 | CLOCAL | CREAD; // CBAUD
oldtio.c_iflag = IGNPAR | ICRNL;
oldtio.c_oflag = 0;
oldtio.c_lflag = ICANON & (~ECHO);
oldtio.c_cc[VEOL] = '\r';
// oldtio.c_cc[VEOL2] = 0; /* '\0' */
tcflush(fd,TCIFLUSH);
if(0 != tcsetattr(fd,TCSANOW,&oldtio)) {
perror("tcsetattr warning");
}
#ifdef HAVE_PTSNAME_R
if(0 != ptsname_r(fd, portname, sizeof(portname))) {
perror("Couldn't get pty slave name");
}
#else
char *ptsname_val = ptsname(fd);
if(NULL == ptsname_val) {
perror("Couldn't get pty slave name");
}
QString portname = QString(ptsname_val);
//strncpy(portname, ptsname_val, sizeof(portname));
#endif //HAVE_PTSNAME_R
fcntl(fd,F_SETFL,O_NONBLOCK); // O_NONBLOCK + fdopen/stdio == bad
qDebug() << "Port:" << portname << "opened for sim!";
unsigned char buffer[1024];
unsigned short nextlocationid;
bool nextislocation = false;
init = false;
while (true)
{
int readlen = read(fd,buffer,1024);
if (readlen < 0)
{
//Nothing on the port
usleep(10000);
}
if (readlen == 0)
{
usleep(10000);
}
for (int i=0;i<readlen;i++)
{
if (buffer[i] == 0xAA)
{
if (m_inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
m_inescape = false;
qbuffer.clear();
}
//qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
m_inpacket = true;
}
else if (buffer[i] == 0xCC && m_inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
m_inpacket = false;
//qbuffer.append(buffer[i]);
//m_logFile->flush();
//emit parseBuffer(qbuffer);
//New Location of checksum
unsigned char sum = 0;
for (int i=0;i<qbuffer.size()-1;i++)
{
sum += qbuffer[i];
}
//qDebug() << "Payload sum:" << QString::number(sum);
//qDebug() << "Checksum sum:" << QString::number((unsigned char)currPacket[currPacket.length()-1]);
if (sum != (unsigned char)qbuffer[qbuffer.size()-1])
{
qDebug() << "BAD CHECKSUM!";
//return QPair<QByteArray,QByteArray>();
}
else
{
QByteArray fullmsg = qbuffer.mid(0,qbuffer.length()-1);
Packet p = parseBuffer(fullmsg);
if (p.payloadid == 0x0104 || p.payloadid == 0x0106)
{
Packet p2 = locationidList[currentLocationPacket++];
QByteArray tosend = generatePacket(p2.header,p2.payload);
write(fd,tosend.data(),tosend.length());
}
else if (p.payloadid == 0xF8E0)
{
Packet p2 = locationidInfoList[currentLocationInfoPacket++];
QByteArray tosend = generatePacket(p2.header,p2.payload);
write(fd,tosend.data(),tosend.length());
}
/*if (p.payloadid == 0x0106 || p.payloadid == 0x0108)
{
//Request for ram.
if (nextislocation)
{
nextislocation = false;
unsigned short lookforpayload = p.payloadid+1;
for (int i=0;i<packetList.size();i++)
{
if (packetList[i].payloadid == lookforpayload)
{
unsigned short tmp = (((unsigned char)p.payload[0]) << 8) + (unsigned char)p.payload[1];
//if (tmp == nextlocationid)
//{
QByteArray tosend = generatePacket(packetList[i].header,packetList[i].payload);
write(fd,tosend.data(),tosend.length());
break;
//}
}
}
}
}*/
else
{
unsigned short lookforpayload = p.payloadid+1;
for (int i=0;i<packetList.size();i++)
{
if (packetList[i].payloadid == lookforpayload)
{
QByteArray tosend = generatePacket(packetList[i].header,packetList[i].payload);
write(fd,tosend.data(),tosend.length());
break;
}
}
}
}
//return qbuffer;
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (m_inpacket && !m_inescape)
{
if (buffer[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
m_inescape = true;
}
else
{
qbuffer.append(buffer[i]);
}
}
else if (m_inpacket && m_inescape)
{
if (buffer[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if (buffer[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if (buffer[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
else
{
qDebug() << "Error, escaped character is not valid!:" << QString::number(buffer[i],16);
}
m_inescape = false;
}
else
{
//qDebug() << "Byte out of a packet:" << QString::number(buffer[i],16);
byteoutofpacket += QString::number(buffer[i],16) + " ";
}
}
}
//qDebug() << "Bytes out of a packet:" << byteoutofpacket;
}
}
void pushData(const char *data) { parseBuffer(data); }
