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_rocon_pwm_magnitude = 2500;
  44
  45 long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
  46 unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
  47
  48 const uint8_t onesin10bits[1024]={
  49   0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
  50   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  51   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  52   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  53   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  54   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  55   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  56   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  57   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  58   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  59   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  60   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  61   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  62   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  63   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  64   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  65   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  66   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  67   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  68   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  69   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  70   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  71   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  72   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  73   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  74   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  75   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  76   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  77   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  78   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  79   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  80   5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,6,7,7,8,7,8,8,9,7,8,8,9,8,9,9,10
  81 };
  82
  83 int
  84 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
  85 {
  86   int chan=mcs->pxms_inp_info;
  87   long irc;
  88   long pos;
  89
  90   /* read position from hardware */
  91   irc = fpga_irc[chan]->count;
  92   irc += pxmc_rocon_irc_offset[chan];
  93   pos = irc << PXMC_SUBDIV(mcs);
  94   mcs->pxms_as = pos - mcs->pxms_ap;
  95   mcs->pxms_ap = pos;
  96
  97   /* Running of the motor commutator */
  98   if (mcs->pxms_flg & PXMS_PTI_m)
  99     pxmc_irc_16bit_commindx(mcs, irc);
 100
 101   return 0;
 102 }
 103
 104 int
 105 pxmc_inp_rocon_is_mark(pxmc_state_t *mcs)
 106 {
 107   uint32_t irc_state;
 108   int mark;
 109   unsigned filt;
 110   int chan=mcs->pxms_inp_info;
 111
 112   irc_state = *fpga_irc_state[chan];
 113
 114   mark = (irc_state ^ (mcs->pxms_cfg >> PXMS_CFG_HPS_b)) & 1;
 115
 116   filt = pxmc_rocon_mark_filt[chan];
 117   filt = (filt << 1) | mark;
 118   pxmc_rocon_mark_filt[chan] = filt;
 119
 120   return onesin10bits[filt & 0x03ff];
 121 }
 122
 123 int
 124 pxmc_inp_rocon_is_index_edge(pxmc_state_t *mcs)
 125 {
 126   uint32_t irc_state;
 127   int index;
 128   int chan=mcs->pxms_inp_info;
 129
 130   irc_state = *fpga_irc_state[chan];
 131   *fpga_irc_state[chan] = 1 << 2;
 132
 133   index = (irc_state >> 2) & 1;
 134
 135   return index;
 136 }
 137
 138 int
 139 pxmc_inp_rocon_adjust_to_irc_change(struct pxmc_state *mcs, long irc_diff)
 140 {
 141   long pos_diff = irc_diff << PXMC_SUBDIV(mcs);
 142
 143   mcs->pxms_ap += pos_diff;
 144   mcs->pxms_rp += pos_diff;
 145   return 0;
 146 }
 147
 148 int
 149 pxmc_inp_rocon_irc_offset_from_index(struct pxmc_state *mcs)
 150 {
 151   int chan=mcs->pxms_inp_info;
 152   long irc_offset;
 153   long irc_diff;
 154
 155   irc_offset = -fpga_irc[chan]->count_index;
 156   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 157   pxmc_rocon_irc_offset[chan] = irc_offset;
 158   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 159 }
 160
 161 int
 162 pxmc_inp_rocon_ap_zero(struct pxmc_state *mcs)
 163 {
 164   int chan=mcs->pxms_inp_info;
 165   long irc_offset;
 166   long irc_diff;
 167
 168   irc_offset = -fpga_irc[chan]->count;
 169   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 170   pxmc_rocon_irc_offset[chan] = irc_offset;
 171   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 172 }
 173
 174 int
 175 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
 176 {
 177   int chan=mcs->pxms_inp_info;
 178   long irc;
 179   long pos_diff;
 180
 181   irc = fpga_irc[chan]->count;
 182   pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
 183   pxmc_rocon_irc_offset[chan] = pos_diff >> PXMC_SUBDIV(mcs);
 184   return 0;
 185 }
 186
 187 inline unsigned
 188 pxmc_rocon_bdc_hal_rd(pxmc_state_t *mcs)
 189 {
 190   unsigned h = 0;
 191   /* FIXME */
 192   h = 1;
 193   /* return 3 bits corresponding to the HAL senzor input */
 194   return h;
 195 }
 196
 197 #if 1
 198 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 199 {
 200   [0] = 0xff,
 201   [7] = 0xff,
 202   [1] = 0, /*0*/
 203   [5] = 1, /*1*/
 204   [4] = 2, /*2*/
 205   [6] = 3, /*3*/
 206   [2] = 4, /*4*/
 207   [3] = 5, /*5*/
 208 };
 209 #else
 210 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 211 {
 212   [0] = 0xff,
 213   [7] = 0xff,
 214   [1] = 0, /*0*/
 215   [5] = 5, /*1*/
 216   [4] = 4, /*2*/
 217   [6] = 3, /*3*/
 218   [2] = 2, /*4*/
 219   [3] = 1, /*5*/
 220 };
 221 #endif
 222
 223 uint32_t pxmc_rocon_pwm_dummy_reg;
 224
 225 static inline volatile uint32_t *
 226 pxmc_rocon_pwm_chan2reg(unsigned chan)
 227 {
 228   volatile uint32_t *pwm_reg;
 229
 230   if (chan >= 16)
 231     return &pxmc_rocon_pwm_dummy_reg;
 232
 233   pwm_reg = fpga_lx_master_transmitter_base;
 234  #if 0 /* FPGA design version 2 */
 235   pwm_reg += 1 + (chan >> 8) + chan;
 236  #else  /* FPGA design version 3 */
 237   pwm_reg += chan + 8;
 238  #endif
 239   return pwm_reg;
 240 }
 241
 242 /**
 243  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 244  * @mcs:  Motion controller state information
 245  */
 246 /*static*/ inline void
 247 pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
 248 {
 249   volatile uint32_t *pwm_reg_a, *pwm_reg_b, *pwm_reg_c;
 250   int chan = mcs->pxms_out_info;
 251
 252   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
 253   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 254   pwm_reg_c = pxmc_rocon_pwm_chan2reg(chan + 2);
 255
 256   *pwm_reg_a = pwm1;
 257   *pwm_reg_b = pwm2;
 258   *pwm_reg_c = pwm3;
 259 }
 260
 261 static inline void
 262 pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 263 {
 264   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
 265   {
 266     pxmc_set_errno(mcs, PXMS_E_HAL);
 267     mcs->pxms_ene = 0;
 268     mcs->pxms_halerc--;
 269   }
 270   else
 271     mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
 272 }
 273
 274 /**
 275  * pxmc_rocon_pwm3ph_out - Phase output for brush-less 3-phase motor
 276  * @mcs:  Motion controller state information
 277  */
 278 int
 279 pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
 280 {
 281   unsigned char hal_pos;
 282   short pwm1;
 283   short pwm2;
 284   short pwm3;
 285   int indx = 0;
 286   short ene;
 287   int wind_current[4];
 288
 289   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 290       (mcs->pxms_flg & PXMS_PRA_m))
 291   {
 292     short ptindx;
 293     short ptirc = mcs->pxms_ptirc;
 294     short divisor = mcs->pxms_ptper * 6;
 295
 296     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bdc_hal_rd(mcs)];
 297
 298     if (hal_pos == 0xff)
 299     {
 300       if (mcs->pxms_ene)
 301         pxmc_rocon_process_hal_error(mcs);
 302     }
 303     else
 304     {
 305       if (mcs->pxms_halerc)
 306         mcs->pxms_halerc--;
 307
 308       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 309
 310       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
 311       {
 312         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
 313         {
 314           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 315
 316           if ((ptindx > ptindx_prev + ptirc / 2) ||
 317               (ptindx_prev > ptindx + ptirc / 2))
 318           {
 319             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 320
 321             if (ptindx < 0)
 322               ptindx += ptirc;
 323           }
 324           else
 325           {
 326             ptindx = (ptindx_prev + ptindx) / 2;
 327           }
 328
 329           mcs->pxms_ptindx = ptindx;
 330
 331           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 332
 333           pxmc_set_flag(mcs, PXMS_PTI_b);
 334           pxmc_clear_flag(mcs, PXMS_PRA_b);
 335
 336           /* if phase table position to mask is know do fine phase table alignment */
 337           if (mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 338             lpc_qei_setup_index_catch(&lpc_qei_state);
 339
 340         }
 341         else
 342         {
 343           if (!(mcs->pxms_flg & PXMS_PTI_m))
 344             mcs->pxms_ptindx = ptindx;
 345         }
 346       }
 347
 348       mcs->pxms_hal = hal_pos;
 349     }
 350   }
 351
 352   {
 353     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 354     /* FIXME - check winding current against limit */
 355     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 356   }
 357
 358   ene = mcs->pxms_ene;
 359
 360   if (ene)
 361   {
 362     indx = mcs->pxms_ptindx;
 363 #if 1
 364     /* tuning of magnetic field/voltage advance angle */
 365     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 366 #endif
 367
 368     if (ene < 0)
 369     {
 370       /* Generating direction of stator mag. field for backward torque */
 371       ene = -ene;
 372
 373       if ((indx -= mcs->pxms_ptvang) < 0)
 374         indx += mcs->pxms_ptirc;
 375     }
 376     else
 377     {
 378       /* Generating direction of stator mag. field for forward torque */
 379       if ((indx += mcs->pxms_ptvang) >= mcs->pxms_ptirc)
 380         indx -= mcs->pxms_ptirc;
 381     }
 382
 383     if (mcs->pxms_ptscale_mult)
 384       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 385
 386     pwm1 = mcs->pxms_ptptr1[indx];
 387     pwm2 = mcs->pxms_ptptr2[indx];
 388     pwm3 = mcs->pxms_ptptr3[indx];
 389
 390 #ifdef PXMC_WITH_PT_ZIC
 391     if (labs(mcs->pxms_as) < (10 << PXMC_SUBDIV(mcs)))
 392     {
 393       pwm1 &= ~PXMC_PT_ZIC_MASK;
 394       pwm2 &= ~PXMC_PT_ZIC_MASK;
 395       pwm3 &= ~PXMC_PT_ZIC_MASK;
 396     }
 397 #endif /*PXMC_WITH_PT_ZIC*/
 398
 399     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 400     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 401     {
 402       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 403       if (pwm1 & PXMC_PT_ZIC_MASK)
 404         pwm1 = 0x8000;
 405       else
 406         pwm1 = (((unsigned long long)pwm1 * pwm_dc) >> (15 + 15)) | 0x4000;
 407       if (pwm2 & PXMC_PT_ZIC_MASK)
 408         pwm2 = 0x8000;
 409       else
 410         pwm2 = (((unsigned long long)pwm2 * pwm_dc) >> (15 + 15)) | 0x4000;
 411       if (pwm3 & PXMC_PT_ZIC_MASK)
 412         pwm3 = 0x8000;
 413       else
 414         pwm3 = (((unsigned long long)pwm3 * pwm_dc) >> (15 + 15)) | 0x4000;
 415     }
 416     pxmc_rocon_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
 417   }
 418   else
 419   {
 420     pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 421   }
 422
 423   return 0;
 424 }
 425
 426 /**
 427  * pxmc_rocon_pwm2ph_wr - Output of the 2-phase stepper motor PWM to the hardware
 428  * @mcs:        Motion controller state information
 429  */
 430 /*static*/ inline void
 431 pxmc_rocon_pwm2ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2)
 432 {
 433   volatile uint32_t *pwm_reg_ap, *pwm_reg_an, *pwm_reg_bp, *pwm_reg_bn;
 434   int chan = mcs->pxms_out_info;
 435
 436   pwm_reg_ap = pxmc_rocon_pwm_chan2reg(chan + 0);
 437   pwm_reg_an = pxmc_rocon_pwm_chan2reg(chan + 1);
 438   pwm_reg_bp = pxmc_rocon_pwm_chan2reg(chan + 2);
 439   pwm_reg_bn = pxmc_rocon_pwm_chan2reg(chan + 3);
 440
 441   if (pwm2 >= 0) {
 442     *pwm_reg_bp = pwm2 | 0x4000;
 443     *pwm_reg_bn = 0;
 444   } else {
 445     *pwm_reg_bp = 0;
 446     *pwm_reg_bn = -pwm2 | 0x4000;
 447   }
 448
 449   if (pwm1 >= 0) {
 450     *pwm_reg_ap = pwm1 | 0x4000;
 451     *pwm_reg_an = 0;
 452   } else {
 453     *pwm_reg_ap = 0;
 454     *pwm_reg_an = -pwm1 | 0x4000;
 455   }
 456 }
 457
 458 /**
 459  * pxmc_rocon_pwm2ph_out - Phase output of the 2-phase stepper motor PWM
 460  * @mcs:  Motion controller state information
 461  */
 462 int
 463 pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
 464 {
 465   short pwm1;
 466   short pwm2;
 467   int indx = 0;
 468   short ene;
 469   int wind_current[4];
 470
 471   if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
 472      (mcs->pxms_flg&PXMS_PRA_m)){
 473     short ptindx;
 474     short ptirc=mcs->pxms_ptirc;
 475
 476     pxmc_irc_16bit_commindx(mcs,mcs->pxms_rp>>PXMC_SUBDIV(mcs));
 477
 478     ptindx = mcs->pxms_ptindx;
 479
 480     if(0 && (!(mcs->pxms_flg&PXMS_PTI_m) || (mcs->pxms_flg&PXMS_PRA_m))) {
 481
 482       mcs->pxms_ptindx = ptindx;
 483
 484       mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 485
 486       pxmc_set_flag(mcs, PXMS_PTI_b);
 487       pxmc_clear_flag(mcs, PXMS_PRA_b);
 488
 489       /* if phase table position to mask is know do fine phase table alignment */
 490       //if(mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 491       //  lpc_qei_setup_index_catch(&lpc_qei_state);
 492
 493     } else {
 494       if(!(mcs->pxms_flg&PXMS_PTI_m))
 495             mcs->pxms_ptindx = ptindx;
 496     }
 497   }
 498
 499   {
 500     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 501     /* FIXME - check winding current against limit */
 502     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 503   }
 504
 505   ene = mcs->pxms_ene;
 506   //if (ene) { /*FIXME*/
 507   if (mcs->pxms_flg&PXMS_BSY_m){
 508     ene = mcs->pxms_me; /*FIXME*/
 509
 510     indx = mcs->pxms_ptindx;
 511    #if 1
 512     /* tuning of magnetic field/voltage advance angle */
 513     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 514    #endif
 515     if (ene<0){
 516       /* Generating direction of stator mag. field for backward torque */
 517       ene = -ene;
 518       if ((indx -= mcs->pxms_ptvang)<0)
 519         indx += mcs->pxms_ptirc;
 520     }else{
 521       /* Generating direction of stator mag. field for forward torque */
 522       if ((indx += mcs->pxms_ptvang) >= mcs->pxms_ptirc)
 523         indx -= mcs->pxms_ptirc;
 524     }
 525
 526     if (mcs->pxms_ptscale_mult)
 527       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 528
 529     pwm1 = mcs->pxms_ptptr1[indx];
 530     pwm2 = mcs->pxms_ptptr2[indx];
 531
 532     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 533     /* Initialized PWM period is 0x200 => divide by value about 2097024 */
 534     {
 535       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 536       pwm1 = ((unsigned long long)pwm1 * pwm_dc) >> (15+15);
 537       pwm2 = ((unsigned long long)pwm2 * pwm_dc) >> (15+15);
 538     }
 539     pxmc_rocon_pwm2ph_wr(mcs, pwm1, pwm2);
 540   }else{
 541     /*pxmc_lpc_wind_current_over_cnt = 0;*/
 542     pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
 543   }
 544
 545   return 0;
 546 }
 547
 548 /**
 549  * pxmc_rocon_pwm_dc_out - DC motor CW and CCW PWM output
 550  * @mcs:  Motion controller state information
 551  */
 552 int
 553 pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 554 {
 555   volatile uint32_t *pwm_reg_a, *pwm_reg_b;
 556   int chan = mcs->pxms_out_info;
 557   int ene = mcs->pxms_ene;
 558
 559   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
 560   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 561
 562   if (ene < 0) {
 563     ene = -ene;
 564     if (ene > 0x7fff)
 565       ene = 0x7fff;
 566     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 567     *pwm_reg_a = 0;
 568     *pwm_reg_b = ene | 0x4000;
 569   } else {
 570     if (ene > 0x7fff)
 571       ene = 0x7fff;
 572     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 573     *pwm_reg_b = 0;
 574     *pwm_reg_a = ene | 0x4000;
 575   }
 576
 577   return 0;
 578 }
 579
 580 /* PWM outputs placed on (PWM1), PWM2, PWM4, PWM6 */
 581 int
 582 pxmc_rocon_pwm_master_init(void)
 583 {
 584   int i;
 585
 586   *fpga_lx_master_reset = 1;
 587   *fpga_lx_master_transmitter_reg = 0;
 588
 589   for (i = 0; i < 8 + 16; i ++)
 590     fpga_lx_master_transmitter_base[i] = 0;
 591
 592   fpga_lx_master_transmitter_base[0] = 0x0808;
 593   fpga_lx_master_transmitter_base[1] = 0x0000;
 594
 595   *fpga_lx_master_reset = 0;
 596
 597   return 0;
 598 }
 599
 600 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
 601 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
 602 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
 603 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
 604 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
 605 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
 606
 607 /* initialize for hard home */
 608 int
 609 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
 610 {
 611   pxmc_set_flag(mcs,PXMS_BSY_b);
 612  #if 0 /* config and speed already set in pxmc_hh */
 613   spd = mcs->pxms_ms;
 614   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
 615   if(!spd) spd = 1;
 616   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
 617     spd = -spd;
 618   mcs->pxms_gen_hsp = spd;
 619  #endif
 620   mcs->pxms_rsfg = 0;
 621   mcs->pxms_gen_htim = 16;
 622   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
 623   return pxmc_rocon_hh_gd10(mcs);
 624 }
 625
 626 /* fill initial mark filter and then decide */
 627 int
 628 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
 629 {
 630   int mark;
 631
 632   if(mcs->pxms_flg & PXMS_ERR_m)
 633     return pxmc_spdnext_gend(mcs);
 634
 635   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
 636   mcs->pxms_rp += mcs->pxms_rs;
 637
 638   mark = pxmc_inp_rocon_is_mark(mcs);
 639
 640   if (mcs->pxms_gen_htim) {
 641     mcs->pxms_gen_htim--;
 642     return 0;
 643   }
 644
 645   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
 646     /* limit switch is not used */
 647     pxmc_inp_rocon_is_index_edge(mcs);
 648     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 649     return 0;
 650   }
 651
 652   if (mark >= 6) {
 653     /* go out from switch */
 654     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
 655   } else {
 656     /* switch is off -> look for it */
 657     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 658   }
 659   return 0;
 660 }
 661
 662 /* go out from switch */
 663 int
 664 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
 665 {
 666   int mark;
 667
 668   if(mcs->pxms_flg & PXMS_ERR_m)
 669     return pxmc_spdnext_gend(mcs);
 670
 671   mark = pxmc_inp_rocon_is_mark(mcs);
 672
 673   if (mark <= 1) {
 674     /* switch is off -> look for it again */
 675     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 676   }
 677
 678   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 679   mcs->pxms_rp += mcs->pxms_rs;
 680
 681   return 0;
 682 }
 683
 684 /* switch is off -> look for it */
 685 int
 686 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
 687 {
 688   long spd;
 689   int mark;
 690
 691   if (mcs->pxms_flg & PXMS_ERR_m)
 692     return pxmc_spdnext_gend(mcs);
 693
 694   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 695   mcs->pxms_rp += mcs->pxms_rs;
 696
 697   mark = pxmc_inp_rocon_is_mark(mcs);
 698
 699   if (mark >= 8){
 700     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
 701     if (!spd)
 702       spd = 1;
 703     mcs->pxms_gen_hsp = spd;
 704     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
 705   }
 706
 707   return 0;
 708 }
 709
 710 /* switch is on -> find edge */
 711 int
 712 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
 713 {
 714   int mark;
 715
 716   if (mcs->pxms_flg & PXMS_ERR_m)
 717     return pxmc_spdnext_gend(mcs);
 718
 719   mark = pxmc_inp_rocon_is_mark(mcs);
 720
 721   if (mark <= 1) {
 722     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
 723       pxmc_inp_rocon_is_index_edge(mcs);
 724       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 725     } else {
 726       pxmc_inp_rocon_ap_zero(mcs);
 727       mcs->pxms_do_gen = pxmc_stop_gi;
 728     }
 729   }
 730
 731   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 732   mcs->pxms_rp += mcs->pxms_rs;
 733
 734   return 0;
 735 }
 736
 737 /* calibrate on revolution index */
 738 int
 739 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
 740 {
 741   if (mcs->pxms_flg & PXMS_ERR_m)
 742     return pxmc_spdnext_gend(mcs);
 743
 744   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 745   mcs->pxms_rp += mcs->pxms_rs;
 746
 747   if (pxmc_inp_rocon_is_index_edge(mcs)){
 748     pxmc_inp_rocon_irc_offset_from_index(mcs);
 749     mcs->pxms_do_gen = pxmc_stop_gi;
 750   }
 751
 752   return 0;
 753 }
 754
 755 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 756 {
 757   return pxmc_rocon_hh_gi;
 758 }
 759
 760 pxmc_state_t mcs0 =
 761 {
 762 pxms_flg:
 763   PXMS_ENI_m,
 764 pxms_do_inp:
 765   pxmc_inp_rocon_inp,
 766 pxms_do_con:
 767   pxmc_pid_con,
 768 pxms_do_out:
 769   pxmc_rocon_pwm_dc_out,
 770   pxms_do_deb: 0,
 771   pxms_do_gen: 0,
 772 pxms_do_ap2hw:
 773   pxmc_inp_rocon_ap2hw,
 774   pxms_ap: 0, pxms_as: 0,
 775   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 776   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 777   pxms_inp_info: 0,
 778   pxms_out_info: 0,
 779   pxms_ene: 0, pxms_erc: 0,
 780   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 781   pxms_me: 0x7e00/*0x7fff*/,
 782 pxms_cfg:
 783   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 784   0x1,
 785
 786   pxms_ptper: 1,
 787   pxms_ptirc: 1000,
 788   /*pxms_ptamp: 0x7fff,*/
 789
 790   pxms_hal: 0x40,
 791 };
 792
 793 pxmc_state_t mcs1 =
 794 {
 795 pxms_flg:
 796   PXMS_ENI_m,
 797 pxms_do_inp:
 798   pxmc_inp_rocon_inp,
 799 pxms_do_con:
 800   pxmc_pid_con,
 801 pxms_do_out:
 802   pxmc_rocon_pwm_dc_out,
 803   pxms_do_deb: 0,
 804   pxms_do_gen: 0,
 805 pxms_do_ap2hw:
 806   pxmc_inp_rocon_ap2hw,
 807   pxms_ap: 0, pxms_as: 0,
 808   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 809   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 810   pxms_inp_info: 1,
 811   pxms_out_info: 2,
 812   pxms_ene: 0, pxms_erc: 0,
 813   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 814   pxms_me: 0x7e00/*0x7fff*/,
 815 pxms_cfg:
 816   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 817   0x1,
 818
 819   pxms_ptper: 1,
 820   pxms_ptirc: 1000,
 821   /*pxms_ptamp: 0x7fff,*/
 822
 823   pxms_hal: 0x40,
 824 };
 825
 826 pxmc_state_t mcs2 =
 827 {
 828 pxms_flg:
 829   PXMS_ENI_m,
 830 pxms_do_inp:
 831   pxmc_inp_rocon_inp,
 832 pxms_do_con:
 833   pxmc_pid_con,
 834 pxms_do_out:
 835   pxmc_rocon_pwm_dc_out,
 836   pxms_do_deb: 0,
 837   pxms_do_gen: 0,
 838 pxms_do_ap2hw:
 839   pxmc_inp_rocon_ap2hw,
 840   pxms_ap: 0, pxms_as: 0,
 841   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 842   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 843   pxms_inp_info: 2,
 844   pxms_out_info: 4,
 845   pxms_ene: 0, pxms_erc: 0,
 846   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 847   pxms_me: 0x7e00/*0x7fff*/,
 848 pxms_cfg:
 849   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 850   0x1,
 851
 852   pxms_ptper: 1,
 853   pxms_ptirc: 1000,
 854   /*pxms_ptamp: 0x7fff,*/
 855
 856   pxms_hal: 0x40,
 857 };
 858
 859 pxmc_state_t mcs3 =
 860 {
 861 pxms_flg:
 862   PXMS_ENI_m,
 863 pxms_do_inp:
 864   pxmc_inp_rocon_inp,
 865 pxms_do_con:
 866   pxmc_pid_con,
 867 pxms_do_out:
 868   pxmc_rocon_pwm_dc_out,
 869   pxms_do_deb: 0,
 870   pxms_do_gen: 0,
 871 pxms_do_ap2hw:
 872   pxmc_inp_rocon_ap2hw,
 873   pxms_ap: 0, pxms_as: 0,
 874   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 875   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 876   pxms_inp_info: 3,
 877   pxms_out_info: 6,
 878   pxms_ene: 0, pxms_erc: 0,
 879   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 880   pxms_me: 0x7e00/*0x7fff*/,
 881 pxms_cfg:
 882   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 883   0x1,
 884
 885   pxms_ptper: 1,
 886   pxms_ptirc: 1000,
 887   /*pxms_ptamp: 0x7fff,*/
 888
 889   pxms_hal: 0x40,
 890 };
 891
 892 pxmc_state_t mcs4 =
 893 {
 894 pxms_flg:
 895   PXMS_ENI_m,
 896 pxms_do_inp:
 897   pxmc_inp_rocon_inp,
 898 pxms_do_con:
 899   pxmc_pid_con,
 900 pxms_do_out:
 901   pxmc_rocon_pwm_dc_out,
 902   pxms_do_deb: 0,
 903   pxms_do_gen: 0,
 904 pxms_do_ap2hw:
 905   pxmc_inp_rocon_ap2hw,
 906   pxms_ap: 0, pxms_as: 0,
 907   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 908   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 909   pxms_inp_info: 4,
 910   pxms_out_info: 8,
 911   pxms_ene: 0, pxms_erc: 0,
 912   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 913   pxms_me: 0x7e00/*0x7fff*/,
 914 pxms_cfg:
 915   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 916   0x1,
 917
 918   pxms_ptper: 1,
 919   pxms_ptirc: 1000,
 920   /*pxms_ptamp: 0x7fff,*/
 921
 922   pxms_hal: 0x40,
 923 };
 924
 925 pxmc_state_t mcs5 =
 926 {
 927 pxms_flg:
 928   PXMS_ENI_m,
 929 pxms_do_inp:
 930   pxmc_inp_rocon_inp,
 931 pxms_do_con:
 932   pxmc_pid_con,
 933 pxms_do_out:
 934   pxmc_rocon_pwm_dc_out,
 935   pxms_do_deb: 0,
 936   pxms_do_gen: 0,
 937 pxms_do_ap2hw:
 938   pxmc_inp_rocon_ap2hw,
 939   pxms_ap: 0, pxms_as: 0,
 940   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 941   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 942   pxms_inp_info: 5,
 943   pxms_out_info: 10,
 944   pxms_ene: 0, pxms_erc: 0,
 945   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 946   pxms_me: 0x7e00/*0x7fff*/,
 947 pxms_cfg:
 948   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 949   0x1,
 950
 951   pxms_ptper: 1,
 952   pxms_ptirc: 1000,
 953   /*pxms_ptamp: 0x7fff,*/
 954
 955   pxms_hal: 0x40,
 956 };
 957
 958 pxmc_state_t mcs6 =
 959 {
 960 pxms_flg:
 961   PXMS_ENI_m,
 962 pxms_do_inp:
 963   pxmc_inp_rocon_inp,
 964 pxms_do_con:
 965   pxmc_pid_con,
 966 pxms_do_out:
 967   pxmc_rocon_pwm_dc_out,
 968   pxms_do_deb: 0,
 969   pxms_do_gen: 0,
 970 pxms_do_ap2hw:
 971   pxmc_inp_rocon_ap2hw,
 972   pxms_ap: 0, pxms_as: 0,
 973   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 974   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 975   pxms_inp_info: 6,
 976   pxms_out_info: 12,
 977   pxms_ene: 0, pxms_erc: 0,
 978   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 979   pxms_me: 0x7e00/*0x7fff*/,
 980 pxms_cfg:
 981   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 982   0x1,
 983
 984   pxms_ptper: 1,
 985   pxms_ptirc: 1000,
 986   /*pxms_ptamp: 0x7fff,*/
 987
 988   pxms_hal: 0x40,
 989 };
 990
 991 pxmc_state_t mcs7 =
 992 {
 993 pxms_flg:
 994   PXMS_ENI_m,
 995 pxms_do_inp:
 996   pxmc_inp_rocon_inp,
 997 pxms_do_con:
 998   pxmc_pid_con,
 999 pxms_do_out:
1000   pxmc_rocon_pwm_dc_out,
1001   pxms_do_deb: 0,
1002   pxms_do_gen: 0,
1003 pxms_do_ap2hw:
1004   pxmc_inp_rocon_ap2hw,
1005   pxms_ap: 0, pxms_as: 0,
1006   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1007   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1008   pxms_inp_info: 7,
1009   pxms_out_info: 14,
1010   pxms_ene: 0, pxms_erc: 0,
1011   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1012   pxms_me: 0x7e00/*0x7fff*/,
1013 pxms_cfg:
1014   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1015   0x1,
1016
1017   pxms_ptper: 1,
1018   pxms_ptirc: 1000,
1019   /*pxms_ptamp: 0x7fff,*/
1020
1021   pxms_hal: 0x40,
1022 };
1023
1024
1025 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
1026 {&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
1027
1028
1029 pxmc_state_list_t  pxmc_main_list =
1030 {
1031 pxml_arr:
1032   pxmc_main_arr,
1033   pxml_cnt: 0
1034 };
1035
1036 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
1037 {
1038   pxmc_state_t *mcs = (pxmc_state_t *)context;
1039   short ofs;
1040   short irc;
1041   irc = qst->index_pos;
1042
1043   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
1044   {
1045     short diff;
1046     ofs = irc - mcs->pxms_ptmark;
1047     diff = ofs - mcs->pxms_ptofs;
1048
1049     if (diff >= mcs->pxms_ptirc / 2)
1050       diff -= mcs->pxms_ptirc;
1051
1052     if (diff <= -mcs->pxms_ptirc / 2)
1053       diff += mcs->pxms_ptirc;
1054
1055     if (diff < 0)
1056       diff = -diff;
1057
1058     if (diff >= mcs->pxms_ptirc / 6)
1059     {
1060       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
1061     }
1062     else
1063     {
1064       mcs->pxms_ptofs = ofs;
1065       pxmc_set_flag(mcs, PXMS_PHA_b);
1066     }
1067   } /*else {
1068
1069     ofs=irc-mcs->pxms_ptofs;
1070     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
1071       if(ofs>0) {
1072         ofs-=mcs->pxms_ptirc;
1073       } else {
1074         ofs+=mcs->pxms_ptirc;
1075       }
1076     }
1077     mcs->pxms_ptmark=ofs;
1078   }*/
1079
1080   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
1081 }
1082
1083 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
1084 {
1085   short ptofs;
1086   short ptmark;
1087   lpc_qei_state_t *qst = &lpc_qei_state;
1088   int irc = lpc_qei_get_pos(qst);
1089   int idx_rel;
1090
1091   if (!qst->index_occ)
1092     return 0;
1093
1094   idx_rel = qst->index_pos - irc;
1095   idx_rel %= mcs->pxms_ptirc;
1096
1097   if (idx_rel < 0)
1098     idx_rel += mcs->pxms_ptirc;
1099
1100   ptofs = irc - mcs->pxms_ptofs;
1101   ptmark = ptofs + idx_rel;
1102
1103   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
1104   {
1105     if (ptmark > 0)
1106       ptmark -= mcs->pxms_ptirc;
1107     else
1108       ptmark += mcs->pxms_ptirc;
1109   }
1110
1111   if ((unsigned short)ptmark < mcs->pxms_ptirc)
1112   {
1113     mcs->pxms_ptmark = ptmark;
1114   }
1115
1116   return 0;
1117 }
1118
1119 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
1120 {
1121   int res;
1122   long r2acq;
1123   long spd;
1124   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
1125   lpc_qei_state_t *qst = &lpc_qei_state;
1126
1127   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
1128   lpc_qei_setup_index_catch(qst);
1129
1130   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
1131   spd = 1 << PXMC_SUBDIV(mcs);
1132
1133   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1134
1135   return res;
1136 }
1137
1138 int pxmc_lpc_pthalalign_run(void)
1139 {
1140   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
1141 }
1142
1143 static inline void pxmc_sfi_input(void)
1144 {
1145   int var;
1146   pxmc_state_t *mcs;
1147   pxmc_for_each_mcs(var, mcs)
1148   {
1149     /* PXMS_ENI_m - check if input (IRC) update is enabled */
1150     if (mcs->pxms_flg & PXMS_ENI_m)
1151     {
1152       pxmc_call(mcs, mcs->pxms_do_inp);
1153     }
1154   }
1155 }
1156
1157 static inline void pxmc_sfi_controller_and_output(void)
1158 {
1159   int var;
1160   pxmc_state_t *mcs;
1161   pxmc_for_each_mcs(var, mcs)
1162   {
1163     /* PXMS_ENR_m - check if controller is enabled */
1164     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
1165     {
1166
1167       /* If output only is enabled, we skip the controller */
1168       if (mcs->pxms_flg & PXMS_ENR_m)
1169       {
1170
1171         pxmc_call(mcs, mcs->pxms_do_con);
1172
1173         /* PXMS_ERR_m - if axis in error state */
1174         if (mcs->pxms_flg & PXMS_ERR_m)
1175           mcs->pxms_ene = 0;
1176       }
1177
1178       /* for bushless motors, it is necessary to call do_out
1179         even if the controller is not enabled and PWM should be provided. */
1180       pxmc_call(mcs, mcs->pxms_do_out);
1181     }
1182   }
1183 }
1184
1185 static inline void pxmc_sfi_generator(void)
1186 {
1187   int var;
1188   pxmc_state_t *mcs;
1189   pxmc_for_each_mcs(var, mcs)
1190   {
1191     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
1192     if (mcs->pxms_flg & PXMS_ENG_m)
1193     {
1194       pxmc_call(mcs, mcs->pxms_do_gen);
1195     }
1196   }
1197 }
1198
1199 static inline void pxmc_sfi_dbg(void)
1200 {
1201   int var;
1202   pxmc_state_t *mcs;
1203   pxmc_for_each_mcs(var, mcs)
1204   {
1205     if (mcs->pxms_flg & PXMS_DBG_m)
1206     {
1207       pxmc_call(mcs, mcs->pxms_do_deb);
1208     }
1209   }
1210 }
1211
1212 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
1213 {
1214   short ptofs;
1215   short ptmark;
1216   int inp_chan = mcs->pxms_inp_info;
1217   int idx_rel;
1218   long irc = fpga_irc[inp_chan]->count;
1219
1220   if (!pxmc_inp_rocon_is_index_edge(mcs))
1221     return 0;
1222
1223   idx_rel = fpga_irc[inp_chan]->count_index - irc;
1224   idx_rel %= mcs->pxms_ptirc;
1225   if(idx_rel < 0)
1226     idx_rel += mcs->pxms_ptirc;
1227
1228   ptofs = irc-mcs->pxms_ptofs;
1229   ptmark = ptofs+idx_rel;
1230
1231   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
1232     if(ptmark>0)
1233       ptmark -= mcs->pxms_ptirc;
1234     else
1235       ptmark += mcs->pxms_ptirc;
1236   }
1237
1238   if((unsigned short)ptmark < mcs->pxms_ptirc) {
1239     mcs->pxms_ptmark = ptmark;
1240   }
1241   return 0;
1242 }
1243
1244 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
1245 {
1246   int res;
1247   long r2acq;
1248   long spd;
1249   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
1250
1251   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
1252
1253   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
1254   spd=1<<PXMC_SUBDIV(mcs);
1255
1256   /* clear bit indicating previous index */
1257   pxmc_inp_rocon_is_index_edge(mcs);
1258
1259   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1260
1261   return res;
1262 }
1263
1264 int
1265 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
1266 {
1267   int res;
1268
1269   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
1270
1271   /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
1272 #ifndef PXMC_WITH_PT_ZIC
1273   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
1274 #else /*PXMC_WITH_PT_ZIC*/
1275   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
1276 #endif /*PXMC_WITH_PT_ZIC*/
1277
1278   return res;
1279 }
1280
1281 /**
1282  * pxmc_axis_mode - Sets axis mode.[extern API]
1283  * @mcs:        Motion controller state information
1284  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
1285  *              2 .. stepper motor with IRC feedback and PWM ,
1286  *              3 .. stepper motor with PWM control
1287  *              4 .. DC motor with IRC feedback and PWM
1288  *
1289  */
1290 int
1291 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
1292 {
1293   int res;
1294
1295   pxmc_set_const_out(mcs, 0);
1296   pxmc_clear_flag(mcs, PXMS_ENI_b);
1297   pxmc_clear_flag(mcs, PXMS_PHA_b);
1298   pxmc_clear_flag(mcs, PXMS_PTI_b);
1299
1300   if (!mode)
1301   {
1302     /*mode=pxmc_axis_rdmode(mcs);*/
1303     if (!mode)
1304       mode = PXMC_AXIS_MODE_BLDC;
1305   }
1306
1307   res = pxmc_axis_pt4mode(mcs, mode);
1308
1309   pxmc_set_flag(mcs, PXMS_ENI_b);
1310   return res;
1311 }
1312
1313 void pxmc_sfi_isr(void)
1314 {
1315   pxmc_sfi_input();
1316   pxmc_sfi_controller_and_output();
1317   pxmc_sfi_generator();
1318   pxmc_sfi_dbg();
1319   /* Kick LX Master watchdog */
1320   if (pxmc_main_list.pxml_cnt != 0)
1321     *fpga_lx_master_transmitter_wdog = 1;
1322 }
1323
1324 int pxmc_done(void)
1325 {
1326   int var;
1327   pxmc_state_t *mcs;
1328
1329   if (!pxmc_main_list.pxml_cnt)
1330     return 0;
1331
1332   pxmc_for_each_mcs(var, mcs)
1333   {
1334     pxmc_set_const_out(mcs,0);
1335   }
1336
1337   pxmc_main_list.pxml_cnt = 0;
1338   __memory_barrier();
1339
1340   return 0;
1341 }
1342
1343 int pxmc_initialize(void)
1344 {
1345   int res;
1346
1347   pxmc_state_t *mcs = &mcs0;
1348   lpc_qei_state_t *qst = &lpc_qei_state;
1349
1350   *fpga_irc_reset = 1;
1351
1352   /* Initialize QEI module for IRC counting */
1353   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
1354
1355   /* Initialize QEI events processing */
1356   if (lpc_qei_setup_irq(qst) < 0)
1357     return -1;
1358
1359   *fpga_irc_reset = 0;
1360
1361   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
1362
1363   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
1364   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
1365
1366   pxmc_rocon_pwm_master_init();
1367
1368   pxmc_main_list.pxml_cnt = 0;
1369   pxmc_dbg_hist = NULL;
1370   __memory_barrier();
1371   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
1372
1373   return 0;
1374 }