Modified program exit behavior.

[fpga/rpi-motor-control.git] / pmsm-control / test_sw / misc.c

diff --git a/pmsm-control/test_sw/misc.c b/pmsm-control/test_sw/misc.c
index 78d4eb523920b762193dd018711cb899dbc657b1..046363047a4838da93fe170b3f6e0d7751e78767 100644 (file)
--- a/pmsm-control/test_sw/misc.c
+++ b/pmsm-control/test_sw/misc.c
@@ -8,21 +8,89 @@
  *     Vetsinou pro komunikaci mezi vlakny, jejich sychronizaci a predavani priorit.
  */
 
-#include "misc.h"
+
 #include <sched.h>
+#include <signal.h>    /*signal handler*/
+#include <stdlib.h>
+#include <stdio.h>     /*printf*/
+#include <pthread.h>
+#include <sys/mman.h>  /*mlockall*/
+
+#include "misc.h"
+
+
 
 /**
- * nastavi prioritu a scheduler podle parametru
- * funkce na konci vrati semafor, aby mohlo dojit k opetovne zmene parametru
- * \param pointer na struct thread_sched_param
+ * \brief
+ * Signal handler.
+ */
+void appl_sig_handler(int sig){
+       exit(0); /*atexit is set*/
+}
+
+/*
+ * \brief Setup initial environment.
+ */
+void setup_environment(){
+       /*struktura pro signal handler*/
+       struct sigaction sighnd;
+
+       if (mlockall(MCL_FUTURE | MCL_CURRENT) < 0) {
+           fprintf(stderr, "mlockall failed - cannot lock application in memory\n");
+           exit(1);
+        }
+
+       atexit(appl_stop);
+
+       /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/
+       sighnd.sa_handler=appl_sig_handler;
+       sigaction(SIGINT, &sighnd, NULL );
+}
+
+/*
+ * \brief
+ * Creates RT thread
  */
-void set_priority(void * param){
-       struct sched_param sp;
-       struct thread_param *tp = ((struct thread_param*) param);
+int create_rt_task(pthread_t *thread, int prio, void *(*start_routine) (void *), void *arg){
+       int ret ;
 
-       sp.sched_priority = tp->sch_prior; /*vysoka priorita*/
-       if(sched_setscheduler(0, tp->sch_policy, &sp) == -1) {
-               perror("pid sched_setscheduler failed");
+       pthread_attr_t attr;
+       struct sched_param schparam;
+
+       /*inicializace implicitnich atributu*/
+       if (pthread_attr_init(&attr) != 0) {
+               fprintf(stderr, "pthread_attr_init failed\n");
+               return -1;
+       }
+
+       /*nastavi dedeni planovace z attr*/
+       if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) != 0) {
+               fprintf(stderr, "pthread_attr_setinheritsched failed\n");
+               return -1;
+       }
+
+       /*nastaveni planovace*/
+       if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO) != 0) {
+               fprintf(stderr, "pthread_attr_setschedpolicy SCHED_FIFO failed\n");
+               return -1;
+       }
+
+       schparam.sched_priority = prio;
+
+       /*nastavit atribut attr podle hodnoty schparam*/
+       if (pthread_attr_setschedparam(&attr, &schparam) != 0) {
+               fprintf(stderr, "pthread_attr_setschedparam failed\n");
+               return -1;
        }
-       sem_post(&thd_par_sem);
+
+       /*vytvori vlakno*/
+       ret = pthread_create(thread, &attr, start_routine, arg);
+
+       /*uvolni strukturu, nema vliv na vlakna jiz vytvorena*/
+       pthread_attr_destroy(&attr);
+
+       return ret;
 }
+
+
+