RPi PXMC Test: command currentcal for current sensing calibration.

Signed-off-by: Pavel Pisa <pisa@cmp.felk.cvut.cz>

diff --git a/src/app/rpi-pmsm-test1/appl_pxmc.c b/src/app/rpi-pmsm-test1/appl_pxmc.c
index f91a21f53e98cd1f2b014baa94070d71d3596029..5c3b486aa50a8f3d50fd586707d328c2a640e1f9 100644 (file)
--- 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 8fdab3378081e6e5f90bc9f66be2ac03b930c0a4..2d692898f94f6656ef34af6e90504dcbe55c845c 100644 (file)
--- a/src/app/rpi-pmsm-test1/appl_pxmccmds.c
+++ b/src/app/rpi-pmsm-test1/appl_pxmccmds.c
@@ -24,6 +24,9 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <semaphore.h>
+
+#include <utils.h>
 
 #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
 };