RPi PXMC Test: command currentcal for current sensing calibration.

[fpga/rpi-motor-control-pxmc.git] / src / app / rpi-pmsm-test1 / appl_pxmc.c

diff --git a/src/app/rpi-pmsm-test1/appl_pxmc.c b/src/app/rpi-pmsm-test1/appl_pxmc.c
index 54e3b58e8aa7217791665cd5d29b6454b2fad34c..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);
@@ -411,20 +412,17 @@ pxmc_spimc_pwm_dc_out(pxmc_state_t *mcs)
   int chan = mcs->pxms_out_info;
   int ene = mcs->pxms_ene;
 
-
   if (ene < 0) {
     ene = -ene;
     if (ene > 0x7fff)
       ene = 0x7fff;
     ene = (ene * (pxmc_spimc_pwm_magnitude + 5)) >> 15;
-    /**pwm_reg_a = 0;*/
-    /**pwm_reg_b = ene | 0x4000;*/
+    pxmc_spimc_pwm3ph_wr(mcs, SPIMC_PWM_ENABLE, ene | SPIMC_PWM_ENABLE, 0);
   } else {
     if (ene > 0x7fff)
       ene = 0x7fff;
     ene = (ene * (pxmc_spimc_pwm_magnitude + 5)) >> 15;
-    /**pwm_reg_b = 0;*/
-    /**pwm_reg_a = ene | 0x4000;*/
+    pxmc_spimc_pwm3ph_wr(mcs, ene | SPIMC_PWM_ENABLE, SPIMC_PWM_ENABLE, 0);
   }
 
   return 0;
@@ -527,7 +525,7 @@ pxmc_spimc_state_t mcs0 =
   .pxms_inp_info = 0,
   .pxms_out_info = 0,
   .pxms_ene = 0, .pxms_erc = 0,
-  .pxms_p = 80, .pxms_i = 10, .pxms_d = 200, .pxms_s1 = 200, .pxms_s2 = 0,
+  .pxms_p = 40, .pxms_i = 10, .pxms_d = 100, .pxms_s1 = 0, .pxms_s2 = 0,
   .pxms_me = 0x7e00/*0x7fff*/,
 .pxms_cfg =
   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
@@ -700,12 +698,21 @@ int pxmc_process_state_check(unsigned long *pbusy_bits,
 
   if (pbusy_bits != NULL)
     *pbusy_bits = busy_bits;
-  if (error_bits != NULL)
+  if (perror_bits != NULL)
     *perror_bits = error_bits;
 
   return flg;
 }
 
+int pxmc_axis_rdmode(pxmc_state_t *mcs)
+{
+  if (mcs->pxms_do_out == pxmc_spimc_pwm_dc_out)
+    return PXMC_AXIS_MODE_DC;
+  if (mcs->pxms_do_out == pxmc_spimc_pwm3ph_out)
+    return PXMC_AXIS_MODE_BLDC;
+  return -1;
+}
+
 /**
  * pxmc_axis_mode - Sets axis mode.[extern API]
  * @mcs:        Motion controller state information
@@ -718,7 +725,45 @@ int pxmc_process_state_check(unsigned long *pbusy_bits,
 int
 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 {
-  return 0;
+  int res = 0;
+  int prev_mode;
+
+  pxmc_axis_release(mcs);
+  pxmc_clear_flag(mcs, PXMS_ENI_b);
+  pxmc_clear_flag(mcs,PXMS_ENO_b);
+  /*TODO Clear possible stall index flags from hardware */
+
+  pxmc_clear_flag(mcs, PXMS_PHA_b);
+  pxmc_clear_flag(mcs, PXMS_PTI_b);
+
+  prev_mode = pxmc_axis_rdmode(mcs);
+
+  if (mode == PXMC_AXIS_MODE_NOCHANGE)
+    mode = prev_mode;
+  if (mode < 0)
+    return -1;
+  if (!mode)
+    mode = PXMC_AXIS_MODE_DC;
+
+  switch (mode) {
+    case PXMC_AXIS_MODE_DC:
+      mcs->pxms_do_out = pxmc_spimc_pwm_dc_out;
+      break;
+    case PXMC_AXIS_MODE_BLDC:
+      mcs->pxms_do_out = pxmc_spimc_pwm3ph_out;
+      pxmc_fill_ptscale_for_sin_fixed(mcs);
+      break;
+    default:
+      return -1;
+  }
+
+  /*TODO Clear possible stall index flags from hardware */
+
+  /* Request new phases alignment for changed parameters */
+  pxmc_clear_flag(mcs, PXMS_PHA_b);
+  pxmc_clear_flag(mcs, PXMS_PTI_b);
+  pxmc_set_flag(mcs, PXMS_ENI_b);
+  return res;
 }
 
 int pxmc_done(void)