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   int diff_to_last_fl = 0;
 138
 139   if (cucalst->accum_cnt >= cucalst->req_accum)
 140     return 0;
 141
 142   curadc_sqn_diff = spimc->curadc_sqn;
 143   if (diff_to_last_fl) {
 144     curadc_sqn_diff -= spimc->curadc_sqn_last;
 145     curadc_sqn_diff &= 0x1ff;
 146   }
 147
 148   cucalst->accum_cnt += curadc_sqn_diff;
 149
 150   for (i = 0; i < SPIMC_CHAN_COUNT; i++) {
 151     curadc_val_diff = spimc->curadc_cumsum[i];
 152     if (diff_to_last_fl) {
 153       curadc_val_diff -= spimc->curadc_cumsum_last[i];
 154       curadc_val_diff &= 0xffffff;
 155     }
 156     cucalst->curadc_accum[i] += curadc_val_diff;
 157   }
 158
 159   if (cucalst->accum_cnt >= cucalst->req_accum)
 160     sem_post(&spimc_currentcal_sem);
 161
 162   if (spimc_logcurrent_buff != NULL)
 163     spimc_logcurrent(mcs);
 164
 165   return 0;
 166 }
 167
 168 int spimc_currentcal_setup(spimc_state_t *spimc, spimc_currentcal_state_t *cucalst,
 169                            unsigned int req_accum,
 170                            unsigned int pwm1, int pwm1_en,
 171                            unsigned int pwm2, int pwm2_en,
 172                            unsigned int pwm3, int pwm3_en)
 173 {
 174   int i;
 175
 176   spimc->pwm[0] = pwm1 | (pwm1_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 177   spimc->pwm[1] = pwm2 | (pwm2_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 178   spimc->pwm[2] = pwm3 | (pwm3_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
 179
 180   cucalst->req_accum = req_accum;
 181   cucalst->accum_cnt = 0;
 182
 183   for (i = 0; i < SPIMC_CHAN_COUNT; i++)
 184     cucalst->curadc_accum[i] = 0;
 185
 186   return 0;
 187 }
 188
 189 int spimc_currentcal_pattern[7][6] = {
 190  /* PWM1, EN1,  PWM2, EN2,  PWM3, EN3 */
 191   {  0,    0,    0,    0,    0,    0},
 192   {  1,    1,    0,    1,    0,    0},
 193   {  0,    1,    1,    1,    0,    0},
 194   {  0,    0,    1,    1,    0,    1},
 195   {  0,    0,    0,    1,    1,    1},
 196   {  0,    1,    0,    0,    1,    1},
 197   {  1,    1,    0,    0,    0,    1},
 198 };
 199
 200 int cmd_do_currentcal(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 201 {
 202   char *ps = param[1];
 203   pxmc_state_t *mcs;
 204   long pwm;
 205   spimc_state_t *spimc = &spimc_state0;
 206   spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
 207   unsigned int req_accum = 30000;
 208   unsigned int skip_accum = 10000;
 209   int cycle;
 210
 211
 212   if (!ps || (si_skspace(&ps), !*ps))
 213       return -CMDERR_BADPAR;
 214
 215   if (si_long(&ps, &pwm, 0) < 0)
 216       return -CMDERR_BADPAR;
 217
 218   if (pxmc_main_list.pxml_cnt < 1)
 219     return -1;
 220
 221   mcs = pxmc_main_list.pxml_arr[0];
 222   pxmc_axis_release(mcs);
 223
 224   if (pxmc_dbgset(mcs, NULL, 0) < 0)
 225     return -1;
 226
 227   if (sem_init(&spimc_currentcal_sem, 0, 0))
 228     return -1;
 229
 230   for (cycle = 0; cycle < 7; cycle++) {
 231     int *p = spimc_currentcal_pattern[cycle];
 232     unsigned int pwm1 = pwm * p[0];
 233     int pwm1_en = p[1];
 234     unsigned int pwm2 = pwm * p[2];
 235     int pwm2_en = p[3];
 236     unsigned int pwm3 = pwm * p[4];
 237     int pwm3_en = p[5];
 238
 239     pxmc_dbgset(mcs, NULL, 0);
 240     spimc_currentcal_setup(spimc, cucalst, skip_accum,
 241                            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
 242     pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
 243     sem_wait(&spimc_currentcal_sem);
 244
 245     pxmc_dbgset(mcs, NULL, 0);
 246     spimc_currentcal_setup(spimc, cucalst, req_accum,
 247                            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
 248     pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
 249     sem_wait(&spimc_currentcal_sem);
 250     pxmc_dbgset(mcs, NULL, 0);
 251
 252     printf("%4u %d %4u %d %4u %d   %f %f %f\n",
 253            pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en,
 254            (double)cucalst->curadc_accum[0] / cucalst->accum_cnt,
 255            (double)cucalst->curadc_accum[1] / cucalst->accum_cnt,
 256            (double)cucalst->curadc_accum[2] / cucalst->accum_cnt);
 257
 258   }
 259   sem_destroy(&spimc_currentcal_sem);
 260
 261   pxmc_axis_release(mcs);
 262
 263   return 0;
 264 }
 265
 266
 267 /**
 268  * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
 269  *
 270  * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
 271  */
 272 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 273 {
 274   int val;
 275   pxmc_state_t *mcs;
 276
 277   if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 278
 279   if(*param[2]=='?') {
 280     return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
 281   }
 282
 283   if(*param[2]!=':') return -CMDERR_OPCHAR;
 284
 285   if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
 286
 287   val=atol(param[3]);
 288   val=pxmc_axis_mode(mcs,val);
 289   if(val<0)
 290     return val;
 291
 292   return 0;
 293 }
 294
 295 cmd_des_t const cmd_des_regcurdp={0, CDESM_OPCHR|CDESM_RW,
 296                         "REGCURDP?","current controller d component p parameter", cmd_do_reg_short_val,
 297                         {(char*)pxmc_spimc_state_offs(cur_d_p),
 298                          0}};
 299
 300 cmd_des_t const cmd_des_regcurdi={0, CDESM_OPCHR|CDESM_RW,
 301                         "REGCURDI?","current controller d component i parameter", cmd_do_reg_short_val,
 302                         {(char*)pxmc_spimc_state_offs(cur_d_i),
 303                          0}};
 304
 305 cmd_des_t const cmd_des_regcurqp={0, CDESM_OPCHR|CDESM_RW,
 306                         "REGCURQP?","current controller q component p parameter", cmd_do_reg_short_val,
 307                         {(char*)pxmc_spimc_state_offs(cur_q_p),
 308                          0}};
 309
 310 cmd_des_t const cmd_des_regcurqi={0, CDESM_OPCHR|CDESM_RW,
 311                         "REGCURQI?","current controller q component i parameter", cmd_do_reg_short_val,
 312                         {(char*)pxmc_spimc_state_offs(cur_q_i),
 313                          0}};
 314
 315 cmd_des_t const cmd_des_regcurhold={0, CDESM_OPCHR|CDESM_RW,
 316                         "REGCURHOLD?","current steady hold value for stepper", cmd_do_reg_short_val,
 317                         {(char*)pxmc_spimc_state_offs(cur_hold),
 318                          0}};
 319
 320 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
 321                         "REGMODE?","axis working mode",cmd_do_axis_mode,
 322                          {}};
 323
 324 cmd_des_t const cmd_des_logcurrent={0, 0,
 325                         "logcurrent","log current history", cmd_do_logcurrent,
 326                         {(char*)0,
 327                          0}};
 328
 329 cmd_des_t const cmd_des_currentcal={0, 0,
 330                         "currentcal","current calibration", cmd_do_currentcal,
 331                         {(char*)0,
 332                          0}};
 333
 334 cmd_des_t const *cmd_appl_pxmc[] =
 335 {
 336   &cmd_des_regcurdp,
 337   &cmd_des_regcurdi,
 338   &cmd_des_regcurqp,
 339   &cmd_des_regcurqi,
 340   &cmd_des_regcurhold,
 341   &cmd_des_axis_mode,
 342   &cmd_des_logcurrent,
 343   &cmd_des_currentcal,
 344   NULL
 345 };