RoCoN: add current D component stabilization to zero for stepper motor with feedback.

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

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 95cab6e9b779d117cf44f3f924a38ace4796b623..62be6afe7b67a370eb58975225d8c762cabe6eb6 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -916,6 +916,12 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 int
 pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
 {
+  pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(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_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
      (mcs->pxms_flg&PXMS_PRA_m)) {
 
@@ -940,6 +946,23 @@ pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
 
     ene = mcs->pxms_ene;
     pwm_d = 0;
+
+    if (mcs->pxms_flg & PXMS_PHA_m &&
+        (mcs->pxms_flg & (PXMS_ENR_m | PXMS_ENO_m))) {
+      int cur_d;
+      int cur_d_req, cur_d_err;
+      int max_pwm = (pxmc_rocon_pwm_magnitude * mcs->pxms_me) >> 15;
+
+      cur_d_req = 0;
+
+      pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
+
+      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_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
 
     pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);