1 /*******************************************************************
2 Motion and Robotic System (MARS) aplication components.
4 appl_pxmccmds.c - position controller RoCoN specific commands
6 Copyright (C) 2001-2013 by Pavel Pisa - originator
8 (C) 2001-2013 by PiKRON Ltd. - originator
11 This file can be used and copied according to next
13 - GPL - GNU Public License
14 - other license provided by project originators
16 *******************************************************************/
18 #include <system_def.h>
23 #include <sys/types.h>
27 #include <semaphore.h>
31 #include "pxmc_cmds.h"
33 #include "appl_defs.h"
34 #include "appl_pxmc.h"
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
43 #include "pxmc_spimc.h"
46 #define SPIMC_LOG_CURRENT_SIZE 1024*1024
48 int32_t *spimc_logcurrent_buff;
49 int32_t *spimc_logcurrent_pos;
51 extern spimc_state_t spimc_state0;
53 int spimc_logcurrent(struct pxmc_state *mcs)
55 /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
56 /*mcsrc->spimc_state*/
57 spimc_state_t *spimc = &spimc_state0;
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))
66 spimc_logcurrent_pos[0] = mcs->pxms_ptindx;
68 spimc_logcurrent_pos[1] = spimc->pwm[0];
69 spimc_logcurrent_pos[2] = spimc->pwm[1];
70 spimc_logcurrent_pos[3] = spimc->pwm[2];
72 spimc_logcurrent_pos[4] = spimc->curadc_sqn;
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];
78 spimc_logcurrent_pos += 8;
83 int cmd_do_logcurrent(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
88 if (pxmc_main_list.pxml_cnt < 1)
91 if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], NULL, 0) < 0)
94 if (spimc_logcurrent_buff == NULL) {
95 spimc_logcurrent_buff = malloc(SPIMC_LOG_CURRENT_SIZE);
96 if (spimc_logcurrent_buff == NULL)
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);
104 if ((spimc_logcurrent_pos > spimc_logcurrent_buff) &&
105 (log_size < SPIMC_LOG_CURRENT_SIZE)) {
107 fd = open("currents.bin", O_WRONLY | O_CREAT | O_TRUNC,
108 S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
113 write(fd, spimc_logcurrent_buff, log_size);
119 spimc_logcurrent_pos = spimc_logcurrent_buff;
121 if (pxmc_dbgset(pxmc_main_list.pxml_arr[0], spimc_logcurrent, 1) < 0)
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];
133 } spimc_currentcal_state_t;
135 spimc_currentcal_state_t spimc_currentcal_state;
136 sem_t spimc_currentcal_sem;
138 int spimc_currentcal_accum(struct pxmc_state *mcs)
140 /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
141 uint32_t curadc_sqn_diff;
142 uint32_t curadc_val_diff;
144 spimc_state_t *spimc = &spimc_state0;
145 spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
146 int diff_to_last_fl = spimc->curadc_use_diff_to_last_fl;
148 if (cucalst->accum_cnt >= cucalst->req_accum)
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;
157 cucalst->accum_cnt += curadc_sqn_diff;
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;
165 cucalst->curadc_accum[i] += curadc_val_diff;
168 if (cucalst->accum_cnt >= cucalst->req_accum)
169 sem_post(&spimc_currentcal_sem);
171 if (spimc_logcurrent_buff != NULL)
172 spimc_logcurrent(mcs);
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)
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);
189 cucalst->req_accum = req_accum;
190 cucalst->accum_cnt = 0;
192 for (i = 0; i < SPIMC_CHAN_COUNT; i++)
193 cucalst->curadc_accum[i] = 0;
198 int spimc_currentcal_pattern[7][6] = {
199 /* PWM1, EN1, PWM2, EN2, PWM3, EN3 */
209 int cmd_do_currentcal(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
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;
221 if (!ps || (si_skspace(&ps), !*ps))
222 return -CMDERR_BADPAR;
224 if (si_long(&ps, &pwm, 0) < 0)
225 return -CMDERR_BADPAR;
227 if (pxmc_main_list.pxml_cnt < 1)
230 mcs = pxmc_main_list.pxml_arr[0];
231 pxmc_axis_release(mcs);
233 if (pxmc_dbgset(mcs, NULL, 0) < 0)
236 if (sem_init(&spimc_currentcal_sem, 0, 0))
239 for (cycle = 0; cycle < 7; cycle++) {
240 int *p = spimc_currentcal_pattern[cycle];
241 unsigned int pwm1 = pwm * p[0];
243 unsigned int pwm2 = pwm * p[2];
245 unsigned int pwm3 = pwm * p[4];
248 pxmc_dbgset(mcs, NULL, 0);
249 spimc_currentcal_setup(spimc, cucalst, skip_accum,
250 pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
252 printf("cycle %d\n",cycle);
254 fgets(buff, 9, stdin);
256 pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
257 sem_wait(&spimc_currentcal_sem);
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);
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);
273 sem_destroy(&spimc_currentcal_sem);
275 pxmc_axis_release(mcs);
282 * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
284 * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
286 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
291 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
294 return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
297 if(*param[2]!=':') return -CMDERR_OPCHAR;
299 if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
302 val=pxmc_axis_mode(mcs,val);
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),
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),
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),
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),
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),
334 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
335 "REGMODE?","axis working mode",cmd_do_axis_mode,
338 cmd_des_t const cmd_des_logcurrent={0, 0,
339 "logcurrent","log current history", cmd_do_logcurrent,
343 cmd_des_t const cmd_des_currentcal={0, 0,
344 "currentcal","current calibration", cmd_do_currentcal,
348 cmd_des_t const *cmd_appl_pxmc[] =