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