Dmitry Kovalev
/
LGfiltr
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
LGstaandart
by 
Dmitry Kovalev
host/Source/App/SIP.c
- Committer:
- Kovalev_D
- Date:
- 2016-02-03
- Revision:
- 23:12e6183f04d4
File content as of revision 23:12e6183f04d4:
#include "mathDSP.h"
#include "SIP.h"
#include "CyclesSync.h"
#include "CntrlGLD.h"
#include "InputOutput.h"
#include "ThermoCalc.h"
#include "el_lin.h"
uint32_t Old_Cnt_Vib = 0;
uint32_t Old_Cnt = 0;
int32_t RefMeand_Cnt_Dif;
int32_t PSdif_sum_Vib_32 = 0;
__int64 PSdif_sum_Vib_64 = 0;
int32_t dif_Curr_32_Ext; //r. ðàçíîñòü (÷èñëî) äëÿ ðåæèìà âíåøíåé çàùåëêè
int32_t dif_Curr_32_previous; //e. Previous (in comparison with Dif_Curr_32) number //r. ïðåäûäóùåå (ïî ñðàâíåíèþ ñ Dif_Curr_32) ÷èñëî
int32_t temp22=0;
//+++++++++++++++++++++++++++++++INPUT DATA++++++++++++++++++++++++++++++++++++++++++++++++
uint32_t Curr_Cnt_Vib;
uint32_t Cnt_curr;
//+++++++++++++++++++++++++++++++variables for output++++++++++++++++++++++++++++++++++++++
int32_t Dif_Curr_Vib; //e. current difference output for dithering control in LightUp mode and Dither regulator
int32_t Dif_Curr_32; //e. current difference without dithering for dithering control
#if (defined GLOBALRATE)//íå âõîäèò
//variables for rate mode
int32_t cntPls_sum_32;
ñññ int32_t last_Cnt_Plus;
int32_t dif_sum_32;
int32_t Cnt_Pls;
int32_t Cnt_Mns;
int32_t preLast_Cnt_Plus;
// uint32_t sumCnt_Mns = 0;
//uint32_t sumCnt_Pls = 0;
extern int32_t dif_cur_test[30];
extern unsigned ii;
uint32_t halfPeriod = 0;
#endif
void ResetBitsOfWord(int * x32, int truncate_bits)
{
int hi_part;
hi_part = *x32 >> truncate_bits;
*x32 -= hi_part << truncate_bits; //r. îñòàâëÿåì ìëàäøèå 16 áèò
}
/******************************************************************************
** Function name: interpolation
**
** Descriptions: êîëè÷åñòâî èòåðïîëÿöèé äî ïîÿâëåíèÿ âíåøíåé çàùåëêè
**
** Returned value: Number in moment of external latch appearing
** parameters: y_curr - current number, y_prev - number at one cycle before time
** x_interp - moment of external latch appearing,
** Precision of interpolation is 1/8 of impulse (3 digits after point in 14.18 format)
******************************************************************************/
int interpolation(int y_curr, int x_interp)
{
__int64 temp,temp3;
temp = (__int64)y_curr *(__int64)x_interp;
temp /= PrevPeriod; // void ServiceTime(void) ||| PrevPeriod = LPC_PWM1->MR0; ||| (CyclesSync.c)
temp3 = (__int64)y_curr *(__int64)(x_interp+1);
temp3 /= PrevPeriod;
temp22 = (int)temp3;
return ((int)temp);
} // interpolation
/******************************************************************************
** Function name: clc_Pulses
**
** Descriptions: Processing of information from SPOI
**
** parameters: None
** Returned value: None
**
******************************************************************************/
void clc_Pulses()
{
#if (!defined GLOBALRATE)
static int32_t cntPls_sum_32;
static int32_t last_Cnt_Plus;
static int32_t dif_sum_32;
static int32_t Cnt_Pls;
static int32_t Cnt_Mns;
static int32_t preLast_Cnt_Plus;
#endif
Dif_Curr_Vib = Curr_Cnt_Vib - Old_Cnt_Vib; //Âû÷èñëåíèå ïðèðàùåíèÿ òåêóùåãî ñ÷åò÷èêà èìïóëüñîâ.
Old_Cnt_Vib = Curr_Cnt_Vib; //ñîõðàíåíèå òåêóùåãî ñ÷åò÷èêà èìïóëüñîâ äëÿ ñëåäóþùåãî öèêëà èçìåðåíèé
Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2); //ïðîâåðêà íà ïåðåïîëíåíèå (Dif_Curr_Vib).
// Uin UpSat DwnSat
//#define Cnt_Overload(Uin, UpSat, DwnSat)
/* if (Uin > UpSat)
{
Uin -= INT32_MAX;
}
if (Uin < DwnSat)
{
Uin += INT32_MAX;
} */
Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // Òî÷íîñòü ôèëüòðàöèè 1/(2^18)
switch (RgConB) //r. äîïîëíèòåëüíûé ðåãèñòð óïðàâëåíèÿ
{
case RATE_VIBRO_1: //r. ðàçíîñòü âèáðîñ÷åò÷èêîâ ïîñëå ôèëüòðà ñêîëüçÿùíãî ñðåäíåãî
if (Latch_Rdy) //e. latch has arrived
{
//dif_Curr_32_Ext ðàçíîñòü (÷èñëî) äëÿ ðåæèìà âíåøíåé çàùåëêè
dif_Curr_32_Ext = interpolation(Dif_Curr_32, LatchPhase ); //èíòåðïîëÿöèÿ
Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
//r. äîáàâëÿåì ê íàêîïëåííîé ñóììå èíòåðïîëèðîâàííûé îòñ÷åò âíåøíåé çàùåëêè
PSdif_sum_Vib_32 += dif_Curr_32_Ext;
PSdif_sum_Vib_64 += dif_Curr_32_Ext; //e. receive last data
//count--;
//r. ïîäãîòîâèòü ÷èñëî äëÿ âûäà÷è
Output.Str.BINS_dif = PSdif_sum_Vib_32 - TermoCompens_Sum; //r. èç íàêîïëåííîãî ÷èñëà âû÷èòàåì íàêîïëåííóþ òåðìîêîìïåíñàöèîííóþ ñîñòàâëÿþùóþ
Output.Str.PS_dif = Output.Str.BINS_dif >> 16;
LatchPhase = INT32_MAX; //INT32_MAX=2147483647 //in Latch_Event it's indicator of latch appearing
Output.Str.SF_dif = PSdif_sum_Vib_64;
TermoCompens_Sum = 0; //r. îáíóëÿåì íàêîïëåííóþ òåðìîêîìïåíñàöèþ äëÿ íà÷àëà íîâîãî öèêëà íàêîïëåíèÿ
if ((Device_Mode == DM_EXT_LATCH_DELTA_BINS_PULSE)||((Device_Mode == DM_EXT_LATCH_DELTA_SF_PULSE) && Ext_Latch_ResetEnable))
{
PSdif_sum_Vib_32 = 0; //r. èíèöèàëèçèðîâàòü íîâûé öèêë èçìåðåíèÿ ïî çàùåëêå
PSdif_sum_Vib_64 = 0;
}
else
ResetBitsOfWord(&PSdif_sum_Vib_32, 16);
dif_Curr_32_Ext = Dif_Curr_32 - temp22;//dif_Curr_32_Ext;
PSdif_sum_Vib_32 += dif_Curr_32_Ext; // preserve rest of counters difference for next measure cycle: PSdif_sum_Vib_32 += Dif_Curr_32 - dif_Curr_32_Ext;
PSdif_sum_Vib_64 += dif_Curr_32_Ext; //ñîõðàíèòü îñòàëüíûå ñ÷åò÷èêè ðàçíèöû äëÿ ñëåäóþùåãî òàêòà öèêëà
}
else //r. çàùåëêè íà íàñòîÿùèé ìîìåíò íå áûëî
{
//r. ïðîäîëæàåì íàêàïëèâàòü ñóììó èç âíóòðåííèõ îòñ÷åòîâ
PSdif_sum_Vib_32 += Dif_Curr_32; // PSdif_sum_Vib_32 += Dif_Curr_32 ;
PSdif_sum_Vib_64 += Dif_Curr_32; //e. sum for scale factor measurement mode
}
dif_Curr_32_previous = Dif_Curr_32; //r. çàïîìèíàåì ïðåäûäóùåå ÷èñëî
break;
case RATE_REPER_OR_REFMEANDR:
if (data_Rdy & HALF_PERIOD) //e. calculate Cnt_Mns or Cnt_Pls
{
RefMeand_Cnt_Dif = Cnt_curr - Old_Cnt;
Old_Cnt = Cnt_curr;
// LPC_GPIO2->FIOCLR = 0x10;
Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2);
if (LPC_QEI->STAT) //e. "+" direction //r. ñòàëè âðàùàòüñÿ â "+" ñòîðîíó
{
//sumCnt_Mns += -RefMeand_Cnt_Dif; //e. accumulation during 1 sec
Cnt_Mns = RefMeand_Cnt_Dif;
}
else
{
//r. ñòàëè âðàùàòüñÿ â "-" ñòîðîíó
//sumCnt_Pls += RefMeand_Cnt_Dif; //e. accumulation during 1 sec
Cnt_Pls = -RefMeand_Cnt_Dif;
}
// UART1_SendByte((dif_sum_32>>8) & 0xff);
// UART1_SendByte((dif_sum_32) & 0xff);
if (data_Rdy & WHOLE_PERIOD) //e. period of vibro elapsed
{
// LPC_GPIO2->FIOSET = 0x10;
last_Cnt_Plus = Cnt_Pls;
dif_sum_32 += Cnt_Pls - Cnt_Mns;
}
data_Rdy &= ~RESET_PERIOD;
}
if (Latch_Rdy) //e it's time for output
{
LatchPhase = INT32_MAX;
Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
cntPls_sum_32 += last_Cnt_Plus - preLast_Cnt_Plus;
Output.Str.Cnt_Dif = dif_sum_32;
Output.Str.Cnt_Dif += cntPls_sum_32 >> 1;
dif_sum_32 = 0; //r. è ñáðîñèòü ðåãèñòðû íàêîïëåíèÿ
ResetBitsOfWord(&cntPls_sum_32, 1); //r. îñòàâëÿåì ìëàäøèé ðàçðÿä, ÷òîáû íå òåðÿëàñü òî÷íîñòü ïðè ñóììèðîâàíèè
preLast_Cnt_Plus = last_Cnt_Plus; //r. òåêóùèé ïîñëåäíèé îòñ÷åò ñòàë ïðåäûäóùèì
Output.Str.Cnt_Mns = Cnt_Mns;//sumCnt_Mns; //e. rewrite accumulated data to output
Output.Str.Cnt_Pls = Cnt_Pls;//sumCnt_Pls;
//sumCnt_Mns = 0; //e. prepare for new accumulation
//sumCnt_Pls = 0;
}
break; // RATE_REPER_OR_REFMEANDR
}
//e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - ïåðåìåííûå äëÿ êîíòðîëÿ â ðåæèìå rate3
Output.Str.WP_scope1 = Dif_Curr_Vib;
Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 äðîáíûõ ðàçðÿäà îñòàâëÿåì äëÿ áîëüøåé íàãëÿäíîñòè ïðè àíàëèçå ñèãíàëà rate3
}
// clc_Pulses
/******************************************************************************
** Function name: SOI_Init
**
** Descriptions: Quadrature encoder initialization.
**
** parameters: None
** Returned value: None
**
******************************************************************************/
void SOI_Init(void)
{
LPC_SC->PCONP |= (1<<18);//0x00040000; âêëþ÷åíèå êâàäðàòóðíîãî ýíêîäåðà.
LPC_SC->PCLKSEL1 |= 0x00000001; // âûáîð ÷àñòîòû äëÿ êâ.ý. CLK=100MHz
LPC_PINCON->PINSEL3 &= ~0x4100;
LPC_PINCON->PINSEL3 |= 0x4100; //P1.20, p1.23 óñòàíîâèòü êàê âõîäû êâ.ý
LPC_PINCON->PINMODE3 |= 0x3C300; //P1.20, p1.23, p1.24 ??are pulled-down??(ïðèòÿíóòü ê ïèòàíèþ)????
LPC_QEI->MAXPOS = MAX_QEI_CNT; //LPC_QEI->"ðåãèñòîð ìàêñèìàëüíîãî ïîëîæåíèÿ" = 2147483646
LPC_QEI->FILTER = 2; // ôèëüòð( ?? )
LPC_QEI->CON = 0xF; //ñáðîñèòü â íîëü âñå ñ÷åò÷èêè êâ.ý. (ñêîðîñòü, íàïðàâëåíèå, ïîçèöèþ è òä)
LPC_QEI->CONF = (0<<2) |(0<<1); //Quadrature inputs, no inverting,only A pulses are counted
LPC_QEI->CLR = 0x1fff; //r. ñáðîñèòü âñå ïðåðûâàíèÿ
LPC_QEI->IEC = 0x1fff; //r. çàïðåòèòü ïðåðûâàíèå ïðè èçìåíåíèè íàïðàâëåíèÿ
NVIC_SetPriority(QEI_IRQn, 0);
NVIC_EnableIRQ(QEI_IRQn);
return;
}
/******************************************************************************
** End Of File
******************************************************************************/