ms/dist_tester
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
c1794a2f1c02d8775926af279e4288feb2392570
dist_tester/src/RNT.c

838 lines
24 KiB
C
Raw Blame History

/*
* RNT.c
*
* Created on: 14-11-2018
* Author: Krzysztof Jakubczyk
*/
#include "tdefs.h"
#include "misc.h"
#include "RNT.h"
#include "analog_in.h"
/*#include "helper.h"
#include "ZDistL.h"
*/
#include <math.h>
int rnt_initlog(void *arguments, void *logic)
{
struct rnt_args *args = (struct rnt_args *)arguments;
struct rnt_logic *log = (struct rnt_logic *)logic;
if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl_))
return -1;
if(set_bit_ptr_struct(args->io.man_in,&log->man_))
return -1;
if(set_bit_ptr_struct(args->io.auto_in,&log->auto_))
return -1;
if(set_float_ptr(args->io.orta_U1_float_in,&log->orta_U1_))
return -1;
if(set_float_ptr(args->io.ortb_U1_float_in,&log->ortb_U1_))
return -1;
if(set_float_ptr(args->io.kwampl_U1_float_in,&log->kwampl_U1_))
return -1;
if(set_bit_ptr_struct(args->io.kon_U1_in,&log->kon_U1_))
return -1;
if(set_float_ptr(args->io.orta_I1_float_in,&log->orta_I1_))
return -1;
if(set_float_ptr(args->io.ortb_I1_float_in,&log->ortb_I1_))
return -1;
if(set_float_ptr(args->io.kwampl_I1_float_in,&log->kwampl_I1_))
return -1;
if(set_float_ptr(args->io.orta_U2_float_in,&log->orta_U2_))
return -1;
if(set_float_ptr(args->io.ortb_U2_float_in,&log->ortb_U2_))
return -1;
if(set_float_ptr(args->io.kwampl_U2_float_in,&log->kwampl_U2_))
return -1;
if(set_bit_ptr_struct(args->io.kon_U2_in,&log->kon_U2_))
return -1;
if(set_float_ptr(args->io.orta_I2_float_in,&log->orta_I2_))
return -1;
if(set_float_ptr(args->io.ortb_I2_float_in,&log->ortb_I2_))
return -1;
if(set_float_ptr(args->io.kwampl_I2_float_in,&log->kwampl_I2_))
return -1;
if(set_float_ptr(args->io.nr_zacz_in,&log->nr_zacz_))
return -1;
if(set_bit_ptr_struct(args->io.zo_zacz_in,&log->zo_zacz_))
return -1;
if(set_bit_ptr_struct(args->io.kon_u_nr_zacz_in,&log->kon_u_nr_zacz_))
return -1;
if(set_bit_ptr_struct(args->io.bieg_in,&log->bieg_))
return -1;
if(set_bit_ptr_struct(args->io.gora_in,&log->gora_))
return -1;
if(set_bit_ptr_struct(args->io.dol_in,&log->dol_))
return -1;
if(set_bit_ptr_struct(args->io.aktU1z_in,&log->aktU1z_))
return -1;
if(set_bit_ptr_struct(args->io.aktU2z_in,&log->aktU2z_))
return -1;
if(set_bit_ptr_struct(args->io.aktU3z_in,&log->aktU3z_))
return -1;
if(set_bit_ptr_struct(args->io.aktU4z_in,&log->aktU4z_))
return -1;
if(set_bit_ptr_struct(args->io.bl_gora_in,&log->bl_gora_))
return -1;
if(set_bit_ptr_struct(args->io.bl_dol_in,&log->bl_dol_))
return -1;
if(set_bit_ptr_struct(args->io.test_in,&log->test_))
return -1;
if(set_bit_ptr_struct(args->io.kas_in,&log->kas_))
return -1;
if(set_bit_ptr_struct(args->io.gora_out,&log->gora_out_))
return -1;
if(set_bit_ptr_struct(args->io.dol_out,&log->dol_out_))
return -1;
if(set_bit_ptr_struct(args->io.e_bieg_out,&log->e_bieg_out_))
return -1;
if(set_bit_ptr_struct(args->io.e_poz_out,&log->e_poz_out_))
return -1;
if(set_bit_ptr_struct(args->io.pob_G_out,&log->pob_G))
return -1;
if(set_bit_ptr_struct(args->io.pob_D_out,&log->pob_D))
return -1;
if(set_bit_ptr_struct(args->io.blok_out,&log->blok_out_))
return -1;
if(set_bit_ptr_struct(args->io.blok_trw_out,&log->blok_trw_out_))
return -1;
if(set_bit_ptr_struct(args->io.ster_poz_in,&log->ster_poz_))
return -1;
if(set_float_ptr(args->io.zad_zacz_in,&log->nr_zacz_zad))
return -1;
if(set_bit_ptr_struct(args->io.blok_trw_in,&log->blok_trw_))
return -1;
if(set_bit_ptr_struct(args->io.blad_trybu_out,&log->blad_trybu_out_))
return -1;
if(set_float_ptr(args->io.Uodn_float_out,&log->U_odn))
return -1;
if(set_float_ptr(args->io.tg_out,&log->tg_out))
return -1;
if(set_float_ptr(args->io.td_out,&log->td_out))
return -1;
if(set_pointer_in_ptr(args->io.I1_params_ptr_in,(u32 *)&log->I_params[0]))
return -1;
if(set_pointer_in_ptr(args->io.I2_params_ptr_in,(u32 *)&log->I_params[1]))
return -1;
if(set_pointer_in_ptr(args->io.U1_params_ptr_in,(u32 *)&log->U_params[0]))
return -1;
if(set_pointer_in_ptr(args->io.U2_params_ptr_in,(u32 *)&log->U_params[1]))
return -1;
////
//status_rej = oczekiwanie;
log->nast_.tryb = args->params.tryb;
log->nast_.t1 = ((args->params.t1) * 1000); //czas dzialania t1
log->nast_.t2 = ((args->params.t2) * 1000); //czas dzialania t2
log->nast_.dUzr = args->params.dUz; //napicie nieczuoci
log->nast_.dUzp = log->nast_.dUzr * args->params.kp; //napicie nieczuoci
log->nast_.Uz1 = args->params.Uz1 ; //napicie zadane nr 1
log->nast_.Uz2 = args->params.Uz2 ; //napicie zadane nr 2
log->nast_.Uz3 = args->params.Uz3 ; //napicie zadane nr 3
log->nast_.Uz4 = args->params.Uz4 ; //napicie zadane nr 4
log->nast_.t_ster_zacz = ((args->params.t_ster_zacz) * 1000); //czas sterowania przecznikiem zaczepw
log->nast_.t_poz_zacz = ((args->params.t_poz_zacz) * 1000); //czas odpowiedzi pozycji przecznika zaczepw
log->nast_.t_imp_ster = ((args->params.t_imp_ster) * 1000);// czas impulsu gra/d
log->nast_.on = ((args->params.akt & 0x0001)!=0) ? 1:0;
log->nast_.kon_biegu = ((args->params.akt & 0x0002)!=0) ? 1:0;
log->nast_.kon_poz_ster = ((args->params.akt & 0x0004)!=0) ? 1:0;
log->nast_.akt_kon_U1 = ((args->params.akt & 0x0008)!=0) ? 1:0;
log->nast_.akt_kon_U2 = ((args->params.akt & 0x0010)!=0) ? 1:0;
log->nast_.akt_kon_Uzacz = ((args->params.akt & 0x0020)!=0) ? 1:0;
log->nast_.akt_blok_biegu = ((args->params.akt & 0x0040)!=0) ? 1:0;
log->nast_.akt_blok_pozycja = ((args->params.akt & 0x0080)!=0) ? 1:0;
log->nast_.akt_mfazowe = ((args->params.akt & 0x0100)!=0) ? 1:0;
log->nast_.akt_komp1 = ((args->params.akt_komp1 & 0x0001)!=0) ? 1:0;
log->nast_.akt_komp2 = ((args->params.akt_komp2 & 0x0001)!=0) ? 1:0;
log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
log->dw.blokada_trw = 0;
log->dw.timer_blokad_pod_nap1 = 0;
log->dw.timer_blokad_pod_nap2 = 0;
log->mul[0]=(log->I_params[0]->znam_pierw / log->U_params[0]->znam_pierw);
log->mul[1]=(log->I_params[1]->znam_pierw / log->U_params[1]->znam_pierw);
log->nast_.R1 = args->params.R1 * log->mul[0];
log->nast_.X1 = - (args->params.X1 * log->mul[0]);
log->nast_.R2 = args->params.R2 * log->mul[1];
log->nast_.X2 = - (args->params.X2 * log->mul[1]);
return 0;
}
void rnt(void *arguments, void *logic)
{
//struct rnt_args *args = (struct rnt_args *)arguments;
struct rnt_logic *log = (struct rnt_logic *)logic;
float local_orta_U1_ ,local_ortb_U1_,local_orta_U2_ ,local_ortb_U2_, local_kwa_U_, local_Uz;
float local_U_;
//u8 wk,wk2,l_nrl;
u8 blok; //blokada
//check_struct(&log->stan_bl_) - sprawdzenie blokady
//check_and_set_struct(log->dw.,&log->gora_out_);
//set_struct(&log->gora_out_);
//log->dw.go
if(log->nast_.on) // jesli zabezpieczenie aktywne
{
log->w.kwampl_U1_ = *log->kwampl_U1_;
log->w.kwampl_U2_ = *log->kwampl_U2_;
log->w.kwampl_I1_ = *log->kwampl_I1_;
log->w.kwampl_I2_ = *log->kwampl_I2_;
if (log->nast_.akt_mfazowe)
{
log->w.orta_U1_ = *log->ortb_U1_;
log->w.orta_U2_ = *log->ortb_U2_;
log->w.ortb_U1_ = -*log->orta_U1_;
log->w.ortb_U2_ = -*log->orta_U2_;
log->w.orta_I1_ = *log->orta_I1_ * 1.73f;
log->w.orta_I2_ = *log->orta_I2_ * 1.73f;
log->w.ortb_I1_ = *log->ortb_I1_ * 1.73f;
log->w.ortb_I2_ = *log->ortb_I2_ * 1.73f;
}
else
{
log->w.orta_U1_ = *log->orta_U1_;
log->w.orta_U2_ = *log->orta_U2_;
log->w.ortb_U1_ = *log->ortb_U1_;
log->w.ortb_U2_ = *log->ortb_U2_;
log->w.orta_I1_ = *log->orta_I1_;
log->w.orta_I2_ = *log->orta_I2_;
log->w.ortb_I1_ = *log->ortb_I1_;
log->w.ortb_I2_ = *log->ortb_I2_;
}
log->w.nr_zacz_ = *log->nr_zacz_;
log->w.nr_zacz_zad = *log->nr_zacz_zad;
//sprawdzenie czy mona sterowa w dol
log->dw.blokada_ster_dol = (check_struct(&log->bl_dol_))!=0 ? 1:0;
//sprawdzenie czy mona sterowa w gore
log->dw.blokada_ster_gora = (check_struct(&log->bl_gora_))!=0 ? 1:0;
if (check_struct(&log->kas_))
{
//kasowanie
clear_struct(&log->e_bieg_out_);
clear_struct(&log->e_poz_out_);
}
if (check_struct(&log->kas_))
{
if (!check_struct(&log->blok_trw_))
{
log->dw.blokada_trw = 0;
}
}
if (check_struct(&log->blok_trw_))
{
log->dw.blokada_trw = 1;
}
check_and_set_struct(log->dw.blokada_trw, &log->blok_trw_out_);
if (check_struct(&log->man_) && (!check_struct(&log->ster_poz_)) && (!check_struct(&log->auto_)))
{
//tryb rczny
clear_struct(&log->blad_trybu_out_);
blok = check_struct(&log->stan_bl_);
check_and_set_struct( blok, &log->blok_out_);
//sprawdzenie blokady trwalej
blok |= check_struct(&log->blok_trw_out_);
clear_struct(&log->pob_G);
clear_struct(&log->pob_D);
*log->U_odn = 0;
*log->tg_out = 0;
*log->td_out = 0;
}
else
if (!check_struct(&log->man_)&&(!check_struct(&log->ster_poz_))&&(check_struct(&log->auto_)))
{
//tryb automatyczny
clear_struct(&log->blad_trybu_out_);
//sprawdzanie kompensacji linii 1
if (log->nast_.akt_komp1)
{
local_orta_U1_ = log->w.orta_U1_ - ((log->w.orta_I1_ * log->nast_.R1 - log->w.ortb_I1_ * log->nast_.X1));
local_ortb_U1_ = log->w.ortb_U1_ - ((log->w.orta_I1_ * log->nast_.X1 + log->w.ortb_I1_ * log->nast_.R1));
}
else
{
local_orta_U1_ = log->w.orta_U1_;
local_ortb_U1_ = log->w.ortb_U1_;
}
//sprawdzanie kompensacji linii 2
if (log->nast_.akt_komp2)
{
local_orta_U2_ = log->w.orta_U2_ - ((log->w.orta_I2_ * log->nast_.R2 - log->w.ortb_I2_ * log->nast_.X2));
local_ortb_U2_ = log->w.ortb_U2_ - ((log->w.orta_I2_ * log->nast_.X2 + log->w.ortb_I2_ * log->nast_.R2));
}
else
{
local_orta_U2_ = log->w.orta_U2_;
local_ortb_U2_ = log->w.ortb_U2_;
}
blok = check_struct(&log->stan_bl_);
//sprawdzenie blokady trwalej
blok |= check_struct(&log->blok_trw_out_);
switch(log->nast_.tryb) ///< nastawa trybu dziaania 0-U1 1-U2 2-redniaU1/U2 3-maxU1/U2
{
case U1:
//U1
local_kwa_U_= (local_orta_U1_ * local_orta_U1_ + local_ortb_U1_* local_ortb_U1_);
if (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1)
log->dw.timer_blokad_pod_nap1 = 0;
log->dw.timer_blokad_pod_nap1 +=LOOP_CYCLE_MS;;
//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
blok |=(log->dw.timer_blokad_pod_nap1 < TP_OP_BLOKADY_NAP);
break;
case U2:
//U2
local_kwa_U_= (local_orta_U2_ * local_orta_U2_ + local_ortb_U2_* local_ortb_U2_);
if (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2)
log->dw.timer_blokad_pod_nap2 = 0;
log->dw.timer_blokad_pod_nap2 +=LOOP_CYCLE_MS;;
//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
blok |=(log->dw.timer_blokad_pod_nap2 < TP_OP_BLOKADY_NAP);
//blok |= (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2);
break;
case sredniaU1U2:
//redniaU1/U2
local_kwa_U_ = (local_orta_U1_ * local_orta_U1_ + local_ortb_U1_* local_ortb_U1_);
local_kwa_U_ += (local_orta_U2_ * local_orta_U2_ + local_ortb_U2_* local_ortb_U2_);
local_kwa_U_ *= 0.5f;
//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
//blok |= (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2);
if (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1)
log->dw.timer_blokad_pod_nap1 = 0;
log->dw.timer_blokad_pod_nap1 +=LOOP_CYCLE_MS;;
blok |=(log->dw.timer_blokad_pod_nap1 < TP_OP_BLOKADY_NAP);
if (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2)
log->dw.timer_blokad_pod_nap2 = 0;
log->dw.timer_blokad_pod_nap2 +=LOOP_CYCLE_MS;;
blok |=(log->dw.timer_blokad_pod_nap2 < TP_OP_BLOKADY_NAP);
break;
case maxU1U2:
//max U1/U2
//blok |= (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1);
//blok |= (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2);
if (check_struct(&log->kon_U1_) && log->nast_.akt_kon_U1)
log->dw.timer_blokad_pod_nap1 = 0;
log->dw.timer_blokad_pod_nap1 +=LOOP_CYCLE_MS;;
blok |=(log->dw.timer_blokad_pod_nap1 < TP_OP_BLOKADY_NAP);
if (check_struct(&log->kon_U2_) && log->nast_.akt_kon_U2)
log->dw.timer_blokad_pod_nap2 = 0;
log->dw.timer_blokad_pod_nap2 +=LOOP_CYCLE_MS;;
blok |=(log->dw.timer_blokad_pod_nap2 < TP_OP_BLOKADY_NAP);
if ((log->w.kwampl_U1_)>(log->w.kwampl_U2_))
{
local_kwa_U_= (local_orta_U1_ * local_orta_U1_ + local_ortb_U1_* local_ortb_U1_);
}
else
{
local_kwa_U_= (local_orta_U2_ * local_orta_U2_ + local_ortb_U2_* local_ortb_U2_);
}
break;
default:
break;
}
//
blok |= (check_struct(&log->kon_u_nr_zacz_) && log->nast_.akt_kon_Uzacz);
check_and_set_struct( blok, &log->blok_out_);
//w local_kwa_U_ - kwadrat napicia odniesienia
local_kwa_U_ *= 0.5f;
local_U_ = sqrtf (local_kwa_U_);
*log->U_odn = local_kwa_U_;
//Wybr napicia Uz - napicie zadane
if (check_struct(&log->aktU1z_))
local_Uz = log->nast_.Uz1;
else
if (check_struct(&log->aktU2z_))
local_Uz = log->nast_.Uz2;
else
if (check_struct(&log->aktU3z_))
local_Uz = log->nast_.Uz3;
else
if (check_struct(&log->aktU4z_))
local_Uz = log->nast_.Uz4;
else
local_Uz = log->nast_.Uz1;
//porownanie war pobudzenia
//local_Uz - local_kwa_U_ ;
sprawdz_P(&log->dw.pob_D,
((local_Uz - local_U_ ) < (- log->nast_.dUzr)),
((local_Uz - local_U_ ) > (- log->nast_.dUzp)),
&log->dw.lp_D,
100,100
);
sprawdz_P(&log->dw.pob_G,
((local_Uz - local_U_ ) > log->nast_.dUzr),
((local_Uz - local_U_ ) < log->nast_.dUzp),
&log->dw.lp_G,
100,100
);
check_and_set_struct((log->dw.pob_G > 0)&&(!blok)&&(!log->dw.blokada_ster_gora),&log->pob_G);
check_and_set_struct((log->dw.pob_D > 0)&&(!blok)&&(!log->dw.blokada_ster_dol),&log->pob_D);
}
else
if (!check_struct(&log->man_)&&(check_struct(&log->ster_poz_))&&(!check_struct(&log->auto_)))
{
//tryb nr zaczepu
clear_struct(&log->blad_trybu_out_);
//jesli zadany jest nr zaczepu
blok = check_struct(&log->stan_bl_);
blok |= (check_struct(&log->kon_u_nr_zacz_) && log->nast_.akt_kon_Uzacz);
check_and_set_struct( blok, &log->blok_out_);
//sprawdzenie blokady trwalej
blok |= check_struct(&log->blok_trw_out_);
//(check_struct(&log->ster_poz_)) == 1
if (((log->w.nr_zacz_zad) < (log->w.nr_zacz_)) && (!log->dw.blokada_ster_dol) && (!blok))
{
log->dw.pob_D = 1;
log->dw.pob_G = 0;
}
else
if (((log->w.nr_zacz_zad) > (log->w.nr_zacz_)) && (!log->dw.blokada_ster_gora) && (!blok))
{
log->dw.pob_D = 0;
log->dw.pob_G = 1;
}
else
{
log->dw.pob_D = 0;
log->dw.pob_G = 0;
}
check_and_set_struct((log->dw.pob_G > 0)&&(!blok)&&(!log->dw.blokada_ster_gora) ,&log->pob_G);
check_and_set_struct((log->dw.pob_D > 0)&&(!blok)&&(!log->dw.blokada_ster_dol) ,&log->pob_D);
}
else
{
//blad wyboru rodzaju pracy
clear_struct(&log->pob_D);
clear_struct(&log->pob_G);
set_struct( &log->blok_out_);
set_struct(&log->blad_trybu_out_);
}
//
switch (log->_stan_man)
{
case CZEKANIE_NA_STEROWANIE_MANUAL:
if (!check_struct(&log->man_))
{
//sprawdzenie czy nie przeczy na AUTO
log->_stan_man = CZEKANIE_NA_STEROWANIE_AUTO;
log->licz_t1G = log->licz_t2G = 0;
log->licz_t1D = log->licz_t2D = 0;
}
else
{
if(check_struct(&log->gora_) && (!check_struct(&log->dol_)) && (!log->dw.blokada_ster_gora) && (!blok))
{
*log->tg_out = 0;
*log->td_out = 0;
log->licznik_ster_gora+=LOOP_CYCLE_MS;
if (log->licznik_ster_gora > T_STER_RECZNEGO)
{
log->_stan_man = STEROWANIE_W_GORE;
log->licznik_ster_gora =0;
}
log->licznik_ster_dol =0;
}
else
//log->dw.pob_G;
if(check_struct(&log->dol_) && (!check_struct(&log->gora_)) && (!log->dw.blokada_ster_dol) && (!blok))
{
log->licznik_ster_dol+=LOOP_CYCLE_MS;
if (log->licznik_ster_dol > T_STER_RECZNEGO)
{
log->_stan_man = STEROWANIE_W_DOL;
log->licznik_ster_dol =0;
}
log->licznik_ster_gora =0;
}
else
{
log->licznik_ster_dol = 0;
log->licznik_ster_gora = 0;
clear_struct(&log->gora_out_);
clear_struct(&log->dol_out_);
}
}
break;
case CZEKANIE_NA_STEROWANIE_NR_POZYCJI:
if (!check_struct(&log->ster_poz_))
{
//sprawdzenie czy nie przeczy na AUTO
log->_stan_man = CZEKANIE_NA_STEROWANIE_AUTO;
log->licz_t1G = log->licz_t2G = 0;
log->licz_t1D = log->licz_t2D = 0;
}
else
{
if(log->dw.pob_G && (!log->dw.pob_D) && (!log->dw.blokada_ster_gora) && (!blok))
{
//(check_struct(&log->ster_poz_)) == 1
//gora nr pozycji
if (log->licz_t2G < log->nast_.t2)
{
log->licz_t2G += LOOP_CYCLE_MS;
}
else
{
log->_stan_man = STEROWANIE_W_GORE;
log->licz_t2G = 0;
}
*log->tg_out = (float)(log->nast_.t2 - log->licz_t2G) * 0.001f;
*log->td_out = 0;
}
else
if((log->dw.pob_D) && (!log->dw.pob_G) && (!log->dw.blokada_ster_dol) && (!blok))
{
//dol nr pozycji
if (log->licz_t2D < log->nast_.t2)
{
log->licz_t2D += LOOP_CYCLE_MS;
}
else
{
log->_stan_man = STEROWANIE_W_DOL;
log->licz_t2D = 0;
}
*log->td_out = (float)(log->nast_.t2 - log->licz_t2D) * 0.001f;
*log->tg_out = 0;
}
}
break;
case CZEKANIE_NA_STEROWANIE_AUTO:
if (check_struct(&log->man_))
{
//sprawdzenie czy nie przeczy na MANUAL
log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
}
else
if (check_struct(&log->ster_poz_))
{
//sprawdzenie czy nie przeczy na NR_POZ
log->_stan_man = CZEKANIE_NA_STEROWANIE_NR_POZYCJI;
}
else
if (log->dw.pob_G && (!log->dw.pob_D) && (!log->dw.blokada_ster_gora)&& (!blok))
{
//gora auto
if (log->licz_t1G < log->nast_.t1)
{
log->licz_t1G += LOOP_CYCLE_MS * ((local_Uz - local_U_ ) / log->nast_.dUzr);
}
else
if (log->licz_t2G < log->nast_.t2)
{
log->licz_t2G += LOOP_CYCLE_MS;
}
else
{
log->_stan_man = STEROWANIE_W_GORE;
log->licz_t1G = log->licz_t2G = 0;
}
*log->tg_out = (float)((log->nast_.t1 + log->nast_.t2 - log->licz_t1G - log->licz_t2G) * 0.001f);
*log->td_out = 0;
}
else
if(log->dw.pob_D && (!log->dw.pob_G) && (!log->dw.blokada_ster_dol)&& (!blok))
{
//dol auto
if (log->licz_t1D < log->nast_.t1)
{
log->licz_t1D += LOOP_CYCLE_MS *((local_Uz - local_U_ ) / -log->nast_.dUzr);
}
else
if (log->licz_t2D < log->nast_.t2)
{
log->licz_t2D += LOOP_CYCLE_MS;
}
else
{
log->_stan_man = STEROWANIE_W_DOL;
log->licz_t1D = log->licz_t2D = 0;
}
*log->td_out = (log->nast_.t1 + log->nast_.t2 - log->licz_t1D - log->licz_t2D) * 0.001f;
*log->tg_out = 0;
}
else
{
*log->td_out = 0;
*log->tg_out = 0;
log->licz_t1G = log->licz_t2G = 0;
log->licz_t1D = log->licz_t2D = 0;
clear_struct(&log->gora_out_);
clear_struct(&log->dol_out_);
}
break;
//gora
case STEROWANIE_W_GORE:
*log->td_out = 0;
*log->tg_out = 0;
set_struct(&log->gora_out_);
log->_stan_man = IMPULS_W_GORE;
log->licznik_ster_biegu = 0;
log->licznik_ster_gora = 0;
log->nr_zaczepu_przed_ster = log->w.nr_zacz_;
clear_struct(&log->e_bieg_out_);
clear_struct(&log->e_poz_out_);
log->dw.byl_bieg = 0;
break;
case IMPULS_W_GORE:
log->licznik_ster_gora+=LOOP_CYCLE_MS;
if ((log->licznik_ster_gora > log->nast_.t_imp_ster))
{
log->licznik_ster_gora = 0;
log->licznik_poz_gora = 0;
log->_stan_man = SPRAWDZENIE_ODP_PRZE_ZACZEPOW_GORA;
clear_struct(&log->gora_out_);
if ((log->nast_.kon_biegu) && (!(log->dw.byl_bieg & 0x1 >0) ))
set_struct(&log->e_bieg_out_);
}
else
{
if ((log->nast_.kon_biegu) && (check_struct(&log->bieg_)))
{
log->dw.byl_bieg |=1;
log->licznik_ster_biegu+=LOOP_CYCLE_MS;
if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz)))
{
set_struct(&log->e_bieg_out_);
}
}
}
break;
case SPRAWDZENIE_ODP_PRZE_ZACZEPOW_GORA:
//sprawdzenie odpowiedzi przecznika zaczepw tj. czas max, zmiana nr +1, itp
//trzeba si zastanowic jak kontrolowa sygna BIEG_PRZE_ZACZ??
if ((((log->nast_.kon_biegu)&&((!check_struct(&log->bieg_))&&(log->dw.byl_bieg & 0x01))||(check_struct(&log->e_bieg_out_))||(!(log->nast_.kon_biegu)))) && (!check_struct(&log->zo_zacz_)))
{
log->_stan_man = KONIEC_STEROWANIA;
if ((log->nast_.kon_poz_ster)&&(log->w.nr_zacz_ <= log->nr_zaczepu_przed_ster))
{
set_struct(&log->e_poz_out_);
}
}
else
{
log->licznik_ster_biegu+=LOOP_CYCLE_MS;
if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz))&&(check_struct(&log->bieg_))&&(log->nast_.kon_biegu))
{
log->_stan_man = KONIEC_STEROWANIA;
set_struct(&log->e_bieg_out_);
}
}
break;
//dol
case STEROWANIE_W_DOL:
*log->td_out = 0;
*log->tg_out = 0;
set_struct(&log->dol_out_);
log->_stan_man = IMPULS_W_DOL;
log->licznik_ster_dol = 0;
log->licznik_ster_biegu = 0;
log->nr_zaczepu_przed_ster = log->w.nr_zacz_;
clear_struct(&log->e_bieg_out_);
clear_struct(&log->e_poz_out_);
log->dw.byl_bieg =0;
break;
case IMPULS_W_DOL:
{
log->licznik_ster_dol+=LOOP_CYCLE_MS;
if ((log->licznik_ster_dol > log->nast_.t_imp_ster))
{
log->licznik_ster_gora = 0;
log->licznik_poz_gora = 0;
log->_stan_man = SPRAWDZENIE_ODP_PRZE_ZACZEPOW_DOL;
clear_struct(&log->dol_out_);
if ((log->nast_.kon_biegu) && (!(log->dw.byl_bieg & 0x1 > 0)))
set_struct(&log->e_bieg_out_);
}
else
{
if ((log->nast_.kon_biegu) && (check_struct(&log->bieg_)))
{
log->dw.byl_bieg |=1;
log->licznik_ster_biegu+=LOOP_CYCLE_MS;
if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz)))
{
set_struct(&log->e_bieg_out_);
}
}
}
}
break;
case SPRAWDZENIE_ODP_PRZE_ZACZEPOW_DOL:
//sprawdzenie odpowiedzi przecznika zaczepw tj. czas max, zmiana nr +1, itp
if ((((log->nast_.kon_biegu)&&((!check_struct(&log->bieg_))&&(log->dw.byl_bieg & 0x01))||(check_struct(&log->e_bieg_out_))||(!(log->nast_.kon_biegu)))) && (!check_struct(&log->zo_zacz_)))
//if ((log->nast_.kon_biegu)&&(!check_struct(&log->bieg_))||(!(log->nast_.kon_biegu)))
{
log->_stan_man = KONIEC_STEROWANIA;
if ((log->nast_.kon_poz_ster)&&(log->w.nr_zacz_ >= log->nr_zaczepu_przed_ster))
{
set_struct(&log->e_poz_out_);
}
}
else
{
log->licznik_ster_biegu+=LOOP_CYCLE_MS;
if (((log->licznik_ster_biegu > log->nast_.t_ster_zacz))&&(check_struct(&log->bieg_))&&(log->nast_.kon_biegu))
{
log->_stan_man = KONIEC_STEROWANIA;
set_struct(&log->e_bieg_out_);
}
}
break;
case KONIEC_STEROWANIA:
if (check_struct(&log->man_))
{
//sprawdzenie czy nie przeczy na MANUAL
log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
}
else
if (check_struct(&log->ster_poz_))
{
//sprawdzenie czy nie przeczy na NR_POZ
log->_stan_man = CZEKANIE_NA_STEROWANIE_NR_POZYCJI;
}
else
{
//sprawdzenie czy nie przeczy na AUTO
log->_stan_man = CZEKANIE_NA_STEROWANIE_AUTO;
log->licz_t1G = log->licz_t2G = 0;
log->licz_t1D = log->licz_t2D = 0;
}
break;
default:
log->_stan_man = CZEKANIE_NA_STEROWANIE_MANUAL;
break;
}
}
else
{
clear_struct(&log->gora_out_);
clear_struct(&log->dol_out_);
clear_struct(&log->e_bieg_out_);
clear_struct(&log->e_poz_out_);
clear_struct(&log->pob_G);
clear_struct(&log->pob_D);
*log->U_odn = 0;
}
}
void rnt_100hz(void *arguments, void *logic)
{
struct rnt_logic *log = (struct rnt_logic *)logic;
// int t;
if(log->nast_.on) // jesli zabezpieczenie aktywne
{
}
}
void rnt_20hz(void *arguments, void *logic)
{
//struct rnt_logic *log = (struct rnt_logic *)logic;
}
Reference in New Issue View Git Blame Copy Permalink