RoCoN: include optional support for BLDC/PMSM modified to run without HAL.

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 905799d4a973f14c948ab910ece12fb9b08824a5..bb86f8bb7f754f9dd697b79dd0c38ecd2ee87f46 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -30,6 +30,9 @@
 #include "appl_defs.h"
 #include "appl_fpga.h"
 
+int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
+                           unsigned long index_irc, int diff2err);
+
 #define PXML_MAIN_CNT 8
 
 #define PXMC_WITH_PT_ZIC 1
@@ -184,6 +187,22 @@ pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
   return 0;
 }
 
+int
+pxmc_inp_rocon_ptofs_from_index_poll(struct pxmc_state *mcs, int diff2err)
+{
+  int chan=mcs->pxms_inp_info;
+  long irc;
+  long index_irc;
+
+  if (!(*fpga_irc_state[chan] & (1 << 2)))
+    return 0;
+
+  irc = fpga_irc[chan]->count;
+  index_irc = fpga_irc[chan]->count_index;
+
+  return pxmc_ptofs_from_index(mcs, irc, index_irc, diff2err);
+}
+
 inline unsigned
 pxmc_rocon_bdc_hal_rd(pxmc_state_t *mcs)
 {
@@ -249,9 +268,9 @@ pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
   volatile uint32_t *pwm_reg_a, *pwm_reg_b, *pwm_reg_c;
   int chan = mcs->pxms_out_info;
 
-  pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
+  pwm_reg_c = pxmc_rocon_pwm_chan2reg(chan + 0);
   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
-  pwm_reg_c = pxmc_rocon_pwm_chan2reg(chan + 2);
+  pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 2);
 
   *pwm_reg_a = pwm1;
   *pwm_reg_b = pwm2;
@@ -293,6 +312,21 @@ pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
     short ptirc = mcs->pxms_ptirc;
     short divisor = mcs->pxms_ptper * 6;
 
+   #if 0
+    pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
+   #elif 1
+    {
+      int res;
+      res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
+      if (res < 0) {
+        pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
+      } else if (res) {
+        pxmc_set_flag(mcs, PXMS_PTI_b);
+        pxmc_set_flag(mcs, PXMS_PHA_b);
+      }
+    }
+   #else
+
     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bdc_hal_rd(mcs)];
 
     if (hal_pos == 0xff)
@@ -334,9 +368,9 @@ pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
           pxmc_clear_flag(mcs, PXMS_PRA_b);
 
           /* if phase table position to mask is know do fine phase table alignment */
-          if (mcs->pxms_cfg & PXMS_CFG_I2PT_m)
-            lpc_qei_setup_index_catch(&lpc_qei_state);
-
+          if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) {
+            /*pxmc_inp_rocon_is_index_edge(mcs);*/
+          }
         }
         else
         {
@@ -347,6 +381,7 @@ pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
 
       mcs->pxms_hal = hal_pos;
     }
+   #endif
   }
 
   {
@@ -355,10 +390,13 @@ pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
   }
 
+ #if 1
   ene = mcs->pxms_ene;
+ #else
+  ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
+ #endif
 
-  if (ene)
-  {
+  if (ene) {
     indx = mcs->pxms_ptindx;
 #if 1
     /* tuning of magnetic field/voltage advance angle */
@@ -502,11 +540,13 @@ pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
   }
 
+ #if 1
   ene = mcs->pxms_ene;
-  //if (ene) { /*FIXME*/
-  if (mcs->pxms_flg&PXMS_BSY_m){
-    ene = mcs->pxms_me; /*FIXME*/
+ #else
+  ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
+ #endif
 
+  if (ene) {
     indx = mcs->pxms_ptindx;
    #if 1
     /* tuning of magnetic field/voltage advance angle */
@@ -597,6 +637,47 @@ pxmc_rocon_pwm_master_init(void)
   return 0;
 }
 
+int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
+                           unsigned long index_irc, int diff2err)
+{
+  long ofsl;
+  short ofs;
+
+  ofs = ofsl = index_irc - mcs->pxms_ptmark;
+
+  if (diff2err) {
+    short diff;
+    diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
+    if (diff >= mcs->pxms_ptirc / 2)
+      diff -= mcs->pxms_ptirc;
+    if (diff <= -mcs->pxms_ptirc / 2)
+      diff += mcs->pxms_ptirc;
+    if (diff < 0)
+      diff = -diff;
+    if(diff >= mcs->pxms_ptirc / 6) {
+      return -1;
+    }
+  } else {
+    long diff;
+    diff = (unsigned long)ofsl - irc;
+    ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
+  }
+
+  mcs->pxms_ptofs = ofs;
+
+  return 1;
+}
+
+/**
+ * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
+ * @mcs:        Motion controller state information
+ */
+int
+pxmc_dummy_con(pxmc_state_t *mcs)
+{
+  return 0;
+}
+
 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);
@@ -764,9 +845,9 @@ pxms_flg:
 pxms_do_inp:
   pxmc_inp_rocon_inp,
 pxms_do_con:
-  pxmc_pid_con,
+  pxmc_pid_con /*pxmc_dummy_con*/,
 pxms_do_out:
-  pxmc_rocon_pwm_dc_out,
+  pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_out*/,
   pxms_do_deb: 0,
   pxms_do_gen: 0,
 pxms_do_ap2hw:
@@ -785,6 +866,7 @@ pxms_cfg:
 
   pxms_ptper: 1,
   pxms_ptirc: 1000,
+  pxms_ptmark: 1180,
   /*pxms_ptamp: 0x7fff,*/
 
   pxms_hal: 0x40,
@@ -1343,12 +1425,18 @@ int pxmc_done(void)
 int pxmc_initialize(void)
 {
   int res;
+  int i;
 
   pxmc_state_t *mcs = &mcs0;
   lpc_qei_state_t *qst = &lpc_qei_state;
 
   *fpga_irc_reset = 1;
 
+  for (i = 0; i < 8; i++) {
+    fpga_irc[i]->count = 0;
+    *fpga_irc_state[i] = 1 << 2;
+  }
+
   /* Initialize QEI module for IRC counting */
   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);