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