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