hw/lx-rocon_firmware/firmware.c

   1 /*******************************************************************
   2   Components for embedded applications builded for
   3   laboratory and medical instruments firmware
   4
   5   firmware.c - multi axis motion controller comprocesor
   6                firmware for FPGA tumble CPU of lx-rocon system
   7
   8   (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
   9   (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com
  10
  11   This file can be used and copied according to next
  12   license alternatives
  13    - GPL - GNU Public License
  14    - other license provided by project originators
  15
  16  *******************************************************************/
  17
  18 #define SUPPRESS_CONDITIONALS 1
  19 #undef COMPUTE_PHASE_SECTOR
  20
  21 #define PXMCC_CURADC_CHANNELS 3
  22
  23 #include <stdint.h>
  24 #include "tumbl_addr.h"
  25
  26 /* k3 = math.sqrt(3) / 2 */
  27
  28 #define CONST16_K3   56756
  29
  30 /* 1 / k3 * 65536 */
  31
  32 #define RECI16_K3    75674
  33
  34 /* 1 / (2 * k3) * 65536 */
  35 #define RECI16_2_K3  37837
  36
  37 typedef struct pxmcc_axis_data_t {
  38   uint32_t  ccflg;
  39   uint32_t  pwm_dq;     /* D and Q components of PWM (pwm_d << 16) | (pwm_q) & 0xffff */
  40   uint32_t  cur_dq;     /* D and Q components current (cur_d << 16) | (cur_q) & 0xffff */
  41   uint32_t  ptindx;     /* index into phase table / irc in the cycle */
  42   uint32_t  ptirc;      /* IRC count per phase table */
  43   uint32_t  ptreci;     /* Reciprocal value of ptirc * 63356  */
  44   uint32_t  ptofs;      /* offset between table and IRC counter */
  45   int32_t   ptsin;
  46   int32_t   ptcos;
  47   uint32_t  ptphs;
  48   uint32_t  pwm_cycle;
  49   uint32_t  cur_d_cum;
  50   uint32_t  cur_q_cum;
  51
  52   uint32_t  act_idle;
  53   uint32_t  min_idle;
  54   uint32_t  rx_done_sqn;
  55 } pxmcc_axis_data_t;
  56
  57 typedef struct pxmcc_curadc_data_t {
  58   int16_t   cur_val;
  59   uint16_t  reserved1;
  60   uint16_t  siroladc_offs;
  61   uint16_t  siroladc_last;
  62 } pxmcc_curadc_data_t;
  63
  64 pxmcc_axis_data_t pxmcc_axis[1];
  65
  66 pxmcc_curadc_data_t pxmcc_curadc[PXMCC_CURADC_CHANNELS];
  67
  68 void init_defvals(void)
  69 {
  70 }
  71
  72 #if 0
  73
  74 uint32_t sin_lat[7];
  75
  76 void find_sin_lat(void)
  77 {
  78   int i;
  79   register uint32_t a0, a1, a2, a3, a4, a5;
  80
  81   *FPGA_FNCAPPROX_SIN = 0;
  82
  83   for (i = 0; i < 20; i++)
  84     asm volatile("": : : "memory");
  85
  86   *FPGA_FNCAPPROX_SIN = 0x40000000;
  87   a0 = *FPGA_FNCAPPROX_SIN;
  88   a1 = *FPGA_FNCAPPROX_SIN;
  89   a2 = *FPGA_FNCAPPROX_SIN;
  90   a3 = *FPGA_FNCAPPROX_SIN;
  91   a4 = *FPGA_FNCAPPROX_SIN;
  92   a5 = *FPGA_FNCAPPROX_SIN;
  93   asm volatile("": : : "memory");
  94
  95   sin_lat[0] = a0;
  96   sin_lat[1] = a1;
  97   sin_lat[2] = a2;
  98   sin_lat[3] = a3;
  99   sin_lat[4] = a4;
 100   sin_lat[5] = a5;
 101   sin_lat[6] = 0x4321;
 102   sin_lat[0] = 0x1234;
 103 }
 104
 105 #endif
 106
 107 void main(void)
 108 {
 109   uint32_t last_rx_done_sqn = 0;
 110   pxmcc_axis_data_t *pxmcc = pxmcc_axis;
 111
 112   pxmcc->ccflg = 0;
 113   pxmcc->ptindx = 0;
 114   /*pxmcc->ptofs = *FPGA_IRC0;*/
 115   pxmcc->ptirc = 1000;
 116   pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
 117   pxmcc->min_idle = 0;
 118   pxmcc->pwm_dq = 0;
 119   pxmcc->pwm_cycle = 2500;
 120   pxmcc_curadc[0].siroladc_offs = 0x0c17;
 121   pxmcc_curadc[1].siroladc_offs = 0x0c66;
 122   pxmcc_curadc[2].siroladc_offs = 0x0c66;
 123
 124   asm volatile("": : : "memory");
 125
 126  #if 0
 127   find_sin_lat();
 128  #endif
 129
 130   while (1) {
 131     uint32_t irc = *FPGA_IRC0;
 132     uint32_t ofs = pxmcc->ptofs;
 133     uint32_t per = pxmcc->ptirc;
 134     int32_t  pti;
 135     uint32_t pta;
 136     uint32_t dummy;
 137
 138     pti = irc - ofs;
 139     if ((uint32_t)pti >= per) {
 140       if (pti < 0) {
 141         ofs -= per;
 142       } else {
 143         ofs += per;
 144       }
 145       pti = irc - ofs;
 146       pxmcc->ptofs = ofs;
 147     }
 148     pxmcc->ptindx = pti;
 149
 150     pta = pti * pxmcc->ptreci;
 151
 152     *FPGA_FNCAPPROX_SIN = pta;
 153
 154     dummy = *FPGA_FNCAPPROX_SIN;
 155     dummy = *FPGA_FNCAPPROX_SIN;
 156     dummy = *FPGA_FNCAPPROX_SIN;
 157     pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
 158     dummy = *FPGA_FNCAPPROX_COS;
 159     pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
 160
 161     if (pxmcc->ccflg) {
 162       int32_t pwm_alp;
 163       int32_t pwm_bet;
 164       int32_t pwm_bet_div_2_k3;
 165       uint32_t pwm1;
 166       uint32_t pwm2;
 167       uint32_t pwm3;
 168       int32_t pwm_d;
 169       int32_t pwm_q;
 170     #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
 171       uint32_t phs;
 172      #endif /*COMPUTE_PHASE_SECTOR*/
 173
 174       pwm_d = (volatile uint32_t)pxmcc->pwm_dq;
 175       pwm_q = (pwm_d << 16) >> 16;
 176       pwm_d >>= 16;
 177
 178       pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
 179       pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
 180
 181       pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
 182
 183      #ifndef SUPPRESS_CONDITIONALS
 184       if (pwm_bet > 0)
 185         if (pwm_alp > 0)
 186           /* pwm_bet > 2 * k3 * pwm_alp */
 187           if (pwm_bet_div_2_k3 > pwm_alp)
 188             phs = 1;
 189           else
 190             phs = 0;
 191         else
 192           /* -pwm_bet > 2 * k3 * pwm_alp */
 193           if (pwm_bet_div_2_k3 < -pwm_alp)
 194             phs = 2;
 195           else
 196             phs = 1;
 197       else
 198         if (pwm_alp > 0)
 199           /* pwm_bet > -2 * k3 * pwm_alp */
 200           if (pwm_bet_div_2_k3 > -pwm_alp)
 201             phs = 5;
 202           else
 203             phs = 4;
 204         else
 205           /* pwm_bet > 2 * k3 * u_alp */
 206           if (pwm_bet_div_2_k3 > pwm_alp)
 207             phs = 3;
 208           else
 209             phs = 4;
 210
 211       if (phs <= 1) {
 212         /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */
 213         pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16;
 214         /* pwm2 = 1/k3 * pwm_bet */
 215         pwm2 = pwm_bet_div_2_k3 >> 15;
 216         pwm3 = 0;
 217       } else if (phs <= 3) {
 218         pwm1 = 0;
 219         /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */
 220         pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16;
 221         /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */
 222         pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16;
 223       } else {
 224         /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
 225         pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
 226         pwm2 = 0;
 227         /* pwm3 = -1/k3 * pwm_bet */
 228         pwm3 = -pwm_bet_div_2_k3 >> 15;
 229       }
 230      #else /*SUPPRESS_CONDITIONALS*/
 231       {
 232         int32_t alp_m_bet_d2k3;
 233         uint32_t state23_msk;
 234         uint32_t pwm23_shift;
 235         uint32_t bet_sgn = pwm_bet >> 31;
 236         uint32_t alp_sgn = pwm_alp >> 31;
 237        #ifdef COMPUTE_PHASE_SECTOR
 238         uint32_t bet_sgn_cpl = ~bet_sgn;
 239        #endif /*COMPUTE_PHASE_SECTOR*/
 240
 241         alp_m_bet_d2k3 = (alp_sgn ^ pwm_alp) - (bet_sgn ^ (pwm_bet_div_2_k3 + bet_sgn));
 242         alp_m_bet_d2k3 = alp_m_bet_d2k3 >> 31;
 243
 244         state23_msk = alp_sgn & ~alp_m_bet_d2k3;
 245
 246         /*
 247          *   bet alp amb s23
 248          *    0   0   0   0 -> 0 (000)
 249          *    0   0  -1   0 -> 1 (001)
 250          *   -1   0  -1   0 -> 1 (001)
 251          *   -1   0   0  -1 -> 2 (010)
 252          *   -1  -1   0  -1 -> 3 (011)
 253          *   -1  -1  -1   0 -> 4 (100)
 254          *    0  -1  -1   0 -> 4 (100)
 255          *    0  -1   0   0 -> 5 (101)
 256          */
 257
 258        #ifdef COMPUTE_PHASE_SECTOR
 259         phs = (bet_sgn & 5) + (bet_sgn_cpl ^ ((alp_m_bet_d2k3 + 2 * state23_msk) + bet_sgn_cpl));
 260        #endif /*COMPUTE_PHASE_SECTOR*/
 261
 262         pwm1 = pwm_alp & state23_msk;
 263         pwm2 = pwm_bet_div_2_k3 - pwm1;
 264         pwm3 = -pwm_bet_div_2_k3 - pwm1;
 265         pwm2 &= (~bet_sgn | state23_msk);
 266         pwm3 &= (bet_sgn | state23_msk);
 267         pwm1 = pwm_alp + pwm2 + pwm3;
 268         pwm1 &= ~state23_msk;
 269         pwm1 >>= 16;
 270         pwm23_shift = 15 - state23_msk;
 271         pwm2 >>= pwm23_shift;
 272         pwm3 >>= pwm23_shift;
 273       }
 274      #endif /*SUPPRESS_CONDITIONALS*/
 275
 276      #ifdef COMPUTE_PHASE_SECTOR
 277       pxmcc->ptphs = phs;
 278      #endif /*COMPUTE_PHASE_SECTOR*/
 279
 280       asm volatile("": : : "memory");
 281
 282       *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
 283       *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
 284       *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
 285     }
 286
 287     asm volatile("": : : "memory");
 288
 289     {
 290       uint32_t idlecnt = 0;
 291       uint32_t sqn;
 292       do {
 293         sqn = *FPGA_LX_MASTER_RX_DDIV;
 294         idlecnt++;
 295       } while (sqn == last_rx_done_sqn);
 296       pxmcc->act_idle = idlecnt;
 297       if (((idlecnt < pxmcc->min_idle) ||
 298           (pxmcc->min_idle == 0)) &&
 299           last_rx_done_sqn) {
 300         pxmcc->min_idle = idlecnt;
 301       }
 302       last_rx_done_sqn = sqn;
 303       pxmcc->rx_done_sqn = last_rx_done_sqn;
 304       asm volatile("": : : "memory");
 305     }
 306     {
 307       int i;
 308       pxmcc_curadc_data_t *curadc = pxmcc_curadc;
 309       volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
 310       uint16_t val;
 311
 312       for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
 313         val = *siroladc;
 314
 315         curadc->cur_val = val - curadc->siroladc_last
 316                               - curadc->siroladc_offs;
 317         curadc->siroladc_last = val;
 318         curadc->reserved1=0x11aa;
 319
 320         i++;
 321         curadc += 1;
 322         siroladc += 2;
 323         /* additional 3 required for 7 -> 8 change */
 324         if (!(i & 7))
 325           siroladc += 3;
 326       }
 327     }
 328     {
 329       int32_t  cur_alp;
 330       int32_t  cur_bet;
 331       int32_t  cur_d;
 332       int32_t  cur_q;
 333       uint32_t pwm[3];
 334       uint32_t pwm1;
 335       uint32_t pwm2;
 336       uint32_t pwm3;
 337       int32_t  cur[3];
 338       int32_t  cur1;
 339       int32_t  cur2;
 340       int32_t  cur3;
 341       uint32_t curmult_idx;
 342       uint32_t pwm_reci;
 343      #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
 344       uint32_t phs;
 345      #ifdef COMPUTE_PHASE_SECTOR
 346       phs = pxmcc->ptphs;
 347      #endif /*COMPUTE_PHASE_SECTOR*/
 348      #endif /*COMPUTE_PHASE_SECTOR*/
 349
 350       pwm[1] = *FPGA_LX_MASTER_TX_PWM0 & 0x3fff;
 351       pwm[2] = *FPGA_LX_MASTER_TX_PWM1 & 0x3fff;
 352       pwm[0] = *FPGA_LX_MASTER_TX_PWM2 & 0x3fff;
 353
 354       cur[1] = pxmcc_curadc[0].cur_val;
 355       cur[2] = pxmcc_curadc[1].cur_val;
 356       cur[0] = pxmcc_curadc[2].cur_val;
 357
 358       pwm1 = pwm[0];
 359       pwm2 = pwm[1];
 360       pwm3 = pwm[2];
 361
 362      #ifndef SUPPRESS_CONDITIONALS
 363       #ifndef COMPUTE_PHASE_SECTOR
 364       if (pwm1 > pwm2)
 365         if (pwm2 > pwm3)
 366           phs = 0;
 367         else if (pwm1 > pwm3)
 368           phs = 5;
 369         else
 370           phs = 4;
 371       else
 372         if (pwm2 < pwm3)
 373           phs = 3;
 374         else if (pwm1 < pwm3)
 375           phs = 2;
 376         else
 377           phs = 1;
 378       #endif /*COMPUTE_PHASE_SECTOR*/
 379
 380       curmult_idx = (0x00102102 >> (4 * phs)) & 3;
 381       pwm_reci = pxmcc->pwm_cycle - pwm[curmult_idx];
 382       pwm_reci = (pxmcc->pwm_cycle << 16) / pwm_reci;
 383       cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
 384
 385       cur1 = cur[0];
 386       cur2 = cur[1];
 387       cur3 = cur[2];
 388
 389       if ((phs == 5) || (phs == 0))
 390         cur_alp = -(cur2 + cur3);
 391       else
 392         cur_alp = cur1;
 393
 394       if ((phs == 5) || (phs == 0))
 395         cur_bet = cur2 - cur3;
 396       else if ((phs == 3) || (phs == 4))
 397         cur_bet = 2 * cur2 + cur1;
 398       else /* 1 2 */
 399         cur_bet = -(2 * cur3 + cur1);
 400      #else /*SUPPRESS_CONDITIONALS*/
 401       {
 402         /*
 403          *   u1>u2 u2>u3 u1>u3               cm
 404          *     0     1     1 ->  1 (1, 0, 2) 2
 405          *     0     1     0 ->  2 (1, 2, 0) 0
 406          *     0     0     0 ->  3 (2, 1, 0) 1
 407          *     1     0     0 ->  4 (2, 0, 1) 2
 408          *     1     0     1 ->  5 (0, 2, 1) 0
 409          *     1     1     1 ->  0 (0, 1, 2) 1
 410          */
 411
 412         uint32_t u1gtu2 = (int32_t)(pwm2 - pwm1) >> 31;
 413         uint32_t u1gtu3 = (int32_t)(pwm3 - pwm1) >> 31;
 414         uint32_t u2gtu3 = (int32_t)(pwm3 - pwm2) >> 31;
 415         uint32_t state50_msk = u1gtu2 & u1gtu3;
 416         uint32_t pwm_reci_bits;
 417
 418         curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
 419         pwm_reci = pxmcc->pwm_cycle - pwm[curmult_idx];
 420        #if 0
 421         pwm_reci_bits = __builtin_clzl(pwm_reci);
 422        #else
 423         asm("clz %0,%1\n":"=r"(pwm_reci_bits):"r"(pwm_reci));
 424        #endif
 425         pwm_reci <<= pwm_reci_bits;
 426         *FPGA_FNCAPPROX_RECI = pwm_reci;
 427         dummy = *FPGA_FNCAPPROX_RECI;
 428         dummy = *FPGA_FNCAPPROX_RECI;
 429         dummy = *FPGA_FNCAPPROX_RECI;
 430         pwm_reci = *FPGA_FNCAPPROX_RECI;
 431         pwm_reci >>= 16;
 432         pwm_reci *= pxmcc->pwm_cycle;
 433         pwm_reci >>= 30 - pwm_reci_bits;
 434         cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
 435
 436         cur1 = cur[0];
 437         cur2 = cur[1];
 438         cur3 = cur[2];
 439
 440         cur_alp = -(cur2 + cur3);                /* 5 0 */
 441         cur_alp &= state50_msk;                  /* 1 2 3 4 */
 442         cur_alp |= cur1 & ~state50_msk;
 443
 444         cur_bet = (-2 * cur3 - cur1) & u2gtu3;   /* 1 2 */
 445         cur_bet |= (2 * cur2 + cur1) & ~u2gtu3;  /* 3 4 */
 446         cur_bet &= ~state50_msk;
 447         cur_bet |= (cur2 - cur3) & state50_msk;  /* 5 0 */
 448       }
 449      #endif /*SUPPRESS_CONDITIONALS*/
 450
 451       cur_alp *= 3;
 452       cur_alp >>= 1;
 453
 454       cur_bet *= CONST16_K3;
 455       cur_bet >>= 16;
 456
 457       cur_d =  cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
 458       cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
 459
 460       pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
 461
 462       pxmcc->cur_d_cum = ((pxmcc->cur_d_cum + cur_d) & ~0xff) |
 463                          (last_rx_done_sqn & 0x1f);
 464       pxmcc->cur_q_cum = ((pxmcc->cur_q_cum + cur_q) & ~0xff) |
 465                          (last_rx_done_sqn & 0x1f);
 466
 467     }
 468   }
 469 }