RoCoN: complete MARS-8 compatible hard-home support.

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 f0df3841d83f32a894ebea1d878269d7fd094908..9c227710adfad6e2e4723a4ed52919fc5327177e 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -42,7 +42,43 @@
 
 unsigned pxmc_lpcpwm1_magnitude;
 
-long pxmc_irc_offset[PXML_MAIN_CNT];
+long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
+unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
+
+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,
+  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,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  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,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  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,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+  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,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+  2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+  3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+  4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+  4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+  5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,6,7,7,8,7,8,8,9,7,8,8,9,8,9,9,10
+};
 
 int
 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
@@ -53,7 +89,7 @@ pxmc_inp_rocon_inp(struct pxmc_state *mcs)
 
   /* read position from hardware */
   irc = fpga_irc[chan]->count;
-  irc += pxmc_irc_offset[chan];
+  irc += pxmc_rocon_irc_offset[chan];
   pos = irc << PXMC_SUBDIV(mcs);
   mcs->pxms_as = pos - mcs->pxms_ap;
   mcs->pxms_ap = pos;
@@ -65,6 +101,76 @@ pxmc_inp_rocon_inp(struct pxmc_state *mcs)
   return 0;
 }
 
+int
+pxmc_inp_rocon_is_mark(pxmc_state_t *mcs)
+{
+  uint32_t irc_state;
+  int mark;
+  unsigned filt;
+  int chan=mcs->pxms_inp_info;
+
+  irc_state = *fpga_irc_state[chan];
+
+  mark = (irc_state ^ (mcs->pxms_cfg >> PXMS_CFG_HPS_b)) & 1;
+
+  filt = pxmc_rocon_mark_filt[chan];
+  filt = (filt << 1) | mark;
+  pxmc_rocon_mark_filt[chan] = filt;
+
+  return onesin10bits[filt & 0x03ff];
+}
+
+int
+pxmc_inp_rocon_is_index_edge(pxmc_state_t *mcs)
+{
+  uint32_t irc_state;
+  int index;
+  int chan=mcs->pxms_inp_info;
+
+  irc_state = *fpga_irc_state[chan];
+  *fpga_irc_state[chan] = 1 << 2;
+
+  index = (irc_state >> 2) & 1;
+
+  return index;
+}
+
+int
+pxmc_inp_rocon_adjust_to_irc_change(struct pxmc_state *mcs, long irc_diff)
+{
+  long pos_diff = irc_diff << PXMC_SUBDIV(mcs);
+
+  mcs->pxms_ap += pos_diff;
+  mcs->pxms_rp += pos_diff;
+  return 0;
+}
+
+int
+pxmc_inp_rocon_irc_offset_from_index(struct pxmc_state *mcs)
+{
+  int chan=mcs->pxms_inp_info;
+  long irc_offset;
+  long irc_diff;
+
+  irc_offset = -fpga_irc[chan]->count_index;
+  irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
+  pxmc_rocon_irc_offset[chan] = irc_offset;
+  return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
+}
+
+int
+pxmc_inp_rocon_ap_zero(struct pxmc_state *mcs)
+{
+  int chan=mcs->pxms_inp_info;
+  long irc_offset;
+  long irc_diff;
+
+  irc_offset = -fpga_irc[chan]->count;
+  irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
+  pxmc_rocon_irc_offset[chan] = irc_offset;
+  return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
+}
+
 int
 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
 {
@@ -74,7 +180,7 @@ pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
 
   irc = fpga_irc[chan]->count;
   pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
-  pxmc_irc_offset[chan] = pos_diff >> PXMC_SUBDIV(mcs);
+  pxmc_rocon_irc_offset[chan] = pos_diff >> PXMC_SUBDIV(mcs);
   return 0;
 }
 
@@ -387,61 +493,146 @@ pxmc_rocon_pwm_master_init(void)
   return 0;
 }
 
-int
-pxmc_fpga_is_mark(pxmc_state_t *mcs)
-{
-  int chan=mcs->pxms_inp_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_rocon_hh_gd10(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
 
+/* initialize for hard home */
 int
-pxmc_fpga_hh_gi(pxmc_state_t *mcs)
+pxmc_rocon_hh_gi(pxmc_state_t *mcs)
 {
-  long spd;
   pxmc_set_flag(mcs,PXMS_BSY_b);
-  mcs->pxms_rsfg = 0;
+ #if 0 /* config and speed already set in pxmc_hh */
   spd = mcs->pxms_ms;
-  spd >>= mcs->pxms_cfg&PXMS_CFG_HSPD_m;
+  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);
+ #endif
+  mcs->pxms_rsfg = 0;
+  mcs->pxms_gen_htim = 16;
+  mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
+  return pxmc_rocon_hh_gd10(mcs);
 }
 
+/* fill initial mark filter and then decide */
 int
-pxmc_fpga_hh_gd10(pxmc_state_t *mcs)
+pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
 {
-  long spd;
+  int mark;
+
+  if(mcs->pxms_flg & PXMS_ERR_m)
+    return pxmc_spdnext_gend(mcs);
+
+  pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
+  mcs->pxms_rp += mcs->pxms_rs;
+
+  mark = pxmc_inp_rocon_is_mark(mcs);
+
+  if (mcs->pxms_gen_htim) {
+    mcs->pxms_gen_htim--;
+    return 0;
+  }
+
+  if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
+    /* limit switch is not used */
+    pxmc_inp_rocon_is_index_edge(mcs);
+    mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
+    return 0;
+  }
+
+  if (mark >= 6) {
+    /* go out from switch */
+    mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
+  } else {
+    /* switch is off -> look for it */
+    mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
+  }
+  return 0;
+}
+
+/* go out from switch */
+int
+pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
+{
+  int mark;
+
   if(mcs->pxms_flg & PXMS_ERR_m)
     return pxmc_spdnext_gend(mcs);
 
+  mark = pxmc_inp_rocon_is_mark(mcs);
+
+  if (mark <= 1) {
+    /* switch is off -> look for it again */
+    mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
+  }
+
+  pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
+  mcs->pxms_rp += mcs->pxms_rs;
+
+  return 0;
+}
+
+/* switch is off -> look for it */
+int
+pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
+{
+  long spd;
+  int mark;
+
+  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;
+  mark = pxmc_inp_rocon_is_mark(mcs);
+
+  if (mark >= 8){
+    spd = mcs->pxms_gen_hsp >> 2; /* slow down */
+    if (!spd)
+      spd = 1;
+    mcs->pxms_gen_hsp = spd;
+    mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
+  }
+
+  return 0;
+}
+
+/* switch is on -> find edge */
+int
+pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
+{
+  int mark;
+
+  if (mcs->pxms_flg & PXMS_ERR_m)
+    return pxmc_spdnext_gend(mcs);
+
+  mark = pxmc_inp_rocon_is_mark(mcs);
+
+  if (mark <= 1) {
+    if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
+      pxmc_inp_rocon_is_index_edge(mcs);
+      mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
     } else {
-      spd >>= 2;
-      spd= spd? -spd: 1;
+      pxmc_inp_rocon_ap_zero(mcs);
+      mcs->pxms_do_gen = pxmc_stop_gi;
     }
-    mcs->pxms_gen_hsp = spd;
-    mcs->pxms_do_gen = pxmc_fpga_hh_gd20;
   }
 
+  pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
+  mcs->pxms_rp += mcs->pxms_rs;
+
   return 0;
 }
 
+/* calibrate on revolution index */
 int
-pxmc_fpga_hh_gd20(pxmc_state_t *mcs)
+pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
 {
   if (mcs->pxms_flg & PXMS_ERR_m)
     return pxmc_spdnext_gend(mcs);
@@ -449,13 +640,8 @@ pxmc_fpga_hh_gd20(pxmc_state_t *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_rp = mcs->pxms_ap = 0;
-      mcs->pxms_do_ap2hw(mcs);
-      mcs->pxms_rp = mcs->pxms_ap = 0;
-    }
+  if (pxmc_inp_rocon_is_index_edge(mcs)){
+    pxmc_inp_rocon_irc_offset_from_index(mcs);
     mcs->pxms_do_gen = pxmc_stop_gi;
   }
 
@@ -464,7 +650,7 @@ pxmc_fpga_hh_gd20(pxmc_state_t *mcs)
 
 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 {
-  return pxmc_fpga_hh_gi;
+  return pxmc_rocon_hh_gi;
 }
 
 pxmc_state_t mcs0 =