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; } }
void process_msg_l(struct local_msg *m) { switch (m->v) { case 0: break; case 1: autoconfig = 1; reset_avr(); break; case 2: autoconfig = 0; reset_avr(); break; case 3: //get number of SPI errors struct local_msg lm; lm.c = 0; lm.t = 253; lm.v=spi_crc_err; local_buf[local_buf_c++] = lm; printf("%u %i\n",lm.t,lm.v); break; case 4: flog_save(); break; default: printf("Unknown local message: %u\n",m->v); } }
/* Specific callback after data packet received */ void network_specific_DataIndicationcallback(APS_DataInd_t* indData) { AppMessage_t *FrameReceived = (AppMessage_t *) indData->asdu; /* Data received indication */ switch(FrameReceived->type) { case NETWORK_RESET: reset_avr(); break; case NETWORK_UART_OVER_ZB: print_raw_data(FrameReceived->data,indData->asduLength-1); break; default: uprintf("Unknown data type received = %x\r\n",FrameReceived->type); break; } }
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); }