X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/44c23daa6e0d5d35e892b79684b239c1a0e67f25..fa2db689500f7faf91c5127ada91170e6b06f4df:/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 5759c97..5195150 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*/
@@ -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{
@@ -185,17 +188,18 @@ void prepare_tx(uint8_t * tx){
 	 * bit 89 - shutdown3
 	 * 	.
 	 * 	.
+	 * 	Unused
+	 * 	.
 	 * 	.
-	 * bits 66 .. 56 - match PWM1
-	 * bits 55 .. 45 - match PWM2
-	 * bit 11,12 - Unused
-	 * bits 42 .. 32  - match PWM3
+	 * bits 47 .. 32 - match PWM1
+	 * bits 31 .. 16 - match PWM2
+	 * bits 15 .. 0  - match PWM3
 	 */
 
 
 	uint16_t tmp;
 
-	/* keep the cap*/
+	/* keep the 11-bit cap*/
 
 	if (rps.pwm1>2047) rps.pwm1=2047;
 	if (rps.pwm2>2047) rps.pwm2=2047;
@@ -203,19 +207,19 @@ void prepare_tx(uint8_t * tx){
 
 	tx[0]=rps.test; /*bit 94 - enable PWM1*/
 
+	/*now we have to switch the bytes due to endianess */
+	/* ARMv6 & ARMv7 instructions are little endian */
 	/*pwm1*/
-	tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm1)[1]); /*MSB*/
-	tx[8]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/
+	tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/
+	tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/
 
 	/*pwm2*/
-	tmp=rps.pwm2;
-	tmp<<=5;
-	tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/
-	tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/
+	tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/
+	tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/
 
 	/*pwm3*/
-	tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm3)[1]); /*MSB*/
-	tx[11]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/
+	tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/
+	tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/
 
 
 }
@@ -426,20 +430,25 @@ 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){
-			prepare_tx(tx);			/*save the data to send*/
+			/* 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*/
 			/*subtract initiate postion */
 			rps.tf_count++;
@@ -461,7 +470,15 @@ void * read_data(void* param){
 				simple_hall_commutator(rps.duty);
 			}
 			sem_post(&thd_par_sem);		/*--post semaphore---*/
-			usleep(1000);				/*1kHz*/
+
+			/* calculate next shot */
+			t.tv_nsec += interval;
+
+			while (t.tv_nsec >= NSEC_PER_SEC) {
+				t.tv_nsec -= NSEC_PER_SEC;
+				t.tv_sec++;
+			}
+
 		}
 }