SW repair for 'IRC malfunction'

[fpga/rpi-motor-control.git] / pmsm-control / test_sw / comp.c

diff --git a/pmsm-control/test_sw/comp.c b/pmsm-control/test_sw/comp.c
index 764726acc4ae46c09a1dc2848e1447517988a009..94274815f08a2c2e345fddb7b7dc379eb29f8faf 100644 (file)
--- a/pmsm-control/test_sw/comp.c
+++ b/pmsm-control/test_sw/comp.c
@@ -8,6 +8,18 @@
 #include "pmsm_state.h"
 #include "rp_spi.h"
 
+#include <stdio.h>
+#include <stdlib.h>
+
+int transitions[6][6]={
+                    {  0,  0,881,  0,730,  0},
+                    {  0,  0, 50,  0,  0,230},
+                    {881, 50,  0,  0,  0,  0},
+                    {  0,  0,  0,  0,550,380},
+                    {730,  0,  0,550,  0,  0},
+                    {  0,230,  0,380,  0,  0}
+                   };
+
 /**
  * \brief
  * Substact initial position.
@@ -32,6 +44,41 @@ uint16_t mult_cap(int32_t s,int d){
        return res;
 }
 
+/**
+ * \brief
+ * Attempt to resolve fail-state in case of irc cable malfunction.
+ *
+ * Pri prechodu mezi haly je pocitan rozdil aktualni a predpokladane
+ * vzdalenosti od indexu. Pokud prekroci MAX_DIFF, je prepnuto na
+ * komutaci jen pomoci halu.
+ */
+#define MAX_DIFF 90
+void repairPositionErr(struct rpi_state* this){
+       int new;
+       int old;
+       int trans;
+       /* doslo k prechodu mezi haly */
+       if (this->h1_old != this->spi_dat->hal1 ||
+           this->h2_old != this->spi_dat->hal2 ||
+           this->h3_old != this->spi_dat->hal3)
+       {
+               new=this->spi_dat->hal1*4+this->spi_dat->hal2*2+this->spi_dat->hal3;
+               old=this->h1_old*4+this->h2_old*2+this->h3_old;
+               trans = transitions[new-1][old-1]; /*predpokladana pozice */
+
+               /*pozice jsou v rozsahu 0-999*/
+               if (abs(this->index_dist-trans)>MAX_DIFF &&
+                   abs(this->index_dist-trans-1000)>MAX_DIFF){
+                       setIndexLost(this);
+               }
+
+               /*nakonec preulozme stare hodnoty*/
+               this->h1_old = this->spi_dat->hal1;
+               this->h2_old = this->spi_dat->hal2;
+               this->h3_old = this->spi_dat->hal3;
+       }
+
+}
 
 /**
  * \brief
@@ -89,6 +136,9 @@ void comIndDist(struct rpi_state* this){
        }
 
        this->index_dist = dist;
+
+       repairPositionErr(this);
+
        return;
 
        index_lost: