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 int
 320 pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 321 {
 322   volatile uint32_t *pwm_reg;
 323   int chan = mcs->pxms_out_info;
 324   int ene = mcs->pxms_ene;
 325
 326   if (chan > 6)
 327     return 0;
 328
 329   pwm_reg = lx_master_base + 1 + chan;
 330
 331   if (ene < 0) {
 332     ene = -ene;
 333     if (ene > 0x7fff)
 334       ene = 0x7fff;
 335     ene = (ene * (2500 + 5)) >> 15;
 336     pwm_reg[0] = 0;
 337     pwm_reg[1] = ene | 0x4000;
 338   } else {
 339     if (ene > 0x7fff)
 340       ene = 0x7fff;
 341     ene = (ene * (2500 + 5)) >> 15;
 342     pwm_reg[1] = 0;
 343     pwm_reg[0] = ene | 0x4000;
 344   }
 345
 346   return 0;
 347 }
 348
 349 /* PWM outputs placed on (PWM1), PWM2, PWM4, PWM6 */
 350 int
 351 pxmc_rocon_pwm_init(pxmc_state_t *mcs, int mode)
 352 {
 353
 354   /* hal_pin_conf(PWM1_EN_PIN); */
 355
 356   /* hal_gpio_set_value(PWM1_EN_PIN,1); */
 357
 358   return 0;
 359 }
 360
 361 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 362 {
 363   /*return pxmc_hh_gi;*/
 364   return NULL;
 365 }
 366
 367 pxmc_state_t mcs0 =
 368 {
 369 pxms_flg:
 370   PXMS_ENI_m,
 371 pxms_do_inp:
 372   pxmc_inp_rocon_inp,
 373 pxms_do_con:
 374   pxmc_pid_con,
 375 pxms_do_out:
 376   pxmc_rocon_pwm_dc_out,
 377   pxms_do_deb: 0,
 378   pxms_do_gen: 0,
 379 pxms_do_ap2hw:
 380   pxmc_inp_rocon_ap2hw,
 381   pxms_ap: 0, pxms_as: 0,
 382   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 383   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 384   pxms_inp_info: 0,
 385   pxms_out_info: 0,
 386   pxms_ene: 0, pxms_erc: 0,
 387   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 388   pxms_me: 0x7e00/*0x7fff*/,
 389 pxms_cfg:
 390   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 391   0x1,
 392
 393   pxms_ptper: 1,
 394   pxms_ptirc: 1000,
 395   /*pxms_ptamp: 0x7fff,*/
 396
 397   pxms_hal: 0x40,
 398 };
 399
 400 pxmc_state_t mcs1 =
 401 {
 402 pxms_flg:
 403   PXMS_ENI_m,
 404 pxms_do_inp:
 405   pxmc_inp_rocon_inp,
 406 pxms_do_con:
 407   pxmc_pid_con,
 408 pxms_do_out:
 409   pxmc_rocon_pwm_dc_out,
 410   pxms_do_deb: 0,
 411   pxms_do_gen: 0,
 412 pxms_do_ap2hw:
 413   pxmc_inp_rocon_ap2hw,
 414   pxms_ap: 0, pxms_as: 0,
 415   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 416   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 417   pxms_inp_info: 1,
 418   pxms_out_info: 2,
 419   pxms_ene: 0, pxms_erc: 0,
 420   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 421   pxms_me: 0x7e00/*0x7fff*/,
 422 pxms_cfg:
 423   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 424   0x1,
 425
 426   pxms_ptper: 1,
 427   pxms_ptirc: 1000,
 428   /*pxms_ptamp: 0x7fff,*/
 429
 430   pxms_hal: 0x40,
 431 };
 432
 433 pxmc_state_t mcs2 =
 434 {
 435 pxms_flg:
 436   PXMS_ENI_m,
 437 pxms_do_inp:
 438   pxmc_inp_rocon_inp,
 439 pxms_do_con:
 440   pxmc_pid_con,
 441 pxms_do_out:
 442   pxmc_rocon_pwm_dc_out,
 443   pxms_do_deb: 0,
 444   pxms_do_gen: 0,
 445 pxms_do_ap2hw:
 446   pxmc_inp_rocon_ap2hw,
 447   pxms_ap: 0, pxms_as: 0,
 448   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 449   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 450   pxms_inp_info: 2,
 451   pxms_out_info: 4,
 452   pxms_ene: 0, pxms_erc: 0,
 453   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 454   pxms_me: 0x7e00/*0x7fff*/,
 455 pxms_cfg:
 456   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 457   0x1,
 458
 459   pxms_ptper: 1,
 460   pxms_ptirc: 1000,
 461   /*pxms_ptamp: 0x7fff,*/
 462
 463   pxms_hal: 0x40,
 464 };
 465
 466 pxmc_state_t mcs3 =
 467 {
 468 pxms_flg:
 469   PXMS_ENI_m,
 470 pxms_do_inp:
 471   pxmc_inp_rocon_inp,
 472 pxms_do_con:
 473   pxmc_pid_con,
 474 pxms_do_out:
 475   pxmc_rocon_pwm_dc_out,
 476   pxms_do_deb: 0,
 477   pxms_do_gen: 0,
 478 pxms_do_ap2hw:
 479   pxmc_inp_rocon_ap2hw,
 480   pxms_ap: 0, pxms_as: 0,
 481   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 482   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 483   pxms_inp_info: 3,
 484   pxms_out_info: 6,
 485   pxms_ene: 0, pxms_erc: 0,
 486   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 487   pxms_me: 0x7e00/*0x7fff*/,
 488 pxms_cfg:
 489   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 490   0x1,
 491
 492   pxms_ptper: 1,
 493   pxms_ptirc: 1000,
 494   /*pxms_ptamp: 0x7fff,*/
 495
 496   pxms_hal: 0x40,
 497 };
 498
 499
 500 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
 501 {&mcs0, &mcs1, &mcs2, &mcs3};
 502
 503
 504 pxmc_state_list_t  pxmc_main_list =
 505 {
 506 pxml_arr:
 507   pxmc_main_arr,
 508   pxml_cnt: 0
 509 };
 510
 511 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
 512 {
 513   pxmc_state_t *mcs = (pxmc_state_t *)context;
 514   short ofs;
 515   short irc;
 516   irc = qst->index_pos;
 517
 518   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
 519   {
 520     short diff;
 521     ofs = irc - mcs->pxms_ptmark;
 522     diff = ofs - mcs->pxms_ptofs;
 523
 524     if (diff >= mcs->pxms_ptirc / 2)
 525       diff -= mcs->pxms_ptirc;
 526
 527     if (diff <= -mcs->pxms_ptirc / 2)
 528       diff += mcs->pxms_ptirc;
 529
 530     if (diff < 0)
 531       diff = -diff;
 532
 533     if (diff >= mcs->pxms_ptirc / 6)
 534     {
 535       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 536     }
 537     else
 538     {
 539       mcs->pxms_ptofs = ofs;
 540       pxmc_set_flag(mcs, PXMS_PHA_b);
 541     }
 542   } /*else {
 543
 544     ofs=irc-mcs->pxms_ptofs;
 545     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
 546       if(ofs>0) {
 547         ofs-=mcs->pxms_ptirc;
 548       } else {
 549         ofs+=mcs->pxms_ptirc;
 550       }
 551     }
 552     mcs->pxms_ptmark=ofs;
 553   }*/
 554
 555   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
 556 }
 557
 558 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
 559 {
 560   short ptofs;
 561   short ptmark;
 562   lpc_qei_state_t *qst = &lpc_qei_state;
 563   int irc = lpc_qei_get_pos(qst);
 564   int idx_rel;
 565
 566   if (!qst->index_occ)
 567     return 0;
 568
 569   idx_rel = qst->index_pos - irc;
 570   idx_rel %= mcs->pxms_ptirc;
 571
 572   if (idx_rel < 0)
 573     idx_rel += mcs->pxms_ptirc;
 574
 575   ptofs = irc - mcs->pxms_ptofs;
 576   ptmark = ptofs + idx_rel;
 577
 578   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
 579   {
 580     if (ptmark > 0)
 581       ptmark -= mcs->pxms_ptirc;
 582     else
 583       ptmark += mcs->pxms_ptirc;
 584   }
 585
 586   if ((unsigned short)ptmark < mcs->pxms_ptirc)
 587   {
 588     mcs->pxms_ptmark = ptmark;
 589   }
 590
 591   return 0;
 592 }
 593
 594 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
 595 {
 596   int res;
 597   long r2acq;
 598   long spd;
 599   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
 600   lpc_qei_state_t *qst = &lpc_qei_state;
 601
 602   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
 603   lpc_qei_setup_index_catch(qst);
 604
 605   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
 606   spd = 1 << PXMC_SUBDIV(mcs);
 607
 608   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
 609
 610   return res;
 611 }
 612
 613 int pxmc_lpc_pthalalign_run(void)
 614 {
 615   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
 616 }
 617
 618 static inline void pxmc_sfi_input(void)
 619 {
 620   int var;
 621   pxmc_state_t *mcs;
 622   pxmc_for_each_mcs(var, mcs)
 623   {
 624     /* PXMS_ENI_m - check if input (IRC) update is enabled */
 625     if (mcs->pxms_flg & PXMS_ENI_m)
 626     {
 627       pxmc_call(mcs, mcs->pxms_do_inp);
 628     }
 629   }
 630 }
 631
 632 static inline void pxmc_sfi_controller_and_output(void)
 633 {
 634   int var;
 635   pxmc_state_t *mcs;
 636   pxmc_for_each_mcs(var, mcs)
 637   {
 638     /* PXMS_ENR_m - check if controller is enabled */
 639     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
 640     {
 641
 642       /* If output only is enabled, we skip the controller */
 643       if (mcs->pxms_flg & PXMS_ENR_m)
 644       {
 645
 646         pxmc_call(mcs, mcs->pxms_do_con);
 647
 648         /* PXMS_ERR_m - if axis in error state */
 649         if (mcs->pxms_flg & PXMS_ERR_m)
 650           mcs->pxms_ene = 0;
 651       }
 652
 653       /* for bushless motors, it is necessary to call do_out
 654         even if the controller is not enabled and PWM should be provided. */
 655       pxmc_call(mcs, mcs->pxms_do_out);
 656     }
 657   }
 658 }
 659
 660 static inline void pxmc_sfi_generator(void)
 661 {
 662   int var;
 663   pxmc_state_t *mcs;
 664   pxmc_for_each_mcs(var, mcs)
 665   {
 666     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
 667     if (mcs->pxms_flg & PXMS_ENG_m)
 668     {
 669       pxmc_call(mcs, mcs->pxms_do_gen);
 670     }
 671   }
 672 }
 673
 674 static inline void pxmc_sfi_dbg(void)
 675 {
 676   int var;
 677   pxmc_state_t *mcs;
 678   pxmc_for_each_mcs(var, mcs)
 679   {
 680     if (mcs->pxms_flg & PXMS_DBG_m)
 681     {
 682       pxmc_call(mcs, mcs->pxms_do_deb);
 683     }
 684   }
 685 }
 686
 687 int
 688 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
 689 {
 690   int res;
 691
 692   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
 693
 694   /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
 695 #ifndef PXMC_WITH_PT_ZIC
 696   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
 697 #else /*PXMC_WITH_PT_ZIC*/
 698   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
 699 #endif /*PXMC_WITH_PT_ZIC*/
 700
 701   return res;
 702 }
 703
 704 /**
 705  * pxmc_axis_mode - Sets axis mode.[extern API]
 706  * @mcs:        Motion controller state information
 707  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
 708  *              2 .. stepper motor with IRC feedback and PWM ,
 709  *              3 .. stepper motor with PWM control
 710  *              4 .. DC motor with IRC feedback and PWM
 711  *
 712  */
 713 int
 714 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 715 {
 716   int res;
 717
 718   pxmc_set_const_out(mcs, 0);
 719   pxmc_clear_flag(mcs, PXMS_ENI_b);
 720   pxmc_clear_flag(mcs, PXMS_PHA_b);
 721   pxmc_clear_flag(mcs, PXMS_PTI_b);
 722
 723   if (!mode)
 724   {
 725     /*mode=pxmc_axis_rdmode(mcs);*/
 726     if (!mode)
 727       mode = PXMC_AXIS_MODE_BLDC;
 728   }
 729
 730   res = pxmc_axis_pt4mode(mcs, mode);
 731
 732   pxmc_set_flag(mcs, PXMS_ENI_b);
 733   return res;
 734 }
 735
 736 void pxmc_sfi_isr(void)
 737 {
 738   pxmc_sfi_input();
 739   pxmc_sfi_controller_and_output();
 740   pxmc_sfi_generator();
 741   pxmc_sfi_dbg();
 742 }
 743
 744 int pxmc_initialize(void)
 745 {
 746   int res;
 747
 748   pxmc_state_t *mcs = &mcs0;
 749   lpc_qei_state_t *qst = &lpc_qei_state;
 750
 751   *irc_reset = 1;
 752
 753   /* Initialize QEI module for IRC counting */
 754   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
 755
 756   /* Initialize QEI events processing */
 757   if (lpc_qei_setup_irq(qst) < 0)
 758     return -1;
 759
 760   *irc_reset = 0;
 761
 762   res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
 763
 764   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
 765   pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 766
 767   pxmc_rocon_pwm_init(mcs, 0);
 768
 769   pxmc_main_list.pxml_cnt = 0;
 770   pxmc_dbg_hist = NULL;
 771   __memory_barrier();
 772   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
 773
 774   return 0;
 775 }