]> rtime.felk.cvut.cz Git - fpga/lx-cpu1/lx-rocon.git/blobdiff - sw/app/rocon/appl_pxmc.c
RoCoN: do not run stepper motor current controller if axis not powered.
[fpga/lx-cpu1/lx-rocon.git] / sw / app / rocon / appl_pxmc.c
index 6954b0b39a1eaa7f9f300ee371061e249b79dd01..95cab6e9b779d117cf44f3f924a38ace4796b623 100644 (file)
@@ -1,8 +1,7 @@
 /*******************************************************************
   Motion and Robotic System (MARS) aplication components.
 
-  appl_pxmc.c - position controller subsystem core generic
-                and LP_MPW1 hardware specific support
+  appl_pxmc.c - position controller RoCoN hardware support
 
   Copyright (C) 2001-2013 by Pavel Pisa - originator
                           pisa@cmp.felk.cvut.cz
 
 #include "appl_defs.h"
 #include "appl_fpga.h"
+#include "appl_pxmc.h"
 #include "pxmcc_types.h"
 #include "pxmcc_interface.h"
 
 #define PXMC_AXIS_MODE_BLDC_PXMCC             (PXMC_AXIS_MODE_BLDC + 1)
 #define PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC (PXMC_AXIS_MODE_BLDC + 2)
+#define PXMC_AXIS_MODE_STEPPER_PXMCC          (PXMC_AXIS_MODE_BLDC + 3)
 
 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
                            unsigned long index_irc, int diff2err);
@@ -66,6 +67,18 @@ 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];
 
+static inline
+pxmc_rocon_state_t *pxmc_state2rocon_state(pxmc_state_t *mcs)
+{
+  pxmc_rocon_state_t *mcsrc;
+ #ifdef UL_CONTAINEROF
+  mcsrc = UL_CONTAINEROF(mcs, pxmc_rocon_state_t, base);
+ #else /*UL_CONTAINEROF*/
+  mcsrc = (pxmc_rocon_state_t*)((char*)mcs - __builtin_offsetof(pxmc_rocon_state_t, base));
+ #endif /*UL_CONTAINEROF*/
+  return mcsrc;
+}
+
 const uint8_t onesin10bits[1024]={
   0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
@@ -736,6 +749,76 @@ void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs, int enable_update)
   }
 }
 
+static inline
+void pxmcc_axis_get_cur_dq_filt_raw(pxmc_state_t *mcs,
+                                    int32_t *p_cur_d_raw, int32_t *p_cur_q_raw)
+{
+  volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+  pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
+  uint32_t cur_d_cum = mcc_axis->cur_d_cum;
+  uint32_t cur_q_cum = mcc_axis->cur_q_cum;
+  int32_t cur_d;
+  int32_t cur_q;
+
+  cur_d = cur_d_cum - mcsrc->cur_d_cum_prev;
+  mcsrc->cur_d_cum_prev = cur_d_cum;
+  cur_q = cur_q_cum - mcsrc->cur_q_cum_prev;
+  mcsrc->cur_q_cum_prev = cur_q_cum;
+
+  *p_cur_d_raw = cur_d;
+  *p_cur_q_raw = cur_q;
+}
+
+static inline
+void pxmcc_axis_cur_dq_raw2filt(int *p_cur, int32_t cur_raw)
+{
+  int cur_div;
+  int32_t cur;
+  cur_div = cur_raw & 0x1f;
+  cur = cur_raw / cur_div;
+  cur += (1 << 11) | 0x20;
+  *p_cur = cur >> 12;
+}
+
+
+void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q)
+{
+  int32_t cur_d_raw;
+  int32_t cur_q_raw;
+
+  pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
+  pxmcc_axis_cur_dq_raw2filt(p_cur_d, cur_d_raw);
+  pxmcc_axis_cur_dq_raw2filt(p_cur_q, cur_q_raw);
+}
+
+static inline
+void pxmcc_cur_ctrl_pi(int *p_pwm, int32_t *p_cur_err_sum,
+               int cur_err, short cur_ctrl_p, short cur_ctrl_i, int max_pwm)
+{
+  int pwm;
+  int32_t cur_err_sum = *p_cur_err_sum;
+
+  pwm = (cur_err * cur_ctrl_p) >> 8;
+
+  if (cur_ctrl_i)
+    cur_err_sum += cur_err * cur_ctrl_i;
+  else
+    cur_err_sum = 0;
+
+  pwm += cur_err_sum >> 16;
+
+  if (pwm > max_pwm) {
+    cur_err_sum -= (pwm - max_pwm) << 16;
+    pwm = max_pwm;
+  } else if (-pwm > max_pwm) {
+    cur_err_sum -= (pwm + max_pwm) << 16;
+    pwm = 0 - max_pwm;
+  }
+  *p_cur_err_sum = cur_err_sum;
+  *p_pwm = pwm;
+}
+
 int
 pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 {
@@ -868,12 +951,97 @@ pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
   return 0;
 }
 
+/**
+ * pxmc_pxmcc_nofb_inp - Dummy input for direct stepper motor control
+ * @mcs:        Motion controller state information
+ */
+int
+pxmc_pxmcc_nofb_inp(pxmc_state_t *mcs)
+{
+  return 0;
+}
+
+/**
+ * pxmc_pxmcc_nofb_con - Empty controller for direct stepper motor control
+ * @mcs:        Motion controller state information
+ */
+int
+pxmc_pxmcc_nofb_con(pxmc_state_t *mcs)
+{
+  mcs->pxms_ene=mcs->pxms_me;
+  return 0;
+}
+
+int
+pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
+{
+  pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
+  volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+  int32_t cur_d_raw;
+  int32_t cur_q_raw;
+
+  pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
+  if ((mcs->pxms_flg & PXMS_ERR_m) ||
+      !(mcs->pxms_flg & (PXMS_ENO_m | PXMS_ENR_m))) {
+    pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
+    pxmcc_axis_pwm_dq_out(mcs, 0, 0);
+    mcc_axis->steps_inc = 0;
+    mcsrc->cur_d_err_sum = 0;
+    mcsrc->cur_q_err_sum = 0;
+  } else {
+    int pwm_d, pwm_q;
+    int cur_d, cur_q;
+    int cur_d_req, cur_d_err;
+    int cur_q_req, cur_q_err;
+    int max_pwm = mcs->pxms_me;
+    int32_t stpinc;
+
+    pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
+    pxmcc_axis_cur_dq_raw2filt(&cur_q, cur_q_raw);
+
+    if (!mcs->pxms_ene)
+      cur_d_req = 0;
+    else
+      cur_d_req = mcsrc->cur_hold;
+
+    cur_d_err = cur_d_req - cur_d;
+
+    pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err,
+                      mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm);
+
+    /* pwm_d = (pxmc_rocon_pwm_magnitude * ene) >> 15; */
+
+    cur_q_req = 0;
+
+    cur_q_err = cur_q_req - cur_q;
+
+    pxmcc_cur_ctrl_pi(&pwm_q, &mcsrc->cur_q_err_sum, cur_q_err,
+                      mcsrc->cur_q_p, mcsrc->cur_q_i, max_pwm);
+
+    pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
+
+    mcs->pxms_ap=mcs->pxms_rp;
+    mcs->pxms_as=mcs->pxms_rs;
+    mcc_axis->steps_lim = mcc_axis->steps_cnt + 6;
+
+    stpinc = mcs->pxms_rs;
+    mcc_axis->steps_inc = stpinc;
+
+      /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */
+      /* pxms_ptscale_mult; pxms_ptscale_shift; */
+  }
+
+  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 ptirc;
   uint32_t ptreci;
+  uint32_t inp_info;
   uint32_t pwmtx_info;
   uint32_t pwmtx_info_dummy = 27;
   uint64_t ull;
@@ -900,6 +1068,9 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
 
   pxmcc_pxmc_ptofs2mcc(mcs, 0);
 
+  inp_info = (char*)&fpga_irc[mcs->pxms_inp_info]->count - (char*)fpga_irc[0];
+  inp_info += mcc_data->common.irc_base;
+
   switch (mode) {
     case PXMCC_MODE_IDLE:
       phcnt = 0;
@@ -910,9 +1081,14 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
     case PXMCC_MODE_STEPPER_WITH_IRC:
       phcnt = 4;
       break;
+    case PXMCC_MODE_STEPPER:
+      phcnt = 4;
+      mcc_axis->ptreci = 1;
+      inp_info = (char*)&mcc_axis->steps_pos - (char*)mcc_data;
+      break;
   }
 
-  mcc_axis->inp_info = mcs->pxms_inp_info;
+  mcc_axis->inp_info = inp_info;
   mcc_axis->out_info = mcs->pxms_out_info;
 
   pwm_chan = mcs->pxms_out_info;
@@ -940,9 +1116,13 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
 
   mcc_axis->ccflg = 0;
   mcc_axis->pwm_dq = 0;
+  mcc_axis->steps_lim = mcc_axis->steps_cnt;
+  mcc_axis->steps_inc = 0;
+  mcc_axis->steps_pos = 0;
 
-  pxmcc_pxmc_ptofs2mcc(mcs, 1);
-
+  if (mode != PXMCC_MODE_STEPPER) {
+    pxmcc_pxmc_ptofs2mcc(mcs, 1);
+  }
   return 0;
 }
 
@@ -1317,8 +1497,9 @@ pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
   return pxmc_rocon_hh_gi;
 }
 
-pxmc_state_t mcs0 =
+pxmc_rocon_state_t mcs0 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1350,10 +1531,17 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
+},
+  .cur_d_p = 150,
+  .cur_d_i = 6000,
+  .cur_q_p = 150,
+  .cur_q_i = 6000,
+  .cur_hold = 200,
 };
 
-pxmc_state_t mcs1 =
+pxmc_rocon_state_t mcs1 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1384,10 +1572,11 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
-};
+}};
 
-pxmc_state_t mcs2 =
+pxmc_rocon_state_t mcs2 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1418,10 +1607,11 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
-};
+}};
 
-pxmc_state_t mcs3 =
+pxmc_rocon_state_t mcs3 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1452,10 +1642,11 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
-};
+}};
 
-pxmc_state_t mcs4 =
+pxmc_rocon_state_t mcs4 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1486,10 +1677,11 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
-};
+}};
 
-pxmc_state_t mcs5 =
+pxmc_rocon_state_t mcs5 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1520,10 +1712,11 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
-};
+}};
 
-pxmc_state_t mcs6 =
+pxmc_rocon_state_t mcs6 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1553,10 +1746,11 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
-};
+}};
 
-pxmc_state_t mcs7 =
+pxmc_rocon_state_t mcs7 =
 {
+.base = {
 pxms_flg:
   PXMS_ENI_m,
 pxms_do_inp:
@@ -1586,11 +1780,11 @@ pxms_cfg:
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
-};
-
+}};
 
 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
-{&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
+{&mcs0.base, &mcs1.base, &mcs2.base, &mcs3.base,
+ &mcs4.base, &mcs5.base, &mcs6.base, &mcs7.base};
 
 
 pxmc_state_list_t  pxmc_main_list =
@@ -1840,6 +2034,8 @@ int pxmc_axis_rdmode(pxmc_state_t *mcs)
     return PXMC_AXIS_MODE_BLDC_PXMCC;
   if (mcs->pxms_do_out == pxmc_pxmcc_pwm2ph_out)
     return PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC;
+  if (mcs->pxms_do_out == pxmc_pxmcc_nofb2ph_out)
+    return PXMC_AXIS_MODE_STEPPER_PXMCC;
   return -1;
 }
 
@@ -1856,7 +2052,8 @@ pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
     return -1;
 
   switch (mode) {
-    /*case PXMC_AXIS_MODE_STEPPER:*/
+    /* case PXMC_AXIS_MODE_STEPPER: */
+    case PXMC_AXIS_MODE_STEPPER_PXMCC:
     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);
@@ -1930,6 +2127,11 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
   if (res < 0)
     return -1;
 
+  if (mcs->pxms_do_inp == pxmc_pxmcc_nofb_inp)
+    mcs->pxms_do_inp = pxmc_inp_rocon_inp;
+  if (mcs->pxms_do_con == pxmc_pxmcc_nofb_con)
+    mcs->pxms_do_con = pxmc_pid_con;
+
   switch (mode) {
     /*case PXMC_AXIS_MODE_STEPPER:*/
     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
@@ -1954,6 +2156,14 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
       pxmcc_axis_enable(mcs, 1);
       mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out;
       break;
+    case PXMC_AXIS_MODE_STEPPER_PXMCC:
+      if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER) < 0)
+        return -1;
+      pxmcc_axis_enable(mcs, 1);
+      mcs->pxms_do_inp = pxmc_pxmcc_nofb_inp;
+      mcs->pxms_do_con = pxmc_pxmcc_nofb_con;
+      mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out;
+      break;
     default:
       return -1;
   }
@@ -2051,7 +2261,7 @@ int pxmc_initialize(void)
   int res;
   int i;
 
-  pxmc_state_t *mcs = &mcs0;
+  pxmc_state_t *mcs = &mcs0.base;
   lpc_qei_state_t *qst = &lpc_qei_state;
 
   *fpga_irc_reset = 1;