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_pwm_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 int
 423 pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 424 {
 425   volatile uint32_t *pwm_reg_a, *pwm_reg_b;
 426   int chan = mcs->pxms_out_info;
 427   int ene = mcs->pxms_ene;
 428
 429   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
 430   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 431
 432   if (ene < 0) {
 433     ene = -ene;
 434     if (ene > 0x7fff)
 435       ene = 0x7fff;
 436     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 437     *pwm_reg_a = 0;
 438     *pwm_reg_b = ene | 0x4000;
 439   } else {
 440     if (ene > 0x7fff)
 441       ene = 0x7fff;
 442     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 443     *pwm_reg_b = 0;
 444     *pwm_reg_a = ene | 0x4000;
 445   }
 446
 447   return 0;
 448 }
 449
 450 /* PWM outputs placed on (PWM1), PWM2, PWM4, PWM6 */
 451 int
 452 pxmc_rocon_pwm_master_init(void)
 453 {
 454   int i;
 455
 456   *fpga_lx_master_reset = 1;
 457   *fpga_lx_master_transmitter_reg = 0;
 458
 459   for (i = 0; i < 8 + 16; i ++)
 460     fpga_lx_master_transmitter_base[i] = 0;
 461
 462   fpga_lx_master_transmitter_base[0] = 0x0808;
 463   fpga_lx_master_transmitter_base[1] = 0x0000;
 464
 465   *fpga_lx_master_reset = 0;
 466
 467   return 0;
 468 }
 469
 470 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
 471 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
 472 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
 473 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
 474 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
 475 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
 476
 477 /* initialize for hard home */
 478 int
 479 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
 480 {
 481   pxmc_set_flag(mcs,PXMS_BSY_b);
 482  #if 0 /* config and speed already set in pxmc_hh */
 483   spd = mcs->pxms_ms;
 484   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
 485   if(!spd) spd = 1;
 486   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
 487     spd = -spd;
 488   mcs->pxms_gen_hsp = spd;
 489  #endif
 490   mcs->pxms_rsfg = 0;
 491   mcs->pxms_gen_htim = 16;
 492   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
 493   return pxmc_rocon_hh_gd10(mcs);
 494 }
 495
 496 /* fill initial mark filter and then decide */
 497 int
 498 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
 499 {
 500   int mark;
 501
 502   if(mcs->pxms_flg & PXMS_ERR_m)
 503     return pxmc_spdnext_gend(mcs);
 504
 505   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
 506   mcs->pxms_rp += mcs->pxms_rs;
 507
 508   mark = pxmc_inp_rocon_is_mark(mcs);
 509
 510   if (mcs->pxms_gen_htim) {
 511     mcs->pxms_gen_htim--;
 512     return 0;
 513   }
 514
 515   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
 516     /* limit switch is not used */
 517     pxmc_inp_rocon_is_index_edge(mcs);
 518     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 519     return 0;
 520   }
 521
 522   if (mark >= 6) {
 523     /* go out from switch */
 524     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
 525   } else {
 526     /* switch is off -> look for it */
 527     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 528   }
 529   return 0;
 530 }
 531
 532 /* go out from switch */
 533 int
 534 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
 535 {
 536   int mark;
 537
 538   if(mcs->pxms_flg & PXMS_ERR_m)
 539     return pxmc_spdnext_gend(mcs);
 540
 541   mark = pxmc_inp_rocon_is_mark(mcs);
 542
 543   if (mark <= 1) {
 544     /* switch is off -> look for it again */
 545     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 546   }
 547
 548   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 549   mcs->pxms_rp += mcs->pxms_rs;
 550
 551   return 0;
 552 }
 553
 554 /* switch is off -> look for it */
 555 int
 556 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
 557 {
 558   long spd;
 559   int mark;
 560
 561   if (mcs->pxms_flg & PXMS_ERR_m)
 562     return pxmc_spdnext_gend(mcs);
 563
 564   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 565   mcs->pxms_rp += mcs->pxms_rs;
 566
 567   mark = pxmc_inp_rocon_is_mark(mcs);
 568
 569   if (mark >= 8){
 570     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
 571     if (!spd)
 572       spd = 1;
 573     mcs->pxms_gen_hsp = spd;
 574     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
 575   }
 576
 577   return 0;
 578 }
 579
 580 /* switch is on -> find edge */
 581 int
 582 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
 583 {
 584   int mark;
 585
 586   if (mcs->pxms_flg & PXMS_ERR_m)
 587     return pxmc_spdnext_gend(mcs);
 588
 589   mark = pxmc_inp_rocon_is_mark(mcs);
 590
 591   if (mark <= 1) {
 592     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
 593       pxmc_inp_rocon_is_index_edge(mcs);
 594       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 595     } else {
 596       pxmc_inp_rocon_ap_zero(mcs);
 597       mcs->pxms_do_gen = pxmc_stop_gi;
 598     }
 599   }
 600
 601   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 602   mcs->pxms_rp += mcs->pxms_rs;
 603
 604   return 0;
 605 }
 606
 607 /* calibrate on revolution index */
 608 int
 609 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
 610 {
 611   if (mcs->pxms_flg & PXMS_ERR_m)
 612     return pxmc_spdnext_gend(mcs);
 613
 614   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 615   mcs->pxms_rp += mcs->pxms_rs;
 616
 617   if (pxmc_inp_rocon_is_index_edge(mcs)){
 618     pxmc_inp_rocon_irc_offset_from_index(mcs);
 619     mcs->pxms_do_gen = pxmc_stop_gi;
 620   }
 621
 622   return 0;
 623 }
 624
 625 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 626 {
 627   return pxmc_rocon_hh_gi;
 628 }
 629
 630 pxmc_state_t mcs0 =
 631 {
 632 pxms_flg:
 633   PXMS_ENI_m,
 634 pxms_do_inp:
 635   pxmc_inp_rocon_inp,
 636 pxms_do_con:
 637   pxmc_pid_con,
 638 pxms_do_out:
 639   pxmc_rocon_pwm_dc_out,
 640   pxms_do_deb: 0,
 641   pxms_do_gen: 0,
 642 pxms_do_ap2hw:
 643   pxmc_inp_rocon_ap2hw,
 644   pxms_ap: 0, pxms_as: 0,
 645   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 646   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 647   pxms_inp_info: 0,
 648   pxms_out_info: 0,
 649   pxms_ene: 0, pxms_erc: 0,
 650   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 651   pxms_me: 0x7e00/*0x7fff*/,
 652 pxms_cfg:
 653   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 654   0x1,
 655
 656   pxms_ptper: 1,
 657   pxms_ptirc: 1000,
 658   /*pxms_ptamp: 0x7fff,*/
 659
 660   pxms_hal: 0x40,
 661 };
 662
 663 pxmc_state_t mcs1 =
 664 {
 665 pxms_flg:
 666   PXMS_ENI_m,
 667 pxms_do_inp:
 668   pxmc_inp_rocon_inp,
 669 pxms_do_con:
 670   pxmc_pid_con,
 671 pxms_do_out:
 672   pxmc_rocon_pwm_dc_out,
 673   pxms_do_deb: 0,
 674   pxms_do_gen: 0,
 675 pxms_do_ap2hw:
 676   pxmc_inp_rocon_ap2hw,
 677   pxms_ap: 0, pxms_as: 0,
 678   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 679   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 680   pxms_inp_info: 1,
 681   pxms_out_info: 2,
 682   pxms_ene: 0, pxms_erc: 0,
 683   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 684   pxms_me: 0x7e00/*0x7fff*/,
 685 pxms_cfg:
 686   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 687   0x1,
 688
 689   pxms_ptper: 1,
 690   pxms_ptirc: 1000,
 691   /*pxms_ptamp: 0x7fff,*/
 692
 693   pxms_hal: 0x40,
 694 };
 695
 696 pxmc_state_t mcs2 =
 697 {
 698 pxms_flg:
 699   PXMS_ENI_m,
 700 pxms_do_inp:
 701   pxmc_inp_rocon_inp,
 702 pxms_do_con:
 703   pxmc_pid_con,
 704 pxms_do_out:
 705   pxmc_rocon_pwm_dc_out,
 706   pxms_do_deb: 0,
 707   pxms_do_gen: 0,
 708 pxms_do_ap2hw:
 709   pxmc_inp_rocon_ap2hw,
 710   pxms_ap: 0, pxms_as: 0,
 711   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 712   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 713   pxms_inp_info: 2,
 714   pxms_out_info: 4,
 715   pxms_ene: 0, pxms_erc: 0,
 716   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 717   pxms_me: 0x7e00/*0x7fff*/,
 718 pxms_cfg:
 719   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 720   0x1,
 721
 722   pxms_ptper: 1,
 723   pxms_ptirc: 1000,
 724   /*pxms_ptamp: 0x7fff,*/
 725
 726   pxms_hal: 0x40,
 727 };
 728
 729 pxmc_state_t mcs3 =
 730 {
 731 pxms_flg:
 732   PXMS_ENI_m,
 733 pxms_do_inp:
 734   pxmc_inp_rocon_inp,
 735 pxms_do_con:
 736   pxmc_pid_con,
 737 pxms_do_out:
 738   pxmc_rocon_pwm_dc_out,
 739   pxms_do_deb: 0,
 740   pxms_do_gen: 0,
 741 pxms_do_ap2hw:
 742   pxmc_inp_rocon_ap2hw,
 743   pxms_ap: 0, pxms_as: 0,
 744   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 745   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 746   pxms_inp_info: 3,
 747   pxms_out_info: 6,
 748   pxms_ene: 0, pxms_erc: 0,
 749   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 750   pxms_me: 0x7e00/*0x7fff*/,
 751 pxms_cfg:
 752   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 753   0x1,
 754
 755   pxms_ptper: 1,
 756   pxms_ptirc: 1000,
 757   /*pxms_ptamp: 0x7fff,*/
 758
 759   pxms_hal: 0x40,
 760 };
 761
 762 pxmc_state_t mcs4 =
 763 {
 764 pxms_flg:
 765   PXMS_ENI_m,
 766 pxms_do_inp:
 767   pxmc_inp_rocon_inp,
 768 pxms_do_con:
 769   pxmc_pid_con,
 770 pxms_do_out:
 771   pxmc_rocon_pwm_dc_out,
 772   pxms_do_deb: 0,
 773   pxms_do_gen: 0,
 774 pxms_do_ap2hw:
 775   pxmc_inp_rocon_ap2hw,
 776   pxms_ap: 0, pxms_as: 0,
 777   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 778   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 779   pxms_inp_info: 4,
 780   pxms_out_info: 8,
 781   pxms_ene: 0, pxms_erc: 0,
 782   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 783   pxms_me: 0x7e00/*0x7fff*/,
 784 pxms_cfg:
 785   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 786   0x1,
 787
 788   pxms_ptper: 1,
 789   pxms_ptirc: 1000,
 790   /*pxms_ptamp: 0x7fff,*/
 791
 792   pxms_hal: 0x40,
 793 };
 794
 795 pxmc_state_t mcs5 =
 796 {
 797 pxms_flg:
 798   PXMS_ENI_m,
 799 pxms_do_inp:
 800   pxmc_inp_rocon_inp,
 801 pxms_do_con:
 802   pxmc_pid_con,
 803 pxms_do_out:
 804   pxmc_rocon_pwm_dc_out,
 805   pxms_do_deb: 0,
 806   pxms_do_gen: 0,
 807 pxms_do_ap2hw:
 808   pxmc_inp_rocon_ap2hw,
 809   pxms_ap: 0, pxms_as: 0,
 810   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 811   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 812   pxms_inp_info: 5,
 813   pxms_out_info: 10,
 814   pxms_ene: 0, pxms_erc: 0,
 815   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 816   pxms_me: 0x7e00/*0x7fff*/,
 817 pxms_cfg:
 818   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 819   0x1,
 820
 821   pxms_ptper: 1,
 822   pxms_ptirc: 1000,
 823   /*pxms_ptamp: 0x7fff,*/
 824
 825   pxms_hal: 0x40,
 826 };
 827
 828 pxmc_state_t mcs6 =
 829 {
 830 pxms_flg:
 831   PXMS_ENI_m,
 832 pxms_do_inp:
 833   pxmc_inp_rocon_inp,
 834 pxms_do_con:
 835   pxmc_pid_con,
 836 pxms_do_out:
 837   pxmc_rocon_pwm_dc_out,
 838   pxms_do_deb: 0,
 839   pxms_do_gen: 0,
 840 pxms_do_ap2hw:
 841   pxmc_inp_rocon_ap2hw,
 842   pxms_ap: 0, pxms_as: 0,
 843   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 844   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 845   pxms_inp_info: 6,
 846   pxms_out_info: 12,
 847   pxms_ene: 0, pxms_erc: 0,
 848   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 849   pxms_me: 0x7e00/*0x7fff*/,
 850 pxms_cfg:
 851   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 852   0x1,
 853
 854   pxms_ptper: 1,
 855   pxms_ptirc: 1000,
 856   /*pxms_ptamp: 0x7fff,*/
 857
 858   pxms_hal: 0x40,
 859 };
 860
 861 pxmc_state_t mcs7 =
 862 {
 863 pxms_flg:
 864   PXMS_ENI_m,
 865 pxms_do_inp:
 866   pxmc_inp_rocon_inp,
 867 pxms_do_con:
 868   pxmc_pid_con,
 869 pxms_do_out:
 870   pxmc_rocon_pwm_dc_out,
 871   pxms_do_deb: 0,
 872   pxms_do_gen: 0,
 873 pxms_do_ap2hw:
 874   pxmc_inp_rocon_ap2hw,
 875   pxms_ap: 0, pxms_as: 0,
 876   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 877   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 878   pxms_inp_info: 7,
 879   pxms_out_info: 14,
 880   pxms_ene: 0, pxms_erc: 0,
 881   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 882   pxms_me: 0x7e00/*0x7fff*/,
 883 pxms_cfg:
 884   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
 885   0x1,
 886
 887   pxms_ptper: 1,
 888   pxms_ptirc: 1000,
 889   /*pxms_ptamp: 0x7fff,*/
 890
 891   pxms_hal: 0x40,
 892 };
 893
 894
 895 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
 896 {&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
 897
 898
 899 pxmc_state_list_t  pxmc_main_list =
 900 {
 901 pxml_arr:
 902   pxmc_main_arr,
 903   pxml_cnt: 0
 904 };
 905
 906 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
 907 {
 908   pxmc_state_t *mcs = (pxmc_state_t *)context;
 909   short ofs;
 910   short irc;
 911   irc = qst->index_pos;
 912
 913   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
 914   {
 915     short diff;
 916     ofs = irc - mcs->pxms_ptmark;
 917     diff = ofs - mcs->pxms_ptofs;
 918
 919     if (diff >= mcs->pxms_ptirc / 2)
 920       diff -= mcs->pxms_ptirc;
 921
 922     if (diff <= -mcs->pxms_ptirc / 2)
 923       diff += mcs->pxms_ptirc;
 924
 925     if (diff < 0)
 926       diff = -diff;
 927
 928     if (diff >= mcs->pxms_ptirc / 6)
 929     {
 930       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 931     }
 932     else
 933     {
 934       mcs->pxms_ptofs = ofs;
 935       pxmc_set_flag(mcs, PXMS_PHA_b);
 936     }
 937   } /*else {
 938
 939     ofs=irc-mcs->pxms_ptofs;
 940     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
 941       if(ofs>0) {
 942         ofs-=mcs->pxms_ptirc;
 943       } else {
 944         ofs+=mcs->pxms_ptirc;
 945       }
 946     }
 947     mcs->pxms_ptmark=ofs;
 948   }*/
 949
 950   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
 951 }
 952
 953 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
 954 {
 955   short ptofs;
 956   short ptmark;
 957   lpc_qei_state_t *qst = &lpc_qei_state;
 958   int irc = lpc_qei_get_pos(qst);
 959   int idx_rel;
 960
 961   if (!qst->index_occ)
 962     return 0;
 963
 964   idx_rel = qst->index_pos - irc;
 965   idx_rel %= mcs->pxms_ptirc;
 966
 967   if (idx_rel < 0)
 968     idx_rel += mcs->pxms_ptirc;
 969
 970   ptofs = irc - mcs->pxms_ptofs;
 971   ptmark = ptofs + idx_rel;
 972
 973   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
 974   {
 975     if (ptmark > 0)
 976       ptmark -= mcs->pxms_ptirc;
 977     else
 978       ptmark += mcs->pxms_ptirc;
 979   }
 980
 981   if ((unsigned short)ptmark < mcs->pxms_ptirc)
 982   {
 983     mcs->pxms_ptmark = ptmark;
 984   }
 985
 986   return 0;
 987 }
 988
 989 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
 990 {
 991   int res;
 992   long r2acq;
 993   long spd;
 994   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
 995   lpc_qei_state_t *qst = &lpc_qei_state;
 996
 997   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
 998   lpc_qei_setup_index_catch(qst);
 999
1000   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
1001   spd = 1 << PXMC_SUBDIV(mcs);
1002
1003   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1004
1005   return res;
1006 }
1007
1008 int pxmc_lpc_pthalalign_run(void)
1009 {
1010   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
1011 }
1012
1013 static inline void pxmc_sfi_input(void)
1014 {
1015   int var;
1016   pxmc_state_t *mcs;
1017   pxmc_for_each_mcs(var, mcs)
1018   {
1019     /* PXMS_ENI_m - check if input (IRC) update is enabled */
1020     if (mcs->pxms_flg & PXMS_ENI_m)
1021     {
1022       pxmc_call(mcs, mcs->pxms_do_inp);
1023     }
1024   }
1025 }
1026
1027 static inline void pxmc_sfi_controller_and_output(void)
1028 {
1029   int var;
1030   pxmc_state_t *mcs;
1031   pxmc_for_each_mcs(var, mcs)
1032   {
1033     /* PXMS_ENR_m - check if controller is enabled */
1034     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
1035     {
1036
1037       /* If output only is enabled, we skip the controller */
1038       if (mcs->pxms_flg & PXMS_ENR_m)
1039       {
1040
1041         pxmc_call(mcs, mcs->pxms_do_con);
1042
1043         /* PXMS_ERR_m - if axis in error state */
1044         if (mcs->pxms_flg & PXMS_ERR_m)
1045           mcs->pxms_ene = 0;
1046       }
1047
1048       /* for bushless motors, it is necessary to call do_out
1049         even if the controller is not enabled and PWM should be provided. */
1050       pxmc_call(mcs, mcs->pxms_do_out);
1051     }
1052   }
1053 }
1054
1055 static inline void pxmc_sfi_generator(void)
1056 {
1057   int var;
1058   pxmc_state_t *mcs;
1059   pxmc_for_each_mcs(var, mcs)
1060   {
1061     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
1062     if (mcs->pxms_flg & PXMS_ENG_m)
1063     {
1064       pxmc_call(mcs, mcs->pxms_do_gen);
1065     }
1066   }
1067 }
1068
1069 static inline void pxmc_sfi_dbg(void)
1070 {
1071   int var;
1072   pxmc_state_t *mcs;
1073   pxmc_for_each_mcs(var, mcs)
1074   {
1075     if (mcs->pxms_flg & PXMS_DBG_m)
1076     {
1077       pxmc_call(mcs, mcs->pxms_do_deb);
1078     }
1079   }
1080 }
1081
1082 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
1083 {
1084   short ptofs;
1085   short ptmark;
1086   int inp_chan = mcs->pxms_inp_info;
1087   int idx_rel;
1088   long irc = fpga_irc[inp_chan]->count;
1089
1090   if (!pxmc_inp_rocon_is_index_edge(mcs))
1091     return 0;
1092
1093   idx_rel = fpga_irc[inp_chan]->count_index - irc;
1094   idx_rel %= mcs->pxms_ptirc;
1095   if(idx_rel < 0)
1096     idx_rel += mcs->pxms_ptirc;
1097
1098   ptofs = irc-mcs->pxms_ptofs;
1099   ptmark = ptofs+idx_rel;
1100
1101   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
1102     if(ptmark>0)
1103       ptmark -= mcs->pxms_ptirc;
1104     else
1105       ptmark += mcs->pxms_ptirc;
1106   }
1107
1108   if((unsigned short)ptmark < mcs->pxms_ptirc) {
1109     mcs->pxms_ptmark = ptmark;
1110   }
1111   return 0;
1112 }
1113
1114 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
1115 {
1116   int res;
1117   long r2acq;
1118   long spd;
1119   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
1120
1121   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
1122
1123   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
1124   spd=1<<PXMC_SUBDIV(mcs);
1125
1126   /* clear bit indicating previous index */
1127   pxmc_inp_rocon_is_index_edge(mcs);
1128
1129   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1130
1131   return res;
1132 }
1133
1134 int
1135 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
1136 {
1137   int res;
1138
1139   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
1140
1141   /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
1142 #ifndef PXMC_WITH_PT_ZIC
1143   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
1144 #else /*PXMC_WITH_PT_ZIC*/
1145   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
1146 #endif /*PXMC_WITH_PT_ZIC*/
1147
1148   return res;
1149 }
1150
1151 /**
1152  * pxmc_axis_mode - Sets axis mode.[extern API]
1153  * @mcs:        Motion controller state information
1154  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
1155  *              2 .. stepper motor with IRC feedback and PWM ,
1156  *              3 .. stepper motor with PWM control
1157  *              4 .. DC motor with IRC feedback and PWM
1158  *
1159  */
1160 int
1161 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
1162 {
1163   int res;
1164
1165   pxmc_set_const_out(mcs, 0);
1166   pxmc_clear_flag(mcs, PXMS_ENI_b);
1167   pxmc_clear_flag(mcs, PXMS_PHA_b);
1168   pxmc_clear_flag(mcs, PXMS_PTI_b);
1169
1170   if (!mode)
1171   {
1172     /*mode=pxmc_axis_rdmode(mcs);*/
1173     if (!mode)
1174       mode = PXMC_AXIS_MODE_BLDC;
1175   }
1176
1177   res = pxmc_axis_pt4mode(mcs, mode);
1178
1179   pxmc_set_flag(mcs, PXMS_ENI_b);
1180   return res;
1181 }
1182
1183 void pxmc_sfi_isr(void)
1184 {
1185   pxmc_sfi_input();
1186   pxmc_sfi_controller_and_output();
1187   pxmc_sfi_generator();
1188   pxmc_sfi_dbg();
1189   /* Kick LX Master watchdog */
1190   if (pxmc_main_list.pxml_cnt != 0)
1191     *fpga_lx_master_transmitter_wdog = 1;
1192 }
1193
1194 int pxmc_done(void)
1195 {
1196   int var;
1197   pxmc_state_t *mcs;
1198
1199   if (!pxmc_main_list.pxml_cnt)
1200     return 0;
1201
1202   pxmc_for_each_mcs(var, mcs)
1203   {
1204     pxmc_set_const_out(mcs,0);
1205   }
1206
1207   pxmc_main_list.pxml_cnt = 0;
1208   __memory_barrier();
1209
1210   return 0;
1211 }
1212
1213 int pxmc_initialize(void)
1214 {
1215   int res;
1216
1217   pxmc_state_t *mcs = &mcs0;
1218   lpc_qei_state_t *qst = &lpc_qei_state;
1219
1220   *fpga_irc_reset = 1;
1221
1222   /* Initialize QEI module for IRC counting */
1223   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
1224
1225   /* Initialize QEI events processing */
1226   if (lpc_qei_setup_irq(qst) < 0)
1227     return -1;
1228
1229   *fpga_irc_reset = 0;
1230
1231   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
1232
1233   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
1234   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
1235
1236   pxmc_rocon_pwm_master_init();
1237
1238   pxmc_main_list.pxml_cnt = 0;
1239   pxmc_dbg_hist = NULL;
1240   __memory_barrier();
1241   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
1242
1243   return 0;
1244 }