Logging function moved to separate file.
[fpga/rpi-motor-control.git] / pmsm-control / test_sw / cmd_proc.c
1 #include <stdlib.h>
2 #include <stdio.h>
3 #include <string.h>
4
5 #include "cmd_proc.h"
6 #include "logs.h"
7
8 #define PRUM_PROUD      2061
9 #define PRUM_SOUC       6183
10
11 static char doPrint = 1;
12 static char error = 0;
13
14 /*
15  * \brief
16  * Help
17  */
18 static void printHelp(){
19         doPrint=0;
20         puts("start - Pripravi rizeni, zapne enable bity pwm.");
21         puts("stop - Vypne komutaci, pwm a rizeni.");
22         puts("0 - Vypne komutaci, pwm a rizeni.");
23         puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
24         puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
25         puts("spd:[hodnota] - Zapne rizeni na danou rychlost.");
26         puts("log - Spusti nebo ulozi logovani.");
27         puts("ao:[hodnota] - Prenastavi alpha offset.");
28
29         puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
30         puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
31         puts("exit - Bezpecne ukonci program.");
32 }
33
34
35 /*
36  * \brief
37  * V prikazech je hodnota od zneni prikazu delena dvojteckou
38  * tato funkce dvojtecku nahradi mezerou
39  */
40 static void delCol(char * txt){
41         unsigned i=0;
42         while(txt[i]!='\0'){
43                 if (txt[i]==':') txt[i]=' ';
44                 i++;
45         }
46 }
47
48 /*
49  * Nastavi enable bity na pwm,
50  * zapne komutaci
51  */
52 static void start(struct rpi_state* state){
53         sem_wait(&state->thd_par_sem);
54         state->test=0x70;       /*konfiguracni byte*/
55         sem_post(&state->thd_par_sem);
56 }
57
58 /*
59  * \brief
60  * Zastavi komutaci, vypne pwm
61  */
62 static void stop(struct rpi_state* state){
63         sem_wait(&state->thd_par_sem);
64         state->commutate=0;
65         state->pos_reg_ena=0;
66         state->spd_reg_ena=0;
67         state->duty=0;
68         state->pwm1=0;
69         state->pwm2=0;
70         state->pwm3=0;
71         sem_post(&state->thd_par_sem);
72 }
73
74 /*
75  * \brief
76  * Nastavi pevnou sirku plneni
77  */
78 static void dutySet(struct rpi_state* state, int duty){
79         sem_wait(&state->thd_par_sem);
80         if (duty>MAX_DUTY) duty=MAX_DUTY;
81         if (duty<-MAX_DUTY) duty=-MAX_DUTY;/*paranoia*/
82         state->duty=duty;
83         state->pos_reg_ena=0;
84         state->spd_reg_ena=0;
85         state->commutate=1;
86         sem_post(&state->thd_par_sem);
87 }
88
89 /*
90  * \brief
91  * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu
92  */
93 static void goAbsolute(struct rpi_state* state, int pos){
94         sem_wait(&state->thd_par_sem);
95         state->spd_reg_ena=0;
96         state->pos_reg_ena=1;
97         state->commutate=1;
98         state->desired_pos=pos;
99         sem_post(&state->thd_par_sem);
100 }
101
102 /*
103  * \brief
104  * Zapne nebo vypne pravidelne vypisovani hodnot.
105  */
106 static void changePrint(){
107         doPrint=!doPrint;
108 }
109
110 /*
111  * \brief
112  * Bezpecne ukonci program.
113  */
114 static void exitApp(struct rpi_state* state){
115         stop(state);
116         /* Note: atexit() is set before*/
117         exit(0);
118 }
119 /*
120  * \brief
121  * Set speed.
122  */
123 static void setSpeed(struct rpi_state* state, int speed){
124         if (speed>MAX_SPEED) speed=MAX_SPEED;
125         if (speed<-MAX_SPEED) speed=-MAX_SPEED;/*paranoia*/
126         sem_wait(&state->thd_par_sem);
127         state->pos_reg_ena=0;
128         state->spd_reg_ena=1;
129         state->commutate=1;
130         state->desired_spd=speed;
131         sem_post(&state->thd_par_sem);
132 }
133
134 /*
135  * \brief
136  * Set alpha offset.
137  */
138 static void setAlphaOff(struct rpi_state* state, int offset){
139         if (offset<0) offset*=-1;
140         offset%=1000;
141         sem_wait(&state->thd_par_sem);
142         state->alpha_offset=(unsigned short)offset;
143         sem_post(&state->thd_par_sem);
144 }
145
146 /*
147  * \brief
148  * Initialize logs
149  */
150 static void logInit(struct rpi_state* state){
151         int r;
152         state->log_col=0;
153         state->log_col_count=LOG_DEF_COL;
154         for (r=0;r<LOG_ROWS;r++){
155                 state->logs[r]=malloc(state->log_col_count*sizeof(int));
156                 if (state->logs[r]==NULL){
157                         error=1;
158                         state->log_col_count=-1;
159                         return;
160                 }
161         }
162         state->doLogs=1;
163 }
164
165
166
167 /**
168  * \brief
169  * SetLog
170  * if logs are being logged, save them
171  * if they are not, start log them
172  */
173 static void setLogSEM(struct rpi_state* state){
174         sem_wait(&state->thd_par_sem);
175         /* ulozim logy a vypnu zachytavani*/
176         if (state->log_col_count){
177                 state->doLogs=0;
178                 sem_post(&state->thd_par_sem);
179                 saveLogs(state);
180         /* spustim zachytavani logu */
181         }else{
182                 logInit(state);
183                 sem_post(&state->thd_par_sem);
184         }
185 }
186
187 /**
188  * \brief
189  * Commands detection.
190  */
191 void poll_cmd(struct rpi_state* state){
192         unsigned int tmp;
193         char buff[50];
194         char cmd[30];
195         int val;
196
197         while(1){
198                 scanf("%49s",buff);
199                 delCol(buff);
200                 sscanf(buff,"%s %d",cmd,&val);
201
202
203                 if (!strcmp(cmd,"start")){
204                         start(state);
205                 }else if (!strcmp(cmd,"0")){
206                         stop(state);
207                 }else if (!strcmp(cmd,"stop")){
208                         stop(state);
209                 }else if (!strcmp(cmd,"ga")){
210                         goAbsolute(state,val);
211                 }else if (!strcmp(cmd,"duty")){
212                         dutySet(state,val);
213                 }else if (!strcmp(cmd,"help")){
214                         printHelp();
215                 }else if (!strcmp(cmd,"print")){
216                         changePrint();
217                 }else if (!strcmp(cmd,"exit")){
218                         exitApp(state);
219                 }else if (!strcmp(cmd,"spd")){
220                         setSpeed(state, val);
221                 }else if (!strcmp(cmd,"log")){
222                         setLogSEM(state);
223                 }else if (!strcmp(cmd,"ao")){
224                         setAlphaOff(state,val);
225                 }
226         }
227
228 }
229
230 /*
231  * pocita procentualni odchylku od prumerneho proudu
232  */
233 static float diff_p(float value){
234         return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
235 }
236 /*
237  * pocita procentualni odchylku od prumerneho souctu proudu
238  */
239 static float diff_s(float value){
240         return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
241 }
242
243 /*
244  * tiskne potrebna data
245  */
246 void printData(struct rpi_state* state){
247         if (!doPrint) return;
248
249         struct rpi_in data_p;
250         struct rpi_state s;     /*state*/
251         float cur0, cur1, cur2;
252         int i;
253         /* copy the data */
254         sem_wait(&state->thd_par_sem);
255         data_p = *state->spi_dat;
256         s=*state;
257         sem_post(&state->thd_par_sem);
258
259         if (data_p.adc_m_count){
260                 cur0=data_p.ch0/data_p.adc_m_count;
261                 cur1=data_p.ch1/data_p.adc_m_count;
262                 cur2=data_p.ch2/data_p.adc_m_count;
263         }
264         for (i = 0; i < 16; i++) {
265                         if (!(i % 6))
266                                 puts("");
267                         printf("%.2X ", data_p.debug_rx[i]);
268         }
269         puts("");
270         printf("\npozice=%ld\n",data_p.pozice);
271         printf("rychlost=%d\n",s.speed);
272         printf("chtena pozice=%d\n",s.desired_pos);
273         printf("chtena rychlost=%d\n",s.desired_spd);
274         printf("alpha offset=%hu\n",s.alpha_offset);
275         printf("transfer count=%u\n",s.tf_count);
276         printf("raw_pozice=%u\n",data_p.pozice_raw);
277         printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
278         printf("index position=%u\n",data_p.index_position);
279         printf("distance to index=%u\n",s.index_dist);
280         printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
281         printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
282         printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
283         printf("  PWM1=%u   PWM2=%u   PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3);
284         printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
285         printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
286         printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
287         printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
288         printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0));
289         printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
290         printf("duty=%d\n",s.duty);
291         if (s.index_ok) printf("index ok\n");
292         if (s.commutate) printf("commutation in progress\n");
293         if (s.doLogs) printf("logujeme\n");
294         if (error) printf("error pri maloc logs!! \n");
295
296 }