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   if (ene < 0) {
 415     ene = -ene;
 416     if (ene > 0x7fff)
 417       ene = 0x7fff;
 418     ene = (ene * (pxmc_spimc_pwm_magnitude + 5)) >> 15;
 419     pxmc_spimc_pwm3ph_wr(mcs, SPIMC_PWM_ENABLE, ene | SPIMC_PWM_ENABLE, 0);
 420   } else {
 421     if (ene > 0x7fff)
 422       ene = 0x7fff;
 423     ene = (ene * (pxmc_spimc_pwm_magnitude + 5)) >> 15;
 424     pxmc_spimc_pwm3ph_wr(mcs, ene | SPIMC_PWM_ENABLE, SPIMC_PWM_ENABLE, 0);
 425   }
 426
 427   return 0;
 428 }
 429
 430 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
 431                            unsigned long index_irc, int diff2err)
 432 {
 433   long ofsl;
 434   short ofs;
 435
 436   ofs = ofsl = index_irc - mcs->pxms_ptmark;
 437
 438   if (diff2err) {
 439     short diff;
 440     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
 441     if (diff >= mcs->pxms_ptirc / 2)
 442       diff -= mcs->pxms_ptirc;
 443     if (diff <= -mcs->pxms_ptirc / 2)
 444       diff += mcs->pxms_ptirc;
 445     if (diff < 0)
 446       diff = -diff;
 447     if(diff >= mcs->pxms_ptirc / 6) {
 448       return -1;
 449     }
 450   } else {
 451     long diff;
 452     diff = (unsigned long)ofsl - irc;
 453     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
 454   }
 455
 456   mcs->pxms_ptofs = ofs;
 457
 458   return 1;
 459 }
 460
 461 /**
 462  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
 463  * @mcs:        Motion controller state information
 464  */
 465 int
 466 pxmc_dummy_con(pxmc_state_t *mcs)
 467 {
 468   return 0;
 469 }
 470
 471 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 472 {
 473   return NULL;
 474 }
 475
 476 int pxmc_fill_ptscale_for_sin_fixed(pxmc_state_t *mcs)
 477 {
 478   unsigned long ptirc = mcs->pxms_ptirc;
 479   unsigned long ptper = mcs->pxms_ptper;
 480   unsigned long scale_mult;
 481   unsigned long scale_shift;
 482   int shift;
 483
 484   shift = clzl(ptper);
 485   ptper <<= shift;
 486
 487   scale_mult = (ptper + ptirc / 2) / ptirc;
 488   if (shift < 32)
 489     scale_mult <<= 32 - shift;
 490   if (shift > 32)
 491     scale_mult >>= shift - 32;
 492
 493   mcs->pxms_ptscale_mult = scale_mult & 0xffff;
 494   mcs->pxms_ptscale_shift = scale_mult >> 16;
 495
 496   return 0;
 497 }
 498
 499 spimc_state_t spimc_state0 = {
 500  .spi_dev = "/dev/spidev0.1",
 501 };
 502
 503 pxmc_spimc_state_t mcs0 =
 504 {
 505 .base = {
 506 .pxms_flg =
 507   PXMS_ENI_m,
 508 .pxms_do_inp =
 509   pxmc_inp_spimc_inp,
 510 .pxms_do_con =
 511   pxmc_pid_con /*pxmc_dummy_con*/,
 512 .pxms_do_out =
 513   pxmc_spimc_pwm3ph_out /*pxmc_spimc_pwm_dc_out*/,
 514   .pxms_do_deb = 0,
 515   .pxms_do_gen = 0,
 516 .pxms_do_ap2hw =
 517   pxmc_inp_spimc_ap2hw,
 518   .pxms_ap = 0, .pxms_as = 0,
 519   .pxms_rp = 55 * 256, .pxms_rs = 0,
 520  #ifndef PXMC_WITH_FIXED_SUBDIV
 521   .pxms_subdiv = 8,
 522  #endif /*PXMC_WITH_FIXED_SUBDIV*/
 523   .pxms_md = 800 << 8, .pxms_ms = 500, .pxms_ma = 10,
 524   .pxms_inp_info = 0,
 525   .pxms_out_info = 0,
 526   .pxms_ene = 0, .pxms_erc = 0,
 527   .pxms_p = 40, .pxms_i = 10, .pxms_d = 100, .pxms_s1 = 0, .pxms_s2 = 0,
 528   .pxms_me = 0x7e00/*0x7fff*/,
 529 .pxms_cfg =
 530   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
 531   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
 532   PXMS_CFG_I2PT_m * 0 | 0x2,
 533
 534   .pxms_ptper = 1,
 535   .pxms_ptirc = 1000,
 536   .pxms_ptmark = 1180,
 537   /*.pxms_ptamp = 0x7fff,*/
 538
 539   .pxms_hal = 0x40,
 540 },
 541   .spimc_state = &spimc_state0,
 542   .cur_d_p = 150,
 543   .cur_d_i = 6000,
 544   .cur_q_p = 150,
 545   .cur_q_i = 6000,
 546   .cur_hold = 200,
 547 };
 548
 549
 550 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] = {&mcs0.base};
 551
 552
 553 pxmc_state_list_t  pxmc_main_list =
 554 {
 555 .pxml_arr =
 556   pxmc_main_arr,
 557   .pxml_cnt = 0
 558 };
 559
 560
 561 static inline void pxmc_sfi_input(void)
 562 {
 563   int var;
 564   pxmc_state_t *mcs;
 565   pxmc_for_each_mcs(var, mcs)
 566   {
 567     /* PXMS_ENI_m - check if input (IRC) update is enabled */
 568     if (mcs->pxms_flg & PXMS_ENI_m)
 569     {
 570       pxmc_call(mcs, mcs->pxms_do_inp);
 571     }
 572   }
 573 }
 574
 575 static inline void pxmc_sfi_controller_and_output(void)
 576 {
 577   int var;
 578   pxmc_state_t *mcs;
 579   pxmc_for_each_mcs(var, mcs)
 580   {
 581     /* PXMS_ENR_m - check if controller is enabled */
 582     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
 583     {
 584
 585       /* If output only is enabled, we skip the controller */
 586       if (mcs->pxms_flg & PXMS_ENR_m)
 587       {
 588
 589         pxmc_call(mcs, mcs->pxms_do_con);
 590
 591         /* PXMS_ERR_m - if axis in error state */
 592         if (mcs->pxms_flg & PXMS_ERR_m)
 593           mcs->pxms_ene = 0;
 594       }
 595
 596       /* for bushless motors, it is necessary to call do_out
 597         even if the controller is not enabled and PWM should be provided. */
 598       pxmc_call(mcs, mcs->pxms_do_out);
 599     }
 600   }
 601 }
 602
 603 static inline void pxmc_sfi_generator(void)
 604 {
 605   int var;
 606   pxmc_state_t *mcs;
 607   pxmc_for_each_mcs(var, mcs)
 608   {
 609     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
 610     if (mcs->pxms_flg & PXMS_ENG_m)
 611     {
 612       pxmc_call(mcs, mcs->pxms_do_gen);
 613     }
 614   }
 615 }
 616
 617 static inline void pxmc_sfi_dbg(void)
 618 {
 619   int var;
 620   pxmc_state_t *mcs;
 621   pxmc_for_each_mcs(var, mcs)
 622   {
 623     if (mcs->pxms_flg & PXMS_DBG_m)
 624     {
 625       pxmc_call(mcs, mcs->pxms_do_deb);
 626     }
 627   }
 628 }
 629
 630 static inline void pxmc_spimc_rq_transfer(void)
 631 {
 632   int var;
 633   pxmc_state_t *mcs;
 634   pxmc_spimc_state_t *mcsrc;
 635
 636   pxmc_for_each_mcs(var, mcs)
 637   {
 638     mcsrc = pxmc_state2spimc_state(mcs);
 639     spimc_transfer(mcsrc->spimc_state);
 640   }
 641 }
 642
 643 void pxmc_samplig_period(void)
 644 {
 645   pxmc_sfi_input();
 646   pxmc_sfi_controller_and_output();
 647   pxmc_spimc_rq_transfer();
 648   pxmc_sfi_generator();
 649   pxmc_sfi_dbg();
 650
 651   do_pxmc_coordmv();
 652 }
 653
 654 void *pxmc_base_thread(void *arg)
 655 {
 656   sample_period_t sample_period;
 657   sample_period_setup(&sample_period, 1000 * 1000);
 658
 659   do {
 660     pxmc_samplig_period();
 661     sample_period_wait_next(&sample_period);
 662   } while(1);
 663 }
 664
 665 int pxmc_clear_power_stop(void)
 666 {
 667   return 0;
 668 }
 669
 670 int pxmc_process_state_check(unsigned long *pbusy_bits,
 671                              unsigned long *perror_bits)
 672 {
 673   unsigned short flg;
 674   unsigned short chan;
 675   unsigned long busy_bits = 0;
 676   unsigned long error_bits = 0;
 677   pxmc_state_t *mcs;
 678   flg=0;
 679   pxmc_for_each_mcs(chan, mcs) {
 680     if(mcs) {
 681       flg |= mcs->pxms_flg;
 682       if (mcs->pxms_flg & PXMS_BSY_m)
 683         busy_bits |= 1 << chan;
 684       if (mcs->pxms_flg & PXMS_ERR_m)
 685         error_bits |= 1 << chan;
 686     }
 687   }
 688   if (appl_errstop_mode) {
 689     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
 690       pxmc_for_each_mcs(chan, mcs) {
 691         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
 692           pxmc_stop(mcs, 0);
 693         }
 694       }
 695     }
 696   }
 697
 698   if (pbusy_bits != NULL)
 699     *pbusy_bits = busy_bits;
 700   if (perror_bits != NULL)
 701     *perror_bits = error_bits;
 702
 703   return flg;
 704 }
 705
 706 int pxmc_axis_rdmode(pxmc_state_t *mcs)
 707 {
 708   if (mcs->pxms_do_out == pxmc_spimc_pwm_dc_out)
 709     return PXMC_AXIS_MODE_DC;
 710   if (mcs->pxms_do_out == pxmc_spimc_pwm3ph_out)
 711     return PXMC_AXIS_MODE_BLDC;
 712   return -1;
 713 }
 714
 715 /**
 716  * pxmc_axis_mode - Sets axis mode.[extern API]
 717  * @mcs:        Motion controller state information
 718  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
 719  *              2 .. stepper motor with IRC feedback and PWM ,
 720  *              3 .. stepper motor with PWM control
 721  *              4 .. DC motor with IRC feedback and PWM
 722  *
 723  */
 724 int
 725 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 726 {
 727   int res = 0;
 728   int prev_mode;
 729
 730   pxmc_axis_release(mcs);
 731   pxmc_clear_flag(mcs, PXMS_ENI_b);
 732   pxmc_clear_flag(mcs,PXMS_ENO_b);
 733   /*TODO Clear possible stall index flags from hardware */
 734
 735   pxmc_clear_flag(mcs, PXMS_PHA_b);
 736   pxmc_clear_flag(mcs, PXMS_PTI_b);
 737
 738   prev_mode = pxmc_axis_rdmode(mcs);
 739
 740   if (mode == PXMC_AXIS_MODE_NOCHANGE)
 741     mode = prev_mode;
 742   if (mode < 0)
 743     return -1;
 744   if (!mode)
 745     mode = PXMC_AXIS_MODE_DC;
 746
 747   switch (mode) {
 748     case PXMC_AXIS_MODE_DC:
 749       mcs->pxms_do_out = pxmc_spimc_pwm_dc_out;
 750       break;
 751     case PXMC_AXIS_MODE_BLDC:
 752       mcs->pxms_do_out = pxmc_spimc_pwm3ph_out;
 753       pxmc_fill_ptscale_for_sin_fixed(mcs);
 754       break;
 755     default:
 756       return -1;
 757   }
 758
 759   /*TODO Clear possible stall index flags from hardware */
 760
 761   /* Request new phases alignment for changed parameters */
 762   pxmc_clear_flag(mcs, PXMS_PHA_b);
 763   pxmc_clear_flag(mcs, PXMS_PTI_b);
 764   pxmc_set_flag(mcs, PXMS_ENI_b);
 765   return res;
 766 }
 767
 768 int pxmc_done(void)
 769 {
 770   int var;
 771   pxmc_state_t *mcs;
 772
 773   if (!pxmc_main_list.pxml_cnt)
 774     return 0;
 775
 776   pxmc_for_each_mcs(var, mcs)
 777   {
 778     pxmc_axis_release(mcs);
 779   }
 780
 781   pxmc_main_list.pxml_cnt = 0;
 782   __memory_barrier();
 783
 784   return 0;
 785 }
 786
 787 int pxmc_initialize(void)
 788 {
 789   int res;
 790
 791   pxmc_main_list.pxml_cnt = 0;
 792   pxmc_dbg_hist = NULL;
 793
 794   if (spimc_init(mcs0.spimc_state) < 0)
 795     return -1;
 796
 797   pxmc_fill_ptscale_for_sin_fixed(pxmc_main_list.pxml_arr[0]);
 798
 799   __memory_barrier();
 800   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
 801
 802   if (create_rt_task(&pxmc_base_thread_id, 60, pxmc_base_thread, NULL) < 0)
 803     return -1;
 804
 805   return 0;
 806 }