From 773cb9625c396417d3c3c7b6877e1f3c28455123 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Sat, 6 Jun 2015 16:25:01 +0200 Subject: [PATCH] RPi PXMC Test: command currentcal for current sensing calibration. Signed-off-by: Pavel Pisa --- src/app/rpi-pmsm-test1/appl_pxmc.c | 1 + src/app/rpi-pmsm-test1/appl_pxmccmds.c | 146 +++++++++++++++++++++++++ 2 files changed, 147 insertions(+) diff --git a/src/app/rpi-pmsm-test1/appl_pxmc.c b/src/app/rpi-pmsm-test1/appl_pxmc.c index f91a21f..5c3b486 100644 --- a/src/app/rpi-pmsm-test1/appl_pxmc.c +++ b/src/app/rpi-pmsm-test1/appl_pxmc.c @@ -220,6 +220,7 @@ pxmc_spimc_process_hal_error(struct pxmc_state *mcs) mcs->pxms_halerc += HAL_ERR_SENSITIVITY; } +int pxmc_inp_spimc_ptofs_from_index_poll(struct pxmc_state *mcs, int diff2err) { pxmc_spimc_state_t *mcsrc = pxmc_state2spimc_state(mcs); diff --git a/src/app/rpi-pmsm-test1/appl_pxmccmds.c b/src/app/rpi-pmsm-test1/appl_pxmccmds.c index 8fdab33..2d69289 100644 --- a/src/app/rpi-pmsm-test1/appl_pxmccmds.c +++ b/src/app/rpi-pmsm-test1/appl_pxmccmds.c @@ -24,6 +24,9 @@ #include #include #include +#include + +#include #include "pxmc_cmds.h" @@ -112,6 +115,144 @@ int cmd_do_logcurrent(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[] 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; + + if (cucalst->accum_cnt >= cucalst->req_accum) + return 0; + + + curadc_sqn_diff = spimc->curadc_sqn - 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] - + 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); + + 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; + + si_skspace(&ps); + 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); + 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 * @@ -174,6 +315,10 @@ cmd_des_t const cmd_des_logcurrent={0, 0, {(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[] = { @@ -184,5 +329,6 @@ cmd_des_t const *cmd_appl_pxmc[] = &cmd_des_regcurhold, &cmd_des_axis_mode, &cmd_des_logcurrent, + &cmd_des_currentcal, NULL }; -- 2.39.2