Upgareded priority and scheduler settings of new threads. Inspired by Pavel Pisa...

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

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index c9b9fe9eab2632b1ef7a62f4032467504ab236c1..cbc687979bba0521b9580a81c1bb5d322e382eb7 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -15,6 +15,7 @@
 #include <sched.h>     /*sheduler*/
 #include <unistd.h>    /*usleep*/
 #include <pthread.h>   /*threads*/
+#include <time.h>      /*nanosleep*/
 
 #include "rpin.h"      /*gpclk*/
 #include "rp_spi.h"    /*spi*/
@@ -33,7 +34,7 @@
 #define PRIOR_LOW      20
 
 #define THREAD_SHARED  0
-#define INIT_VALUE     0       /*init value for semaphor*/
+#define INIT_VALUE     1       /*init value for semaphor*/
 
 
 #define PXMC_SIN_FIX_TAB_BITS 9
@@ -50,7 +51,10 @@
 #define PXMC_SIN_FIX_PI2      0x40000000
 #define PXMC_SIN_FIX_2PI3     0x55555555
 
-struct sigaction sighnd; /*struktura pro signal handler*/
+#define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
+
+
+
 struct rpi_in data;
 struct rpi_state{
        uint8_t test;
@@ -86,13 +90,12 @@ inline void clk_disable(){
 /**
  * \brief Signal handler pro Ctrl+C
  */
-void sighnd_fnc(){
+void appl_stop(){
        spi_disable();
        clk_disable();
        /*muzeme zavrit semafor*/
        sem_destroy(&thd_par_sem);
         printf("\nprogram bezpecne ukoncen\n");
-        exit(0);
 }
 
 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
@@ -224,7 +227,6 @@ void prepare_tx(uint8_t * tx){
  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
  */
 void * pos_monitor(void* param){
-       set_priority(param);            /*set priority*/
        while(1){
                printData();
                usleep(1000000);        /*1 Hz*/
@@ -427,18 +429,22 @@ inline void pid(){
 /*
  * \brief
  * Feedback loop.
- * TODO: replace usleep with real-time wait
- *     measure times
  */
 void * read_data(void* param){
        int i;
        struct rpi_in pocatek;
+       struct timespec t;
+       int interval = 1000000; /* 1ms ~ 1kHz*/
        uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
        char first=1;
        uint16_t last_index;                            /*we have index up-to date*/
-       set_priority(param);                            /*set priority*/
        pocatek = spi_read(tx);
+       clock_gettime(CLOCK_MONOTONIC ,&t);
+       /* start after one second */
+       t.tv_sec++;
                while(1){
+                       /* wait until next shot */
+                       clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
                        sem_wait(&thd_par_sem);         /*---take semaphore---*/
                        prepare_tx(tx);                 /*save the data to send*/
                        data = spi_read(tx);            /*exchange data*/
@@ -462,49 +468,25 @@ void * read_data(void* param){
                                simple_hall_commutator(rps.duty);
                        }
                        sem_post(&thd_par_sem);         /*--post semaphore---*/
-                       usleep(500);                            /*1kHz*/
+
+                       /* calculate next shot */
+                       t.tv_nsec += interval;
+
+                       while (t.tv_nsec >= NSEC_PER_SEC) {
+                               t.tv_nsec -= NSEC_PER_SEC;
+                               t.tv_sec++;
+                       }
+
                }
 }
 
+
 /**
- * \brief Main function.
+ * \brief
+ * Commands detection.
  */
-
-int main(){
+void poll_cmd(){
        unsigned int tmp;
-
-       /*nastaveni priorit vlaken*/
-       struct thread_param tsp;
-       tsp.sch_policy = SCHED_FIFO;
-
-       /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/
-       sighnd.sa_handler=&sighnd_fnc;
-       sigaction(SIGINT, &sighnd, NULL );
-
-       clk_init();             /* inicializace gpio hodin */
-       spi_init();             /* iniicializace spi*/
-
-       /*semafor pro detekci zpracovani parametru vlaken*/
-       sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE);
-
-       /*vlakna*/
-       pthread_t tid;                  /*identifikator vlakna*/
-       pthread_attr_t attr;            /*atributy vlakna*/
-       pthread_attr_init(&attr);       /*inicializuj implicitni atributy*/
-
-
-
-       /*ziskavani dat z motoru*//*vysoka priorita*/
-       tsp.sch_prior = PRIOR_HIGH;
-       pthread_create(&tid, &attr, read_data, (void*)&tsp);
-
-       /*vypisovani lokalni pozice*//*nizka priorita*/
-       tsp.sch_prior = PRIOR_LOW;
-       sem_wait(&thd_par_sem);
-       pthread_create(&tid, &attr, pos_monitor, (void*)&tsp);
-
-
-
        /*
         * Note:
         * pri pouziti scanf("%u",&simple_hall_duty); dochazelo
@@ -565,6 +547,31 @@ int main(){
                }
 
        }
-       return 0;
+       return ;
 }
+/**
+ * \brief Main function.
+ */
 
+int main(){
+       pthread_t base_thread_id;
+       clk_init();             /* inicializace gpio hodin */
+       spi_init();             /* iniicializace spi*/
+
+       /*semafor pro detekci zpracovani parametru vlaken*/
+       sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE);
+       setup_environment();
+
+       base_thread_id=pthread_self();
+
+       /*main control loop*/
+       create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
+
+       /*monitor of current state*/
+       create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
+
+       /*wait for commands*/
+       poll_cmd();
+
+       return 0;
+}