예제 #1
0
파일: main.c 프로젝트: ffavaro/uQuad2
int main(int argc, char *argv[])
{
#if !DISABLE_IMU
	#if SIMULATE_ALTITUDE
		#error No se puede simular altura con la IMU activada!
	#endif
#endif


   /// mensajes al usuario
#if PC_TEST
   printf("----------------------\n  WARN: Modo 'PC test' - Ver common/quadcop_config.h  \n----------------------\n");
   sleep_ms(500);
#endif
#if SIMULATE_GPS
   printf("----------------------\n  WARN: GPS simulado - Ver common/quadcop_config.h  \n----------------------\n");
   sleep_ms(500);
#endif
#if DISABLE_UAVTALK
   printf("----------------------\n  WARN: UAVTalk desabilitado - Ver common/quadcop_config.h  \n----------------------\n");
   sleep_ms(500);
#endif
#if DISABLE_IMU
   printf("----------------------\n  WARN: IMU desabilitada - Ver common/quadcop_config.h  \n----------------------\n");
   sleep_ms(500);
#endif

   /// Init socket comm
#if SOCKET_TEST
   socket_comm_init();
#endif

   int retval;
   
  
   int err_imu = 0;		//Aumenta si no hay datos nuevos de imu
   int err_count_no_data = 0; 	//si no tengo datos nuevos varias veces es peligroso
   int no_gps_data = 0;

   // Para log
   char* log_name;
   char buff_log[512]; 		//TODO determinar valor
   char buff_log_aux[512];	//TODO determinar valor
   int buff_log_len;

   bool first_time = true; // para saber cuando es la primer ejecucion del loop
   int8_t count_50 = 1; // controla tiempo de loop 100ms

   if(argc<3)
   {
	err_log("USAGE: ./auto_pilot log_name thrust_hovering");
	exit(1);
   }
   else
   {
	log_name = argv[1];
	thrust_hovering = atoi(argv[2]);
	if( thrust_hovering < 16 || thrust_hovering > 40) {
	   puts("Con este throttle dudo que hagas hovering, cerrando");
	   //exit(0);
	}
	printf("Throttle hovering: %u\n", (uint16_t) (thrust_hovering*17.41+1212.53) );
    }

    // Configurar pin de uart1 tx
#if !PC_TEST
   retval = system("echo 2 > /sys/kernel/debug/omap_mux/dss_data6");
   if (retval < 0)
   {
      err_log("system() failed - UART1 mux config");
      exit(0);
   }
#endif

   //setea senales y mascara
   set_signals();

   // Control de tiempos
   struct timeval tv_in_loop,
		  tv_out_loop, tv_out_last_loop,
                  tv_start_main,
                  tv_diff;
#if DEBUG
   struct timeval dt;
#endif

   // -- -- -- -- -- -- -- -- --
   // Inicializacion
   // -- -- -- -- -- -- -- -- --

   /// Tiempo
   set_main_start_time();
   tv_start_main = get_main_start_time(); //uquad_aux_time.h

#if SOCKET_TEST
   if (socket_comm_wait_client() == -1)
	exit(0);
#endif

   /// Path planning & Following
   // init lista path
   lista_path = (Lista_path *)malloc(sizeof(struct ListaIdentificar_path));
   inicializacion_path(lista_path);

   // init lista wp
   lista_way_point = (Lista_wp *)malloc(sizeof(struct ListaIdentificar_wp));
   inicializacion_wp(lista_way_point);
   retval = way_points_input(lista_way_point); //carga waypoints en la lista desde un archivo de texto
   if (retval < 0) {
	puts("No se pudo cargar lista de waypoints, cerrando");
	exit(0);
   }

   // Generacion de trayectoria
   path_planning(lista_way_point, lista_path);

   log_trayectoria(lista_path);    //dbg
   //visualizacion_path(lista_path); // dbg

#if SOCKET_TEST
   if (socket_comm_send_path(lista_path) == -1)
	exit(0);
#endif

   /// Control yaw
#if CONTROL_YAW_ADD_DERIVATIVE
   control_yaw_init_error_buff();
#endif

   /// Control velocidad
   // TODO

   // Se calcula el valor de pitch necesario para alcanzar la velocidad deseada.
   // El signo negativo es para que sea coherente con el sentido de giro de la cc3d (angulo positivo = giro horario)
   pitch = -atan(B_ROZ*VEL_DESIRED/MASA/G);
   //printf("pitch: %lf\n", pitch*180/M_PI); //dbg

   /// Control altura
   control_alt_init_error_buff();
   //thrust_hovering = throttle_hovering*0.0694-88.81;
   printf("Thrust hovering: %lf\n", thrust_hovering);

   /// Log
   log_fd = open(log_name, O_RDWR | O_CREAT | O_NONBLOCK );
   if(log_fd < 0)
   {
      err_log_stderr("Failed to open log file!");
      exit(0);
   }
   bool log_writeOK;

   ///GPS config - Envia comandos al gps a traves del puerto serie - //
#if !SIMULATE_GPS
   retval = preconfigure_gps();
   if(retval < 0) 
   {
      err_log("Failed to preconfigure gps!");
      exit(0);
   } 
     
   /// Ejecuta GPS daemon - proceso independiente
   gpsd_child_pid = init_gps();
   if(gpsd_child_pid == -1)
   {
      err_log_stderr("Failed to init gps!");
      exit(0);
   }
#endif //!SIMULATE_GPS

   /// Ejecuta Demonio S-BUS - proceso independiente
   sbusd_child_pid = futaba_sbus_start_daemon(); 
   if(sbusd_child_pid == -1)
   {
      err_log_stderr("Failed to start child process (sbusd)!"); 
      quit(1);  
   }

   /// inicializa kernel messages queues - para comunicacion con sbusd
   kmsgq = uquad_kmsgq_init(SERVER_KEY, DRIVER_KEY);
   if(kmsgq == NULL)
   {
      quit_log_if(ERROR_FAIL,"Failed to start message queue!");
   }

   //Doy tiempo a que inicien bien los procesos secundarios
   sleep_ms(500); //TODO verificar cuanto es necesario


   /// inicializa UAVTalk
#if !DISABLE_UAVTALK
   uavtalk_child_pid = uavtalk_parser_start(tv_start_main);
   if(uavtalk_child_pid == -1)
   {
      err_log_stderr("Failed to start child process (uavtalk)!"); 
      quit(3);  
   }
#endif

   /// init IMU
#if !DISABLE_IMU
   fd_IMU = imu_comm_init(IMU_DEVICE);
   if(fd_IMU < 0) 
   {
      err_log("Failed to init IMU!");
      quit(0);  
   } 
   //imu_data_alloc(&imu_data);
   //magn_calib_init();
   int bytes_avail = 0; // Para obener bytes disponibles en el RX buffer de IMU
#endif
   
#if !DISABLE_IMU
   // Clean IMU serial buffer
   err_log("Clearing IMU input buffer...");
   serial_flush(fd_IMU);
   sleep_ms(40);
#endif

#if !DISABLE_UAVTALK 
   retval = uavtalk_init_shm();
   if (retval < 0) {
	puts("Error al inicializar shm, cerrando");
	quit(0);
   }
#endif //!DISABLE_UAVTALK 

   printf("----------------------\n  Entrando al loop  \n----------------------\n");
   // -- -- -- -- -- -- -- -- -- 
   // Loop
   // -- -- -- -- -- -- -- -- -- 
   for(;;)
   {
	//para tener tiempo de entrada en cada loop
	gettimeofday(&tv_in_loop,NULL);
	
	//TODO Mejorar control de errores
	if(err_count_no_data > 10)
	{
	   err_log("mas de 10 errores en recepcion de datos!");
	   // TODO que hago? me quedo en hovering hasta obtener datos?
	   err_count_no_data = 0; //por ahora...
	}

#if !DISABLE_IMU
	/// Lectura de datos de la IMU
//--------------------------------------------------------------------------------------------------------
	leer_imu: // Vuelvo a leer si es necesario

	if(err_imu > 2) {
		err_log("No hay datos nuevos de IMU, cerrando.");
		quit(0);
	} 

	// Reviso si tengo una trama de datos completa antes de leer
	ioctl(fd_IMU, FIONREAD, &bytes_avail);
	if(bytes_avail < 34) {
		//puts("datos no completos IMU!"); //dbg
		sleep_ms(5);
		err_imu++;
		goto leer_imu;
	}

	IMU_readOK = check_read_locks(fd_IMU);
	if (IMU_readOK) {

            // Leo datos
	    retval = imu_comm_read(fd_IMU);            
 	    if (retval < 0 ) {
		puts("WARN: unable to read IMU data!");
	    }
            // Paso los datos del buffer RX a imu_raw.
            imu_comm_parse_frame_binary(&imu_raw);
	    //print_imu_raw(&imu_raw); // dbg

	    // Si no estoy calibrando convierto datos para usarlos
	    if (baro_calibrated) {
		imu_raw2data(&imu_raw, &imu_data);
		//print_imu_data(&imu_data); //dbg
	    }

	    imu_updated = true;
            IMU_readOK = false;
	    err_imu = 0;

        } else {
	   err_log("IMU: read NOT ok");
	}

/*	/// Reviso si quedan datos para no atrasarme
	IMU_readOK = check_read_locks(fd_IMU);
	if (IMU_readOK) {
		printf("todavia quedan datos IMU!\n");
		IMU_readOK = false;
		ioctl(fd_IMU, FIONREAD, &bytes_avail);
		//printf("%d\n ",bytes_avail);
		if(bytes_avail > 33) goto leer_imu; //continue;
	}*/

	// Reviso si tengo una trama de datos completa atrasada
	retval = ioctl(fd_IMU, FIONREAD, &bytes_avail);
	if (retval == ERROR_OK) {
		if(bytes_avail > 33) {
			//puts("todavia quedan datos IMU!"); //dbg
			goto leer_imu;
		}	
	} else puts("FIONREAD failed!");

	if (!baro_calibrated) {
		if(imu_updated) {		
			po += imu_raw.pres;
			baro_calib_cont++;
			if (baro_calib_cont == BARO_CALIB_SAMPLES) {
			   pres_calib_init(po/BARO_CALIB_SAMPLES);
			   baro_calibrated = true;
			   puts("Barometro calibrado!");
		}	}
		goto end_loop; //si estoy calibrando no hago nada mas!
	}
//--------------------------------------------------------------------------------------------------------
#endif

	/** loop 50 ms **/

#if !DISABLE_UAVTALK
	/// Leo datos de CC3D
	retval = uavtalk_read(&act);
	if(retval < 0) {
		err_log("No hay datos nuevos de actitud, cerrando.");
		quit(0);
	}
	uavtalk_updated = true;
	//uav_talk_print_attitude(act); //dbg
#endif

	// Calcula diferencia respecto a cero
	act.yaw = act.yaw - get_yaw_zero();

	++count_50; // control de loop 100ms

	/// Check stdin
	stdin_readOK = check_read_locks(STDIN_FILENO);
	if (stdin_readOK) {
	   read_from_stdin();
	}

	/** loop 100 ms **/
	if(count_50 > 1)
	{ 
#if !SIMULATE_GPS
	   /// Obener datos del GPS
	   retval = get_gps_data(&gps);
	   if (retval < 0 )
	   {  
		//que hago si NO hay datos!?
		err_log("No hay datos de gps");
	   } else {
		//que hago si SI hay datos!?
		printf("%lf\t%lf\t%lf\t%lf\t%lf\n",   \
			gps.latitude,gps.longitude,gps.altitude,gps.speed,gps.track);
		gps_updated = true;
	   }
#else //GPS SIMULADO
	   if(control_status == STARTED && !first_time)
	      gps_simulate_position(&position, &velocity, act.yaw, pitch);
	   gps_updated = true;
#endif //!SIMULATE_GPS

	   count_50 = 0;
	} /** end loop 100 ms **/


	if(control_status == STARTED)
        {
	   if (first_time)	
		first_time = false;

//#if 0
           /// Con datos de actitud hago control de yaw y path following (si tengo gps)
	   if(uavtalk_updated)
	   {

	      /// Path Follower - si hay datos de gps y de yaw hago carrot chase	      
	      if(gps_updated) {
#if !SIMULATE_GPS
	        /* guardo en waypoint p los valores de (x,y) hallados mediante el gps.
	         * primero deberia hacer convert_gps2utm(&utm, gps) y restarle la utm inicial
	         */
	         //convert_gps2waypoint(&wp, gps); // TODO no implementado!!
	         wp.angulo = act.yaw;
#else
                 wp.x = position.x;
	         wp.y = position.y;
	         wp.z = position.z;
	         wp.angulo = act.yaw;
#endif //!SIMULATE_GPS
	      
	         //carrot chase
	         retval = path_following(wp, lista_path, &yaw_d);
	         if (retval == -1) {
		     control_status = FINISHED;
		     puts("¡¡ Trayectoria finalizada !!");
		     ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL; // detengo los motores
	         }
		 
		 no_gps_data = 0;

	      } else {
		 no_gps_data++;
		 if (no_gps_data > 1) { // Debo tener datos del gps cada 2 loops de 50 ms
		    err_count_no_data++;
		    no_gps_data = 0;
		    err_log("No tengo datos de gps para seguimiento de trayectorias");
		 }
	      }

	      /// Control Yaw - necesito solo medida de yaw
	      u_yaw = control_yaw_calc_input(yaw_d, act.yaw);
	      //printf("senal de control: %lf\n", u); // dbg

	      //Convertir velocidad en comando
	      ch_buff[YAW_CH_INDEX] = (uint16_t) (u_yaw*25/11 + 1500);
	      //printf("  %u\n", ch_buff[YAW_CH_INDEX]); // dbg

              if (err_count_no_data > 0)
	            err_count_no_data--;

	   } else {
	      err_log("No tengo datos para control de yaw");
	      err_count_no_data++;
	   }
	
	   /// Control de Velocidad
	   if(gps_updated) {
	      //TODO...

	   }
//#endif //if 0

	   /// Despegue
	   if (takeoff == 1) {
		retval = control_altitude_takeoff(&h_d);
		if (retval > 0)
		   takeoff = 0;		
	   }

	   /// Aterrizaje
	   if (takeoff == -1) {
		retval = control_altitude_land(&h_d);
		if (retval > 0) {
		   takeoff = 0;
		   //ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL;
	    	   puts("Pronto para aterrizar!");
            	   //control_status = STOPPED;
		}	
	   }

#if SIMULATE_ALTITUDE
	   /// Control de Altura
	   u_h = control_alt_calc_input(h_d, h);
	   //U_h = u_h + 18.1485;
	   U_h = u_h + thrust_hovering + control_alt_integral(h_d, h);

	   //sim 
	   imu_simulate_altitude(&h, U_h, 0, 0);

	   //Convertir empuje en comando
	   if (U_h <= 0) {
	  	ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL;
	   } else if (U_h > 43.6) {
	   	ch_buff[THROTTLE_CH_INDEX] = MAX_COMMAND;
	   } else {
	   	ch_buff[THROTTLE_CH_INDEX] = (uint16_t) (-0.2984*pow(U_h,2) + 26.0289*U_h + 1168.8);
	   }
	   

	   //printf("h: %lf\th_d: %lf\tu_h: %lf\tU_h: %lf\tThrot: %u\n",h,h_d,u_h,U_h,ch_buff[THROTTLE_CH_INDEX]); // dbg
#endif

#if !DISABLE_IMU
	   /// Control de Altura
	   u_h = control_alt_calc_input(h_d, imu_data.us_altitude);
	   //U_h = u_h + 18.1485;
	   U_h = u_h + thrust_hovering + control_alt_integral(h_d, imu_data.us_altitude);
	   
	   //Convertir empuje en comando
	   if (U_h <= 0) {
	  	ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL;
	   } else if (U_h > 22) {
	   	ch_buff[THROTTLE_CH_INDEX] = (uint16_t) (17.41*U_h+1212.53);
	   } else {
	   	ch_buff[THROTTLE_CH_INDEX] = (uint16_t) (-0.2984*pow(U_h,2) + 26.0289*U_h + 1168.8);
	   }
#endif

	   // Luego de finalizados los controles reseteo flags
	   uavtalk_updated = false;
	   gps_updated = false;
	   imu_updated = false;
	
	} // end if(control_started)


	// Envia actitud y throttle deseados a sbusd (a traves de mensajes de kernel)
	retval = uquad_kmsgq_send(kmsgq, buff_out, MSGSZ);
	if(retval != ERROR_OK)
	{
	   quit_log_if(ERROR_FAIL,"Failed to send message!");
	}

#if SOCKET_TEST
       if (socket_comm_update_position(position) == -1)
	  quit(1);
#endif

	/** Log - T_s_act/T_us_act/roll/pitch/yaw/C_roll/C_pitch/C_yaw/C_throt/T_s_main/T_us_main/pos.x/pos.y/pos.z/yaw_d **/
	
	//Timestamp main
	uquad_timeval_substract(&tv_diff, tv_in_loop, tv_start_main);

	// Duracion del main
	gettimeofday(&tv_out_loop,NULL);
	uquad_timeval_substract(&dt, tv_out_loop, tv_out_last_loop);

	//datos de CC3D para log
	buff_log_len = uavtalk_to_str(buff_log, act);

	//otros logs
	buff_log_len += sprintf(buff_log_aux, "%u %u %u %u %lu %lu %lf %lf %lf %lf %lf %lf %lf",
				ch_buff[ROLL_CH_INDEX],
				ch_buff[PITCH_CH_INDEX],
				ch_buff[YAW_CH_INDEX],
				ch_buff[THROTTLE_CH_INDEX],
				tv_diff.tv_sec,
				tv_diff.tv_usec,
				position.x,
				position.y,
				position.z,
				yaw_d,
				u_yaw,
				h_d,
				U_h);

	strcat(buff_log, buff_log_aux);
	
	//datos de IMU para log
	buff_log_len += imu_to_str(buff_log_aux, imu_data);

	strcat(buff_log, buff_log_aux);

	log_writeOK = check_write_locks(log_fd);
	if (log_writeOK) {
	   retval = write(log_fd, buff_log, buff_log_len);
	   if(retval < 0)
		err_log_stderr("Failed to write to log file!");
	}

	end_loop:

/*#if DEBUG 
	// checkeo de tiempos del main
	gettimeofday(&tv_out_loop,NULL);
	retval = uquad_timeval_substract(&dt, tv_out_loop, tv_in_loop);
	if(retval > 0) {
	   if(dt.tv_usec > 50000) {
		err_log("WARN: se atraso main");
           }
	} else {
	   err_log("WARN: Main absurd timing!");
	}
#endif // DEBUG*/

	/// Control de tiempos del loop
	wait_loop_T_US(MAIN_LOOP_50_MS,tv_in_loop);

   } // for(;;)

   return 0; //nunca llego aca

} //FIN MAIN
예제 #2
0
int main(int argc, char *argv[]){
    int retval;
    char * device_imu;
    char * device_file;
    FILE * file;

    if(argc<3){
	fprintf(stderr,IO_TEST_HOW_TO);
	exit(1);
    }else{
	device_imu = argv[1];
	device_file = argv[2];
    }

    io_t * io = io_init();
    if(io==NULL){
	err_check(ERROR_FAIL,"io test failed");
    }

    // Lets add the IMU and some random file
    // The plan is to have the constant stream of data from the imu, a
    // sporadic data from the file, which would represent the diff in
    // update rate from the GPS and the IMU.

    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    /// Init
    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    // Init IMU
    imu_t * imu = imu_comm_init(device_imu);
    if(imu == NULL){
	err_check(ERROR_FAIL,"Failed to init imu");
    }

    // Init some other file
    file = fopen(device_file,"r+");
    if(file == NULL){
	err_check(ERROR_FAIL,"Failed to open file");
    }

    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    /// Register IO devices
    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    // imu
    int imu_fds;
    retval = imu_comm_get_fds(imu,& imu_fds);
    FREE_N_DIE_IF_ERROR(retval,"Failed to get imu fds!!");
    retval = io_add_dev(io,imu_fds);
    FREE_N_DIE_IF_ERROR(retval,"Failed to add imu to dev list");
    if(io->dev_list.dev_count != 1){
	FREE_N_DIE_IF_ERROR(retval,"Reg device counter error!"); 
    }
    // Pipe
    // To create a pipe, do:
    //     mknode my.pipe p
    // Then open my.pipe as a regular file.
    int file_fds;
    file_fds = fileno(file);
    retval = io_add_dev(io,file_fds);
    FREE_N_DIE_IF_ERROR(retval,"Failed to add file to dev list");
    if(io->dev_list.dev_count != 2){
	FREE_N_DIE_IF_ERROR(retval,"Reg device counter error!"); 
    }
    // stdin
    retval = io_add_dev(io,STDIN_FILENO);
    FREE_N_DIE_IF_ERROR(retval,"Failed to add STDIN to dev list");
    if(io->dev_list.dev_count != 3){
	FREE_N_DIE_IF_ERROR(retval,"Reg device counter error!"); 
    }

    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    /// Poll n read loop
    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    uquad_bool_t read = false,write = false;
    uquad_bool_t reg_imu = true, reg_pipe = true, reg_stdin = true;
    uquad_bool_t imu_ready = false;
    imu_data_t imu_data;
    unsigned char tmp_buff[2];
    int counter = 0;
    poll_n_read:
    while(1){
	retval = io_poll(io);
	FREE_N_DIE_IF_ERROR(retval,"io_poll() error");
	// imu
	if(reg_imu){
	    retval = io_dev_ready(io,imu_fds,&read,&write);
	    FREE_N_DIE_IF_ERROR(retval,"io_dev_ready() error");
	    if(read){
		retval = imu_comm_read(imu,&imu_ready);
		if(imu_ready)
		{
		    if(retval != ERROR_OK){
			fprintf(stdout,"\nIMU missed frame?\n\n");
		    }else{
			retval = imu_comm_get_data_latest_unread(imu,&imu_data);
			if(retval != ERROR_OK)
			    fprintf(stdout,"\nIMU had no data!\n\n");
		    }
		}
	    }
	}
	// pipe
	if(reg_pipe){
	    retval = io_dev_ready(io,file_fds,&read,&write);
	    FREE_N_DIE_IF_ERROR(retval,"io_dev_ready() error");
	    if(read){
		retval = fgetc(file);
		if(retval<0)
		    fprintf(stdout,"\nEOF or error!!\n\n");
		if(retval>0){
		    fprintf(stdout,"Read data from file.\n");
		}
		if(retval==0)
		    fprintf(stdout,"File had no data!.\n");
	    }
	}
	// stdin
	if(reg_stdin){
	    retval = io_dev_ready(io,STDIN_FILENO,&read,&write);
	    FREE_N_DIE_IF_ERROR(retval,"io_dev_ready() error");
	    if(read){
		retval = fread(tmp_buff,1,1,stdin);
		if(retval<=0)
		    fprintf(stdout,"\nNo user input!!\n\n");
		else
		    // Each time user presses RET a device will be removed from
		    // the registered dev list.
		    goto rm;
	    }
	}
	fflush(stdout);
    }
	
    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    /// Remove test
    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    rm:
    switch (counter){
    case 0:
	retval = io_rm_dev(io,file_fds);
	FREE_N_DIE_IF_ERROR(retval,"Failed to remove PIPE from dev list");
	fprintf(stdout,"Removed PIPE from dev list.\n");
	reg_pipe = false;
	fflush(stdout);
	break;
    case 1:
	retval = io_rm_dev(io,imu_fds);
	FREE_N_DIE_IF_ERROR(retval,"Failed to remove IMU from dev list");
	fprintf(stdout,"Removed IMU from dev list.\n");
	reg_imu = false;
	fflush(stdout);
	break;
    case 2:
	fprintf(stdout,"Removed STDIN from dev list.\n");
	retval = io_rm_dev(io,STDIN_FILENO);
	FREE_N_DIE_IF_ERROR(retval,"Failed to remove STDIN from dev list");
	reg_stdin = false;
	fflush(stdout);
	break;	
    default:
	FREE_N_DIE_IF_ERROR(ERROR_FAIL,"COUNTER out of bounds!");
	break;
    }

    if(++counter < 3)
	goto poll_n_read;

    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    /// Free everything
    /// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    kill_n_close:
    retval = io_deinit(io);
    if(retval != ERROR_OK){
	fprintf(stderr,"Warning! Failed to free io structure!\nIgnoring error...\n");
    }
    retval = imu_comm_deinit(imu);
    if(retval != ERROR_OK){
	fprintf(stderr,"Warning! Failed to free imu structure!\nIgnoring error...\n");
    }
    return 0;
}