#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"
#include "params.h"

//#include "pwm_test_lines.h"
#include "params_norma.h"
#include "edrk_main.h"
#include "params_pwm24.h"
#include "v_rotor_22220.h"

#include "rmp_cntl_v1.h"
#include "mathlib.h"


//#define _ENABLE_PWM_LED2_PROFILE    1

#if (_ENABLE_PWM_LED2_PROFILE)
extern unsigned int profile_pwm[30];
extern unsigned int pos_profile_pwm;
#endif

//
//#pragma DATA_SECTION(buf1,".logs2")
//int buf1[SIZE_BUF_W];
//#pragma DATA_SECTION(buf2,".logs2")
//int buf2[SIZE_BUF_W];
//#pragma DATA_SECTION(buf3,".logs2")
//int buf3[SIZE_BUF_W];
//#pragma DATA_SECTION(buf4,".logs2")
//int buf4[SIZE_BUF_W];
//#pragma DATA_SECTION(buf5,".logs2")
//int buf5[SIZE_BUF_W];
//#pragma DATA_SECTION(buf6,".logs2")
//int buf6[SIZE_BUF_W];
//#pragma DATA_SECTION(buf7,".logs2")
//int buf7[SIZE_BUF_W];
//#pragma DATA_SECTION(buf8,".logs2")
//int buf8[SIZE_BUF_W];
//

///////////////////////////////////////////////
///////////////////////////////////////////////
///////////////////////////////////////////////
RMP_V1 rmp_wrot = RMP_V1_DEFAULTS;

#pragma DATA_SECTION(rotor_22220,".fast_vars");
ROTOR_VALUE_22220 rotor_22220 = ROTOR_VALUE_22220_DEFAULTS;


void rotorInit_22220(void)
{
    unsigned int i = 0, size = 0, *pint = 0;

    rmp_wrot.RampLowLimit = 0;
    rmp_wrot.RampHighLimit = _IQ(1.0);
    rmp_wrot.RampPlus = _IQ(0.0015/NORMA_FROTOR);
    rmp_wrot.RampMinus = _IQ(-0.0015/NORMA_FROTOR);
    rmp_wrot.DesiredInput = 0;
    rmp_wrot.Out = 0;


//    pint = (unsigned int*)&rotor;
//    size = sizeof(rotor) / sizeof(unsigned int);
//    for(i = 0; i < size; i++)
//    {
//        *(pint + i) = 0;
//    }

}

_iq koef_Wout_filter = _IQ(0.2); //_IQ(0.15);
_iq koef_Wout_filter_long = _IQ(0.001);//_IQ(0.03);

#define SIZE_BUF_F1 10
#pragma DATA_SECTION(f1,".slow_vars")
static _iq f1[SIZE_BUF_F1]={0,0,0,0,0,0,0,0,0,0};
#pragma DATA_SECTION(f1_int,".slow_vars")
static long f1_int[SIZE_BUF_F1]={0,0,0,0,0,0,0,0,0,0};

#pragma CODE_SECTION(clear_iqFsensors,".fast_run");
void clear_iqFsensors(void)
{
    int i;

    for (i=0;i<SENSORS_NUMBER;i++)
        rotor_22220.iqFsensors[i] = 0;
}

//#pragma CODE_SECTION(Rotor_measure_22220,".fast_run");
void Rotor_measure_22220(void)
{
    static long c_s=-1;
//    static int flag_buf = 0, prev_flag_buf = 0 ;



    static unsigned long PrevAngle1 = 0, PrevAngle2 = 0, PrevAngle3 = 0, PrevAngle4 = 0;
    static unsigned int peroid_shim_mks = (unsigned int)(1000000L / FREQ_PWM / 2.0);
    static _iq prev_wrot = 0;
    static unsigned int prev_wrot_count = 0;
    static int direct_accum = 0;
    static int sens1_err_count = 0;
    static int sens2_err_count = 0;




    unsigned int s_number = 0, begin_data = 0, end_data = 0, i;
    unsigned int s_number2 = 0;
    unsigned int s_number3 = 0;
    _iq accumulator = 0, deltaF = 0, deltaAngle = 0;
    long sum_count=0;

    int direction1, direction2, sum_direct, sens_err1, sens_err2;

#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif

      clear_iqFsensors();

#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif

//    rotation_sensor.read_sensors(&rotation_sensor);
    inc_sensor.read_sensors(&inc_sensor);

#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif

    direction1 =  project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == ROTOR_SENSOR_CODE_CLOCKWISE ? 1 :
                  project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == ROTOR_SENSOR_CODE_COUNTERCLOCKWISE ? -1 :
                  0;

    direction2 =  project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == ROTOR_SENSOR_CODE_COUNTERCLOCKWISE ? 1 :
                  project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == ROTOR_SENSOR_CODE_CLOCKWISE ? -1 :
                  0;

    sens_err1 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == 3;
    sens_err2 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == 3;



    if(inc_sensor.pm67regs.read_comand_reg.bit.update_registers)
    {
        rotor_22220.error_update_count++;
    }
//  logpar.log22 = rotation_sensor.in_plane.out.CountOne1;
//  logpar.log23 = rotation_sensor.in_plane.out.CountZero1;


//    if (edrk.logs_rotor==1)
//        flag_buf = edrk.logs_rotor;
//
//
//    if (flag_buf==1 && prev_flag_buf==0)
//        c_s = -1;
//
//    if (flag_buf)
//    {
//        if (c_s>=(SIZE_BUF_W-1))
//        {
//            flag_buf = 0;
//            c_s = 0;
//        }
//        else
//            c_s++;
//
//
//        buf1[c_s] = inc_sensor.data.CountOne1;//   rotation_sensor.in_plane.out.CountOne1;
//        buf2[c_s] = inc_sensor.data.CountZero1;//rotation_sensor.in_plane.out.CountZero1;
//        buf3[c_s] = inc_sensor.data.CountOne2;//rotation_sensor.in_plane.out.CountOne2;
//        buf4[c_s] = inc_sensor.data.CountZero2;//rotation_sensor.in_plane.out.CountZero2;
//        buf5[c_s] = direction1;//inc_sensor.data.;//(rotation_sensor.in_plane.out.direction1);
//        buf6[c_s] = direction2;//(rotation_sensor.in_plane.out.direction2);
//        buf7[c_s] = (project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1);
//        buf8[c_s] = (project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2);
//
//    }
//    prev_flag_buf = flag_buf;


#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif


    if(inc_sensor.use_sensor1)
    {
        if((inc_sensor.data.CountOne1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1)
                        || inc_sensor.data.CountOne1 == 65535)
        { inc_sensor.data.CountOne1 = 0; }
        if((inc_sensor.data.CountZero1 <= 200)//  && !rotation_sensor.in_plane.out.counter_freq1)
                        || inc_sensor.data.CountZero1 == 65535)
        { inc_sensor.data.CountZero1 = 0; }

        //Когда импульсов мало считаем по периоду
        if (inc_sensor.data.Impulses1 < 5) {

            if(inc_sensor.data.CountOne1 && inc_sensor.data.CountZero1 )
            {
                sum_count = (long)inc_sensor.data.CountOne1 + (long)inc_sensor.data.CountZero1;

                if (s_number3<SIZE_BUF_F1)    f1[s_number3++] = counter_To_iqF2(sum_count, inc_sensor.data.counter_freq1);

                if (s_number2<SIZE_BUF_F1)
                {
                if (inc_sensor.data.counter_freq1)
                    f1_int[s_number2++] = sum_count/6;
                else
                    f1_int[s_number2++] = sum_count*100/6;
                }
                if (s_number<SENSORS_NUMBER)
                rotor_22220.iqFsensors[s_number++] = counter_To_iqF2(sum_count,  inc_sensor.data.counter_freq1);

            }
//            if(rotation_sensor.in_plane.out.CountOne1) {
//                rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne1,
//                                                            rotation_sensor.in_plane.out.counter_freq1);
//            }
//            if(rotation_sensor.in_plane.out.CountZero1) {
//                rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero1,
//                                                            rotation_sensor.in_plane.out.counter_freq1);
//            }
        }
        else
        {
            if (s_number2<SIZE_BUF_F1)
            f1_int[s_number2++] = -2;
        }

        if(inc_sensor.data.Impulses1 > 2)
        {
            if (s_number<SENSORS_NUMBER)
            rotor_22220.iqFsensors[s_number++] = impulses_To_iqF(inc_sensor.data.Time1,
                                                           inc_sensor.data.Impulses1);

            if (s_number3<SIZE_BUF_F1)  f1[s_number3++] = impulses_To_iqF(inc_sensor.data.Time1,
                                              inc_sensor.data.Impulses1);

            if (s_number2<SIZE_BUF_F1)  f1_int[s_number2++] = inc_sensor.data.Time1*10/inc_sensor.data.Impulses1;

//          logpar.log11 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]);
        } else {
//          logpar.log11 = 0;
            if (s_number2<SIZE_BUF_F1)
              f1_int[s_number2++] = -3;
        }

        if (rotor_22220.iqF > 139810L)//10 rpm
        {
            if (inc_sensor.data.CountOne1 == 0 && inc_sensor.data.CountZero1 == 0
                    && inc_sensor.data.Impulses1 == 0) {
                sens1_err_count += 1;
            } else {
                sens1_err_count = 0;
            }
            if (sens1_err_count > 50) {
                sens1_err_count = 50;
                edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 1;
//                faults.faults4.bit.Speed_Datchik_1_Off |= 1;
            }
        } else {
            sens1_err_count = 0;
            edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 0;
        }
    }


#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif


//  logpar.log4 = rotation_sensor.in_plane.out.CountOne2;
//  logpar.log20 = rotation_sensor.in_plane.out.CountZero2;
    if(inc_sensor.use_sensor2)
    {
        if((inc_sensor.data.CountOne2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2)
                        || inc_sensor.data.CountOne2 == 65535)
        { inc_sensor.data.CountOne2 = 0; }
        if((inc_sensor.data.CountZero2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2)
                        || inc_sensor.data.CountZero2 == 65535)
        { inc_sensor.data.CountZero2 = 0; }

        //Кодда импульсов мало, считаем по периоду
        if (inc_sensor.data.Impulses2 < 5) {

            if(inc_sensor.data.CountOne2 && inc_sensor.data.CountZero2 )
                        {
                            sum_count = (long)inc_sensor.data.CountOne2+(long)inc_sensor.data.CountZero2;
                            if (s_number3<SIZE_BUF_F1)  f1[s_number3++] = counter_To_iqF2(sum_count, inc_sensor.data.counter_freq2);

                            //f1_int[s_number2++] = sum_count/60;
                            if (s_number2<SIZE_BUF_F1)
                            {
                            if (inc_sensor.data.counter_freq2)
                                f1_int[s_number2++] = sum_count/6;
                            else
                                f1_int[s_number2++] = sum_count*100/6;
                            }
                            if (s_number<SENSORS_NUMBER)
                            rotor_22220.iqFsensors[s_number++] = counter_To_iqF2(sum_count, inc_sensor.data.counter_freq2);

                        }
//            if(rotation_sensor.in_plane.out.CountOne2) {
//                rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne2,
//                                                            rotation_sensor.in_plane.out.counter_freq2);
//            }
//            if(rotation_sensor.in_plane.out.CountZero2) {
//                rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero2,
//                                                            rotation_sensor.in_plane.out.counter_freq2);
//            }
        }
        else
        {
            if (s_number2<SIZE_BUF_F1)
            f1_int[s_number2++] = -4;
        }


#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif


        if(inc_sensor.data.Impulses2 > 2)
        {
            if (s_number<SENSORS_NUMBER)
            rotor_22220.iqFsensors[s_number++] = impulses_To_iqF(inc_sensor.data.Time2,
                                                           inc_sensor.data.Impulses2);

            if (s_number2<SIZE_BUF_F1)  f1_int[s_number2++] = inc_sensor.data.Time2*10/inc_sensor.data.Impulses2;

//          logpar.log16 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]);
        }
        else
        {
            if (s_number2<SIZE_BUF_F1)
            f1_int[s_number2++] = -5;
        }


#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif


        if (rotor_22220.iqF > 139810L)//10 rpm
        {
            if (inc_sensor.data.CountOne2 == 0 && inc_sensor.data.CountZero2 == 0
                    && inc_sensor.data.Impulses2 == 0) {
                sens2_err_count += 1;
            } else {
                sens2_err_count = 0;
            }
            if (sens2_err_count > 50) {
                sens2_err_count = 50;
//                faults.faults4.bit.Speed_Datchik_2_Off |= 1;
                edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 1;
            }
        } else {
            sens2_err_count = 0;
            edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 0;
        }

    }

    if(s_number > SENSORS_NUMBER_ONLY_IN)   {s_number = SENSORS_NUMBER_ONLY_IN;}    //TODO set SENSORS_NUMBER when tune angle measure
    if(s_number > 3)
    {
        sort_F_array(rotor_22220.iqFsensors, s_number);
        deltaF = rotor_22220.iqFout >> 2;
        if(deltaF < 43000) // ~3 ob/min
        {
            deltaF = 43000;
        }
        i = 0;
        begin_data = 0;
        end_data = s_number;    //TODO test, as usial
        while(i < s_number)
        {
            if(_IQabs(rotor_22220.iqFout - rotor_22220.iqFsensors[i]) >= deltaF)
            {
                i++;
            }
            else
            {
                break;
            }
        }
        if(i < s_number) { begin_data = i; }
        else {begin_data = 0;}
        while((i < s_number) && (_IQabs(rotor_22220.iqFout - rotor_22220.iqFsensors[i]) < deltaF))
        {
            i++;
        }
        if(i <= SENSORS_NUMBER)
        {
            end_data = i;
        }
        else
        {
            end_data = SENSORS_NUMBER;
        }
    }
    else
    {
        begin_data = 0;
        end_data = s_number;
    }
    if (begin_data >= end_data) {   //This part to prevent freeze of speed on some level if signal lost
        begin_data = 0;
        end_data = s_number;
    }
    // суммируем
    for(i = begin_data; i < end_data; i++)
    {
        accumulator += rotor_22220.iqFsensors[i];
    }
    // усредняем
    if(end_data != begin_data)
    {
        rotor_22220.iqFdirty = accumulator / (end_data - begin_data);
        prev_wrot_count = 0;
    }
    else
    {
        rotor_22220.iqFdirty = prev_wrot;
        prev_wrot_count += 1;
    }

//  logpar.log19 = (int16)(_IQtoIQ15(rotor.iqF));
//    rotor_22220.iqFdirty = rotor_22220.iqF;

    if (prev_wrot != rotor_22220.iqFdirty || rotor_22220.iqFdirty==0 )
    {
        rmp_wrot.DesiredInput = rotor_22220.iqFdirty;
        rmp_wrot.calc(&rmp_wrot);
        rotor_22220.iqF = rmp_wrot.Out;
    }
    else
    {
        rotor_22220.iqF = rotor_22220.iqFdirty;
    }
    prev_wrot=rotor_22220.iqF;


#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif


    //От зависаниЙа старого значениЙа.
    if (prev_wrot_count > 10) {
        prev_wrot = 0;
        prev_wrot_count = 10;
    }

    rotor_22220.iqFout = exp_regul_iq(koef_Wout_filter, rotor_22220.iqFout, rotor_22220.iqF);
    rotor_22220.iqFlong = exp_regul_iq(koef_Wout_filter_long, rotor_22220.iqFlong, rotor_22220.iqF);

#if (_ENABLE_PWM_LED2_PROFILE)
        if (profile_pwm[pos_profile_pwm++])
            i_led2_on_off(1);
        else
            i_led2_on_off(0);
#endif


    rotor_22220.direct_rotor_in1 = direction1;//rotation_sensor.in_plane.out.direction1;

#if (_STEND_40MWT==1)
    // меняем направление вращения для второго канала, т.к. на стенде туда завели инверсные каналы, а датчик физически один!!!

    rotor_22220.direct_rotor_in2 = -rotation_sensor.in_plane.out.direction2;

#else

    rotor_22220.direct_rotor_in2 = direction2;//rotation_sensor.in_plane.out.direction2;

#endif
 //   rotor.direct_rotor_angle = rotation_sensor.rotation_plane.out.direction;

    rotor_22220.error.sens_err1 = sens_err1;
    rotor_22220.error.sens_err2 = sens_err2;

//  rotor.direct_rotor = (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0 ? 1 :    // + rotor.direct_rotor_angle
//                      (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) < 0 ? -1 :    // + rotor.direct_rotor_angle
//                      0;
    if(rotor_22220.iqFout >139810L)   //10ob/min
    {
        if((rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) > 0)
        {
            direct_accum++;
        }
        else if((rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) < 0)
        {
            direct_accum--;
        }
        else
        {
            if(direct_accum > 0) {direct_accum--;}
            if(direct_accum < 0) {direct_accum++;}
        }
        if(direct_accum > 60) { direct_accum = 60; }
        if(direct_accum < -60) { direct_accum = -60; }
        rotor_22220.direct_rotor = direct_accum > 0 ? 1 :
                            direct_accum < 0 ? -1 :
                                0;
//      if (f.flag_second_PCH) {
//          rotor.direct_rotor = - rotor.direct_rotor;
//      }
    }
    else
    {
        rotor_22220.direct_rotor = (rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) > 0 ? 1 :    // + rotor.direct_rotor_angle
                                (rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) < 0 ? -1 :    // + rotor.direct_rotor_angle
                                0;
//      if (f.flag_second_PCH) {
//          rotor.direct_rotor = - rotor.direct_rotor;
//      }
        direct_accum = rotor_22220.direct_rotor;
    }

//  if(rotation_sensor.in_plane.write.regs.comand_reg.bit.set_sampling_time)    //Импульсы короткие
//  {
//      rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0x5C;
//  }
//  else    //Импульсы длинные, фильтр усиливаем
//  {
//      rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0xA8;
//  }
    if (s_number2<SIZE_BUF_F1)
        f1_int[s_number2++] = -1;
}

#pragma CODE_SECTION(impulses_To_iqF,".fast_run");
_iq impulses_To_iqF(unsigned int time, unsigned int impulses) //time mks. impulses count
{
    //Flong = (impulses / time / IMPULSES_PER_TURN) * _IQ(1) / NORMA_FROTOR;
    static unsigned long long koeff_to_F = 16777216LL / IMPULSES_PER_TURN * 1000000LL / NORMA_FROTOR;
    long long Flong = (long long)impulses * koeff_to_F / time;
    return (_iq)Flong;
}

//Frot = Fimp / IMPULSES_PER_TURN / NORMA_FROTOR =
// = counter_freq / counter / 2 / IMPULSES_PER_TURN / NORMA_FROTOR
//делю на 2, т.к. считаем длительность половины периода
//дромножаю на (1LL << 24) чтобы привести к iq24
#pragma CODE_SECTION(counter_To_iqF,".fast_run");
_iq counter_To_iqF(unsigned int count, unsigned int freq_mode)
{
    static long long koeff_to_F = 60000000LL * _IQ(1.0) / IMPULSES_PER_TURN / 2 / NORMA_FROTOR;
    long long Flong = 0;
    if(freq_mode == 0) // 60Mhz
    {
        Flong = koeff_to_F / count / 100;
    }
    else    //600KHz
    {
        Flong = koeff_to_F / count;
    }
    return Flong;
}

#pragma CODE_SECTION(counter_To_iqF2,".fast_run");
_iq counter_To_iqF2(long count, unsigned int freq_mode)
{
    static long long koeff_to_F = 60000000LL * _IQ(1.0) / IMPULSES_PER_TURN  / NORMA_FROTOR;
    long long Flong = 0;
    if(freq_mode == 0) // 60Mhz
    {
        Flong = koeff_to_F / count / 100;
    }
    else    //600KHz
    {
        Flong = koeff_to_F / count;
    }
    return Flong;
}

//#pragma CODE_SECTION(delta_Angle_To_iqF,".fast_run");
//_iq delta_Angle_To_iqF(unsigned long delta, unsigned int period)
//{
//    // iqF = (delta / ANGLE_RESOLUTION) / (period * 10^-6) * (1LL << 24) / NORMA_FROTOR;
//    // iqF = delta * 10^6 * (1LL << 24) / ANGLE_RESOLUTION / period / NORMA_FROTOR;
//    static long long koeff_to_F = 1000000LL * (1LL << 24) / ANGLE_RESOLUTION / NORMA_FROTOR;
//    return (_iq)(koeff_to_F * delta / period);
//}

// сортировка массива пузырьком
#pragma CODE_SECTION(sort_F_array,".fast_run2");
void sort_F_array(_iq *array, unsigned int size)
{
    unsigned int i, j;
    _iq tmp = 0;
    for(i = size; i > 0; i--)
    {
        for(j = 1; j < size; j++)
        {
            if(array[j - 1] > array[j])
            {
                tmp = array[j];
                array[j] = array[j - 1];
                array[j - 1] = tmp;
            }
        }
    }
}


void select_values_wrotor_22220(void)
{
 //   static _iq level_switch_to_get_impulses_hz = _IQ(LEVEL_SWITCH_TO_GET_IMPULSES_OBOROTS/60.0/NORMA_FROTOR);
    static unsigned int prev_RotorDirectionInstant = 0;
    static unsigned int status_RotorRotation = 0; // есть вращение?
    static _iq wrotor_add = _IQ(0.002/NORMA_FROTOR);

    WRotor.RotorDirectionSlow = rotor_22220.direct_rotor;
    WRotor.iqWRotorSum        = rotor_22220.iqFout;//  * rotor_22220.direct_rotor;
    WRotor.iqWRotorSumFilter  = rotor_22220.iqFout  * rotor_22220.direct_rotor;
    WRotor.iqWRotorSumFilter2 = rotor_22220.iqFlong * rotor_22220.direct_rotor;
    WRotor.iqWRotorSumFilter3 = rotor_22220.iqFlong * rotor_22220.direct_rotor;
    WRotor.iqWRotorSumRamp = zad_intensiv_q(wrotor_add, wrotor_add, WRotor.iqWRotorSumRamp, WRotor.iqWRotorSumFilter);


}