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