RoCoN and TUMBL firmware for control of stepper motor with IRC sensor.

Signed-off-by: Pavel Pisa <ppisa@pikron.com>

diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c
index 86a6076742cc22fe5355b082df01a6e266f3006f..0d8bfec9fa562d8e16609968bdebf4e30b70507b 100644 (file)
--- a/hw/lx-rocon_firmware/firmware.c
+++ b/hw/lx-rocon_firmware/firmware.c
@@ -87,6 +87,7 @@ void main(void)
   pxmcc_data.curadc[0].siroladc_offs = 0x0c17;
   pxmcc_data.curadc[1].siroladc_offs = 0x0c66;
   pxmcc_data.curadc[2].siroladc_offs = 0x0c66;
+  pxmcc_data.curadc[3].siroladc_offs = 0x0c66;
 
   pxmcc = pxmcc_data.axis;
   do {
@@ -162,7 +163,7 @@ void main(void)
         pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
         pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
 
-        if (pxmcc->mode == PXMCC_MODE_MODE_BLDC) {
+        if (pxmcc->mode == PXMCC_MODE_BLDC) {
           pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
 
          #ifndef SUPPRESS_CONDITIONALS
@@ -279,14 +280,14 @@ void main(void)
           *uptr = pwm1 | 0x4000;
          #endif
         } else {
-          uint32_t bet_sgn = pwm_bet >> 31;
           uint32_t alp_sgn = pwm_alp >> 31;
-          pwm1 = pwm2 = pwm_bet | 0x4000;
-          pwm3 = pwm4 = pwm_alp | 0x4000;
-          pwm1 &= ~bet_sgn;
-          pwm2 &= bet_sgn;
-          pwm3 &= ~alp_sgn;
-          pwm4 &= alp_sgn;
+          uint32_t bet_sgn = pwm_bet >> 31;
+          pwm_alp >>= 16;
+          pwm_bet >>= 16;
+          pwm1 = -pwm_alp & alp_sgn;
+          pwm2 = pwm_alp & ~alp_sgn;
+          pwm3 = -pwm_bet & bet_sgn;
+          pwm4 = pwm_bet & ~bet_sgn;
 
           pwmtx_info = pxmcc->pwmtx_info;
           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
@@ -303,7 +304,7 @@ void main(void)
           *uptr = pwm4 | 0x4000;
         }
       } else {
-        if (pxmcc->mode == PXMCC_MODE_MODE_BLDC) {
+        if (pxmcc->mode == PXMCC_MODE_BLDC) {
           pwmtx_info = pxmcc->pwmtx_info;
           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
           pxmcc->pwm_prew[1] = *uptr & 0x3fff;
@@ -392,7 +393,7 @@ void main(void)
      #endif /*COMPUTE_PHASE_SECTOR*/
 
       out_info = pxmcc->out_info;
-      if (pxmcc->mode == PXMCC_MODE_MODE_BLDC) {
+      if (pxmcc->mode == PXMCC_MODE_BLDC) {
 
         pwm1 = pxmcc->pwm_prew[0];
         pwm2 = pxmcc->pwm_prew[1];
@@ -534,14 +535,14 @@ void main(void)
         cur_bet *= CONST16_K3;
         cur_bet >>= 16;
       } else {
-        int32_t bet_pwm = pxmcc->pwm_prew[1];
+        int32_t bet_pwm = pxmcc->pwm_prew[2];
         uint32_t bet_sgn = (bet_pwm - 1) >> 31;
-        int32_t alp_pwm = pxmcc->pwm_prew[3];
+        int32_t alp_pwm = pxmcc->pwm_prew[0];
         uint32_t alp_sgn = (alp_pwm - 1) >> 31;
-        cur_bet = (pxmcc_data.curadc[out_info + 0].cur_val & ~bet_sgn) -
-                  (pxmcc_data.curadc[out_info + 1].cur_val & bet_sgn);
-        cur_alp = (pxmcc_data.curadc[out_info + 2].cur_val & ~alp_sgn) -
-                  (pxmcc_data.curadc[out_info + 3].cur_val & alp_sgn);
+        cur_bet = (pxmcc_data.curadc[out_info + 3].cur_val & ~bet_sgn) -
+                  (pxmcc_data.curadc[out_info + 2].cur_val & bet_sgn);
+        cur_alp = (pxmcc_data.curadc[out_info + 1].cur_val & ~alp_sgn) -
+                  (pxmcc_data.curadc[out_info + 0].cur_val & alp_sgn);
       }
 
       cur_d =  cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;

diff --git a/hw/lx-rocon_firmware/pxmcc_types.h b/hw/lx-rocon_firmware/pxmcc_types.h
index 2e7856afe95e076f0716135b1a1e8779f29a686b..c09a18da03a32d56ad257c8246b1b6b1539cfa69 100644 (file)
--- a/hw/lx-rocon_firmware/pxmcc_types.h
+++ b/hw/lx-rocon_firmware/pxmcc_types.h
@@ -25,7 +25,7 @@
 #define PXMCC_CURADC_CHANNELS 16
 
 #define PXMCC_MODE_IDLE              2
-#define PXMCC_MODE_MODE_BLDC         0
+#define PXMCC_MODE_BLDC              0
 #define PXMCC_MODE_STEPPER_WITH_IRC  1
 
 typedef struct pxmcc_common_data_t {

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 1f9287f3109db6c9ecd2eb1e26b456a9991acde2..f49f5e1c4049449cc921074c9d4bac2bd699b06c 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -35,7 +35,8 @@
 #include "pxmcc_types.h"
 #include "pxmcc_interface.h"
 
-#define PXMC_AXIS_MODE_BLDC_PXMCC (PXMC_AXIS_MODE_BLDC + 1)
+#define PXMC_AXIS_MODE_BLDC_PXMCC             (PXMC_AXIS_MODE_BLDC + 1)
+#define PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC (PXMC_AXIS_MODE_BLDC + 2)
 
 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
                            unsigned long index_irc, int diff2err);
@@ -715,6 +716,21 @@ pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 /*******************************************************************/
 /* PXMCC - PXMC coprocessor support and communication */
 
+void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs)
+{
+  volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+  int inp_chan=mcs->pxms_inp_info;
+  uint32_t ptofs;
+  uint32_t irc;
+
+  if (mcc_axis != NULL) {
+    ptofs = mcs->pxms_ptofs - pxmc_rocon_irc_offset[inp_chan];
+    irc = fpga_irc[inp_chan]->count;
+    ptofs = (int16_t)(ptofs - irc) + irc;
+    mcc_axis->ptofs = ptofs;
+  }
+}
+
 int
 pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 {
@@ -761,16 +777,10 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
             set_ptofs_fl = 1;
         }
         if (set_ptofs_fl) {
-          volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
-          uint32_t ptofs;
-          uint32_t irc;
-
           mcs->pxms_ptindx = ptindx;
           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 
-          irc = fpga_irc[mcs->pxms_inp_info]->count;
-          ptofs = (int16_t)(mcs->pxms_ptofs - irc) + irc;
-          mcc_axis->ptofs = ptofs;
+          pxmcc_pxmc_ptofs2mcc(mcs);
         }
       } else {
         /* if phase table position to mask is know do fine phase table alignment */
@@ -781,6 +791,7 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
           if (res < 0) {
             pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
           } else if (res) {
+            pxmcc_pxmc_ptofs2mcc(mcs);
             pxmc_set_flag(mcs, PXMS_PTI_b);
             pxmc_set_flag(mcs, PXMS_PHA_b);
           }
@@ -812,13 +823,29 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
   return 0;
 }
 
+int
+pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
+{
+  {
+    int ene, pwm_d, pwm_q;
+
+    ene = mcs->pxms_ene;
+    pwm_d = 0;
+    pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
+
+    pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
+
+    if (mcs->pxms_flg & PXMS_ERR_m)
+      pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
+  }
+
+  return 0;
+}
 
 int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
 {
   volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
-  uint32_t ptofs;
-  uint32_t irc;
   uint32_t ptirc;
   uint32_t ptreci;
   uint32_t pwmtx_info;
@@ -836,22 +863,20 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
 
   mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE;
 
-  irc = fpga_irc[mcs->pxms_inp_info]->count;
-  ptofs = (int16_t)(mcs->pxms_ptofs - irc) + irc;
-
   ptirc = mcs->pxms_ptirc;
   ull = (1ULL << 32) * mcs->pxms_ptper;
   ptreci = (ull + ptirc / 2) / ptirc;
 
   mcc_axis->ptirc = ptirc;
   mcc_axis->ptreci = ptreci;
-  mcc_axis->ptofs = ptofs;
+
+  pxmcc_pxmc_ptofs2mcc(mcs);
 
   switch (mode) {
     case PXMCC_MODE_IDLE:
       phcnt = 0;
       break;
-    case PXMCC_MODE_MODE_BLDC:
+    case PXMCC_MODE_BLDC:
       phcnt = 3;
       break;
     case PXMCC_MODE_STEPPER_WITH_IRC:
@@ -1801,6 +1826,7 @@ pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
   switch (mode) {
     /*case PXMC_AXIS_MODE_STEPPER:*/
     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
       break;
     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
@@ -1844,6 +1870,7 @@ int
 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 {
   int res;
+  int prev_mode;
 
   pxmc_set_const_out(mcs, 0);
   pxmc_clear_flag(mcs, PXMS_ENI_b);
@@ -1854,13 +1881,19 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
   pxmc_clear_flag(mcs, PXMS_PTI_b);
 
 
+  prev_mode = pxmc_axis_rdmode(mcs);
+
   if (mode == PXMC_AXIS_MODE_NOCHANGE)
-    mode = pxmc_axis_rdmode(mcs);
+    mode = prev_mode;
   if (mode < 0)
     return -1;
   if (!mode)
     mode = PXMC_AXIS_MODE_DC;
 
+  if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) ||
+      (prev_mode == PXMCC_MODE_STEPPER_WITH_IRC))
+    pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE);
+
   res = pxmc_axis_pt4mode(mcs, mode);
   if (res < 0)
     return -1;
@@ -1878,11 +1911,17 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
       break;
     case PXMC_AXIS_MODE_BLDC_PXMCC:
-      if (pxmcc_axis_setup(mcs, PXMCC_MODE_MODE_BLDC) < 0)
+      if (pxmcc_axis_setup(mcs, PXMCC_MODE_BLDC) < 0)
         return -1;
       pxmcc_axis_enable(mcs, 1);
       mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out;
       break;
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
+      if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER_WITH_IRC) < 0)
+        return -1;
+      pxmcc_axis_enable(mcs, 1);
+      mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out;
+      break;
     default:
       return -1;
   }

diff --git a/sw/app/rocon/appl_tests.c b/sw/app/rocon/appl_tests.c
index a8d977a9c34b3076c05cc4f3f457c1f92f4c893a..481be41c8a16f2e3630c7e9c4c6e5c04fdb1312f 100644 (file)
--- a/sw/app/rocon/appl_tests.c
+++ b/sw/app/rocon/appl_tests.c
@@ -419,26 +419,28 @@ int cmd_do_testtumblefw(cmd_io_t *cmd_io, const struct cmd_des *des, char *param
         return -CMDERR_BADPAR;
 
 
-  pxmcc_axis_setup(mcs, PXMCC_MODE_MODE_BLDC);
+  if (0) {
+    pxmcc_axis_setup(mcs, PXMCC_MODE_MODE_BLDC);
+  }
   pxmc_clear_flags(mcs,PXMS_ENO_m|PXMS_ENG_m|PXMS_ENR_m|PXMS_BSY_m);
   pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
   pxmcc_axis_enable(mcs, 1);
 
-  if (1) {
+  if (0) {
     mcc_data->axis[1].inp_info = 1;
     mcc_data->axis[1].out_info = 3;
     mcc_data->axis[1].pwmtx_info = (12 << 0) | (13 << 8) | (14 << 16);
-    mcc_data->axis[1].mode = PXMCC_MODE_MODE_BLDC;
+    mcc_data->axis[1].mode = PXMCC_MODE_BLDC;
     mcc_data->axis[1].ccflg = 1;
     mcc_data->axis[2].inp_info = 2;
     mcc_data->axis[2].out_info = 6;
     mcc_data->axis[2].pwmtx_info = (15 << 0) | (16 << 8) | (18 << 16);
-    mcc_data->axis[2].mode = PXMCC_MODE_MODE_BLDC;
+    mcc_data->axis[2].mode = PXMCC_MODE_BLDC;
     mcc_data->axis[2].ccflg = 1;
     mcc_data->axis[3].inp_info = 3;
     mcc_data->axis[3].out_info = 9;
     mcc_data->axis[3].pwmtx_info = (19 << 0) | (20 << 8) | (21 << 16);
-    mcc_data->axis[3].mode = PXMCC_MODE_MODE_BLDC;
+    mcc_data->axis[3].mode = PXMCC_MODE_BLDC;
     mcc_data->axis[3].ccflg = 1;
   }
 

diff --git a/sw/app/rocon/pxmcc_interface.h b/sw/app/rocon/pxmcc_interface.h
index be0163dd6fec12b3c6cdb41bdead949a2a34d625..e55ade48e61d2eb556b4be966c30ee0f76d45a65 100644 (file)
--- a/sw/app/rocon/pxmcc_interface.h
+++ b/sw/app/rocon/pxmcc_interface.h
@@ -52,6 +52,8 @@ void pxmcc_axis_enable(pxmc_state_t *mcs, int enable)
   mcc_axis->ccflg = enable? 1: 0;
 }
 
+void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs);
+
 int pxmcc_axis_setup(pxmc_state_t *mcs, int mode);
 
 #endif /*_PXMCC_INTERFACE_H_*/