RoCoN - initial version which can use PXMCC for positional control.

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

diff --git a/hw/lx-rocon_firmware/pxmcc_types.h b/hw/lx-rocon_firmware/pxmcc_types.h
index 284e8da6eaba0d1e6d3a97d4b423b48859e56c9e..2e7856afe95e076f0716135b1a1e8779f29a686b 100644 (file)
--- a/hw/lx-rocon_firmware/pxmcc_types.h
+++ b/hw/lx-rocon_firmware/pxmcc_types.h
@@ -2,7 +2,7 @@
   Components for embedded applications builded for
   laboratory and medical instruments firmware
 
-  pxmcc_types.h - multi axis motion controller comprocesor
+  pxmcc_types.h - multi axis motion controller coprocessor
         for FPGA tumble CPU of lx-rocon system - data types
 
   (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
@@ -15,13 +15,19 @@
 
  *******************************************************************/
 
-#include <stdint.h>
+#ifndef _PXMCC_TYPES_H_
+#define _PXMCC_TYPES_H_
 
+#include <stdint.h>
 
 #define PXMCC_FWVERSION       0xACCE0001
 #define PXMCC_AXIS_COUNT      4
 #define PXMCC_CURADC_CHANNELS 16
 
+#define PXMCC_MODE_IDLE              2
+#define PXMCC_MODE_MODE_BLDC         0
+#define PXMCC_MODE_STEPPER_WITH_IRC  1
+
 typedef struct pxmcc_common_data_t {
   uint32_t  fwversion;
   uint32_t  pwm_cycle;
@@ -61,3 +67,5 @@ typedef struct pxmcc_data_t {
   pxmcc_axis_data_t   axis[PXMCC_AXIS_COUNT];
   pxmcc_curadc_data_t curadc[PXMCC_CURADC_CHANNELS];
 } pxmcc_data_t;
+
+#endif /*_PXMCC_TYPES_H_*/

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 77f16deb41b076c2044ee5b8d08c10cdad639d54..1f9287f3109db6c9ecd2eb1e26b456a9991acde2 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -33,6 +33,9 @@
 #include "appl_defs.h"
 #include "appl_fpga.h"
 #include "pxmcc_types.h"
+#include "pxmcc_interface.h"
+
+#define PXMC_AXIS_MODE_BLDC_PXMCC (PXMC_AXIS_MODE_BLDC + 1)
 
 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
                            unsigned long index_irc, int diff2err);
@@ -55,7 +58,9 @@ int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
 
 #define LX_MASTER_DATA_OFFS  8
 
-unsigned pxmc_rocon_pwm_magnitude = 2500;
+#define PXMC_LXPWR_PWM_CYCLE 2500
+
+unsigned pxmc_rocon_pwm_magnitude = PXMC_LXPWR_PWM_CYCLE;
 
 long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
 unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
@@ -707,6 +712,187 @@ pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
   return 0;
 }
 
+/*******************************************************************/
+/* PXMCC - PXMC coprocessor support and communication */
+
+int
+pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
+{
+  if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
+      (mcs->pxms_flg & PXMS_PRA_m)) {
+    short ptindx;
+    short ptirc = mcs->pxms_ptirc;
+    short divisor = mcs->pxms_ptper * 6;
+    unsigned char hal_pos;
+
+    hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bldc_hal_rd(mcs)];
+
+    if (hal_pos == 0xff) {
+      if (mcs->pxms_ene)
+        pxmc_rocon_process_hal_error(mcs);
+    } else {
+      if (mcs->pxms_halerc)
+        mcs->pxms_halerc--;
+
+      ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
+
+      if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m)) {
+        int set_ptofs_fl = 0;
+
+        if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1) {
+          short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
+
+          if ((ptindx > ptindx_prev + ptirc / 2) ||
+              (ptindx_prev > ptindx + ptirc / 2)) {
+            ptindx = (ptindx_prev + ptindx - ptirc) / 2;
+
+            if (ptindx < 0)
+              ptindx += ptirc;
+          } else {
+            ptindx = (ptindx_prev + ptindx) / 2;
+          }
+
+          set_ptofs_fl = 1;
+
+          pxmc_set_flag(mcs, PXMS_PTI_b);
+          pxmc_clear_flag(mcs, PXMS_PRA_b);
+        } else {
+          if (!(mcs->pxms_flg & PXMS_PTI_m))
+            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;
+        }
+      } else {
+        /* if phase table position to mask is know do fine phase table alignment */
+        if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) {
+          int res;
+
+          res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
+          if (res < 0) {
+            pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
+          } else if (res) {
+            pxmc_set_flag(mcs, PXMS_PTI_b);
+            pxmc_set_flag(mcs, PXMS_PHA_b);
+          }
+        }
+      }
+      mcs->pxms_hal = hal_pos;
+    }
+  }
+
+  {
+    /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
+    /* FIXME - check winding current against limit */
+    /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
+  }
+
+  {
+    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_pwm3ph_wr(mcs, 0, 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;
+  uint32_t pwmtx_info_dummy = 27;
+  uint64_t ull;
+  int      i;
+  int      phcnt = 0;
+  int      pwm_chan;
+
+  if (mcc_axis == NULL)
+    return -1;
+
+  mcc_axis->ccflg = 0;
+  mcc_axis->mode = PXMCC_MODE_IDLE;
+
+  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;
+
+  switch (mode) {
+    case PXMCC_MODE_IDLE:
+      phcnt = 0;
+      break;
+    case PXMCC_MODE_MODE_BLDC:
+      phcnt = 3;
+      break;
+    case PXMCC_MODE_STEPPER_WITH_IRC:
+      phcnt = 4;
+      break;
+  }
+
+  mcc_axis->inp_info = mcs->pxms_inp_info;
+  mcc_axis->out_info = mcs->pxms_out_info;
+
+  pwm_chan = mcs->pxms_out_info;
+
+  pwmtx_info = (pwmtx_info_dummy << 0) | (pwmtx_info_dummy << 8) |
+               (pwmtx_info_dummy << 16) | (pwmtx_info_dummy << 24);
+
+  for (i = phcnt; --i >= 0; ) {
+    volatile uint32_t *pwm_reg;
+    volatile uint32_t *pwm_reg_base = fpga_lx_master_transmitter_base;
+
+    pwmtx_info <<= 8;
+
+    pwm_reg = pxmc_rocon_pwm_chan2reg(pwm_chan + i);
+    if (pwm_reg == &pxmc_rocon_pwm_dummy_reg) {
+      pwmtx_info |= pwmtx_info_dummy;
+    } else {
+      pwmtx_info |= pwm_reg - pwm_reg_base;
+    }
+  }
+
+  mcc_axis->pwmtx_info = pwmtx_info;
+
+  mcc_axis->mode = mode;
+
+  mcc_axis->ccflg = 0;
+  mcc_axis->pwm_dq = 0;
+
+  return 0;
+}
+
+/*******************************************************************/
+
 volatile void *pxmc_rocon_rx_data_hist_buff;
 volatile void *pxmc_rocon_rx_data_hist_buff_end;
 int pxmc_rocon_rx_data_hist_mode;
@@ -753,7 +939,7 @@ IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
       } else if (pxmc_rocon_rx_data_hist_mode == 1) {
         int i;
         uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff;
-        pxmcc_data_t *mcc_data = (pxmcc_data_t *)fpga_tumbl_dmem;
+        pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
         pxmcc_axis_data_t *mcc_axis = mcc_data->axis + 0;
         uint32_t *ptumbl = (uint32_t *)mcc_axis;
 
@@ -841,7 +1027,7 @@ pxmc_rocon_pwm_master_init(void)
 
   *fpga_lx_master_reset = 1;
   *fpga_lx_master_transmitter_reg = 0;
-  *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
+  *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
 
   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
@@ -869,7 +1055,7 @@ pxmc_rocon_pwm_master_init(void)
   fpga_lx_master_transmitter_base[grp_out++] = 0x0000;
 
   *fpga_lx_master_reset = 0;
-  *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
+  *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
 
   return 0;
@@ -1595,6 +1781,8 @@ int pxmc_axis_rdmode(pxmc_state_t *mcs)
     return PXMC_AXIS_MODE_DC;
   if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
     return PXMC_AXIS_MODE_BLDC;
+  if (mcs->pxms_do_out == pxmc_pxmcc_pwm3ph_out)
+    return PXMC_AXIS_MODE_BLDC_PXMCC;
   return -1;
 }
 
@@ -1626,6 +1814,7 @@ pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
       mcs->pxms_ptscale_shift=15;
       break;
     case PXMC_AXIS_MODE_BLDC:
+    case PXMC_AXIS_MODE_BLDC_PXMCC:
       /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
      #ifndef PXMC_WITH_PT_ZIC
       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
@@ -1688,6 +1877,12 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
     case PXMC_AXIS_MODE_BLDC:
       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)
+        return -1;
+      pxmcc_axis_enable(mcs, 1);
+      mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out;
+      break;
     default:
       return -1;
   }

diff --git a/sw/app/rocon/appl_tests.c b/sw/app/rocon/appl_tests.c
index b7686d89337c16f5e12469b38918d48b16887c81..a8d977a9c34b3076c05cc4f3f457c1f92f4c893a 100644 (file)
--- a/sw/app/rocon/appl_tests.c
+++ b/sw/app/rocon/appl_tests.c
@@ -21,6 +21,7 @@
 #include "appl_defs.h"
 #include "appl_fpga.h"
 #include "pxmcc_types.h"
+#include "pxmcc_interface.h"
 
 int cmd_do_test_memusage(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 {
@@ -404,17 +405,10 @@ int cmd_do_testtumblefw(cmd_io_t *cmd_io, const struct cmd_des *des, char *param
   char *ps = param[1];
   long pwm_d;
   long pwm_q;
-  uint32_t ptofs;
-  uint32_t irc;
-  uint32_t ptirc;
-  uint32_t ptreci;
-  uint32_t pwmtx_info;
-  uint64_t ull;
   pxmc_state_t *mcs = pxmc_main_list.pxml_arr[0];
-  volatile pxmcc_data_t *mcc_data = (pxmcc_data_t *)fpga_tumbl_dmem;
-  volatile pxmcc_axis_data_t *mcc_axis = mcc_data->axis + 0;
+  volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
 
-  mcc_axis->ccflg = 0;
+  pxmcc_axis_enable(mcs, 0);
 
   si_skspace(&ps);
   if (si_long(&ps, &pwm_d, 0) < 0)
@@ -424,47 +418,27 @@ int cmd_do_testtumblefw(cmd_io_t *cmd_io, const struct cmd_des *des, char *param
   if (si_ulong(&ps, &pwm_q, 0) < 0)
         return -CMDERR_BADPAR;
 
-  irc = fpga_irc[0]->count;
-  ptofs = (int16_t)(mcs->pxms_ptofs - irc) + irc;
-
-  ptirc = mcs->pxms_ptirc;
-  ull = (1ULL << 32) * mcs->pxms_ptper;
-  ptreci = (ull + ptirc / 2) / ptirc;
-
-  pwmtx_info = (9 << 0) | (10 << 8) | (11 << 16);
-
-  mcc_axis->mode = 0;
-
-  mcc_axis->inp_info = mcs->pxms_inp_info;
-  mcc_axis->out_info = mcs->pxms_out_info;
-  mcc_axis->pwmtx_info = pwmtx_info;
-
-  mcc_axis->ptirc = ptirc;
-  mcc_axis->ptreci = ptreci;
-  mcc_axis->ptofs = ptofs;
-
-  mcc_axis->ccflg = 0;
-  mcc_axis->pwm_dq = (pwm_d << 16) | (pwm_q & 0xffff);
 
+  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);
 
-  mcc_axis->ccflg = 1;
-
-  if (0) {
+  if (1) {
     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 = 0;
+    mcc_data->axis[1].mode = PXMCC_MODE_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 = 0;
+    mcc_data->axis[2].mode = PXMCC_MODE_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 = 0;
+    mcc_data->axis[3].mode = PXMCC_MODE_MODE_BLDC;
     mcc_data->axis[3].ccflg = 1;
   }
 

diff --git a/sw/app/rocon/pxmcc_interface.h b/sw/app/rocon/pxmcc_interface.h
new file mode 100644 (file)
index 0000000..be0163d
--- /dev/null
+++ b/sw/app/rocon/pxmcc_interface.h
@@ -0,0 +1,57 @@
+/*******************************************************************
+  Components for embedded applications builded for
+  laboratory and medical instruments firmware
+
+  pxmcc_interface.h - multi axis motion controller coprocessor
+               interface for access lx-rocon system
+
+  (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
+  (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com
+
+  This file can be used and copied according to next
+  license alternatives
+   - GPL - GNU Public License
+   - other license provided by project originators
+
+ *******************************************************************/
+
+#ifndef _PXMCC_INTERFACE_H_
+#define _PXMCC_INTERFACE_H_
+
+#include <pxmc.h>
+#include "pxmcc_types.h"
+
+static inline
+pxmcc_data_t *pxmc_rocon_mcc_data(void)
+{
+  return (pxmcc_data_t *)fpga_tumbl_dmem;
+}
+
+static inline
+pxmcc_axis_data_t *pxmc_rocon_mcs2pxmcc(pxmc_state_t *mcs)
+{
+  pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
+
+  if (mcs->pxms_inp_info >= PXMCC_AXIS_COUNT)
+    return NULL;
+
+  return mcc_data->axis + mcs->pxms_inp_info;
+}
+
+static inline
+void pxmcc_axis_pwm_dq_out(pxmc_state_t *mcs, int pwm_d, int pwm_q)
+{
+  volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+  mcc_axis->pwm_dq = (pwm_d << 16) | (pwm_q & 0xffff);
+}
+
+static inline
+void pxmcc_axis_enable(pxmc_state_t *mcs, int enable)
+{
+  volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+  mcc_axis->ccflg = enable? 1: 0;
+}
+
+int pxmcc_axis_setup(pxmc_state_t *mcs, int mode);
+
+#endif /*_PXMCC_INTERFACE_H_*/