Пример #1
0
void* nanoclock_timer_run_thread(rtdal_timer_t* obj) {
    int s;
    int n;
    assert(obj->period_function);

    obj->stop = 0;
    if (obj->wait_futex) {
        futex_wait(obj->wait_futex);
        obj->next.tv_sec+=TIMER_FUTEX_GUARD_SEC;
        obj->next.tv_nsec=0;
    } else {
        clock_gettime(CLOCK_REALTIME, &obj->next);
    }
    n=0;
    while(!obj->stop) {
        timespec_add_us(&obj->next, obj->period);
        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME,
                        &obj->next, NULL);

        n++;
        if (n>=obj->multiple) {
            obj->period_function(obj->arg, &obj->next);
            n=0;
        }
    }

    s = 0;
    pthread_exit(&s);
    return NULL;
}
Пример #2
0
int main(int *argc, char argv[]) {


    char sendBuf[100];
    char receiveBuf[100];
    struct timespec waitTime;
    clock_gettime(CLOCK_REALTIME, &waitTime);
    udp_init_client(&conn, 9999, "192.168.0.1");

    //VARIABLES FOR REGULATING
    double error, integral, u, y;
    double reference = 1;
    double Kp = 10;
    double Ki = 800;
    double period = PERIOD;

    strncpy(sendBuf,"START", sizeof(sendBuf));
    udp_send(&conn,sendBuf,sizeof(sendBuf)); //strlen(sendBuf)+1

    //strncpy(sendBuf,"SET:12.3456",sizeof(sendBuf));
    //udp_send(&conn,sendBuf,sizeof(sendBuf));


    int numRep = RUNTIME/PERIOD;

    int i;
    for ( i = 0 ; i < numRep ; i++ ) {
        timespec_add_us(&waitTime,PERIOD*1000000);
        clock_nanosleep(&waitTime);

        strncpy(sendBuf,"GET", sizeof(sendBuf));
        udp_send(&conn,sendBuf,sizeof(sendBuf));
        udp_receive(&conn,receiveBuf,sizeof(receiveBuf));


        char * numVal = receiveBuf + 8;
        y = atof(numVal);

        //printf("Y-value from server is %f\n",y);



        //WE HAVE A Y-VALUE, DO PI REGULATING
        error = reference - y;
        integral = integral + (error * period);
        u = Kp * error + Ki * integral;

        setU(u);


    }


    strncpy(sendBuf,"STOP", sizeof(sendBuf));
    udp_send(&conn,sendBuf,sizeof(sendBuf));

    return 0;
}
Пример #3
0
void* periodicPrint() {
	while(1) {
		printf("hei\n");
		struct timespec next;
		clock_gettime(CLOCK_REALTIME, &next);
	
		timespec_add_us(&next, 500000);
		clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next, NULL);
	}
}
Пример #4
0
void PIreg 	( void ){

	
	double error, integral, u, y;
	const double reference = 1;
	const double Kp = 10;
	const double Ki = 800;
	const double period = PERIOD;		
	int numRep = RUNTIME/PERIOD;

	struct timespec waitTime;
	clock_gettime(CLOCK_REALTIME, &waitTime);
	
	sendMsg("START",6);
	
	int i;
	for ( i = 0 ; i < numRep ; i++ ){

		timespec_add_us(&waitTime,PERIOD*1000000);
		clock_nanosleep(&waitTime);

		// request new value
		sendMsg("GET",4);
	
		// wait for value from listener
		while(!newPIval){ 
			/* wait */ 
		}
		
		// reset flag
		pthread_mutex_lock(&newPIval_mut);
		newPIval = false;
		pthread_mutex_unlock(&newPIval_mut);
		
		// extract value
		char * numVal = globalMsgArray + 8;	
		y = atof(numVal);


		// run PI-regulator
		error = reference - y;
		integral = integral + (error * period);
		u = Kp * error + Ki * integral;
		
		setU(u);
		
	}
	sendMsg("STOP",5);
}
Пример #5
0
struct periodic_task *start_periodic_timer(uint64_t offs, int t)
{
    struct periodic_task *p;

    p = malloc(sizeof(struct periodic_task));
    if (p == NULL) {
        return NULL;
    }

    clock_gettime(CLOCK_REALTIME, &p->r);
    timespec_add_us(&p->r, offs);
    p->period = t;

    return p;
}
Пример #6
0
void* periodicTest(void* num_param) {
	int num = (long) num_param;
	set_cpu(CPU_CORE);
	io_write(num, 1);
	struct timespec next;
	clock_gettime(CLOCK_REALTIME, &next);
	while(1) {
		while (io_read(num));

		io_write(num, 0);
		
		
		clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next, NULL);
		timespec_add_us(&next, 100);
		io_write(num, 1);
	}
}
Пример #7
0
void checkChannel ( int channel ) {

	set_cpu(CPU_USED);
	struct timespec t;
	clock_gettime(CLOCK_REALTIME, &t);


	while(1){

		timespec_add_us(&t,100);
		clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&t,NULL);
		
		if ( ! io_read(channel) ) {
			io_write(channel,LOW);
			usleep(5);
			io_write(channel,HIGH);
		}		
	}
}
Пример #8
0
void *pi_controller(void *arg)
{
	char msg[BUF_LEN];
	char output[BUF_LEN];
	double integral = 0.0;
	double u = 0.0;
	double error = 0.0;

	struct timespec sleep;

	printf("PI controller thread initialized\n");
	clock_gettime(CLOCK_REALTIME, &sleep);

	while (run_pi)
	{
		// Wait for new y value
		sem_wait(&pi_sem);
		// Calculate errortimespec_add_us(&sleep, TICK);
		pthread_mutex_lock(&y_mutex);
		error = ref - y;
		pthread_mutex_unlock(&y_mutex);

		// Calculate output
		integral = integral + (error*PERIOD);
		u = (KP*error) + (KI*integral);

		// Create output string
		snprintf(output,BUF_LEN,"%f",u);
		memset(msg, 0, BUF_LEN);
		strcat(msg, "SET:");
		strcat(msg, output);

		// Send calculated u value to server
		pthread_mutex_lock(&udp_mutex);
		udp_send(&conn, msg, strlen(msg)+1);
		pthread_mutex_unlock(&udp_mutex);

		timespec_add_us(&sleep, TICK);
		clock_nanosleep(&sleep);
	}

	return 0;
}
Пример #9
0
static void wait_next_activation(struct periodic_task *t)
{
    clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &t->r, NULL);
    timespec_add_us(&t->r, t->period);
}
Пример #10
0
void *thread_pid(void *argpointer){
	shared_system_output_t *y = (shared_system_output_t*) argpointer;

	float controller_output;

	/* Buffer used for sending to socket */
	char buffer[BUFFER_SIZE];

	/* Init the PID controller struct*/
	PID_ctrl_t Controller = {
		.Kp			= PID_KP,
		.Ki			= PID_KI,
		.Kd			= PID_KD,
		.reference	= PID_REFERENCE,
		.period		= PID_PERIOD,
		.previous_error 	= PID_INIT_ERROR,
		.integral	= 0.0
	};

	struct timespec period_timer;
	clock_gettime(CLOCK_REALTIME, &period_timer);

	int counter = 0;

	while(1){
		counter++;
		timespec_add_us(&period_timer, PID_PERIOD*1000000);

		/* Request system Output from server */
		if(udp_send(&shared_server_con.socket, "GET", sizeof("GET")) == FAIL){
			printf("\tERROR: send failed.\n");
			exit(1);
		}
		
		/* Wait for new value */
		sem_wait(&shared_pid);

		pthread_mutex_lock(&(y->lock));
		float system_output = y->value;
		pthread_mutex_unlock(&(y->lock));

		/* Calculate the corresponding output */
		controller_output = get_controller_output(system_output, &Controller);
		sprintf(buffer, "SET:%f", controller_output);

		/* Send the calculated controller output to the system on server */
		if(udp_send(&shared_server_con.socket, buffer, sizeof(buffer)) == FAIL){
			printf("\tERROR: send failed.\n");
			exit(1);
		}

		if(counter*PID_PERIOD > RUNTIME) {
			break;
		}

		/* Sleep until next period */
		clock_nanosleep(&period_timer);
	}
	return NULL;
}

float get_controller_output(float system_output, PID_ctrl_t *Controller){
	float error, derivative;

	/* Calculate the different contributions */
	error = Controller->reference - system_output;
	Controller->integral = Controller->integral + (error * Controller->period);
	derivative = (error - Controller->previous_error);

	/* Store the error to be used in next calculation */
	Controller->previous_error = error;

	/* Return the  controller output */
	return Controller->Kp*error + Controller->Ki*Controller->integral + Controller->Kd*derivative;
}
Пример #11
0
void* PIDctrl()
{
    float y_upd = 0;
    float u = 0;
    char buf[MSG_LENGTH];

    //Init the PID struct parameters

    pid_param_t PI = {
        .Kp = K_P,
        .Ki = K_I,
        .integral = 0.0,
        .error = INIT_ERR,
        .reference = INIT_REF,
        .period = PERIOD,
    };

    printf("this is PID calling\n");
    struct timespec period;
    clock_gettime(CLOCK_REALTIME, &period);
    int i = 0;

    while(1) {
        i++;
        timespec_add_us(&period, (PERIOD*1000000));
        pthread_mutex_lock(&tinfo_connection.sendlock);
        if((udp_send(&tinfo_connection.conn, "GET", 4))==-1)
        {
            printf("Failed GET PIDcntrl\n");
            exit(1);
        }

        pthread_mutex_unlock(&tinfo_connection.sendlock);

        //Signal new value
        sem_wait(&y_updt);

        pthread_mutex_lock(&tinfo_output.lock);
        y_upd = tinfo_output.y_val;
        pthread_mutex_unlock(&tinfo_output.lock);

        //Calculate output value
        PI.error = PI.reference - y_upd;
        PI.integral = PI.integral + (PI.error * PI.period);
        u = PI.Kp * PI.error + PI.Ki * PI.integral;

        //Add u value to buffer
        sprintf(buf, "SET:%f", u);

        //Send new output to server
        pthread_mutex_lock(&tinfo_connection.sendlock);
        if((udp_send(&tinfo_connection.conn, buf,sizeof(buf)))==-1)
        {
            perror("Failed SET\n");
        }
        pthread_mutex_unlock(&tinfo_connection.sendlock);

        if((i*PERIOD) > RUNTIME)
        {
            break;
        }

        //Sleeep until new period zzzzz
        clock_nanosleep1(&period);
    }
    return NULL;
}
Пример #12
0
// ------------------------------------------------------------------------------
//   Write Thread
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
write_thread(void)
{
  //Timespec Structures
  struct timespec now;
  struct timespec next;
  struct timespec start;
  start.tv_sec = 0;
  start.tv_nsec = 0;
  
  struct timespec miss_time;
  struct timespec wcet;
  // signal startup
  writing_status = 2;
  printf("Writing Thread Running...\n");

  // prepare an initial setpoint, just stay put
  mavlink_set_position_target_local_ned_t sp;
  sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY &
    MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE;
  sp.coordinate_frame = MAV_FRAME_LOCAL_NED;
  sp.vx       = 0.0;
  sp.vy       = 0.0;
  sp.vz       = 0.0;
  sp.yaw_rate = 0.0;

  // set position target
  current_setpoint = sp;

  // write a message and signal writing
  //write_setpoint();
  writing_status = true;

  // Pixhawk needs to see off-board commands at minimum 2Hz,
  // otherwise it will go into fail safe

  long period_us = WritePeriod.tv_sec*1e6 + WritePeriod.tv_nsec/1000;
  printf("Write Thread period: %ld us\n",period_us);
  clock_gettime(CLOCK_REALTIME, &next);
  while ( not time_to_exit )
    {

      clock_gettime(CLOCK_REALTIME,&now);
      timespec_add_us(&next,period_us);
      
      timespec_sub(&miss_time,&next,&now);
      timespec_sub(&wcet,&now,&start);
      int64_t miss_time_us = miss_time.tv_sec*1e6 + miss_time.tv_nsec/1e3;
      int64_t wcet_us = wcet.tv_sec*1e6 + wcet.tv_nsec/1e3;


      fprintf(logfile,"%d, %ld, %ld, %d, %ld, %d, %ld \n",start.tv_sec, start.tv_nsec, wcet_us, now.tv_sec, now.tv_nsec, next.tv_sec, next.tv_nsec);
      /*
	printf("\n");
	printf("START : %ld us \n", start.tv_nsec/1000);
	printf("NEXT  : %ld us \n", next.tv_nsec/1000);
	printf("END   : %ld us \n", now.tv_nsec/1000);
	printf("WCET  : %d  us \n", wcet_us);
	printf("MISS  : %d  us \n", miss_time_us);
	printf("\n");
      */
      
      if(timespec_cmp(&next,&now) == -1)
	{
	  fprintf(stderr,"write thread: DeadLine Miss!       DT: %ld us\n",miss_time_us);
	  printf("wcet : %ld us \n\n",wcet_us);
	  continue;
	}
      
      clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&next,0);
      clock_gettime(CLOCK_REALTIME,&start);
      
      if (hil_mode)
	{
	  //write_setpoint();
	  /*
	    pthread_mutex_lock(&mut);
       
	    if(blocked == 0)
	    {
	    blocked = 1;
	    pthread_cond_wait(&cond,&mut);
	    blocked = 0;
	    }
	    else
	    {
	    pthread_cond_signal(&cond);
	    }
	    write_hilsensors();
	    pthread_mutex_unlock(&mut);
	  */
	  write_hilsensors();
	}
      else
        {
	  write_hilsensors();
	  //write_heartbeat();
	  //write_raw();
	  
	  //pthread_mutex_lock(&mut);
       
	  //if(blocked == 0)
	  //  {
	  //    blocked = 1;
	  //    pthread_cond_wait(&cond,&mut);
	  //    blocked = 0;
	  //  }
	  //else
	  //  {
	  //    pthread_cond_signal(&cond);
	  //  }
	  //write_setpoint();
	  
	  //write_heartbeat();
	  //pthread_mutex_unlock(&mut);
	  
	}
      
    }
  
  // signal end
  writing_status = false;
  return;

}
Пример #13
0
int main (void)
{
	int port = 9999;
	char *ip = "192.168.0.1";
	char msg[BUF_LEN];
	pthread_t pi, listener;

#if WITH_SIGNAL
	pthread_t signal_handler;
#endif

	struct timespec start, stop;

	// Init mutexes
	pthread_mutex_init(&y_mutex, NULL);
	pthread_mutex_init(&udp_mutex, NULL);

	sem_init(&pi_sem, 0 ,1);
#if WITH_SIGNAL
	sem_init(&signal_sem, 0, 1);
#endif

	// UDP init
	udp_init_client(&conn, port, ip);
	printf("UDP Client initialized\n");

	// Spawn  UDP listener
	pthread_create(&listener, NULL, udp_listen, NULL);

	// Send start command
	strcpy(msg, "START");
	pthread_mutex_lock(&udp_mutex);
	udp_send(&conn, msg, strlen(msg)+1);
	pthread_mutex_unlock(&udp_mutex);
	printf("START sent\n");

	// Spawn pi controller and signal acknowledger
	pthread_create(&pi, NULL, pi_controller, NULL);
#if WITH_SIGNAL
	pthread_create(&signal_handler, NULL, signal_ack, NULL);
#endif

	// Set get command
	strcpy(msg, "GET");

	// Init clocks
	clock_gettime(CLOCK_REALTIME, &start);
	clock_gettime(CLOCK_REALTIME, &stop);

	// Define run time
	timespec_add_us(&start, 0);
	timespec_add_us(&stop, END);

	while ((start.tv_sec <= stop.tv_sec) && (start.tv_nsec < stop.tv_nsec))
	{
		// Send get command
		pthread_mutex_lock(&udp_mutex);
		udp_send(&conn, msg, strlen(msg)+1);
		pthread_mutex_unlock(&udp_mutex);

		// Add timer TICK to start
		timespec_add_us(&start, TICK);
		// Wait for next tick
		clock_nanosleep(&start);
	}

	// Send stop command to server
	strcpy(msg, "STOP");
	udp_send(&conn, msg, strlen(msg)+1);
	printf("Stop sent\n");

	// Destroy mutexes
	pthread_mutex_destroy(&y_mutex);
	pthread_mutex_destroy(&udp_mutex);

	// Destroy semaphores
	sem_destroy(&pi_sem);
#if WITH_SIGNAL
	sem_destroy(&signal_sem);
#endif

	// Kill threads
	run_listen = 0;
	run_pi = 0;
#if WITH_SIGNAL
	run_signal = 0;
#endif

	// Join threads
	pthread_join(pi, NULL);
	pthread_join(listener, NULL);

#if WITH_SIGNAL
	pthread_join(signal_handler, NULL);
#endif

	// Close UDP
	udp_close(&conn);

	return 0;
}
Пример #14
0
int main(void){
    struct udp_conn udpcon;
    
    
    
	udp_init_client(&udpcon, port, server_ip);
    
    char buffer[255];
           
	struct PID_parameters parameters= (struct PID_parameters){
		10,
		800,
		0,
		1,
		period,
	};
	double y, u;
	int i=0;
	struct timespec next;
	clock_gettime(CLOCK_REALTIME, &next);		
	timespec_add_us(&next, (int)(period*1000000));
	udp_send(&udpcon, "START", 6);
	while(1){
	    udp_send(&udpcon, "GET", 4);
	    
	    while(1){
	        udp_receive(&udpcon, buffer, 255);
	        if (buffer[0]=='G'){
	            break;
	        }
	    }
	    
	    y = doubleFromReply(buffer);
	    u = PID_controller(parameters, y);
	    
	    setFromDouble(buffer,u);
	    udp_send(&udpcon, buffer, 255);
	    clock_nanosleep(&next);
	    timespec_add_us(&next, (int)(period*1000000));
	    if (i++*period >= 0.5){
	        break;
	    }
	}
	
	udp_send(&udpcon, "STOP",5);
	printf("stop\n");
	return 0;
}

void setFromDouble(char *buffer, double u){
    buffer[0]='S';
    buffer[1]='E';
    buffer[2]='T';
    buffer[3]=':';
    char dummy[255];
    sprintf(dummy, "%f", u);
    char* ptr = dummy;
    int i=4;
    while(*ptr !='\0'){
        buffer[i++]=*ptr;
        ptr++;
    }
    buffer[i]='\0';
    return;
}