RoCoN and TUMBL firmware: reimplemented control of stepper motor without feedback.

[fpga/lx-cpu1/lx-rocon.git] / sw / app / rocon / appl_pxmc.c

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 17f6e2bdd78d022ac611be316a51e6264b8ce683..fdd9ef98fba155032e30e7aa2f607e6bc794aec4 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -106,6 +106,35 @@ void pxmc_rocon_vin_compute(void)
   pxmc_rocon_vin_act = vin_act;
 }
 
+unsigned int pxmc_rocon_rx_done_sqn;
+unsigned int pxmc_rocon_rx_done_sqn_inc;
+unsigned int pxmc_rocon_rx_done_sqn_misscnt;
+unsigned int pxmc_rocon_rx_done_sqn_missoffs;
+
+static inline
+void pxmc_rocon_rx_done_sqn_compute(void)
+{
+  uint32_t sqn_act;
+  uint32_t sqn_offs;
+  unsigned int sqn_expect = pxmc_rocon_rx_done_sqn + pxmc_rocon_rx_done_sqn_inc;
+
+  sqn_act = *fpga_lx_master_receiver_done_div;
+  sqn_offs = (sqn_act - sqn_expect) & 0x1f;
+  if (sqn_offs) {
+    if (pxmc_rocon_rx_done_sqn_missoffs != sqn_offs) {
+      pxmc_rocon_rx_done_sqn_misscnt = 1;
+    } else {
+      pxmc_rocon_rx_done_sqn_misscnt++;
+      if (pxmc_rocon_rx_done_sqn_misscnt >= 10)
+        sqn_expect += 1 - ((sqn_offs >> 3) & 2);
+    }
+  } else {
+    pxmc_rocon_rx_done_sqn_misscnt = 0;
+  }
+  pxmc_rocon_rx_done_sqn = sqn_expect;
+  pxmc_rocon_rx_done_sqn_missoffs = sqn_offs;
+}
+
 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,
@@ -1023,12 +1052,16 @@ pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
 }
 
 /**
- * pxmc_pxmcc_nofb_inp - Dummy input for direct stepper motor control
+ * pxmc_pxmcc_nofb2ph_inp - Dummy input for direct stepper motor control
  * @mcs:        Motion controller state information
  */
 int
-pxmc_pxmcc_nofb_inp(pxmc_state_t *mcs)
+pxmc_pxmcc_nofb2ph_inp(pxmc_state_t *mcs)
 {
+  volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+  uint32_t steps_pos_act = mcc_axis->steps_pos;
+  mcs->pxms_as = (int32_t)(steps_pos_act - mcs->pxms_ap);
+  mcs->pxms_ap += mcs->pxms_as;
   return 0;
 }
 
@@ -1058,6 +1091,7 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
     pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
     pxmcc_axis_pwm_dq_out(mcs, 0, 0);
     mcc_axis->steps_inc = 0;
+    mcc_axis->steps_inc_next = 0;
     mcsrc->cur_d_err_sum = 0;
     mcsrc->cur_q_err_sum = 0;
   } else {
@@ -1067,6 +1101,7 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
     int cur_q_req, cur_q_err;
     int max_pwm = mcs->pxms_me;
     int32_t stpinc;
+    int32_t stpdiv;
 
     pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
     pxmcc_axis_cur_dq_raw2filt(&cur_q, cur_q_raw);
@@ -1092,15 +1127,16 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
 
     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_rp - mcsrc->steps_pos_prev;
+
+    stpdiv = pxmc_rocon_rx_done_sqn_inc;
+    mcc_axis->steps_inc_next = (stpinc + stpdiv / 2) / stpdiv;
+    mcc_axis->steps_pos_next = mcsrc->steps_pos_prev;
 
-    stpinc = mcs->pxms_rs;
-    mcc_axis->steps_inc = stpinc;
+    mcsrc->steps_pos_prev = mcs->pxms_rp;
 
-      /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */
-      /* pxms_ptscale_mult; pxms_ptscale_shift; */
+    mcc_axis->steps_sqn_next = pxmc_rocon_rx_done_sqn +
+                               pxmc_rocon_rx_done_sqn_inc - 1;
   }
 
   return 0;
@@ -1110,6 +1146,7 @@ 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);
+  pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
   uint32_t ptirc;
   uint32_t ptreci;
   uint32_t inp_info;
@@ -1132,6 +1169,9 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
   mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE;
 
   ptirc = mcs->pxms_ptirc;
+  if (mode == PXMCC_MODE_STEPPER)
+    ptirc <<= PXMC_SUBDIV(mcs);
+
   ull = (1ULL << 32) * mcs->pxms_ptper;
   ptreci = (ull + ptirc / 2) / ptirc;
 
@@ -1154,7 +1194,6 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
       break;
     case PXMCC_MODE_STEPPER:
       phcnt = 4;
-      mcc_axis->ptreci = 1;
       inp_info = (char*)&mcc_axis->steps_pos - (char*)mcc_data;
       break;
   }
@@ -1187,11 +1226,16 @@ 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;
 
-  if (mode != PXMCC_MODE_STEPPER) {
+  if (mode == PXMCC_MODE_STEPPER) {
+    mcsrc->steps_pos_prev = mcs->pxms_rp = mcs->pxms_ap;
+    mcs->pxms_rs = mcs->pxms_as = 0;
+    mcc_axis->steps_inc_next = 0;
+    mcc_axis->steps_pos_next = mcsrc->steps_pos_prev;
+    mcc_axis->steps_inc = 0;
+    mcc_axis->steps_pos = mcsrc->steps_pos_prev;
+    mcc_axis->ptirc = mcs->pxms_ptirc << PXMC_SUBDIV(mcs);
+  } else {
     pxmcc_pxmc_ptofs2mcc(mcs, 1);
   }
   return 0;
@@ -1226,6 +1270,7 @@ IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
     hal_gpio_set_value(T2MAT1_PIN, 0);
     hal_gpio_set_value(T2MAT0_PIN, 0);
 
+    pxmc_rocon_rx_done_sqn_compute();
     pxmc_rocon_vin_compute();
 
     if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
@@ -1337,6 +1382,7 @@ pxmc_rocon_pwm_master_init(void)
   *fpga_lx_master_transmitter_reg = 0;
   *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
+  pxmc_rocon_rx_done_sqn_inc = receiver_done_div;
 
   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
     fpga_lx_master_receiver_base[i] = 0;
@@ -2200,7 +2246,7 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
   if (res < 0)
     return -1;
 
-  if (mcs->pxms_do_inp == pxmc_pxmcc_nofb_inp)
+  if (mcs->pxms_do_inp == pxmc_pxmcc_nofb2ph_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;
@@ -2233,7 +2279,7 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
       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_inp = pxmc_pxmcc_nofb2ph_inp;
       mcs->pxms_do_con = pxmc_pxmcc_nofb_con;
       mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out;
       break;