sw/app/rocon/appl_pxmc.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components.
   3
   4   appl_pxmc.c - position controller subsystem core generic
   5                 and LP_MPW1 hardware specific support
   6
   7   Copyright (C) 2001-2013 by Pavel Pisa - originator
   8                           pisa@cmp.felk.cvut.cz
   9             (C) 2001-2013 by PiKRON Ltd. - originator
  10                     http://www.pikron.com
  11
  12   This file can be used and copied according to next
  13   license alternatives
  14    - GPL - GNU Public License
  15    - other license provided by project originators
  16
  17  *******************************************************************/
  18
  19 #include <cpu_def.h>
  20 #include <system_def.h>
  21 #include <pxmc.h>
  22 #include <pxmc_lpc_qei.h>
  23 #include <pxmc_internal.h>
  24 #include <pxmc_inp_common.h>
  25 #include <pxmc_gen_info.h>
  26 #include <hal_gpio.h>
  27 #include <hal_machperiph.h>
  28 #include <stdlib.h>
  29
  30 #include "appl_defs.h"
  31 #include "appl_fpga.h"
  32
  33 #define PXML_MAIN_CNT 8
  34
  35 #define PXMC_WITH_PT_ZIC 1
  36 #define PXMC_PT_ZIC_MASK 0x8000
  37
  38 #define LPCPWM1_FREQ 20000
  39
  40 #define HAL_ERR_SENSITIVITY 20
  41 #define HAL_ERR_MAX_COUNT    5
  42
  43 unsigned pxmc_lpcpwm1_magnitude;
  44
  45 long pxmc_irc_offset[PXML_MAIN_CNT];
  46
  47 int
  48 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
  49 {
  50   int chan=mcs->pxms_inp_info;
  51   long irc;
  52   long pos;
  53
  54   /* read position from hardware */
  55   irc = fpga_irc[chan]->count;
  56   irc += pxmc_irc_offset[chan];
  57   pos = irc << PXMC_SUBDIV(mcs);
  58   mcs->pxms_as = pos - mcs->pxms_ap;
  59   mcs->pxms_ap = pos;
  60
  61   /* Running of the motor commutator */
  62   if (mcs->pxms_flg & PXMS_PTI_m)
  63     pxmc_irc_16bit_commindx(mcs, irc);
  64
  65   return 0;
  66 }
  67
  68 int
  69 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
  70 {
  71   int chan=mcs->pxms_inp_info;
  72   long irc;
  73   long pos_diff;
  74
  75   irc = fpga_irc[chan]->count;
  76   pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
  77   pxmc_irc_offset[chan] = pos_diff >> PXMC_SUBDIV(mcs);
  78   return 0;
  79 }
  80
  81 inline unsigned
  82 pxmc_rocon_bdc_hal_rd(pxmc_state_t *mcs)
  83 {
  84   unsigned h = 0;
  85   /* FIXME */
  86   /* return 3 bits corresponding to the HAL senzor input */
  87   return h;
  88 }
  89
  90 #if 1
  91 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
  92 {
  93   [0] = 0xff,
  94   [7] = 0xff,
  95   [1] = 0, /*0*/
  96   [5] = 1, /*1*/
  97   [4] = 2, /*2*/
  98   [6] = 3, /*3*/
  99   [2] = 4, /*4*/
 100   [3] = 5, /*5*/
 101 };
 102 #else
 103 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 104 {
 105   [0] = 0xff,
 106   [7] = 0xff,
 107   [1] = 0, /*0*/
 108   [5] = 5, /*1*/
 109   [4] = 4, /*2*/
 110   [6] = 3, /*3*/
 111   [2] = 2, /*4*/
 112   [3] = 1, /*5*/
 113 };
 114 #endif
 115
 116 /**
 117  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 118  * @mcs:  Motion controller state information
 119  */
 120 /*static*/ inline void
 121 pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
 122 {
 123   unsigned long out_info = mcs->pxms_out_info;
 124
 125   /* FIXME */
 126 }
 127
 128 static inline void
 129 pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 130 {
 131   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
 132   {
 133     pxmc_set_errno(mcs, PXMS_E_HAL);
 134     mcs->pxms_ene = 0;
 135     mcs->pxms_halerc--;
 136   }
 137   else
 138     mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
 139 }
 140
 141 /**
 142  * pxmc_rocon_pwm_out - Phase output for brush-less 3-phase motor
 143  * @mcs:  Motion controller state information
 144  */
 145 int
 146 pxmc_rocon_pwm_out(pxmc_state_t *mcs)
 147 {
 148   unsigned char hal_pos;
 149   short pwm1;
 150   short pwm2;
 151   short pwm3;
 152   int indx = 0;
 153   short ene;
 154   int wind_current[4];
 155
 156   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 157       (mcs->pxms_flg & PXMS_PRA_m))
 158   {
 159     short ptindx;
 160     short ptirc = mcs->pxms_ptirc;
 161     short divisor = mcs->pxms_ptper * 6;
 162
 163     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bdc_hal_rd(mcs)];
 164
 165     if (hal_pos == 0xff)
 166     {
 167       if (mcs->pxms_ene)
 168         pxmc_rocon_process_hal_error(mcs);
 169     }
 170     else
 171     {
 172       if (mcs->pxms_halerc)
 173         mcs->pxms_halerc--;
 174
 175       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 176
 177       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
 178       {
 179         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
 180         {
 181           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 182
 183           if ((ptindx > ptindx_prev + ptirc / 2) ||
 184               (ptindx_prev > ptindx + ptirc / 2))
 185           {
 186             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 187
 188             if (ptindx < 0)
 189               ptindx += ptirc;
 190           }
 191           else
 192           {
 193             ptindx = (ptindx_prev + ptindx) / 2;
 194           }
 195
 196           mcs->pxms_ptindx = ptindx;
 197
 198           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 199
 200           pxmc_set_flag(mcs, PXMS_PTI_b);
 201           pxmc_clear_flag(mcs, PXMS_PRA_b);
 202
 203           /* if phase table position to mask is know do fine phase table alignment */
 204           if (mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 205             lpc_qei_setup_index_catch(&lpc_qei_state);
 206
 207         }
 208         else
 209         {
 210           if (!(mcs->pxms_flg & PXMS_PTI_m))
 211             mcs->pxms_ptindx = ptindx;
 212         }
 213       }
 214
 215       mcs->pxms_hal = hal_pos;
 216     }
 217   }
 218
 219   {
 220     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 221     /* FIXME - check winding current against limit */
 222     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 223   }
 224
 225   ene = mcs->pxms_ene;
 226
 227   if (ene)
 228   {
 229     indx = mcs->pxms_ptindx;
 230 #if 1
 231     /* tuning of magnetic field/voltage advance angle */
 232     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 233 #endif
 234
 235     if (ene < 0)
 236     {
 237       /* Generating direction of stator mag. field for backward torque */
 238       ene = -ene;
 239
 240       if ((indx -= mcs->pxms_ptvang) < 0)
 241         indx += mcs->pxms_ptirc;
 242     }
 243     else
 244     {
 245       /* Generating direction of stator mag. field for forward torque */
 246       if ((indx += mcs->pxms_ptvang) >= mcs->pxms_ptirc)
 247         indx -= mcs->pxms_ptirc;
 248     }
 249
 250     if (mcs->pxms_ptscale_mult)
 251       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 252
 253     pwm1 = mcs->pxms_ptptr1[indx];
 254     pwm2 = mcs->pxms_ptptr2[indx];
 255     pwm3 = mcs->pxms_ptptr3[indx];
 256
 257 #ifdef PXMC_WITH_PT_ZIC
 258
 259     if (labs(mcs->pxms_as) > (10 << PXMC_SUBDIV(mcs)))
 260     {
 261       if (pwm1 & PXMC_PT_ZIC_MASK)
 262       {
 263         /*hal_gpio_set_value(PWM2_EN_PIN,0);*/
 264       }
 265       else
 266       {
 267         /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 268       }
 269
 270       if (pwm2 & PXMC_PT_ZIC_MASK)
 271       {
 272         /*hal_gpio_set_value(PWM4_EN_PIN,0);*/
 273       }
 274       else
 275       {
 276         /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 277       }
 278
 279       if (pwm3 & PXMC_PT_ZIC_MASK)
 280       {
 281         /*hal_gpio_set_value(PWM6_EN_PIN,0);*/
 282       }
 283       else
 284       {
 285         /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 286       }
 287     }
 288     else
 289     {
 290       /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 291       /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 292       /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 293     }
 294
 295     pwm1 &= ~PXMC_PT_ZIC_MASK;
 296     pwm2 &= ~PXMC_PT_ZIC_MASK;
 297     pwm3 &= ~PXMC_PT_ZIC_MASK;
 298 #endif /*PXMC_WITH_PT_ZIC*/
 299
 300     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 301     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 302     {
 303       unsigned long pwm_dc = pxmc_lpcpwm1_magnitude * (unsigned long)ene;
 304       pwm1 = ((unsigned long long)pwm1 * pwm_dc) >> (15 + 15);
 305       pwm2 = ((unsigned long long)pwm2 * pwm_dc) >> (15 + 15);
 306       pwm3 = ((unsigned long long)pwm3 * pwm_dc) >> (15 + 15);
 307     }
 308     pxmc_rocon_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
 309   }
 310   else
 311   {
 312     /*pxmc_lpc_wind_current_over_cnt=0;*/
 313     pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 314 #ifdef PXMC_WITH_PT_ZIC
 315     /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 316     /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 317     /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 318 #endif /*PXMC_WITH_PT_ZIC*/
 319   }
 320
 321   return 0;
 322 }
 323
 324 uint32_t pxmc_rocon_pwm_dummy_reg;
 325
 326 static inline volatile uint32_t *
 327 pxmc_rocon_pwm_chan2reg(unsigned chan)
 328 {
 329   volatile uint32_t *pwm_reg;
 330
 331   if (chan >= 16)
 332     return &pxmc_rocon_pwm_dummy_reg;
 333
 334   pwm_reg = fpga_lx_master_transmitter_base;
 335   pwm_reg += 1 + (chan >> 8) + chan;
 336   return pwm_reg;
 337 }
 338
 339
 340 int
 341 pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 342 {
 343   volatile uint32_t *pwm_reg_a, *pwm_reg_b;
 344   int chan = mcs->pxms_out_info;
 345   int ene = mcs->pxms_ene;
 346
 347   if (chan > 6)
 348     return 0;
 349
 350   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
 351   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 352
 353   if (ene < 0) {
 354     ene = -ene;
 355     if (ene > 0x7fff)
 356       ene = 0x7fff;
 357     ene = (ene * (2500 + 5)) >> 15;
 358     *pwm_reg_a = 0;
 359     *pwm_reg_b = ene | 0x4000;
 360   } else {
 361     if (ene > 0x7fff)
 362       ene = 0x7fff;
 363     ene = (ene * (2500 + 5)) >> 15;
 364     *pwm_reg_b = 0;
 365     *pwm_reg_a = ene | 0x4000;
 366   }
 367
 368   return 0;
 369 }
 370
 371 /* PWM outputs placed on (PWM1), PWM2, PWM4, PWM6 */
 372 int
 373 pxmc_rocon_pwm_master_init(void)
 374 {
 375   int i;
 376
 377   *fpga_lx_master_reset = 1;
 378   *fpga_lx_master_transmitter_reg = 0;
 379
 380   for (i = 0; i < 16 + 3; i ++)
 381     fpga_lx_master_transmitter_base[0] = 0;
 382
 383   fpga_lx_master_transmitter_base[0] = 8;
 384
 385   *fpga_lx_master_reset = 0;
 386
 387   return 0;
 388 }
 389
 390 int
 391 pxmc_fpga_is_mark(pxmc_state_t *mcs)
 392 {
 393   int chan=mcs->pxms_inp_info;
 394
 395   return *fpga_irc_state[chan] & 1;
 396 }
 397
 398 int pxmc_fpga_hh_gd10(pxmc_state_t *mcs);
 399 int pxmc_fpga_hh_gd20(pxmc_state_t *mcs);
 400
 401 int
 402 pxmc_fpga_hh_gi(pxmc_state_t *mcs)
 403 {
 404   long spd;
 405   pxmc_set_flag(mcs,PXMS_BSY_b);
 406   mcs->pxms_rsfg = 0;
 407   spd = mcs->pxms_ms;
 408   spd >>= mcs->pxms_cfg&PXMS_CFG_HSPD_m;
 409   if(!spd) spd = 1;
 410   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
 411     spd = -spd;
 412   mcs->pxms_gen_hsp = spd;
 413   mcs->pxms_do_gen = pxmc_fpga_hh_gd10;
 414   return pxmc_fpga_hh_gd10(mcs);
 415 }
 416
 417 int
 418 pxmc_fpga_hh_gd10(pxmc_state_t *mcs)
 419 {
 420   long spd;
 421   if(mcs->pxms_flg & PXMS_ERR_m)
 422     return pxmc_spdnext_gend(mcs);
 423
 424   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 425   mcs->pxms_rp += mcs->pxms_rs;
 426
 427   if (!pxmc_fpga_is_mark(mcs)){
 428     spd=mcs->pxms_gen_hsp;
 429     if (spd > 0){
 430       spd >>= 2;
 431       spd = spd? -spd: -1;
 432     } else {
 433       spd >>= 2;
 434       spd= spd? -spd: 1;
 435     }
 436     mcs->pxms_gen_hsp = spd;
 437     mcs->pxms_do_gen = pxmc_fpga_hh_gd20;
 438   }
 439
 440   return 0;
 441 }
 442
 443 int
 444 pxmc_fpga_hh_gd20(pxmc_state_t *mcs)
 445 {
 446   if (mcs->pxms_flg & PXMS_ERR_m)
 447     return pxmc_spdnext_gend(mcs);
 448
 449   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 450   mcs->pxms_rp += mcs->pxms_rs;
 451
 452   if (pxmc_fpga_is_mark(mcs)){
 453     /* pxmc_axis_set_pos(mcs, 0); */
 454     if (mcs->pxms_do_ap2hw != NULL) {
 455       mcs->pxms_rp = mcs->pxms_ap = 0;
 456       mcs->pxms_do_ap2hw(mcs);
 457       mcs->pxms_rp = mcs->pxms_ap = 0;
 458     }
 459     mcs->pxms_do_gen = pxmc_stop_gi;
 460   }
 461
 462   return 0;
 463 }
 464
 465 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 466 {
 467   return pxmc_fpga_hh_gi;
 468 }
 469
 470 pxmc_state_t mcs0 =
 471 {
 472 pxms_flg:
 473   PXMS_ENI_m,
 474 pxms_do_inp:
 475   pxmc_inp_rocon_inp,
 476 pxms_do_con:
 477   pxmc_pid_con,
 478 pxms_do_out:
 479   pxmc_rocon_pwm_dc_out,
 480   pxms_do_deb: 0,
 481   pxms_do_gen: 0,
 482 pxms_do_ap2hw:
 483   pxmc_inp_rocon_ap2hw,
 484   pxms_ap: 0, pxms_as: 0,
 485   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 486   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 487   pxms_inp_info: 0,
 488   pxms_out_info: 0,
 489   pxms_ene: 0, pxms_erc: 0,
 490   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 491   pxms_me: 0x7e00/*0x7fff*/,
 492 pxms_cfg:
 493   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 494   0x1,
 495
 496   pxms_ptper: 1,
 497   pxms_ptirc: 1000,
 498   /*pxms_ptamp: 0x7fff,*/
 499
 500   pxms_hal: 0x40,
 501 };
 502
 503 pxmc_state_t mcs1 =
 504 {
 505 pxms_flg:
 506   PXMS_ENI_m,
 507 pxms_do_inp:
 508   pxmc_inp_rocon_inp,
 509 pxms_do_con:
 510   pxmc_pid_con,
 511 pxms_do_out:
 512   pxmc_rocon_pwm_dc_out,
 513   pxms_do_deb: 0,
 514   pxms_do_gen: 0,
 515 pxms_do_ap2hw:
 516   pxmc_inp_rocon_ap2hw,
 517   pxms_ap: 0, pxms_as: 0,
 518   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 519   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 520   pxms_inp_info: 1,
 521   pxms_out_info: 2,
 522   pxms_ene: 0, pxms_erc: 0,
 523   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 524   pxms_me: 0x7e00/*0x7fff*/,
 525 pxms_cfg:
 526   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 527   0x1,
 528
 529   pxms_ptper: 1,
 530   pxms_ptirc: 1000,
 531   /*pxms_ptamp: 0x7fff,*/
 532
 533   pxms_hal: 0x40,
 534 };
 535
 536 pxmc_state_t mcs2 =
 537 {
 538 pxms_flg:
 539   PXMS_ENI_m,
 540 pxms_do_inp:
 541   pxmc_inp_rocon_inp,
 542 pxms_do_con:
 543   pxmc_pid_con,
 544 pxms_do_out:
 545   pxmc_rocon_pwm_dc_out,
 546   pxms_do_deb: 0,
 547   pxms_do_gen: 0,
 548 pxms_do_ap2hw:
 549   pxmc_inp_rocon_ap2hw,
 550   pxms_ap: 0, pxms_as: 0,
 551   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 552   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 553   pxms_inp_info: 2,
 554   pxms_out_info: 4,
 555   pxms_ene: 0, pxms_erc: 0,
 556   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 557   pxms_me: 0x7e00/*0x7fff*/,
 558 pxms_cfg:
 559   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 560   0x1,
 561
 562   pxms_ptper: 1,
 563   pxms_ptirc: 1000,
 564   /*pxms_ptamp: 0x7fff,*/
 565
 566   pxms_hal: 0x40,
 567 };
 568
 569 pxmc_state_t mcs3 =
 570 {
 571 pxms_flg:
 572   PXMS_ENI_m,
 573 pxms_do_inp:
 574   pxmc_inp_rocon_inp,
 575 pxms_do_con:
 576   pxmc_pid_con,
 577 pxms_do_out:
 578   pxmc_rocon_pwm_dc_out,
 579   pxms_do_deb: 0,
 580   pxms_do_gen: 0,
 581 pxms_do_ap2hw:
 582   pxmc_inp_rocon_ap2hw,
 583   pxms_ap: 0, pxms_as: 0,
 584   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 585   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 586   pxms_inp_info: 3,
 587   pxms_out_info: 6,
 588   pxms_ene: 0, pxms_erc: 0,
 589   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 590   pxms_me: 0x7e00/*0x7fff*/,
 591 pxms_cfg:
 592   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 593   0x1,
 594
 595   pxms_ptper: 1,
 596   pxms_ptirc: 1000,
 597   /*pxms_ptamp: 0x7fff,*/
 598
 599   pxms_hal: 0x40,
 600 };
 601
 602 pxmc_state_t mcs4 =
 603 {
 604 pxms_flg:
 605   PXMS_ENI_m,
 606 pxms_do_inp:
 607   pxmc_inp_rocon_inp,
 608 pxms_do_con:
 609   pxmc_pid_con,
 610 pxms_do_out:
 611   pxmc_rocon_pwm_dc_out,
 612   pxms_do_deb: 0,
 613   pxms_do_gen: 0,
 614 pxms_do_ap2hw:
 615   pxmc_inp_rocon_ap2hw,
 616   pxms_ap: 0, pxms_as: 0,
 617   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 618   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 619   pxms_inp_info: 4,
 620   pxms_out_info: 8,
 621   pxms_ene: 0, pxms_erc: 0,
 622   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 623   pxms_me: 0x7e00/*0x7fff*/,
 624 pxms_cfg:
 625   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 626   0x1,
 627
 628   pxms_ptper: 1,
 629   pxms_ptirc: 1000,
 630   /*pxms_ptamp: 0x7fff,*/
 631
 632   pxms_hal: 0x40,
 633 };
 634
 635 pxmc_state_t mcs5 =
 636 {
 637 pxms_flg:
 638   PXMS_ENI_m,
 639 pxms_do_inp:
 640   pxmc_inp_rocon_inp,
 641 pxms_do_con:
 642   pxmc_pid_con,
 643 pxms_do_out:
 644   pxmc_rocon_pwm_dc_out,
 645   pxms_do_deb: 0,
 646   pxms_do_gen: 0,
 647 pxms_do_ap2hw:
 648   pxmc_inp_rocon_ap2hw,
 649   pxms_ap: 0, pxms_as: 0,
 650   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 651   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 652   pxms_inp_info: 5,
 653   pxms_out_info: 10,
 654   pxms_ene: 0, pxms_erc: 0,
 655   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 656   pxms_me: 0x7e00/*0x7fff*/,
 657 pxms_cfg:
 658   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 659   0x1,
 660
 661   pxms_ptper: 1,
 662   pxms_ptirc: 1000,
 663   /*pxms_ptamp: 0x7fff,*/
 664
 665   pxms_hal: 0x40,
 666 };
 667
 668 pxmc_state_t mcs6 =
 669 {
 670 pxms_flg:
 671   PXMS_ENI_m,
 672 pxms_do_inp:
 673   pxmc_inp_rocon_inp,
 674 pxms_do_con:
 675   pxmc_pid_con,
 676 pxms_do_out:
 677   pxmc_rocon_pwm_dc_out,
 678   pxms_do_deb: 0,
 679   pxms_do_gen: 0,
 680 pxms_do_ap2hw:
 681   pxmc_inp_rocon_ap2hw,
 682   pxms_ap: 0, pxms_as: 0,
 683   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 684   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 685   pxms_inp_info: 6,
 686   pxms_out_info: 12,
 687   pxms_ene: 0, pxms_erc: 0,
 688   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 689   pxms_me: 0x7e00/*0x7fff*/,
 690 pxms_cfg:
 691   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 692   0x1,
 693
 694   pxms_ptper: 1,
 695   pxms_ptirc: 1000,
 696   /*pxms_ptamp: 0x7fff,*/
 697
 698   pxms_hal: 0x40,
 699 };
 700
 701 pxmc_state_t mcs7 =
 702 {
 703 pxms_flg:
 704   PXMS_ENI_m,
 705 pxms_do_inp:
 706   pxmc_inp_rocon_inp,
 707 pxms_do_con:
 708   pxmc_pid_con,
 709 pxms_do_out:
 710   pxmc_rocon_pwm_dc_out,
 711   pxms_do_deb: 0,
 712   pxms_do_gen: 0,
 713 pxms_do_ap2hw:
 714   pxmc_inp_rocon_ap2hw,
 715   pxms_ap: 0, pxms_as: 0,
 716   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 717   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 718   pxms_inp_info: 7,
 719   pxms_out_info: 14,
 720   pxms_ene: 0, pxms_erc: 0,
 721   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 722   pxms_me: 0x7e00/*0x7fff*/,
 723 pxms_cfg:
 724   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 725   0x1,
 726
 727   pxms_ptper: 1,
 728   pxms_ptirc: 1000,
 729   /*pxms_ptamp: 0x7fff,*/
 730
 731   pxms_hal: 0x40,
 732 };
 733
 734
 735 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
 736 {&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
 737
 738
 739 pxmc_state_list_t  pxmc_main_list =
 740 {
 741 pxml_arr:
 742   pxmc_main_arr,
 743   pxml_cnt: 0
 744 };
 745
 746 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
 747 {
 748   pxmc_state_t *mcs = (pxmc_state_t *)context;
 749   short ofs;
 750   short irc;
 751   irc = qst->index_pos;
 752
 753   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
 754   {
 755     short diff;
 756     ofs = irc - mcs->pxms_ptmark;
 757     diff = ofs - mcs->pxms_ptofs;
 758
 759     if (diff >= mcs->pxms_ptirc / 2)
 760       diff -= mcs->pxms_ptirc;
 761
 762     if (diff <= -mcs->pxms_ptirc / 2)
 763       diff += mcs->pxms_ptirc;
 764
 765     if (diff < 0)
 766       diff = -diff;
 767
 768     if (diff >= mcs->pxms_ptirc / 6)
 769     {
 770       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 771     }
 772     else
 773     {
 774       mcs->pxms_ptofs = ofs;
 775       pxmc_set_flag(mcs, PXMS_PHA_b);
 776     }
 777   } /*else {
 778
 779     ofs=irc-mcs->pxms_ptofs;
 780     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
 781       if(ofs>0) {
 782         ofs-=mcs->pxms_ptirc;
 783       } else {
 784         ofs+=mcs->pxms_ptirc;
 785       }
 786     }
 787     mcs->pxms_ptmark=ofs;
 788   }*/
 789
 790   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
 791 }
 792
 793 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
 794 {
 795   short ptofs;
 796   short ptmark;
 797   lpc_qei_state_t *qst = &lpc_qei_state;
 798   int irc = lpc_qei_get_pos(qst);
 799   int idx_rel;
 800
 801   if (!qst->index_occ)
 802     return 0;
 803
 804   idx_rel = qst->index_pos - irc;
 805   idx_rel %= mcs->pxms_ptirc;
 806
 807   if (idx_rel < 0)
 808     idx_rel += mcs->pxms_ptirc;
 809
 810   ptofs = irc - mcs->pxms_ptofs;
 811   ptmark = ptofs + idx_rel;
 812
 813   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
 814   {
 815     if (ptmark > 0)
 816       ptmark -= mcs->pxms_ptirc;
 817     else
 818       ptmark += mcs->pxms_ptirc;
 819   }
 820
 821   if ((unsigned short)ptmark < mcs->pxms_ptirc)
 822   {
 823     mcs->pxms_ptmark = ptmark;
 824   }
 825
 826   return 0;
 827 }
 828
 829 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
 830 {
 831   int res;
 832   long r2acq;
 833   long spd;
 834   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
 835   lpc_qei_state_t *qst = &lpc_qei_state;
 836
 837   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
 838   lpc_qei_setup_index_catch(qst);
 839
 840   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
 841   spd = 1 << PXMC_SUBDIV(mcs);
 842
 843   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
 844
 845   return res;
 846 }
 847
 848 int pxmc_lpc_pthalalign_run(void)
 849 {
 850   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
 851 }
 852
 853 static inline void pxmc_sfi_input(void)
 854 {
 855   int var;
 856   pxmc_state_t *mcs;
 857   pxmc_for_each_mcs(var, mcs)
 858   {
 859     /* PXMS_ENI_m - check if input (IRC) update is enabled */
 860     if (mcs->pxms_flg & PXMS_ENI_m)
 861     {
 862       pxmc_call(mcs, mcs->pxms_do_inp);
 863     }
 864   }
 865 }
 866
 867 static inline void pxmc_sfi_controller_and_output(void)
 868 {
 869   int var;
 870   pxmc_state_t *mcs;
 871   pxmc_for_each_mcs(var, mcs)
 872   {
 873     /* PXMS_ENR_m - check if controller is enabled */
 874     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
 875     {
 876
 877       /* If output only is enabled, we skip the controller */
 878       if (mcs->pxms_flg & PXMS_ENR_m)
 879       {
 880
 881         pxmc_call(mcs, mcs->pxms_do_con);
 882
 883         /* PXMS_ERR_m - if axis in error state */
 884         if (mcs->pxms_flg & PXMS_ERR_m)
 885           mcs->pxms_ene = 0;
 886       }
 887
 888       /* for bushless motors, it is necessary to call do_out
 889         even if the controller is not enabled and PWM should be provided. */
 890       pxmc_call(mcs, mcs->pxms_do_out);
 891     }
 892   }
 893 }
 894
 895 static inline void pxmc_sfi_generator(void)
 896 {
 897   int var;
 898   pxmc_state_t *mcs;
 899   pxmc_for_each_mcs(var, mcs)
 900   {
 901     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
 902     if (mcs->pxms_flg & PXMS_ENG_m)
 903     {
 904       pxmc_call(mcs, mcs->pxms_do_gen);
 905     }
 906   }
 907 }
 908
 909 static inline void pxmc_sfi_dbg(void)
 910 {
 911   int var;
 912   pxmc_state_t *mcs;
 913   pxmc_for_each_mcs(var, mcs)
 914   {
 915     if (mcs->pxms_flg & PXMS_DBG_m)
 916     {
 917       pxmc_call(mcs, mcs->pxms_do_deb);
 918     }
 919   }
 920 }
 921
 922 int
 923 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
 924 {
 925   int res;
 926
 927   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
 928
 929   /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
 930 #ifndef PXMC_WITH_PT_ZIC
 931   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
 932 #else /*PXMC_WITH_PT_ZIC*/
 933   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
 934 #endif /*PXMC_WITH_PT_ZIC*/
 935
 936   return res;
 937 }
 938
 939 /**
 940  * pxmc_axis_mode - Sets axis mode.[extern API]
 941  * @mcs:        Motion controller state information
 942  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
 943  *              2 .. stepper motor with IRC feedback and PWM ,
 944  *              3 .. stepper motor with PWM control
 945  *              4 .. DC motor with IRC feedback and PWM
 946  *
 947  */
 948 int
 949 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 950 {
 951   int res;
 952
 953   pxmc_set_const_out(mcs, 0);
 954   pxmc_clear_flag(mcs, PXMS_ENI_b);
 955   pxmc_clear_flag(mcs, PXMS_PHA_b);
 956   pxmc_clear_flag(mcs, PXMS_PTI_b);
 957
 958   if (!mode)
 959   {
 960     /*mode=pxmc_axis_rdmode(mcs);*/
 961     if (!mode)
 962       mode = PXMC_AXIS_MODE_BLDC;
 963   }
 964
 965   res = pxmc_axis_pt4mode(mcs, mode);
 966
 967   pxmc_set_flag(mcs, PXMS_ENI_b);
 968   return res;
 969 }
 970
 971 void pxmc_sfi_isr(void)
 972 {
 973   pxmc_sfi_input();
 974   pxmc_sfi_controller_and_output();
 975   pxmc_sfi_generator();
 976   pxmc_sfi_dbg();
 977 }
 978
 979 int pxmc_done(void)
 980 {
 981   int var;
 982   pxmc_state_t *mcs;
 983
 984   if (!pxmc_main_list.pxml_cnt)
 985     return 0;
 986
 987   pxmc_for_each_mcs(var, mcs)
 988   {
 989     pxmc_set_const_out(mcs,0);
 990   }
 991
 992   pxmc_main_list.pxml_cnt = 0;
 993   __memory_barrier();
 994
 995   return 0;
 996 }
 997
 998 int pxmc_initialize(void)
 999 {
1000   int res;
1001
1002   pxmc_state_t *mcs = &mcs0;
1003   lpc_qei_state_t *qst = &lpc_qei_state;
1004
1005   *fpga_irc_reset = 1;
1006
1007   /* Initialize QEI module for IRC counting */
1008   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
1009
1010   /* Initialize QEI events processing */
1011   if (lpc_qei_setup_irq(qst) < 0)
1012     return -1;
1013
1014   *fpga_irc_reset = 0;
1015
1016   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
1017
1018   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
1019   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
1020
1021   pxmc_rocon_pwm_master_init();
1022
1023   pxmc_main_list.pxml_cnt = 0;
1024   pxmc_dbg_hist = NULL;
1025   __memory_barrier();
1026   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
1027
1028   return 0;
1029 }