1 /*******************************************************************
2 Components for embedded applications builded for
3 laboratory and medical instruments firmware
5 firmware.c - multi axis motion controller comprocesor
6 firmware for FPGA tumble CPU of lx-rocon system
8 (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
9 (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com
11 This file can be used and copied according to next
13 - GPL - GNU Public License
14 - other license provided by project originators
16 *******************************************************************/
18 #define SUPPRESS_CONDITIONALS 1
19 #undef COMPUTE_PHASE_SECTOR
22 #include "pxmcc_types.h"
23 #include "tumbl_addr.h"
25 /* k3 = math.sqrt(3) / 2 */
27 #define CONST16_K3 56756
31 #define RECI16_K3 75674
33 /* 1 / (2 * k3) * 65536 */
34 #define RECI16_2_K3 37837
36 pxmcc_data_t pxmcc_data;
38 void init_defvals(void)
46 void find_sin_lat(void)
49 register uint32_t a0, a1, a2, a3, a4, a5;
51 *FPGA_FNCAPPROX_SIN = 0;
53 for (i = 0; i < 20; i++)
54 asm volatile("": : : "memory");
56 *FPGA_FNCAPPROX_SIN = 0x40000000;
57 a0 = *FPGA_FNCAPPROX_SIN;
58 a1 = *FPGA_FNCAPPROX_SIN;
59 a2 = *FPGA_FNCAPPROX_SIN;
60 a3 = *FPGA_FNCAPPROX_SIN;
61 a4 = *FPGA_FNCAPPROX_SIN;
62 a5 = *FPGA_FNCAPPROX_SIN;
63 asm volatile("": : : "memory");
79 uint32_t last_rx_done_sqn = 0;
80 pxmcc_axis_data_t *pxmcc;
82 pxmcc_data.common.fwversion = PXMCC_FWVERSION;
83 pxmcc_data.common.pwm_cycle = 2500;
84 pxmcc_data.common.min_idle = 0x7fff;
85 pxmcc = pxmcc_data.axis;
88 /*pxmcc->ptofs = *FPGA_IRC0;*/
90 pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
92 pxmcc_data.curadc[0].siroladc_offs = 0x0c17;
93 pxmcc_data.curadc[1].siroladc_offs = 0x0c66;
94 pxmcc_data.curadc[2].siroladc_offs = 0x0c66;
96 asm volatile("": : : "memory");
103 pxmcc = pxmcc_data.axis;
106 uint32_t ofs = pxmcc->ptofs;
107 uint32_t per = pxmcc->ptirc;
111 volatile uint32_t *uptr;
113 irc = *(FPGA_IRC0 + (FPGA_IRC1 - FPGA_IRC0) * pxmcc->inp_info);
116 if ((uint32_t)pti >= per) {
127 pta = pti * pxmcc->ptreci;
129 *FPGA_FNCAPPROX_SIN = pta;
130 asm volatile("nop\n");
131 asm volatile("nop\n");
132 asm volatile("nop\n");
133 pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
134 asm volatile("nop\n");
135 pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
140 int32_t pwm_bet_div_2_k3;
147 #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
149 #endif /*COMPUTE_PHASE_SECTOR*/
151 pwm_d = (volatile uint32_t)pxmcc->pwm_dq;
152 pwm_q = (pwm_d << 16) >> 16;
155 pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
156 pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
159 pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
161 #ifndef SUPPRESS_CONDITIONALS
164 /* pwm_bet > 2 * k3 * pwm_alp */
165 if (pwm_bet_div_2_k3 > pwm_alp)
170 /* -pwm_bet > 2 * k3 * pwm_alp */
171 if (pwm_bet_div_2_k3 < -pwm_alp)
177 /* pwm_bet > -2 * k3 * pwm_alp */
178 if (pwm_bet_div_2_k3 > -pwm_alp)
183 /* pwm_bet > 2 * k3 * u_alp */
184 if (pwm_bet_div_2_k3 > pwm_alp)
190 /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */
191 pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16;
192 /* pwm2 = 1/k3 * pwm_bet */
193 pwm2 = pwm_bet_div_2_k3 >> 15;
195 } else if (phs <= 3) {
197 /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */
198 pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16;
199 /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */
200 pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16;
202 /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
203 pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
205 /* pwm3 = -1/k3 * pwm_bet */
206 pwm3 = -pwm_bet_div_2_k3 >> 15;
208 #else /*SUPPRESS_CONDITIONALS*/
210 int32_t alp_m_bet_d2k3;
211 uint32_t state23_msk;
212 uint32_t pwm23_shift;
213 uint32_t bet_sgn = pwm_bet >> 31;
214 uint32_t alp_sgn = pwm_alp >> 31;
215 #ifdef COMPUTE_PHASE_SECTOR
216 uint32_t bet_sgn_cpl = ~bet_sgn;
217 #endif /*COMPUTE_PHASE_SECTOR*/
219 alp_m_bet_d2k3 = (alp_sgn ^ pwm_alp) - (bet_sgn ^ (pwm_bet_div_2_k3 + bet_sgn));
220 alp_m_bet_d2k3 = alp_m_bet_d2k3 >> 31;
222 state23_msk = alp_sgn & ~alp_m_bet_d2k3;
227 * 0 0 -1 0 -> 1 (001)
228 * -1 0 -1 0 -> 1 (001)
229 * -1 0 0 -1 -> 2 (010)
230 * -1 -1 0 -1 -> 3 (011)
231 * -1 -1 -1 0 -> 4 (100)
232 * 0 -1 -1 0 -> 4 (100)
233 * 0 -1 0 0 -> 5 (101)
236 #ifdef COMPUTE_PHASE_SECTOR
237 phs = (bet_sgn & 5) + (bet_sgn_cpl ^ ((alp_m_bet_d2k3 + 2 * state23_msk) + bet_sgn_cpl));
238 #endif /*COMPUTE_PHASE_SECTOR*/
240 pwm1 = pwm_alp & state23_msk;
241 pwm2 = pwm_bet_div_2_k3 - pwm1;
242 pwm3 = -pwm_bet_div_2_k3 - pwm1;
243 pwm2 &= (~bet_sgn | state23_msk);
244 pwm3 &= (bet_sgn | state23_msk);
245 pwm1 = pwm_alp + pwm2 + pwm3;
246 pwm1 &= ~state23_msk;
248 pwm23_shift = 15 - state23_msk;
249 pwm2 >>= pwm23_shift;
250 pwm3 >>= pwm23_shift;
252 #endif /*SUPPRESS_CONDITIONALS*/
254 #ifdef COMPUTE_PHASE_SECTOR
256 #endif /*COMPUTE_PHASE_SECTOR*/
259 *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
260 *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
261 *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
263 pwmtx_info = pxmcc->pwmtx_info;
264 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
265 pxmcc->pwm_prew[1] = *uptr & 0x3fff;
266 *uptr = pwm2 | 0x4000;
267 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
268 pxmcc->pwm_prew[2] = *uptr & 0x3fff;
269 *uptr = pwm3 | 0x4000;
270 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
271 pxmcc->pwm_prew[0] = *uptr & 0x3fff;
272 *uptr = pwm1 | 0x4000;
275 uint32_t bet_sgn = pwm_bet >> 31;
276 uint32_t alp_sgn = pwm_alp >> 31;
277 pwm1 = pwm2 = pwm_bet | 0x4000;
278 pwm3 = pwm4 = pwm_alp | 0x4000;
284 pwmtx_info = pxmcc->pwmtx_info;
285 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
286 pxmcc->pwm_prew[0] = *uptr & 0x3fff;
287 *uptr = pwm1 | 0x4000;
288 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
289 pxmcc->pwm_prew[1] = *uptr & 0x3fff;
290 *uptr = pwm2 | 0x4000;
291 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
292 pxmcc->pwm_prew[2] = *uptr & 0x3fff;
293 *uptr = pwm3 | 0x4000;
294 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 24) & 0xff);
295 pxmcc->pwm_prew[3] = *uptr & 0x3fff;
296 *uptr = pwm4 | 0x4000;
300 pwmtx_info = pxmcc->pwmtx_info;
301 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
302 pxmcc->pwm_prew[1] = *uptr & 0x3fff;
303 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
304 pxmcc->pwm_prew[2] = *uptr & 0x3fff;
305 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
306 pxmcc->pwm_prew[0] = *uptr & 0x3fff;
308 pwmtx_info = pxmcc->pwmtx_info;
309 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
310 pxmcc->pwm_prew[0] = *uptr & 0x3fff;
311 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
312 pxmcc->pwm_prew[1] = *uptr & 0x3fff;
313 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
314 pxmcc->pwm_prew[2] = *uptr & 0x3fff;
315 uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 24) & 0xff);
316 pxmcc->pwm_prew[3] = *uptr & 0x3fff;
321 } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
323 asm volatile("": : : "memory");
326 uint32_t idlecnt = 0;
329 sqn = *FPGA_LX_MASTER_RX_DDIV;
331 } while (sqn == last_rx_done_sqn);
332 pxmcc_data.common.act_idle = idlecnt;
333 if ((idlecnt < pxmcc_data.common.min_idle) &&
335 pxmcc_data.common.min_idle = idlecnt;
337 last_rx_done_sqn = sqn;
338 pxmcc_data.common.rx_done_sqn = last_rx_done_sqn;
339 asm volatile("": : : "memory");
344 pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
345 volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
348 for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
351 curadc->cur_val = (int16_t)(val - curadc->siroladc_last
352 - curadc->siroladc_offs);
353 curadc->siroladc_last = val;
358 /* additional 3 required for 7 -> 8 change */
364 pxmcc = pxmcc_data.axis;
377 uint32_t curmult_idx;
380 #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
382 #ifdef COMPUTE_PHASE_SECTOR
384 #endif /*COMPUTE_PHASE_SECTOR*/
385 #endif /*COMPUTE_PHASE_SECTOR*/
387 out_info = pxmcc->out_info;
390 pwm1 = pxmcc->pwm_prew[0];
391 pwm2 = pxmcc->pwm_prew[1];
392 pwm3 = pxmcc->pwm_prew[2];
394 #ifndef SUPPRESS_CONDITIONALS
395 #ifndef COMPUTE_PHASE_SECTOR
399 else if (pwm1 > pwm3)
406 else if (pwm1 < pwm3)
410 #endif /*COMPUTE_PHASE_SECTOR*/
412 curmult_idx = (0x00201201 >> (4 * phs)) & 3;
413 pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
414 pwm_reci = (pxmcc_data.common.pwm_cycle << 16) / pwm_reci;
417 * Translate index from pwm1, pwm2, pwm3 order to
418 * to order of current sources 0->2 1->0 2->1
420 * This solution modifies directly value in pxmcc_curadc_data_t
421 * so it is destructive and has not to be applied twice,
422 * but it is much better optimized
424 curmult_idx = (0x102 >> (curmult_idx * 4)) & 3;
425 pcurmult = &pxmcc_data.curadc[out_info + curmult_idx].cur_val;
426 *pcurmult = (int32_t)(pwm_reci * (*pcurmult)) >> 16;
428 cur2 = pxmcc_data.curadc[out_info + 0].cur_val;
429 cur3 = pxmcc_data.curadc[out_info + 1].cur_val;
430 cur1 = pxmcc_data.curadc[out_info + 2].cur_val;
432 if ((phs == 5) || (phs == 0))
433 cur_alp = -(cur2 + cur3);
437 if ((phs == 5) || (phs == 0))
438 cur_bet = cur2 - cur3;
439 else if ((phs == 3) || (phs == 4))
440 cur_bet = 2 * cur2 + cur1;
442 cur_bet = -(2 * cur3 + cur1);
443 #else /*SUPPRESS_CONDITIONALS*/
446 * u1>u2 u2>u3 u1>u3 cm
447 * 0 1 1 -> 1 (1, 0, 2) 0
448 * 0 1 0 -> 2 (1, 2, 0) 2
449 * 0 0 0 -> 3 (2, 1, 0) 1
450 * 1 0 0 -> 4 (2, 0, 1) 0
451 * 1 0 1 -> 5 (0, 2, 1) 2
452 * 1 1 1 -> 0 (0, 1, 2) 1
455 uint32_t u1gtu2 = (int32_t)(pwm2 - pwm1) >> 31;
456 uint32_t u1gtu3 = (int32_t)(pwm3 - pwm1) >> 31;
457 uint32_t u2gtu3 = (int32_t)(pwm3 - pwm2) >> 31;
458 uint32_t state50_msk = u1gtu2 & u1gtu3;
459 uint32_t pwm_reci_bits;
461 curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
463 pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
466 pwm_reci_bits = __builtin_clzl(pwm_reci);
468 asm("clz %0,%1\n":"=r"(pwm_reci_bits):"r"(pwm_reci));
470 pwm_reci <<= pwm_reci_bits;
471 *FPGA_FNCAPPROX_RECI = pwm_reci;
472 asm volatile("nop\n");
473 asm volatile("nop\n");
474 asm volatile("nop\n");
475 pwm_reci = *FPGA_FNCAPPROX_RECI;
477 pwm_reci *= pxmcc_data.common.pwm_cycle;
478 pwm_reci >>= 30 - pwm_reci_bits;
481 * Translate index from pwm1, pwm2, pwm3 order to
482 * to order of current sources 0->2 1->0 2->1
484 * This solution modifies directly value in pxmcc_curadc_data_t
485 * so it is destructive and has not to be applied twice,
486 * but it is much better optimized
488 curmult_idx = (0x102 >> (curmult_idx * 4)) & 3;
489 pcurmult = &pxmcc_data.curadc[out_info + curmult_idx].cur_val;
490 *pcurmult = (int32_t)(pwm_reci * (*pcurmult)) >> 16;
492 cur2 = pxmcc_data.curadc[out_info + 0].cur_val;
493 cur3 = pxmcc_data.curadc[out_info + 1].cur_val;
494 cur1 = pxmcc_data.curadc[out_info + 2].cur_val;
496 cur_alp = -(cur2 + cur3); /* 5 0 */
497 cur_alp &= state50_msk; /* 1 2 3 4 */
498 cur_alp |= cur1 & ~state50_msk;
500 cur_bet = (-2 * cur3 - cur1) & u2gtu3; /* 1 2 */
501 cur_bet |= (2 * cur2 + cur1) & ~u2gtu3; /* 3 4 */
502 cur_bet &= ~state50_msk;
503 cur_bet |= (cur2 - cur3) & state50_msk; /* 5 0 */
505 #endif /*SUPPRESS_CONDITIONALS*/
510 cur_bet *= CONST16_K3;
513 int32_t bet_pwm = pxmcc->pwm_prew[1];
514 uint32_t bet_sgn = (bet_pwm - 1) >> 31;
515 int32_t alp_pwm = pxmcc->pwm_prew[3];
516 uint32_t alp_sgn = (alp_pwm - 1) >> 31;
517 cur_bet = (pxmcc_data.curadc[out_info + 0].cur_val & ~bet_sgn) -
518 (pxmcc_data.curadc[out_info + 1].cur_val & bet_sgn);
519 cur_alp = (pxmcc_data.curadc[out_info + 2].cur_val & ~alp_sgn) -
520 (pxmcc_data.curadc[out_info + 3].cur_val & alp_sgn);
523 cur_d = cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
524 cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
526 pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
528 pxmcc->cur_d_cum = ((pxmcc->cur_d_cum + (cur_d >> 4)) & ~0x3f) |
529 (last_rx_done_sqn & 0x1f);
530 pxmcc->cur_q_cum = ((pxmcc->cur_q_cum + (cur_q >> 4)) & ~0x3f) |
531 (last_rx_done_sqn & 0x1f);
534 } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);