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