Usleep replaced by clock_nasosleep.

diff --git a/pmsm-control/test_sw/Makefile b/pmsm-control/test_sw/Makefile
index 6b2be77cc6688177097ecc6534ccfa2b28c211fd..10b8442173b0a5f121d318b4975b59dfea4b9c70 100644 (file)
--- a/pmsm-control/test_sw/Makefile
+++ b/pmsm-control/test_sw/Makefile
@@ -50,4 +50,4 @@ spi: rp_spi.o
        gcc -o spi rp_spi.c
 #pro rpi
 pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o
-       gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o  misc.o pxmc_sin_fixtab.o -lpthread
+       gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o  misc.o pxmc_sin_fixtab.o -lpthread -lrt

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index c9b9fe9eab2632b1ef7a62f4032467504ab236c1..51951504c3ea5621e02d9a887ffa58b5547234b7 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*/
@@ -50,6 +51,8 @@
 #define PXMC_SIN_FIX_PI2      0x40000000
 #define PXMC_SIN_FIX_2PI3     0x55555555
 
+#define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
+
 struct sigaction sighnd; /*struktura pro signal handler*/
 struct rpi_in data;
 struct rpi_state{
@@ -427,18 +430,23 @@ 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,7 +470,15 @@ 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++;
+                       }
+
                }
 }