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;
}
}
void initialize(void)
{
// Set the background to light gray
glClearColor(0.8, 0.8, 0.8, 1);
// Enables depth test
glEnable(GL_DEPTH_TEST);
// Disable color material
glDisable(GL_COLOR_MATERIAL);
// Enable normalize
glEnable(GL_NORMALIZE);
// Enable Blend and transparency
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// Select a smooth shading
glShadeModel(GL_SMOOTH);
// Sets the light
glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbient);
glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse);
glLightfv(GL_LIGHT0, GL_SPECULAR, lightSpecular);
// Enables lighting
glEnable(GL_LIGHTING);
// Enables the light
glEnable(GL_LIGHT0);
// Enable fog
//glEnable(GL_FOG);
// Set fog formula
glFogi(GL_FOG_MODE, GL_EXP2);
// Set fog density
glFogf(GL_FOG_DENSITY, 0.04);
// Set fog start (only for GL_LINEAR mode)
glFogf(GL_FOG_START, 10);
// Set fog end (only for GL_LINEAR mode)
glFogf(GL_FOG_END, 25);
// Set fog color
glFogfv(GL_FOG_COLOR, fogColor);
// Set fog quality
glHint(GL_FOG_HINT, GL_NICEST);
// The parameter should be 1 / AverageFPS
mot_Init(1 / 80.0);
// Teleport camera
mot_TeleportCamera(5, mot_GetConstant(MOT_EYE_HEIGHT), 5);
mot_SetState(MOT_IS_OP, true);
mot_SetConstant(MOT_MAX_SPEED, 10);
mot_ExitFunc(exitFunc);
// Loads the shaders
loadShaders("./src/vshader.vsh", GL_VERTEX_SHADER, "./src/fshader.fsh", GL_FRAGMENT_SHADER);
}
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);
}
// Begin control algorithm main method
int main()
{
printf("Start of Control\r\n");
mot_Init();
// Declare variables for navdata
int rc;
nav_struct nav;
// Initialize navdata
rc = nav_Init(&nav);
// Check if navdata initializes
int nav_fail = 0;
if (rc==0) {
printf("navdata failed to initialize\n");
nav_fail = 1;
}
// Calibrate navdata
rc=nav_FlatTrim();
if(rc) {
printf("Failed navdata: retcode=%d\r\n",rc);
nav_fail = 1;
}
// Kick off value getting thing in a separate thread!
pthread_create(&image_processing_thread, NULL, process_images, NULL);
int angle = getAngle();
printf("Angle: %i\r\n",angle);
int dir = angle != 0 ? angle/abs(angle) : 0;
/*
//first pulse
pulse(dir,pulseDuration);
printf("Wait: %f\r\n",wait(angle)*1000000);
for(int i=0; i<100; i++){
usleep(wait(angle)/100*1000000);
checkKeypress();
}
pulse(-dir,pulseDuration -.04);
*/
prevAngle = angle;
// start timer
gettimeofday(&t1, NULL);
// PID Loop
float s = .01;
dir= 1;
while(1) {
checkKeypress();
if(waitToStart) continue;
if(stopLoop) break;
// Check navdata if navdata initiates
if (nav_fail == 0) {
checkNavdata(&nav);
}
pid_controller();
/* smallPulse(dir,.9);
usleep(s * 1000000);
smallPulse(dir,.9);
usleep(1.5 * 1000000);
printf("%f\n",s);
if(dir > 0) dir = -1; else dir = 1;
s+=.01;
smallPulse(dir,.9);
usleep(s * 1000000);
smallPulse(dir,.9);
usleep(1.5 * 1000000);
printf("%f\n",s);
s+=.01;
*/
//yield to other threads
pthread_yield();
}
// Cleanup
// Delete the mutex
pthread_mutex_destroy(&video_results_mutex);
//close(sockfd);
//close(newsockfd); // Close TCP socket
//video_Close(&vid); // Close video thread
mot_Close(); // Close motor thread
printf("\nDone!\n");
return 0;
}
