X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/lx-cpu1/lx-rocon.git/blobdiff_plain/5f0a3f30353515eb28bcfc80a96624fb9d88c749..b92be0601a9c6391283b2c045d963cb07ffd30e9:/sw/app/rocon/appl_pxmc.c

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 6208037..36e0e56 100644
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -749,7 +749,9 @@ void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs, int enable_update)
   }
 }
 
-void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q)
+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);
@@ -757,21 +759,64 @@ void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q)
   uint32_t cur_q_cum = mcc_axis->cur_q_cum;
   int32_t cur_d;
   int32_t cur_q;
-  int cur_d_div;
-  int cur_q_div;
 
   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;
 
-  cur_d_div = cur_d & 0x1f;
-  cur_d = cur_d / cur_d_div;
-  cur_q_div = cur_q & 0x1f;
-  cur_q = cur_q / cur_q_div;
+  *p_cur_d_raw = cur_d;
+  *p_cur_q_raw = cur_q;
+}
 
-  *p_cur_d = cur_d >> 12;
-  *p_cur_q = cur_q >> 12;
+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
@@ -932,11 +977,17 @@ 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) {
     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;
@@ -945,10 +996,8 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
     int max_pwm = mcs->pxms_me;
     int32_t stpinc;
 
-    /* cur_d_err_sum; */
-    /* cur_q_err_sum; */
-
-    pxmcc_axis_get_cur_dq_filt(mcs, &cur_d, &cur_q);
+    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;
@@ -957,45 +1006,17 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
 
     cur_d_err = cur_d_req - cur_d;
 
-    pwm_d = (cur_d_err * mcsrc->cur_d_p) >> 8;
-
-    if (mcsrc->cur_d_i)
-      mcsrc->cur_d_err_sum += cur_d_err * mcsrc->cur_d_i;
-    else
-      mcsrc->cur_d_err_sum = 0;
-
-    pwm_d += mcsrc->cur_d_err_sum >> 16;
+    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; */
 
-    if (pwm_d > max_pwm) {
-      mcsrc->cur_d_err_sum -= (pwm_d - max_pwm) << 16;
-      pwm_d = max_pwm;
-    } else if (-pwm_d > max_pwm) {
-      mcsrc->cur_d_err_sum -= (pwm_d + max_pwm) << 16;
-      pwm_d = 0 - max_pwm;
-    }
-
     cur_q_req = 0;
 
     cur_q_err = cur_q_req - cur_q;
 
-    pwm_q = (cur_q_req * mcsrc->cur_q_p) >> 8;
-
-    if (mcsrc->cur_q_i)
-      mcsrc->cur_q_err_sum += cur_q_err * mcsrc->cur_q_i;
-    else
-      mcsrc->cur_q_err_sum = 0;
-
-    pwm_q += mcsrc->cur_q_err_sum >> 16;
-
-    if (pwm_q > max_pwm) {
-      mcsrc->cur_q_err_sum -= (pwm_q - max_pwm) << 16;
-      pwm_q = max_pwm;
-    } else if (-pwm_q > max_pwm) {
-      mcsrc->cur_q_err_sum -= (pwm_q + max_pwm) << 16;
-      pwm_q = 0 - max_pwm;
-    }
+    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);