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 <hal_gpio.h>
  26 #include <hal_machperiph.h>
  27 #include <stdlib.h>
  28
  29 #include "appl_defs.h"
  30 #include "appl_fpga.h"
  31
  32 #define PXML_MAIN_CNT 4
  33
  34 #define PXMC_WITH_PT_ZIC 1
  35 #define PXMC_PT_ZIC_MASK 0x8000
  36
  37 #define LPCPWM1_FREQ 20000
  38
  39 #define HAL_ERR_SENSITIVITY 20
  40 #define HAL_ERR_MAX_COUNT    5
  41
  42 unsigned pxmc_lpcpwm1_magnitude;
  43
  44 int pxmc_hh_gi(pxmc_state_t *mcs);
  45
  46 int
  47 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
  48 {
  49   int chan=mcs->pxms_inp_info;
  50   long irc;
  51   long pos;
  52
  53   /* read position from hardware */
  54   irc = irc1[chan].count;
  55   pos = irc << PXMC_SUBDIV(mcs);
  56   mcs->pxms_as = pos - mcs->pxms_ap;
  57   mcs->pxms_ap = pos;
  58
  59   /* Running of the motor commutator */
  60   if (mcs->pxms_flg & PXMS_PTI_m)
  61     pxmc_irc_16bit_commindx(mcs, irc);
  62
  63   return 0;
  64 }
  65
  66 int
  67 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
  68 {
  69   /* int chan=mcs->pxms_inp_info */
  70   /* Optional set of the actual position to the HW */
  71   /* We do not want to change real HW counter at any situation */
  72   /* It can be used only to set some software maintained offset */
  73   return 0;
  74 }
  75
  76 inline unsigned
  77 pxmc_rocon_bdc_hal_rd(pxmc_state_t *mcs)
  78 {
  79   unsigned h = 0;
  80   /* FIXME */
  81   /* return 3 bits corresponding to the HAL senzor input */
  82   return h;
  83 }
  84
  85 #if 1
  86 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
  87 {
  88   [0] = 0xff,
  89   [7] = 0xff,
  90   [1] = 0, /*0*/
  91   [5] = 1, /*1*/
  92   [4] = 2, /*2*/
  93   [6] = 3, /*3*/
  94   [2] = 4, /*4*/
  95   [3] = 5, /*5*/
  96 };
  97 #else
  98 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
  99 {
 100   [0] = 0xff,
 101   [7] = 0xff,
 102   [1] = 0, /*0*/
 103   [5] = 5, /*1*/
 104   [4] = 4, /*2*/
 105   [6] = 3, /*3*/
 106   [2] = 2, /*4*/
 107   [3] = 1, /*5*/
 108 };
 109 #endif
 110
 111 /**
 112  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 113  * @mcs:  Motion controller state information
 114  */
 115 /*static*/ inline void
 116 pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
 117 {
 118   unsigned long out_info = mcs->pxms_out_info;
 119
 120   /* FIXME */
 121 }
 122
 123 static inline void
 124 pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 125 {
 126   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
 127   {
 128     pxmc_set_errno(mcs, PXMS_E_HAL);
 129     mcs->pxms_ene = 0;
 130     mcs->pxms_halerc--;
 131   }
 132   else
 133     mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
 134 }
 135
 136 /**
 137  * pxmc_rocon_pwm_out - Phase output for brush-less 3-phase motor
 138  * @mcs:  Motion controller state information
 139  */
 140 int
 141 pxmc_rocon_pwm_out(pxmc_state_t *mcs)
 142 {
 143   unsigned char hal_pos;
 144   short pwm1;
 145   short pwm2;
 146   short pwm3;
 147   int indx = 0;
 148   short ene;
 149   int wind_current[4];
 150
 151   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 152       (mcs->pxms_flg & PXMS_PRA_m))
 153   {
 154     short ptindx;
 155     short ptirc = mcs->pxms_ptirc;
 156     short divisor = mcs->pxms_ptper * 6;
 157
 158     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bdc_hal_rd(mcs)];
 159
 160     if (hal_pos == 0xff)
 161     {
 162       if (mcs->pxms_ene)
 163         pxmc_rocon_process_hal_error(mcs);
 164     }
 165     else
 166     {
 167       if (mcs->pxms_halerc)
 168         mcs->pxms_halerc--;
 169
 170       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 171
 172       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
 173       {
 174         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
 175         {
 176           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 177
 178           if ((ptindx > ptindx_prev + ptirc / 2) ||
 179               (ptindx_prev > ptindx + ptirc / 2))
 180           {
 181             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 182
 183             if (ptindx < 0)
 184               ptindx += ptirc;
 185           }
 186           else
 187           {
 188             ptindx = (ptindx_prev + ptindx) / 2;
 189           }
 190
 191           mcs->pxms_ptindx = ptindx;
 192
 193           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 194
 195           pxmc_set_flag(mcs, PXMS_PTI_b);
 196           pxmc_clear_flag(mcs, PXMS_PRA_b);
 197
 198           /* if phase table position to mask is know do fine phase table alignment */
 199           if (mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 200             lpc_qei_setup_index_catch(&lpc_qei_state);
 201
 202         }
 203         else
 204         {
 205           if (!(mcs->pxms_flg & PXMS_PTI_m))
 206             mcs->pxms_ptindx = ptindx;
 207         }
 208       }
 209
 210       mcs->pxms_hal = hal_pos;
 211     }
 212   }
 213
 214   {
 215     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 216     /* FIXME - check winding current against limit */
 217     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 218   }
 219
 220   ene = mcs->pxms_ene;
 221
 222   if (ene)
 223   {
 224     indx = mcs->pxms_ptindx;
 225 #if 1
 226     /* tuning of magnetic field/voltage advance angle */
 227     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 228 #endif
 229
 230     if (ene < 0)
 231     {
 232       /* Generating direction of stator mag. field for backward torque */
 233       ene = -ene;
 234
 235       if ((indx -= mcs->pxms_ptvang) < 0)
 236         indx += mcs->pxms_ptirc;
 237     }
 238     else
 239     {
 240       /* Generating direction of stator mag. field for forward torque */
 241       if ((indx += mcs->pxms_ptvang) >= mcs->pxms_ptirc)
 242         indx -= mcs->pxms_ptirc;
 243     }
 244
 245     if (mcs->pxms_ptscale_mult)
 246       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 247
 248     pwm1 = mcs->pxms_ptptr1[indx];
 249     pwm2 = mcs->pxms_ptptr2[indx];
 250     pwm3 = mcs->pxms_ptptr3[indx];
 251
 252 #ifdef PXMC_WITH_PT_ZIC
 253
 254     if (labs(mcs->pxms_as) > (10 << PXMC_SUBDIV(mcs)))
 255     {
 256       if (pwm1 & PXMC_PT_ZIC_MASK)
 257       {
 258         /*hal_gpio_set_value(PWM2_EN_PIN,0);*/
 259       }
 260       else
 261       {
 262         /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 263       }
 264
 265       if (pwm2 & PXMC_PT_ZIC_MASK)
 266       {
 267         /*hal_gpio_set_value(PWM4_EN_PIN,0);*/
 268       }
 269       else
 270       {
 271         /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 272       }
 273
 274       if (pwm3 & PXMC_PT_ZIC_MASK)
 275       {
 276         /*hal_gpio_set_value(PWM6_EN_PIN,0);*/
 277       }
 278       else
 279       {
 280         /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 281       }
 282     }
 283     else
 284     {
 285       /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 286       /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 287       /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 288     }
 289
 290     pwm1 &= ~PXMC_PT_ZIC_MASK;
 291     pwm2 &= ~PXMC_PT_ZIC_MASK;
 292     pwm3 &= ~PXMC_PT_ZIC_MASK;
 293 #endif /*PXMC_WITH_PT_ZIC*/
 294
 295     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 296     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 297     {
 298       unsigned long pwm_dc = pxmc_lpcpwm1_magnitude * (unsigned long)ene;
 299       pwm1 = ((unsigned long long)pwm1 * pwm_dc) >> (15 + 15);
 300       pwm2 = ((unsigned long long)pwm2 * pwm_dc) >> (15 + 15);
 301       pwm3 = ((unsigned long long)pwm3 * pwm_dc) >> (15 + 15);
 302     }
 303     pxmc_rocon_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
 304   }
 305   else
 306   {
 307     /*pxmc_lpc_wind_current_over_cnt=0;*/
 308     pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 309 #ifdef PXMC_WITH_PT_ZIC
 310     /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 311     /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 312     /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 313 #endif /*PXMC_WITH_PT_ZIC*/
 314   }
 315
 316   return 0;
 317 }
 318
 319 /* PWM outputs placed on (PWM1), PWM2, PWM4, PWM6 */
 320 int
 321 pxmc_rocon_pwm_init(pxmc_state_t *mcs, int mode)
 322 {
 323
 324   /* hal_pin_conf(PWM1_EN_PIN); */
 325
 326   /* hal_gpio_set_value(PWM1_EN_PIN,1); */
 327
 328   return 0;
 329 }
 330
 331 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 332 {
 333   /*return pxmc_hh_gi;*/
 334   return NULL;
 335 }
 336
 337 pxmc_state_t mcs0 =
 338 {
 339 pxms_flg:
 340   PXMS_ENI_m,
 341 pxms_do_inp:
 342   pxmc_inp_rocon_inp,
 343 pxms_do_con:
 344   pxmc_pid_con,
 345 pxms_do_out:
 346   pxmc_rocon_pwm_out,
 347   pxms_do_deb: 0,
 348   pxms_do_gen: 0,
 349 pxms_do_ap2hw:
 350   pxmc_inp_rocon_ap2hw,
 351   pxms_ap: 0, pxms_as: 0,
 352   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 353   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 354   pxms_inp_info: 0,
 355   pxms_out_info: 0,
 356   pxms_ene: 0, pxms_erc: 0,
 357   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 358   pxms_me: 0x7e00/*0x7fff*/,
 359 pxms_cfg:
 360   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 361   0x1,
 362
 363   pxms_ptper: 1,
 364   pxms_ptirc: 1000,
 365   /*pxms_ptamp: 0x7fff,*/
 366
 367   pxms_hal: 0x40,
 368 };
 369
 370 pxmc_state_t mcs1 =
 371 {
 372 pxms_flg:
 373   PXMS_ENI_m,
 374 pxms_do_inp:
 375   pxmc_inp_rocon_inp,
 376 pxms_do_con:
 377   pxmc_pid_con,
 378 pxms_do_out:
 379   pxmc_rocon_pwm_out,
 380   pxms_do_deb: 0,
 381   pxms_do_gen: 0,
 382 pxms_do_ap2hw:
 383   pxmc_inp_rocon_ap2hw,
 384   pxms_ap: 0, pxms_as: 0,
 385   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 386   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 387   pxms_inp_info: 1,
 388   pxms_out_info: 1,
 389   pxms_ene: 0, pxms_erc: 0,
 390   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 391   pxms_me: 0x7e00/*0x7fff*/,
 392 pxms_cfg:
 393   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 394   0x1,
 395
 396   pxms_ptper: 1,
 397   pxms_ptirc: 1000,
 398   /*pxms_ptamp: 0x7fff,*/
 399
 400   pxms_hal: 0x40,
 401 };
 402
 403 pxmc_state_t mcs2 =
 404 {
 405 pxms_flg:
 406   PXMS_ENI_m,
 407 pxms_do_inp:
 408   pxmc_inp_rocon_inp,
 409 pxms_do_con:
 410   pxmc_pid_con,
 411 pxms_do_out:
 412   pxmc_rocon_pwm_out,
 413   pxms_do_deb: 0,
 414   pxms_do_gen: 0,
 415 pxms_do_ap2hw:
 416   pxmc_inp_rocon_ap2hw,
 417   pxms_ap: 0, pxms_as: 0,
 418   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 419   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 420   pxms_inp_info: 2,
 421   pxms_out_info: 2,
 422   pxms_ene: 0, pxms_erc: 0,
 423   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 424   pxms_me: 0x7e00/*0x7fff*/,
 425 pxms_cfg:
 426   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 427   0x1,
 428
 429   pxms_ptper: 1,
 430   pxms_ptirc: 1000,
 431   /*pxms_ptamp: 0x7fff,*/
 432
 433   pxms_hal: 0x40,
 434 };
 435
 436 pxmc_state_t mcs3 =
 437 {
 438 pxms_flg:
 439   PXMS_ENI_m,
 440 pxms_do_inp:
 441   pxmc_inp_rocon_inp,
 442 pxms_do_con:
 443   pxmc_pid_con,
 444 pxms_do_out:
 445   pxmc_rocon_pwm_out,
 446   pxms_do_deb: 0,
 447   pxms_do_gen: 0,
 448 pxms_do_ap2hw:
 449   pxmc_inp_rocon_ap2hw,
 450   pxms_ap: 0, pxms_as: 0,
 451   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 452   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 453   pxms_inp_info: 3,
 454   pxms_out_info: 3,
 455   pxms_ene: 0, pxms_erc: 0,
 456   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 457   pxms_me: 0x7e00/*0x7fff*/,
 458 pxms_cfg:
 459   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 460   0x1,
 461
 462   pxms_ptper: 1,
 463   pxms_ptirc: 1000,
 464   /*pxms_ptamp: 0x7fff,*/
 465
 466   pxms_hal: 0x40,
 467 };
 468
 469
 470 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
 471 {&mcs0, &mcs1, &mcs2, &mcs3};
 472
 473
 474 pxmc_state_list_t  pxmc_main_list =
 475 {
 476 pxml_arr:
 477   pxmc_main_arr,
 478   pxml_cnt: 0
 479 };
 480
 481 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
 482 {
 483   pxmc_state_t *mcs = (pxmc_state_t *)context;
 484   short ofs;
 485   short irc;
 486   irc = qst->index_pos;
 487
 488   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
 489   {
 490     short diff;
 491     ofs = irc - mcs->pxms_ptmark;
 492     diff = ofs - mcs->pxms_ptofs;
 493
 494     if (diff >= mcs->pxms_ptirc / 2)
 495       diff -= mcs->pxms_ptirc;
 496
 497     if (diff <= -mcs->pxms_ptirc / 2)
 498       diff += mcs->pxms_ptirc;
 499
 500     if (diff < 0)
 501       diff = -diff;
 502
 503     if (diff >= mcs->pxms_ptirc / 6)
 504     {
 505       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 506     }
 507     else
 508     {
 509       mcs->pxms_ptofs = ofs;
 510       pxmc_set_flag(mcs, PXMS_PHA_b);
 511     }
 512   } /*else {
 513
 514     ofs=irc-mcs->pxms_ptofs;
 515     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
 516       if(ofs>0) {
 517         ofs-=mcs->pxms_ptirc;
 518       } else {
 519         ofs+=mcs->pxms_ptirc;
 520       }
 521     }
 522     mcs->pxms_ptmark=ofs;
 523   }*/
 524
 525   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
 526 }
 527
 528 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
 529 {
 530   short ptofs;
 531   short ptmark;
 532   lpc_qei_state_t *qst = &lpc_qei_state;
 533   int irc = lpc_qei_get_pos(qst);
 534   int idx_rel;
 535
 536   if (!qst->index_occ)
 537     return 0;
 538
 539   idx_rel = qst->index_pos - irc;
 540   idx_rel %= mcs->pxms_ptirc;
 541
 542   if (idx_rel < 0)
 543     idx_rel += mcs->pxms_ptirc;
 544
 545   ptofs = irc - mcs->pxms_ptofs;
 546   ptmark = ptofs + idx_rel;
 547
 548   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
 549   {
 550     if (ptmark > 0)
 551       ptmark -= mcs->pxms_ptirc;
 552     else
 553       ptmark += mcs->pxms_ptirc;
 554   }
 555
 556   if ((unsigned short)ptmark < mcs->pxms_ptirc)
 557   {
 558     mcs->pxms_ptmark = ptmark;
 559   }
 560
 561   return 0;
 562 }
 563
 564 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
 565 {
 566   int res;
 567   long r2acq;
 568   long spd;
 569   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
 570   lpc_qei_state_t *qst = &lpc_qei_state;
 571
 572   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
 573   lpc_qei_setup_index_catch(qst);
 574
 575   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
 576   spd = 1 << PXMC_SUBDIV(mcs);
 577
 578   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
 579
 580   return res;
 581 }
 582
 583 int pxmc_lpc_pthalalign_run(void)
 584 {
 585   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
 586 }
 587
 588 static inline void pxmc_sfi_input(void)
 589 {
 590   int var;
 591   pxmc_state_t *mcs;
 592   pxmc_for_each_mcs(var, mcs)
 593   {
 594     /* PXMS_ENI_m - check if input (IRC) update is enabled */
 595     if (mcs->pxms_flg & PXMS_ENI_m)
 596     {
 597       pxmc_call(mcs, mcs->pxms_do_inp);
 598     }
 599   }
 600 }
 601
 602 static inline void pxmc_sfi_controller_and_output(void)
 603 {
 604   int var;
 605   pxmc_state_t *mcs;
 606   pxmc_for_each_mcs(var, mcs)
 607   {
 608     /* PXMS_ENR_m - check if controller is enabled */
 609     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
 610     {
 611
 612       /* If output only is enabled, we skip the controller */
 613       if (mcs->pxms_flg & PXMS_ENR_m)
 614       {
 615
 616         pxmc_call(mcs, mcs->pxms_do_con);
 617
 618         /* PXMS_ERR_m - if axis in error state */
 619         if (mcs->pxms_flg & PXMS_ERR_m)
 620           mcs->pxms_ene = 0;
 621       }
 622
 623       /* for bushless motors, it is necessary to call do_out
 624         even if the controller is not enabled and PWM should be provided. */
 625       pxmc_call(mcs, mcs->pxms_do_out);
 626     }
 627   }
 628 }
 629
 630 static inline void pxmc_sfi_generator(void)
 631 {
 632   int var;
 633   pxmc_state_t *mcs;
 634   pxmc_for_each_mcs(var, mcs)
 635   {
 636     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
 637     if (mcs->pxms_flg & PXMS_ENG_m)
 638     {
 639       pxmc_call(mcs, mcs->pxms_do_gen);
 640     }
 641   }
 642 }
 643
 644 static inline void pxmc_sfi_dbg(void)
 645 {
 646   int var;
 647   pxmc_state_t *mcs;
 648   pxmc_for_each_mcs(var, mcs)
 649   {
 650     if (mcs->pxms_flg & PXMS_DBG_m)
 651     {
 652       pxmc_call(mcs, mcs->pxms_do_deb);
 653     }
 654   }
 655 }
 656
 657 int
 658 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
 659 {
 660   int res;
 661
 662   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
 663
 664   /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
 665 #ifndef PXMC_WITH_PT_ZIC
 666   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
 667 #else /*PXMC_WITH_PT_ZIC*/
 668   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
 669 #endif /*PXMC_WITH_PT_ZIC*/
 670
 671   return res;
 672 }
 673
 674 /**
 675  * pxmc_axis_mode - Sets axis mode.[extern API]
 676  * @mcs:        Motion controller state information
 677  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
 678  *              2 .. stepper motor with IRC feedback and PWM ,
 679  *              3 .. stepper motor with PWM control
 680  *              4 .. DC motor with IRC feedback and PWM
 681  *
 682  */
 683 int
 684 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 685 {
 686   int res;
 687
 688   pxmc_set_const_out(mcs, 0);
 689   pxmc_clear_flag(mcs, PXMS_ENI_b);
 690   pxmc_clear_flag(mcs, PXMS_PHA_b);
 691   pxmc_clear_flag(mcs, PXMS_PTI_b);
 692
 693   if (!mode)
 694   {
 695     /*mode=pxmc_axis_rdmode(mcs);*/
 696     if (!mode)
 697       mode = PXMC_AXIS_MODE_BLDC;
 698   }
 699
 700   res = pxmc_axis_pt4mode(mcs, mode);
 701
 702   pxmc_set_flag(mcs, PXMS_ENI_b);
 703   return res;
 704 }
 705
 706 void pxmc_sfi_isr(void)
 707 {
 708   pxmc_sfi_input();
 709   pxmc_sfi_controller_and_output();
 710   pxmc_sfi_generator();
 711   pxmc_sfi_dbg();
 712 }
 713
 714 int pxmc_initialize(void)
 715 {
 716   int res;
 717
 718   pxmc_state_t *mcs = &mcs0;
 719   lpc_qei_state_t *qst = &lpc_qei_state;
 720
 721   *irc_reset = 1;
 722
 723   /* Initialize QEI module for IRC counting */
 724   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
 725
 726   /* Initialize QEI events processing */
 727   if (lpc_qei_setup_irq(qst) < 0)
 728     return -1;
 729
 730   *irc_reset = 0;
 731
 732   res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
 733
 734   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
 735   pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 736
 737   pxmc_rocon_pwm_init(mcs, 0);
 738
 739   pxmc_main_list.pxml_cnt = 0;
 740   pxmc_dbg_hist = NULL;
 741   __memory_barrier();
 742   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
 743
 744   return 0;
 745 }