Dmitry Kovalev
/
LG
n
Fork of LG by
Diff: ThermoCalc.c
- Revision:
- 0:8ad47e2b6f00
- Child:
- 1:f2adcae3d304
diff -r 000000000000 -r 8ad47e2b6f00 ThermoCalc.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ThermoCalc.c Sat Jan 30 13:00:39 2016 +0000 @@ -0,0 +1,297 @@ + #include "ThermoCalc.h" + #include "CyclesSync.h" + #include "CntrlGLD.h" + #include "InputOutput.h" + #include "mathDSp.h" + +#define debug +#define TEMP_AVER_PERIOD 4 // e. number of seconds for average + int TermoCompens_Sum = 0; +unsigned int IsHeating; + //int Tmp_Out[NUM_OF_THERMOSENS]; + int dThermoHeatDeltaPer_dTermo[TERMO_FUNC_SIZE]; + int dThermoCoolDeltaPer_dTermo[TERMO_FUNC_SIZE]; + int dFuncPer_dTermo[TERMO_FUNC_SIZE]; + int TermoCompDelta; + int Temp_Aver; //e. the mean temperature for 1 Sec for T4 sensor //r. ñðåäíÿÿ òåìïåðàòóðà çà 1 ñåêóíäó äëÿ äàò÷èêà T4 + int TempEvolution = 0; + int StartTermoCompens = 0; //e. initial thermocompensation (in XXX seconds after start ) //r. íà÷àëüíàÿ òåðìîêîìïåíñàöèÿ (÷åðåç ÕÕÕ ñåêóíä ïîñëå ñòàðòà) + +extern int WP_reset_heating; //e. voltage of reset at heating //r. íàïðÿæåíèå ñáðîñà ïðè íàãðåâàíèè +extern int WP_reset_cooling; //e. voltage of reset at cooling //r. íàïðÿæåíèå ñáðîñà ïðè îõëàæäåíèè + +__inline Max_Saturation(unsigned *lvl, unsigned limit) +{ + if (*lvl>limit) *lvl = limit; +} + +/*{ + switch (Device_blk.Str.TermoMode) //e. selecting thermocompensation mode //r. âûáîð ðåæèìà òåðìîêîìïåíñàöèè + { + case TERMO_ON: + case TERMO_ON_NUMB_OFF: + + TermoCompens_Sum += StartTermoCompens + DynamicDeltaCalc(); //e. accumulation of the value of thermocompensation from request to request //r. íàêîïëåíèå âåëè÷èíû òåðìîêîìïåíñàöèè îò çàïðîñà äî çàïðîñà + + break; + + case TERMO_ON_STATIC_ONLY: + case TERMO_ON_STATIC_ONLY_NUMB_OFF: +#if !defined debug_SOI + TermoCompens_Sum += StartTermoCompens; //e. accumulation of the value of thermocompensation from request to request //r. íàêîïëåíèå âåëè÷èíû òåðìîêîìïåíñàöèè îò çàïðîñà äî çàïðîñà +#endif + break; + + case TERMO_ON_DYNAMIC_ONLY: + case TERMO_ON_DYNAMIC_ONLY_NUMB_OFF: +#if !defined debug_SOI + TermoCompens_Sum += DynamicTermoCompens(); //e. accumulation of the value of thermocompensation from request to request //r. íàêîïëåíèå âåëè÷èíû òåðìîêîìïåíñàöèè îò çàïðîñà äî çàïðîñà +#endif + break; + + case TERMO_OFF: + default: + TermoCompens_Sum = 0; //e. thermocompensation is disable, therefore its part is equal to zero //r. òåðìîêîìïåíñàöèÿ âûêëþ÷åíà, ïîýòîìó åå âêëàä ðàâåí íóëþ + + } //of thermomode switch +} */ +/****************************************************************************** +** Function name: StaticTermoCompens +** +** Descriptions: Procedure of +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +int StaticTermoCompens(int temperature) //r. ðàñ÷åò ñòàòè÷åñêîé ñîñòàâëÿþùåé òåðìîêîìïåíñàöèè çà îäèí ïåðèîä ïðèáîðà (100 ìêñ) +{ + + float TermoCompens_Curr; //r. âåëè÷èíà òåðìîêîìïåíñàöèè çà îäèí ïåðèîä ïðèáîðà (100 ìêñ) + + int i, t; + + //r. èñïîëüçóåì òîëüêî òåðìîäàò÷èê TSENS_NUMB + //r. äëÿ íîâûõ òåðìîäàò÷èêîâ: T4, äëÿ ñòàðûõ: T1 + //r. è èñïîëüçóåì êóñî÷íî-íåïðåðûâíóþ òåðìîêîìïåíñàöèþ + + //r. Tmp_Out[TSENS_NUMB] = 8960; //-2560; //5120; //8000; // -2600; //-5000; + + t = temperature; + if(t > Device_blk.Str.TemperInt[TERMO_FUNC_SIZE - 1]) + { + t = Device_blk.Str.TemperInt[TERMO_FUNC_SIZE - 1]; + } + + i = 0; + while( t > Device_blk.Str.TemperInt[i] ) i++; + + //r. Tmp_Out[5] = i; + TermoCompens_Curr = Device_blk.Str.TermoFunc[i] - dFuncPer_dTermo[i] * (float)( Device_blk.Str.TemperInt[i] - t ); + + //r. Îòëàäêà + //r. TermoCompens_Curr = 1.111111125; // îòëàäêà + //r.TermoCompens_Curr = // 0.25; // çà 1 ñåê íàêàïëèâàåòñÿ 2500 èìïóëüñîâ + /*r. + //0.000100; // êîýôô., ïðè êîòîðîì çà 1 ñåê íàêàïëèâàåòñÿ 1 èìïóëüñ + //1.0001; // êîýôô., ïðè êîòîðîì çà 1 ñåê íàêàïëèâàåòñÿ 10001 èìïóëüñ + // 0.000125; // êîýôô., ïðè êîòîðîì çà 1 ñåê íàêàïëèâàåòñÿ 1.25 èìïóëüñà (çà 100 âûâîäèòñÿ 122 èìï.???) + // 0.000105; // çà 100 ñåê ä.íàêàïëèâàòüñÿ 105 èìï., íàêàïë. 103??? + */ + + // TermoCompens_Curr = LONG_2_FRACT_14_18(TermoCompens_Curr); //r. TermoCompens_Curr ïåðåâîäèì â ôîðìàò 14.18 + + return TermoCompens_Curr; +} // StaticTermoCompens + +/****************************************************************************** +** Function name: DynamicDeltaCalc +** +** Descriptions: Procedure of +** +** parameters: None +** Returned value: Thermocompensation addition +** +******************************************************************************/ +int DynamicDeltaCalc() //e. calculation the addition termocompensation for 1 reset //r. ðàñ÷åò äîáàâêè òåðìîêîìïåíñàöèè íà îäíî îáíóëåíèå +{ + + int i, t; + + t = Temp_Aver; + + if (IsHeating) + { + if(t > Device_blk.Str.TemperIntDyn[TERMO_FUNC_SIZE - 1]) + { + t = Device_blk.Str.TemperIntDyn[TERMO_FUNC_SIZE - 1]; + } + + i = 0; + while( t > Device_blk.Str.TemperIntDyn[i] ) i++; + + TermoCompDelta = ( Device_blk.Str.ThermoHeatDelta[i] - dThermoHeatDeltaPer_dTermo[i] * (float)( Device_blk.Str.TemperIntDyn[i] - t ) ); + } + else + { + if(t > Device_blk.Str.TemperCoolIntDyn[TERMO_FUNC_SIZE - 1]) + { + t = Device_blk.Str.TemperCoolIntDyn[TERMO_FUNC_SIZE - 1]; + } + + i = 0; + while( t > Device_blk.Str.TemperCoolIntDyn[i] ) i++; + + TermoCompDelta = ( Device_blk.Str.ThermoCoolDelta[i] - dThermoCoolDeltaPer_dTermo[i] * (float)( Device_blk.Str.TemperCoolIntDyn[i] - t ) ); + } + return TermoCompDelta; + +} // DynamicDeltaCalc + +/****************************************************************************** +** Function name: clc_ThermoSensors +** +** Descriptions: Procedure of calculating of the normalized temperaturre vector +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void clc_ThermoSensors(void) +{ + unsigned i; + static int TS_sum = 0; + static int seconds_aver = 0, TenSeconds = 0; + static int Temp_AverPrevDynCalc = -7000; + static int StartRdy = 1; + static int PrevTemp = -7000; + + for (i=0; i<2; i++) + { + //e. conversion of temperature values on ADC output //r. ïðåîáðàçîâàíèå çíà÷åíèé òåìïåðàòóðû íà âûõîäå ÀÖÏ + //e. to range -32768 .. +32767 ( additional code; format 1.15 ) //r. ê äèàïàçîíó -32768 .. +32767 (äîïîëíèò. êîä; ôîðìàò 1.15) + /* Output.Str.Tmp_Out[i] = mac_r(Device_blk.Str.Tmp_bias[i] << 16, + (Input.StrIn.Tmp_in[i] - 0x8000), + Device_blk.Str.Tmp_scal[i]);*/ + Output.Str.Tmp_Out[i+4] = Input.StrIn.Tmp_in[i]; + } + + if (time_1_Sec == DEVICE_SAMPLE_RATE_uks) //r. èñòåêëà 1 ñåêóíäà + { + seconds_aver++; + } + + if (seconds_aver > TEMP_AVER_PERIOD) //r. èñòåêëè TEMP_AVER_PERIOD(4 ñåêóíäû) ñåêóíä + { + seconds_aver = 0; + TenSeconds++; + PrevTemp = Temp_Aver; //e. save the previous mean temperature for 1 Sec //r. çàïîìèíàåì ïðåäûäóùóþ ñðåäíþþ òåìïåðàòóðó çà ñåêóíäó + Temp_Aver = TS_sum / (DEVICE_SAMPLE_RATE_HZ * TEMP_AVER_PERIOD); //e. calculating mean temperature for 1 Sec //r. âû÷èñëÿåì ñðåäíþþ òåìïåðàòóðó çà ñåêóíäó + + if (Temp_Aver > PrevTemp) + { + TempEvolution++; + } + if (Temp_Aver < PrevTemp) + { + TempEvolution--; + } + + TS_sum = 0; //e. reset the sum for calculation of an mean //r. îáíóëÿåì ñóììó äëÿ âû÷èñëåíèÿ ñðåäíåãî + } + else + { + TS_sum += Output.Str.Tmp_Out[TSENS_NUMB]; + } + + if (TenSeconds == 10) // 10 * TEMP_AVER_PERIOD = 40 + { + TenSeconds = 0; + if (TempEvolution > 0) + { + IsHeating = 1; + } + + if (TempEvolution < 0) + { + IsHeating = 0; + } + TempEvolution = 0; + } + + //e. single calculaiton of some device parameters (measurement on the VALID_START_SEC second after start) //r. îäíîêðàòíûé ðàñ÷åò íåêîòîðûõ ïàðàìåòðîâ ïðèáîðà (èçìåðåíèå íà VALID_START_SEC ñåêóíäå ïîñëå ñòàðòà) + if (StartRdy) + { + if (TenSeconds > VALID_START_4SEC) + { + + StartRdy = 0; //r. ñàìîáëîêèðîâêà, ïîýòîìó áîëüøå ñþäà íå çàõîäèì + + if ((Device_blk.Str.TermoMode != TERMO_OFF) && \ + (Device_blk.Str.TermoMode != TERMO_ON_DYNAMIC_ONLY) && \ + (Device_blk.Str.TermoMode != TERMO_ON_DYNAMIC_ONLY_NUMB_OFF)) //r. ñòàòè÷åñêàÿ òåðìîêîìïåíñàöèÿ âêëþ÷åíà + { + //r. ðàñ÷åò ñòàòè÷åñêîé òåðìîêîìïåíñàöèè ÷èñëà + StartTermoCompens = StaticTermoCompens(Temp_Aver); //r. ñòàðòîâàÿ òåìïåðàòóðû ïðèáîðà + } + DynamicDeltaCalc(); + + //r. ðàñ÷åò ãðàíèö äëÿ ÷àñòîòû âèáðîïðèâîäà, çàâèñÿùåãî îò ñòàðòîâîé òåìïåðàòóðû + // DithFreqRangeCalc(); + + //r. êîýôôèöèåíòû äëÿ ïîëîñîâîãî ôèëüòðà êâàçè ÄÓÏ íå ïåðåñ÷èòûâàåì: ñ÷èòàåì, ÷òî ïîëîñà ôèëüòðà çàâåäîìî øèðå + + + //r. íàïðÿæåíèå ñáðîñà ïðè íàãðåâàíèè + WP_reset_heating = CPL_reset_calc(Device_blk.Str.WP_reset, Device_blk.Str.K_WP_rst_heating, Temp_Aver, Device_blk.Str.TemperNormal); + //e. voltage of reset at cooling //r. íàïðÿæåíèå ñáðîñà ïðè îõëàæäåíèè + WP_reset_cooling = CPL_reset_calc(Device_blk.Str.WP_reset2, Device_blk.Str.K_WP_rst_cooling, Temp_Aver, Device_blk.Str.TemperNormal); + } + } + + if ( abs(Temp_Aver - Temp_AverPrevDynCalc) > Device_blk.Str.DeltaTempRecalc) + { + Temp_AverPrevDynCalc = Temp_Aver; + DynamicDeltaCalc(); + }//r. ðàñ÷åò ñðåäíåé çà 1 ñåêóíäó òåìïåðàòóðû äàò÷èêîâ T4, T5 + + // cyclic built-in test + if ( (Output.Str.Tmp_Out[4] < TS_MIN) || (Output.Str.Tmp_Out[4] > TS_MAX) || (Output.Str.Tmp_Out[5] < TS_MIN) || (Output.Str.Tmp_Out[5] > TS_MAX) ) + { + Valid_Data |= THERMO_RANGE_ERROR; + } + + if ( abs( Output.Str.Tmp_Out[4] - Output.Str.Tmp_Out[5]) > TS_DIFF_MAX) + { + Valid_Data |= THERMO_DIFF_ERROR; + } +} // clc_ThermoSensors + + +/****************************************************************************** +** Function name: clc_PLC +** +** Descriptions: Procedure of initial processing for the CPLC regulator +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void DithFreqRangeCalc(void) //r. ðàñ÷åò ãðàíèö êîýôôèöèåíòà äåëåíèÿ äëÿ ÷àñòîòû âèáðîïðèâîäà, çàâèñÿùèõ îò òåêóùåé òåìïåðàòóðû +{ + + unsigned int min_level, max_level; + int delta_VB_N; + + delta_VB_N = mult_r(Device_blk.Str.K_vb_tu >> DITH_VBN_SHIFT, (Temp_Aver - Device_blk.Str.TemperNormal)); //r.200; + //r. !!! ñäåëàòü ñóììèðîâàíèå ñ íàñûùåíèåì, à çàòåì ñäâèã + min_level = VB_Nmin0 + delta_VB_N; + max_level = VB_Nmax0 + delta_VB_N; + // maximum saturation for unsigned levels + Max_Saturation(&min_level, ((unsigned int)0xFFFF >> DITH_VBN_SHIFT)-1); // min should be always less then max_level by 1 + Max_Saturation(&max_level, ((unsigned int)0xFFFF >> DITH_VBN_SHIFT)); + Device_blk.Str.VB_Nmin = min_level << DITH_VBN_SHIFT; + Device_blk.Str.VB_Nmax = max_level << DITH_VBN_SHIFT; + +} // DithFreqRange_calc +