From 25daf80493da75ffb9993b09ba6b5ca1e785078f Mon Sep 17 00:00:00 2001
From: Pavel Pisa <ppisa@pikron.com>
Date: Sun, 1 Jun 2014 14:19:41 +0200
Subject: [PATCH] RoCoN: enhance stepper and BLDC motor support to allow build
 for calibration.

Signed-off-by: Pavel Pisa <ppisa@pikron.com>
---
 sw/app/rocon/appl_pxmc.c | 56 ++++++++++++++++++++++++++++------------
 1 file changed, 40 insertions(+), 16 deletions(-)

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index d9a8408..31de21e 100644
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -305,6 +305,8 @@ pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 int
 pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
 {
+  typeof(mcs->pxms_ptvang) ptvang;
+  int sync_mode = 0;
   unsigned char hal_pos;
   short pwm1;
   short pwm2;
@@ -322,6 +324,7 @@ pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
 
    #if 0
     pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
+    sync_mode = 1;
    #elif 1
     {
       int res;
@@ -398,11 +401,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 (!sync_mode) {
+    ptvang = mcs->pxms_ptvang;
+    ene = mcs->pxms_ene;
+  } else {
+    ptvang = 0;
+    ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
+  }
 
   if (ene) {
     indx = mcs->pxms_ptindx;
@@ -416,13 +421,13 @@ pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
       /* Generating direction of stator mag. field for backward torque */
       ene = -ene;
 
-      if ((indx -= mcs->pxms_ptvang) < 0)
+      if ((indx -= ptvang) < 0)
         indx += mcs->pxms_ptirc;
     }
     else
     {
       /* Generating direction of stator mag. field for forward torque */
-      if ((indx += mcs->pxms_ptvang) >= mcs->pxms_ptirc)
+      if ((indx += ptvang) >= mcs->pxms_ptirc)
         indx -= mcs->pxms_ptirc;
     }
 
@@ -508,6 +513,8 @@ pxmc_rocon_pwm2ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2)
 int
 pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
 {
+  typeof(mcs->pxms_ptvang) ptvang;
+  int sync_mode = 0;
   short pwm1;
   short pwm2;
   int indx = 0;
@@ -519,7 +526,21 @@ pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
     short ptindx;
     short ptirc=mcs->pxms_ptirc;
 
-    pxmc_irc_16bit_commindx(mcs,mcs->pxms_rp>>PXMC_SUBDIV(mcs));
+   #if 0
+    pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
+    sync_mode = 1;
+   #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
 
     ptindx = mcs->pxms_ptindx;
 
@@ -540,6 +561,7 @@ pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
       if(!(mcs->pxms_flg&PXMS_PTI_m))
             mcs->pxms_ptindx = ptindx;
     }
+   #endif
   }
 
   {
@@ -548,26 +570,28 @@ pxmc_rocon_pwm2ph_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 (!sync_mode) {
+    ptvang = mcs->pxms_ptvang;
+    ene = mcs->pxms_ene;
+  } else {
+    ptvang = 0;
+    ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
+  }
 
   if (ene) {
     indx = mcs->pxms_ptindx;
-   #if 1
+   #if 0
     /* tuning of magnetic field/voltage advance angle */
     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
    #endif
     if (ene<0){
       /* Generating direction of stator mag. field for backward torque */
       ene = -ene;
-      if ((indx -= mcs->pxms_ptvang)<0)
+      if ((indx -= ptvang)<0)
         indx += mcs->pxms_ptirc;
     }else{
       /* Generating direction of stator mag. field for forward torque */
-      if ((indx += mcs->pxms_ptvang) >= mcs->pxms_ptirc)
+      if ((indx += ptvang) >= mcs->pxms_ptirc)
         indx -= mcs->pxms_ptirc;
     }
 
-- 
2.39.2