src/app/rpi-pmsm-test1/appl_pxmc.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components.
   3
   4   appl_pxmc.c - SPI connected motor control board specific
   5                 extensionsposition
   6
   7   Copyright (C) 2001-2015 by Pavel Pisa - originator
   8                           pisa@cmp.felk.cvut.cz
   9             (C) 2001-2015 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_internal.h>
  23 #include <pxmc_inp_common.h>
  24 #include <pxmc_gen_info.h>
  25 #include <pxmc_dq_trans.h>
  26 #include <pxmc_sin_fixed.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29
  30 #if !defined(clzl) && !defined(HAVE_CLZL)
  31 #define clzl __builtin_clzl
  32 #endif
  33
  34 #include "appl_defs.h"
  35 #include "appl_pxmc.h"
  36 #include "appl_utils.h"
  37
  38 #include "pxmc_spimc.h"
  39
  40 pthread_t pxmc_base_thread_id;
  41
  42 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
  43                            unsigned long index_irc, int diff2err);
  44
  45 #define PXML_MAIN_CNT 1
  46
  47 #undef PXMC_WITH_PT_ZIC
  48 #define PXMC_PT_ZIC_MASK 0x8000
  49
  50 #define HAL_ERR_SENSITIVITY 20
  51 #define HAL_ERR_MAX_COUNT    5
  52
  53 #define PXMC_LXPWR_PWM_CYCLE 2048
  54
  55 unsigned pxmc_spimc_pwm_magnitude = PXMC_LXPWR_PWM_CYCLE;
  56
  57 unsigned pxmc_spimc_mark_filt[PXML_MAIN_CNT];
  58 unsigned pxmc_spimc_lxpwr_chips = 0;
  59 int appl_errstop_mode = 0;
  60
  61 static inline
  62 pxmc_spimc_state_t *pxmc_state2spimc_state(pxmc_state_t *mcs)
  63 {
  64   pxmc_spimc_state_t *mcsrc;
  65  #ifdef UL_CONTAINEROF
  66   mcsrc = UL_CONTAINEROF(mcs, pxmc_spimc_state_t, base);
  67  #else /*UL_CONTAINEROF*/
  68   mcsrc = (pxmc_spimc_state_t*)((char*)mcs - __builtin_offsetof(pxmc_spimc_state_t, base));
  69  #endif /*UL_CONTAINEROF*/
  70   return mcsrc;
  71 }
  72
  73 const uint8_t onesin10bits[1024]={
  74   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,
  75   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,
  76   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,
  77   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,
  78   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,
  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   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   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,
  83   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,
  84   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,
  85   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,
  86   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,
  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   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,
  89   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,
  90   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,
  91   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,
  92   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,
  93   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,
  94   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,
  95   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,
  96   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,
  97   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,
  98   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,
  99   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,
 100   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,
 101   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,
 102   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,
 103   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,
 104   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,
 105   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
 106 };
 107
 108 int
 109 pxmc_inp_spimc_inp(struct pxmc_state *mcs)
 110 {
 111   pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs);
 112   int chan=mcs->pxms_inp_info;
 113   long irc;
 114   long pos;
 115
 116   /* read position from hardware */
 117   irc = mcsrc->spimc_state->act_pos;
 118   irc += mcsrc->spimc_state->pos_offset;
 119   pos = irc << PXMC_SUBDIV(mcs);
 120   mcs->pxms_as = pos - mcs->pxms_ap;
 121   mcs->pxms_ap = pos;
 122
 123   /* Running of the motor commutator */
 124   if (mcs->pxms_flg & PXMS_PTI_m)
 125     pxmc_irc_16bit_commindx(mcs, irc);
 126
 127   return 0;
 128 }
 129
 130 int
 131 pxmc_inp_spimc_ap2hw(struct pxmc_state *mcs)
 132 {
 133   pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs);
 134   int chan=mcs->pxms_inp_info;
 135   long irc;
 136   long pos_diff;
 137
 138   irc = mcsrc->spimc_state->act_pos;
 139   pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
 140
 141   irc = pos_diff >> PXMC_SUBDIV(mcs);
 142
 143   /* Adjust phase table alignemt to modified IRC readout  */
 144   mcs->pxms_ptofs += irc - mcsrc->spimc_state->pos_offset;
 145
 146   mcsrc->spimc_state->pos_offset = irc;
 147   return 0;
 148 }
 149
 150 inline unsigned
 151 pxmc_spimc_bldc_hal_rd(pxmc_state_t *mcs)
 152 {
 153   pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs);
 154   unsigned h = 0;
 155   int chan = mcs->pxms_out_info;
 156
 157   h  = mcsrc->spimc_state->hal_sensors;
 158
 159   /* return 3 bits corresponding to the HAL senzor input */
 160   return h;
 161 }
 162
 163 #if 1
 164 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 165 {
 166   [0] = 0xff,
 167   [7] = 0xff,
 168   [1] = 0, /*0*/
 169   [5] = 1, /*1*/
 170   [4] = 2, /*2*/
 171   [6] = 3, /*3*/
 172   [2] = 4, /*4*/
 173   [3] = 5, /*5*/
 174 };
 175 #else
 176 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 177 {
 178   [0] = 0xff,
 179   [7] = 0xff,
 180   [1] = 0, /*0*/
 181   [5] = 5, /*1*/
 182   [4] = 4, /*2*/
 183   [6] = 3, /*3*/
 184   [2] = 2, /*4*/
 185   [3] = 1, /*5*/
 186 };
 187 #endif
 188
 189 int pxmc_spimc_pwm_direct_wr(unsigned chan, unsigned pwm, int en)
 190 {
 191
 192   return 0;
 193 }
 194
 195 /**
 196  * pxmc_spimc_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 197  * @mcs:  Motion controller state information
 198  */
 199 /*static*/ inline void
 200 pxmc_spimc_pwm3ph_wr(pxmc_state_t *mcs, uint32_t pwm1, uint32_t pwm2, uint32_t pwm3)
 201 {
 202   pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs);
 203   int chan = mcs->pxms_out_info;
 204
 205   mcsrc->spimc_state->pwm[0] = pwm1;
 206   mcsrc->spimc_state->pwm[1] = pwm2;
 207   mcsrc->spimc_state->pwm[2] = pwm3;
 208 }
 209
 210 static inline void
 211 pxmc_spimc_process_hal_error(struct pxmc_state *mcs)
 212 {
 213   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
 214   {
 215     pxmc_set_errno(mcs, PXMS_E_HAL);
 216     mcs->pxms_ene = 0;
 217     mcs->pxms_halerc--;
 218   }
 219   else
 220     mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
 221 }
 222
 223 pxmc_inp_spimc_ptofs_from_index_poll(struct pxmc_state *mcs, int diff2err)
 224 {
 225   pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs);
 226   int chan=mcs->pxms_inp_info;
 227   long irc;
 228   long index_irc;
 229
 230   if (1)
 231     return 0;
 232
 233   irc = mcsrc->spimc_state->act_pos + mcsrc->spimc_state->pos_offset;
 234   index_irc = mcsrc->spimc_state->index_pos + mcsrc->spimc_state->pos_offset;
 235
 236   return pxmc_ptofs_from_index(mcs, irc, index_irc, diff2err);
 237 }
 238
 239 /**
 240  * pxmc_spimc_pwm3ph_out - Phase output for brush-less 3-phase motor
 241  * @mcs:  Motion controller state information
 242  */
 243 int
 244 pxmc_spimc_pwm3ph_out(pxmc_state_t *mcs)
 245 {
 246   pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs);
 247   typeof(mcs->pxms_ptvang) ptvang;
 248   int sync_mode = 0;
 249   unsigned char hal_pos;
 250   uint32_t pta;
 251   int32_t  sin_val, cos_val;
 252   int32_t  pwm_d,   pwm_q;
 253   int32_t  pwm_alp, pwm_bet;
 254   uint32_t pwm1, pwm2, pwm3;
 255   short ene;
 256   int wind_current[4];
 257
 258   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 259       (mcs->pxms_flg & PXMS_PRA_m))
 260   {
 261     short ptindx;
 262     short ptirc = mcs->pxms_ptirc;
 263     short divisor = mcs->pxms_ptper * 6;
 264
 265     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_spimc_bldc_hal_rd(mcs)];
 266
 267     if (hal_pos == 0xff)
 268     {
 269       if (mcs->pxms_ene)
 270         pxmc_spimc_process_hal_error(mcs);
 271     }
 272     else
 273     {
 274       if (mcs->pxms_halerc)
 275         mcs->pxms_halerc--;
 276
 277       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 278
 279       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
 280       {
 281         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
 282         {
 283           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 284
 285           if ((ptindx > ptindx_prev + ptirc / 2) ||
 286               (ptindx_prev > ptindx + ptirc / 2))
 287           {
 288             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 289
 290             if (ptindx < 0)
 291               ptindx += ptirc;
 292           }
 293           else
 294           {
 295             ptindx = (ptindx_prev + ptindx) / 2;
 296           }
 297
 298           mcs->pxms_ptindx = ptindx;
 299
 300           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 301
 302           pxmc_set_flag(mcs, PXMS_PTI_b);
 303           pxmc_clear_flag(mcs, PXMS_PRA_b);
 304         }
 305         else
 306         {
 307           if (!(mcs->pxms_flg & PXMS_PTI_m))
 308             mcs->pxms_ptindx = ptindx;
 309         }
 310       } else {
 311         /* if phase table position to mask is know do fine phase table alignment */
 312         if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) {
 313           int res;
 314           res = pxmc_inp_spimc_ptofs_from_index_poll(mcs, 0);
 315           if (res < 0) {
 316             pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 317           } else if (res) {
 318             pxmc_set_flag(mcs, PXMS_PTI_b);
 319             pxmc_set_flag(mcs, PXMS_PHA_b);
 320           }
 321         }
 322       }
 323       mcs->pxms_hal = hal_pos;
 324     }
 325   }
 326
 327   {
 328     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 329     /* FIXME - check winding current against limit */
 330     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 331   }
 332
 333   if (!sync_mode) {
 334     ptvang = mcs->pxms_ptvang;
 335     pwm_d = 0;
 336     pwm_q = mcs->pxms_ene;
 337   } else {
 338     ptvang = 0;
 339     pwm_d = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
 340     pwm_q = 0;
 341   }
 342
 343   if (pwm_d || pwm_q) {
 344     int32_t indx;
 345     uint32_t ptscale_mult = mcs->pxms_ptscale_mult |
 346                   (mcs->pxms_ptscale_shift << 16);
 347
 348     indx = mcs->pxms_ptindx;
 349
 350     if (indx < 0)
 351       pta = indx + mcs->pxms_ptirc;
 352     else
 353       pta = indx;
 354
 355     pta *= ptscale_mult;
 356
 357     pta -= PXMC_SIN_FIX_2PI3;
 358
 359     pxmc_sincos_fixed_inline(&sin_val, &cos_val, pta, 16);
 360     pxmc_dq2alpbet(&pwm_alp, &pwm_bet, pwm_d, pwm_q, sin_val, cos_val);
 361     pxmc_alpbet2pwm3ph(&pwm1, &pwm2, &pwm3, pwm_alp, pwm_bet);
 362
 363     /*printf("pwm d %4d q %4d alp %4d bet %4d a %4d b %4d c %4d\n",
 364            pwm_d, pwm_q, pwm_alp >> 16, pwm_bet >> 16, pwm1, pwm2, pwm3);*/
 365
 366
 367 #ifdef PXMC_WITH_PT_ZIC
 368     if (labs(mcs->pxms_as) < (10 << PXMC_SUBDIV(mcs)))
 369     {
 370     }
 371 #endif /*PXMC_WITH_PT_ZIC*/
 372
 373     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 374     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 375     {
 376       unsigned long pwm_mag = pxmc_spimc_pwm_magnitude;
 377       if (0)
 378         pwm1 = SPIMC_PWM_SHUTDOWN;
 379       else
 380         pwm1 = (((unsigned long long)pwm1 * pwm_mag) >> 15) | SPIMC_PWM_ENABLE;
 381       if (0)
 382         pwm2 = SPIMC_PWM_SHUTDOWN;
 383       else
 384         pwm2 = (((unsigned long long)pwm2 * pwm_mag) >> 15) | SPIMC_PWM_ENABLE;
 385       if (0)
 386         pwm3 = SPIMC_PWM_SHUTDOWN;
 387       else
 388         pwm3 = (((unsigned long long)pwm3 * pwm_mag) >> 15) | SPIMC_PWM_ENABLE;
 389     }
 390     pxmc_spimc_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
 391
 392   }
 393   else
 394   {
 395     pxmc_spimc_pwm3ph_wr(mcs, 0, 0, 0);
 396   }
 397
 398   /*printf("hal_pos %d pwm %4d %4d %4d\n", hal_pos, pwm1 & 0xffff,
 399                                   pwm2 & 0xffff, pwm3  & 0xffff);*/
 400
 401   return 0;
 402 }
 403
 404 /**
 405  * pxmc_spimc_pwm_dc_out - DC motor CW and CCW PWM output
 406  * @mcs:  Motion controller state information
 407  */
 408 int
 409 pxmc_spimc_pwm_dc_out(pxmc_state_t *mcs)
 410 {
 411   int chan = mcs->pxms_out_info;
 412   int ene = mcs->pxms_ene;
 413
 414
 415   if (ene < 0) {
 416     ene = -ene;
 417     if (ene > 0x7fff)
 418       ene = 0x7fff;
 419     ene = (ene * (pxmc_spimc_pwm_magnitude + 5)) >> 15;
 420     /**pwm_reg_a = 0;*/
 421     /**pwm_reg_b = ene | 0x4000;*/
 422   } else {
 423     if (ene > 0x7fff)
 424       ene = 0x7fff;
 425     ene = (ene * (pxmc_spimc_pwm_magnitude + 5)) >> 15;
 426     /**pwm_reg_b = 0;*/
 427     /**pwm_reg_a = ene | 0x4000;*/
 428   }
 429
 430   return 0;
 431 }
 432
 433 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
 434                            unsigned long index_irc, int diff2err)
 435 {
 436   long ofsl;
 437   short ofs;
 438
 439   ofs = ofsl = index_irc - mcs->pxms_ptmark;
 440
 441   if (diff2err) {
 442     short diff;
 443     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
 444     if (diff >= mcs->pxms_ptirc / 2)
 445       diff -= mcs->pxms_ptirc;
 446     if (diff <= -mcs->pxms_ptirc / 2)
 447       diff += mcs->pxms_ptirc;
 448     if (diff < 0)
 449       diff = -diff;
 450     if(diff >= mcs->pxms_ptirc / 6) {
 451       return -1;
 452     }
 453   } else {
 454     long diff;
 455     diff = (unsigned long)ofsl - irc;
 456     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
 457   }
 458
 459   mcs->pxms_ptofs = ofs;
 460
 461   return 1;
 462 }
 463
 464 /**
 465  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
 466  * @mcs:        Motion controller state information
 467  */
 468 int
 469 pxmc_dummy_con(pxmc_state_t *mcs)
 470 {
 471   return 0;
 472 }
 473
 474 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 475 {
 476   return NULL;
 477 }
 478
 479 int pxmc_fill_ptscale_for_sin_fixed(pxmc_state_t *mcs)
 480 {
 481   unsigned long ptirc = mcs->pxms_ptirc;
 482   unsigned long ptper = mcs->pxms_ptper;
 483   unsigned long scale_mult;
 484   unsigned long scale_shift;
 485   int shift;
 486
 487   shift = clzl(ptper);
 488   ptper <<= shift;
 489
 490   scale_mult = (ptper + ptirc / 2) / ptirc;
 491   if (shift < 32)
 492     scale_mult <<= 32 - shift;
 493   if (shift > 32)
 494     scale_mult >>= shift - 32;
 495
 496   mcs->pxms_ptscale_mult = scale_mult & 0xffff;
 497   mcs->pxms_ptscale_shift = scale_mult >> 16;
 498
 499   return 0;
 500 }
 501
 502 spimc_state_t spimc_state0 = {
 503  .spi_dev = "/dev/spidev0.1",
 504 };
 505
 506 pxmc_spimc_state_t mcs0 =
 507 {
 508 .base = {
 509 .pxms_flg =
 510   PXMS_ENI_m,
 511 .pxms_do_inp =
 512   pxmc_inp_spimc_inp,
 513 .pxms_do_con =
 514   pxmc_pid_con /*pxmc_dummy_con*/,
 515 .pxms_do_out =
 516   pxmc_spimc_pwm3ph_out /*pxmc_spimc_pwm_dc_out*/,
 517   .pxms_do_deb = 0,
 518   .pxms_do_gen = 0,
 519 .pxms_do_ap2hw =
 520   pxmc_inp_spimc_ap2hw,
 521   .pxms_ap = 0, .pxms_as = 0,
 522   .pxms_rp = 55 * 256, .pxms_rs = 0,
 523  #ifndef PXMC_WITH_FIXED_SUBDIV
 524   .pxms_subdiv = 8,
 525  #endif /*PXMC_WITH_FIXED_SUBDIV*/
 526   .pxms_md = 800 << 8, .pxms_ms = 500, .pxms_ma = 10,
 527   .pxms_inp_info = 0,
 528   .pxms_out_info = 0,
 529   .pxms_ene = 0, .pxms_erc = 0,
 530   .pxms_p = 40, .pxms_i = 10, .pxms_d = 100, .pxms_s1 = 0, .pxms_s2 = 0,
 531   .pxms_me = 0x7e00/*0x7fff*/,
 532 .pxms_cfg =
 533   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
 534   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
 535   PXMS_CFG_I2PT_m * 0 | 0x2,
 536
 537   .pxms_ptper = 1,
 538   .pxms_ptirc = 1000,
 539   .pxms_ptmark = 1180,
 540   /*.pxms_ptamp = 0x7fff,*/
 541
 542   .pxms_hal = 0x40,
 543 },
 544   .spimc_state = &spimc_state0,
 545   .cur_d_p = 150,
 546   .cur_d_i = 6000,
 547   .cur_q_p = 150,
 548   .cur_q_i = 6000,
 549   .cur_hold = 200,
 550 };
 551
 552
 553 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] = {&mcs0.base};
 554
 555
 556 pxmc_state_list_t  pxmc_main_list =
 557 {
 558 .pxml_arr =
 559   pxmc_main_arr,
 560   .pxml_cnt = 0
 561 };
 562
 563
 564 static inline void pxmc_sfi_input(void)
 565 {
 566   int var;
 567   pxmc_state_t *mcs;
 568   pxmc_for_each_mcs(var, mcs)
 569   {
 570     /* PXMS_ENI_m - check if input (IRC) update is enabled */
 571     if (mcs->pxms_flg & PXMS_ENI_m)
 572     {
 573       pxmc_call(mcs, mcs->pxms_do_inp);
 574     }
 575   }
 576 }
 577
 578 static inline void pxmc_sfi_controller_and_output(void)
 579 {
 580   int var;
 581   pxmc_state_t *mcs;
 582   pxmc_for_each_mcs(var, mcs)
 583   {
 584     /* PXMS_ENR_m - check if controller is enabled */
 585     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
 586     {
 587
 588       /* If output only is enabled, we skip the controller */
 589       if (mcs->pxms_flg & PXMS_ENR_m)
 590       {
 591
 592         pxmc_call(mcs, mcs->pxms_do_con);
 593
 594         /* PXMS_ERR_m - if axis in error state */
 595         if (mcs->pxms_flg & PXMS_ERR_m)
 596           mcs->pxms_ene = 0;
 597       }
 598
 599       /* for bushless motors, it is necessary to call do_out
 600         even if the controller is not enabled and PWM should be provided. */
 601       pxmc_call(mcs, mcs->pxms_do_out);
 602     }
 603   }
 604 }
 605
 606 static inline void pxmc_sfi_generator(void)
 607 {
 608   int var;
 609   pxmc_state_t *mcs;
 610   pxmc_for_each_mcs(var, mcs)
 611   {
 612     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
 613     if (mcs->pxms_flg & PXMS_ENG_m)
 614     {
 615       pxmc_call(mcs, mcs->pxms_do_gen);
 616     }
 617   }
 618 }
 619
 620 static inline void pxmc_sfi_dbg(void)
 621 {
 622   int var;
 623   pxmc_state_t *mcs;
 624   pxmc_for_each_mcs(var, mcs)
 625   {
 626     if (mcs->pxms_flg & PXMS_DBG_m)
 627     {
 628       pxmc_call(mcs, mcs->pxms_do_deb);
 629     }
 630   }
 631 }
 632
 633 static inline void pxmc_spimc_rq_transfer(void)
 634 {
 635   int var;
 636   pxmc_state_t *mcs;
 637   pxmc_spimc_state_t *mcsrc;
 638
 639   pxmc_for_each_mcs(var, mcs)
 640   {
 641     mcsrc = pxmc_state2spimc_state(mcs);
 642     spimc_transfer(mcsrc->spimc_state);
 643   }
 644 }
 645
 646 void pxmc_samplig_period(void)
 647 {
 648   pxmc_sfi_input();
 649   pxmc_sfi_controller_and_output();
 650   pxmc_spimc_rq_transfer();
 651   pxmc_sfi_generator();
 652   pxmc_sfi_dbg();
 653
 654   do_pxmc_coordmv();
 655 }
 656
 657 void *pxmc_base_thread(void *arg)
 658 {
 659   sample_period_t sample_period;
 660   sample_period_setup(&sample_period, 1000 * 1000);
 661
 662   do {
 663     pxmc_samplig_period();
 664     sample_period_wait_next(&sample_period);
 665   } while(1);
 666 }
 667
 668 int pxmc_clear_power_stop(void)
 669 {
 670   return 0;
 671 }
 672
 673 int pxmc_process_state_check(unsigned long *pbusy_bits,
 674                              unsigned long *perror_bits)
 675 {
 676   unsigned short flg;
 677   unsigned short chan;
 678   unsigned long busy_bits = 0;
 679   unsigned long error_bits = 0;
 680   pxmc_state_t *mcs;
 681   flg=0;
 682   pxmc_for_each_mcs(chan, mcs) {
 683     if(mcs) {
 684       flg |= mcs->pxms_flg;
 685       if (mcs->pxms_flg & PXMS_BSY_m)
 686         busy_bits |= 1 << chan;
 687       if (mcs->pxms_flg & PXMS_ERR_m)
 688         error_bits |= 1 << chan;
 689     }
 690   }
 691   if (appl_errstop_mode) {
 692     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
 693       pxmc_for_each_mcs(chan, mcs) {
 694         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
 695           pxmc_stop(mcs, 0);
 696         }
 697       }
 698     }
 699   }
 700
 701   if (pbusy_bits != NULL)
 702     *pbusy_bits = busy_bits;
 703   if (error_bits != NULL)
 704     *perror_bits = error_bits;
 705
 706   return flg;
 707 }
 708
 709 /**
 710  * pxmc_axis_mode - Sets axis mode.[extern API]
 711  * @mcs:        Motion controller state information
 712  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
 713  *              2 .. stepper motor with IRC feedback and PWM ,
 714  *              3 .. stepper motor with PWM control
 715  *              4 .. DC motor with IRC feedback and PWM
 716  *
 717  */
 718 int
 719 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 720 {
 721   return 0;
 722 }
 723
 724 int pxmc_done(void)
 725 {
 726   int var;
 727   pxmc_state_t *mcs;
 728
 729   if (!pxmc_main_list.pxml_cnt)
 730     return 0;
 731
 732   pxmc_for_each_mcs(var, mcs)
 733   {
 734     pxmc_axis_release(mcs);
 735   }
 736
 737   pxmc_main_list.pxml_cnt = 0;
 738   __memory_barrier();
 739
 740   return 0;
 741 }
 742
 743 int pxmc_initialize(void)
 744 {
 745   int res;
 746
 747   pxmc_main_list.pxml_cnt = 0;
 748   pxmc_dbg_hist = NULL;
 749
 750   if (spimc_init(mcs0.spimc_state) < 0)
 751     return -1;
 752
 753   pxmc_fill_ptscale_for_sin_fixed(pxmc_main_list.pxml_arr[0]);
 754
 755   __memory_barrier();
 756   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
 757
 758   if (create_rt_task(&pxmc_base_thread_id, 60, pxmc_base_thread, NULL) < 0)
 759     return -1;
 760
 761   return 0;
 762 }