int testLoopback(int fd,char testVal)
{
double t0=util_timestamp();
int rept=50;
for(int i=0; i<rept; i++) write(fd,&testVal,1);
double t1=util_timestamp();
char resultData;
for(int i=0; i<rept; i++) {
int readOk=safe_read(fd,&resultData,1);
if(readOk!=1) {
printf("Unable to read byte\n");
return 2;
}
if(resultData!=testVal) {
printf("Read data inconsistent\n");
return 1;
}
}
double t2=util_timestamp();
double dt1=t1-t0;
double dt2=t2-t1;
double dt3=t2-t0;
printf("DT: %2f ms %2f ms VAL %d\n",dt1*1000,dt2*1000,resultData);
return 0;
}
void updateatt_u(super_twisting *c, float th, float thp, float ph, float php, float ps, float psp)
{
float tmp; //thp, php, psp;
float l = 0.13, Ixx = 24.1e-3, Iyy = 23.2e-3, Izz = 45.1e-2;
float b = 0.0006646195542576290, d = b*9.72;
float ginvth = Ixx/l, ginvph = Iyy/l, ginvps = Izz;
c->dt = util_timestamp() - c->ta;
c->ta = util_timestamp();
c->th = th;
c->ph = ph;
c->ps = ps;
c->eth = c->thd - c->th;
c->eph = c->phd - c->ph;
c->eps = c->psd - c->ps;
c->epth = c->thpd - thp;
c->epph = c->phpd - php;
c->epps = c->pspd - psp;
tmp = c->epth + c->lambda*c->eth; // s
c->intsgnth += c->dt * (sign(tmp) + sign(c->sth))/2; // Integrate
c->sth = tmp;
tmp = c->epph + c->lambda*c->eph; // s
c->intsgnph += c->dt * (sign(tmp) + sign(c->sph))/2; // Integrate
c->sph = tmp;
tmp = c->epps + c->lambda*c->eps; // s
c->intsgnps += c->dt * (sign(tmp) + sign(c->sps))/2; // Integrate
c->sps = tmp;
// Debug
printf("e= %.3f %.3f %.3f\n",c->eth, c->eph, c->eps); //Imprime el error
printf("\nxp= %.3f %.3f %.3f\n",thp, php, psp); // Imprime la derivada
printf("s= %f %f %f\n",c->sth, c->sph, c->sps); // Imprime s
printf("intsgn= %f %f %f\n",c->intsgnth, c->intsgnph, c->intsgnps);
printf("lmda*ep= %f %f %f\n",c->lambda*c->epth, c->lambda*c->epph, c->lambda*c->epps);
printf("K2·intsignps= %.3f\n",c->K2*c->intsgnps);
printf("K1·sqrt(s)·sign(s)= %.3f",c->K1*sqrt(fabs(c->sth))*sign(c->sth));
printf(" %.3f",c->K1*sqrt(fabs(c->sph))*sign(c->sph));
printf(" %.3f\n",c->K1*sqrt(abs(c->sps))*sign(c->sps));
c->uth = c->lambda * c->epth - c->K1 * sqrt(fabs(c->sth)) * sign(c->sth) - c->K2 * c->intsgnth;
printf("ug= %.3f",c->uth);
c->uth = ginvth * c->uth;
c->uph = c->lambda * c->epph - c->K1 * sqrt(fabs(c->sph)) * sign(c->sph) - c->K2 * c->intsgnph;
printf(" %.3f",c->uph);
c->uph = ginvph * c->uph;
c->ups = c->lambda * c->epps - c->K1 * sqrt(fabs(c->sps)) * sign(c->sps) - c->K2 * c->intsgnps;
printf(" %.3f\n",c->ups);
c->ups = 0;//ginvps * c->ups;
printf("u= %.3f %.3f %.3f\n",c->uth,c->uph,c->ups);
}
void updatealt_u(PID *c, float alt)
{
float P, D, e;
c->dt = util_timestamp() - c->ta;
c->ta = util_timestamp();
e = c->xd - alt;
P = e;
c->I += c->dt * (e + c->eant)/2; // Integrate
D = (e - c->eant)/c->dt;
c->eant = e;
c->ualt = c->kp * P + c->ki * c->I + c->kd * D;
if(c->ualt < 0) c->ualt = 0;
}
void *horizontal_velocities_thread_main(void *data)
{
//file_fd = fopen("test.yuv", "wb");
double prevTime = 0;
video_GrabImageGrey(img_old);
for (;;)
{
//double t1=util_timestamp();
video_GrabImageGrey(img_new);
//double t2=util_timestamp();
//printf("video grab took %f ms\n", (t2-t1)*1000);
double currentTime = img_new->timestamp;
int dxi, dyi;
video_blocksum(img_old, img_new, &dxi, &dyi);
//double t3=util_timestamp();
//printf("blocksum took %f ms\n", (t3-t2)*1000);
if (dxi != 0 || dyi != 0)
{
//swap buffers
struct img_struct* tmp = img_new;
img_new = img_old;
img_old = tmp;
}
pthread_mutex_lock(&velocity_access_mutex);
dt = currentTime - prevTime;
xv_buffer[buf_ind] = dxi / dt;
yv_buffer[buf_ind] = dyi / dt;
buf_ind = (buf_ind+1)%BUF_SIZE;
seqNum++;
prevTime = currentTime;
pthread_mutex_unlock(&velocity_access_mutex);
printf("%f\n", util_timestamp());
//printf("\ndxi=%i dyi=%i cur=%f pre=%f dt=%f\n", dxi, dyi, currentTime, prevTime, dt);
//if(writeImagesToDisk)
// fwrite((const void *)img_new->buf, 320*240, 1, file_fd);
// bottom camera = 60Hz
//usleep(10000);
}
video_close();
return 0;
}
void updatealt_u(PID *c, float alt)
{
float P, e, m = 0.42, g = 9.78;
c->dt = util_timestamp() - c->ta;
c->ta = util_timestamp();
e = c->xd - alt;
P = e;
c->I += c->dt * (e + c->eant)/2; // Integrate
if(e - c->eant == 0)
{
c->n++;
} else {
c->D = (e - c->eant)/c->dt/c->n;
c->n = 1;
}
c->eant = e;
c->ualt = c->kp * P + c->ki * c->I + c->kd * c->D;
// Se le suma m*g para contrarrestar el peso del cuadricoptero
// para que el controlador solo tenga que estabilizarlo en un punto
c->ualt += m * g;
if(c->ualt < 0) c->ualt = 0;
}
int init_controller(super_twisting *st, PID *pid)
{
st->uth = 0;
st->uph = 0;
st->ups = 0;
printf("Introduce la ganancia K1 : ");
scanf("%f",&st->K1);
printf("K1 = %f\n",st->K1);
printf("Introduce la ganancia K2: ");
scanf("%f",&st->K2);
printf("K2 = %f\n",st->K2);
st->th = 0;
st->ph = 0;
st->ps = 0;
st->thd = 0;
st->phd = 0;
st->psd = 0;
st->thpd = 0;
st->phpd = 0;
st->pspd = 0;
printf("Introduce la ganancia lambda: ");
scanf("%f",&st->lambda);
st->intsgnth = 0;
st->intsgnph = 0;
st->intsgnps = 0;
st->antth = 0;
st->antph = 0;
st->antps = 0;
st->dt = 1;
st->ta = util_timestamp();
pid->ualt = 0;
printf("Introduce la ganancia kp: ");
scanf("%f",&pid->kp);
printf("Introduce la ganancia ki: ");
scanf("%f",&pid->ki);
printf("Introduce la ganancia kd: ");
scanf("%f",&pid->kd);
pid->I = 0;
pid->D = 0;
printf("Introduce la altura deseada: ");
scanf("%f",&pid->xd);
pid->eant = 0;
pid->n = 1;
pid->dt = 1;
pid->ta = st->ta;
return 0;
}
void *object_detect_thread_main(void *data)
{
printf("Tracking...\n");
double prevTime = 0.0;
for (;;)
{
//videoF_GrabImage(&vidF, current_frame);
// double start=util_timestamp();
if (current_frame != NULL)
{
struct point blob_loc = findBlob(current_frame, 91, 100, 130, 136, 160, 240);
double currentTime = util_timestamp();
pthread_mutex_lock(&location_access_mutex);
dt2 = currentTime-prevTime;
int temp = (int)(blob_loc.x)/10;
locX = 10*temp - current_frame->w/2;
temp = (int)(blob_loc.y)/10;
locY = -10*temp + current_frame->h/2;
//yaw -= (float)locX/(4*1700);
//height += (float)locY/(4*2000);
prevTime=currentTime;
seqNumF++;
pthread_mutex_unlock(&location_access_mutex);
}
// double endTime=util_timestamp();
// printf(" Loop took %f ms\n", (endTime-start)*1000);
}
//videoF_close();
return 0;
}
int init_controller(super_twisting *st, PID *pid)
{
st->uth = 0;
st->uph = 0;
st->ups = 0;
st->K1 = 4.5 * 12;
st->K2 = 1.1 * 12;
st->th = 0;
st->ph = 0;
st->ps = 0;
st->thd = 0;
st->phd = 0;
st->psd = 0;
st->thpd = 0;
st->phpd = 0;
st->pspd = 0;
st->lambda = 3;
st->intsgnth = 0;
st->intsgnph = 0;
st->intsgnps = 0;
st->antth = 0;
st->antph = 0;
st->antps = 0;
st->dt = 1;
st->ta = util_timestamp();
pid->ualt = 0;
pid->kp = 80;
pid->ki = 10;
pid->kd = 10;
pid->I = 0;
pid->xd = 30;
pid->eant = 0;
pid->dt = 1;
pid->ta = st->ta;
return 0;
}
