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