File content as of revision 21:bc8c1cec3da6:

#include "Global.h"



#define 	SHIFT_7680_12500		15	  //e. 14 digits for 7680 to 12500 clock converting and 1 division digit  
#define		SHIFT_C_7680_12500		11	  //
#define		DITH_VBN_SHIFT			2 //e. //r. определяет сдвиг (деление на 4) коэффициента деления вибропривода, чтобы иметь запас на регулирование
#define		DITH_VB_TAU_SHIFT		2



int32_t RI_diff;		//e.input signal of "recovery" APS //r. входной сигнал "восстановленного" ДУП
int32_t MaxDelay;
int32_t VB_tau_Ins;        //r.  внутреннее значение контура регулирования Тау
int32_t VB_Nmin0;  				//r. минимум  выходного значения регулятора периода для температуры Device_blk.Str.TemperNormal
int32_t VB_Nmax0;  				//r. максимум выходного значения регулятора периода для температуры Device_blk.Str.TemperNormal

uint32_t In_Flag = 0;
uint32_t SwitchCntInq = 0;

int32_t accum_error = 0;
int32_t ph_error = 0;
int32_t accum_error_old = 0;
int32_t PhaseShift;
int32_t temp2;
int32_t temp3;

#if defined DITHERSIM
int32_t timeDither = 0;
int32_t LIM0;
#endif
extern uint32_t  Vibro_2_CountIn;
void clc_Noise_regulator(void);

/******************************************************************************
** Function name:		VibroDither_Set
**
** Descriptions:		Set period and pulse width for dither.
**
** parameters:			duration of vibro pulses, period of dither
** Returned value:		None
**
******************************************************************************/
void VibroDither_Set()
{
//коэфф.деления N вибропривода (период колебаний) ВП = T_Vibro длительность импульса вибропривода>>


    Device_blk.Str.VB_N = Output.Str.T_Vibro;
    LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;//#define 	SHIFT_7680_12500		15	  смешение для конвертации частоты из 7680 в 12500
#if defined DITHERSIM
    6565  LIM0 = (Output.Str.T_Vibro*86)>>16;
#endif
#if !defined CONSTCYCLE
    5655 SwitchCntInq = 1;	 //to enable inquiry timer reloading
#endif
}
/******************************************************************************
** Function name:		VibroDither_SwitchOn
**
** Descriptions:		VibroDither switching on.
**
** parameters:			None
** Returned value:		None
**
******************************************************************************/
void VibroDither_SwitchOn()
{
    LPC_MCPWM->CON_SET = 1<<8;	  //start vibro dither
}
/******************************************************************************
** Function name:		VibroDither_SwitchOff
**
** Descriptions:		VibroDither switching off.
**
** parameters:			None
** Returned value:		None
**
******************************************************************************/
void VibroDither_SwitchOff()
{
    LPC_MCPWM->CON_CLR = 1<<8;	  //stop vibro dither
}

/******************************************************************************
** Function name:		VB_MeanderDelay
**
** Descriptions:		Routine for addition of delay to meander
**
** parameters:			meander, delay magnitude, max delay
** Returned value:		delayed meander
**
******************************************************************************/
int VB_MeanderDelay(int VB_Meander, int Delay100uS, int MaxDly)
{
    static int poz_counter = 0, neg_counter = 0, flg_delay;

    if (Delay100uS == 0) {
        return (VB_Meander);
    }

    if (Delay100uS > 0) {
        if (Delay100uS > MaxDly) {
            Delay100uS = MaxDly;
        }
        if (VB_Meander) { //e. outgoing WP_flg flag, which delayed by the WP_ref //r. формирование задержанного на величину WP_ref флага poz_sin_flag
            neg_counter = 0;
            poz_counter++;
        } else {
            poz_counter = 0;
            neg_counter++;
        }
        if (poz_counter == Delay100uS) {
            flg_delay = 1;
        }
        if (neg_counter == Delay100uS) {
            flg_delay = 0;
        }
    } else {
        Delay100uS = -Delay100uS;
        if (Delay100uS > MaxDly) {
            Delay100uS = MaxDly;
        }
        if (VB_Meander) { //e. outgoing WP_flg flag, which delayed by the WP_ref //r. формирование задержанного на величину WP_ref флага poz_sin_flag
            neg_counter = MaxDly + 1;
            poz_counter--;
        } else {
            poz_counter = MaxDly + 1;
            neg_counter--;
        }
        if (poz_counter == Delay100uS) {
            flg_delay = 0;
        }
        if (neg_counter == Delay100uS) {
            flg_delay = 1;
        }
    }
    return (flg_delay);
} // VB_MeanderDelay
/******************************************************************************
** Function name:		VB_PhaseDetectorRate
**
** Descriptions:		Routine for accumulation of dither error
**
** parameters:			None
** Returned value:		None
**
******************************************************************************/
int VB_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime)
{

    static int SampleAndHoldOut = 0, VB_PhasDet_integr = 0;

    if (IntegrateTime == DEVICE_SAMPLE_RATE_uks) {
        SampleAndHoldOut = VB_PhasDet_integr;
        VB_PhasDet_integr = 0;
    } else {
        VB_PhasDet_integr += PhaseDetInput;
    }
    return (SampleAndHoldOut);
} // VB_PhaseDetectorRate

/*r.

	DelayedDithMeander - задержанный меандр (на величину VB_phs)
Выход
	VB_N - коэффициент деления
*/
/******************************************************************************
** Function name:		clc_Dith_regulator
**
** Descriptions:		Routine for dither frequency controller
**
** parameters:			None
** Returned value:		None
**
******************************************************************************/
void clc_Dith_regulator(void)
{
//	static int smooth=0, buf[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, i = 0;
//	int ph_error;
    static int dith_period = 0;//, accum_error = 0;

    RI_diff = DUP_Filt(Dif_Curr_Vib<<2);

    if (RI_diff >= 0)
        ph_error = 1;
    else
        ph_error = 0;

    if (LPC_MCPWM->INTF & 0x0001) { //vibro pulse has been formed
        LPC_MCPWM->INTF_CLR |= 0x0001;
        if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1) {
//	  LPC_GPIO2->FIOSET = 0x000000FF;		// turn on the LED
            if (SwitchCntInq) { //inquiry cycle duration must be changed
                LPC_PWM1->MR0 = (Output.Str.T_Vibro*Vibro_2_CountIn)>>SHIFT_C_7680_12500;
                LPC_PWM1->LER = LER0_EN ;  //e. enable updating of register
                SwitchCntInq = 0;
            }
            LPC_MCPWM->MAT1 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;
            LPC_MCPWM->MAT2 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500;
            In_Flag = 0;
        } else {
//	  LPC_GPIO2->FIOCLR = 0x000000FF;		// turn off the LED
            LPC_MCPWM->MAT2 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;
            LPC_MCPWM->MAT1 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500;
            In_Flag = 1;
            dith_period++;
        }
    }

    temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //r. формирование задержанного сигнала меандр
    temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1;                         //r. аналоговый выход XOR ФД(-1..+1, т.к. const=1)
    accum_error += temp2;

    Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //r. формирование проинтегрированного за 1 сек аналогового сигнала ФД вибропривода
    if ( dith_period > DITHER_REG_PERIOD ) {                     //r. проверка состояния счетчика dith_period
        dith_period = 0; //r. 40 периодов - обнуление счетчика периодов вибропривода
        //r. масштабирование и суммирование с округлением и насыщением
        if ( loop_is_closed(VB_FREQ_ON) ) {
            Device_blk.Str.VB_N = mac_r(Device_blk.Str.VB_N << (16 - DITH_VBN_SHIFT),-accum_error,Device_blk.Str.VB_scl) << DITH_VBN_SHIFT;

            Saturation(Device_blk.Str.VB_N, Device_blk.Str.VB_Nmax, Device_blk.Str.VB_Nmin);	//r. проверка верхнего диапазона регулирования
            accum_error = 0;               //r. обнуление суммы _VB_Uab40
        }
    }

    if ( loop_is_closed(VB_FREQ_ON) ) { //r. фронт был, проверить включен ли контур стабилизации
        Output.Str.T_Vibro = Device_blk.Str.VB_N;
        LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;  //r. включен, загрузить вычисленные значения периода
    }
    // cyclic built-in test
    if ((Output.Str.T_Vibro > Device_blk.Str.VB_Nmax) || (Output.Str.T_Vibro < Device_blk.Str.VB_Nmin)) {
        Valid_Data |= DITH_FREQ_ERROR;
    }
} // clc_Dith_regulator

/******************************************************************************
** Function name:		clc_OutFreq_regulator
**
** Descriptions:		Routine for output frequency controller
**
** parameters:			None
** Returned value:		None
**
******************************************************************************/
void clc_OutFreq_regulator(void)
{
    static int out_freq_sum = 0;
    static int temp;
    /*
      	if (Dif_Curr_Vib > 0) //e. angular speed > 0 //r.скорость положительна
      	{
          if (RI_diff > 0)
            out_freq_sum += (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
          else
            out_freq_sum -= (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
      	}
      	else //e. angular speed < 0 //r.скорость отрицательна
      	{
      	 if (RI_diff < 0)
            out_freq_sum += (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
          else
            out_freq_sum -= (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
      	}
    */
    if(Dif_Curr_Vib>0)
        out_freq_sum += Dif_Curr_Vib;
    else
        out_freq_sum -= Dif_Curr_Vib;


    if (time_1_Sec == DEVICE_SAMPLE_RATE_uks) { //e. second has elapsed, fix the output frequency value //r. секунда прошла, зафиксировать значение частоты расщепления
        if (loop_is_closed(VB_TAU_ON)) { //e. the regulator loop is closed //r. контур замкнут
            temp = Device_blk.Str.VB_Fdf_Hi << 16;
            temp |= Device_blk.Str.VB_Fdf_Lo;
            temp = L_sub(out_freq_sum, temp) >> 3; // (out_freq_sum - temp) with saturation, then >> 3
            Saturation(temp, 32767, -32768); // error saturation if error is out of range
            //e. scaling and summing with rounding and saturation //r. масштабирование и суммирование с округлением и насыщением
            VB_tau_Ins = mac_r( VB_tau_Ins << (16 - DITH_VB_TAU_SHIFT),
                                temp,
                                Device_blk.Str.VB_Fsc ); // << DITH_VB_TAU_SHIFT;
            //e. reduction the VB_Err value to 16 digits (arithmetic right shift to 3 digits) //r. сведение величины VB_Err к 16 разрядам (арифметический сдвиг вправо на 3 разряда)

            Saturation(VB_tau_Ins, \
                       (int)Device_blk.Str.VB_Tmax >> DITH_VB_TAU_SHIFT, \
                       (int)Device_blk.Str.VB_Tmin >> DITH_VB_TAU_SHIFT);	//e. checking upper and lower levels in sign range
            VB_tau_Ins <<= DITH_VB_TAU_SHIFT;
        }

        Output.Str.F_ras = out_freq_sum >> 5; //e. once more divide output frequency by 2, in order to coincide with frequency meter //r. поделить частоту расщепления еще на 2, чтобы совпало с частотомером
        out_freq_sum = 0;	//e. reset accumulated values for next cycle of measurement //r. сбросить накопленные значения для следующего цикла измерения

        // cyclic built-in test
        // if output frequency is less than 3/4 of nominal then data is invalid
        if (Output.Str.F_ras < ((temp >> 7)*3)) {
            Valid_Data |= OUT_FREQ_ERROR;
        } else {
            Valid_Data &= ~OUT_FREQ_ERROR;
        }
    }

    clc_Noise_regulator();

    if ( loop_is_closed(VB_TAU_ON) ) { //r. контур стабилизации включен?
        Output.Str.L_Vibro = Device_blk.Str.VB_tau;  //r. иначе загрузить новое значение
        //r. длительности импульсов вибропривода
    }

} // clc_OutFreq_regulator

//e. noise regulator //r. система электронного ошумления вибропривода
/*r.
PeriodCount (VBN_Cnt) - счетчик периодов сигнала Meander.
Tnoise (VBN_Per)- текущий период ошумления.
PeriodNoise (VBN_Tzd) - средний период ошумления, заданный пользователем.
AmpNoise(VBN_Ran) - максимальная амплитуда периода ошумления (задается пользователем).
Delta (VBN_k) - глубина ошумления (задается пользователем).
Flag(VBN_Mod) - флаг знака изменения амплитуды.
Tu(VBN_Tau) - длительность импульса одновибратора.
Tp(VBN_tau_Ins) - длительность импульса одновибратора, задаваемая системой регулировки частоты расщепления.
*/

void clc_Noise_regulator(void)
{
    int temp;
    static uint32_t Flag = 0;
    static int PeriodCount = 0, Tnoise = 0;

    if ( PeriodCount >= Tnoise ) {
        PeriodCount = 0;
        srand(Device_blk.Str.VB_N);// Srand(период колебаний ВП) -инициализация генератора случайных чисел с зерном (VB_N)
        //заданный период ошумления
        Tnoise = add( Device_blk.Str.VBN_Tzd,     mult_r(Device_blk.Str.VBN_Ran, rand())); // Tnoise = Device_blk.Str.VBN_Tzd + MULT_RND_SAT( Device_blk.Str.VBN_Ran, rand() );
        if ( Flag ) {                                                                      //e. calculation +dF/-dF //r. расчет +dF/-dF
            temp = Device_blk.Str.VBN_k;                                                     //r. 25 - заданная константа ошумления
        } else {
            temp = -Device_blk.Str.VBN_k;
        }
///Длительность импульса до ошумления
        Device_blk.Str.VB_tau = add(VB_tau_Ins, (mult_r( VB_tau_Ins, temp ) << 1)); // VB_tau = VB_tau_Ins + VB_tau_Ins * temp; with saturation
        Saturation(Device_blk.Str.VB_tau, Device_blk.Str.VB_Tmax, Device_blk.Str.VB_Tmin);	//e. checking upper and lower levels of control range //r. проверка верхнего диапазона регулирования
        Flag = !Flag;
    } else {
        PeriodCount++;
    }

} // clc_Noise_regulator
/******************************************************************************
** Function name:		VibroDither_Init
**
** Descriptions:		VibroDither initialization.
**
** parameters:			None
** Returned value:		None
**
******************************************************************************/
void VibroDither_Init()
{
    LPC_SC->PCONP |= 0x00020000;		   //включение ШИМ.
    LPC_SC->PCLKSEL1 |= 0xC0000000;	   //CLK=12.5MHz выбор частоты

    /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*///выбор ножек для двух (трех) каналов ШИМ(PhA и PhB).
// LPC_PINCON->PINSEL3 &= ~(0x3CF<<18);
// LPC_PINCON->PINSEL3 |= (0x145 << 18) |(1<<6)|(1<<12);//P1.19 - MCOA0; P1.22 - MCOB0; P1.25 - MCOA1; P1.26 - MCOB1;	P1.28 - MCOA2;	 P1.29 - MCOB2;
    //выходы ШИМ (MCOA и MCOB) разной полярности.

    LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 )
    //АС режим (3-х фазный АС режим) все ШИМ используют
    //счетчик времени и регистр период канала 0.

    LPC_MCPWM->TC0 = 0;// инициализация (обнуление) таймера 0;
    LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //период ШИМ(Виропривода).

    LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500;	  // set LPC_MCPWM->MAT0  for defineteness  						                          |  установление временных интервалов
    LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500;	// pulse width of the PhA dither drive                                          |  (MAT) при достижении которых
    LPC_MCPWM->MAT1 = ((Device_blk.Str.VB_N - Device_blk.Str.VB_tau)*MULT_7680_12500)>>SHIFT_7680_12500;	// pulse width of the PhB dither drive  at first time   |  таймером, что то происходит.

    LPC_MCPWM->DT &= ~0x3FF; //e. reset dead timer register
    LPC_MCPWM->INTEN_SET = 1;	//e. enable lim0 interrupt

    LPC_MCPWM->CON_SET |= (1<<8) |1 |(1<<16);	  //start PWM channel 0,1,2

    VB_tau_Ins = Device_blk.Str.VB_tau;         // VB_tau_Ins - внутреннее значение контура регулирования Тау

    Output.Str.L_Vibro = Device_blk.Str.VB_tau;  //to update the period and pulse duration for displaying
    Output.Str.T_Vibro = Device_blk.Str.VB_N;    //запись в выходной масив длительности и периуда импульсов для отображения
    return;
}
/******************************************************************************
** Function name:		init_Dither_reg
**
** Descriptions:		Initialization of dither regulator.
**
** parameters:			None
** Returned value:		None
**
******************************************************************************/
void init_Dither_reg()
{

    init_VibroReduce(); // расчет коэфициентов (вибро апертуры)
    Device_blk.Str.VB_N = 29538; //коэфф.деления N вибропривода (период колебаний) ВП (? задается только сдесь но используется уже при вычислении Vibro_Filter_Aperture в предъидущей функции)
    VibroDither_Init();// Выбор ножек для двух каналов ШИМ(1-2(Канал 0 тоже определен)),период ШИМ,режим и тд.
    VibroDither_SwitchOn(); //LPC_MCPWM->CON_SET = 1<<8; старт таймера 1. выставление 8 бита mscon_set изменяет 8 бит в регистре mscon (PDF CTP. - 526)
    init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP);	//линейный фильтр (то же самое что и в СРП)полософой фильтр для выделения частоты колебания.
    MaxDelay = Vibro_Filter_Aperture >> 1; //r. макс. задержка меандра вибропривода (Vibro_Filter_Aperture определяетс в init_VibroReduce();)
    CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500);  //задание периода сброса счетчика, запрет прерывания.
}
/******************************************************************************
**                            End Of File
******************************************************************************/