328 lines
7.2 KiB
C
328 lines
7.2 KiB
C
/*
|
|
* chka_ieee.c
|
|
*
|
|
* Created on: 08-08-2018
|
|
* Author: Krzysztof Jakubczyk
|
|
*/
|
|
|
|
#include "tdefs.h"
|
|
#include "misc.h"
|
|
#include "helper.h"
|
|
|
|
#include "chka_ieee.h"
|
|
#include <math.h>
|
|
|
|
int chka_ieee_initlog(void *arguments, void *logic)
|
|
{
|
|
struct chka_ieee_args *args = (struct chka_ieee_args *)arguments;
|
|
struct chka_ieee_logic *log = (struct chka_ieee_logic *)logic;
|
|
|
|
if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.test_in,&log->test))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.zezw_l1_in,&log->zezw[0]))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.zezw_l2_in,&log->zezw[1]))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.zezw_l3_in,&log->zezw[2]))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.pl1_out,&log->P[0]))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.pl2_out,&log->P[1]))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.pl3_out,&log->P[2]))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.p3f_out,&log->P[3]))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.w3f_out,&log->W))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.z3f_out,&log->Z))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.relax_out,&log->relax))
|
|
return -1;
|
|
|
|
if(set_float_ptr(args->io.wel1_float_in,&log->We[0]))
|
|
return -1;
|
|
if(set_float_ptr(args->io.wel2_float_in,&log->We[1]))
|
|
return -1;
|
|
if(set_float_ptr(args->io.wel3_float_in,&log->We[2]))
|
|
return -1;
|
|
|
|
log->Wn = args->params.Wn * args->params.Wn;
|
|
log->Wn_hyst = log->Wn * args->params.kp * args->params.kp;
|
|
|
|
/// predefined curves
|
|
if(args->params.typ_chki == CHKA_INVERSE)
|
|
{
|
|
args->params.typ_chki=CHKA_SIL_ZAL;
|
|
args->params.k=0.14;
|
|
args->params.c=0;
|
|
args->params.alfa=0.02;
|
|
args->params.tr=0;
|
|
}
|
|
else if(args->params.typ_chki == CHKA_VERY_INVERSE)
|
|
{
|
|
args->params.typ_chki=CHKA_SIL_ZAL;
|
|
args->params.k=13.5;
|
|
args->params.c=0;
|
|
args->params.alfa=1;
|
|
args->params.tr=0;
|
|
}
|
|
else if(args->params.typ_chki == CHKA_EXTREMELY_INVERSE)
|
|
{
|
|
args->params.typ_chki=CHKA_SIL_ZAL;
|
|
args->params.k=80;
|
|
args->params.c=0;
|
|
args->params.alfa=2;
|
|
args->params.tr=0;
|
|
}
|
|
else if(args->params.typ_chki == CHKA_IEEE_MODERATELY_INVERSE)
|
|
{
|
|
args->params.typ_chki=CHKA_IDMT;
|
|
args->params.k=0.0515;
|
|
args->params.c=0.1140;
|
|
args->params.alfa=0.02;
|
|
args->params.tr=4.85;
|
|
log->ieee_mi=1;
|
|
}
|
|
else if(args->params.typ_chki == CHKA_IEEE_VERY_INVERSE)
|
|
{
|
|
args->params.typ_chki=CHKA_IDMT;
|
|
args->params.k=19.61;
|
|
args->params.c=0.491;
|
|
args->params.alfa=2;
|
|
args->params.tr=21.6;
|
|
}
|
|
else if(args->params.typ_chki == CHKA_IEEE_EXTREMELY_INVERSE)
|
|
{
|
|
args->params.typ_chki=CHKA_IDMT;
|
|
args->params.k=28.2;
|
|
args->params.c=0.1217;
|
|
args->params.alfa=2;
|
|
args->params.tr=29.1;
|
|
}
|
|
///
|
|
|
|
if(args->params.typ_chki == CHKA_STALA)
|
|
log->t0=(u32)(args->params.c*1000);
|
|
else
|
|
log->t0=(u32)(args->params.c*1000*args->params.TMS);
|
|
|
|
log->kTMS=args->params.TMS*args->params.k;
|
|
log->trTMS=args->params.TMS*args->params.tr;
|
|
|
|
log->t_blok_max = (u32)(args->params.t_blok * 1000);
|
|
log->t_blok = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void chka_ieee(void *arguments, void *logic)
|
|
{
|
|
struct chka_ieee_logic *log = (struct chka_ieee_logic *)logic;
|
|
struct chka_ieee_args *args = (struct chka_ieee_args *)arguments;
|
|
float tplus_value;
|
|
u8 w,i;
|
|
|
|
if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked
|
|
{
|
|
log->t_blok = 0;
|
|
|
|
for(i=0;i<3;i++)
|
|
{
|
|
if(!check_struct(&log->zezw[i]))
|
|
{
|
|
log->dw.p[i]=0;
|
|
log->dw.w[i]=0;
|
|
log->dw.t[i]=0;
|
|
log->dw.lp[i]=0;
|
|
log->dw.lp2[i]=0;
|
|
continue;
|
|
}
|
|
|
|
w=0;
|
|
|
|
sprawdz_P(&log->dw.p[i],
|
|
*log->We[i]>log->Wn,
|
|
*log->We[i]<log->Wn_hyst,
|
|
&log->dw.lp[i],
|
|
3,3);
|
|
|
|
if(log->dw.p[i])
|
|
{
|
|
if(args->params.typ_chki == CHKA_SIL_ZAL)
|
|
{
|
|
tplus_value=sqrtf(*log->We[i]);
|
|
tplus_value/=args->params.Wn;
|
|
tplus_value=powf(tplus_value,(float)args->params.alfa);
|
|
tplus_value-=1;
|
|
|
|
if(!tplus_value || tplus_value<0)
|
|
tplus_value=0.0000001;
|
|
tplus_value=log->kTMS/tplus_value;
|
|
tplus_value*=(1000/LOOP_CYCLE_MS);
|
|
|
|
if(tplus_value)
|
|
{
|
|
tplus_value=1000000000/tplus_value;
|
|
log->dw.t[i]+=tplus_value;
|
|
}
|
|
else
|
|
log->dw.t[i]=1000000000;
|
|
|
|
|
|
if(log->dw.t[i]>=1000000000)
|
|
{
|
|
log->dw.t[i]=1000000000;
|
|
w=1;
|
|
}
|
|
else
|
|
w=0;
|
|
}
|
|
else if(args->params.typ_chki == CHKA_STALA)
|
|
{
|
|
w=1;
|
|
}
|
|
else if(args->params.typ_chki == CHKA_IDMT)
|
|
{
|
|
tplus_value=sqrtf(*log->We[i]);
|
|
tplus_value/=args->params.Wn;
|
|
tplus_value=powf(tplus_value,(float)args->params.alfa);
|
|
tplus_value-=1;
|
|
|
|
if(!tplus_value || tplus_value<0)
|
|
tplus_value=0.0000001;
|
|
tplus_value=log->kTMS/tplus_value;
|
|
tplus_value*=(1000/LOOP_CYCLE_MS);
|
|
|
|
if(tplus_value)
|
|
{
|
|
tplus_value=1000000000/tplus_value;
|
|
log->dw.t[i]+=tplus_value;
|
|
}
|
|
else
|
|
log->dw.t[i]=1000000000;
|
|
|
|
|
|
if(log->dw.t[i]>=1000000000)
|
|
{
|
|
log->dw.t[i]=1000000000;
|
|
w=1;
|
|
}
|
|
else
|
|
w=0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(args->params.typ_chki == CHKA_IDMT)
|
|
{
|
|
tplus_value=sqrtf(*log->We[i]);
|
|
tplus_value/=args->params.Wn;
|
|
if(log->ieee_mi)
|
|
tplus_value=powf(tplus_value,(float)2.00);
|
|
else
|
|
tplus_value=powf(tplus_value,(float)args->params.alfa);
|
|
|
|
tplus_value=(1-tplus_value);
|
|
|
|
if(!tplus_value || tplus_value<0)
|
|
tplus_value=0.0000001;
|
|
tplus_value=log->trTMS/tplus_value;
|
|
tplus_value*=(1000/LOOP_CYCLE_MS);
|
|
|
|
if(tplus_value)
|
|
{
|
|
tplus_value=1000000000/tplus_value;
|
|
if(tplus_value>=(float)log->dw.t[i])
|
|
log->dw.t[i]=0;
|
|
else
|
|
log->dw.t[i]-=tplus_value;
|
|
}
|
|
else
|
|
log->dw.t[i]=0;
|
|
}
|
|
else
|
|
log->dw.t[i]=0;
|
|
}
|
|
|
|
if(w)
|
|
{
|
|
if(log->dw.lp2[i]>log->t0)
|
|
log->dw.w[i]=1;
|
|
else
|
|
log->dw.lp2[i]+=LOOP_CYCLE_MS;
|
|
}
|
|
else
|
|
{
|
|
log->dw.lp2[i]=0;
|
|
log->dw.w[i]=0;
|
|
}
|
|
}
|
|
|
|
if(args->params.bity & 0x00000004) // OR logic
|
|
{
|
|
log->dw.w[3]=log->dw.w[0]||log->dw.w[1]||log->dw.w[2];
|
|
log->dw.p[3]=log->dw.p[0]||log->dw.p[1]||log->dw.p[2];
|
|
}
|
|
else
|
|
{
|
|
log->dw.w[3]=log->dw.w[0]&&log->dw.w[1]&&log->dw.w[2];
|
|
log->dw.p[3]=log->dw.p[0]&&log->dw.p[1]&&log->dw.p[2];
|
|
}
|
|
|
|
|
|
if(czy_test_R() && check_struct(&log->test))
|
|
{
|
|
log->dw.p[0]=1;
|
|
log->dw.p[1]=1;
|
|
log->dw.p[2]=1;
|
|
log->dw.p[3]=1;
|
|
log->dw.w[0]=1;
|
|
log->dw.w[1]=1;
|
|
log->dw.w[2]=1;
|
|
log->dw.w[3]=1;
|
|
}
|
|
|
|
|
|
check_and_set_struct(log->dw.p[0],&log->P[0]);
|
|
check_and_set_struct(log->dw.p[1],&log->P[1]);
|
|
check_and_set_struct(log->dw.p[2],&log->P[2]);
|
|
check_and_set_struct(log->dw.p[3],&log->P[3]);
|
|
check_and_set_struct(log->dw.w[3],&log->Z);
|
|
|
|
if(args->params.bity & 0x00000008) // na wylacz
|
|
{
|
|
check_and_set_struct(log->dw.w[3],&log->W);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (log->t_blok < log->t_blok_max)
|
|
{
|
|
log->t_blok += LOOP_CYCLE_MS;
|
|
}
|
|
else
|
|
{
|
|
for(i=0;i<3;i++)
|
|
{
|
|
log->dw.p[i]=0;
|
|
log->dw.w[i]=0;
|
|
log->dw.t[i]=0;
|
|
log->dw.lp2[i]=0;
|
|
}
|
|
}
|
|
|
|
|
|
clear_struct(&log->P[0]);
|
|
clear_struct(&log->P[1]);
|
|
clear_struct(&log->P[2]);
|
|
clear_struct(&log->P[3]);
|
|
clear_struct(&log->W);
|
|
clear_struct(&log->Z);
|
|
}
|
|
|
|
check_and_set_struct(!log->dw.t[0]&&!log->dw.t[1]&&!log->dw.t[2] && !((args->params.typ_chki == CHKA_STALA)&&w),&log->relax);
|
|
}
|