Provide cumulative D and Q current components for filtering.

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

diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c
index 8e9cd7172879da758e7e5509e9a4a2d42a7bdae7..96d9f04b8e3ce3eb27b70be9119afad84a29e743 100644 (file)
--- a/hw/lx-rocon_firmware/firmware.c
+++ b/hw/lx-rocon_firmware/firmware.c
@@ -46,6 +46,8 @@ typedef struct pxmcc_axis_data_t {
   int32_t   ptcos;
   uint32_t  ptphs;
   uint32_t  pwm_cycle;
+  uint32_t  cur_d_cum;
+  uint32_t  cur_q_cum;
 
   uint32_t  act_idle;
   uint32_t  min_idle;
@@ -456,6 +458,12 @@ void main(void)
       cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
 
       pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
+
+      pxmcc->cur_d_cum = ((pxmcc->cur_d_cum + cur_d) & ~0xff) |
+                         (last_rx_done_sqn & 0x1f);
+      pxmcc->cur_q_cum = ((pxmcc->cur_q_cum + cur_q) & ~0xff) |
+                         (last_rx_done_sqn & 0x1f);
+
     }
   }
 }