X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/b4d468f4be4d8e91bbd775b5b37f0ae431ae6648..1cf729421e63f859e426a429f9ce935a86afd167:/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 f4ce30b..2eb9262 100644
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -25,9 +25,8 @@
 #include "cmd_proc.h"
 #include "controllers.h"
 #include "commutators.h"
-
-
-
+#include "comp.h"
+#include "logs.h"
 
 #define PRIOR_KERN	50
 #define PRIOR_HIGH	49
@@ -64,6 +63,7 @@ struct rpi_state rps={
 	.alpha_offset=960
 };
 
+
 /**
  * \brief Initilizes GPCLK.
  */
@@ -74,73 +74,6 @@ int clk_init()
 	gpioSetMode(4, FSEL_ALT0);
 	return 0;
 }
-/*
- * \brief Terminates GPCLK.
- */
-
-inline void clk_disable(){
-	termClock(0);
-}
-
-
-/*
- * \brief
- * Free logs
- */
-void freeLogs(){
-	int r;
-	if (rps.log_col_count){
-		for (r=0;r<LOG_ROWS;r++){
-			free(rps.logs[r]);
-		}
-	}
-	rps.log_col_count=0;
-	rps.doLogs=0;
-}
-
-/*
- * \brief
- * Makes log.
- */
-void makeLog(){
-	int r;
-	if (rps.log_col==MAX_LOGS-1){
-		rps.doLogs=0;
-		return;
-	}
-	rps.logs[0][rps.log_col]=(int)rps.tf_count;
-	rps.logs[1][rps.log_col]=(int)rps.spi_dat->pozice;
-
-	rps.logs[2][rps.log_col]=(int)rps.pwm1;
-	rps.logs[3][rps.log_col]=(int)rps.pwm2;
-	rps.logs[4][rps.log_col]=(int)rps.pwm3;
-	rps.logs[5][rps.log_col]=rps.duty;
-
-	rps.logs[6][rps.log_col]=rps.desired_spd;
-	rps.logs[7][rps.log_col]=rps.speed;
-
-	rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count);
-	rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count);
-	rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count);
-
-	rps.log_col++;
-     /*
-        if (rps.log_col==rps.log_col_count-1){
-        	rps.log_col_count*=2;
-        	rps.log_col_count%=MAX_LOGS;
-        	for (r=0;r<LOG_ROWS;r++){
-        		rps.logs[r]=realloc(rps.logs[r],rps.log_col_count*sizeof(int));
-        		if (rps.logs[r]==NULL){
-        			rps.doLogs=0;
-        			rps.error=1;
-        		}
-        	}
-        }
-     */
-}
-
-
-
 
 /**
  * \brief Signal handler pro Ctrl+C
@@ -156,20 +89,13 @@ void appl_stop(){
 	spi_read(&rps);
 
 	spi_disable();
-	clk_disable();
-	freeLogs();
+	termClock(0);
+	freeLogs(&rps);
 	/*muzeme zavrit semafor*/
 	sem_destroy(&rps.thd_par_sem);
         printf("\nprogram bezpecne ukoncen\n");
 }
 
-void substractOffset(struct rpi_in* data, struct rpi_in* offset){
-	data->pozice=data->pozice_raw-offset->pozice_raw;
-	return;
-}
-
-
-
 /**
  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
  */
@@ -180,97 +106,6 @@ void * pos_monitor(void* param){
 	}
 	return (void*)0;
 }
-/*
- * \brief
- * Multiplication of 11 bit
- * Zaporne vysledky prvede na nulu.
- */
-inline uint16_t mult_cap(int32_t s,int d){
-	int j;
-	int res=0;
-	for(j=0;j!=11;j++){
-		/* multiplicate as if maximum sinus value was unity */
-		res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
-	}
-	return res;
-}
-
-
-/**
- * \brief
- * Computation of distance to index.
- *
- * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
- * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
- * 	to je 3999 bodu
- * 	=>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
- * 		signalu indexu
- */
-void comIndDist(){
-	uint16_t pos = 0x0FFF & data.pozice_raw;
-	uint16_t dist;
-	uint16_t index = data.index_position;
-
-	if (index<1999){		/*index e<0,1998> */
-		if (pos<index){			/*pozice e<0,index-1> */
-			/*proti smeru h.r. od indexu*/
-			dist=pos+2000-index;
-		}else if (pos<=index+1999){	/*pozice e<index,index+1999> */
-			/*po smeru h.r. od indexu*/
-			dist=pos-index;
-		}else if (pos<index+2096){	/*pozice e<index+2000,index+2095> */
-			goto index_lost;
-		}else{				/*pozice e<index+2096,4095> */
-			/*proti smeru h.r. od indexu - podtecena pozice*/
-			dist=pos-index-2096;
-		}
-	}else if (index<=2096){		/*index e<1999,2096>*/
-		if (pos<index-1999){		/*pozice e<0,index-2000> */
-			goto index_lost;
-		}else if (pos<index){		/*pozice e<index-1999,index-1> */
-			/*proti smeru h.r. od indexu*/
-			dist=pos+2000-index;
-		}else if (pos<=index+1999){	/*pozice e<index,index+1999> */
-			/*po smeru h.r. od indexu*/
-			dist=pos-index;
-		}else {				/*pozice e<index+2000,4095> */
-			goto index_lost;
-		}
-	}else{				/*index e<2097,4095> */
-		if (pos<=index-2097){		/*pozice e<0,index-2097> */
-			/*po smeru h.r. od indexu - pretecena pozice*/
-			dist=4096+pos-index;
-		}else if (pos<index-1999){	/*pozice e<index-2096,index-2000> */
-			goto index_lost;
-		}else if (pos<index){		/*pozice e<index-1999,index-1> */
-			/*proti smeru h.r. od indexu*/
-			dist=pos+2000-index;
-		}else{ 				/*pozice e<index,4095> */
-			/*po smeru h.r. od indexu*/
-			dist=pos-index;
-		}
-	}
-
-	rps.index_dist = dist;
-	return;
-
-	index_lost:
-		rps.index_ok=0;
-		return;
-}
-
-
-/*
- * \brief
- * Computate speed.
- */
-void compSpeed(){
-	signed long int spd;
-	spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM];
-	rps.speed=(int32_t)spd;
-}
-
-
 
 /*
  * \brief
@@ -304,7 +139,7 @@ void * read_data(void* param){
 			/*subtract initiate postion */
 			rps.tf_count++;
 			substractOffset(&data,poc.spi_dat);
-			compSpeed();			/*spocita rychlost*/
+			compSpeed(&rps);			/*spocita rychlost*/
 
 			if (!rps.index_ok){
 				if (first){
@@ -312,10 +147,10 @@ void * read_data(void* param){
 					first=0;
 				}else if (last_index!=data.index_position){
 					rps.index_ok=1;
-					comIndDist();	/*vypocet vzdalenosti indexu*/
+					comIndDist(&rps);	/*vypocet vzdalenosti indexu*/
 				}
 			}else{ /*index je v poradku*/
-				comIndDist();		/*vypocet vzdalenosti indexu*/
+				comIndDist(&rps);		/*vypocet vzdalenosti indexu*/
 			}
 
 			/* pocitame sirku plneni podle potreb rizeni*/
@@ -337,7 +172,7 @@ void * read_data(void* param){
 
 			/*zalogujeme hodnoty*/
 			if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
-				makeLog();
+				makeLog(&rps);
 			}
 
 			sem_post(&rps.thd_par_sem);		/*--post semaphore---*/
@@ -353,11 +188,9 @@ void * read_data(void* param){
 		}
 }
 
-
 /**
  * \brief Main function.
  */
-
 int main(){
 	pthread_t base_thread_id;
 	clk_init(); 		/* inicializace gpio hodin */