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