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);
}
}
