RoCoN: copy of PXMC basic hard-home routine adapted for FPGA based system.

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 3f6a2cd47a154804e90c3a3737b62b96cbcec26e..4572667cee4dc1792f0ac79040d9988dc1befaac 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -22,6 +22,7 @@
 #include <pxmc_lpc_qei.h>
 #include <pxmc_internal.h>
 #include <pxmc_inp_common.h>
+#include <pxmc_gen_info.h>
 #include <hal_gpio.h>
 #include <hal_machperiph.h>
 #include <stdlib.h>
@@ -41,7 +42,7 @@
 
 unsigned pxmc_lpcpwm1_magnitude;
 
-int pxmc_hh_gi(pxmc_state_t *mcs);
+long pxmc_irc_offset[PXML_MAIN_CNT];
 
 int
 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
@@ -52,6 +53,7 @@ pxmc_inp_rocon_inp(struct pxmc_state *mcs)
 
   /* read position from hardware */
   irc = fpga_irc[chan]->count;
+  irc += pxmc_irc_offset[chan];
   pos = irc << PXMC_SUBDIV(mcs);
   mcs->pxms_as = pos - mcs->pxms_ap;
   mcs->pxms_ap = pos;
@@ -66,10 +68,13 @@ pxmc_inp_rocon_inp(struct pxmc_state *mcs)
 int
 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
 {
-  /* int chan=mcs->pxms_inp_info */
-  /* Optional set of the actual position to the HW */
-  /* We do not want to change real HW counter at any situation */
-  /* It can be used only to set some software maintained offset */
+  int chan=mcs->pxms_inp_info;
+  long irc;
+  long pos_diff;
+
+  irc = fpga_irc[chan]->count;
+  pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
+  pxmc_irc_offset[chan] = pos_diff >> PXMC_SUBDIV(mcs);
   return 0;
 }
 
@@ -358,10 +363,81 @@ pxmc_rocon_pwm_init(pxmc_state_t *mcs, int mode)
   return 0;
 }
 
+int
+pxmc_fpga_is_mark(pxmc_state_t *mcs)
+{
+  int chan = mcs->pxms_out_info;
+
+  return *fpga_irc_state[chan] & 1;
+}
+
+int pxmc_fpga_hh_gd10(pxmc_state_t *mcs);
+int pxmc_fpga_hh_gd20(pxmc_state_t *mcs);
+
+int
+pxmc_fpga_hh_gi(pxmc_state_t *mcs)
+{
+  long spd;
+  pxmc_set_flag(mcs,PXMS_BSY_b);
+  mcs->pxms_rsfg = 0;
+  spd = mcs->pxms_ms;
+  spd >>= mcs->pxms_cfg&PXMS_CFG_HSPD_m;
+  if(!spd) spd = 1;
+  if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
+    spd = -spd;
+  mcs->pxms_gen_hsp = spd;
+  mcs->pxms_do_gen = pxmc_fpga_hh_gd10;
+  return pxmc_fpga_hh_gd10(mcs);
+}
+
+int
+pxmc_fpga_hh_gd10(pxmc_state_t *mcs)
+{
+  long spd;
+  if(mcs->pxms_flg & PXMS_ERR_m)
+    return pxmc_spdnext_gend(mcs);
+
+  pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
+  mcs->pxms_rp += mcs->pxms_rs;
+
+  if (!pxmc_fpga_is_mark(mcs)){
+    spd=mcs->pxms_gen_hsp;
+    if (spd > 0){
+      spd >>= 2;
+      spd = spd? -spd: -1;
+    } else {
+      spd >>= 2;
+      spd= spd? -spd: 1;
+    }
+    mcs->pxms_gen_hsp = spd;
+    mcs->pxms_do_gen = pxmc_fpga_hh_gd20;
+  }
+
+  return 0;
+}
+
+int
+pxmc_fpga_hh_gd20(pxmc_state_t *mcs)
+{
+  if (mcs->pxms_flg & PXMS_ERR_m)
+    return pxmc_spdnext_gend(mcs);
+
+  pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
+  mcs->pxms_rp += mcs->pxms_rs;
+
+  if (pxmc_fpga_is_mark(mcs)){
+    /* pxmc_axis_set_pos(mcs, 0); */
+    if (mcs->pxms_do_ap2hw != NULL)
+      mcs->pxms_do_ap2hw(mcs);
+    mcs->pxms_do_gen = pxmc_stop_gi;
+  }
+
+  return 0;
+}
+
 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 {
-  /*return pxmc_hh_gi;*/
-  return NULL;
+  return pxmc_fpga_hh_gi;
 }
 
 pxmc_state_t mcs0 =