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

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components.
   3
   4   appl_pxmccmds.c - position controller RoCoN specific commands
   5
   6   Copyright (C) 2001-2013 by Pavel Pisa - originator
   7                           pisa@cmp.felk.cvut.cz
   8             (C) 2001-2013 by PiKRON Ltd. - originator
   9                     http://www.pikron.com
  10
  11   This file can be used and copied according to next
  12   license alternatives
  13    - GPL - GNU Public License
  14    - other license provided by project originators
  15
  16  *******************************************************************/
  17
  18 #include <system_def.h>
  19 #include <pxmc.h>
  20 #include <stdlib.h>
  21 #include <string.h>
  22
  23 #include <sys/types.h>
  24 #include <sys/stat.h>
  25 #include <fcntl.h>
  26 #include <unistd.h>
  27 #include <semaphore.h>
  28
  29 #include <utils.h>
  30
  31 #include "pxmc_cmds.h"
  32
  33 #include "appl_defs.h"
  34 #include "appl_pxmc.h"
  35
  36 #ifdef APPL_WITH_ZYNQ_DRV
  37 #include "zynq_3pmdrv1_mc.h"
  38 typedef z3pmdrv1_state_t spimc_state_t;
  39 #define SPIMC_PWM_ENABLE   Z3PMDRV1_PWM_ENABLE
  40 #define SPIMC_PWM_SHUTDOWN Z3PMDRV1_PWM_SHUTDOWN
  41 #define SPIMC_CHAN_COUNT   Z3PMDRV1_CHAN_COUNT
  42 #else
  43 #include "pxmc_spimc.h"
  44 #endif
  45
  46 #define SPIMC_LOG_CURRENT_SIZE 1024*1024
  47
  48 int32_t *spimc_logcurrent_buff;
  49 int32_t *spimc_logcurrent_pos;
  50
  51 extern spimc_state_t spimc_state0;
  52
  53 int spimc_logcurrent(struct pxmc_state *mcs)
  54 {
  55   /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
  56   /*mcsrc->spimc_state*/
  57   spimc_state_t *spimc = &spimc_state0;
  58
  59   if ((spimc_logcurrent_buff == NULL) ||
  60       (spimc_logcurrent_pos == NULL) ||
  61       ((char*)spimc_logcurrent_pos -
  62        (char*)spimc_logcurrent_buff + 64 >= SPIMC_LOG_CURRENT_SIZE))
  63     return 0;
  64
  65
  66   spimc_logcurrent_pos[0] = mcs->pxms_ptindx;
  67
  68   spimc_logcurrent_pos[1] = spimc->pwm[0];
  69   spimc_logcurrent_pos[2] = spimc->pwm[1];
  70   spimc_logcurrent_pos[3] = spimc->pwm[2];
  71
  72   spimc_logcurrent_pos[4] = spimc->curadc_sqn;
  73
  74   spimc_logcurrent_pos[5] = spimc->curadc_cumsum[0];
  75   spimc_logcurrent_pos[6] = spimc->curadc_cumsum[1];
  76   spimc_logcurrent_pos[7] = spimc->curadc_cumsum[2];
  77
  78   spimc_logcurrent_pos += 8;
  79
  80   return 0;
  81 }
  82
  83 int cmd_do_logcurrent(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  84 {
  85   int fd;
  86   size_t log_size;
  87
  88   if (pxmc_main_list.pxml_cnt < 1)
  89     return -1;
  90
  91   if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], NULL, 0) < 0)
  92     return -1;
  93
  94   if (spimc_logcurrent_buff == NULL) {
  95     spimc_logcurrent_buff = malloc(SPIMC_LOG_CURRENT_SIZE);
  96     if (spimc_logcurrent_buff == NULL)
  97       return -1;
  98   }
  99
 100   if (spimc_logcurrent_pos != NULL) {
 101     log_size = (char*)spimc_logcurrent_pos - (char*)spimc_logcurrent_buff;
 102     printf("Log size %ld\n", (long)log_size);
 103
 104     if ((spimc_logcurrent_pos > spimc_logcurrent_buff) &&
 105         (log_size < SPIMC_LOG_CURRENT_SIZE)) {
 106
 107       fd = open("currents.bin", O_WRONLY | O_CREAT | O_TRUNC,
 108               S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
 109
 110       if (fd == -1)
 111         return -1;
 112
 113       write(fd, spimc_logcurrent_buff, log_size);
 114
 115       close(fd);
 116     }
 117   }
 118
 119   spimc_logcurrent_pos = spimc_logcurrent_buff;
 120
 121   if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], spimc_logcurrent, 1) < 0)
 122     return -1;
 123
 124   return 0;
 125 }
 126
 127
 128 typedef struct spimc_currentcal_state_t {
 129   unsigned int req_accum;
 130   unsigned int accum_cnt;
 131   uint64_t curadc_accum[SPIMC_CHAN_COUNT];
 132
 133 } spimc_currentcal_state_t;
 134
 135 spimc_currentcal_state_t spimc_currentcal_state;
 136 sem_t spimc_currentcal_sem;
 137
 138 int spimc_currentcal_accum(struct pxmc_state *mcs)
 139 {
 140   /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
 141   uint32_t curadc_sqn_diff;
 142   uint32_t curadc_val_diff;
 143   int i;
 144   spimc_state_t *spimc = &spimc_state0;
 145   spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
 146   int diff_to_last_fl = 0;
 147
 148   if (cucalst->accum_cnt >= cucalst->req_accum)
 149     return 0;
 150
 151   curadc_sqn_diff = spimc->curadc_sqn;
 152   if (diff_to_last_fl) {
 153     curadc_sqn_diff -= spimc->curadc_sqn_last;
 154     curadc_sqn_diff &= 0x1ff;
 155   }
 156
 157   cucalst->accum_cnt += curadc_sqn_diff;
 158
 159   for (i = 0; i < SPIMC_CHAN_COUNT; i++) {
 160     curadc_val_diff = spimc->curadc_cumsum[i];
 161     if (diff_to_last_fl) {
 162       curadc_val_diff -= spimc->curadc_cumsum_last[i];
 163       curadc_val_diff &= 0xffffff;
 164     }
 165     cucalst->curadc_accum[i] += curadc_val_diff;
 166   }
 167
 168   if (cucalst->accum_cnt >= cucalst->req_accum)
 169     sem_post(&spimc_currentcal_sem);
 170
 171   if (spimc_logcurrent_buff != NULL)
 172     spimc_logcurrent(mcs);
 173
 174   return 0;
 175 }
 176
 177 int spimc_currentcal_setup(spimc_state_t *spimc, spimc_currentcal_state_t *cucalst,
 178                            unsigned int req_accum,
 179                            unsigned int pwm1, int pwm1_en,
 180                            unsigned int pwm2, int pwm2_en,
 181                            unsigned int pwm3, int pwm3_en)
 182 {
 183   int i;
 184
 185   spimc->pwm[0] = pwm1 | (pwm1_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 186   spimc->pwm[1] = pwm2 | (pwm2_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 187   spimc->pwm[2] = pwm3 | (pwm3_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 188
 189   cucalst->req_accum = req_accum;
 190   cucalst->accum_cnt = 0;
 191
 192   for (i = 0; i < SPIMC_CHAN_COUNT; i++)
 193     cucalst->curadc_accum[i] = 0;
 194
 195   return 0;
 196 }
 197
 198 int spimc_currentcal_pattern[7][6] = {
 199  /* PWM1, EN1,  PWM2, EN2,  PWM3, EN3 */
 200   {  0,    0,    0,    0,    0,    0},
 201   {  1,    1,    0,    1,    0,    0},
 202   {  0,    1,    1,    1,    0,    0},
 203   {  0,    0,    1,    1,    0,    1},
 204   {  0,    0,    0,    1,    1,    1},
 205   {  0,    1,    0,    0,    1,    1},
 206   {  1,    1,    0,    0,    0,    1},
 207 };
 208
 209 int cmd_do_currentcal(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 210 {
 211   char *ps = param[1];
 212   pxmc_state_t *mcs;
 213   long pwm;
 214   spimc_state_t *spimc = &spimc_state0;
 215   spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
 216   unsigned int req_accum = 30000;
 217   unsigned int skip_accum = 10000;
 218   int cycle;
 219
 220
 221   if (!ps || (si_skspace(&ps), !*ps))
 222       return -CMDERR_BADPAR;
 223
 224   if (si_long(&ps, &pwm, 0) < 0)
 225       return -CMDERR_BADPAR;
 226
 227   if (pxmc_main_list.pxml_cnt < 1)
 228     return -1;
 229
 230   mcs = pxmc_main_list.pxml_arr[0];
 231   pxmc_axis_release(mcs);
 232
 233   if (pxmc_dbgset(mcs, NULL, 0) < 0)
 234     return -1;
 235
 236   if (sem_init(&spimc_currentcal_sem, 0, 0))
 237     return -1;
 238
 239   for (cycle = 0; cycle < 7; cycle++) {
 240     int *p = spimc_currentcal_pattern[cycle];
 241     unsigned int pwm1 = pwm * p[0];
 242     int pwm1_en = p[1];
 243     unsigned int pwm2 = pwm * p[2];
 244     int pwm2_en = p[3];
 245     unsigned int pwm3 = pwm * p[4];
 246     int pwm3_en = p[5];
 247
 248     pxmc_dbgset(mcs, NULL, 0);
 249     spimc_currentcal_setup(spimc, cucalst, skip_accum,
 250                            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
 251
 252     printf("cycle %d\n",cycle);
 253     char buff[10];
 254     fgets(buff, 9, stdin);
 255
 256     pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
 257     sem_wait(&spimc_currentcal_sem);
 258
 259     pxmc_dbgset(mcs, NULL, 0);
 260     spimc_currentcal_setup(spimc, cucalst, req_accum,
 261                            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
 262     pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
 263     sem_wait(&spimc_currentcal_sem);
 264     pxmc_dbgset(mcs, NULL, 0);
 265
 266     printf("%4u %d %4u %d %4u %d   %f %f %f\n",
 267            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en,
 268            (double)cucalst->curadc_accum[0] / cucalst->accum_cnt,
 269            (double)cucalst->curadc_accum[1] / cucalst->accum_cnt,
 270            (double)cucalst->curadc_accum[2] / cucalst->accum_cnt);
 271
 272   }
 273   sem_destroy(&spimc_currentcal_sem);
 274
 275   pxmc_axis_release(mcs);
 276
 277   return 0;
 278 }
 279
 280
 281 /**
 282  * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
 283  *
 284  * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
 285  */
 286 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 287 {
 288   int val;
 289   pxmc_state_t *mcs;
 290
 291   if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 292
 293   if(*param[2]=='?') {
 294     return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
 295   }
 296
 297   if(*param[2]!=':') return -CMDERR_OPCHAR;
 298
 299   if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
 300
 301   val=atol(param[3]);
 302   val=pxmc_axis_mode(mcs,val);
 303   if(val<0)
 304     return val;
 305
 306   return 0;
 307 }
 308
 309 cmd_des_t const cmd_des_regcurdp={0, CDESM_OPCHR|CDESM_RW,
 310                         "REGCURDP?","current controller d component p parameter", cmd_do_reg_short_val,
 311                         {(char*)pxmc_spimc_state_offs(cur_d_p),
 312                          0}};
 313
 314 cmd_des_t const cmd_des_regcurdi={0, CDESM_OPCHR|CDESM_RW,
 315                         "REGCURDI?","current controller d component i parameter", cmd_do_reg_short_val,
 316                         {(char*)pxmc_spimc_state_offs(cur_d_i),
 317                          0}};
 318
 319 cmd_des_t const cmd_des_regcurqp={0, CDESM_OPCHR|CDESM_RW,
 320                         "REGCURQP?","current controller q component p parameter", cmd_do_reg_short_val,
 321                         {(char*)pxmc_spimc_state_offs(cur_q_p),
 322                          0}};
 323
 324 cmd_des_t const cmd_des_regcurqi={0, CDESM_OPCHR|CDESM_RW,
 325                         "REGCURQI?","current controller q component i parameter", cmd_do_reg_short_val,
 326                         {(char*)pxmc_spimc_state_offs(cur_q_i),
 327                          0}};
 328
 329 cmd_des_t const cmd_des_regcurhold={0, CDESM_OPCHR|CDESM_RW,
 330                         "REGCURHOLD?","current steady hold value for stepper", cmd_do_reg_short_val,
 331                         {(char*)pxmc_spimc_state_offs(cur_hold),
 332                          0}};
 333
 334 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
 335                         "REGMODE?","axis working mode",cmd_do_axis_mode,
 336                          {}};
 337
 338 cmd_des_t const cmd_des_logcurrent={0, 0,
 339                         "logcurrent","log current history", cmd_do_logcurrent,
 340                         {(char*)0,
 341                          0}};
 342
 343 cmd_des_t const cmd_des_currentcal={0, 0,
 344                         "currentcal","current calibration", cmd_do_currentcal,
 345                         {(char*)0,
 346                          0}};
 347
 348 cmd_des_t const *cmd_appl_pxmc[] =
 349 {
 350   &cmd_des_regcurdp,
 351   &cmd_des_regcurdi,
 352   &cmd_des_regcurqp,
 353   &cmd_des_regcurqi,
 354   &cmd_des_regcurhold,
 355   &cmd_des_axis_mode,
 356   &cmd_des_logcurrent,
 357   &cmd_des_currentcal,
 358   NULL
 359 };