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_reset(void)
{
sil_ui_will_reset();
if (gpsSocket) UDBSocket_close(gpsSocket);
if (telemetrySocket) UDBSocket_close(telemetrySocket);
if (serialSocket) UDBSocket_close(serialSocket);
char *args[3] = {mp_argv[0], UDB_HW_RESET_ARG, 0};
execv(mp_argv[0], args);
fprintf(stderr, "Failed to reset UDB %s\n", mp_argv[0]);
exit(1);
}
void sil_reset(void)
{
const char *args[3] = {mp_argv[0], UDB_HW_RESET_ARG, 0};
sil_ui_will_reset();
if (gpsSocket) UDBSocket_close(gpsSocket);
if (telemetrySocket) UDBSocket_close(telemetrySocket);
if (serialSocket) UDBSocket_close(serialSocket);
_execv(mp_argv[0], args); // this version keeps VC++ happy (along with <process.h> above)
fprintf(stderr, "Failed to reset UDB %s\n", mp_argv[0]);
exit(1);
}
void CloseComms(void)
{
if (serialSock) {
UDBSocket_close(serialSock);
serialSock = NULL;
LoggingFile.mLogFile << "Closed serial port" << endl;
printf("Closed serial port\n");
}
if (udpSock) {
UDBSocket_close(udpSock);
udpSock = NULL;
LoggingFile.mLogFile << "Closed udp port" << endl;
printf("Closed udp port\n");
}
activeSock = NULL;
}
void sil_reset(void)
{
const char *args[3] = {mp_argv[0], UDB_HW_RESET_ARG, 0};
sil_ui_will_reset();
if (gpsSocket) UDBSocket_close(gpsSocket);
if (telemetrySocket) UDBSocket_close(telemetrySocket);
if (serialSocket) UDBSocket_close(serialSocket);
#ifdef _MSC_VER
_execv(mp_argv[0], args);
#else
execv(mp_argv[0], args);
#endif
fprintf(stderr, "Failed to reset UDB %s\n", mp_argv[0]);
exit(1);
}
void ReceiveFromComPort(void)
{
if (ReceiveFromSocket(serialSock) && !activeSock) {
activeSock = serialSock;
if (udpSock) {
UDBSocket_close(udpSock);
udpSock = NULL;
}
LoggingFile.mLogFile << "Using serial port. Closed udp port." << endl;
printf("Using serial port. Closed udp port.\n");
}
if (ReceiveFromSocket(udpSock) && !activeSock) {
activeSock = udpSock;
if (serialSock) {
UDBSocket_close(serialSock);
serialSock = NULL;
}
LoggingFile.mLogFile << "Using udp port. Closed serial port." << endl;
printf("Using udp port. Closed serial port.\n");
}
}
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;
}
// Call this function to initiate sending a data to the serial port
void udb_serial_start_sending_data(void)
{
if (!telemetrySocket) return;
uint8_t buffer[BUFLEN];
int16_t c;
int16_t pos=0;
while (pos < BUFLEN && (c = udb_serial_callback_get_byte_to_send()) != -1) {
buffer[pos++] = c;
}
int16_t bytesWritten = UDBSocket_write(telemetrySocket, (uint8_t*)buffer, pos);
if (bytesWritten == -1) {
UDBSocket_close(telemetrySocket);
telemetrySocket = NULL;
}
}
// Call this function to initiate sending data to the GPS
void udb_gps_start_sending_data(void)
{
uint8_t buffer[BUFLEN];
int16_t bytesWritten;
int16_t c;
int16_t pos=0;
if (!gpsSocket) return;
while (pos < BUFLEN && (c = udb_gps_callback_get_byte_to_send()) != -1) {
buffer[pos++] = c;
}
bytesWritten = UDBSocket_write(gpsSocket, (uint8_t*)buffer, pos);
if (bytesWritten < 0) {
UDBSocket_close(gpsSocket);
gpsSocket = NULL;
}
}
void mavlink_start_sending_data(void)
{
uint8_t buffer[BUFLEN];
int16_t bytesWritten;
int16_t c;
int16_t pos = 0;
if (!telemetrySocket) return;
//#error here - this doesn't work when telemetry is UDB_EXTRA etc
// while (pos < BUFLEN && (c = udb_serial_callback_get_byte_to_send()) != -1) {
while (pos < BUFLEN && (c = mavlink_callback_get_byte_to_send()) != -1) {
buffer[pos++] = c;
}
bytesWritten = UDBSocket_write(telemetrySocket, (uint8_t*)buffer, pos);
if (bytesWritten == -1) {
UDBSocket_close(telemetrySocket);
telemetrySocket = NULL;
}
}
UDBSocket UDBSocket_init(UDBSocketType type, uint16_t UDP_port, char* UDP_host, char* serial_port, long serial_baud)
{
UDBSocket newSocket = (UDBSocket)malloc(sizeof(UDBSocket_t));
if (!newSocket)
{
return NULL;
}
memset((char*)newSocket, 0, sizeof(UDBSocket_t));
newSocket->type = type;
newSocket->UDP_port = UDP_port;
newSocket->UDP_host = (UDP_host) ? _strdup(UDP_host) : NULL;
newSocket->serial_port = (serial_port) ? _strdup(serial_port) : NULL;
newSocket->serial_baud = serial_baud;
switch (newSocket->type)
{
case UDBSocketStandardInOut:
{
SetTermIOs();
break;
}
case UDBSocketUDPClient:
{
u_long on = 1;
if (!hasInitializedWSA)
{
if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "WSAStartup Failed. Error Code : %d", WSAGetLastError());
return NULL;
}
hasInitializedWSA = 1;
}
if ((newSocket->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "socket() failed. Error Code : %d", WSAGetLastError());
free(newSocket);
return NULL;
}
if (ioctlsocket(newSocket->fd, FIONBIO, &on) < 0)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "ioctl() failed. Error Code : %d", WSAGetLastError());
UDBSocket_close(newSocket);
return NULL;
}
memset((char*) &newSocket->si_other, 0, sizeof(newSocket->si_other));
newSocket->si_other.sin_family = AF_INET;
newSocket->si_other.sin_port = htons(newSocket->UDP_port);
newSocket->si_other.sin_addr.S_un.S_addr = inet_addr(UDP_host);
UDBSocket_write(newSocket, (unsigned char*)"", 0); // Initiate connection
break;
}
case UDBSocketUDPServer:
{
struct sockaddr_in si_me;
u_long on = 1;
if (!hasInitializedWSA)
{
if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "WSAStartup Failed. Error Code : %d", WSAGetLastError());
return NULL;
}
hasInitializedWSA = 1;
}
if ((newSocket->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "socket() Error Code : %d", WSAGetLastError());
free(newSocket);
return NULL;
}
if (ioctlsocket(newSocket->fd, FIONBIO, &on) < 0)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "ioctl() Error Code : %d", WSAGetLastError());
UDBSocket_close(newSocket);
return NULL;
}
newSocket->si_other.sin_family = AF_INET;
memset((char*) &si_me, 0, sizeof(si_me));
si_me.sin_family = AF_INET;
si_me.sin_port = htons(newSocket->UDP_port);
si_me.sin_addr.s_addr = htonl(INADDR_ANY);
if (bind(newSocket->fd, (const struct sockaddr*)&si_me, sizeof(si_me)) == -1)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "bind() Error Code : %d", WSAGetLastError());
UDBSocket_close(newSocket);
return NULL;
}
break;
}
case UDBSocketSerial:
{
DCB Dcb;
COMMTIMEOUTS CommTimeouts;
DWORD dwRetFlag;
if (newSocket->hComms == 0)
{
char port[512]; // sketchy, but should be plenty long
strcpy(port, "\\\\.\\");
strncpy(port+strlen(port), newSocket->serial_port, 512-6);
newSocket->hComms = CreateFile(port,
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,// | FILE_FLAG_RANDOM_ACCESS,
NULL);
if (newSocket->hComms == INVALID_HANDLE_VALUE)
{
//sprintf(ErrorString, "Could not open comm port");
//ShowMessage(ErrorString);
//LoggingFile.mLogFile << "Could not open Com Port";
//LoggingFile.mLogFile << endl;
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "Could not open comm port");
UDBSocket_close(newSocket);
return NULL;
}
else
{
dwRetFlag = GetCommState(newSocket->hComms, &Dcb);
if (!dwRetFlag)
{
//sprintf(ErrorString, "GetCommState Error = %d", GetLastError());
//ShowMessage(ErrorString);
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "GetCommState Error = %d", GetLastError());
UDBSocket_close(newSocket);
return NULL;
}
Dcb.DCBlength = sizeof(Dcb);
Dcb.BaudRate = newSocket->serial_baud;
Dcb.ByteSize = 8;
Dcb.Parity = NOPARITY;
Dcb.StopBits = ONESTOPBIT;
Dcb.fTXContinueOnXoff = true;
Dcb.fOutxCtsFlow = false; // disable CTS output flow control
Dcb.fOutxDsrFlow = false; // disable DSR output flow control
Dcb.fDtrControl = DTR_CONTROL_DISABLE; // DTR_CONTROL_HANDSHAKE /*DTR_CONTROL_DISABLE DTR_CONTROL_ENABLE*/;
Dcb.fDsrSensitivity = false; // enable DSR sensitivity
Dcb.fOutX = false; // disable XON/XOFF out flow control
Dcb.fInX = false; // disable XON/XOFF in flow control
Dcb.fErrorChar = false; // disable error replacement
Dcb.fNull = false; // disable null stripping
Dcb.fRtsControl = RTS_CONTROL_DISABLE; // RTS_CONTROL_HANDSHAKE /* RTS_CONTROL_ENABLE RTS_CONTROL_DISABLE*/; // enable RTS line
Dcb.fAbortOnError = true; // don't abort reads/writes on error
dwRetFlag = SetCommState(newSocket->hComms, &Dcb);
if (!dwRetFlag)
{
//sprintf(ErrorString, "SetCommState Error = %d", GetLastError());
//ShowMessage(ErrorString);
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "SetCommState Error = %d", GetLastError());
UDBSocket_close(newSocket);
return NULL;
}
dwRetFlag = GetCommTimeouts(newSocket->hComms, &CommTimeouts);
if (!dwRetFlag)
{
//sprintf(ErrorString, "GetCommTimeouts Error = %d", GetLastError());
//ShowMessage(ErrorString);
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "GetCommTimeouts Error = %d", GetLastError());
UDBSocket_close(newSocket);
return NULL;
}
CommTimeouts.ReadIntervalTimeout = MAXDWORD; //Don't use interval timeouts
CommTimeouts.ReadTotalTimeoutMultiplier = 0; //Don't use multipliers
CommTimeouts.ReadTotalTimeoutConstant = 0; //150ms total read timeout
CommTimeouts.WriteTotalTimeoutMultiplier = 0; //Don't use multipliers
CommTimeouts.WriteTotalTimeoutConstant = 0; //2200ms total write timeout
dwRetFlag = SetCommTimeouts(newSocket->hComms, &CommTimeouts);
if (!dwRetFlag)
{
//sprintf(ErrorString, "SetCommTimeouts Error = %d", GetLastError());
//ShowMessage(ErrorString);
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "SetCommTimeouts Error = %d", GetLastError());
UDBSocket_close(newSocket);
return NULL;
}
}
}
else
{
//ShowMessage("Comm port already open");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "Com Port already open");
UDBSocket_close(newSocket);
return NULL;
}
break;
}
default:
break;
}
return newSocket;
}
void sil_ui_will_reset(void)
{
if (stdioSocket) {
UDBSocket_close(stdioSocket);
}
}
UDBSocket UDBSocket_init(UDBSocketType type, uint16_t UDP_port, char* UDP_host, char* serial_port, long serial_baud)
{
#if (LIN == 1)
speed_t BAUD;
#endif
UDBSocket newSocket = (UDBSocket)malloc(sizeof(UDBSocket_t));
if (!newSocket)
{
return NULL;
}
memset((char*)newSocket, 0, sizeof(UDBSocket_t));
newSocket->type = type;
newSocket->UDP_port = UDP_port;
newSocket->UDP_host = (UDP_host) ? strdup(UDP_host) : NULL;
newSocket->serial_port = (serial_port) ? strdup(serial_port) : NULL;
newSocket->serial_baud = serial_baud;
switch (newSocket->type)
{
case UDBSocketStandardInOut:
{
SetTermIOs();
break;
}
case UDBSocketUDPClient:
{
if ((newSocket->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
{
perror("socket() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "socket() failed");
free(newSocket);
return NULL;
}
int on = 1;
if (ioctl(newSocket->fd, FIONBIO, (char*)&on) < 0)
{
perror("ioctl() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "ioctl() failed");
UDBSocket_close(newSocket);
return NULL;
}
memset((char*) &newSocket->si_other, 0, sizeof(newSocket->si_other));
newSocket->si_other.sin_family = AF_INET;
newSocket->si_other.sin_port = htons(newSocket->UDP_port);
if (inet_aton(UDP_host, &newSocket->si_other.sin_addr) == 0)
{
fprintf(stderr, "inet_aton() failed\n");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "inet_aton() failed");
UDBSocket_close(newSocket);
return NULL;
}
UDBSocket_write(newSocket, (unsigned char*)"", 0); // Initiate connection
break;
}
case UDBSocketUDPServer:
{
struct sockaddr_in si_me;
if ((newSocket->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
{
perror("socket() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "socket() failed");
free(newSocket);
return NULL;
}
int on = 1;
if (ioctl(newSocket->fd, FIONBIO, (char*)&on) < 0)
{
perror("ioctl() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "ioctl() failed");
UDBSocket_close(newSocket);
return NULL;
}
newSocket->si_other.sin_family = AF_INET;
memset((char*) &si_me, 0, sizeof(si_me));
si_me.sin_family = AF_INET;
si_me.sin_port = htons(newSocket->UDP_port);
si_me.sin_addr.s_addr = htonl(INADDR_ANY);
if (bind(newSocket->fd, (const struct sockaddr*)&si_me, sizeof(si_me)) == -1)
{
perror("bind() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "bind() failed");
UDBSocket_close(newSocket);
return NULL;
}
break;
}
case UDBSocketSerial:
{
struct termios config;
newSocket->fd = open(newSocket->serial_port, O_RDWR | O_NOCTTY | O_NDELAY);
if (newSocket->fd == -1) {
perror("serial open() failed");
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "serial open() failed");
UDBSocket_close(newSocket);
return NULL;
}
if (!isatty(newSocket->fd))
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "serial port not a tty");
UDBSocket_close(newSocket);
return NULL;
}
if (tcgetattr(newSocket->fd, &config) < 0)
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "tcgetattr() failed");
UDBSocket_close(newSocket);
return NULL;
}
#if (LIN == 1)
switch (newSocket->serial_baud)
{
case 1152000:
BAUD = 1152000;
break;
case 1000000:
BAUD = 1000000;
break;
case 921600:
BAUD = 921600;
break;
case 576000:
BAUD = 576000;
break;
case 500000:
BAUD = 500000;
break;
case 460800:
BAUD = 460800;
break;
case 230400:
BAUD = B230400;
break;
case 115200:
BAUD = B115200;
break;
case 57600:
BAUD = B57600;
break;
case 38400:
BAUD = B38400;
break;
case 19200:
BAUD = B19200;
break;
case 9600:
BAUD = B9600;
break;
case 4800:
BAUD = B4800;
break;
default:
BAUD = B38400;
break;
} //end of switch CommPortSpeed
#endif
//
// Input flags - Turn off input processing
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
config.c_oflag = 0;
//
// No line processing:
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
//
// Turn off character processing
// clear current char size mask, no parity checking,
// no output processing, force 8 bit input
//
config.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
config.c_cflag |= CS8;
//
// One input byte is enough to return from read()
// Inter-character timer off
//
config.c_cc[VMIN] = 1;
config.c_cc[VTIME] = 0;
#if (LIN == 1)
if (cfsetospeed(&config, BAUD) < 0)
#else
if (cfsetospeed(&config, newSocket->serial_baud) < 0)
#endif
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "cfsetospeed() failed");
UDBSocket_close(newSocket);
return NULL;
}
#if (LIN == 1)
if (cfsetispeed(&config, BAUD) < 0)
#else
if (cfsetispeed(&config, newSocket->serial_baud) < 0)
#endif
{
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "cfsetispeed() failed");
UDBSocket_close(newSocket);
return NULL;
}
//
// Finally, apply the configuration
//
if (tcsetattr(newSocket->fd, TCSAFLUSH, &config) < 0) {
snprintf(UDBSocketLastError, LAST_ERR_BUF_SIZE, "tcsetattr() failed");
UDBSocket_close(newSocket);
return NULL;
}
break;
}
default:
break;
}
return newSocket;
}