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()
{
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;
}
// 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;
}
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;
}