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