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