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 ctl_Init(char *client_addr)
{
int rc;
//----- Initialize PID controller -----//
//some saturation for controller
setpoint.pitch_roll_max=DEG2RAD(10); //degrees
setpoint.throttle_min=0.05;//0.5
setpoint.throttle_max=0.95; //0.85
//init pid pitch/roll //Kp, Ki, Kd, imax
pid_Init(&pid_roll, 0.33,0.0,0.25,1); //[0.35 0.25] with additional part [0.33 0.25] without ...
pid_Init(&pid_pitch, 0.33,0.0,0.25,1);
throttle=0.000;
//----- Initialize AHRS system -----//
printf("Initialize Attitude.. \n");
//rc = att_Init(&att);
//if(rc) return rc;
if(navdata_init()) printf("navdata initialized\n");
navdata_flattrim(&ahrsdata);
//printf("bias_after:%f\t%f\t%f\n",gyros_offset[0],gyros_offset[1],gyros_offset[2]);
sleep(1);
//----- Initialize UDP -----//
// Udp logger
printf("client\n");
udpClient_Init(&udpNavLog, "192.168.43.176", 9930); // Update the IP of ground station to send data
//udpClient_Init(&udpNavLog, "192.168.1.4", 9930);
//navLog_Send();
printf("udpClient_Init\n", rc);
//----- Start motor thread -----//
printf("Initialize motor\n");
rc = mot_Init();
if(rc) return rc;
//----- Start ctl thread -----//
printf("Initialize ctl_thread_main\n");
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;
}
else printf("rc = %d\n", rc);
}