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 #include "pxmc_spimc.h"
  36
  37 #define SPIMC_LOG_CURRENT_SIZE 1024*1024
  38
  39 int32_t *spimc_logcurrent_buff;
  40 int32_t *spimc_logcurrent_pos;
  41
  42 extern spimc_state_t spimc_state0;
  43
  44 int spimc_logcurrent(struct pxmc_state *mcs)
  45 {
  46   /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
  47   /*mcsrc->spimc_state*/
  48   spimc_state_t *spimc = &spimc_state0;
  49
  50   if ((spimc_logcurrent_buff == NULL) ||
  51       (spimc_logcurrent_pos == NULL) ||
  52       ((char*)spimc_logcurrent_pos -
  53        (char*)spimc_logcurrent_buff + 64 >= SPIMC_LOG_CURRENT_SIZE))
  54     return 0;
  55
  56
  57   spimc_logcurrent_pos[0] = mcs->pxms_ptindx;
  58
  59   spimc_logcurrent_pos[1] = spimc->pwm[0];
  60   spimc_logcurrent_pos[2] = spimc->pwm[1];
  61   spimc_logcurrent_pos[3] = spimc->pwm[2];
  62
  63   spimc_logcurrent_pos[4] = spimc->curadc_sqn;
  64
  65   spimc_logcurrent_pos[5] = spimc->curadc_cumsum[0];
  66   spimc_logcurrent_pos[6] = spimc->curadc_cumsum[1];
  67   spimc_logcurrent_pos[7] = spimc->curadc_cumsum[2];
  68
  69   spimc_logcurrent_pos += 8;
  70
  71   return 0;
  72 }
  73
  74 int cmd_do_logcurrent(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  75 {
  76   int fd;
  77   size_t log_size;
  78
  79   if (pxmc_main_list.pxml_cnt < 1)
  80     return -1;
  81
  82   if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], NULL, 0) < 0)
  83     return -1;
  84
  85   if (spimc_logcurrent_buff == NULL) {
  86     spimc_logcurrent_buff = malloc(SPIMC_LOG_CURRENT_SIZE);
  87     if (spimc_logcurrent_buff == NULL)
  88       return -1;
  89   }
  90
  91   if (spimc_logcurrent_pos != NULL) {
  92     log_size = (char*)spimc_logcurrent_pos - (char*)spimc_logcurrent_buff;
  93     printf("Log size %ld\n", (long)log_size);
  94
  95     if ((spimc_logcurrent_pos > spimc_logcurrent_buff) &&
  96         (log_size < SPIMC_LOG_CURRENT_SIZE)) {
  97
  98       fd = open("currents.bin", O_WRONLY | O_CREAT | O_TRUNC,
  99               S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
 100
 101       if (fd == -1)
 102         return -1;
 103
 104       write(fd, spimc_logcurrent_buff, log_size);
 105
 106       close(fd);
 107     }
 108   }
 109
 110   spimc_logcurrent_pos = spimc_logcurrent_buff;
 111
 112   if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], spimc_logcurrent, 1) < 0)
 113     return -1;
 114
 115   return 0;
 116 }
 117
 118
 119 typedef struct spimc_currentcal_state_t {
 120   unsigned int req_accum;
 121   unsigned int accum_cnt;
 122   uint64_t curadc_accum[SPIMC_CHAN_COUNT];
 123
 124 } spimc_currentcal_state_t;
 125
 126 spimc_currentcal_state_t spimc_currentcal_state;
 127 sem_t spimc_currentcal_sem;
 128
 129 int spimc_currentcal_accum(struct pxmc_state *mcs)
 130 {
 131   /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
 132   uint32_t curadc_sqn_diff;
 133   uint32_t curadc_val_diff;
 134   int i;
 135   spimc_state_t *spimc = &spimc_state0;
 136   spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
 137
 138   if (cucalst->accum_cnt >= cucalst->req_accum)
 139     return 0;
 140
 141
 142   curadc_sqn_diff = spimc->curadc_sqn - spimc->curadc_sqn_last;
 143   curadc_sqn_diff &= 0x1ff;
 144
 145   cucalst->accum_cnt += curadc_sqn_diff;
 146
 147   for (i = 0; i < SPIMC_CHAN_COUNT; i++) {
 148     curadc_val_diff = spimc->curadc_cumsum[i] -
 149                       spimc->curadc_cumsum_last[i];
 150     curadc_val_diff &= 0xffffff;
 151     cucalst->curadc_accum[i] += curadc_val_diff;
 152   }
 153
 154   if (cucalst->accum_cnt >= cucalst->req_accum)
 155     sem_post(&spimc_currentcal_sem);
 156
 157   return 0;
 158 }
 159
 160 int spimc_currentcal_setup(spimc_state_t *spimc, spimc_currentcal_state_t *cucalst,
 161                            unsigned int req_accum,
 162                            unsigned int pwm1, int pwm1_en,
 163                            unsigned int pwm2, int pwm2_en,
 164                            unsigned int pwm3, int pwm3_en)
 165 {
 166   int i;
 167
 168   spimc->pwm[0] = pwm1 | (pwm1_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 169   spimc->pwm[1] = pwm2 | (pwm2_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 170   spimc->pwm[2] = pwm3 | (pwm3_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 171
 172   cucalst->req_accum = req_accum;
 173   cucalst->accum_cnt = 0;
 174
 175   for (i = 0; i < SPIMC_CHAN_COUNT; i++)
 176     cucalst->curadc_accum[i] = 0;
 177
 178   return 0;
 179 }
 180
 181 int spimc_currentcal_pattern[7][6] = {
 182  /* PWM1, EN1,  PWM2, EN2,  PWM3, EN3 */
 183   {  0,    0,    0,    0,    0,    0},
 184   {  1,    1,    0,    1,    0,    0},
 185   {  0,    1,    1,    1,    0,    0},
 186   {  0,    0,    1,    1,    0,    1},
 187   {  0,    0,    0,    1,    1,    1},
 188   {  0,    1,    0,    0,    1,    1},
 189   {  1,    1,    0,    0,    0,    1},
 190 };
 191
 192 int cmd_do_currentcal(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 193 {
 194   char *ps = param[1];
 195   pxmc_state_t *mcs;
 196   long pwm;
 197   spimc_state_t *spimc = &spimc_state0;
 198   spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
 199   unsigned int req_accum = 30000;
 200   unsigned int skip_accum = 10000;
 201   int cycle;
 202
 203   si_skspace(&ps);
 204   if (si_long(&ps, &pwm, 0) < 0)
 205       return -CMDERR_BADPAR;
 206
 207   if (pxmc_main_list.pxml_cnt < 1)
 208     return -1;
 209
 210   mcs = pxmc_main_list.pxml_arr[0];
 211   pxmc_axis_release(mcs);
 212
 213   if (pxmc_dbgset(mcs, NULL, 0) < 0)
 214     return -1;
 215
 216   if (sem_init(&spimc_currentcal_sem, 0, 0))
 217     return -1;
 218
 219   for (cycle = 0; cycle < 7; cycle++) {
 220     int *p = spimc_currentcal_pattern[cycle];
 221     unsigned int pwm1 = pwm * p[0];
 222     int pwm1_en = p[1];
 223     unsigned int pwm2 = pwm * p[2];
 224     int pwm2_en = p[3];
 225     unsigned int pwm3 = pwm * p[4];
 226     int pwm3_en = p[5];
 227
 228     pxmc_dbgset(mcs, NULL, 0);
 229     spimc_currentcal_setup(spimc, cucalst, skip_accum,
 230                            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
 231     pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
 232     sem_wait(&spimc_currentcal_sem);
 233
 234     pxmc_dbgset(mcs, NULL, 0);
 235     spimc_currentcal_setup(spimc, cucalst, req_accum,
 236                            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
 237     pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
 238     sem_wait(&spimc_currentcal_sem);
 239     pxmc_dbgset(mcs, NULL, 0);
 240
 241     printf("%4u %d %4u %d %4u %d   %f %f %f\n",
 242            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en,
 243            (double)cucalst->curadc_accum[0] / cucalst->accum_cnt,
 244            (double)cucalst->curadc_accum[1] / cucalst->accum_cnt,
 245            (double)cucalst->curadc_accum[2] / cucalst->accum_cnt);
 246
 247   }
 248   sem_destroy(&spimc_currentcal_sem);
 249
 250   pxmc_axis_release(mcs);
 251
 252   return 0;
 253 }
 254
 255
 256 /**
 257  * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
 258  *
 259  * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
 260  */
 261 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 262 {
 263   int val;
 264   pxmc_state_t *mcs;
 265
 266   if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 267
 268   if(*param[2]=='?') {
 269     return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
 270   }
 271
 272   if(*param[2]!=':') return -CMDERR_OPCHAR;
 273
 274   if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
 275
 276   val=atol(param[3]);
 277   val=pxmc_axis_mode(mcs,val);
 278   if(val<0)
 279     return val;
 280
 281   return 0;
 282 }
 283
 284 cmd_des_t const cmd_des_regcurdp={0, CDESM_OPCHR|CDESM_RW,
 285                         "REGCURDP?","current controller d component p parameter", cmd_do_reg_short_val,
 286                         {(char*)pxmc_spimc_state_offs(cur_d_p),
 287                          0}};
 288
 289 cmd_des_t const cmd_des_regcurdi={0, CDESM_OPCHR|CDESM_RW,
 290                         "REGCURDI?","current controller d component i parameter", cmd_do_reg_short_val,
 291                         {(char*)pxmc_spimc_state_offs(cur_d_i),
 292                          0}};
 293
 294 cmd_des_t const cmd_des_regcurqp={0, CDESM_OPCHR|CDESM_RW,
 295                         "REGCURQP?","current controller q component p parameter", cmd_do_reg_short_val,
 296                         {(char*)pxmc_spimc_state_offs(cur_q_p),
 297                          0}};
 298
 299 cmd_des_t const cmd_des_regcurqi={0, CDESM_OPCHR|CDESM_RW,
 300                         "REGCURQI?","current controller q component i parameter", cmd_do_reg_short_val,
 301                         {(char*)pxmc_spimc_state_offs(cur_q_i),
 302                          0}};
 303
 304 cmd_des_t const cmd_des_regcurhold={0, CDESM_OPCHR|CDESM_RW,
 305                         "REGCURHOLD?","current steady hold value for stepper", cmd_do_reg_short_val,
 306                         {(char*)pxmc_spimc_state_offs(cur_hold),
 307                          0}};
 308
 309 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
 310                         "REGMODE?","axis working mode",cmd_do_axis_mode,
 311                          {}};
 312
 313 cmd_des_t const cmd_des_logcurrent={0, 0,
 314                         "logcurrent","log current history", cmd_do_logcurrent,
 315                         {(char*)0,
 316                          0}};
 317
 318 cmd_des_t const cmd_des_currentcal={0, 0,
 319                         "currentcal","current calibration", cmd_do_currentcal,
 320                         {(char*)0,
 321                          0}};
 322
 323 cmd_des_t const *cmd_appl_pxmc[] =
 324 {
 325   &cmd_des_regcurdp,
 326   &cmd_des_regcurdi,
 327   &cmd_des_regcurqp,
 328   &cmd_des_regcurqi,
 329   &cmd_des_regcurhold,
 330   &cmd_des_axis_mode,
 331   &cmd_des_logcurrent,
 332   &cmd_des_currentcal,
 333   NULL
 334 };