void do_avr_init() {
if (avrstatus>=5) return;
if (autoconfig==0) return;
static int prev_status = -1;
static long dt_ms;
struct timespec *dt;
dt = TimeSpecDiff(&time_now,&reset_time_prev);
dt_ms = dt->tv_sec*1000 + dt->tv_nsec/1000000;
if (dt_ms>reset_timeout) { //we exceeded timeout waiting for a status change
reset_avr();
prev_status = -1;
return;
}
if (prev_status == avrstatus) return;
prev_status = avrstatus;
reset_time_prev = time_now;
if (verbose) printf("AVR status: %i\n",avrstatus);
switch(avrstatus) {
case -1: break;
case 0: reset_avr(); prev_status = -1; break; //AVR should boot into status 1 so 0 means something wrong
case 1: sendConfig(); prev_status = avrstatus = -1; reset_timeout=initTimeout; break;
case 2: break; //AVR should arm motors and set status to 3
case 3: break; //AVR is initializing MPU
case 4: reset_timeout=20000; break; //AVR is calibration gyro
case 5: if (verbose) printf("Initialization OK.\n"); break;
case 255: printf("AVRCONFIG: Gyro calibration failed!\n"); reset_avr(); break; //calibration failed
default: printf("AVRCONFIG: Unknown AVR status %i\n",avrstatus); break;
}
}
static void CalibrateTicks() {
struct timespec begints, endts;
uint64_t begin = 0, end = 0;
clock_gettime(CLOCK_MONOTONIC, &begints);
begin = RDTSC();
for( ; i < 300000000 ; i++) {
j = (j * 1.12345) / 1.054321;
}
end = RDTSC();
clock_gettime(CLOCK_MONOTONIC, &endts);
struct timespec *tmpts = TimeSpecDiff(&endts, &begints);
uint64_t nsecElapsed = tmpts->tv_sec * 1000000000LL + tmpts->tv_nsec;
g_TicksPerNanoSec = (double)(end - begin)/(double)nsecElapsed;
printf( "calibrateTicks timeMS=%d, ticksPerNanoSec=%f\n", (nsecElapsed/1000000), g_TicksPerNanoSec );
}
int main(int argc, char **argv)
{
/* SELECT */
int max_fd;
fd_set readfds;
struct timeval timeout;
/* END SELECT */
int i,j,ret;
unsigned char bufout[BUF_SIZE];
struct avr_msg dummy_msg = {t: 255, v: 254};
struct avr_msg status_msg = {t: 255, v: 0};
long dt_ms = 0;
clock_gettime(CLOCK_REALTIME, &time_now);
clock_gettime(CLOCK_REALTIME, &spi_time_prev);
//we use this to measure UDP connection time each second
clock_gettime(CLOCK_REALTIME, &udp_time_prev);
int option;
verbose = 1;
background = 0;
echo = 0;
while ((option = getopt(argc, argv,"dep:fv:u:y:")) != -1) {
switch (option) {
case 'd': background = 1; verbose=0; break;
case 'p': portno = atoi(optarg); break;
case 'v': verbose = atoi(optarg); break;
case 'y': initTimeout = atoi(optarg); break;
case 'f': spi=0; break;
case 'e': echo=1; break;
case 'u': strcpy(socket_path,optarg); break;
default:
print_usage();
return -1;
}
}
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
unix_sock = 0;
for (i=0; i<MAX_UNIX_CLIENTS; i++) {
sock[i] = 0;
sock_type[i] = -1;
if (i>0) {
buf_c[i] = 0;
}
}
unix_sock = socket(AF_UNIX, SOCK_STREAM, 0);
if (unix_sock < 0) {
perror("opening unix socket");
exit(1);
}
usock = socket(AF_INET, SOCK_DGRAM, 0);
if (usock < 0) {
perror("opening datagram socket");
exit(1);
}
/* Create name sock, usock */
bzero((char *) &address, sizeof(address));
address.sin_family = AF_INET;
address.sin_addr.s_addr = INADDR_ANY;
address.sin_port = htons(portno);
if (bind(usock, (struct sockaddr *) &address, sizeof(struct sockaddr_in))) {
perror("binding datagram socket");
exit(1);
}
printf("Socket created on port %i\n", portno);
/* UNIX */
bzero((char *) &local, sizeof(local));
local.sun_family = AF_UNIX;
strcpy(local.sun_path, socket_path);
unlink(local.sun_path);
locallen = strlen(local.sun_path) + sizeof(local.sun_family);
if (bind(unix_sock, (struct sockaddr *) &local, locallen)) {
perror("binding unix socket");
exit(1);
}
flog_init(CFG_PATH);
log_mode = flog_getmode();
if (listen(unix_sock,3) < 0) {
perror("listen");
stop=1;
}
if (background) {
if (daemon(0,1) < 0) {
perror("daemon");
return -1;
}
if (verbose) printf("Running in the background\n");
}
if (spi) {
ret = spi_init();
if (ret < 0) {
printf("Error initiating SPI! %i\n",ret);
stop = 1;
}
}
reset_avr();
clock_gettime(CLOCK_REALTIME, &spi_last_msg);
if (verbose) printf("Starting main loop\n");
while (!stop) {
FD_ZERO(&readfds);
max_fd = unix_sock;
FD_SET(unix_sock, &readfds);
for (i=0;i<MAX_UNIX_CLIENTS;i++) {
if (sock[i]<=0) continue;
FD_SET(sock[i], &readfds);
max_fd = sock[i]>max_fd?sock[i]:max_fd;
}
FD_SET(usock, &readfds);
max_fd = usock>max_fd?usock:max_fd;
timeout.tv_sec = 0;
timeout.tv_usec = MSG_PERIOD*1000L; //ms
int sel = select( max_fd + 1 , &readfds , NULL , NULL , &timeout);
if ((sel<0) && (errno!=EINTR)) {
perror("select");
stop=1;
}
clock_gettime(CLOCK_REALTIME, &time_now);
//check for orphan UDP connections
dt = TimeSpecDiff(&time_now,&udp_time_prev);
dt_ms = dt->tv_sec*1000 + dt->tv_nsec/1000000;
if (dt_ms>1000) {
udp_time_prev = time_now;
for (i=0;i<MAX_UDP_CLIENTS;i++)
if (uclients[i]>0) {
uclients[i]--;
if (uclients[i]<=0) {
if (verbose) printf("Client %i timeout.\n",i);
}
}
}
//check UDP
if (!stop && FD_ISSET(usock, &readfds)) {
ret = 0;
int t = 0;
do {
t = recvfrom(usock, ubuf+ret, BUF_SIZE-ret, MSG_DONTWAIT, (struct sockaddr *)&tmpaddress, &addrlen);
if (t>0) ret+=t;
} while (t>0);
if (ret<=0) {
printf("UDP recvfrom error? %i\n",ret);
} else {
int msg_c = ret / LOCAL_MSG_SIZE;
if (verbose) printf("UDP received %i msgs\n", msg_c);
for (i=0;i<msg_c;i++) {
struct local_msg m;
unpack_lm(ubuf+i*LOCAL_MSG_SIZE,&m);
process_udp_msg(&m,&tmpaddress);
}
}
}
//If something happened on the master socket , then its an incoming connection
if (!stop && FD_ISSET(unix_sock, &readfds)) {
int t = accept(unix_sock, NULL, NULL);
if (t<0) {
perror("accept");
continue;
}
for (i=0;i<MAX_UNIX_CLIENTS;i++)
if (sock[i] == 0) {
if (verbose) printf("Incoming client: %i\n",i);
sock[i] = t;
sock_type[i] = -1;
buf_c[i] = 0;
break;
}
if (i==MAX_UNIX_CLIENTS) {
printf("AVRSPI: No space in connection pool! Disconnecting client.\n");
close(t);
}
}
for (i=0;(i<MAX_UNIX_CLIENTS) && (!stop) && sock[i]>0;i++) {
if (FD_ISSET(sock[i], &readfds)) {
if (sock_type[i]==-1) {
ret = read(sock[i],&sock_type[i],1);
if (ret<=0) {
perror("Reading error");
close(sock[i]);
sock[i] = 0;
}
continue;
}
ret = read(sock[i] , buf[i]+buf_c[i], BUF_SIZE - buf_c[i]);
if (ret < 0) {
perror("Reading error");
close(sock[i]);
sock[i] = 0;
}
else if (ret == 0) { //client disconnected
if (verbose) printf("Client %i disconnected.\n",i);
close(sock[i]);
sock[i] = 0;
buf_c[i] = 0;
} else { //got data
buf_c[i] += ret;
if (verbose) printf("Received: %i bytes. Buf size: %i\n",ret,buf_c[i]);
process_socket_queue(i);
}
}
}
//process of SPI received messages
for (i=0;i<spi_buf_c;i++) {
process_avr_msg(&spi_buf[i]);
avr2local(&spi_buf[i],&local_buf[local_buf_c++]);
}
spi_buf_c = 0;
//send out any available messages to clients
if (local_buf_c*LOCAL_MSG_SIZE>BUF_SIZE) {
printf("output buffer overflow (bufout)!");
local_buf_c = 0;
}
for (j=0;j<local_buf_c;j++) {
if (verbose>=2) printf("To clients: c: %u, t: %u, v: %i\n",local_buf[j].c,local_buf[j].t,local_buf[j].v);
//local_buf[j].c = 0;
pack_lm(bufout+j*LOCAL_MSG_SIZE,&local_buf[j]);
}
if (local_buf_c) {
if (verbose) printf("To clients msgs: %i bytes: %i\n",local_buf_c,local_buf_c*LOCAL_MSG_SIZE);
for (int k=0;k<MAX_UNIX_CLIENTS;k++) {
if (sock[k]!=0 && sock_type[k]==0) {
ret = send(sock[k], bufout, local_buf_c*LOCAL_MSG_SIZE, MSG_NOSIGNAL );
if (ret == -1) {
if (verbose) printf("Lost connection to client %i.\n",k);
close(sock[k]);
sock[k] = 0;
}
}
}
for (int k=0;k<MAX_UDP_CLIENTS;k++) {
if (uclients[k]>0) {
ret = sendto(usock,bufout,LOCAL_MSG_SIZE,0,(struct sockaddr *)&uaddress[k],addrlen);
if (ret<0) {
printf("Error sending datagram packet\n");
uclients[k] = 0;
}
}
}
}
local_buf_c = 0;
do_avr_init();
flog_loop();
//ping avr if needed
dt = TimeSpecDiff(&time_now,&spi_time_prev);
dt_ms = dt->tv_sec*1000 + dt->tv_nsec/1000000;
if (dt_ms>=MSG_PERIOD) {
spi_time_prev = time_now;
if (spi) for (i=msg_counter;i<MSG_RATE;i++)
if (i==msg_counter && autoconfig && avrstatus<5) spi_sendMsg(&status_msg);
else spi_sendMsg(&dummy_msg);
msg_counter = 0;
}
}
if (echo) spi_close();
bufout[0] = 1; //disconnect msg
for (int k=0;k<MAX_UNIX_CLIENTS;k++) {
if (sock[k]!=0)
send(sock[k], bufout, LOCAL_MSG_SIZE, MSG_NOSIGNAL );
}
close(unix_sock);
mssleep(1000);
if (verbose) {
printf("closing\n");
fflush(NULL);
}
for (i=0;i<MAX_UNIX_CLIENTS;i++)
if (sock[i]!=0) close(sock[i]);
close(usock);
}
