#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "DSP281x_SWPrioritizedIsrLevels.h"   // DSP281x Examples Include File
#include "DSP281x_Device.h"     // DSP281x Headerfile Include File
#include "IQmathLib.h"

#include <params_bsu.h>
#include <v_rotor.h>

#include "filter_v1.h"
#include "xp_cds_in.h"
#include "xp_inc_sensor.h"
#include "xp_project.h"






#pragma DATA_SECTION(WRotor,".fast_vars");
WRotorValues WRotor = WRotorValues_DEFAULTS;

#pragma DATA_SECTION(WRotorPBus,".fast_vars");
WRotorValuesAngle WRotorPBus = WRotorValuesAngle_DEFAULTS;


#pragma DATA_SECTION(rotor_error_update_count,".fast_vars");
unsigned int rotor_error_update_count = 0;


#define SIZE_BUF_SENSOR_LOGS	32
#pragma DATA_SECTION(sensor_1_zero,".slow_vars");
unsigned int sensor_1_zero[6+4+8][SIZE_BUF_SENSOR_LOGS], count_sensor_1_zero=0;


void rotorInit(void)
{
	WRotorPBus.ModeAutoDiscret = 1;
}


#pragma CODE_SECTION(update_rot_sensors,".fast_run");
void update_rot_sensors(void)
{
	inc_sensor.update_sensors(&inc_sensor);
}


#define MAX_COUNT_OVERFULL_DISCRET	2250
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////

#define LEVEL_VALUE_SENSOR_OVERFULL	65535
#define MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS	4000

#pragma CODE_SECTION(AnalisatorRotorSensorPBus,".fast_run");
int AnalisatorRotorSensorPBus(_iq d1, _iq d2, unsigned int *count_overfull_discret, unsigned int *count_zero_discret, _iq *prev_iqTimeRotor,
								unsigned int *discret_out, unsigned int discret_in, _iq *iqWRotorCalcBeforeRegul, _iq *iqWRotorCalc, 
								int *RotorDirection, 
								int modeS1, int modeS2,
								int valid_sensor_direct, int valid_sensor_90,
								unsigned int *error_count  )
{
	int flag_not_ready_rotor, flag_overfull_rotor;
	_iq iqTimeRotor;
    static _iq koefW = _IQ(0.05);//0.05
	// discret0 = 2 mks
//   static long long KoefNorm_discret0 =     409600000LL;//((500 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
   static long long KoefNorm_discret0 =     102400000LL;//((500 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
	// discret1 = 20 ns
//    static long long KoefNorm_discret1 =  40960000000LL;//((50 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
    static long long KoefNorm_discret1 =  10240000000LL;//((50 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129

//	_iq iqWRotorSumm;//,iqWRotorCalc;

	static _iq time_level_discret_1to0	= 60000 ;//682666; // KoefNorm_discret1/60000 = 0.813801288604736328125 Ãö.
	static _iq time_level_discret_0to1	= 400;//204800; // KoefNorm_discret0/2000  = 0.244140625 Ãö.
	static unsigned int discret;

	
	if (valid_sensor_direct == 0)
		d1 = 0;
	if (valid_sensor_90 == 0)
		d2 = 0;


// òóò ÷òî-òî ïîøëî íå òàê, áûëà ñìåíà äèñêðåòèçàöèè ïî îáîèì êàíàëàì.
	if (valid_sensor_direct == 0 && valid_sensor_90 == 0)
	{
	  if (*error_count<MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS)
	  {
	  	(*error_count)++;
		return 0;
	  }
	  else
	    return 1; // îøèáêà!!! è èõ ìíîãî == MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS!!!
	}


	if (valid_sensor_direct == 1 && valid_sensor_90 == 0)
	{
		modeS2 = modeS1;
	}

	if (valid_sensor_direct == 0 && valid_sensor_90 == 1)
	{
		modeS1 = modeS2;		
	}

	if (modeS1 == modeS2)
	{
		discret = modeS1;
		*error_count = 0;	
	}
	else
	{
	  discret = 0;
	  if (*error_count<MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS)
	  {
	  	(*error_count)++;
		return 0;
	  }
	  else
	    return 1; // îøèáêà!!! è èõ ìíîãî == MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS!!!
	}

// òóò óæå îøèáîê íåò, ïîýòîìó îáíóëßåì
	*error_count = 0;


	flag_not_ready_rotor = 0;
	flag_overfull_rotor  = 0;

	if (d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0 && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
	{
		// îáà ïîêàçûâàþò

	}
	else
	if (d1 == 0 && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
	{
		// d1 - âûïàë, d2 ÷åòî ïîêàçûâàåò
		d1 = d2;
	}
	else
	if (d1 == LEVEL_VALUE_SENSOR_OVERFULL && d2 != LEVEL_VALUE_SENSOR_OVERFULL && d2 != 0)
	{
		// d1 - âûïàë, d2 ÷åòî ïîêàçûâàåò
		d1 = d2;
	}
	else
	if (d2 == LEVEL_VALUE_SENSOR_OVERFULL && d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0)
	{
		// d2 - âûïàë, d1 ÷åòî ïîêàçûâàåò
		d2 = d1;
	}
	else
	if (d2 == 0 && d1 != LEVEL_VALUE_SENSOR_OVERFULL && d1 != 0)
	{
		// d2 - âûïàë, d1 ÷åòî ïîêàçûâàåò
		d2 = d1;
	}
	else
	if (d1 == 0 && d2 == 0)
	{
		flag_not_ready_rotor = 1;
	}
	else
	if (d2 == LEVEL_VALUE_SENSOR_OVERFULL && d1 == LEVEL_VALUE_SENSOR_OVERFULL)
	{
		flag_overfull_rotor = 1;
		d1 = d2 = 0;
	}
	else
	if (d2 == LEVEL_VALUE_SENSOR_OVERFULL && d1 == 0)
	{
		flag_overfull_rotor = 1;
		d1 = d2 = 0;
	}
	else
	if (d1 == LEVEL_VALUE_SENSOR_OVERFULL && d2 == 0)
	{
		flag_overfull_rotor = 1;
		d1 = d2 = 0;
	}
	
	iqTimeRotor =  (d1+d2)>>1;



// max OVERFULL
	if (flag_overfull_rotor)
	{
		if (*count_overfull_discret<MAX_COUNT_OVERFULL_DISCRET)
			(*count_overfull_discret)++;
	}
	else
	{
		if (*count_overfull_discret>0)
			(*count_overfull_discret)--;
	}

// zero?
	if (flag_not_ready_rotor)
	{
		if (*count_zero_discret<MAX_COUNT_OVERFULL_DISCRET)
			(*count_zero_discret)++;
	}
	else
	{
		if (*count_zero_discret>0)
			(*count_zero_discret)--;
	}

// real zero?
	if (*count_zero_discret==MAX_COUNT_OVERFULL_DISCRET)
	{
	  // íîëü áûë ñëèøêîì äîëãî, çíà÷èò òî÷íî íîëü!
		iqWRotorCalc = 0;
		*prev_iqTimeRotor = 0;
		iqTimeRotor = 0;
	}
	else
	{
	  // íîëü åùå íå ñëèøêîì äîëãî, çíà÷èò áåðåì ñòàðîå çíà÷åíèå prev_iqTimeRotor
	  if (iqTimeRotor==0)
		iqTimeRotor = *prev_iqTimeRotor;
	}
	*prev_iqTimeRotor = iqTimeRotor;



// âûáîð íóæíîãî äèàïàçîíà
	if (WRotorPBus.ModeAutoDiscret==1)
	{
	 if (  (*count_overfull_discret==MAX_COUNT_OVERFULL_DISCRET) || (iqTimeRotor==0)  )
	 {
	// òóò èëè ïåðåïîëíåíèå ïðîèçîøëî èëè âäðóã îñòàíîâèëèñü, îáîðîòû=0
	// òîãäà âêëþ÷àåì discret_out = 0
	   if (discret_in == 1) // èëè òóò íàäî èñïëüçîâàòü discret?
	   {
	     // discret áûë =1, ïåðåêëþ÷àåì íà 0.
 	     *discret_out = 0;
		 *count_overfull_discret = 0; // äàëè åùå îäèí øàíñ!
	   }
	     
	 }
	 else
	 {
	 // òåêóù. óðîâåíü discret==0 òîãäà...
	   if (discret==0 && iqTimeRotor<time_level_discret_0to1 && iqTimeRotor!=65535)
	     *discret_out = 1;

	 // òåêóù. óðîâåíü discret==1 òîãäà...
	   if (discret==1 && iqTimeRotor>time_level_discret_1to0 && iqTimeRotor!=65535)
	     *discret_out = 0;
	 }
	}

	if (WRotorPBus.ModeAutoDiscret==2)
	{
	   *discret_out = 0;
	}

	if (WRotorPBus.ModeAutoDiscret==3)
	{
	   *discret_out = 1;
	}

	if (  (*count_overfull_discret==MAX_COUNT_OVERFULL_DISCRET) )
	{
	   // òóò óæå òî÷íî â 0, ò.ê. ñëèøêîì ìåäëåííî èäóò èìïóëüñû!
		*prev_iqTimeRotor = iqTimeRotor = 0;
	}




    if ((iqTimeRotor != 0))  // && (WRotorPBus.iqTimeRotor<65535)
    {
	    if (discret==0)
          *iqWRotorCalcBeforeRegul = KoefNorm_discret0 / iqTimeRotor;
	    if (discret==1)
          *iqWRotorCalcBeforeRegul = KoefNorm_discret1 / iqTimeRotor;

        *iqWRotorCalc = exp_regul_iq(koefW, *iqWRotorCalc, *iqWRotorCalcBeforeRegul);
    }
    else
    {
        *iqWRotorCalc = 0;
		*iqWRotorCalcBeforeRegul = 0;
    }


    if (*iqWRotorCalc == 0)
        *RotorDirection = 0;


	return 0;

}
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////

	
#pragma CODE_SECTION(RotorMeasurePBus,".fast_run");
void RotorMeasurePBus(void)
{    
	// discret0 = 2 mks
//   static long long KoefNorm_discret0 =     409600000LL;//((500 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
   static long long KoefNorm_discret0 =     102400000LL;//((500 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
	// discret1 = 20 ns
//    static long long KoefNorm_discret1 =  40960000000LL;//((50 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
    static long long KoefNorm_discret1 =  10240000000LL;//((50 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
 
	static _iq time_level_discret_1to0	= 60000 ;//682666; // KoefNorm_discret1/60000 = 0.813801288604736328125 Ãö.
	static _iq time_level_discret_0to1	= 400;//204800; // KoefNorm_discret0/2000  = 0.244140625 Ãö.
		
    static long long KoefNorm_angle = 16384LL;    //2^24/1024
    volatile float MyVar0 = 0;

    unsigned int MyVar3 = 0;
    int direction1 = 0, direction2 = 0;
	volatile unsigned int discret;
	
	static unsigned int discret_out1, discret_out2;

	static int count_full_oborots = 0;
	static unsigned int count_overfull_discret1 = 0;
	static unsigned int count_zero_discret1 = 0;
	static unsigned int count_overfull_discret2 = 0;
	static unsigned int count_zero_discret2 = 0;

	static unsigned int count_discret_to_1 = 0;
	static unsigned int count_discret_to_0 = 0;

	static unsigned int c_error_pbus_1 = 0;
	static unsigned int c_error_pbus_2 = 0;


	static _iq prev_iqTimeRotor1 = 0, prev_iqTimeRotor2 = 0;

    _iq iqWRotorSumm = 0;
    _iq koefW = _IQ(0.05);//0.05

	int flag_not_ready_rotor1, flag_overfull_rotor1;
	int flag_not_ready_rotor2, flag_overfull_rotor2;

    //i_led1_on_off(1);



	flag_not_ready_rotor1   = 0;
    flag_overfull_rotor1    = 0;
    flag_not_ready_rotor2   = 0;
    flag_overfull_rotor2    = 0;

#if(C_cds_in_number>=1)
	project.cds_in[0].read_pbus(&project.cds_in[0]);
#endif

	discret = project.cds_in[0].read.sbus.enabled_channels.bit.discret;
	if (project.cds_in[0].read.sbus.enabled_channels.bit.discret != project.cds_in[0].write.sbus.enabled_channels.bit.discret)
	  discret = 2;

	sensor_1_zero[0][count_sensor_1_zero] = project.cds_in[0].read.pbus.Time_since_zero_point_S1;
	sensor_1_zero[1][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S1;
	sensor_1_zero[2][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S1;
	sensor_1_zero[3][count_sensor_1_zero] = project.cds_in[0].read.pbus.Time_since_zero_point_S2;
	sensor_1_zero[4][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S2;
	sensor_1_zero[5][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S2;

	sensor_1_zero[6][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS1_cnt;
	sensor_1_zero[7][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS1_cnt90;
	sensor_1_zero[8][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS2_cnt;
	sensor_1_zero[9][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS2_cnt90;

	sensor_1_zero[10][count_sensor_1_zero] = inc_sensor.data.Time1;
	sensor_1_zero[11][count_sensor_1_zero] = inc_sensor.data.Impulses1;
	sensor_1_zero[12][count_sensor_1_zero] = inc_sensor.data.CountZero1;
	sensor_1_zero[13][count_sensor_1_zero] = inc_sensor.data.CountOne1;

	sensor_1_zero[14][count_sensor_1_zero] = inc_sensor.data.Time2;
	sensor_1_zero[15][count_sensor_1_zero] = inc_sensor.data.Impulses2;
	sensor_1_zero[16][count_sensor_1_zero] = inc_sensor.data.CountZero2;
	sensor_1_zero[17][count_sensor_1_zero] = inc_sensor.data.CountOne2;

	count_sensor_1_zero++;
	if (count_sensor_1_zero>=SIZE_BUF_SENSOR_LOGS)
	{
	  count_sensor_1_zero = 0;
	  count_full_oborots++;
	  if (count_full_oborots>3)
	   count_full_oborots = 0;
	}
/*
	if (count_sensor_1_zero==904)
	{
		discret = 3;
	}
*/	
#if (ENABLE_ROTOR_SENSOR_1==1)
	WRotorPBus.iqWRotorRawAngle1F = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S1-32768;
	WRotorPBus.iqWRotorRawAngle1R = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S1-32768;
	WRotorPBus.iqAngle1F 		 = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle1F;
	WRotorPBus.iqAngle1R 		 = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle1R;
#else
    WRotorPBus.iqWRotorRawAngle1F = 0;
	WRotorPBus.iqWRotorRawAngle1R = 0;
    WRotorPBus.iqAngle1F 		  = 0;
	WRotorPBus.iqAngle1R 		  = 0;
#endif

#if (ENABLE_ROTOR_SENSOR_2==1)
	WRotorPBus.iqWRotorRawAngle2F = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S2-32768;
	WRotorPBus.iqWRotorRawAngle2R = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S2-32768;
	WRotorPBus.iqAngle2F 		 = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle2F;
	WRotorPBus.iqAngle2R 		 = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle2R;
#else
    WRotorPBus.iqWRotorRawAngle2F = 0;
	WRotorPBus.iqWRotorRawAngle2R = 0;
    WRotorPBus.iqAngle2F 		  = 0;
	WRotorPBus.iqAngle2R 		  = 0;    
#endif




#if (ENABLE_ROTOR_SENSOR_1==1)
    //**************************************************************************************************
    MyVar3 = project.cds_in[0].read.pbus.SpeedS1_cnt;

    if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotorPBus.iqWRotorRaw0 = MyVar3;
    }
    else
    {
        WRotorPBus.iqWRotorRaw0 = 0;
    }

    MyVar3 = project.cds_in[0].read.pbus.SpeedS1_cnt90;

    if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotorPBus.iqWRotorRaw1 = MyVar3;
    }
    else
    {
        WRotorPBus.iqWRotorRaw1 = 0;
    }
#else
   WRotorPBus.iqWRotorRaw0 = 0;
   WRotorPBus.iqWRotorRaw1 = 0;
#endif


#if (ENABLE_ROTOR_SENSOR_2==1)
    //***************************************************************************************************
    MyVar3 = project.cds_in[0].read.pbus.SpeedS2_cnt;

    if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotorPBus.iqWRotorRaw2 = MyVar3;
    }
    else
    {
        WRotorPBus.iqWRotorRaw2 = 0;
    }

    MyVar3 = project.cds_in[0].read.pbus.SpeedS2_cnt90;

    if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotorPBus.iqWRotorRaw3 = MyVar3;
    }
    else
    {
        WRotorPBus.iqWRotorRaw3 = 0;
    }
#else
   WRotorPBus.iqWRotorRaw2 = 0;
   WRotorPBus.iqWRotorRaw3 = 0;
#endif


#if (ENABLE_ROTOR_SENSOR_1==1)
//	if (project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_direct && project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_90 )
       AnalisatorRotorSensorPBus(WRotorPBus.iqWRotorRaw0, WRotorPBus.iqWRotorRaw1, &count_overfull_discret1, &count_zero_discret1, 
							  &prev_iqTimeRotor1, &discret_out1, project.cds_in[0].read.sbus.enabled_channels.bit.discret, 
							  &WRotorPBus.iqWRotorCalcBeforeRegul1, &WRotorPBus.iqWRotorCalc1,
							  &WRotorPBus.RotorDirection1, 
							  project.cds_in[0].read.pbus.direction_in.bit.mode_sensor1_direct,        project.cds_in[0].read.pbus.direction_in.bit.mode_sensor1_90,
							  project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_direct, project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_90,
							  &c_error_pbus_1 );
#endif

#if (ENABLE_ROTOR_SENSOR_2==1)
//	if (project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_direct && project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_90 )
       AnalisatorRotorSensorPBus(WRotorPBus.iqWRotorRaw2, WRotorPBus.iqWRotorRaw3, &count_overfull_discret2, &count_zero_discret2, 
							  &prev_iqTimeRotor2, &discret_out2, project.cds_in[0].read.sbus.enabled_channels.bit.discret, 
							  &WRotorPBus.iqWRotorCalcBeforeRegul2, &WRotorPBus.iqWRotorCalc2,
							  &WRotorPBus.RotorDirection2, 
							  project.cds_in[0].read.pbus.direction_in.bit.mode_sensor2_direct,        project.cds_in[0].read.pbus.direction_in.bit.mode_sensor2_90,
							  project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_direct, project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_90,
							  &c_error_pbus_2);
#endif

	if (discret_out1==1 || discret_out2==1)
	{
	  project.cds_in[0].write.sbus.enabled_channels.bit.discret = 1;
	  count_discret_to_1++;
	}
	else
	{
	  project.cds_in[0].write.sbus.enabled_channels.bit.discret = 0; 
	  count_discret_to_0++;
	}


}






#pragma CODE_SECTION(RotorMeasure,".fast_run");
void RotorMeasure(void)
{
    
    // 600 Khz clock on every edge
//    static long long KoefNorm = 53635601LL;//((600 000/6256/NORMA_WROTOR/2) * ((long)2 << 24)); //15 - NormaWRotor 782*8 = 6256
//    static long long KoefNormMS = 491520000LL;//((600 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
//    static long long KoefNormNS = 49152000000LL;//((60 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
    static long long KoefNormMS = 122880000LL;//((600 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
    static long long KoefNormNS = 12288000000LL;//((60 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129
    static long long KoefNormImpulses = 838860800000000LL;// (2^24 * 1000000000 / (Impulses(ns)) / NORMA_WROTOR 
	
    volatile float MyVar0 = 0;
//    volatile unsigned int MyVar1 = 0;
//    volatile unsigned int MyVar2 = 0;
    unsigned int MyVar3 = 0;
    int direction1 = 0, direction2 = 0;
    _iq koefW = _IQ(0.05);//0.05


	inc_sensor.read_sensors(&inc_sensor);

//    rotation_sensor.read_sensors(&rotation_sensor);
//    if(rotation_sensor.in_plane.read.regs.comand_reg.bit.update_registers)
//    {
//        rotor_error_update_count ++;
//    }

//    WRotor.RotorDirection = (rotation_sensor.in_plane.out.direction1 + rotation_sensor.in_plane.out.direction2) > 0 ? 1 :
//                      (rotation_sensor.in_plane.out.direction1 + rotation_sensor.in_plane.out.direction2) < 0 ? -1 :
//                      0;
//


  //  direction1 = rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir0 == 2 ? 1 :
    //            rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir0 == 1 ? -1 :
//                0;
//    direction2 = rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir1 == 2 ? 1 :
//                rotation_sensor.in_plane.cds_in->read.pbus.direction_in.bit.dir1 == 1 ? -1 :
//                0;
//    WRotor.RotorDirection = (direction1 + direction2) > 0 ? 1 :
//                            (direction1 + direction2) < 0 ? -1 :
 //                           0;




    //**************************************************************************************************
    // sensor 1
//    if((rotation_sensor.in_plane.out.CountOne1 <= 200)
//                            || rotation_sensor.in_plane.out.CountOne1 == 65535)
//    { rotation_sensor.in_plane.out.CountOne1 = 0; }
//    if((rotation_sensor.in_plane.out.CountZero1 <= 200)
//                    || rotation_sensor.in_plane.out.CountZero1 == 65535)
//    { rotation_sensor.in_plane.out.CountZero1 = 0; }

  if (inc_sensor.use_sensor1)
  {
    MyVar3 = inc_sensor.data.CountOne1;
//    MyVar3 = (unsigned long) rotation_sensor.in_plane.out.CountOne1;

    if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotor.iqWRotorRaw0 = MyVar3;
    //  WRotor.iqWRotorRaw0 = KoefNorm / MyVar3;
    //  MyVar0 =    9590 / MyVar0;//100000 / MyVar0;                // 100000 = 60MHz/Impulses, Balzam: Dents = 600, Impuls per dent = 1; Pr 162 : Dents = 782, Impuls per dent = 8
    //  WRotor.iqWRotorRaw0 = _IQ(MyVar0/NORMA_WROTOR);
    }
    else
    {
        WRotor.iqWRotorRaw0 = 0;
    }

//    MyVar3 = (unsigned long) rotation_sensor.in_plane.out.CountZero1;
    MyVar3 = inc_sensor.data.CountZero1;

    if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotor.iqWRotorRaw1 = MyVar3;
    }
    else
    {
        WRotor.iqWRotorRaw1 = 0;
    }
  }
  else
  {
	WRotor.iqWRotorRaw0 = 0;
	WRotor.iqWRotorRaw1 = 0;
  }
    //logpar.uns_log0 = (Uint16)(my_var1);
    //logpar.uns_log1 = (Uint16)(my_var2);

    //***************************************************************************************************
    // sensor 2
//    if((rotation_sensor.in_plane.out.CountOne2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2)
//                    || rotation_sensor.in_plane.out.CountOne2 == 65535)
//    { rotation_sensor.in_plane.out.CountOne2 = 0; }
//    if((rotation_sensor.in_plane.out.CountZero2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2)
//                    || rotation_sensor.in_plane.out.CountZero2 == 65535)
//    { rotation_sensor.in_plane.out.CountZero2 = 0; }

  if (inc_sensor.use_sensor2)
  {
    MyVar3 = inc_sensor.data.CountOne2;

    if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotor.iqWRotorRaw2 = MyVar3;
    }
    else
    {
        WRotor.iqWRotorRaw2 = 0;
    }

    MyVar3 = inc_sensor.data.CountZero2;

    if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) 
		&& (MyVar3 > COUNT_DECODER_MAX_WROTOR))
    {
        WRotor.iqWRotorRaw3 = MyVar3;
    }
    else
    {
        WRotor.iqWRotorRaw3 = 0;
    }
//  i_led1_on_off(0);
   }
   else
   {
	WRotor.iqWRotorRaw2 = 0;
	WRotor.iqWRotorRaw3 = 0;
   }



	WRotor.iqTimeSensor1 = WRotor.iqWRotorRaw0 + WRotor.iqWRotorRaw1;
	WRotor.iqTimeSensor2 = WRotor.iqWRotorRaw2 + WRotor.iqWRotorRaw3;


    if (WRotor.iqTimeSensor1 != 0 && inc_sensor.use_sensor1)
    {
		if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time1==0)
          WRotor.iqWRotorCalcBeforeRegul1 = KoefNormMS / WRotor.iqTimeSensor1;
		if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time1==1)
          WRotor.iqWRotorCalcBeforeRegul1 = KoefNormNS / WRotor.iqTimeSensor1;

        WRotor.iqWRotorCalc1 = exp_regul_iq(koefW, WRotor.iqWRotorCalc1, WRotor.iqWRotorCalcBeforeRegul1);
    }
    else
    {
        WRotor.iqWRotorCalc1 = 0;
		WRotor.iqWRotorCalcBeforeRegul1 = 0;
    }

    if (WRotor.iqTimeSensor2 != 0 && inc_sensor.use_sensor2)
    {
		if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time2==0)
          WRotor.iqWRotorCalcBeforeRegul2 = KoefNormMS / WRotor.iqTimeSensor2;
		if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time2==1)
          WRotor.iqWRotorCalcBeforeRegul2 = KoefNormNS / WRotor.iqTimeSensor2;
        WRotor.iqWRotorCalc2 = exp_regul_iq(koefW, WRotor.iqWRotorCalc2, WRotor.iqWRotorCalcBeforeRegul2);
    }
    else
    {
        WRotor.iqWRotorCalc2 = 0;
		WRotor.iqWRotorCalcBeforeRegul2 = 0;
    }

	if (inc_sensor.data.TimeCalcFromImpulses1 && inc_sensor.use_sensor1)
	  WRotor.iqWRotorImpulses1 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses1 * ROTOR_SENSOR_IMPULSES_PER_ROTATE);
	else
	  WRotor.iqWRotorImpulses1 = 0;

	if (inc_sensor.data.TimeCalcFromImpulses2 && inc_sensor.use_sensor2)
	  WRotor.iqWRotorImpulses2 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses2 * ROTOR_SENSOR_IMPULSES_PER_ROTATE);
	else
	  WRotor.iqWRotorImpulses2 = 0;



  //  WRotor.iqWRotorCalcBeforeRegul = _IQdiv(WRotor.iqWRotorCalcBeforeRegul,IQ_CONST_3);
//  i_led1_on_off(0);
    if (WRotor.iqWRotorCalc1 == 0 && inc_sensor.use_sensor1)
        WRotor.RotorDirection1 = 0;
    if (WRotor.iqWRotorCalc2 == 0 && inc_sensor.use_sensor2)
        WRotor.RotorDirection2 = 0;
    //wrotor.iq_wrotor_calc = 0;
    //i_led2_on_off(0);

}