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