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