Logging function moved to separate file.
[fpga/rpi-motor-control.git] / pmsm-control / test_sw / main_pmsm.c
1 /**
2  * \file main_pmsm.c
3  * \author Martin Prudek
4  * \brief Mainfile pro pmsm control.
5  */
6
7 #ifndef NULL
8 #define NULL (void*) 0
9 #endif /*NULL*/
10
11
12 #include <stdlib.h>     /*exit*/
13 #include <signal.h>     /*signal handler Ctrl+C*/
14 #include <stdio.h>      /*printf*/
15 #include <sched.h>      /*sheduler*/
16 #include <unistd.h>     /*usleep*/
17 #include <pthread.h>    /*threads*/
18 #include <time.h>       /*nanosleep*/
19 #include <string.h>
20
21 #include "rpin.h"       /*gpclk*/
22 #include "rp_spi.h"     /*spi*/
23 #include "misc.h"       /*structure for priorities*/
24 #include "pmsm_state.h"
25 #include "cmd_proc.h"
26 #include "controllers.h"
27 #include "commutators.h"
28 #include "comp.h"
29 #include "logs.h"
30
31 #define PRIOR_KERN      50
32 #define PRIOR_HIGH      49
33 #define PRIOR_LOW       20
34
35 #define THREAD_SHARED   0
36 #define INIT_VALUE      1       /*init value for semaphor*/
37
38
39 #define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
40
41
42 struct rpi_in data;
43 struct rpi_state rps={
44         //.MAX_DUTY=170,
45         .spi_dat=&data,
46         .test=0,
47         .pwm1=0,.pwm2=0, .pwm3=0,
48         .pwm1=0, .t_pwm2=0, .t_pwm3=0,
49         .commutate=0,
50         .duty=0,                        /* duty cycle of pwm */
51         .index_dist=0,          /* distance to index position */
52         .index_ok=0,
53         .tf_count=0,            /*number of transfer*/
54         .desired_pos=0,         /* desired position */
55         .pos_reg_ena=0,
56         .desired_spd=0,
57         .spd_reg_ena=0,
58         .old_pos={0},
59         .spd_err_sum=0,
60         .log_col_count=0,       /* pocet radku zaznamu */
61         .log_col=0,
62         .doLogs=0,
63         .alpha_offset=960
64 };
65
66 /**
67  * \brief Initilizes GPCLK.
68  */
69 int clk_init()
70 {
71         initialise(); /*namapovani gpio*/
72         initClock(PLLD_500_MHZ, 10, 0);
73         gpioSetMode(4, FSEL_ALT0);
74         return 0;
75 }
76 /*
77  * \brief Terminates GPCLK.
78  */
79
80 inline void clk_disable(){
81         termClock(0);
82 }
83
84 /**
85  * \brief Signal handler pro Ctrl+C
86  */
87 void appl_stop(){
88         uint8_t tx[16];
89         sem_wait(&rps.thd_par_sem);
90
91         memset(tx,0,16*sizeof(int));
92         rps.pwm1=0;
93         rps.pwm2=0;
94         rps.pwm3=0;
95         spi_read(&rps);
96
97         spi_disable();
98         clk_disable();
99         freeLogs(&rps);
100         /*muzeme zavrit semafor*/
101         sem_destroy(&rps.thd_par_sem);
102         printf("\nprogram bezpecne ukoncen\n");
103 }
104
105 /**
106  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
107  */
108 void * pos_monitor(void* param){
109         while(1){
110                 printData(&rps);
111                 usleep(1000000);        /*1 Hz*/
112         }
113         return (void*)0;
114 }
115
116 /*
117  * \brief
118  * Feedback loop.
119  * TODO: replace bunch of 'IFs' with Object-like pattern
120  */
121 void * read_data(void* param){
122         int i;
123         struct rpi_in pocatek;
124         struct rpi_state poc={
125                 .spi_dat=&pocatek,
126                 .test=0,
127                 .pwm1=0, .pwm1=0, .pwm3=0
128         };
129         struct timespec t;
130         int interval = 1000000; /* 1ms ~ 1kHz*/
131         char first=1;
132         uint16_t last_index;                            /*we have index up-to date*/
133         spi_read(&poc);         /*pocatecni informace*/
134         clock_gettime(CLOCK_MONOTONIC ,&t);
135         /* start after one second */
136         t.tv_sec++;
137                 while(1){
138                         /* wait until next shot */
139                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
140                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
141
142                         /*old positions*/
143                         rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
144                         spi_read(&rps);         /*exchange data*/
145                         /*subtract initiate postion */
146                         rps.tf_count++;
147                         substractOffset(&data,poc.spi_dat);
148                         compSpeed(&rps);                        /*spocita rychlost*/
149
150                         if (!rps.index_ok){
151                                 if (first){
152                                         last_index=data.index_position;
153                                         first=0;
154                                 }else if (last_index!=data.index_position){
155                                         rps.index_ok=1;
156                                         comIndDist(&rps);       /*vypocet vzdalenosti indexu*/
157                                 }
158                         }else{ /*index je v poradku*/
159                                 comIndDist(&rps);               /*vypocet vzdalenosti indexu*/
160                         }
161
162                         /* pocitame sirku plneni podle potreb rizeni*/
163                         if (rps.pos_reg_ena){           /*pozicni rizeni*/
164                                 pos_pid(&rps);
165                         }else if(rps.spd_reg_ena){      /*rizeni na rychlost*/
166                                 spd_pid(&rps);
167                         }
168
169                         /* sirka plneni prepoctena na jednotlive pwm */
170                         if (rps.index_ok && rps.commutate){
171                                 /*simple_ind_dist_commutator(rps.duty);*/
172                                 /*sin_commutator(rps.duty);*/
173                                 inv_trans_comm(&rps);
174                                 inv_trans_comm_2(&rps);
175                         }else if(!rps.index_ok && rps.commutate){
176                                 simple_hall_commutator(&rps);
177                         }
178
179                         /*zalogujeme hodnoty*/
180                         if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
181                                 makeLog(&rps);
182                         }
183
184                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
185
186                         /* calculate next shot */
187                         t.tv_nsec += interval;
188
189                         while (t.tv_nsec >= NSEC_PER_SEC) {
190                                 t.tv_nsec -= NSEC_PER_SEC;
191                                 t.tv_sec++;
192                         }
193
194                 }
195 }
196
197
198 /**
199  * \brief Main function.
200  */
201
202 int main(){
203         pthread_t base_thread_id;
204         clk_init();             /* inicializace gpio hodin */
205         spi_init();             /* iniicializace spi*/
206
207         /*semafor pro detekci zpracovani parametru vlaken*/
208         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
209         setup_environment();
210
211         base_thread_id=pthread_self();
212
213         /*main control loop*/
214         create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
215
216         /*monitor of current state*/
217         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
218
219         /*wait for commands*/
220         poll_cmd(&rps);
221
222         return 0;
223 }