// Entry Point
void *OCP_DataStreamInput_Thread::Entry()
{
wxString msg;
OVERLAPPED osReader = {0};
OVERLAPPED osWriter = {0};
m_launcher->SetSecThreadActive(); // I am alive
wxSleep(1); // allow Bluetooth SPP connections to re-cycle after the parent's test for existence.
// In the MS Bluetooth stack, there is apparently a minimum time required
// between CloseHandle() and CreateFile() on the same port.
// FS#1008
bool not_done;
HANDLE hSerialComm = (HANDLE)(-1);
// Request the com port from the comm manager
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) < 0)
{
wxString msg(_T("NMEA input device initial open failed: "));
msg.Append(m_PortName);
wxString msg_error;
msg_error.Printf(_T("...GetLastError(): %d"), GetLastError());
msg.Append(msg_error);
ThreadMessage(msg);
m_gps_fd = NULL;
// goto thread_exit;
}
hSerialComm = (HANDLE)m_gps_fd;
int n_reopen_wait = 2000;
COMMTIMEOUTS timeouts;
// Short read timeout for faster response
timeouts.ReadIntervalTimeout = 1;
timeouts.ReadTotalTimeoutMultiplier = 0;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.WriteTotalTimeoutMultiplier = 0;
timeouts.WriteTotalTimeoutConstant = 500;
if(m_gps_fd){
if (!SetCommTimeouts(hSerialComm, &timeouts)){ // Error setting time-outs.
CloseComPortPhysical(m_gps_fd);
m_gps_fd = NULL;
}
}
// Create the reader overlapped event.
osReader.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Create the writer overlapped event.
osWriter.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
not_done = true;
#define READ_BUF_SIZE 200
char szBuf[READ_BUF_SIZE];
#define READ_TIMEOUT 50 // milliseconds
DWORD dwRead;
DWORD dwWritten;
DWORD dwRes;
DWORD dwToWrite;
int n_timeout = 0;
int max_timeout = 100;
bool fWaitingOnRead = false;
bool fWaitingOnWrite = false;
// The main loop
while(not_done)
{
if(TestDestroy())
not_done = false; // smooth exit
// Was port closed due to error condition?
while(!m_gps_fd)
{
if((TestDestroy()) || (m_launcher->m_Thread_run_flag == 0))
goto thread_exit; // smooth exit
if(n_reopen_wait){
int nrwd10 = wxMax(1, n_reopen_wait /10);
while(n_reopen_wait > 0){
wxThread::Sleep(nrwd10); // stall for a bit
if((TestDestroy()) || (m_launcher->m_Thread_run_flag == 0))
goto thread_exit; // smooth exit
n_reopen_wait -= nrwd10;
}
n_reopen_wait = 0;
}
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) > 0)
{
hSerialComm = (HANDLE)m_gps_fd;
wxThread::Sleep(100); // stall for a bit
if (!SetCommTimeouts(hSerialComm, &timeouts)){ // Error setting time-outs.
int errt = GetLastError(); // so just retry
CloseComPortPhysical(m_gps_fd);
m_gps_fd = NULL;
}
fWaitingOnWrite = FALSE;
fWaitingOnRead = FALSE;
n_timeout = 0;
}
else
{
m_gps_fd = 0;
int nwait = 2000;
while(nwait > 0){
wxThread::Sleep(200); // stall for a bit
if((TestDestroy()) || (m_launcher->m_Thread_run_flag == 0))
goto thread_exit; // smooth exit
nwait -= 200;
}
}
}
if( (m_io_select == DS_TYPE_INPUT_OUTPUT) || (m_io_select == DS_TYPE_OUTPUT) ) {
m_outCritical.Enter();
bool b_qdata = !out_que.empty();
bool b_break = false;
while(!b_break && b_qdata){
char msg[MAX_OUT_QUEUE_MESSAGE_LENGTH];
// printf("wl %d\n", out_que.size());
{
if(fWaitingOnWrite){
// printf("wow\n");
dwRes = WaitForSingleObject(osWriter.hEvent, INFINITE);
switch(dwRes)
{
case WAIT_OBJECT_0:
if (!GetOverlappedResult(hSerialComm, &osWriter, &dwWritten, FALSE)) {
if (GetLastError() == ERROR_OPERATION_ABORTED){
// UpdateStatus("Write aborted\r\n");
}
else{
b_break = true;
}
}
if (dwWritten != dwToWrite) {
//ErrorReporter("Error writing data to port (overlapped)");
}
else {
// Delayed write completed
fWaitingOnWrite = false;
// printf("-wow\n");
}
break;
//
// wait timed out
//
case WAIT_TIMEOUT:
break;
case WAIT_FAILED:
default:
break;
}
}
if(!fWaitingOnWrite){ // not waiting on Write, OK to issue another
// Take a copy of message
char *qmsg = out_que.front();
strncpy( msg, qmsg, MAX_OUT_QUEUE_MESSAGE_LENGTH-1 );
out_que.pop();
free(qmsg);
dwToWrite = strlen(msg);
//
// issue write
//
n_timeout = 0;
// printf("w\n");
if (!WriteFile(hSerialComm, msg, dwToWrite, &dwWritten, &osWriter)) {
if (GetLastError() == ERROR_IO_PENDING) {
//
// write is delayed
//
fWaitingOnWrite = true;
// printf("+wow\n");
}
else{
b_break = true;
}
}
else {
//
// writefile returned immediately
//
}
b_qdata = !out_que.empty();
}
}
} //while b_qdata
m_outCritical.Leave();
}
//
// if no read is outstanding, then issue another one
//
// printf("r\n");
if (!fWaitingOnRead) {
if (!ReadFile(hSerialComm, szBuf, READ_BUF_SIZE, &dwRead, &osReader)) {
if (GetLastError() != ERROR_IO_PENDING) { // read not delayed?
CloseComPortPhysical(m_gps_fd);
m_gps_fd = NULL;
fWaitingOnRead = FALSE;
n_reopen_wait = 2000;
}
else
fWaitingOnRead = TRUE;
}
else { // read completed immediately
n_timeout = 0;
if (dwRead)
HandleASuccessfulRead(szBuf, dwRead);
}
}
//
// wait for pending operations to complete
//
if ( fWaitingOnRead ) {
dwRes = WaitForSingleObject(osReader.hEvent, READ_TIMEOUT);
switch(dwRes)
{
//
// read completed
//
case WAIT_OBJECT_0:
if (!GetOverlappedResult(hSerialComm, &osReader, &dwRead, FALSE)) {
int err = GetLastError();
if (GetLastError() == ERROR_OPERATION_ABORTED){
}
else{
// Some other error
n_reopen_wait = 2000;
CloseComPortPhysical(m_gps_fd);
m_gps_fd = 0;
}
}
else { // read completed successfully
if (dwRead)
HandleASuccessfulRead(szBuf, dwRead);
}
fWaitingOnRead = FALSE;
n_timeout = 0;
break;
case WAIT_TIMEOUT:
n_timeout++;
break;
default: // error of some kind with handles
fWaitingOnRead = FALSE;
break;
}
}
if(m_launcher->m_Thread_run_flag <= 0)
not_done = false;
} // the big while...
thread_exit:
// Close the port cleanly
CloseComPortPhysical(m_gps_fd);
m_launcher->SetSecThreadInActive(); // I am dead
m_launcher->m_Thread_run_flag = -1;
return 0;
}
void OCP_DataStreamInput_Thread::HandleASuccessfulRead( char *szBuf, int nread )
{
if(nread > 0)
{
if((g_total_NMEAerror_messages < g_nNMEADebug) && (g_nNMEADebug > 1000))
{
g_total_NMEAerror_messages++;
wxString msg;
msg.Printf(_T("NMEA activity...%d bytes"), nread);
ThreadMessage(msg);
}
int nchar = nread;
char *pb = szBuf;
while(nchar)
{
if(0x0a == *pb)
m_nl_found = true;
*put_ptr++ = *pb++;
if((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
put_ptr = rx_buffer;
nchar--;
}
if((g_total_NMEAerror_messages < g_nNMEADebug) && (g_nNMEADebug > 1000))
{
g_total_NMEAerror_messages++;
wxString msg1 = _T("Buffer is: ");
int nc = nread;
char *pb = szBuf;
while(nc)
{
msg1.Append(*pb++);
nc--;
}
ThreadMessage(msg1);
}
}
// Found a NL char, thus end of message?
if(m_nl_found)
{
char *tptr;
char *ptmpbuf;
bool partial = false;
while (!partial)
{
// Copy the message into a temp buffer
tptr = tak_ptr;
ptmpbuf = temp_buf;
while((*tptr != 0x0a) && (tptr != put_ptr))
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > RX_BUFFER_SIZE)
tptr = rx_buffer;
// wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, "temp_buf overrun");
}
if((*tptr == 0x0a) && (tptr != put_ptr)) // well formed sentence
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
// wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, "temp_buf overrun");
*ptmpbuf = 0;
tak_ptr = tptr;
// parse and send the message
// wxString str_temp_buf(temp_buf, wxConvUTF8);
if(temp_buf[0] == '\r')
Parse_And_Send_Posn(&temp_buf[1]);
else
Parse_And_Send_Posn(temp_buf);
}
else
{
partial = true;
}
} // while !partial
} // nl found
}
// Entry Point
void *OCP_DataStreamInput_Thread::Entry()
{
bool not_done = true;
bool nl_found = false;
wxString msg;
// Request the com port from the comm manager
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) < 0)
{
wxString msg(_T("NMEA input device open failed: "));
msg.Append(m_PortName);
ThreadMessage(msg);
goto thread_exit;
}
m_launcher->SetSecThreadActive(); // I am alive
// The main loop
while((not_done) && (m_launcher->m_Thread_run_flag > 0))
{
if(TestDestroy())
not_done = false; // smooth exit
// Blocking, timeout protected read of one character at a time
// Timeout value is set by c_cc[VTIME]
// Storing incoming characters in circular buffer
// And watching for new line character
// On new line character, send notification to parent
char next_byte = 0;
ssize_t newdata;
newdata = read(m_gps_fd, &next_byte, 1); // read of one char
// return (-1) if no data available, timeout
#ifdef __WXOSX__
if (newdata < 0 )
wxThread::Sleep(100) ;
#endif
// Fulup patch for handling hot-plug or wakeup events
// from serial port drivers
{
static int maxErrorLoop;
if (newdata > 0)
{
// we have data, so clear error
maxErrorLoop =0;
}
else
{
// no need to retry every 1ms when on error
sleep (1);
// if we have more no character for 5 second then try to reopen the port
if (maxErrorLoop++ > 5)
{
// do not retry for the next 5s
maxErrorLoop = 0;
// free old unplug current port
CloseComPortPhysical(m_gps_fd);
// Request the com port from the comm manager
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) < 0) {
// wxString msg(_T("NMEA input device open failed (will retry): "));
// msg.Append(m_PortName);
// ThreadMessage(msg);
} else {
// wxString msg(_T("NMEA input device open on hotplug OK: "));
// msg.Append(m_PortName);
// ThreadMessage(msg);
}
}
}
}
// And process any character
if(newdata > 0)
{
nl_found = false;
*put_ptr++ = next_byte;
if((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
put_ptr = rx_buffer;
if(0x0a == next_byte)
nl_found = true;
// Found a NL char, thus end of message?
if(nl_found)
{
char *tptr;
char *ptmpbuf;
// Copy the message into a temporary _buffer
tptr = tak_ptr;
ptmpbuf = temp_buf;
while((*tptr != 0x0a) && (tptr != put_ptr))
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, (const wxChar *)"temp_buf overrun1");
}
if((*tptr == 0x0a) && (tptr != put_ptr)) // well formed sentence
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, (const wxChar *)"temp_buf overrun2");
*ptmpbuf = 0;
tak_ptr = tptr;
// Message is ready to parse and send out
// Messages may be coming in as <blah blah><lf><cr>.
// One example device is KVH1000 heading sensor.
// If that happens, the first character of a new captured message will the <cr>,
// and we need to discard it.
// This is out of spec, but we should handle it anyway
if(temp_buf[0] == '\r')
Parse_And_Send_Posn(&temp_buf[1]);
else
Parse_And_Send_Posn(temp_buf);
}
} //if nl
} // if newdata > 0
// Check for any pending output message
m_outCritical.Enter();
{
bool b_qdata = !out_que.empty();
while(b_qdata){
// Take a copy of message
char *qmsg = out_que.front();
char msg[MAX_OUT_QUEUE_MESSAGE_LENGTH];
strncpy( msg, qmsg, MAX_OUT_QUEUE_MESSAGE_LENGTH-1 );
out_que.pop();
free(qmsg);
m_outCritical.Leave();
WriteComPortPhysical(m_gps_fd, msg);
m_outCritical.Enter();
b_qdata = !out_que.empty();
} //while b_qdata
}
m_outCritical.Leave();
} // the big while...
// Close the port cleanly
CloseComPortPhysical(m_gps_fd);
thread_exit:
m_launcher->SetSecThreadInActive(); // I am dead
m_launcher->m_Thread_run_flag = -1;
return 0;
}
// Entry Point
void *OCP_DataStreamInput_Thread::Entry()
{
wxString msg;
bool not_done;
HANDLE hSerialComm = (HANDLE)(-1);
// Request the com port from the comm manager
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) < 0)
{
wxString msg(_T("NMEA input device open failed: "));
msg.Append(m_PortName);
wxString msg_error;
msg_error.Printf(_T("...GetLastError(): %d"), GetLastError());
msg.Append(msg_error);
ThreadMessage(msg);
goto thread_exit;
}
m_launcher->SetSecThreadActive(); // I am alive
hSerialComm = (HANDLE)m_gps_fd;
// If port supports output, set a short timeout so that output polling mechanism works
int max_timeout = 5;
int loop_timeout = 2000;
int n_reopen_wait = 2000;
if( (m_io_select == DS_TYPE_INPUT_OUTPUT) || (m_io_select == DS_TYPE_OUTPUT) ) {
loop_timeout = 2;
max_timeout = 5000;
}
COMMTIMEOUTS timeouts;
timeouts.ReadIntervalTimeout = MAXDWORD;
timeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
timeouts.ReadTotalTimeoutConstant = loop_timeout;
timeouts.WriteTotalTimeoutMultiplier = MAXDWORD;
timeouts.WriteTotalTimeoutConstant = MAXDWORD;
if (!SetCommTimeouts((HANDLE)m_gps_fd, &timeouts)) // Error setting time-outs.
goto thread_exit;
not_done = true;
bool nl_found;
#define READ_BUF_SIZE 200
char szBuf[READ_BUF_SIZE];
DWORD dwRead;
DWORD dwOneRead;
char chRead;
int ic;
int n_timeout;
// The main loop
while((not_done) && (m_launcher->m_Thread_run_flag > 0))
{
if(TestDestroy())
not_done = false; // smooth exit
// Was port closed due to error condition?
while(!m_gps_fd)
{
if((TestDestroy()) || (m_launcher->m_Thread_run_flag == 0))
goto thread_exit; // smooth exit
if(n_reopen_wait)
{
wxThread::Sleep(n_reopen_wait); // stall for a bit
n_reopen_wait = 0;
}
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) > 0)
{
hSerialComm = (HANDLE)m_gps_fd;
if(!SetCommMask((HANDLE)m_gps_fd, EV_RXCHAR)) // Setting Event Type
goto thread_exit;
COMMTIMEOUTS timeouts;
timeouts.ReadIntervalTimeout = MAXDWORD;
timeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
timeouts.ReadTotalTimeoutConstant = loop_timeout;
timeouts.WriteTotalTimeoutMultiplier = MAXDWORD;
timeouts.WriteTotalTimeoutConstant = MAXDWORD;
if (!SetCommTimeouts((HANDLE)m_gps_fd, &timeouts)) // Error setting time-outs.
goto thread_exit;
}
else
{
m_gps_fd = NULL;
wxThread::Sleep(2000); // stall for a bit
}
}
bool b_inner = true;
bool b_sleep = false;
dwRead = 0;
n_timeout = 0;
wxDateTime now = wxDateTime::Now();
int t = now.GetTicks(); // set a separate timer not controlled by serial port
ic=0;
while( b_inner ) {
if( m_pout_mutex && (wxMUTEX_NO_ERROR == m_pout_mutex->TryLock()) ){
bool b_qdata = (m_takIndex != (-1) || m_putIndex != (-1));
while(b_qdata){
if(m_takIndex < OUT_QUEUE_LENGTH) {
// Take a copy of message
char msg[MAX_OUT_QUEUE_MESSAGE_LENGTH];
strncpy( msg, m_poutQueue[m_takIndex], MAX_OUT_QUEUE_MESSAGE_LENGTH-1 );
// Update and release the taker index
if(m_takIndex==m_putIndex)
m_takIndex=m_putIndex=(-1);
else if(m_takIndex == (OUT_QUEUE_LENGTH-1) )
m_takIndex=0;
else
m_takIndex++;
m_pout_mutex->Unlock();
WriteComPortPhysical(m_gps_fd, msg);
if( wxMUTEX_NO_ERROR == m_pout_mutex->TryLock() )
b_qdata = (m_takIndex != (-1) || m_putIndex != (-1));
else
b_qdata = false;
}
else { // some index error
m_takIndex = (-1);
m_putIndex = (-1);
b_qdata = false;
}
} //while b_qdata
m_pout_mutex->Unlock();
} // Mutex lock
if( b_sleep ) // we need a sleep if the serial port does not honor commtimeouts
wxSleep(1);
if(ReadFile((HANDLE)m_gps_fd, &chRead, 1, &dwOneRead, NULL))
{
if(1 == dwOneRead) {
b_sleep = false;
szBuf[ic] = chRead;
dwRead++;
if(++ic > READ_BUF_SIZE - 1)
goto HandleASuccessfulRead;
if(chRead == 0x0a)
goto HandleASuccessfulRead;
}
else { // timed out
n_timeout++;;
if( n_timeout > max_timeout ) {
bool b_close = true;
// If the port is input only, double check the timeout
// We do this since some virtual serial port emulators
// do not seem to honor SetCommTimeouts() function
if( m_io_select == DS_TYPE_INPUT ) {
wxDateTime then = wxDateTime::Now();
int tt = then.GetTicks();
if( (tt - t) < (max_timeout * loop_timeout)/1000 ) {
b_close = false;
n_timeout = 0;
b_sleep = true;
}
}
if( b_close ) {
b_inner = false;
CloseComPortPhysical(m_gps_fd);
m_gps_fd = NULL;
dwRead = 0;
nl_found = false;
n_reopen_wait = 2000;
}
}
else if((TestDestroy()) || (m_launcher->m_Thread_run_flag == 0)) {
goto thread_exit; // smooth exit
}
}
}
else { // ReadFile Erorr
b_inner = false;
CloseComPortPhysical(m_gps_fd);
m_gps_fd = NULL;
dwRead = 0;
nl_found = false;
n_reopen_wait = 2000;
}
}
HandleASuccessfulRead:
if(dwRead > 0)
{
if((g_total_NMEAerror_messages < g_nNMEADebug) && (g_nNMEADebug > 1000))
{
g_total_NMEAerror_messages++;
wxString msg;
msg.Printf(_T("NMEA activity...%d bytes"), dwRead);
ThreadMessage(msg);
}
int nchar = dwRead;
char *pb = szBuf;
while(nchar)
{
if(0x0a == *pb)
nl_found = true;
*put_ptr++ = *pb++;
if((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
put_ptr = rx_buffer;
nchar--;
}
if((g_total_NMEAerror_messages < g_nNMEADebug) && (g_nNMEADebug > 1000))
{
g_total_NMEAerror_messages++;
wxString msg1 = _T("Buffer is: ");
int nc = dwRead;
char *pb = szBuf;
while(nc)
{
msg1.Append(*pb++);
nc--;
}
ThreadMessage(msg1);
}
}
// Found a NL char, thus end of message?
if(nl_found)
{
char *tptr;
char *ptmpbuf;
bool partial = false;
while (!partial)
{
// Copy the message into a temp buffer
tptr = tak_ptr;
ptmpbuf = temp_buf;
while((*tptr != 0x0a) && (tptr != put_ptr))
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > RX_BUFFER_SIZE)
tptr = rx_buffer;
// wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, "temp_buf overrun");
}
if((*tptr == 0x0a) && (tptr != put_ptr)) // well formed sentence
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
// wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, "temp_buf overrun");
*ptmpbuf = 0;
tak_ptr = tptr;
// parse and send the message
wxString str_temp_buf(temp_buf, wxConvUTF8);
Parse_And_Send_Posn(str_temp_buf);
}
else
{
partial = true;
}
} // while !partial
} // nl found
} // the big while...
thread_exit:
// Close the port cleanly
CloseComPortPhysical(m_gps_fd);
m_launcher->SetSecThreadInActive(); // I am dead
m_launcher->m_Thread_run_flag = -1;
return 0;
}
// Entry Point
void *OCP_DataStreamInput_Thread::Entry()
{
bool not_done = true;
bool nl_found;
wxString msg;
// Request the com port from the comm manager
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) < 0)
{
wxString msg(_T("NMEA input device open failed: "));
msg.Append(m_PortName);
ThreadMessage(msg);
goto thread_exit;
}
m_launcher->SetSecThreadActive(); // I am alive
// The main loop
while((not_done) && (m_launcher->m_Thread_run_flag > 0))
{
if(TestDestroy())
not_done = false; // smooth exit
// Blocking, timeout protected read of one character at a time
// Timeout value is set by c_cc[VTIME]
// Storing incoming characters in circular buffer
// And watching for new line character
// On new line character, send notification to parent
char next_byte = 0;
ssize_t newdata;
newdata = read(m_gps_fd, &next_byte, 1); // read of one char
// return (-1) if no data available, timeout
#ifdef __WXOSX__
if (newdata < 0 )
wxThread::Sleep(100) ;
#endif
// Fulup patch for handling hot-plug or wakeup events
// from serial port drivers
{
static int maxErrorLoop;
if (newdata > 0)
{
// we have data, so clear error
maxErrorLoop =0;
}
else
{
// no need to retry every 1ms when on error
sleep (1);
// if we have more no character for 5 second then try to reopen the port
if (maxErrorLoop++ > 5)
{
// do not retry for the next 5s
maxErrorLoop = 0;
// free old unplug current port
CloseComPortPhysical(m_gps_fd);
// Request the com port from the comm manager
if ((m_gps_fd = OpenComPortPhysical(m_PortName, m_baud)) < 0) {
// wxString msg(_T("NMEA input device open failed (will retry): "));
// msg.Append(m_PortName);
// ThreadMessage(msg);
} else {
wxString msg(_T("NMEA input device open on hotplug OK: "));
msg.Append(m_PortName);
ThreadMessage(msg);
}
}
}
}
// And process any character
if(newdata > 0)
{
nl_found = false;
*put_ptr++ = next_byte;
if((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
put_ptr = rx_buffer;
if(0x0a == next_byte)
nl_found = true;
// Found a NL char, thus end of message?
if(nl_found)
{
char *tptr;
char *ptmpbuf;
// Copy the message into a temporary _buffer
tptr = tak_ptr;
ptmpbuf = temp_buf;
while((*tptr != 0x0a) && (tptr != put_ptr))
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, (const wxChar *)"temp_buf overrun1");
}
if((*tptr == 0x0a) && (tptr != put_ptr)) // well formed sentence
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, (const wxChar *)"temp_buf overrun2");
*ptmpbuf = 0;
tak_ptr = tptr;
// Message is ready to parse and send out
wxString str_temp_buf(temp_buf, wxConvUTF8);
Parse_And_Send_Posn(str_temp_buf);
}
} //if nl
} // if newdata > 0
// Check for any pending output message
if( m_pout_mutex && (wxMUTEX_NO_ERROR == m_pout_mutex->TryLock()) ){
bool b_qdata = (m_takIndex != (-1) || m_putIndex != (-1));
while(b_qdata){
if(m_takIndex < OUT_QUEUE_LENGTH) {
// Take a copy of message
char msg[MAX_OUT_QUEUE_MESSAGE_LENGTH];
strncpy( msg, m_poutQueue[m_takIndex], MAX_OUT_QUEUE_MESSAGE_LENGTH-1 );
// Update and release the taker index
if(m_takIndex==m_putIndex)
m_takIndex=m_putIndex=(-1);
else if(m_takIndex == (OUT_QUEUE_LENGTH-1) )
m_takIndex=0;
else
m_takIndex++;
m_pout_mutex->Unlock();
WriteComPortPhysical(m_gps_fd, msg);
if( wxMUTEX_NO_ERROR == m_pout_mutex->TryLock() )
b_qdata = (m_takIndex != (-1) || m_putIndex != (-1));
else
b_qdata = false;
}
else { // some index error
m_takIndex = (-1);
m_putIndex = (-1);
b_qdata = false;
}
} //while b_qdata
m_pout_mutex->Unlock();
}
bail_output:
bool bail = true;
} // the big while...
// Close the port cleanly
CloseComPortPhysical(m_gps_fd);
thread_exit:
m_launcher->SetSecThreadInActive(); // I am dead
m_launcher->m_Thread_run_flag = -1;
return 0;
}
void *OCP_DataStreamInput_Thread::Entry()
{
bool not_done = true;
bool nl_found = false;
wxString msg;
// Request the com port from the comm manager
if (!OpenComPortPhysical(m_PortName, m_baud))
{
wxString msg(_T("NMEA input device open failed: "));
msg.Append(m_PortName);
ThreadMessage(msg);
//goto thread_exit; // This means we will not be trying to connect = The device must be connected when the thread is created. Does not seem to be needed/what we want as the reconnection logic is able to pick it up whenever it actually appears (Of course given it appears with the expected device name).
}
m_launcher->SetSecThreadActive(); // I am alive
// The main loop
static size_t retries = 0;
while((not_done) && (m_launcher->m_Thread_run_flag > 0))
{
if(TestDestroy())
not_done = false; // smooth exit
uint8_t next_byte = 0;
size_t newdata = -1;
if( m_serial.isOpen() ) {
try {
newdata = m_serial.read(&next_byte, 1);
} catch (std::exception &e) {
//std::cerr << "Serial read exception: " << e.what() << std::endl;
if(10 < retries++) {
// We timed out waiting for the next character 10 times, let's close the port so that the reconnection logic kicks in and tries to fix our connection.
CloseComPortPhysical();
retries = 0;
}
}
} else {
// Reconnection logic. Let's try to reopen the port while waiting longer every time (until we simply keep trying every 2.5 seconds)
//std::cerr << "Serial port seems closed." << std::endl;
wxMilliSleep(250 * retries);
CloseComPortPhysical();
if(OpenComPortPhysical(m_PortName, m_baud))
retries = 0;
else if(retries < 10)
retries++;
}
if(newdata == 1)
{
nl_found = false;
*put_ptr++ = next_byte;
if((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
put_ptr = rx_buffer;
if(0x0a == next_byte)
nl_found = true;
// Found a NL char, thus end of message?
if(nl_found)
{
char *tptr;
char *ptmpbuf;
// Copy the message into a temporary _buffer
tptr = tak_ptr;
ptmpbuf = temp_buf;
while((*tptr != 0x0a) && (tptr != put_ptr))
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, (const wxChar *)L"temp_buf overrun1");
}
if((*tptr == 0x0a) && (tptr != put_ptr)) // well formed sentence
{
*ptmpbuf++ = *tptr++;
if((tptr - rx_buffer) > DS_RX_BUFFER_SIZE)
tptr = rx_buffer;
wxASSERT_MSG((ptmpbuf - temp_buf) < DS_RX_BUFFER_SIZE, (const wxChar *)L"temp_buf overrun2");
*ptmpbuf = 0;
tak_ptr = tptr;
// Message is ready to parse and send out
// Messages may be coming in as <blah blah><lf><cr>.
// One example device is KVH1000 heading sensor.
// If that happens, the first character of a new captured message will the <cr>,
// and we need to discard it.
// This is out of spec, but we should handle it anyway
if(temp_buf[0] == '\r')
Parse_And_Send_Posn(&temp_buf[1]);
else
Parse_And_Send_Posn(temp_buf);
}
} //if nl
} // if newdata > 0
// Check for any pending output message
bool b_qdata = !out_que.empty();
while(b_qdata){
// Take a copy of message
char *qmsg = out_que.front();
out_que.pop();
//m_outCritical.Leave();
char msg[MAX_OUT_QUEUE_MESSAGE_LENGTH];
strncpy( msg, qmsg, MAX_OUT_QUEUE_MESSAGE_LENGTH-1 );
free(qmsg);
if( static_cast<size_t>(-1) == WriteComPortPhysical(msg) && 10 < retries++ ) {
// We failed to write the port 10 times, let's close the port so that the reconnection logic kicks in and tries to fix our connection.
retries = 0;
CloseComPortPhysical();
}
b_qdata = !out_que.empty();
} //while b_qdata
}
thread_exit:
CloseComPortPhysical();
m_launcher->SetSecThreadInActive(); // I am dead
m_launcher->m_Thread_run_flag = -1;
return 0;
}
void FRawProfilerSession::ProcessStatPacketArray( const FStatPacketArray& StatPacketArray, FProfilerFrame& out_ProfilerFrame, int32 FrameIndex )
{
// @TODO yrx 2014-03-24 Standardize thread names and id
// @TODO yrx 2014-04-22 Remove all references to the data provider, event graph etc once data graph can visualize.
// Raw stats callstack for this stat packet array.
TMap<FName,FProfilerStackNode*> ThreadNodes;
const FProfilerStatMetaDataRef MetaData = GetMetaData();
FProfilerSampleArray& MutableCollection = const_cast<FProfilerSampleArray&>(DataProvider->GetCollection());
// Add a root sample for this frame.
const uint32 FrameRootSampleIndex = DataProvider->AddHierarchicalSample( 0, MetaData->GetStatByID( 1 ).OwningGroup().ID(), 1, 0.0f, 0.0f, 1 );
// Iterate through all stats packets and raw stats messages.
FName GameThreadFName = NAME_None;
for( int32 PacketIndex = 0; PacketIndex < StatPacketArray.Packets.Num(); PacketIndex++ )
{
const FStatPacket& StatPacket = *StatPacketArray.Packets[PacketIndex];
FName ThreadFName = StatsThreadStats.Threads.FindChecked( StatPacket.ThreadId );
const uint32 NewThreadID = MetaData->ThreadIDtoStatID.FindChecked( StatPacket.ThreadId );
// @TODO yrx 2014-04-29 Only game or render thread is supported at this moment.
if( StatPacket.ThreadType != EThreadType::Game && StatPacket.ThreadType != EThreadType::Renderer )
{
continue;
}
// Workaround for issue with rendering thread names.
if( StatPacket.ThreadType == EThreadType::Renderer )
{
ThreadFName = NAME_RenderThread;
}
else if( StatPacket.ThreadType == EThreadType::Game )
{
GameThreadFName = ThreadFName;
}
FProfilerStackNode* ThreadNode = ThreadNodes.FindRef( ThreadFName );
if( !ThreadNode )
{
FString ThreadIdName = FStatsUtils::BuildUniqueThreadName( StatPacket.ThreadId );
FStatMessage ThreadMessage( ThreadFName, EStatDataType::ST_int64, STAT_GROUP_TO_FStatGroup( STATGROUP_Threads )::GetGroupName(), STAT_GROUP_TO_FStatGroup( STATGROUP_Threads )::GetGroupCategory(), *ThreadIdName, true, true );
//FStatMessage ThreadMessage( ThreadFName, EStatDataType::ST_int64, nullptr, nullptr, TEXT( "" ), true, true );
ThreadMessage.NameAndInfo.SetFlag( EStatMetaFlags::IsPackedCCAndDuration, true );
ThreadMessage.Clear();
// Add a thread sample.
const uint32 ThreadRootSampleIndex = DataProvider->AddHierarchicalSample
(
NewThreadID,
MetaData->GetStatByID( NewThreadID ).OwningGroup().ID(),
NewThreadID,
-1.0f,
-1.0f,
1,
FrameRootSampleIndex
);
ThreadNode = ThreadNodes.Add( ThreadFName, new FProfilerStackNode( nullptr, ThreadMessage, ThreadRootSampleIndex, FrameIndex ) );
}
TArray<const FStatMessage*> StartStack;
TArray<FProfilerStackNode*> Stack;
Stack.Add( ThreadNode );
FProfilerStackNode* Current = Stack.Last();
const FStatMessagesArray& Data = StatPacket.StatMessages;
for( int32 Index = 0; Index < Data.Num(); Index++ )
{
const FStatMessage& Item = Data[Index];
const EStatOperation::Type Op = Item.NameAndInfo.GetField<EStatOperation>();
const FName LongName = Item.NameAndInfo.GetRawName();
const FName ShortName = Item.NameAndInfo.GetShortName();
const FName RenderingThreadTickCommandName = TEXT("RenderingThreadTickCommand");
// Workaround for render thread hierarchy. EStatOperation::AdvanceFrameEventRenderThread is called within the scope.
if( ShortName == RenderingThreadTickCommandName )
{
continue;
}
if( Op == EStatOperation::CycleScopeStart || Op == EStatOperation::CycleScopeEnd || Op == EStatOperation::AdvanceFrameEventRenderThread )
{
//check( Item.NameAndInfo.GetFlag( EStatMetaFlags::IsCycle ) );
if( Op == EStatOperation::CycleScopeStart )
{
FProfilerStackNode* ChildNode = new FProfilerStackNode( Current, Item, -1, FrameIndex );
Current->Children.Add( ChildNode );
// Add a child sample.
const uint32 SampleIndex = DataProvider->AddHierarchicalSample
(
NewThreadID,
MetaData->GetStatByFName( ShortName ).OwningGroup().ID(), // GroupID
MetaData->GetStatByFName( ShortName ).ID(), // StatID
MetaData->ConvertCyclesToMS( ChildNode->CyclesStart ), // StartMS
MetaData->ConvertCyclesToMS( 0 ), // DurationMS
1,
Current->SampleIndex
);
ChildNode->SampleIndex = SampleIndex;
Stack.Add( ChildNode );
StartStack.Add( &Item );
Current = ChildNode;
}
// Workaround for render thread hierarchy. EStatOperation::AdvanceFrameEventRenderThread is called within the scope.
if( Op == EStatOperation::AdvanceFrameEventRenderThread )
{
int k=0;k++;
}
if( Op == EStatOperation::CycleScopeEnd )
{
const FStatMessage ScopeStart = *StartStack.Pop();
const FStatMessage ScopeEnd = Item;
const int64 Delta = int32( uint32( ScopeEnd.GetValue_int64() ) - uint32( ScopeStart.GetValue_int64() ) );
Current->CyclesEnd = Current->CyclesStart + Delta;
Current->CycleCounterStartTimeMS = MetaData->ConvertCyclesToMS( Current->CyclesStart );
Current->CycleCounterEndTimeMS = MetaData->ConvertCyclesToMS( Current->CyclesEnd );
if( Current->CycleCounterStartTimeMS > Current->CycleCounterEndTimeMS )
{
int k=0;k++;
}
check( Current->CycleCounterEndTimeMS >= Current->CycleCounterStartTimeMS );
FProfilerStackNode* ChildNode = Current;
// Update the child sample's DurationMS.
MutableCollection[ChildNode->SampleIndex].SetDurationMS( MetaData->ConvertCyclesToMS( Delta ) );
verify( Current == Stack.Pop() );
Current = Stack.Last();
}
}
}
}
// Calculate thread times.
for( auto It = ThreadNodes.CreateIterator(); It; ++It )
{
FProfilerStackNode& ThreadNode = *It.Value();
const int32 ChildrenNum = ThreadNode.Children.Num();
if( ChildrenNum > 0 )
{
const int32 LastChildIndex = ThreadNode.Children.Num() - 1;
ThreadNode.CyclesStart = ThreadNode.Children[0]->CyclesStart;
ThreadNode.CyclesEnd = ThreadNode.Children[LastChildIndex]->CyclesEnd;
ThreadNode.CycleCounterStartTimeMS = MetaData->ConvertCyclesToMS( ThreadNode.CyclesStart );
ThreadNode.CycleCounterEndTimeMS = MetaData->ConvertCyclesToMS( ThreadNode.CyclesEnd );
FProfilerSample& ProfilerSample = MutableCollection[ThreadNode.SampleIndex];
ProfilerSample.SetStartAndEndMS( MetaData->ConvertCyclesToMS( ThreadNode.CyclesStart ), MetaData->ConvertCyclesToMS( ThreadNode.CyclesEnd ) );
}
}
// Get the game thread time.
check( GameThreadFName != NAME_None );
const FProfilerStackNode& GameThreadNode = *ThreadNodes.FindChecked( GameThreadFName );
const double GameThreadStartMS = MetaData->ConvertCyclesToMS( GameThreadNode.CyclesStart );
const double GameThreadEndMS = MetaData->ConvertCyclesToMS( GameThreadNode.CyclesEnd );
MutableCollection[FrameRootSampleIndex].SetStartAndEndMS( GameThreadStartMS, GameThreadEndMS );
// Advance frame
const uint32 LastFrameIndex = DataProvider->GetNumFrames();
DataProvider->AdvanceFrame( GameThreadEndMS - GameThreadStartMS );
// Update aggregated stats
UpdateAggregatedStats( LastFrameIndex );
// Update aggregated events.
UpdateAggregatedEventGraphData( LastFrameIndex );
// RootNode is the same as the game thread node.
out_ProfilerFrame.Root->CycleCounterStartTimeMS = GameThreadStartMS;
out_ProfilerFrame.Root->CycleCounterEndTimeMS = GameThreadEndMS;
for( auto It = ThreadNodes.CreateIterator(); It; ++It )
{
out_ProfilerFrame.AddChild( It.Value() );
}
out_ProfilerFrame.SortChildren();
}
