X-Git-Url: http://rtime.felk.cvut.cz/gitweb/fpga/lx-cpu1/lx-rocon.git/blobdiff_plain/04dfa25c5b1fd599ed5f3799efe0a8e3726a8cf6..19ee858a44cfc628efd1299cd0923184aaa47b79:/sw/app/rocon/appl_pxmc.c

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 99c652f..1378b59 100644
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -66,6 +66,7 @@ unsigned pxmc_rocon_pwm_magnitude = PXMC_LXPWR_PWM_CYCLE;
 
 long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
 unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
+unsigned pxmc_rocon_lxpwr_chips = 0;
 
 static inline
 pxmc_rocon_state_t *pxmc_state2rocon_state(pxmc_state_t *mcs)
@@ -1290,6 +1291,39 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
   return 0;
 }
 
+int pxmcc_curadc_zero(int wait)
+{
+  int chan;
+  unsigned try = wait? 200: 0;
+  volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
+  volatile pxmcc_curadc_data_t *curadc;
+
+  for (chan = 0; chan < PXMCC_CURADC_CHANNELS; chan++)
+    pxmc_rocon_pwm_direct_wr(chan, 0, 0);
+
+  do {
+    if (mcc_data->common.fwversion == PXMCC_FWVERSION)
+      break;
+    if (!try--)
+      return -1;
+  } while(1);
+
+  if (wait) {
+    if (pxmc_rocon_wait_rx_done() < 0)
+      return -1;
+
+    if (pxmc_rocon_wait_rx_done() < 0)
+      return -1;
+  }
+
+  for (chan = 0; chan < PXMCC_CURADC_CHANNELS; chan++) {
+    curadc = mcc_data->curadc + chan;
+    curadc->siroladc_offs += curadc->cur_val;
+  }
+
+  return 0;
+}
+
 /*******************************************************************/
 
 volatile void *pxmc_rocon_rx_data_hist_buff;
@@ -1350,6 +1384,33 @@ IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
           *(pbuf++) = *(ptumbl++);
 
         pxmc_rocon_rx_data_hist_buff = pbuf;
+      } else if (!((pxmc_rocon_rx_data_hist_mode & 0xf8) ^ 0x10)) {
+        uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff;
+        volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
+        volatile pxmcc_curadc_data_t *curadc;
+        pxmc_rocon_state_t *mcsrc = NULL;
+        int chan = pxmc_rocon_rx_data_hist_mode & 7;
+        if (chan < pxmc_main_list.pxml_cnt)
+          mcsrc = pxmc_state2rocon_state(pxmc_main_list.pxml_arr[chan]);
+        if (mcsrc) {
+          *(pbuf++) = pxmc_rocon_vin_act;
+          chan = mcsrc->base.pxms_inp_info;
+          *(pbuf++) = fpga_irc[chan]->count + pxmc_rocon_irc_offset[chan];;
+          *(pbuf++) = mcsrc->base.pxms_rp >> PXMC_SUBDIV(&mcsrc->base);
+          *(pbuf++) = mcsrc->base.pxms_ene;
+          *(pbuf++) = mcsrc->base.pxms_foi;
+          chan = mcsrc->base.pxms_out_info;
+          curadc = mcc_data->curadc + chan;
+          *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+          *(pbuf++) = (curadc++)->cur_val;
+          *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+          *(pbuf++) = (curadc++)->cur_val;
+          *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+          *(pbuf++) = (curadc++)->cur_val;
+          *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+          *(pbuf++) = (curadc++)->cur_val;
+          pxmc_rocon_rx_data_hist_buff = pbuf;
+        }
       }
     }
 
@@ -1405,22 +1466,21 @@ pxmc_rocon_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
 }
 
 int
-pxmc_rocon_pwm_master_init(void)
+pxmc_rocon_pwm_master_setup(unsigned lxpwr_chips)
 {
   int i;
   int grp_in = 0;
   int grp_out = 0;
   unsigned word_slot;
   unsigned receiver_done_div = 1;
+  unsigned lxpwr_chips_max = 2;
  #ifdef LXPWR_WITH_SIROLADC
   unsigned lxpwr_header = 1;
   unsigned lxpwr_words = 1 + 8 * 2 + 2;
-  unsigned lxpwr_chips = 2;
   unsigned lxpwr_chip_pwm_cnt = 8;
  #else /*LXPWR_WITH_SIROLADC*/
   unsigned lxpwr_header = 0;
   unsigned lxpwr_words = 8;
-  unsigned lxpwr_chips = 2;
   unsigned lxpwr_chip_pwm_cnt = 8;
  #endif /*LXPWR_WITH_SIROLADC*/
 
@@ -1434,14 +1494,23 @@ pxmc_rocon_pwm_master_init(void)
   *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++)
+  if (lxpwr_chips > lxpwr_chips_max)
+    return -1;
+
+  for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips_max; i++)
     fpga_lx_master_receiver_base[i] = 0;
 
+  if (lxpwr_chips >= 2) {
+    word_slot = LX_MASTER_DATA_OFFS + lxpwr_words;
+    fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
+  }
+
   word_slot = LX_MASTER_DATA_OFFS;
   fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
+
   fpga_lx_master_receiver_base[grp_in++] = 0x0000;
 
-  for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
+  for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips_max; i++)
     fpga_lx_master_transmitter_base[i] = 0;
 
   word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
@@ -1465,6 +1534,76 @@ pxmc_rocon_pwm_master_init(void)
   return 0;
 }
 
+int
+pxmc_rocon_wait_rx_done(void)
+{
+  uint32_t sqn_last;
+  uint32_t sqn_act;
+  uint32_t timeout = 10000;
+
+  sqn_last = *fpga_lx_master_receiver_done_div;
+  sqn_last = sqn_last & 0x1f;
+
+  do {
+    sqn_act = *fpga_lx_master_receiver_done_div;
+    sqn_act = sqn_act & 0x1f;
+    if (sqn_act != sqn_last)
+      return 0;
+  } while(timeout--);
+
+  return -1;
+}
+
+int
+pxmc_rocon_pwm_master_init(void)
+{
+  int res;
+  volatile uint32_t *lxpwr_header_ptr;
+  unsigned lxpwr_words = 1 + 8 * 2 + 2;
+
+  pxmc_rocon_lxpwr_chips = 0;
+
+  res = pxmc_rocon_pwm_master_setup(2);
+  if (res < 0)
+    return 0;
+
+  if (pxmc_rocon_wait_rx_done() < 0)
+    return -1;
+  if (pxmc_rocon_wait_rx_done() < 0)
+    return -1;
+
+  lxpwr_header_ptr = fpga_lx_master_receiver_base;
+  lxpwr_header_ptr += LX_MASTER_DATA_OFFS;
+
+  if (lxpwr_header_ptr[0] == 0xb100 + lxpwr_words - 1) {
+    if (lxpwr_header_ptr[lxpwr_words] == 0xb100 + lxpwr_words - 1) {
+      pxmc_rocon_lxpwr_chips = 2;
+      return 2;
+    }
+    return -1;
+  }
+
+  if (lxpwr_header_ptr[lxpwr_words] != 0xb100 + lxpwr_words - 1) {
+    return -1;
+  }
+
+  res = pxmc_rocon_pwm_master_setup(1);
+  if (res < 0)
+    return 0;
+
+  if (pxmc_rocon_wait_rx_done() < 0)
+    return -1;
+  if (pxmc_rocon_wait_rx_done() < 0)
+    return -1;
+
+  if (lxpwr_header_ptr[0] != 0xb100 + lxpwr_words - 1)
+    return -1;
+
+  pxmc_rocon_lxpwr_chips = 1;
+
+  return 1;
+}
+
 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
                            unsigned long index_irc, int diff2err)
 {
@@ -2285,7 +2424,7 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
   int res;
   int prev_mode;
 
-  pxmc_set_const_out(mcs, 0);
+  pxmc_axis_release(mcs);
   pxmc_clear_flag(mcs, PXMS_ENI_b);
   pxmc_clear_flags(mcs,PXMS_ENO_m);
   /* Clear possible stall index flags from hardware */
@@ -2304,7 +2443,8 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
     mode = PXMC_AXIS_MODE_DC;
 
   if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) ||
-      (prev_mode == PXMCC_MODE_STEPPER_WITH_IRC))
+      (prev_mode == PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC) ||
+      (prev_mode == PXMC_AXIS_MODE_STEPPER_PXMCC))
     pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE);
 
   res = pxmc_axis_pt4mode(mcs, mode);
@@ -2380,6 +2520,37 @@ void pxmc_sfi_isr(void)
 
 }
 
+pxmc_call_t *const pxmc_reg_type_table[] = {
+  pxmc_pid_con,
+  pxmc_pid_con,
+  pxmc_pidnl_con
+};
+
+
+int pxmc_get_reg_type(pxmc_state_t *mcs)
+{
+  int reg_type;
+  int max_type = sizeof(pxmc_reg_type_table) / sizeof(*pxmc_reg_type_table);
+
+  for (reg_type = 1; reg_type < max_type; reg_type++)
+    if (mcs->pxms_do_con == pxmc_reg_type_table[reg_type])
+      return reg_type;
+  return 0;
+}
+
+int pxmc_set_reg_type(pxmc_state_t *mcs, int reg_type)
+{
+  int max_type = sizeof(pxmc_reg_type_table) / sizeof(*pxmc_reg_type_table);
+
+  if ((reg_type < 0) || (reg_type >= max_type))
+    return -1;
+  if (mcs->pxms_flg & PXMS_ENR_m)
+    return -1;
+
+  mcs->pxms_do_con = pxmc_reg_type_table[reg_type];
+  return 0;
+}
+
 int pxmc_clear_power_stop(void)
 {
   return 0;
@@ -2415,7 +2586,7 @@ int pxmc_process_state_check(unsigned long *pbusy_bits,
 
   if (pbusy_bits != NULL)
     *pbusy_bits = busy_bits;
-  if (error_bits != NULL)
+  if (perror_bits != NULL)
     *perror_bits = error_bits;
 
   return flg;
@@ -2431,7 +2602,7 @@ int pxmc_done(void)
 
   pxmc_for_each_mcs(var, mcs)
   {
-    pxmc_set_const_out(mcs,0);
+    pxmc_axis_release(mcs);
   }
 
   pxmc_main_list.pxml_cnt = 0;
@@ -2445,6 +2616,13 @@ int pxmc_initialize(void)
   int res;
   int i;
 
+  pxmc_main_list.pxml_cnt = 0;
+  pxmc_dbg_hist = NULL;
+ #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
+  disable_irq(ROCON_RX_IRQn);
+ #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
+  __memory_barrier();
+
   pxmc_state_t *mcs = &mcs0.base;
   lpc_qei_state_t *qst = &lpc_qei_state;
 
@@ -2470,13 +2648,14 @@ int pxmc_initialize(void)
   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 
-  pxmc_rocon_pwm_master_init();
+  res = pxmc_rocon_pwm_master_init();
+  if (res < 0)
+    return -1;
+
  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
   pxmc_rocon_rx_done_isr_setup(pxmc_rocon_rx_done_isr);
  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
 
-  pxmc_main_list.pxml_cnt = 0;
-  pxmc_dbg_hist = NULL;
   __memory_barrier();
   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;