boolean handleUDBSockets(void)
{
uint8_t buffer[BUFLEN];
int32_t bytesRead;
int16_t i;
boolean didRead = false;
// Handle GPS Socket
if (gpsSocket) {
bytesRead = UDBSocket_read(gpsSocket, buffer, BUFLEN);
if (bytesRead < 0) {
UDBSocket_close(gpsSocket);
gpsSocket = NULL;
}
else {
for (i=0; i<bytesRead; i++) {
udb_gps_callback_received_byte(buffer[i]);
}
if (bytesRead>0) didRead = true;
}
}
// Handle Telemetry Socket
if (telemetrySocket) {
bytesRead = UDBSocket_read(telemetrySocket, buffer, BUFLEN);
if (bytesRead < 0) {
UDBSocket_close(telemetrySocket);
telemetrySocket = NULL;
}
else {
for (i=0; i<bytesRead; i++) {
udb_serial_callback_received_byte(buffer[i]);
}
if (bytesRead>0) didRead = true;
}
}
// Handle optional Serial RC input Socket
if (serialSocket) {
bytesRead = UDBSocket_read(serialSocket, buffer, BUFLEN);
if (bytesRead < 0) {
UDBSocket_close(serialSocket);
serialSocket = NULL;
}
else {
if (bytesRead>0) {
sil_handle_seial_rc_input(buffer, bytesRead);
didRead = true;
}
}
}
return didRead;
}
void sil_ui_update(void)
{
uint8_t buffer[BUFLEN];
int32_t bytesRead;
int16_t i;
// Handle stdin
if (stdioSocket) {
bytesRead = UDBSocket_read(stdioSocket, buffer, BUFLEN);
for (i = 0; i < bytesRead; i++) {
sil_handle_key_input(buffer[i]);
}
}
sil_checkForLedUpdates();
if (hasShownInitStates == 0 && waggle != 0) {
printf("INIT: sensors calibrated and trims recorded.\n");
printf("INIT: waiting for GPS...\n");
hasShownInitStates = 1;
} else if (hasShownInitStates == 1 && dcm_flags._.dead_reckon_enable == 1) {
printf("INIT: GPS link acquired.\n");
printf("INIT: Ready.\n");
hasShownInitStates = 2;
}
}
int ReceiveFromSocket(UDBSocket sock)
{
int didReceive = 0;
if (sock) {
unsigned char Buffer[BUFLEN];
int loops = 10;
while (loops--) {
long n = UDBSocket_read(sock, Buffer, BUFLEN);
if (n < 0) {
LoggingFile.mLogFile << "serial read failed" << endl;
LoggingFile.mLogFile << UDBSocketLastErrorMessage() << endl;
printf("%s\n", UDBSocketLastErrorMessage());
CloseComms();
break;
} else {
if (n == 0) {
break;
} else {
int i;
for (i = 0; i < n; i++) {
if (HandleMsgByte(Buffer[i]) == 1) {
didReceive = 1;
}
}
}
}
}
}
return didReceive;
}
int UDBSocket_write(UDBSocket socket, unsigned char* data, int dataLength)
{
switch (socket->type)
{
case UDBSocketStandardInOut:
{
int i;
for (i = 0; i < dataLength; i++)
{
_putch(data[i]);
}
fflush(stdout);
return i;
}
case UDBSocketUDPClient:
case UDBSocketUDPServer:
{
int bytesWritten;
if (socket->type == UDBSocketUDPServer && socket->si_other.sin_port == 0)
{
UDBSocket_read(socket, NULL, 0);
if (socket->si_other.sin_port == 0)
{
return 0;
}
}
bytesWritten = (int)sendto(socket->fd, (char*)data, dataLength, 0, (const struct sockaddr*)&socket->si_other, sizeof(socket->si_other));
if (bytesWritten < 0)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "sendto() Error Code : %d", WSAGetLastError());
return -1;
}
return bytesWritten;
}
case UDBSocketSerial:
{
unsigned long bytesWritten;
DWORD dwRetFlag;
if (socket->hComms != 0)
{
dwRetFlag = WriteFile(socket->hComms, data, dataLength, &bytesWritten, NULL);
if (!dwRetFlag)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "WriteFile Error = %d", GetLastError());
//ShowMessage(ErrorString);
return -1;
}
}
else
{
//ShowMessage("Comm port not open");
}
return bytesWritten;
}
default:
break;
}
return -1;
}
int UDBSocket_write(UDBSocket socket, unsigned char* data, int dataLength)
{
switch (socket->type)
{
case UDBSocketStandardInOut:
{
int i;
for (i = 0; i < dataLength; i++)
{
fputc(data[i], stdout);
}
fflush(stdout);
return i;
}
case UDBSocketUDPClient:
case UDBSocketUDPServer:
{
int bytesWritten;
if (socket->type == UDBSocketUDPServer && socket->si_other.sin_port == 0)
{
UDBSocket_read(socket, NULL, 0);
if (socket->si_other.sin_port == 0)
{
return 0;
}
}
bytesWritten = (int)sendto(socket->fd, data, dataLength, 0, (const struct sockaddr*)&socket->si_other, sizeof(socket->si_other));
if (bytesWritten < 0)
{
perror("sendto() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "sendto() failed");
return -1;
}
return bytesWritten;
}
case UDBSocketSerial:
{
int bytesWritten = (int)write(socket->fd, data, dataLength);
if (bytesWritten < 0)
{
perror("write() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "write() failed");
return -1;
}
return bytesWritten;
}
default:
break;
}
return -1;
}
uint8_t readSockets(void)
{
uint8_t buffer[BUFLEN];
int32_t bytesRead;
uint8_t didRead = 0;
if (transportSocket) {
bytesRead = UDBSocket_read(transportSocket, buffer, BUFLEN);
if (bytesRead < 0) {
UDBSocket_close(transportSocket);
transportSocket = NULL;
printf("ERROR: read failed: %s\n", UDBSocketLastErrorMessage());
exit(1);
}
else if (bytesRead > 0) {
bytesRead = UDBSocket_write(stdioSocket, buffer, bytesRead);
didRead = 1;
}
}
if (stdioSocket) {
bytesRead = UDBSocket_read(stdioSocket, buffer, BUFLEN);
if (bytesRead > 0) {
bytesRead = UDBSocket_write(transportSocket, buffer, bytesRead);
if (bytesRead < 0) {
UDBSocket_close(transportSocket);
transportSocket = NULL;
printf("ERROR: write failed: %s\n", UDBSocketLastErrorMessage());
exit(1);
}
didRead = 1;
}
}
return didRead;
}
