From: Pavel Pisa <pisa@cmp.felk.cvut.cz>
Date: Tue, 12 May 2015 10:35:09 +0000 (+0200)
Subject: RPi PXMC Test: command to switch axis mode and DC motor support added.
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control-pxmc.git/commitdiff_plain/39ecfe3cbf7fe000d59b2e3652b398c8bc90e69d

RPi PXMC Test: command to switch axis mode and DC motor support added.

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 b1ca244..f91a21f 100644
--- a/src/app/rpi-pmsm-test1/appl_pxmc.c
+++ b/src/app/rpi-pmsm-test1/appl_pxmc.c
@@ -411,20 +411,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;
@@ -700,12 +697,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 +724,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)
diff --git a/src/app/rpi-pmsm-test1/appl_pxmccmds.c b/src/app/rpi-pmsm-test1/appl_pxmccmds.c
index 7348848..8fdab33 100644
--- a/src/app/rpi-pmsm-test1/appl_pxmccmds.c
+++ b/src/app/rpi-pmsm-test1/appl_pxmccmds.c
@@ -112,6 +112,34 @@ int cmd_do_logcurrent(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]
   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),
@@ -137,6 +165,10 @@ cmd_des_t const cmd_des_regcurhold={0, CDESM_OPCHR|CDESM_RW,
                         {(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,
@@ -150,6 +182,7 @@ cmd_des_t const *cmd_appl_pxmc[] =
   &cmd_des_regcurqp,
   &cmd_des_regcurqi,
   &cmd_des_regcurhold,
+  &cmd_des_axis_mode,
   &cmd_des_logcurrent,
   NULL
 };