Пример #1
0
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;


}
Пример #2
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);

}
Пример #3
0
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;
}
Пример #5
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;
}
Пример #6
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;
}
Пример #7
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;
}
Пример #8
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;
}