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
  28 #include "pxmc_cmds.h"
  29
  30 #include "appl_defs.h"
  31 #include "appl_pxmc.h"
  32 #include "pxmc_spimc.h"
  33
  34 #define SPIMC_LOG_CURRENT_SIZE 1024*1024
  35
  36 int32_t *spimc_logcurrent_buff;
  37 int32_t *spimc_logcurrent_pos;
  38
  39 extern spimc_state_t spimc_state0;
  40
  41 int spimc_logcurrent(struct pxmc_state *mcs)
  42 {
  43   /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
  44   /*mcsrc->spimc_state*/
  45   spimc_state_t *spimc = &spimc_state0;
  46
  47   if ((spimc_logcurrent_buff == NULL) ||
  48       (spimc_logcurrent_pos == NULL) ||
  49       ((char*)spimc_logcurrent_pos -
  50        (char*)spimc_logcurrent_buff + 64 >= SPIMC_LOG_CURRENT_SIZE))
  51     return 0;
  52
  53
  54   spimc_logcurrent_pos[0] = mcs->pxms_ptindx;
  55
  56   spimc_logcurrent_pos[1] = spimc->pwm[0];
  57   spimc_logcurrent_pos[2] = spimc->pwm[1];
  58   spimc_logcurrent_pos[3] = spimc->pwm[2];
  59
  60   spimc_logcurrent_pos[4] = spimc->curadc_sqn;
  61
  62   spimc_logcurrent_pos[5] = spimc->curadc_cumsum[0];
  63   spimc_logcurrent_pos[6] = spimc->curadc_cumsum[1];
  64   spimc_logcurrent_pos[7] = spimc->curadc_cumsum[2];
  65
  66   spimc_logcurrent_pos += 8;
  67
  68   return 0;
  69 }
  70
  71 int cmd_do_logcurrent(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  72 {
  73   int fd;
  74   size_t log_size;
  75
  76   if (pxmc_main_list.pxml_cnt < 1)
  77     return -1;
  78
  79   if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], NULL, 0) < 0)
  80     return -1;
  81
  82   if (spimc_logcurrent_buff == NULL) {
  83     spimc_logcurrent_buff = malloc(SPIMC_LOG_CURRENT_SIZE);
  84     if (spimc_logcurrent_buff == NULL)
  85       return -1;
  86   }
  87
  88   if (spimc_logcurrent_pos != NULL) {
  89     log_size = (char*)spimc_logcurrent_pos - (char*)spimc_logcurrent_buff;
  90     printf("Log size %ld\n", (long)log_size);
  91
  92     if ((spimc_logcurrent_pos > spimc_logcurrent_buff) &&
  93         (log_size < SPIMC_LOG_CURRENT_SIZE)) {
  94
  95       fd = open("currents.bin", O_WRONLY | O_CREAT | O_TRUNC,
  96               S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
  97
  98       if (fd == -1)
  99         return -1;
 100
 101       write(fd, spimc_logcurrent_buff, log_size);
 102
 103       close(fd);
 104     }
 105   }
 106
 107   spimc_logcurrent_pos = spimc_logcurrent_buff;
 108
 109   if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], spimc_logcurrent, 1) < 0)
 110     return -1;
 111
 112   return 0;
 113 }
 114
 115 /**
 116  * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
 117  *
 118  * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
 119  */
 120 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 121 {
 122   int val;
 123   pxmc_state_t *mcs;
 124
 125   if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 126
 127   if(*param[2]=='?') {
 128     return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
 129   }
 130
 131   if(*param[2]!=':') return -CMDERR_OPCHAR;
 132
 133   if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
 134
 135   val=atol(param[3]);
 136   val=pxmc_axis_mode(mcs,val);
 137   if(val<0)
 138     return val;
 139
 140   return 0;
 141 }
 142
 143 cmd_des_t const cmd_des_regcurdp={0, CDESM_OPCHR|CDESM_RW,
 144                         "REGCURDP?","current controller d component p parameter", cmd_do_reg_short_val,
 145                         {(char*)pxmc_spimc_state_offs(cur_d_p),
 146                          0}};
 147
 148 cmd_des_t const cmd_des_regcurdi={0, CDESM_OPCHR|CDESM_RW,
 149                         "REGCURDI?","current controller d component i parameter", cmd_do_reg_short_val,
 150                         {(char*)pxmc_spimc_state_offs(cur_d_i),
 151                          0}};
 152
 153 cmd_des_t const cmd_des_regcurqp={0, CDESM_OPCHR|CDESM_RW,
 154                         "REGCURQP?","current controller q component p parameter", cmd_do_reg_short_val,
 155                         {(char*)pxmc_spimc_state_offs(cur_q_p),
 156                          0}};
 157
 158 cmd_des_t const cmd_des_regcurqi={0, CDESM_OPCHR|CDESM_RW,
 159                         "REGCURQI?","current controller q component i parameter", cmd_do_reg_short_val,
 160                         {(char*)pxmc_spimc_state_offs(cur_q_i),
 161                          0}};
 162
 163 cmd_des_t const cmd_des_regcurhold={0, CDESM_OPCHR|CDESM_RW,
 164                         "REGCURHOLD?","current steady hold value for stepper", cmd_do_reg_short_val,
 165                         {(char*)pxmc_spimc_state_offs(cur_hold),
 166                          0}};
 167
 168 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
 169                         "REGMODE?","axis working mode",cmd_do_axis_mode,
 170                          {}};
 171
 172 cmd_des_t const cmd_des_logcurrent={0, 0,
 173                         "logcurrent","log current history", cmd_do_logcurrent,
 174                         {(char*)0,
 175                          0}};
 176
 177
 178 cmd_des_t const *cmd_appl_pxmc[] =
 179 {
 180   &cmd_des_regcurdp,
 181   &cmd_des_regcurdi,
 182   &cmd_des_regcurqp,
 183   &cmd_des_regcurqi,
 184   &cmd_des_regcurhold,
 185   &cmd_des_axis_mode,
 186   &cmd_des_logcurrent,
 187   NULL
 188 };