Ejemplo n.º 1
0
int main ()
{
    char message [512];
    int n = 1;
    int tps = 1;

    if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0)
    {
        printf("[FAILED] Socket initialization failed\n");
    }
    else
    {
        sleep(1);
        set_trim(message, n++);
        usleep(500000);
        while(tps < 100)
	{
            take_off(message, n++);
            usleep(50000);
            tps++;
        }
        tps = 0;
        while(tps < 100)
        {
            reset_com(message);
            usleep(50000);
            tps++;
        }
        tps = 0;
        landing(message, n++);
        sleep(1);
    }
    close_socket();
    return 0;
}
Ejemplo n.º 2
0
int main ()
{
    char message [512];
    int n = 1;
    int tps = 1;

    if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0)
    {
        printf("[FAILED] Socket initialization failed\n");
    }
    else
    {
        sleep(1);
        set_trim(message, n++);
        usleep(500000);
////////////// take off //////////////////////////
        while(tps < 167)                        //100 avec 50ms
        {
            take_off(message, n++);
            usleep(30000);
            tps++;
        }
        tps = 0;
////////////// WAIT ////////////////////
        while(tps < 133)                        //80
        {
            reset_com(message);
            usleep(30000);
            tps++;
        }
        tps = 0;
////////////// GO ////////////////////
        while(tps < 58)                         //35
        {
            set_pitch(message, n++, FRONT, 0.25);
            usleep(30000);
            tps++;
        }
        tps = 0;
        while(tps < 17)                         //10
        {
            set_pitch(message, n++, BACK, 0.25);
            usleep(30000);
            tps++;
        }
        tps = 0;
        while(tps < 17)
        {
            set_pitch(message, n++, FRONT, 0.0);
            usleep(30000);
            tps++;
        }
        tps = 0;
////////////// WAIT ////////////////////
        while(tps < 25)
        {
            reset_com(message);
            usleep(30000);
            tps++;
        }
        tps = 0;
////////////// ROTATE ////////////////////
        while(tps < 110)                        //140
        {
            set_yaw(message, n++, LEFT, 0.4);
            usleep(30000);
            tps++;
        }
        tps = 0;
        set_yaw(message, n++, RIGHT, 0.0);
        usleep(30000);
////////////// WAIT ////////////////////
        while(tps < 80)
        {
            reset_com(message);
            usleep(30000);
            tps++;
        }
        tps = 0;
////////////// RETURN ////////////////////
        while(tps < 58)
        {
            set_pitch(message, n++, FRONT, 0.25);
            usleep(30000);
            tps++;
        }
        tps = 0;
        while(tps < 17)
        {
            set_pitch(message, n++, BACK, 0.25);
            usleep(30000);
            tps++;
        }
        tps = 0;
        while(tps < 17)
        {
            set_pitch(message, n++, FRONT, 0.0);
            usleep(30000);
            tps++;
        }
        tps = 0;
////////////// WAIT ////////////////////
        while(tps < 167)
        {
            reset_com(message);
            usleep(30000);
            tps++;
        }
        tps = 0;
////////////// LAND ////////////////////
        landing(message, n++);
        sleep(1);
    }
    close_socket();
    return 0;
}
Ejemplo n.º 3
0
int main ()
{
	char message [512];
	int n = 1;
	int tps = 1;
	int wait =1;
	/*
	//handle the ctrl -c to make the drone land
	struct sigaction act;
	memset(&act,0,sizeof(act));
	act.sa_handler = intHandler;
	sigaction(SIGINT, &act, NULL);
	*/

    if (init_socket() != 0)
    {
        printf("[FAILED] Socket initialization failed\n");
    }
    else //complex_move(...;float roll_power, float pitch_power, float vertical_power, float yaw_power)
    {
		sleep(1);
    	printf("demarrage\n");
		set_trim(message, n++, wait);
		
		while(tps < 167)
		{
			take_off(message, n++, wait);
			tps++;
		}
		tps = 0;
		
		//test de fonction
		at_config(message, n++, "pic:ultrasound_freq","2");
		
		//while(tps < 133)
		//while(1)
		while(keepRunning)
		{
			reset_com(message, wait);
	 		tps++;
		}
		tps = 0;
/*
        printf("debut commande\n");

		while(tps < 500)
		{
			//go front and up and turning clockwise			
			set_complex_move(message,n++,0,-0.05,0.1,0.05,wait);
			tps++;
		}
		tps = 0;

		set_complex_move(message, n++,0,0,0,0,wait);
		while(tps < 133)
		{
			reset_com(message,wait);
	 		tps++;
		}
		tps = 0;
*/
		landing(message, n++,wait);
		sleep(1);

	}
    close(sockfd);
	return 0;
}
Ejemplo n.º 4
0
void SWITCH_DRONE_COMMANDE(int _order)
{
  char message [64];
  
  switch(_order){

        case 0 : 
            reset_com(message);
            break;

        case 1 :
            landing(message);
            inC.flag_land = 0;
            break;

        case 2 :
            //printf("SWITCH COMMANDE take off\n");
            take_off(message);
            inC.flag_takeoff = 0;
            break;

        case 3 :
            //printf("case 3 : set_roll, roll_power=%f\n", roll_power);
            set_roll(message, roll_move, roll_power);
            inC.flag_rollcalled = 0;
            break;

        case 4 :
            //printf("case 4 : set_pitch, pitch_power=%f, yaw_power=%f\n", pitch_power, yaw_power);
            set_pitch(message, pitch_move, pitch_power);
            inC.flag_pitchcalled = 0;
            break;

        case 5 :
            set_yaw(message, yaw_move, yaw_power);
            inC.flag_yawcalled = 0;
            break;

        case 6 :
            set_roll(message, roll_move, roll_power);
            set_pitch(message, pitch_move, pitch_power);
            inC.flag_rollpitchcalled = 0;
            break;

        case 7 :
            set_trim(message);
            inC.flag_calibH = 0;
            break;

        case 8 :
            calibrate_magneto(message);
            sleep(3);

            while(1)
            {
              set_yaw(message, RIGHT, 0.1);
              while(isControllerReady()==0);
              
              Main_Nav = getNavdata();
              if (Main_Nav.magneto.heading_fusion_unwrapped > 0.0)
                nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped - 360.0;
              else
                nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped + 360.0;
              
              printf("angle_trouve et angle +/- 360 = %.2f, %.2f                   \r", Main_Nav.magneto.heading_fusion_unwrapped, nav_suiv);
              if(nav_suiv < 0.0 && nav_prec > 0.0 || nav_suiv > 0.0 && nav_prec < 0.0)
              {
                inC.flag_calibM = 0;
                printf("\nnav_suiv = %.2f | nav_prec = %.2f\n", nav_suiv, nav_prec);
                break;
              }
              nav_prec = nav_suiv;
            }

            set_yaw(message, LEFT, 0.0);

            inC.flag_calibM = 0;
            break;

        case 9 :
            //set_emergency(message);
            inC.flag_emergency = 0;
            set_gaz(message, UP, 0.2);// this is an ugly hack (don't commit this)
            break;

        case 10 :
            //anti_emergency(message);
            inC.falg_antiemergency = 0;
            set_gaz(message, UP, 0.0);// this is an ugly hack (don't commit this)
            break;

        case 20 : 
            calcul_mission();
            break;

        default :
            printf("Scade ne marche pas si bien que ça finalement\n");
            break;
      }  
}
Ejemplo n.º 5
0
/**
 * @brief	function designed to be the main of a thread
 *			Sends movement commands to the drone in order to follow the beacon
 */
void * track_position(void * arg){

	// check time interval
    struct timeval old_tv = {0};
    struct timeval tv = {0};
    long unsigned int elapsed_time = 0; // in microsecondss
    
    // moves
    char message [512];
	int tps = 1;
	int wait =1;

	//handle the ctrl -c to make the drone land
	struct sigaction act;
	memset(&act,0,sizeof(act));
	act.sa_handler = intHandlerThread3;
	sigaction(SIGINT, &act, NULL);

    gettimeofday(&old_tv, NULL); 

	if (init_socket() != 0)
    {
        printf("[FAILED] Socket initialization failed\n");
    }
    else
    {

    	//////////////////////////////////////////////////////////
    	//	TAKING OFF 
    	//////////////////////////////////////////////////////////
		sleep(1);
        printf("Drone starts flying...\n");
		set_trim(message, wait);
		
		printf("Taking off...\n");
		while(tps < 167)
		{
			take_off(message, wait);
			tps++;
		}
		
		//stop waiting 40 us after a command send
		wait = 0;

		// Go up to be at shoulder level
		printf("Going up...\n");
		gettimeofday(&old_tv, NULL);
		elapsed_time = 0;

		while(elapsed_time < 2)
		{
			set_simple_move(message, UP, 1, wait);
			gettimeofday(&tv, NULL);
			elapsed_time = (tv.tv_sec-old_tv.tv_sec);
		}

		set_simple_move(message, UP, 0.0, wait);
		elapsed_time = 35000;

		while(keepRunning){
		    while (elapsed_time < 35000)
			{
				gettimeofday(&tv, NULL);
				elapsed_time = (tv.tv_sec-old_tv.tv_sec)*1000000 + (tv.tv_usec-old_tv.tv_usec);
			}

			pthread_mutex_lock(&track_pos_mux);

			print_position();
			
			///////////////////////////////////////////////////////////////////////
			// MOVES TO HAVE THE RIGHT ANGLE AND RIGHT DISTANCE FROM THE EMIITER
			///////////////////////////////////////////////////////////////////////
			reset_com(message, wait);

			//If no signal has been detected
			if(!pos.signalDetected)
			{
				// stop moving
				set_simple_move(message, FRONT, 0, wait);
			}
			// If a signal has been detected, move !
			else
			{

				if(pos.angle >= -ANGLE_PRECISION/2 && pos.angle <= ANGLE_PRECISION/2)
				{
					// For now, always move forward when the source in front of the drone
					set_simple_move(message, FRONT, 0.05, wait);

					// And now manage distance
					// if(pos.distance > 200) // in cm
					// 	set_simple_move(message, FRONT, 0.05, wait);
					// else if(pos.distance < 180) // in cm
					// 	set_simple_move(message, BACK, 0.05, wait);
					// else
					// 	set_simple_move(message, FRONT, 0, wait);	
				}				
	      	 	else if (pos.angle > ANGLE_PRECISION/2)
					set_simple_move(message, CLKWISE, 0.5, wait);
				else
					set_simple_move(message, ANTI_CLKWISE, 0.5,wait);
				
			}
			pthread_mutex_unlock(&compute_pos_mux);

			gettimeofday(&old_tv, NULL);
			elapsed_time=0;
		}

		///////////////////////////////////////////
		// LANDING
		///////////////////////////////////////////
		landing(message, wait);
		sleep(1);

	}
	pthread_exit(NULL);
}
Ejemplo n.º 6
0
int main ()
{
    char message [512];
    int n = 1;
    int tps = 1;

    if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0)
    {
        printf("[FAILED] Socket initialization failed\n");
    }
    else
    {
        sleep(1);
        set_trim(message, n++);
        usleep(500000);
        while(tps < 167)
        {
                take_off(message, n++);
                usleep(30000);
                tps++;
        }
        tps = 0;
        while(tps < 133)
        {
                reset_com(message);
                usleep(30000);
                tps++;
        }
        tps = 0;
/*              while(tps < 30)
        {
                set_pitch(message, n++, BACK, 0.25);
                usleep(50000);
                tps++;
        }
        tps = 0;
        while(tps < 10)
        {
                set_pitch(message, n++, FRONT, 0.1);
                usleep(50000);
                tps++;
        }
        tps = 0;
        while(tps < 30)
        {
                set_pitch(message, n++, BACK, 0.0);
                usleep(50000);
                tps++;
        }
        tps = 0;*/
/*              while(tps < 60)
        {
                take_off(message, n++);
                usleep(50000);
                tps++;
        }
        tps = 0;*/
        while(tps < 150)
        {
                set_yaw(message, n++, LEFT, 0.4);
                usleep(30000);
                tps++;
        }
        tps = 0;
        set_yaw(message, n++, RIGHT, 0.0);
        usleep(30000);
        while(tps < 133)
        {
                reset_com(message);
                usleep(30000);
                tps++;
        }
        tps = 0;
        landing(message, n++);
        sleep(1);
    }
    close_socket();
    return 0;
}