RoCoN: add current D component keep zero for BDLC/PMSM PXMCC based control.

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 62be6afe7b67a370eb58975225d8c762cabe6eb6..f2f919da0d90e0902bf22fa08ffee141e7a5040e 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -822,6 +822,10 @@ void pxmcc_cur_ctrl_pi(int *p_pwm, int32_t *p_cur_err_sum,
 int
 pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 {
+  int32_t cur_d_raw;
+  int32_t cur_q_raw;
+  pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
+
   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
       (mcs->pxms_flg & PXMS_PRA_m)) {
     short ptindx;
@@ -891,6 +895,8 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
     }
   }
 
+  pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
   {
     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
     /* FIXME - check winding current against limit */
@@ -904,6 +910,21 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
     pwm_d = 0;
     pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
 
+    if (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);
+    }
+
     pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
 
     if (mcs->pxms_flg & PXMS_ERR_m)