void* native_thread_main(void* data) { CVD::Image<unsigned char> frame_grey = CVD::Image<unsigned char>(CVD::ImageRef(WIDTH, HEIGHT)); pose received_location; received_location.x = 0.0; received_location.y = 0.0; received_location.z = 0.0; received_location.yaw = 0.0; start_time = util_timestamp(); while(true) { //double t1 = util_timestamp(); get_frame_grey(frame_grey.data()); corners.build_from_image(frame_grey, drone_xy_lookup, true);//use_rhips client_socket.sendto(corners, server_addr, server_addr_len); //fprintf(log_file2,"%f,%d\n", (util_timestamp()-t1), corners.size()); pthread_yield(); if(client_socket.recvfrom(command, server_addr, server_addr_len)) { received_location.x = command.packet.transform[0]; received_location.y = command.packet.transform[1]; received_location.z = command.packet.transform[2]; received_location.yaw = command.packet.transform[4]; if(filter_pose(&received_location)) { pthread_mutex_lock(&the_mutex); last_good_location.x = received_location.x; last_good_location.y = received_location.y; last_good_location.z = received_location.z; last_good_location.yaw = received_location.yaw; time_last_good = util_timestamp(); pthread_mutex_unlock(&the_mutex); fprintf(log_file,"%f,%f,%f,%f,%f\n" ,time_last_good // timestamp in sec ,received_location.x // x loction ,received_location.y // y location ,received_location.z // Z location ,received_location.yaw); } } frames_processed++; //printf("found %d corners\n", corners.size()); //printf("estimated pose (x,y,z,yaw): %f, %f, %f, %f\n", received_location.x, received_location.y, received_location.z, received_location.yaw); while(!video_frame_ready()) usleep(50); } }