269 lines
5.9 KiB
C
269 lines
5.9 KiB
C
#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;
|
|
} }
|
|
|