#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
+#include <semaphore.h>
+
+#include <utils.h>
#include "pxmc_cmds.h"
#include "appl_defs.h"
#include "appl_pxmc.h"
+
+#ifdef APPL_WITH_ZYNQ_DRV
+#include "zynq_3pmdrv1_mc.h"
+typedef z3pmdrv1_state_t spimc_state_t;
+#define SPIMC_PWM_ENABLE Z3PMDRV1_PWM_ENABLE
+#define SPIMC_PWM_SHUTDOWN Z3PMDRV1_PWM_SHUTDOWN
+#define SPIMC_CHAN_COUNT Z3PMDRV1_CHAN_COUNT
+#else
#include "pxmc_spimc.h"
+#endif
#define SPIMC_LOG_CURRENT_SIZE 1024*1024
return 0;
}
+
+typedef struct spimc_currentcal_state_t {
+ unsigned int req_accum;
+ unsigned int accum_cnt;
+ uint64_t curadc_accum[SPIMC_CHAN_COUNT];
+
+} spimc_currentcal_state_t;
+
+spimc_currentcal_state_t spimc_currentcal_state;
+sem_t spimc_currentcal_sem;
+
+int spimc_currentcal_accum(struct pxmc_state *mcs)
+{
+ /*pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); */
+ uint32_t curadc_sqn_diff;
+ uint32_t curadc_val_diff;
+ int i;
+ spimc_state_t *spimc = &spimc_state0;
+ spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
+ int diff_to_last_fl = 0;
+
+ if (cucalst->accum_cnt >= cucalst->req_accum)
+ return 0;
+
+ curadc_sqn_diff = spimc->curadc_sqn;
+ if (diff_to_last_fl) {
+ curadc_sqn_diff -= spimc->curadc_sqn_last;
+ curadc_sqn_diff &= 0x1ff;
+ }
+
+ cucalst->accum_cnt += curadc_sqn_diff;
+
+ for (i = 0; i < SPIMC_CHAN_COUNT; i++) {
+ curadc_val_diff = spimc->curadc_cumsum[i];
+ if (diff_to_last_fl) {
+ curadc_val_diff -= spimc->curadc_cumsum_last[i];
+ curadc_val_diff &= 0xffffff;
+ }
+ cucalst->curadc_accum[i] += curadc_val_diff;
+ }
+
+ if (cucalst->accum_cnt >= cucalst->req_accum)
+ sem_post(&spimc_currentcal_sem);
+
+ if (spimc_logcurrent_buff != NULL)
+ spimc_logcurrent(mcs);
+
+ return 0;
+}
+
+int spimc_currentcal_setup(spimc_state_t *spimc, spimc_currentcal_state_t *cucalst,
+ unsigned int req_accum,
+ unsigned int pwm1, int pwm1_en,
+ unsigned int pwm2, int pwm2_en,
+ unsigned int pwm3, int pwm3_en)
+{
+ int i;
+
+ spimc->pwm[0] = pwm1 | (pwm1_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
+ spimc->pwm[1] = pwm2 | (pwm2_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
+ spimc->pwm[2] = pwm3 | (pwm3_en? SPIMC_PWM_ENABLE: SPIMC_PWM_SHUTDOWN);
+
+ cucalst->req_accum = req_accum;
+ cucalst->accum_cnt = 0;
+
+ for (i = 0; i < SPIMC_CHAN_COUNT; i++)
+ cucalst->curadc_accum[i] = 0;
+
+ return 0;
+}
+
+int spimc_currentcal_pattern[7][6] = {
+ /* PWM1, EN1, PWM2, EN2, PWM3, EN3 */
+ { 0, 0, 0, 0, 0, 0},
+ { 1, 1, 0, 1, 0, 0},
+ { 0, 1, 1, 1, 0, 0},
+ { 0, 0, 1, 1, 0, 1},
+ { 0, 0, 0, 1, 1, 1},
+ { 0, 1, 0, 0, 1, 1},
+ { 1, 1, 0, 0, 0, 1},
+};
+
+int cmd_do_currentcal(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ char *ps = param[1];
+ pxmc_state_t *mcs;
+ long pwm;
+ spimc_state_t *spimc = &spimc_state0;
+ spimc_currentcal_state_t *cucalst = &spimc_currentcal_state;
+ unsigned int req_accum = 30000;
+ unsigned int skip_accum = 10000;
+ int cycle;
+
+
+ if (!ps || (si_skspace(&ps), !*ps))
+ return -CMDERR_BADPAR;
+
+ if (si_long(&ps, &pwm, 0) < 0)
+ return -CMDERR_BADPAR;
+
+ if (pxmc_main_list.pxml_cnt < 1)
+ return -1;
+
+ mcs = pxmc_main_list.pxml_arr[0];
+ pxmc_axis_release(mcs);
+
+ if (pxmc_dbgset(mcs, NULL, 0) < 0)
+ return -1;
+
+ if (sem_init(&spimc_currentcal_sem, 0, 0))
+ return -1;
+
+ for (cycle = 0; cycle < 7; cycle++) {
+ int *p = spimc_currentcal_pattern[cycle];
+ unsigned int pwm1 = pwm * p[0];
+ int pwm1_en = p[1];
+ unsigned int pwm2 = pwm * p[2];
+ int pwm2_en = p[3];
+ unsigned int pwm3 = pwm * p[4];
+ int pwm3_en = p[5];
+
+ pxmc_dbgset(mcs, NULL, 0);
+ spimc_currentcal_setup(spimc, cucalst, skip_accum,
+ pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
+
+ printf("cycle %d\n",cycle);
+ char buff[10];
+ fgets(buff, 9, stdin);
+
+ pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
+ sem_wait(&spimc_currentcal_sem);
+
+ pxmc_dbgset(mcs, NULL, 0);
+ spimc_currentcal_setup(spimc, cucalst, req_accum,
+ pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en);
+ pxmc_dbgset(mcs, spimc_currentcal_accum, 1);
+ sem_wait(&spimc_currentcal_sem);
+ pxmc_dbgset(mcs, NULL, 0);
+
+ printf("%4u %d %4u %d %4u %d %f %f %f\n",
+ pwm1, pwm1_en, pwm2, pwm2_en, pwm3, pwm3_en,
+ (double)cucalst->curadc_accum[0] / cucalst->accum_cnt,
+ (double)cucalst->curadc_accum[1] / cucalst->accum_cnt,
+ (double)cucalst->curadc_accum[2] / cucalst->accum_cnt);
+
+ }
+ sem_destroy(&spimc_currentcal_sem);
+
+ pxmc_axis_release(mcs);
+
+ return 0;
+}
+
+
+/**
+ * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
+ *
+ * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
+ */
+int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ int val;
+ pxmc_state_t *mcs;
+
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+
+ if(*param[2]=='?') {
+ return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
+ }
+
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+
+ if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
+
+ val=atol(param[3]);
+ val=pxmc_axis_mode(mcs,val);
+ if(val<0)
+ return val;
+
+ return 0;
+}
+
cmd_des_t const cmd_des_regcurdp={0, CDESM_OPCHR|CDESM_RW,
"REGCURDP?","current controller d component p parameter", cmd_do_reg_short_val,
{(char*)pxmc_spimc_state_offs(cur_d_p),
{(char*)pxmc_spimc_state_offs(cur_hold),
0}};
+cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
+ "REGMODE?","axis working mode",cmd_do_axis_mode,
+ {}};
+
cmd_des_t const cmd_des_logcurrent={0, 0,
"logcurrent","log current history", cmd_do_logcurrent,
{(char*)0,
0}};
+cmd_des_t const cmd_des_currentcal={0, 0,
+ "currentcal","current calibration", cmd_do_currentcal,
+ {(char*)0,
+ 0}};
cmd_des_t const *cmd_appl_pxmc[] =
{
&cmd_des_regcurqp,
&cmd_des_regcurqi,
&cmd_des_regcurhold,
+ &cmd_des_axis_mode,
&cmd_des_logcurrent,
+ &cmd_des_currentcal,
NULL
};
projects / fpga / rpi-motor-control-pxmc.git / blobdiff