Exemplo n.º 1
0
int ctl_Init(char *client_addr) 
{
	int rc;
  
	//defaults from AR.Drone app:  pitch,roll max=12deg; yawspeed max=100deg/sec; height limit=on; vertical speed max=700mm/sec; 
	setpoint.pitch_roll_max=DEG2RAD(12); //degrees     
  //setpoint.yawsp_max=DEG2RAD(100); //degrees/sec
  setpoint.h_max=600; //cm
  setpoint.h_min=40; //cm
  setpoint.throttle_hover=0.46;
  setpoint.throttle_min=0.30;
  setpoint.throttle_max=0.85;
  			
	//init pid pitch/roll 
	pid_Init(&pid_roll,  0.4,0,-0.05,0);//0.5
	pid_Init(&pid_pitch, 0.4,0,-0.05,0);//0.5
	pid_Init(&pid_yaw,   0.8,0,-0.06,0);//1.00
	//pid_Init(&pid_yaw,   0,0,0,0);//1.00
	pid_Init(&pid_h,     0.003,0,-0.001,1);//0.0005
//	pid_Init(&pid_x,     0.01,0,0,0);//0.0005
//	pid_Init(&pid_y,     0.01,0,0,0);//0.0005

  throttle=0.00;

  //Attitude Estimate
	rc = att_Init(&att);
	if(rc) return rc;

  
  //udp logger
  udpClient_Init(&udpNavLog, client_addr, 7778);
  //navLog_Send();
  printf("udpClient_Init\n", rc);
  
	//start motor thread
	rc = mot_Init();
	if(rc) return rc;
  
	vid.device = (char*)"/dev/video1";
	vid.w=176;
	vid.h=144;
	vid.n_buffers = 4;
	video_Init(&vid);

	img_old = video_CreateImage(&vid);
	img_new = video_CreateImage(&vid);
	
	video_GrabImage(&vid, img_old);

	//start ctl thread 
	rc = pthread_create(&ctl_thread, NULL, ctl_thread_main, NULL); 
	if(rc) {
		printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
		return 202;
	}
}
Exemplo n.º 2
0
int main(int argc,char ** argv)
{
	vid_struct vid;
	vid.device = (char*)"/dev/video1";
	vid.w=176;
	vid.h=144;
	vid.n_buffers = 4;
	video_Init(&vid);

	img_struct* img_old = video_CreateImage(&vid);
	img_struct* img_new = video_CreateImage(&vid);
	
	int dx,dy;
	int x=0,y=0;

	video_GrabImage(&vid, img_old);
    for (;;) {
		video_GrabImage(&vid, img_new);

		//process
		video_blocksum(img_old, img_new, &dx, &dy);
		x+=dx;
		y+=dy;
		printf("diff between img %5d and %5d -> dx=%2d dy=%2d x=%4d y=%4d\n",img_old->seq,img_new->seq,dx,dy,x,y);
		
		if(dx!=0 || dy!=0) {
			//swap buffers
			img_struct* tmp = img_new;
			img_new=img_old;
			img_old=tmp;
		}
    }

	video_Close(&vid);
	
    return 0;
}
Exemplo n.º 3
0
void * process_images(void * param)
{
    // Initialize used variables
    int n, sum;

    // Buffer for image data
    unsigned char * buf1;

    // Buffer for message passing info
    char buffer[4];


    // Initialize getting a picture
    vid_struct vid;

    // Device location
    // Video0 is front camera, video1 is bottom camera
    vid.device = (char*)"/dev/video0";

    // Other params for video
    vid.w = VIDEO_WIDTH;
    vid.h = VIDEO_HEIGHT;
    vid.n_buffers = NUM_BUFFERS;

    // Initialize video thread for streaming
    video_Init(&vid);

    // Create blank image
    img_struct * img = video_CreateImage(&vid);

    // INITIALIZE TCP CONNECTION
    // Note: AR.Drone is server and listens for connection opening
    // IP:192.168.1.1 Port: 7777

    // Socket file descriptor, new socket file descriptor, port number
    int sockfd, newsockfd, portno;
    socklen_t clilen;
    struct sockaddr_in serv_addr, cli_addr;

    // Open tcp socket
    sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (sockfd < 0) {
        error("ERROR opening socket");
    }

    //zero-initialize serv_addr
    bzero((char *) &serv_addr, sizeof(serv_addr));

    //set port number
    portno = PORT_NUM;

    //set parameters for serv_addr
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = INADDR_ANY;
    serv_addr.sin_port = htons(portno);

    //bind socket
    if (bind(sockfd, (struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0) {
        error("ERROR on binding");
    }

    //listen for client to open connection
    listen(sockfd,5);
    clilen = sizeof(cli_addr);

    //open new socket
    newsockfd = accept(sockfd,(struct sockaddr *) &cli_addr,&clilen);
    if (newsockfd < 0) {
        error("ERROR on accept");
    }

    // Now constantly fetch this and update the global variables
    while(1) {
        // Get picture into image buffer from video thread
        video_GrabImage(&vid, img);
        buf1 = img->buf;

        // Set image buffer
        unsigned char image[chopped_size];

        // Copy over data from buf1 to image
        memcpy(image, buf1 + y_upper, y_lower - y_upper + 1); // Copy Y values
        memcpy(image + (y_lower - y_upper + 1), buf1 + cr_upper, cr_lower - cr_upper + 1); // Copy Cr values
        memcpy(image + (y_lower - y_upper + 1)+ (cr_lower - cr_upper + 1), buf1 + cb_upper, cb_lower - cb_upper + 1); // Copy Cb values

        // Send packet to client
        n = 0;
        sum = 0;
        while (sum < chopped_size) {
            n = write(newsockfd, image + sum, chopped_size - sum);
            if (n < 0) {
                error("ERROR reading image data from socket!");
            }
            sum += n;
        }

        // Read 4 character return message from client
        bzero(buffer,4);
        n = 0;
        sum = 0;
        while (sum < 4) {
            n = read(newsockfd, buffer + sum, 4 - sum);
            if (n < 0) {
                error("ERROR reading client message!");
            }
            sum += n;
        }

        // Convert buffer to integer or NULL
        char none[] = "None";
        int equality = 0;

        //Check that buffer is "None"
        int check;
        for (check=0;check<4;check++) {
            if (buffer[check]==none[check]) {
                equality = 1;
            }
            else {
                equality = 0;
                break;
            }
        }

        // Message received is integer string if not equal
        if (equality==0) {
            // Lock the position value and update it + timestamp
            pthread_mutex_lock(&video_results_mutex);
            position_value = atoi(buffer);
            time(&img_recv_timestamp);
            pthread_mutex_unlock(&video_results_mutex);
        }
        else {
            // Lock the position value and update it
            pthread_mutex_lock(&video_results_mutex);
            position_value = 9999;
            time(&img_recv_timestamp);
            pthread_mutex_unlock(&video_results_mutex);
        }

        // Relinquish CPU before starting again
        pthread_yield();
    }

}