//Polls all given sensors in the order specified.
void Sensor::PollSensors(Sensor *sens, const sC::sensorNumber *order, const byte OrderLength){
if (_sensorInitComplete == false) {
ERROR_PRINTLN("Please call initSensors() before attempting to poll");
return;
}
sC::sensorNumber currentSensorIndex = sC::FR;
for (byte n = 0; n < OrderLength; n++) {
currentSensorIndex = order[n];
SelectSensor(static_cast<byte>(currentSensorIndex));
SLASTVAL(currentSensorIndex, RVAL(currentSensorIndex));
sens[currentSensorIndex].GetReading();
SVAL(currentSensorIndex, sens[currentSensorIndex].tileWhite);
}
printbw();
}
void echotester (int sock)
{
setcntl(sock, F_SETFL, O_NONBLOCK, "O_NONBLOCK");
long long sent = 0, recvd = 0, recvdnow = 0;
int ptrsend = 0;
int ptrrecv = 0;
struct pollfd pollfd = { .fd = sock, .events = POLLIN | POLLOUT, };
struct timeval tb, ti, te;
gettimeofday(&tb, NULL);
ti = tb;
while (1)
{
int ret = poll(&pollfd, 1, 1000 /*ms*/);
if (ret == -1)
{
perror("poll");
exit(1);
}
if (pollfd.revents & POLLIN)
{
ssize_t ret = read(sock, bufin, BUFLEN);
if (ret == -1)
{
perror("read");
exit(1);
}
size_t pr = 0;
while (ret)
{
ssize_t size = ret;
if (size > BUFLEN - ptrrecv)
size = BUFLEN - ptrrecv;
if (memcmp(bufin + pr, bufout + ptrrecv, size) != 0)
{
fprintf(stderr, "\ndata differ (sent=%Li revcd=%Li ptrsend=%i ptrrecv=%i ret=%i size=%i)\n", sent, recvd, ptrsend, ptrrecv, (int)ret, (int)size);
int i = 0;
for (i = 0; i < size; i++)
if (bufin[i + pr] != bufout[i + ptrrecv])
{
printf("offset-diff @%Li @0x%Lx\n", i + recvd, i + recvd);
break;
}
int j = i > 16? i - 16: 0;
int k = i + 16 < size? i + 16: size - 1;
while (j++ < k)
printf("@%Lx:R%02x/S%02x ", j + recvd, bufin[j + pr], bufout[j + ptrrecv]);
printf("\n");
exit(1);
}
recvd += size;
recvdnow += size;
ptrrecv = (ptrrecv + size) & (BUFLEN - 1);
ret -= size;
pr += size;
}
}
if (pollfd.revents & POLLOUT)
{
ssize_t size = BUFLEN - ptrsend;
ssize_t ret = write(sock, bufout + ptrsend, size);
if (ret == -1)
{
perror("write");
exit(1);
}
sent += ret;
ptrsend = (ptrsend + ret) & (BUFLEN - 1);
}
gettimeofday(&te, NULL);
if (te.tv_sec - ti.tv_sec > 1)
{
printbw(te.tv_sec - tb.tv_sec, te.tv_usec - tb.tv_usec, recvd, "avg:");
printbw(te.tv_sec - ti.tv_sec, te.tv_usec - ti.tv_usec, recvdnow, "now:");
printsz(recvd, "size:");
printf("-----\r"); fflush(stdout);
ti = te;
recvdnow = 0;
}
}
my_close(sock);
}
void echoserver (int sock)
{
setcntl(sock, F_SETFL, O_NONBLOCK, "O_NONBLOCK");
int ptrsend = 0;
int ptrrecv = 0;
size_t inbuf = 0;
struct pollfd pollfd = { .fd = sock, .events = POLLIN | POLLOUT, };
while (1)
{
pollfd.events = 0;
if (inbuf < BUFLEN) pollfd.events |= POLLIN;
if (inbuf) pollfd.events |= POLLOUT;
int ret = poll(&pollfd, 1, 1000 /*ms*/);
if (ret == -1)
{
perror("poll");
exit(1);
}
if (pollfd.revents & POLLIN)
{
ssize_t maxrecv = BUFLEN - inbuf;
if (maxrecv > BUFLEN - ptrrecv)
maxrecv = BUFLEN - ptrrecv;
ssize_t ret = read(sock, bufin + ptrrecv, maxrecv);
if (ret == -1)
{
perror("read");
exit(1);
}
inbuf += ret;
ptrrecv = (ptrrecv + ret) & (BUFLEN - 1);
}
if (pollfd.revents & POLLOUT)
{
ssize_t maxsend = inbuf;
if (maxsend > BUFLEN - ptrsend)
maxsend = BUFLEN - ptrsend;
ssize_t ret = write(sock, bufin + ptrsend, maxsend);
if (ret == -1)
{
perror("write");
exit(1);
}
inbuf -= ret;
ptrsend = (ptrsend + ret) & (BUFLEN - 1);
}
}
my_close(sock);
}
int main (int argc, char* argv[])
{
int op;
const char* host = "localhost";
int port = 10102;
int server = 0;
int i;
//int nodelay = 0;
while ((op = getopt(argc, argv, "hp:d:fs")) != EOF) switch(op)
{
case 'h':
help();
return 0;
case 'p':
port = atoi(optarg);
break;
case 'd':
host = optarg;
break;
// case 'f':
// nodelay = 1;
// break;
case 's':
server = 1;
break;
default:
printf("option '%c' not recognized\n", op);
help();
return 1;
}
for (i = 0; i < BUFLEN; i++)
{
#if 1
bufout[i] = random() >> 23;
#else
char c = i & 0x0f;
c += c > 9? 'a' - 10: '0';
bufout[i] = c;
#endif
}
int sock = my_socket();
if (server)
{
printf("waiting on port %i\n", port);
my_bind_listen(sock, port);
int clisock = my_accept(sock);
echoserver(clisock);
close(sock);
}
else
{
printf("remote host: %s\n"
"port: %i\n",
host, port);
my_connect(host, port, sock);
echotester(sock);
}
return 0;
}
