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 #include <stdint.h>
  22 #include "pxmcc_types.h"
  23 #include "tumbl_addr.h"
  24
  25 /* k3 = math.sqrt(3) / 2 */
  26
  27 #define CONST16_K3   56756
  28
  29 /* 1 / k3 * 65536 */
  30
  31 #define RECI16_K3    75674
  32
  33 /* 1 / (2 * k3) * 65536 */
  34 #define RECI16_2_K3  37837
  35
  36 pxmcc_data_t pxmcc_data;
  37
  38 void init_defvals(void)
  39 {
  40 }
  41
  42 #if 0
  43
  44 uint32_t sin_lat[7];
  45
  46 void find_sin_lat(void)
  47 {
  48   int i;
  49   register uint32_t a0, a1, a2, a3, a4, a5;
  50
  51   *FPGA_FNCAPPROX_SIN = 0;
  52
  53   for (i = 0; i < 20; i++)
  54     asm volatile("": : : "memory");
  55
  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");
  64
  65   sin_lat[0] = a0;
  66   sin_lat[1] = a1;
  67   sin_lat[2] = a2;
  68   sin_lat[3] = a3;
  69   sin_lat[4] = a4;
  70   sin_lat[5] = a5;
  71   sin_lat[6] = 0x4321;
  72   sin_lat[0] = 0x1234;
  73 }
  74
  75 #endif
  76
  77 void main(void)
  78 {
  79   uint32_t last_rx_done_sqn = 0;
  80   pxmcc_axis_data_t *pxmcc;
  81
  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;
  86   pxmcc->ccflg = 0;
  87   pxmcc->ptindx = 0;
  88   /*pxmcc->ptofs = *FPGA_IRC0;*/
  89   pxmcc->ptirc = 1000;
  90   pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
  91   pxmcc->pwm_dq = 0;
  92   pxmcc_data.curadc[0].siroladc_offs = 0x0c17;
  93   pxmcc_data.curadc[1].siroladc_offs = 0x0c66;
  94   pxmcc_data.curadc[2].siroladc_offs = 0x0c66;
  95
  96   asm volatile("": : : "memory");
  97
  98  #if 0
  99   find_sin_lat();
 100  #endif
 101
 102   while (1) {
 103     pxmcc = pxmcc_data.axis;
 104     do {
 105       uint32_t irc;
 106       uint32_t ofs = pxmcc->ptofs;
 107       uint32_t per = pxmcc->ptirc;
 108       int32_t  pti;
 109       uint32_t pta;
 110       uint32_t pwmtx_info;
 111       volatile uint32_t *uptr;
 112
 113       irc = *(FPGA_IRC0 + (FPGA_IRC1 - FPGA_IRC0) * pxmcc->inp_info);
 114
 115       pti = irc - ofs;
 116       if ((uint32_t)pti >= per) {
 117         if (pti < 0) {
 118           ofs -= per;
 119         } else {
 120           ofs += per;
 121         }
 122         pti = irc - ofs;
 123         pxmcc->ptofs = ofs;
 124       }
 125       pxmcc->ptindx = pti;
 126
 127       pta = pti * pxmcc->ptreci;
 128
 129       *FPGA_FNCAPPROX_SIN = pta;
 130       asm volatile("nop\n": : : "memory");
 131       asm volatile("nop\n": : : "memory");
 132       asm volatile("nop\n": : : "memory");
 133       pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
 134       asm volatile("nop\n": : : "memory");
 135       pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
 136
 137       if (pxmcc->ccflg) {
 138         int32_t pwm_alp;
 139         int32_t pwm_bet;
 140         int32_t pwm_bet_div_2_k3;
 141         uint32_t pwm1;
 142         uint32_t pwm2;
 143         uint32_t pwm3;
 144         uint32_t pwm4;
 145         int32_t pwm_d;
 146         int32_t pwm_q;
 147        #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
 148         uint32_t phs;
 149        #endif /*COMPUTE_PHASE_SECTOR*/
 150
 151         pwm_d = (volatile uint32_t)pxmcc->pwm_dq;
 152         pwm_q = (pwm_d << 16) >> 16;
 153         pwm_d >>= 16;
 154
 155         pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
 156         pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
 157
 158         if (!pxmcc->mode) {
 159           pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
 160
 161          #ifndef SUPPRESS_CONDITIONALS
 162           if (pwm_bet > 0)
 163             if (pwm_alp > 0)
 164             /* pwm_bet > 2 * k3 * pwm_alp */
 165               if (pwm_bet_div_2_k3 > pwm_alp)
 166                 phs = 1;
 167               else
 168                 phs = 0;
 169             else
 170               /* -pwm_bet > 2 * k3 * pwm_alp */
 171               if (pwm_bet_div_2_k3 < -pwm_alp)
 172                 phs = 2;
 173               else
 174                 phs = 1;
 175           else
 176             if (pwm_alp > 0)
 177               /* pwm_bet > -2 * k3 * pwm_alp */
 178               if (pwm_bet_div_2_k3 > -pwm_alp)
 179                 phs = 5;
 180               else
 181                 phs = 4;
 182             else
 183               /* pwm_bet > 2 * k3 * u_alp */
 184               if (pwm_bet_div_2_k3 > pwm_alp)
 185                 phs = 3;
 186               else
 187                 phs = 4;
 188
 189           if (phs <= 1) {
 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;
 194             pwm3 = 0;
 195           } else if (phs <= 3) {
 196             pwm1 = 0;
 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;
 201           } else {
 202             /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
 203             pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
 204             pwm2 = 0;
 205             /* pwm3 = -1/k3 * pwm_bet */
 206             pwm3 = -pwm_bet_div_2_k3 >> 15;
 207           }
 208          #else /*SUPPRESS_CONDITIONALS*/
 209           {
 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*/
 218
 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;
 221
 222             state23_msk = alp_sgn & ~alp_m_bet_d2k3;
 223
 224             /*
 225              *   bet alp amb s23
 226              *    0   0   0   0 -> 0 (000)
 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)
 234              */
 235
 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*/
 239
 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;
 247             pwm1 >>= 16;
 248             pwm23_shift = 15 - state23_msk;
 249             pwm2 >>= pwm23_shift;
 250             pwm3 >>= pwm23_shift;
 251           }
 252          #endif /*SUPPRESS_CONDITIONALS*/
 253
 254          #ifdef COMPUTE_PHASE_SECTOR
 255           pxmcc->ptphs = phs;
 256          #endif /*COMPUTE_PHASE_SECTOR*/
 257
 258          #if 0
 259           *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
 260           *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
 261           *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
 262          #else
 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;
 273          #endif
 274         } else {
 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;
 279           pwm1 &= ~bet_sgn;
 280           pwm2 &= bet_sgn;
 281           pwm3 &= ~alp_sgn;
 282           pwm4 &= alp_sgn;
 283
 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;
 297         }
 298       } else {
 299         if (!pxmcc->mode) {
 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;
 307         } else {
 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;
 317         }
 318       }
 319       pxmcc++;
 320
 321     } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
 322
 323     asm volatile("": : : "memory");
 324
 325     {
 326       uint32_t idlecnt = 0;
 327       uint32_t sqn;
 328       do {
 329         sqn = *FPGA_LX_MASTER_RX_DDIV;
 330         idlecnt++;
 331       } while (sqn == last_rx_done_sqn);
 332       pxmcc_data.common.act_idle = idlecnt;
 333       if ((idlecnt < pxmcc_data.common.min_idle) &&
 334           last_rx_done_sqn) {
 335         pxmcc_data.common.min_idle = idlecnt;
 336       }
 337       last_rx_done_sqn = sqn;
 338       pxmcc_data.common.rx_done_sqn = last_rx_done_sqn;
 339       asm volatile("": : : "memory");
 340     }
 341
 342     {
 343       int i;
 344       pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
 345       volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
 346       uint16_t val;
 347
 348       for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
 349         val = *siroladc;
 350
 351         curadc->cur_val = val - curadc->siroladc_last
 352                               - curadc->siroladc_offs;
 353         curadc->siroladc_last = val;
 354         curadc->reserved1=0x11aa;
 355
 356         i++;
 357         curadc += 1;
 358         siroladc += 2;
 359         /* additional 3 required for 7 -> 8 change */
 360         if (!(i & 7))
 361           siroladc += 3;
 362       }
 363     }
 364
 365     pxmcc = pxmcc_data.axis;
 366     do {
 367       int32_t  cur_alp;
 368       int32_t  cur_bet;
 369       int32_t  cur_d;
 370       int32_t  cur_q;
 371       uint32_t pwm1;
 372       uint32_t pwm2;
 373       uint32_t pwm3;
 374       int32_t  cur[3];
 375       int32_t  cur1;
 376       int32_t  cur2;
 377       int32_t  cur3;
 378       uint32_t curmult_idx;
 379       uint32_t pwm_reci;
 380       uint32_t out_info;
 381      #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
 382       uint32_t phs;
 383      #ifdef COMPUTE_PHASE_SECTOR
 384       phs = pxmcc->ptphs;
 385      #endif /*COMPUTE_PHASE_SECTOR*/
 386      #endif /*COMPUTE_PHASE_SECTOR*/
 387
 388       out_info = pxmcc->out_info;
 389       if (!pxmcc->mode) {
 390         cur[1] = pxmcc_data.curadc[out_info + 0].cur_val;
 391         cur[2] = pxmcc_data.curadc[out_info + 1].cur_val;
 392         cur[0] = pxmcc_data.curadc[out_info + 2].cur_val;
 393
 394         pwm1 = pxmcc->pwm_prew[0];
 395         pwm2 = pxmcc->pwm_prew[1];
 396         pwm3 = pxmcc->pwm_prew[2];
 397
 398        #ifndef SUPPRESS_CONDITIONALS
 399         #ifndef COMPUTE_PHASE_SECTOR
 400         if (pwm1 > pwm2)
 401           if (pwm2 > pwm3)
 402             phs = 0;
 403           else if (pwm1 > pwm3)
 404             phs = 5;
 405           else
 406             phs = 4;
 407         else
 408           if (pwm2 < pwm3)
 409             phs = 3;
 410           else if (pwm1 < pwm3)
 411             phs = 2;
 412           else
 413             phs = 1;
 414         #endif /*COMPUTE_PHASE_SECTOR*/
 415
 416         curmult_idx = (0x00201201 >> (4 * phs)) & 3;
 417         pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
 418         pwm_reci = (pxmcc_data.common.pwm_cycle << 16) / pwm_reci;
 419         cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
 420
 421         cur1 = cur[0];
 422         cur2 = cur[1];
 423         cur3 = cur[2];
 424
 425         if ((phs == 5) || (phs == 0))
 426           cur_alp = -(cur2 + cur3);
 427         else
 428           cur_alp = cur1;
 429
 430         if ((phs == 5) || (phs == 0))
 431           cur_bet = cur2 - cur3;
 432         else if ((phs == 3) || (phs == 4))
 433           cur_bet = 2 * cur2 + cur1;
 434         else /* 1 2 */
 435           cur_bet = -(2 * cur3 + cur1);
 436        #else /*SUPPRESS_CONDITIONALS*/
 437         {
 438           /*
 439            *   u1>u2 u2>u3 u1>u3               cm
 440            *     0     1     1 ->  1 (1, 0, 2) 0
 441            *     0     1     0 ->  2 (1, 2, 0) 2
 442            *     0     0     0 ->  3 (2, 1, 0) 1
 443            *     1     0     0 ->  4 (2, 0, 1) 0
 444            *     1     0     1 ->  5 (0, 2, 1) 2
 445            *     1     1     1 ->  0 (0, 1, 2) 1
 446            */
 447
 448           uint32_t u1gtu2 = (int32_t)(pwm2 - pwm1) >> 31;
 449           uint32_t u1gtu3 = (int32_t)(pwm3 - pwm1) >> 31;
 450           uint32_t u2gtu3 = (int32_t)(pwm3 - pwm2) >> 31;
 451           uint32_t state50_msk = u1gtu2 & u1gtu3;
 452           uint32_t pwm_reci_bits;
 453
 454           curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
 455           pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
 456          #if 0
 457           pwm_reci_bits = __builtin_clzl(pwm_reci);
 458          #else
 459           asm("clz %0,%1\n":"=r"(pwm_reci_bits):"r"(pwm_reci));
 460          #endif
 461           pwm_reci <<= pwm_reci_bits;
 462           *FPGA_FNCAPPROX_RECI = pwm_reci;
 463           asm volatile("nop\n": : : "memory");
 464           asm volatile("nop\n": : : "memory");
 465           asm volatile("nop\n": : : "memory");
 466           pwm_reci = *FPGA_FNCAPPROX_RECI;
 467           pwm_reci >>= 16;
 468           pwm_reci *= pxmcc_data.common.pwm_cycle;
 469           pwm_reci >>= 30 - pwm_reci_bits;
 470           cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
 471
 472           cur1 = cur[0];
 473           cur2 = cur[1];
 474           cur3 = cur[2];
 475
 476           cur_alp = -(cur2 + cur3);                /* 5 0 */
 477           cur_alp &= state50_msk;                  /* 1 2 3 4 */
 478           cur_alp |= cur1 & ~state50_msk;
 479
 480           cur_bet = (-2 * cur3 - cur1) & u2gtu3;   /* 1 2 */
 481           cur_bet |= (2 * cur2 + cur1) & ~u2gtu3;  /* 3 4 */
 482           cur_bet &= ~state50_msk;
 483           cur_bet |= (cur2 - cur3) & state50_msk;  /* 5 0 */
 484         }
 485        #endif /*SUPPRESS_CONDITIONALS*/
 486
 487         cur_alp *= 3;
 488         cur_alp >>= 1;
 489
 490         cur_bet *= CONST16_K3;
 491         cur_bet >>= 16;
 492       } else {
 493         int32_t bet_pwm = pxmcc->pwm_prew[1];
 494         uint32_t bet_sgn = (bet_pwm - 1) >> 31;
 495         int32_t alp_pwm = pxmcc->pwm_prew[3];
 496         uint32_t alp_sgn = (alp_pwm - 1) >> 31;
 497         cur_bet = (pxmcc_data.curadc[out_info + 0].cur_val & ~bet_sgn) -
 498                   (pxmcc_data.curadc[out_info + 1].cur_val & bet_sgn);
 499         cur_alp = (pxmcc_data.curadc[out_info + 2].cur_val & ~alp_sgn) -
 500                   (pxmcc_data.curadc[out_info + 3].cur_val & alp_sgn);
 501       }
 502
 503       cur_d =  cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
 504       cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
 505
 506       pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
 507
 508       pxmcc->cur_d_cum = ((pxmcc->cur_d_cum + (cur_d >> 4)) & ~0x3f) |
 509                          (last_rx_done_sqn & 0x1f);
 510       pxmcc->cur_q_cum = ((pxmcc->cur_q_cum + (cur_q >> 4)) & ~0x3f) |
 511                          (last_rx_done_sqn & 0x1f);
 512
 513       pxmcc++;
 514     } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
 515   }
 516 }