int main(){ uint8_t start_r, old_IPL; uint8_t hz50_scaler, hz5_scaler, hz1_scaler, sec; uint32_t tick = 0; hz50_scaler = hz5_scaler = hz1_scaler = sec = 0; touch_init(); __delay_ms(200); lcd_initialize(); // Initialize the LCD motor_init(); lcd_clear(); lcd_locate(0,0); lcd_printf("-- Ball position: --"); timers_initialize(); // Initialize timers while (1) { start_r = 0; while(!start_r) { // disable all maskable interrupts SET_AND_SAVE_CPU_IPL(old_IPL, 7); start_r = start; // enable all maskable interrupts RESTORE_CPU_IPL(old_IPL); } // Periodic real-time task code starts here = CENTER_X + RADIUS * cos(tick * SPEED); // Ypos_set = CENT!!! double pidX, pidY; uint16_t duty_us_x, duty_us_y; // 50Hz control task if(hz50_scaler == 0) { calcQEI(Xpos_set, Xpos, Ypos_set, Ypos); // Xpos_set = CENTER_X; // Xpos_set = CENTER_Y; Xpos_set = CENTER_X + RADIUS * cos(tick * SPEED); Ypos_set = CENTER_Y + RADIUS * sin(tick * SPEED); tick++; pidX = pidX_controller(Xpos); pidY = pidY_controller(Ypos); // TODO: Convert PID to motor duty cycle (900-2100 us) // setMotorDuty is a wrapper function that calls your motor_set_duty // implementation in flexmotor.c. The 2nd parameter expects a value // between 900-2100 us // duty_us_x = cap((pidX*1000.0), 2100, 900); // duty_us_y = cap((pidY*1000.0), 2100, 900); duty_us_x = cap((pidX + 1500), 2100, 900); duty_us_y = cap((pidY + 1500), 2100, 900); motor_set_duty(1, duty_us_x); motor_set_duty(2, duty_us_y+100); // setMotorDuty(MOTOR_X_CHAN, duty_us_x); // setMotorDuty(MOTOR_Y_CHAN, duty_us_y); } // 5Hz display task if(hz5_scaler == 0) { // lcd_locate(0,1); // lcd_printf("Xp=%.1f,Yp=%.1f", Xpos, Ypos); // lcd_locate(0,2); // lcd_printf("X*=%.1f, Y*=%.1f", Xpos_set, Ypos_set); // lcd_locate(0,3); // lcd_printf("pX=%.1f,pY=%.1f", pidX, pidY); // lcd_locate(0,4); // lcd_printf("dx=%u, dY=%u", duty_us_x, duty_us_y); // if(deadline_miss >= 1) { lcd_locate(0,6); lcd_printf("%4d d_misses!!!", deadline_miss); } } // 1Hz seconds display task if(hz1_scaler == 0) { lcd_locate(0,7); lcd_printf("QEI: %5u", getQEI()); sec++; } hz50_scaler = (hz50_scaler + 1) % 2; hz5_scaler = (hz5_scaler + 1) % 20; hz1_scaler = (hz1_scaler + 1) % 100; start = 0; } return 0; }
int main(int ac, char *av[]) { //TODO extract the serial port codes into another source file, this is too ugly! uint16_t i, j, len; int16_t res=0, result=0; uint8_t destination_ip[32]; uint8_t outbuf[512]; uint8_t linebuf[32]; uint8_t local_filename[32]="\0"; FILE* sptr; if(ac == 1) usage(); //for(i=0; i<ac; i++) // printf("%s\n",av[i]); if(!strcmp("uhf", av[1])) { mode=RADIOTFTP_MODE_UHF; preamble_length=RADIOTFTP_BIM2A_PREAMBLE_LENGTH; baud=RADIOTFTP_BIM2A_BAUD_RATE; printf("Running with UHF band 19200 baud version\n"); openLogFile(RADIOTFTP_BIM2A_EVENTLOG); } else if(!strcmp("vhf", av[1])) { mode=RADIOTFTP_MODE_VHF; preamble_length=RADIOTFTP_UHX1_PREAMBLE_LENGTH; baud=RADIOTFTP_UHX1_BAUD_RATE; printf("Running with VHF band 2400 baud version\n"); openLogFile(RADIOTFTP_UHX1_EVENTLOG); } else if(!strcmp("serial", av[1])) { mode=RADIOTFTP_MODE_SERIAL; preamble_length=RADIOTFTP_SERIAL_PREAMBLE_LENGTH; baud=RADIOTFTP_SERIAL_BAUD_RATE; printf("Couldn't understand the uhf/vhf mode: using %d mode\n", mode); printf("Running with NO BAND %d baud version\n", baud); printf("Opening file %s for logging\n",RADIOTFTP_SERIAL_EVENTLOG); openLogFile(RADIOTFTP_SERIAL_EVENTLOG); } else { printf("Mode not entered, exiting...\n"); return 0; } //setting defaults udp_get_broadcast_ip(destination_ip); strcpy(dial_tty, "/dev/ttyUSB0"); //scanning command line parameters for(i=2; (i < ac) && (av[i][0] == '-'); i++) { if(strcmp(av[i], "-b") == 0) { background=1; } else if(strncmp(av[i], "-f", 2) == 0) { strncpy(local_filename, av[i] + 2, 32); printf("different filename = '%s'\n", local_filename); } else if(strncmp(av[i], "-dst", 4) == 0) { memset(destination_ip, 0, 32); memcpy(destination_ip, av[i] + 4, strlen(av[i]) - 4); /*! convert text ip to numerical */ text_to_ip(destination_ip, strlen(av[i]) - 4 + 1); printf("Destination: "); print_addr_dec(destination_ip); } else { printf("unknown parameter: %s\n", av[i]); usage(); } } res=0; result=0; strncpy(dial_tty, devtag_get(av[i]), sizeof(dial_tty)); while(!get_lock(dial_tty)) { if(decrementLockRetries() == 0) exit(-1); sleep(1); } if((serialportFd=open(devtag_get(av[i]), O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0) { perror("bad terminal device, try another"); exit(-1); } signal(SIGIO, SIG_IGN); signal(SIGINT, sigINT_handler); // Make the file descriptor asynchronous (the manual page says only // O_APPEND and O_NONBLOCK, will work with F_SETFL...) fcntl(serialportFd, F_GETFL); fcntl(serialportFd, F_SETFL, O_RDWR | O_SYNC); if(tcgetattr(serialportFd, &tp) < 0) { perror("Couldn't get term attributes"); exit(-1); } old=tp; /* SANE is a composite flag that sets the following parameters from termio(M): CREAD BRKINT IGNPAR ISTRIP ICRNL IXON ISIG ICANON ECHO ECHOK OPOST ONLCR SANE also clears the following modes: CLOCAL IGNBRK PARMRK INPCK INLCR IUCLC IXOFF XCASE ECHOE ECHONL NOFLSH OLCUC OCRNL ONOCR ONLRET OFILL OFDEL NLDLY CRDLY TABDLY BSDLY VTDLY FFDLY */ /* 8 bits + baud rate + local control */ tp.c_cflag=baud | CS8 | CLOCAL | CREAD; tp.c_cflag&=~PARENB; tp.c_cflag&=~CSTOPB; tp.c_cflag&=~CSIZE; tp.c_cflag|=CS8; tp.c_cflag|=CRTSCTS; tp.c_oflag=0; /* Raw Input */ tp.c_lflag=0; /* No conoical */ tp.c_cc[VTIME]=0; tp.c_cc[VMIN]=1; /* ignore CR, ignore parity */ //ISTRIP is a dangerous flag, it strips the 8th bit of bytes //tp.c_iflag= ~( ISTRIP ) | IGNPAR | IGNBRK; tp.c_iflag=~(IGNBRK | PARMRK | INPCK | ISTRIP | INLCR | IUCLC | IXOFF) | BRKINT | IGNPAR | ICRNL | IXON | ISIG | ICANON; /* set output and input baud rates */ cfsetospeed(&tp, baud); cfsetispeed(&tp, baud); tcflush(serialportFd, TCIFLUSH); tcflush(serialportFd, TCOFLUSH); if(tcsetattr(serialportFd, TCSANOW, &tp) < 0) { perror("Couldn't set term attributes"); goto error; } restore=1; //TODO go over the background codes if(background) { int i; if(getppid() == 1) return 0; /* Already a daemon */ i=fork(); if(i < 0) exit(1); /* error */ if(i > 0) _exit(0); /* parent exits */ /* child */ setsid(); /* obtain a new process group */ for(i=getdtablesize(); i >= 0; --i) { if(i == serialportFd) continue; if(i == 1) continue; close(i); /* close all descriptors */ } i=open("/dev/null", O_RDWR); dup(i); dup(i); /* handle standard I/O */ umask(027); /* set newly created file permissions */ chdir("/"); /* change running directory */ } srand((unsigned) time(NULL)); j=0; i++; for(; i < ac; i++) { len=strlen(av[i]); memcpy(command_buffer + j, av[i], len); j+=len; if(i != ac - 1) { command_buffer[j++]=' '; } } //process the escape characters for(i=0; i < j; i++) { if(command_buffer[i] == '\\') { if(i + 1 < j) { if(command_buffer[i + 1] == 'n') { command_buffer[i]='\r'; command_buffer[i + 1]='\n'; } } } } timers_initialize(&sigRTALRM_handler); /*! read settings from radiotftp.conf file */ sptr=fopen("radiotftp.conf", "r"); if(sptr != NULL) { readnline(sptr, linebuf, 32); linebuf[6]=atoi(linebuf + 6); linebuf[8]=0; printf("AX.25 Callsign: "); print_callsign(linebuf); #if AX25_ENABLED==1 ax25_initialize_network(linebuf); printf("USING AX25 LINK LAYER!!!\n"); #else AX25_ENABLED==0 printf("NOT USING ANY LINK LAYER!!!\n"); #endif readnline(sptr, linebuf, 32); text_to_ip(linebuf, strlen(linebuf)); printf("IPv4 Address: "); print_addr_dec(linebuf); udp_initialize_ip_network(linebuf, &queueSerialData); } else { #if AX25_ENABLED==1 ax25_initialize_network(my_ax25_callsign); printf("AX.25 CALLSIGN = "); print_callsign(ax25_get_local_callsign(NULL)); #endif udp_initialize_ip_network(my_ip_address, &queueSerialData); printf("IPv4 Address = "); print_addr_dec(udp_get_localhost_ip(NULL)); } switch(mode) { case RADIOTFTP_MODE_UHF: tftp_initialize(udp_get_data_queuer_fptr(), RADIOTFTP_BIM2A_ACK_TIMEOUT_MIN, RADIOTFTP_BIM2A_ACK_TIMEOUT_MAX, RADIOTFTP_BIM2A_READ_TIMEOUT); break; case RADIOTFTP_MODE_VHF: tftp_initialize(udp_get_data_queuer_fptr(), RADIOTFTP_UHX1_ACK_TIMEOUT_MIN, RADIOTFTP_UHX1_ACK_TIMEOUT_MAX, RADIOTFTP_UHX1_READ_TIMEOUT); break; case RADIOTFTP_MODE_SERIAL: tftp_initialize(udp_get_data_queuer_fptr(), RADIOTFTP_SERIAL_ACK_TIMEOUT_MIN, RADIOTFTP_SERIAL_ACK_TIMEOUT_MAX, RADIOTFTP_SERIAL_READ_TIMEOUT); break; default: perror("unknown baud rate"); safe_exit(-1); break; } if(!strncasecmp(RADIOTFTP_COMMAND_PUT, command_buffer, strlen(RADIOTFTP_COMMAND_PUT))) { printf(RADIOTFTP_COMMAND_PUT"\n"); registerEvent(RADIOTFTP_COMMAND_PUT, command_buffer + strlen(RADIOTFTP_COMMAND_PUT) + 1); if((res=tftp_sendRequest(TFTP_OPCODE_WRQ, destination_ip, local_filename, command_buffer + strlen(RADIOTFTP_COMMAND_PUT) + 1, j - strlen(RADIOTFTP_COMMAND_PUT) - 1, 0))) { printf("%d\n", res); perror("tftp request fail"); goto error; } } else if(!strncasecmp(RADIOTFTP_COMMAND_BEACON, command_buffer, strlen(RADIOTFTP_COMMAND_BEACON))) { printf(RADIOTFTP_COMMAND_BEACON"\n"); registerEvent(RADIOTFTP_COMMAND_BEACON, command_buffer + strlen(RADIOTFTP_COMMAND_BEACON) + 1); if((res=tftp_sendSingleBlockData(destination_ip, command_buffer + strlen(RADIOTFTP_COMMAND_BEACON) + 1, j - strlen(RADIOTFTP_COMMAND_BEACON) - 1, local_filename))) { printf("%d\n", res); perror("tftp request fail"); goto error; } } else if(!strncasecmp(RADIOTFTP_COMMAND_APPEND_LINE, command_buffer, strlen(RADIOTFTP_COMMAND_APPEND_LINE))) { printf(RADIOTFTP_COMMAND_APPEND_LINE"\n"); registerEvent(RADIOTFTP_COMMAND_APPEND_LINE, command_buffer + strlen(RADIOTFTP_COMMAND_APPEND_LINE) + 1); i=createTempFile(TEMPFILE_PREFIX, TEMPFILE_POSTFIX); if(tempFile == NULL) { perror("couldn't create temp file to store the line feed"); goto error; } fwrite(command_buffer + strlen(RADIOTFTP_COMMAND_APPEND_LINE) + 1, 1, j - strlen(RADIOTFTP_COMMAND_APPEND_LINE) - 1, tempFile); fflush(tempFile); fclose(tempFile); printf("tempfileName=%s\nlocal_filename=%s\n", tempFileName, local_filename); if((res=tftp_sendRequest(TFTP_OPCODE_WRQ, destination_ip, tempFileName, local_filename, strlen(local_filename), 1))) { printf("%d\n", res); perror("tftp request fail"); goto error; } } else if(!strncasecmp(RADIOTFTP_COMMAND_APPEND_FILE, command_buffer, strlen(RADIOTFTP_COMMAND_APPEND_FILE))) { printf(RADIOTFTP_COMMAND_APPEND_FILE"\n"); registerEvent(RADIOTFTP_COMMAND_APPEND_FILE, command_buffer + strlen(RADIOTFTP_COMMAND_APPEND_FILE) + 1); if((res=tftp_sendRequest(TFTP_OPCODE_WRQ, destination_ip, local_filename, command_buffer + strlen(RADIOTFTP_COMMAND_APPEND_FILE) + 1, j - strlen(RADIOTFTP_COMMAND_APPEND_FILE) - 1, 1))) { printf("%d\n", res); perror("tftp request fail"); goto error; } } else if(!strncasecmp(RADIOTFTP_COMMAND_GET, command_buffer, strlen(RADIOTFTP_COMMAND_GET))) { printf(RADIOTFTP_COMMAND_GET"\n"); registerEvent(RADIOTFTP_COMMAND_GET, command_buffer + strlen(RADIOTFTP_COMMAND_GET) + 1); if((res=tftp_sendRequest(TFTP_OPCODE_RRQ, destination_ip, local_filename, command_buffer + strlen(RADIOTFTP_COMMAND_GET) + 1, j - strlen(RADIOTFTP_COMMAND_GET) - 1, 0))) { printf("%d\n", res); perror("tftp request fail"); goto error; } } else { printf("hello radio world!\n"); registerEvent("helloWorld", ""); if((res=queueSerialData(udp_get_localhost_ip(NULL), HELLO_WORLD_PORT, udp_get_broadcast_ip(NULL), HELLO_WORLD_PORT, "hello world\n\0x00", strlen("hello world\n\0x00")))) { printf("%d\n", res); perror("tftp request fail"); goto error; } } int sync_counter=0; int sync_passed=0; int save_index=0; //entering the main while loop printf("started listening...\n"); while(1) { if(timer_flag) { idle_timer_handler(); tftp_timer_handler(); timer_flag=0; } if(idle_flag) { //print_time("System Idle"); idle_flag=0; } if(queue_flag) { // printf("queue flag\n"); if(!sync_passed && sync_counter < 1) { //wait_some switch(mode) { case RADIOTFTP_MODE_UHF: usleep(RADIOTFTP_BIM2A_TX_MESSAGE_INTERVAL); break; case RADIOTFTP_MODE_VHF: usleep(RADIOTFTP_UHX1_TX_MESSAGE_INTERVAL); break; case RADIOTFTP_MODE_SERIAL: usleep(RADIOTFTP_SERIAL_TX_MESSAGE_INTERVAL); break; default: perror("unknown baud rate"); safe_exit(-1); break; } transmitSerialData(); queue_flag=0; } } if((res=read(serialportFd, io, IO_BUFSIZE)) > 0) { io_index=0; //printf("# of bytes read = %d\n", res); for(i=0; i < res; i++) { //putchar(io[i]); //printf("%\n",io[i]); if(sync_counter < SYNC_LENGTH && io[i] == syncword[sync_counter]) { sync_counter++; /* sync continued */ //printf("sync counting %d\n",sync_counter); } else { //printf("sync reset %d\n",sync_counter); sync_counter=0; /* not a preamble, reset counter */ } if(sync_counter >= SYNC_LENGTH && sync_passed == 0) { /* preamble passed */ sync_passed=1; } if(sync_passed) { //printf("getting data '%c'\n", io[i]); if(io[i] == END_OF_FILE && !isManchester_encoded(io[i])) { // printf("non-manchester character received\n"); outbuf[0]=0; result=manchester_decode(buf + 1, manchester_buffer, save_index); #if AX25_ENABLED==1 result=ax25_open_ui_packet(NULL, NULL, ax25_buffer, manchester_buffer, result); #else result=1; #endif if(result) { // printf("opened link-layer packet\n"); #if AX25_ENABLED==1 result=udp_open_packet(udp_src, &udp_src_prt, udp_dst, &udp_dst_prt, udp_buffer, ax25_buffer); #else result = udp_open_packet(udp_src, &udp_src_prt, udp_dst, &udp_dst_prt, udp_buffer, manchester_buffer); #endif if(result) { // printf("opened transport-layer packet\n"); udp_packet_demultiplexer(udp_src, udp_src_prt, udp_dst, udp_dst_prt, udp_buffer, result); } else { strcat(outbuf, "!udp discarded!"); if(write(1, outbuf, strlen(outbuf)) <= 0) { fputs("couldn't write to tty\n", stderr); } if(write(1, "\n", 1) <= 0) { fputs("couldn't write to tty\n", stderr); } } } else { strcat(outbuf, "!ax25 discarded!"); if(write(1, outbuf, strlen(outbuf)) <= 0) { fputs("couldn't write to tty\n", stderr); } if(write(1, "\n", 1) <= 0) { fputs("couldn't write to tty\n", stderr); } } sync_passed=0; sync_counter=0; save_index=0; } else { // printf("saved data '%c'\n", io[i]); // printf("save_index=%d/%d\n",save_index, sizeof(buf)); if(save_index >= sizeof(buf)) { sync_passed=0; sync_counter=0; save_index=0; } buf[save_index++]=io[i]; buf[save_index + 1]=0; // printf("-\n%s\n-\n", buf); } } } io_flag=0; } } safe_exit(0); error: safe_exit(-1); return -2; }
int main(int argc, char **argv) { (void)argc; (void)argv; tor_libevent_cfg cfg; memset(&cfg, 0, sizeof(cfg)); tor_libevent_initialize(&cfg); timers_initialize(); int i; int ret; struct timeval now; tor_gettimeofday(&now); monotime_get(&started_at); for (i = 0; i < N_TIMERS; ++i) { struct timeval delay; delay.tv_sec = crypto_rand_int_range(0,MAX_DURATION); delay.tv_usec = crypto_rand_int_range(0,1000000); delay_usec[i] = delay.tv_sec * 1000000 + delay.tv_usec; timeradd(&now, &delay, &fire_at[i]); timers[i] = timer_new(timer_cb, &timers[i]); timer_schedule(timers[i], &delay); ++n_active_timers; } /* Disable some; we'll make sure they don't trigger. */ for (i = 0; i < N_DISABLE; ++i) { int idx = crypto_rand_int_range(0, N_TIMERS); if (is_disabled[idx]) continue; is_disabled[idx] = 1; timer_disable(timers[idx]); --n_active_timers; } tor_libevent_run_event_loop(tor_libevent_get_base(), 0); int64_t total_difference = 0; uint64_t total_square_difference = 0; tor_assert(n_fired == n_active_timers); for (i = 0; i < N_TIMERS; ++i) { if (is_disabled[i]) { tor_assert(fired[i] == 0); continue; } tor_assert(fired[i] == 1); //int64_t diff = difference[i].tv_usec + difference[i].tv_sec * 1000000; int64_t diff = diffs_mono_usec[i]; total_difference += diff; total_square_difference += diff*diff; } const int64_t mean_diff = total_difference / n_active_timers; printf("mean difference: "I64_FORMAT" usec\n", I64_PRINTF_ARG(mean_diff)); const double mean_sq = ((double)total_square_difference)/ n_active_timers; const double sq_mean = mean_diff * mean_diff; const double stddev = sqrt(mean_sq - sq_mean); printf("standard deviation: %lf usec\n", stddev); #define MAX_DIFF_USEC (500*1000) #define MAX_STDDEV_USEC (500*1000) #define ODD_DIFF_USEC (2000) #define ODD_STDDEV_USEC (2000) if (mean_diff < 0 || mean_diff > MAX_DIFF_USEC || stddev > MAX_STDDEV_USEC) { printf("Either your system is under ridiculous load, or the " "timer backend is broken.\n"); ret = 1; } else if (mean_diff > ODD_DIFF_USEC || stddev > ODD_STDDEV_USEC) { printf("Either your system is a bit slow or the " "timer backend is odd.\n"); ret = 0; } else { printf("Looks good enough.\n"); ret = 0; } timer_free_(NULL); for (i = 0; i < N_TIMERS; ++i) { timer_free(timers[i]); } timers_shutdown(); return ret; }
bool server_game_start(char *game_script_name,int skill,network_reply_join_remotes *remotes,char *err_str) { int n; network_request_object_add *obj_add; // initialize lists model_initialize(); object_initialize_list(); weapon_initialize_list(); proj_setup_initialize_list(); scripts_initialize(); script_globals_initialize(); timers_initialize(); // setup skill level server.skill=skill; // run game script map.info.name[0]=0x0; map.info.player_start_name[0]=0x0; map.info.player_start_type[0]=0x0; map.info.in_load=FALSE; server.player_obj_uid=-1; js.game_attach.thing_type=thing_type_game; js.game_attach.thing_uid=-1; scripts_clear_attach_data(&js.game_attach); if (!scripts_add(&js.game_attach,"Game",game_script_name,NULL,err_str)) return(FALSE); // editor map override? if (setup.editor_override.on) { strcpy(map.info.name,setup.editor_override.map); } // can't start a game without a map if (map.info.name[0]==0x0) { strcpy(err_str,"Game: No start map specified in game script"); return(FALSE); } // prepare for any script based spawns object_script_spawn_start(); // put in additional objects (remotes, bots) if (remotes!=NULL) { obj_add=remotes->objects; for (n=0;n!=remotes->count;n++) { remote_add(obj_add,FALSE); obj_add++; } } // start player object server.player_obj_uid=object_start(NULL,TRUE,bt_game,-1,err_str); if (server.player_obj_uid==-1) { scripts_dispose(js.game_attach.script_uid); return(FALSE); } // force player to auto-spawn // spawing of player needs to happen before map_start events object_spawn(object_find_uid(server.player_obj_uid)); // finish any script based spawns object_script_spawn_finish(); return(TRUE); }