Почист
This commit is contained in:
268
isolatio.c
Normal file
268
isolatio.c
Normal file
@@ -0,0 +1,268 @@
|
||||
#include "DSP2833x_Device.h" // DSP2833x Headerfile Include File
|
||||
#include "DSP2833x_SWPrioritizedIsrLevels.h"
|
||||
|
||||
#include "RS485.h"
|
||||
#include "filter_bat2.h"
|
||||
#include "measure.h"
|
||||
#include "message.h"
|
||||
#include "package.h"
|
||||
#include "peripher.h"
|
||||
#include "crc16.h"
|
||||
#include "isolatio.h"
|
||||
#include <math.h> // Ýòî ÷òîáû ìåðèòü àìïëèòóäó! sqrt áåç ýòîãî áóäåò êðèâ!!!
|
||||
|
||||
OPTOCANAL opt[2];
|
||||
|
||||
ISOLATION iso[2];
|
||||
|
||||
float iso_KOEF[2] = {-0.5, -0.5};
|
||||
|
||||
void init_isolation_struct(void);
|
||||
|
||||
void DCLK(int i, int x)
|
||||
{
|
||||
x=!x;
|
||||
|
||||
if(i)
|
||||
{
|
||||
if(x) GpioDataRegs.GPASET.bit.GPIO26=1;
|
||||
else GpioDataRegs.GPACLEAR.bit.GPIO26=1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(x) GpioDataRegs.GPBSET.bit.GPIO32=1;
|
||||
else GpioDataRegs.GPBCLEAR.bit.GPIO32=1;
|
||||
} }
|
||||
|
||||
int DIN(int i)
|
||||
{
|
||||
if(i) return !GpioDataRegs.GPBDAT.bit.GPIO52;
|
||||
else return !GpioDataRegs.GPADAT.bit.GPIO23;
|
||||
}
|
||||
|
||||
void BLIN(int i)
|
||||
{
|
||||
if(i) GpioDataRegs.GPBTOGGLE.bit.GPIO53=1;
|
||||
else GpioDataRegs.GPATOGGLE.bit.GPIO24=1;
|
||||
}
|
||||
|
||||
interrupt void cpu_timer1_isr_ISOL(void)
|
||||
{
|
||||
ERROR error;
|
||||
float Riso=0;
|
||||
long numb=0;
|
||||
int i;
|
||||
static unsigned int count_ready=0;
|
||||
|
||||
EALLOW;
|
||||
CpuTimer1.InterruptCount++;
|
||||
IER |= MINT13; // Set "global" priority
|
||||
EINT;
|
||||
EDIS; // This is needed to disable write to EALLOW protected registers
|
||||
|
||||
if(!cReset) ServiceDog();
|
||||
|
||||
if(++CanPowse >= CANPOWSE*2)
|
||||
{
|
||||
CanPowse = 0;
|
||||
CanGO = 1;
|
||||
}
|
||||
|
||||
if(++count_ready >= period_ready)
|
||||
{
|
||||
count_ready=0;
|
||||
|
||||
if((!sig.bit.Error)|(cTestLamp)) toggle_READY();
|
||||
else set_READY();
|
||||
}
|
||||
|
||||
for(i=0;i<2;i++)
|
||||
{
|
||||
iso[i].pause_counter++;
|
||||
|
||||
if(sens_error[i].bit.Bypas)
|
||||
{
|
||||
sens_error[i].all = 0;
|
||||
sens_error[i].bit.Bypas = 1;
|
||||
Modbus[i+DATASTART].all = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
if(opt[i].Wait)
|
||||
{
|
||||
opt[i].Wait--;
|
||||
opt[i].bit = 0;
|
||||
opt[i].clk = 0;
|
||||
DCLK(i,0);
|
||||
continue;
|
||||
}
|
||||
|
||||
opt[i].clk=!opt[i].clk;
|
||||
DCLK(i,opt[i].clk);
|
||||
if(!opt[i].clk)
|
||||
{
|
||||
opt[i].Numb = (opt[i].Numb<<1) | DIN(i);
|
||||
if(++opt[i].bit>=32)
|
||||
{
|
||||
error.all = 0;
|
||||
|
||||
opt[i].Wait = (TELE_FREQ/1000)*optopowse;
|
||||
opt[!i].Wait =(TELE_FREQ/2000)*optopowse;
|
||||
if(get_crc32(&(opt[i].Numb)))
|
||||
{
|
||||
numb = opt[i].Numb;
|
||||
numb = numb / 256; // óäàëàåì êîíòðîëüíóþ ñóììó
|
||||
|
||||
Riso=numb;
|
||||
/*
|
||||
if(ist[i]) { kff=1; ist[i]=0; }
|
||||
else kff = optofiltr;
|
||||
fRiso[i] += (Riso-fRiso[i])/kff;
|
||||
numb = (long)fRiso[i];
|
||||
*/
|
||||
Modbus[i*2+0x10].all = (int)(numb & 0xFFFF);
|
||||
Modbus[i*2+0x11].all = (int)(numb>>16);
|
||||
|
||||
Riso=numb;
|
||||
Riso = Riso/1024;//256; // ïðåäïîëîæèì
|
||||
|
||||
iso[i].adc_value = Riso; // íà Ëåøèí àëãîðèòì
|
||||
|
||||
Modbus[i+DATASTART].all = Riso; // êàêáå
|
||||
opt[i].ers = 0; BLIN(i);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(++opt[i].ers > 20)
|
||||
{
|
||||
opt[i].ers = 20;
|
||||
error.bit.Tear = 1;
|
||||
} }
|
||||
|
||||
reset_errs(i,error);
|
||||
|
||||
} } }
|
||||
|
||||
sig.all = chk.all;
|
||||
chk.all = 0;
|
||||
|
||||
}
|
||||
void timer_Init()
|
||||
{
|
||||
EALLOW; // This is needed to write to EALLOW protected registers
|
||||
PieVectTable.XINT13 = &cpu_timer1_isr_ISOL;
|
||||
EDIS; // This is needed to disable write to EALLOW protected registers
|
||||
|
||||
ConfigCpuTimer(&CpuTimer1, SYSCLKOUT/1000000, 1000000/TELE_FREQ);
|
||||
CpuTimer1Regs.TCR.all = 0x4020; // Use write-only instruction to set TSS bit = 0
|
||||
IER |= M_INT13;
|
||||
|
||||
period_ready = TELE_FREQ / (READY_FREQ * 2);
|
||||
|
||||
init_isolation_struct();
|
||||
Calcul_Iso_Koef();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void Calcul_Iso_Koef()
|
||||
{
|
||||
iso_KOEF[0] = (isoMOM[1]-isoMOM[0]);
|
||||
iso_KOEF[0]/= (isoADC[1]-isoADC[0]);
|
||||
|
||||
iso_KOEF[1] = (isoMOM[3]-isoMOM[2]);
|
||||
iso_KOEF[1]/= (isoADC[3]-isoADC[2]);
|
||||
}
|
||||
|
||||
void init_isolation_struct(void)
|
||||
{
|
||||
int i;
|
||||
int* pint = (int*)iso;
|
||||
int size = sizeof(ISOLATION)/sizeof(int);
|
||||
|
||||
for(i=0; i < size*2; i++) pint[i] = 0;
|
||||
|
||||
iso[1].pause_counter = ISO_PAUSE / 2;
|
||||
}
|
||||
|
||||
#define iso_time_pause 6200
|
||||
|
||||
void isolation_calc(void)
|
||||
{
|
||||
float our_delta;
|
||||
static int i;
|
||||
int k, l, m;
|
||||
int buff[BUFF_LEN];
|
||||
|
||||
for(i=0; i<2; i++)
|
||||
|
||||
if(iso[i].pause_counter > ISO_PAUSE) //Ïîñêîëüêó âñ¸ ìåäëåííî, ñîõðàíàþ çíà÷åíèà ðàç 3 ñåêóíäû
|
||||
{ //÷òîáû íåìüøå îòñ÷¸òîâ ïîïàäàëî, íàâûáðîñû
|
||||
iso[i].pause_counter = 0;
|
||||
|
||||
iso[i].buff[iso[i].buff_position] = iso[i].adc_value;
|
||||
|
||||
// Ýòî èäåò ñîðòèðîâêà âñåãî ìàññèâà äàííûõ ---------------------------
|
||||
|
||||
for (k = 0; k < BUFF_LEN; k++) buff[k] = 0x8000; //minimal int
|
||||
for (k = 0; k <=iso[i].prim_position; k++)
|
||||
{
|
||||
for (l = 0; (l < BUFF_LEN) && (iso[i].buff[k] < buff[l]); l++);
|
||||
if (l < BUFF_LEN)
|
||||
{
|
||||
for (m = BUFF_LEN - 1; (m > l) && (m > 0); m--) buff[m] = buff[m-1];
|
||||
buff[l] = iso[i].buff[k];
|
||||
} }
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
m = (buff[0]+buff[iso[i].prim_position])/2;
|
||||
|
||||
for(k=0; k < iso[i].prim_position; k++) if( buff[k] <= m) break;
|
||||
|
||||
iso[i].max_val = buff[k - 1 - iso[i].cycles];
|
||||
|
||||
iso[i].min_val = buff[k + iso[i].cycles];
|
||||
|
||||
/* Òàê áûëî
|
||||
iso[i].max_val = buff[iso[i].cycles];
|
||||
iso[i].min_val = buff[iso[i].prim_position - iso[i].cycles];
|
||||
*/
|
||||
our_delta = iso[i].max_val - iso[i].min_val;
|
||||
|
||||
|
||||
// Íîâûé àëãîðèòì ñ÷åòà ÌÎìîâ -------------------------------------------
|
||||
|
||||
if(!iso[i].cycles)
|
||||
iso[i].MOms_x_10 = isoMOM[i*2+1];
|
||||
else
|
||||
iso[i].MOms_x_10 = (our_delta-isoADC[i*2]) * iso_KOEF[i] + isoMOM[i*2];
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
Modbus[26+i].all = iso[i].MOms_x_10;
|
||||
Modbus[32+i].all = iso[i].adc_value;
|
||||
Modbus[40+i].all = iso[i].max_val;
|
||||
Modbus[48+i].all = iso[i].min_val;
|
||||
Modbus[56+i].all = our_delta;
|
||||
Modbus[64+i].all = iso[i].prim_position;
|
||||
Modbus[72+i].all = iso[i].buff_position;
|
||||
Modbus[80+i].all = iso[i].cycles;
|
||||
|
||||
//for(j=0;j<35;j++) Modbus[0x20+j].all = iso[i].buff[j];
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
iso[i].buff_position++;
|
||||
|
||||
if(iso[i].cycles < BUFF_CYCLES)
|
||||
{
|
||||
iso[i].prim_position = iso[i].buff_position;
|
||||
iso[i].cycles = iso[i].prim_position / CYCLE_LEN;
|
||||
}
|
||||
else
|
||||
iso[i].prim_position = BUFF_LEN-1;
|
||||
|
||||
if (iso[i].buff_position >= BUFF_LEN) iso[i].buff_position = 0;
|
||||
} }
|
||||
|
||||
Reference in New Issue
Block a user