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