X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/70b98ccdfac6bb0b8284cb0b9b702e5eddb86a7c..b7f2d913caef06457b1b68889895ba01f141935f:/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 c9b9fe9..cbc6879 100644
--- 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;
+}