/* beware - read_bird_data and read_bird_words are not the same... */
static int read_bird_data(int fd, short *wds, int n) {
unsigned char buf[MAXWORDS*2];
int i;
if (n > MAXWORDS)
veFatalError(MODULE, "tried to read too many words from bird: %d", n);
if ((i = forceread(fd,buf,2*n)) <= 0)
return i;
/* return raw words */
for(i = 0; i < n; i++)
wds[i] = (short)(buf[2*i]&0xff)|((short)buf[2*i+1]<<8);
return n;
}
int main() {
int sockFd;
if ((sockFd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
perror("socket() error");
struct sockaddr_in servAddr;
bzero(&servAddr, sizeof(servAddr));
servAddr.sin_family = AF_INET;
servAddr.sin_port = htons(57345);
servAddr.sin_addr.s_addr = htonl(INADDR_ANY);
if (bind(sockFd, (const struct sockaddr *) &servAddr, sizeof(servAddr)) < 0)
perror("bind() error");
if (listen(sockFd, 16) < 0)
perror("listen() error");
int maxFd = sockFd, maxClient = -1;
int clients[FD_SETSIZE];
for (int i = 0; i < FD_SETSIZE; ++i)
clients[i] = -1;
fd_set allFds;
FD_ZERO(&allFds);
FD_SET(sockFd, &allFds);
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
while (1) {
int readyNum;
fd_set readFds = allFds;
if ((readyNum = select(maxFd + 1, &readFds, NULL, NULL, NULL)) < 0)
perror("select() error");
if (FD_ISSET(sockFd, &readFds)) {
int connFd;
struct sockaddr_in clieAddr;
socklen_t clieLen = sizeof(clieAddr);
if ((connFd = accept(sockFd, (struct sockaddr *) &clieAddr, &clieLen)) < 0)
perror("accept() error");
int i;
for (i = 0; i < FD_SETSIZE; ++i) {
if (clients[i] < 0) {
clients[i] = connFd;
break;
}
}
if (i == FD_SETSIZE)
fprintf(stderr, "maximum number of clients reached\n");
if (i > maxClient)
maxClient = i;
FD_SET(connFd, &allFds);
if (connFd > maxFd)
maxFd = connFd;
if (forcewrite(connFd, "**hello**\n", 11) < 0)
perror("write() error");
if (--readyNum <= 0)
continue;
}
for (int i = 0; i <= maxClient; i++) {
int clieFd = clients[i];
if (clieFd < 0)
continue;
if (FD_ISSET(clieFd, &readFds)) {
char buf[2048];
ssize_t len;
if ((len = forceread(clieFd, buf, 2048)) < 0)
perror("read() error");
else if (len == 0) {
if (close(clieFd) < 0)
perror("close() error");
FD_CLR(clieFd, &allFds);
clients[i] = -1;
}
else if (!strcmp(buf, "\n")) {
if (forcewrite(clieFd, "**bye**\n", 9) < 0)
perror("write() error");
if (close(clieFd) < 0)
perror("close() error");
FD_CLR(clieFd, &allFds);
clients[i] = -1;
}
else {
for (int j = 0; j < len; ++j)
if ('a' <= buf[j] && buf[j] <= 'z')
buf[j] -= 0x20;
if (forcewrite(clieFd, buf, (size_t) len) < 0)
perror("write() error");
}
if (--readyNum <= 0)
break;
}
}
}
#pragma clang diagnostic pop
}
static void show_bird_info(VeDevice *d) {
/* get various bits of info from the FOB and spit it out on stdout */
unsigned char obuf[2], ibuf[11];
short wds[10];
VeiFlockOfBirdsGroup *b = (VeiFlockOfBirdsGroup *)(d->instance->idata);
obuf[0] = EXAMINE_VALUE;
/* printf("Flock of Birds tracker (%s)\n", d->name); */
/* get system model */
obuf[1] = BIRD1;
obuf[2] = SYSTEM_MODEL_ID;
send_bird_data(b->fd,obuf,3);
memset(ibuf,0,11);
if (forceread(b->fd,ibuf,10) != 10)
veFatalError(MODULE, "failed to read response from bird %s (%s)",
d->name, strerror(errno));
/* printf("Model: %s\n", ibuf); */
/* get PROM revision */
obuf[1] = BIRD1;
obuf[2] = SOFTWARE_REV;
send_bird_data(b->fd,obuf,3);
if (read_bird_data(b->fd,wds,1) != 1)
veFatalError(MODULE, "failed to read response from bird %s (%s)",
d->name, strerror(errno));
/* printf("PROM Revision: %d.%d\n", (int)(wds[0]>>8)&0xff,
(wds[0]&0xff)); */
/* get Crystal speed */
obuf[1] = BIRD1;
obuf[2] = CRYSTAL_SPEED;
send_bird_data(b->fd,obuf,3);
if (read_bird_data(b->fd,wds,1) != 1)
veFatalError(MODULE, "failed to read response from bird %s (%s)",
d->name, strerror(errno));
/* printf("Crystal speed: %d MHz\n", (int)(wds[0])); */
/* check filters */
obuf[1] = 3;
send_bird_data(b->fd,obuf,2);
if (read_bird_data(b->fd,wds,1) != 1)
veFatalError(MODULE, "failed to read response from bird %s (%s)",
d->name, strerror(errno));
/* printf("AC NARROW notch filter is %s\n", wds[0]&0x04 ? "on" : "off" ); */
/* printf("AC WIDE notch filter is %s\n", wds[0]&0x02 ? "on" : "off");
printf("DC filter is %s\n", wds[0]&0x01 ? "on" : "off");
*/
/* measurement rate */
obuf[1] = 7;
send_bird_data(b->fd,obuf,2);
if (read_bird_data(b->fd,wds,1) != 1)
veFatalError(MODULE, "failed to read response from bird %s (%s)",
d->name, strerror(errno));
/* printf("Bird measurement rate: %g Hz\n",
(*(unsigned short *)(&wds[0]))/256.0); */
/* get bird status */
obuf[1] = 0;
send_bird_data(b->fd,obuf,2);
if (read_bird_data(b->fd,wds,1) != 1)
veFatalError(MODULE, "failed to read response from bird %s (%s)",
d->name, strerror(errno));
/* printf("Bird is %s\n", (wds[0] & 0x8000) ? "master" : "slave");
printf("Bird has %sbeen initialized\n", (wds[0] & 0x4000) ? "" : "not ");
if (wds[0] & 0x2000)
printf("Bird has detected error\n");
printf("Bird is %srunning\n", (wds[0] & 0x1000) ? "" : "not ");
if (wds[0] & 0x0800)
printf("Bird is in HOST SYNC mode\n");
printf("Addressing mode is %s\n", (wds[0] & 0x0400) ? "expanded" : "normal");
if (wds[0] & 0x0200)
printf("Bird is in CRT SYNC mode\n");
printf("Bird is in %s mode\n", (wds[0] & 0x0020) ? "sleep" : "run");
switch((((unsigned)wds[0])>>1)&0x0f) {
case 1: printf("POSITION outputs selected\n"); break;
case 2: printf("ANGLE outputs selected\n"); break;
case 3: printf("MATRIX outputs selected\n"); break;
case 4: printf("POSITION/ANGLE outputs selected\n"); break;
case 5: printf("POSITION/MATRIX outputs selected\n"); break;
case 6: printf("Output is set to factory mode!\n"); break;
case 7: printf("QUATERNION outputs selected\n"); break;
case 8: printf("POSITION/QUATERNION outputs selected\n"); break;
default:
printf("Output state is invalid (= %d)\n", (((unsigned)wds[0])>>1)&0x0f);
}
printf("Bird is in %s mode\n", (wds[0] & 0x1) ? "stream" : "point");
*/
}
int main(int argc, char **argv) {
if (argc != 3) {
fprintf(stderr, "Usage: %s <SERVER IP> <SERVER PORT>\n", argv[0]);
return 1;
}
int sockFd;
if ((sockFd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
perror("socket() error");
struct sockaddr_in servAddr;
bzero(&servAddr, sizeof(servAddr));
servAddr.sin_family = AF_INET;
servAddr.sin_port = htons((uint16_t) strtoul(argv[2], NULL, 0));
if (inet_pton(AF_INET, argv[1], &servAddr.sin_addr) <= 0) {
struct hostent *host;
if ((host = gethostbyname(argv[1])) != NULL) {
struct in_addr **addrs = (struct in_addr **) host->h_addr_list;
if (addrs[0] != NULL)
servAddr.sin_addr = *addrs[0];
}
}
if (connect(sockFd, (const struct sockaddr *) &servAddr, sizeof(servAddr)) < 0) {
perror("connect() error");
return 1;
}
char buf[2048];
ssize_t len;
if ((len = forceread(sockFd, buf, 2048)) < 0)
perror("read() error");
else if (len > 0)
fputs(buf, stdout);
bool stdinEof = false;
while (1) {
fd_set readFds;
FD_ZERO(&readFds);
if (!stdinEof)
FD_SET(STDIN_FILENO, &readFds);
FD_SET(sockFd, &readFds);
if (select(sockFd + 1, &readFds, NULL, NULL, NULL) < 0)
perror("select() error");
if (FD_ISSET(STDIN_FILENO, &readFds)) {
fgets(buf, 2048, stdin);
std::string trimmed(buf);
trim(trimmed);
if (trimmed == "exit") {
if (shutdown(sockFd, SHUT_WR) < 0)
perror("shutdown() error");
stdinEof = true;
}
else if (forcewrite(sockFd, buf, strlen(buf) + 1) < 0)
perror("write() error");
}
if (FD_ISSET(sockFd, &readFds)) {
if ((len = forceread(sockFd, buf, 2048)) < 0)
perror("read() error");
else if (len == 0) {
if (!stdinEof)
fprintf(stderr, "server disconnected\n");
break;
}
else
fputs(buf, stdout);
}
}
if (close(sockFd) < 0)
perror("close() error");
return 0;
}
