From d086ef28de9fe345ef375dd14403823e7ffcc6d0 Mon Sep 17 00:00:00 2001 From: Mirek Date: Thu, 19 Feb 2026 11:59:33 +0100 Subject: [PATCH] =?UTF-8?q?dodany=20kod=20c=20od=20Krzyska=20po=20wielu=20?= =?UTF-8?q?modyfikacjach=20przez=20Gemini=20zeby=20usunac=20zalezno=C5=9B?= =?UTF-8?q?=C4=87=20od=20TI-RTOS?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ZDistA_dummy.c | 8 + ZDistA_komp.c | 38 +- ZDistA_komp.dll | Bin 0 -> 100352 bytes ZDistA_komp.h | 76 +- ZDistA_wrapper.c | 202 +++ ZDistA_wrapper.h | 50 + ZDistL_komp.h | 8 +- fix_includes.py | 29 + git-setup.bat | 40 +- mocks.c | 359 +---- mocks.h | 52 +- requirements.txt | 2 +- src/21g.c | 278 ++++ src/21g.h | 123 ++ src/CBR.c | 371 +++++ src/CBR.h | 96 ++ src/Diff.c | 1465 +++++++++++++++++ src/Diff.h | 431 +++++ src/Diff_1.c | 126 ++ src/Diff_1.h | 52 + src/Diff_1_bis.c | 1287 +++++++++++++++ src/Diff_1_bis.h | 249 +++ src/Diff_2.c | 124 ++ src/Diff_2.h | 49 + src/LMZ.c | 791 ++++++++++ src/LMZ.h | 143 ++ src/LRW.c | 410 +++++ src/LRW.h | 167 ++ src/R87L.c | 1726 +++++++++++++++++++++ src/R87L.h | 289 ++++ src/R87L_8.c | 1698 ++++++++++++++++++++ src/R87L_8.h | 269 ++++ src/RNT.c | 837 ++++++++++ src/RNT.h | 324 ++++ src/R_67N_LWN.c | 507 ++++++ src/R_67N_LWN.h | 220 +++ src/R_67N_LWN2.c | 691 +++++++++ src/R_67N_LWN2.h | 262 ++++ src/ZDistA.c | 1189 ++++++++++++++ src/ZDistA.h | 444 ++++++ src/ZDistA_kopia.c_ | 1124 ++++++++++++++ src/ZDistA_kopia.h_ | 425 +++++ src/ZDistL.c | 1424 +++++++++++++++++ src/ZDistL.h | 409 +++++ src/ZDistL_marian.c_ | 1259 +++++++++++++++ src/ZDistL_marian.h_ | 336 ++++ src/ZDistS.c | 599 +++++++ src/ZDistS.h | 185 +++ src/an_gen.c | 60 + src/an_gen.h | 50 + src/analog_in.c | 90 ++ src/analog_in.h | 77 + src/and.c | 49 + src/and.h | 46 + src/and4.c | 46 + src/and4.h | 45 + src/and8.c | 54 + src/and8.h | 49 + src/bank_sel.c | 98 ++ src/bank_sel.h | 79 + src/bin_in.c | 97 ++ src/bin_in.h | 46 + src/buzzer.c | 38 + src/c37_94.c | 586 +++++++ src/c37_94.h | 126 ++ src/cap_comp.c | 137 ++ src/cap_comp.h | 64 + src/cfg_state.c | 27 + src/cfg_state.h | 38 + src/cfg_var.h | 2 + src/chka.c | 239 +++ src/chka.h | 82 + src/chka_ieee.c | 327 ++++ src/chka_ieee.h | 101 ++ src/cnt.c | 64 + src/cnt.h | 57 + src/comm.h | 2 + src/comp_s.c | 84 + src/comp_s.h | 59 + src/const_flt.c | 28 + src/const_flt.h | 37 + src/counter.c | 46 + src/counter.h | 42 + src/ddr.c | 219 +++ src/ddr.h | 102 ++ src/ddr_drv.c | 98 ++ src/ddr_drv.h | 44 + src/dec_month.c | 118 ++ src/dec_month.h | 57 + src/demux8.c | 64 + src/demux8.h | 52 + src/destream.c | 58 + src/destream.h | 52 + src/dev_ctrl.c | 258 +++ src/dev_ctrl.h | 142 ++ src/dfr.c | 219 +++ src/dfr.h | 84 + src/dfr_drv.c | 122 ++ src/dfr_drv.h | 45 + src/e_addvec.c | 69 + src/e_addvec.h | 49 + src/e_avg.c | 84 + src/e_avg.h | 44 + src/e_avg_1t.c | 109 ++ src/e_avg_1t.h | 61 + src/e_avg_dt.c | 139 ++ src/e_avg_dt.h | 52 + src/e_cu.c | 221 +++ src/e_cu.h | 62 + src/e_df_3f.c | 306 ++++ src/e_df_3f.h | 55 + src/e_df_dt.c | 100 ++ src/e_df_dt.h | 46 + src/e_df_dt_3f.c | 152 ++ src/e_df_dt_3f.h | 60 + src/e_du.c | 121 ++ src/e_du.h | 59 + src/e_du_dt.c | 99 ++ src/e_du_dt.h | 43 + src/e_f.c | 317 ++++ src/e_f.h | 106 ++ src/e_f_3f.c | 80 + src/e_f_3f.h | 49 + src/e_filtr1h.c | 105 ++ src/e_filtr1h.h | 48 + src/e_filtr1h_2T.c | 91 ++ src/e_filtr1h_2T.h | 47 + src/e_filtr2h.c | 102 ++ src/e_filtr2h.h | 47 + src/e_filtr2h_1khz.c | 73 + src/e_filtr2h_1khz.h | 47 + src/e_filtr3h.c | 101 ++ src/e_filtr3h.h | 48 + src/e_filtr5h.c | 102 ++ src/e_filtr5h.h | 48 + src/e_mem.c | 365 +++++ src/e_mem.h | 132 ++ src/e_mem1.c | 159 ++ src/e_mem1.h | 58 + src/e_mulcon.c | 63 + src/e_mulcon.h | 49 + src/e_mulvec.c | 67 + src/e_mulvec.h | 49 + src/e_phi.c | 49 + src/e_phi.h | 50 + src/e_pq_1f.c | 67 + src/e_pq_1f.h | 47 + src/e_pq_3f.c | 123 ++ src/e_pq_3f.h | 71 + src/e_pq_A.c | 89 ++ src/e_pq_A.h | 55 + src/e_rms.c | 82 + src/e_rms.h | 44 + src/e_rms_1t.c | 130 ++ src/e_rms_1t.h | 52 + src/e_sc_n.c | 81 + src/e_sc_n.h | 53 + src/e_sc_p.c | 81 + src/e_sc_p.h | 53 + src/e_sc_z.c | 79 + src/e_sc_z.h | 53 + src/e_subvec.c | 64 + src/e_subvec.h | 49 + src/edge_det.c | 57 + src/edge_det.h | 47 + src/elements.c | 175 +++ src/elements.h | 201 +++ src/eth.c | 354 +++++ src/eth.h | 117 ++ src/eth_demux.c | 205 +++ src/eth_demux.h | 100 ++ src/eth_ench.c | 226 +++ src/eth_ench.h | 90 ++ src/event.c | 33 + src/event.h | 42 + src/events_reg.c | 165 ++ src/events_reg.h | 74 + src/goose.c | 380 +++++ src/goose.h | 153 ++ src/goose_drv.c | 394 +++++ src/goose_drv.h | 45 + src/goose_out.c | 281 ++++ src/goose_out.h | 82 + src/helper.c | 149 ++ src/helper.h | 85 + src/hr_3ph2h.c | 151 ++ src/hr_3ph2h.h | 127 ++ src/hw_types.h | 6 + src/i_fail.c | 256 +++ src/i_fail.h | 132 ++ src/kier_pr.c | 244 +++ src/kier_pr.c_ | 226 +++ src/kier_pr.h | 134 ++ src/kier_pr.h_ | 131 ++ src/klapacz.c | 49 + src/klapacz.h | 41 + src/l_comm.c | 317 ++++ src/l_comm.h | 141 ++ src/lacz.c | 159 ++ src/lacz.h | 85 + src/led.c | 39 + src/led.h | 49 + src/leds_drv.c | 80 + src/leds_drv.h | 41 + src/log_state.c | 33 + src/log_state.h | 41 + src/log_wyl.c | 153 ++ src/log_wyl.h | 76 + src/logman.c | 9 +- src/main.c | 533 ------- src/measurand.c | 80 + src/measurand.h | 62 + src/misc.h | 3 + src/mki7.c | 56 + src/mki7.h | 48 + src/mki7_2.c | 55 + src/mki7_2.h | 48 + src/mkstream.c | 95 ++ src/mkstream.h | 71 + src/mocks.c | 98 ++ src/mocks.h | 188 +++ src/mul_float.c | 36 + src/mul_float.h | 40 + src/mux_bin.c | 41 + src/mux_bin.h | 43 + src/mux_float.c | 42 + src/mux_float.h | 43 + src/mux_t.c | 143 ++ src/mux_t.h | 79 + src/nand.c | 52 + src/nand.h | 46 + src/nor.c | 52 + src/nor.h | 47 + src/not.c | 44 + src/not.h | 43 + src/nr_zacz.c | 151 ++ src/nr_zacz.h | 72 + src/od_bl.c | 67 + src/od_bl.h | 60 + src/or.c | 49 + src/or.h | 47 + src/or4.c | 46 + src/or4.h | 45 + src/or8.c | 54 + src/or8.h | 49 + src/out_drv.c | 80 + src/out_drv.h | 39 + src/ov_1ph.c | 106 ++ src/ov_1ph.h | 86 + src/ov_1ph2h.c | 149 ++ src/ov_1ph2h.h | 101 ++ src/ov_1ph_old.c_ | 70 + src/ov_1ph_old.h_ | 50 + src/ov_3ph.c | 226 +++ src/ov_3ph.h | 106 ++ src/ov_3ph2h-2018-04-19.c_old | 299 ++++ src/ov_3ph2h.c | 320 ++++ src/ov_3ph2h.h | 145 ++ src/ov_3ph2h_slow.c | 315 ++++ src/ov_3ph2h_slow.h | 141 ++ src/ov_3ph_zezw.h | 104 ++ src/pkw.c | 88 ++ src/pkw.h | 65 + src/ports/am1808/include/netif/sitaraif.h | 6 + src/pow.c | 39 + src/pow.h | 39 + src/prog_czas.c | 84 + src/prog_czas.h | 51 + src/ps.c | 472 ++++++ src/ps.h | 216 +++ src/ps2.c | 413 +++++ src/ps2.h | 192 +++ src/r_freq.c | 122 ++ src/r_freq.h | 87 ++ src/rec_an.c | 36 + src/rec_an.h | 42 + src/rec_bin.c | 27 + src/rec_bin.h | 42 + src/rec_buf.c | 42 + src/rec_buf.h | 42 + src/rec_float.c | 43 + src/rec_float.h | 49 + src/relay_out.c | 80 + src/relay_out.h | 52 + src/s_demux.c | 218 +++ src/s_demux.h | 82 + src/s_demux2.c | 209 +++ src/s_demux2.h | 81 + src/sck.c | 336 ++++ src/sck.h | 186 +++ src/spz_3f.c | 1282 +++++++++++++++ src/spz_3f.h | 309 ++++ src/spz_3f_2.c | 1305 ++++++++++++++++ src/spz_3f_2.h | 290 ++++ src/sqrt.c | 35 + src/sqrt.h | 39 + src/sr.c | 49 + src/sr.h | 47 + src/sum_e_3f.c | 131 ++ src/sum_e_3f.h | 100 ++ src/sum_float.c | 42 + src/sum_float.h | 43 + src/sum_flt_3f.c | 64 + src/sum_flt_3f.h | 61 + src/sv.c | 515 ++++++ src/sv.h | 169 ++ src/ti/ipc/Notify.h | 6 + src/ti/sysbios/hal/Hwi.h | 6 + src/ti/sysbios/hal/Timer.h | 10 + src/ti/sysbios/knl/Semaphore.h | 7 + src/tp.c | 70 + src/tp.h | 50 + src/tz.c | 55 + src/tz.h | 46 + src/tz_8.c | 110 ++ src/tz_8.h | 77 + src/u_fail.c | 349 +++++ src/u_fail.h | 164 ++ src/under_1ph.c | 105 ++ src/under_1ph.c_ | 68 + src/under_1ph.h | 86 + src/under_1ph.h_ | 49 + src/under_3ph.c | 209 +++ src/under_3ph.h | 106 ++ src/valf_gt.c | 43 + src/valf_gt.h | 41 + src/virt_in.c | 29 + src/virt_in.h | 44 + src/virt_in_drv.c | 125 ++ src/virt_in_drv.h | 43 + src/xor.c | 62 + src/xor.h | 47 + src/zzr.c | 285 ++++ src/zzr.h | 133 ++ stub_sdk/ti/sysbios/hal/Timer.h | 1 + stub_sdk/ti/sysbios/knl/Semaphore.h | 7 + 336 files changed, 52294 insertions(+), 1006 deletions(-) create mode 100644 ZDistA_dummy.c create mode 100644 ZDistA_komp.dll create mode 100644 ZDistA_wrapper.c create mode 100644 ZDistA_wrapper.h create mode 100644 fix_includes.py create mode 100644 src/21g.c create mode 100644 src/21g.h create mode 100644 src/CBR.c create mode 100644 src/CBR.h create mode 100644 src/Diff.c create mode 100644 src/Diff.h create mode 100644 src/Diff_1.c create mode 100644 src/Diff_1.h create mode 100644 src/Diff_1_bis.c create mode 100644 src/Diff_1_bis.h create mode 100644 src/Diff_2.c create mode 100644 src/Diff_2.h create mode 100644 src/LMZ.c create mode 100644 src/LMZ.h create mode 100644 src/LRW.c create mode 100644 src/LRW.h create mode 100644 src/R87L.c create mode 100644 src/R87L.h create mode 100644 src/R87L_8.c create mode 100644 src/R87L_8.h create mode 100644 src/RNT.c create mode 100644 src/RNT.h create mode 100644 src/R_67N_LWN.c create mode 100644 src/R_67N_LWN.h create mode 100644 src/R_67N_LWN2.c create mode 100644 src/R_67N_LWN2.h create mode 100644 src/ZDistA.c create mode 100644 src/ZDistA.h create mode 100644 src/ZDistA_kopia.c_ create mode 100644 src/ZDistA_kopia.h_ create mode 100644 src/ZDistL.c create mode 100644 src/ZDistL.h create mode 100644 src/ZDistL_marian.c_ create mode 100644 src/ZDistL_marian.h_ create mode 100644 src/ZDistS.c create mode 100644 src/ZDistS.h create mode 100644 src/an_gen.c create mode 100644 src/an_gen.h create mode 100644 src/analog_in.c create mode 100644 src/analog_in.h create mode 100644 src/and.c create mode 100644 src/and.h create mode 100644 src/and4.c create mode 100644 src/and4.h create mode 100644 src/and8.c create mode 100644 src/and8.h create mode 100644 src/bank_sel.c create mode 100644 src/bank_sel.h create mode 100644 src/bin_in.c create mode 100644 src/bin_in.h create mode 100644 src/buzzer.c create mode 100644 src/c37_94.c create mode 100644 src/c37_94.h create mode 100644 src/cap_comp.c create mode 100644 src/cap_comp.h create mode 100644 src/cfg_state.c create mode 100644 src/cfg_state.h create mode 100644 src/chka.c create mode 100644 src/chka.h create mode 100644 src/chka_ieee.c create mode 100644 src/chka_ieee.h create mode 100644 src/cnt.c create mode 100644 src/cnt.h create mode 100644 src/comp_s.c create mode 100644 src/comp_s.h create mode 100644 src/const_flt.c create mode 100644 src/const_flt.h create mode 100644 src/counter.c create mode 100644 src/counter.h create mode 100644 src/ddr.c create mode 100644 src/ddr.h create mode 100644 src/ddr_drv.c create mode 100644 src/ddr_drv.h create mode 100644 src/dec_month.c create mode 100644 src/dec_month.h create mode 100644 src/demux8.c create mode 100644 src/demux8.h create mode 100644 src/destream.c create mode 100644 src/destream.h create mode 100644 src/dev_ctrl.c create mode 100644 src/dev_ctrl.h create mode 100644 src/dfr.c create mode 100644 src/dfr.h create mode 100644 src/dfr_drv.c create mode 100644 src/dfr_drv.h create mode 100644 src/e_addvec.c create mode 100644 src/e_addvec.h create mode 100644 src/e_avg.c create mode 100644 src/e_avg.h create mode 100644 src/e_avg_1t.c create mode 100644 src/e_avg_1t.h create mode 100644 src/e_avg_dt.c create mode 100644 src/e_avg_dt.h create mode 100644 src/e_cu.c create mode 100644 src/e_cu.h create mode 100644 src/e_df_3f.c create mode 100644 src/e_df_3f.h create mode 100644 src/e_df_dt.c create mode 100644 src/e_df_dt.h create mode 100644 src/e_df_dt_3f.c create mode 100644 src/e_df_dt_3f.h create mode 100644 src/e_du.c create mode 100644 src/e_du.h create mode 100644 src/e_du_dt.c create mode 100644 src/e_du_dt.h create mode 100644 src/e_f.c create mode 100644 src/e_f.h create mode 100644 src/e_f_3f.c create mode 100644 src/e_f_3f.h create mode 100644 src/e_filtr1h.c create mode 100644 src/e_filtr1h.h create mode 100644 src/e_filtr1h_2T.c create mode 100644 src/e_filtr1h_2T.h create mode 100644 src/e_filtr2h.c create mode 100644 src/e_filtr2h.h create mode 100644 src/e_filtr2h_1khz.c create mode 100644 src/e_filtr2h_1khz.h create mode 100644 src/e_filtr3h.c create mode 100644 src/e_filtr3h.h create mode 100644 src/e_filtr5h.c create mode 100644 src/e_filtr5h.h create mode 100644 src/e_mem.c create mode 100644 src/e_mem.h create mode 100644 src/e_mem1.c create mode 100644 src/e_mem1.h create mode 100644 src/e_mulcon.c create mode 100644 src/e_mulcon.h create mode 100644 src/e_mulvec.c create mode 100644 src/e_mulvec.h create mode 100644 src/e_phi.c create mode 100644 src/e_phi.h create mode 100644 src/e_pq_1f.c create mode 100644 src/e_pq_1f.h create mode 100644 src/e_pq_3f.c create mode 100644 src/e_pq_3f.h create mode 100644 src/e_pq_A.c create mode 100644 src/e_pq_A.h create mode 100644 src/e_rms.c create mode 100644 src/e_rms.h create mode 100644 src/e_rms_1t.c create mode 100644 src/e_rms_1t.h create mode 100644 src/e_sc_n.c create mode 100644 src/e_sc_n.h create mode 100644 src/e_sc_p.c create mode 100644 src/e_sc_p.h create mode 100644 src/e_sc_z.c create mode 100644 src/e_sc_z.h create mode 100644 src/e_subvec.c create mode 100644 src/e_subvec.h create mode 100644 src/edge_det.c create mode 100644 src/edge_det.h create mode 100644 src/elements.c create mode 100644 src/elements.h create mode 100644 src/eth.c create mode 100644 src/eth.h create mode 100644 src/eth_demux.c create mode 100644 src/eth_demux.h create mode 100644 src/eth_ench.c create mode 100644 src/eth_ench.h create mode 100644 src/event.c create mode 100644 src/event.h create mode 100644 src/events_reg.c create mode 100644 src/events_reg.h create mode 100644 src/goose.c create mode 100644 src/goose.h create mode 100644 src/goose_drv.c create mode 100644 src/goose_drv.h create mode 100644 src/goose_out.c create mode 100644 src/goose_out.h create mode 100644 src/helper.c create mode 100644 src/helper.h create mode 100644 src/hr_3ph2h.c create mode 100644 src/hr_3ph2h.h create mode 100644 src/hw_types.h create mode 100644 src/i_fail.c create mode 100644 src/i_fail.h create mode 100644 src/kier_pr.c create mode 100644 src/kier_pr.c_ create mode 100644 src/kier_pr.h create mode 100644 src/kier_pr.h_ create mode 100644 src/klapacz.c create mode 100644 src/klapacz.h create mode 100644 src/l_comm.c create mode 100644 src/l_comm.h create mode 100644 src/lacz.c create mode 100644 src/lacz.h create mode 100644 src/led.c create mode 100644 src/led.h create mode 100644 src/leds_drv.c create mode 100644 src/leds_drv.h create mode 100644 src/log_state.c create mode 100644 src/log_state.h create mode 100644 src/log_wyl.c create mode 100644 src/log_wyl.h delete mode 100644 src/main.c create mode 100644 src/measurand.c create mode 100644 src/measurand.h create mode 100644 src/mki7.c create mode 100644 src/mki7.h create mode 100644 src/mki7_2.c create mode 100644 src/mki7_2.h create mode 100644 src/mkstream.c create mode 100644 src/mkstream.h create mode 100644 src/mocks.c create mode 100644 src/mocks.h create mode 100644 src/mul_float.c create mode 100644 src/mul_float.h create mode 100644 src/mux_bin.c create mode 100644 src/mux_bin.h create mode 100644 src/mux_float.c create mode 100644 src/mux_float.h create mode 100644 src/mux_t.c create mode 100644 src/mux_t.h create mode 100644 src/nand.c create mode 100644 src/nand.h create mode 100644 src/nor.c create mode 100644 src/nor.h create mode 100644 src/not.c create mode 100644 src/not.h create mode 100644 src/nr_zacz.c create mode 100644 src/nr_zacz.h create mode 100644 src/od_bl.c create mode 100644 src/od_bl.h create mode 100644 src/or.c create mode 100644 src/or.h create mode 100644 src/or4.c create mode 100644 src/or4.h create mode 100644 src/or8.c create mode 100644 src/or8.h create mode 100644 src/out_drv.c create mode 100644 src/out_drv.h create mode 100644 src/ov_1ph.c create mode 100644 src/ov_1ph.h create mode 100644 src/ov_1ph2h.c create mode 100644 src/ov_1ph2h.h create mode 100644 src/ov_1ph_old.c_ create mode 100644 src/ov_1ph_old.h_ create mode 100644 src/ov_3ph.c create mode 100644 src/ov_3ph.h create mode 100644 src/ov_3ph2h-2018-04-19.c_old create mode 100644 src/ov_3ph2h.c create mode 100644 src/ov_3ph2h.h create mode 100644 src/ov_3ph2h_slow.c create mode 100644 src/ov_3ph2h_slow.h create mode 100644 src/ov_3ph_zezw.h create mode 100644 src/pkw.c create mode 100644 src/pkw.h create mode 100644 src/ports/am1808/include/netif/sitaraif.h create mode 100644 src/pow.c create mode 100644 src/pow.h create mode 100644 src/prog_czas.c create mode 100644 src/prog_czas.h create mode 100644 src/ps.c create mode 100644 src/ps.h create mode 100644 src/ps2.c create mode 100644 src/ps2.h create mode 100644 src/r_freq.c create mode 100644 src/r_freq.h create mode 100644 src/rec_an.c create mode 100644 src/rec_an.h create mode 100644 src/rec_bin.c create mode 100644 src/rec_bin.h create mode 100644 src/rec_buf.c create mode 100644 src/rec_buf.h create mode 100644 src/rec_float.c create mode 100644 src/rec_float.h create mode 100644 src/relay_out.c create mode 100644 src/relay_out.h create mode 100644 src/s_demux.c create mode 100644 src/s_demux.h create mode 100644 src/s_demux2.c create mode 100644 src/s_demux2.h create mode 100644 src/sck.c create mode 100644 src/sck.h create mode 100644 src/spz_3f.c create mode 100644 src/spz_3f.h create mode 100644 src/spz_3f_2.c create mode 100644 src/spz_3f_2.h create mode 100644 src/sqrt.c create mode 100644 src/sqrt.h create mode 100644 src/sr.c create mode 100644 src/sr.h create mode 100644 src/sum_e_3f.c create mode 100644 src/sum_e_3f.h create mode 100644 src/sum_float.c create mode 100644 src/sum_float.h create mode 100644 src/sum_flt_3f.c create mode 100644 src/sum_flt_3f.h create mode 100644 src/sv.c create mode 100644 src/sv.h create mode 100644 src/ti/ipc/Notify.h create mode 100644 src/ti/sysbios/hal/Hwi.h create mode 100644 src/ti/sysbios/hal/Timer.h create mode 100644 src/ti/sysbios/knl/Semaphore.h create mode 100644 src/tp.c create mode 100644 src/tp.h create mode 100644 src/tz.c create mode 100644 src/tz.h create mode 100644 src/tz_8.c create mode 100644 src/tz_8.h create mode 100644 src/u_fail.c create mode 100644 src/u_fail.h create mode 100644 src/under_1ph.c create mode 100644 src/under_1ph.c_ create mode 100644 src/under_1ph.h create mode 100644 src/under_1ph.h_ create mode 100644 src/under_3ph.c create mode 100644 src/under_3ph.h create mode 100644 src/valf_gt.c create mode 100644 src/valf_gt.h create mode 100644 src/virt_in.c create mode 100644 src/virt_in.h create mode 100644 src/virt_in_drv.c create mode 100644 src/virt_in_drv.h create mode 100644 src/xor.c create mode 100644 src/xor.h create mode 100644 src/zzr.c create mode 100644 src/zzr.h create mode 100644 stub_sdk/ti/sysbios/hal/Timer.h create mode 100644 stub_sdk/ti/sysbios/knl/Semaphore.h diff --git a/ZDistA_dummy.c b/ZDistA_dummy.c new file mode 100644 index 0000000..e645007 --- /dev/null +++ b/ZDistA_dummy.c @@ -0,0 +1,8 @@ + +void ZDistA_komp_initlog(void *arguments, void *logic) { + // Dummy implementation +} + +void ZDistA_komp(void *args, void *logic) { + // Dummy implementation +} diff --git a/ZDistA_komp.c b/ZDistA_komp.c index 44a810b..d18dcc7 100644 --- a/ZDistA_komp.c +++ b/ZDistA_komp.c @@ -8,8 +8,8 @@ #include -#include "../tdefs.h" -#include "../misc.h" +#include "tdefs.h" +#include "misc.h" #include "helper.h" #include "analog_in.h" #include "e_phi.h" @@ -1010,17 +1010,17 @@ void ZDistA_komp(void *arguments, void *logic) //#define K_SMALL = 0.03; //energia dla wlasciwej petli >= 0.7...0.9 maksymalnej? Kluczowa w near-end, mniej znaczaca w far-end //mozna dodatkowo sprobowac porownania kata impedancji? max 40..60stopni roznicy? -//wykryæ near-end mo¿na przez Z<0.15 Zlinii oraz I1>5...10*Inom +//wykry� near-end mo�na przez Z<0.15 Zlinii oraz I1>5...10*Inom - if(k0>(0.2f*0.2f) && /*k2<0.12 dodaæ? &&*/ Eratio>2.0f && (Zratio < 0.25f/* || near_end*/)) // 1ph-G... moznaby blokowaæ Pob_G gdy Zratio du¿y? + if(k0>(0.2f*0.2f) && /*k2<0.12 doda�? &&*/ Eratio>2.0f && (Zratio < 0.25f/* || near_end*/)) // 1ph-G... moznaby blokowa� Pob_G gdy Zratio du�y? { typ_zw=1; // blokowac miedzyfazowe? // zezwol_L1L2=0; // zezwol_L2L3=0; // zezwol_L1L3=0; } - else if(k0<0.05f && k2>0.2f && Eratio>1.85f) // L-L [brak I0 za to pojawia siê I2] + else if(k0<0.05f && k2>0.2f && Eratio>1.85f) // L-L [brak I0 za to pojawia si� I2] { typ_zw=2; // blokowac fazowe? zezwol_L1=0; @@ -1144,7 +1144,7 @@ void ZDistA_komp(void *arguments, void *logic) float phi1,phi2; // Kryterium Radka - // 4. K¹t (I0-I2) |fiI0_I2| + // 4. K�t (I0-I2) |fiI0_I2| phi1=get_phase(*log->sI0_ortb,*log->sI0_orta); phi2=get_phase(*log->sI2_ortb,*log->sI2_orta); fiI0_I2=get_phase_diff(phi1,phi2); @@ -1178,7 +1178,7 @@ void ZDistA_komp(void *arguments, void *logic) zakr_I0_I2 = *log->sI0 * _rcpsp(*log->sI2); //if(Z_Z1<0.9f) - //jak sprawdziæ ¿e impedancja Z_L1/2/3 jest w przód lub w ty³ + //jak sprawdzi� �e impedancja Z_L1/2/3 jest w prz�d lub w ty� ///??? if(1/*blok_do_szyn*/)//zwarcie w przod @@ -1225,24 +1225,24 @@ void ZDistA_komp(void *arguments, void *logic) //*log->z[4]=A; } //Kryterium Radka B -//Blokada fa³szywych zwaræ doziemnych podczas poprawnego zwarcia doziemnego +//Blokada fa�szywych zwar� doziemnych podczas poprawnego zwarcia doziemnego /* float fiI0_I2; if((ZDISTA_POPRAWKA_4)&&(ZDISTA_POPRAWKA_5)) { float phi3,phi4; // Kryterium Radka - // 4. K¹t (I0-I2) |fiI0_I2| + // 4. K�t (I0-I2) |fiI0_I2| phi3=get_phase(*log->sI0_ortb,*log->sI0_orta); phi4=get_phase(*log->sI2_ortb,*log->sI2_orta); fiI0_I2=get_phase_diff(phi3,phi4); - //sprzwdzanie BRAK aktywnych pêtli zwaræ miêdzyfazowych LL + //sprzwdzanie BRAK aktywnych p�tli zwar� mi�dzyfazowych LL //???????????? - //czy tylko w 1 strefie, czy we wszystkich trzeba sprawdziæ zwarcia LL + //czy tylko w 1 strefie, czy we wszystkich trzeba sprawdzi� zwarcia LL //sprawdzenie fi - // dla ka¿dej fazy + // dla ka�dej fazy if (((fabs(fiI0_I2)>30) && (fabs(fiI0_I2-120)<25)) ||(fabs(fiI0_I2+120)<25)) zezwol_L1 =0; if (((fabs(fiI0_I2-120)>30) && (fabs(fiI0_I2)<25)) ||(fabs(fiI0_I2+120)<25)) @@ -1260,7 +1260,7 @@ void ZDistA_komp(void *arguments, void *logic) if(ZDISTA_POPRAWKA_4C) { //---------------------------------------------------------------------------------- - //Blokada fa³szywych zwaræ doziemnych podczas zwarcia miêdzyfazowego + //Blokada fa�szywych zwar� doziemnych podczas zwarcia mi�dzyfazowego // 1. |I0/I2|<0.25 float kdiff; kdiff = *log->sI0 * _rcpsp(*log->sI2); // |I0|/|I2| @@ -1415,7 +1415,7 @@ void ZDistA_komp(void *arguments, void *logic) ); } - sprawdz_P( // blokada od pradow obcia¿enia + sprawdz_P( // blokada od pradow obcia�enia &log->dw.BL_Load[i], (log->dw.Z.R[i] > local1 || log->dw.Z.R[i] < local2) && fabs(log->dw.Z.X[i]) < fabs(local3 * log->dw.Z.R[i]), @@ -1531,7 +1531,7 @@ void ZDistA_komp(void *arguments, void *logic) &log->dw.liczps[0][i], l_powt, 5 ); - log->dw.Pbk[0][i] = log->dw.P_s[0][i] && log->dw.Iogr && !(log->nast_.Bl_L[0] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + log->dw.Pbk[0][i] = log->dw.P_s[0][i] && log->dw.Iogr && !(log->nast_.Bl_L[0] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia�enia oraz kryterium zwarc z udzialem ziemi log->dw.P_sss[0][i]= wk1; } @@ -1579,7 +1579,7 @@ void ZDistA_komp(void *arguments, void *logic) &log->dw.liczps[1][i], l_powt, 5 ); - log->dw.Pbk[1][i] = log->dw.P_s[1][i] && log->dw.Iogr && !(log->nast_.Bl_L[1] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + log->dw.Pbk[1][i] = log->dw.P_s[1][i] && log->dw.Iogr && !(log->nast_.Bl_L[1] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia�enia oraz kryterium zwarc z udzialem ziemi log->dw.P_sss[1][i]= wk1; } @@ -1629,7 +1629,7 @@ void ZDistA_komp(void *arguments, void *logic) l_powt, 5 ); - log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && log->dw.Iogr && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && log->dw.Iogr && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia�enia oraz kryterium zwarc z udzialem ziemi log->dw.P_sss[j][i-3]= wk1; } @@ -1678,7 +1678,7 @@ void ZDistA_komp(void *arguments, void *logic) &log->dw.liczps[j][i-3], l_powt, 5 ); - log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz minimalnej wartosci pradu + log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia�enia oraz minimalnej wartosci pradu log->dw.P_sss[j][i-3]= wk1; } } @@ -1767,7 +1767,7 @@ void ZDistA_komp(void *arguments, void *logic) // *log->z[5]=Zm_vect; - if (!check_struct(&log->Bl_K))//jeœli nie ma blokady kierunku + if (!check_struct(&log->Bl_K))//je�li nie ma blokady kierunku { for(j=0;j<2;j++) { diff --git a/ZDistA_komp.dll b/ZDistA_komp.dll new file mode 100644 index 0000000000000000000000000000000000000000..432e617d4c4a6c202e346c4edc7f3af791ac9422 GIT binary patch literal 100352 zcmeFadwg5PwdgNdlC8vcq&yT1Bp`wUF6Ln{4;v*xie<7DWMW8CLP>(bB!qWPA|;{Z zVI#Xl-l$FA^ptZ-PkT=3EiJUAAxT@XW3U~PkXM0lIFvvij1Uy_3Qj1|@4IG8w!`Co z?)`l3pTB%Sd-m*^HEU+ptkQ%D<5Q{onr#@OjLs+s4=) zAGQ0mE%w;%)8;L^Wu<@lio0%Jal;+{mK*N8^RBr6#wGq0i97wb-06?Znd`sfuA7#e zTV6gokOw{a?xjW7R=BS!{O|ukdDC9r{XZycny;UwP1oz^sHU&za-*&q!*krXN}HPW zv#jYF{VZ>~TYq<7wSecoZ;x)0=he5gEECv;o-{SsY&U(a*fuKjy;}--1GXvt(xNdJ z*!s!owQ}c;=jqkyR=YeTU1YNr>#X6YZ7*MC@c36`Yv#qORCs*$eJDrQao%aSRq9m! z=OMf8p)n&ey6m=?G4x|XHe39=WF6xF@A3aG{vYu#1i74R6|+6;<3lTml<~ZABtn{| zy3Sp071VvCuexo9vVe0Nh$ z+8^;RWV6j2@%sNKM~r}xe9LR3<^|kFsw`k6pZ4}Fm4D}YPHoqvKKp&wasT*U*|9%f zmweM6uauvO$)oo8=q)k>fUE3Le}<~EF7+r6&*mOPy~-j|U!jU>xgHavoJYu?h)bHrpP;ONYQKkC&o?L&*_0KhSoBBPC>XGp$r=KK9CoWcdr0cix zL5&4upDRZ}`A1I{fNstHQ0E@Cf|O_TLA9U4W>5Af77h=Ry9p!=^Jycsz#mJ6DpF0% zkKz9EHF|3{Gf{71bY&zjdY{e=xfljCg+<2LpIC? zrlesY=d0t*>@v=m^i(UIZmh80%jLbHg6acC_aVPgL%OD6z^Lgqw&wIdAf3)j&2Vdm zC(>=CT;l`_5N^)A=BD=UL`cZ{I^Hu|;K@eX)keB! z=n3CYZ_uKVbO+Smtwa$3D|>d)?MgKZDrMeITVj~bsnracVH)nN%Lw+zC)9Ke7@l93 z8vsoF;{uDPJZ8^+Bx5IaORc&kYAJM_JyBf;c$Vb@=WCG83%ar;YVJiMS9F6?2j+4P z!~=8rt_80EYYxb_cO|6KICWO7ekoCneE+2WJ{ zyjazC&l6WoFp}FV497mt6X9YbUDg^)mpyBwCIq6veenu&c3CXgy<)UE)9vVv1-n+b zDP5U;wPwEo!xPGwko;=EwoX!`p~GUHMVc%tdaN<(d`Yc_6l?Y~E~V`+CrXU_e4Q)x zY|)RxIBp=v*7;K0*}gwfre0@=V9xK;$wG&PrqZs=I@VF2>uXY)e?7a*9lO88@*9>B zh_A^G1MunT|D0vFWyc|Xgb5bse6eR`pnKd#@YNL=i?W2CES{w)l)5iciMo3~RoD9k zb+wbact?8=S;rWlYckhfZxxMYZTkc#EE!}4-%`MvApS9%9MCpOF zk@NhAc_guT>cC|=kJ0BEyqR9Lf@7cSLz2Rhrgs(FZSl(K&BaBw_-WIZ%i|>1V@Ljp zVDYaG7+Xcu%UBqmpLDOB5HVdVkptmmzde$2EhCvNA@6z;4K+I&%$Jg%l&qauvm=}i zm(ccm5%Yj&+h0blETu*fH7=$g5^HVw77@T{2sY!3KA2-y_BS9+Zz*Bo=Ty2`dnj%Kb=al&t>Nqh9!kg z_9QO>0BzX!8r`q^jZ#?cynvrilRQa*ZJ3dOKTtfa*wgVYbxc>^QoHRphm!s7KG)+U zp)y;_^&3**!`uDw8&`{eaX~o6WHoynjqOa{4~7`b5^3yao3qF3~Ma z*?#Hvgh;`)WGBmS;5S6Um-}3END8)8X(pL1q0luXJlp<$!pbtkvuywha5az<_H27? z{K_kb;nf?a?`-*i!0cAuu(>mNsJQL5oXu%7%D-f@*;D0zT+Ub`UrwjrJ{x6QkJwk0 z8p+q6;(cI=1=P3Fr-9Pgq46t6@wn(ud;*h&flAScZZD`)=(E@N?5gRib3KMa2x~9M z<{_ROo)?}U{&HC8H3Uy?G-1#EaOjx%rkV#MYcy|mH=373&FgC$t3Ei~;9MFuHyECd zzv&^6h9LLa3`@Xfeq=Zfo00dOvEYE`UQuUwam!2c$GjxhzVEep+Qoh%S!R!)%;z#A z9Sc-P9ZyHi=W~0}bvAbJeSN%bovmf%9fuN?MsoUsKC~+jGp)|wX zo%7X_ZN}dBhV{~lI(7j}wLl3`b5%-y1b|N6bcB)bS?+t*VkQm6Gu1d^4X0 z=n_ij`hr2vy%)3MGU5e5f7{H%93MRDN&bvf)LiY4R()XZF&sP0$WE)9_ezn-PU+^# zyCdd&XE^z~KOSMFVHMOinB9$zu9;?}$7uuyR`?|a>ls)%kviU)b&(!#qLgHrB+KIN zaIQNNe0D{DBv_ibM2f()$|#>Wr2$SmHG4vOmYvRpQ?rbSRQt8P(eZRRx7QpG@<%=Q zHiNt#gjjlE0Il@8>Zo~JZPZ*z`Y++@!;8aAtkg zn;326y4rUn-mB^AIk^i4(zDCr%{gD7hrtG$@rGF;D*ofBBnEVhkb7Gs@^zyBh&=W{ zG(-Br*BMq`N>rHKN(Aonigb78FeSc z5j9s+b${Z$^i0vN7?_%_?BQLpmeDcCj^tOJwzVh3T4p+pmaE-S)9E%^oJCPbSJctn z6E(Zh{lxyWH+E+LBqSEm|yp* zlC9-rh=doMWmd+b1pM?i7$v%}OPy6v@A7kEmLF_}XCKMMt8>0|7t6D}j|`L*^I*=` zP6k99p8$+z4dYB80rFMFDo4VpW!Pv(~!yJ(E zQw(!&&bP}d=P}GZeV4DnPQti5mK|%BXRnvO<$S-O049|>nSPi%bH1d0@5?LCVq0sf z&|{b`|FuBy-AtL3>*qj74(8%#Fd>b%f_Hz;x5#R4OrPsfa-=YNU5OeEl6iZBNflUT z`K?l&(PojHN3A~Bd@`s&4Lx+b^7B&jGVt#JS_2fCF8d+>KfCQAkBpheYHe)JmEe1> z2ZLb6e%;`xLW9C!EM8<%K$BT&t@}w`bO{%Wl*+>Cwf)h2T|tiGr_qbm zytd3dijA8MI~*Po&$AAx4cTJ{a?;Y5-Nke35!kkh*yYNYnz7 zC=MrgNEwRdIq;Iv$}<>$D?u1%KFFP!=;YC<~3|yN%{x%zRz_6E59o z&bOJWF*PxJQ?zB4H@7p2&F0plk)F0Gc>5*J^;hug-p~*=ak$A)Lk~IMq(9>QXo+3o zt=S)QVDus0E-8zc-Ue2Hk3sxav?Y=%i^40$<+g`|dsk%TM@I62?1ZS<05_2`v6R;^ z%k0tA80pM{sM#yp5HlSQERzLrybKgxJHkF4(LY$}gMn5Cl71PY6Ul@`&37Bj!HBuC zOpT?h4K@2s)quC9{S7_htK1lHM$2{Hh}jh}RoHwkmY#GFVJzN{69iXDhS!HN$Zrb1ej< zu0v5}?T6+Y>DvRZ8EtPJ(w%p`{}nh^4GyvfPt2T;gU0nuoFE=ABlwb5@;qL{>lthJ zctd{A*r=D+YR}lPpZ5^&)x03_a={bV=Iu!Y9?#BVYY6vUYC2ursRu}YLgrq z>K`l0St(6_!!R4>@cfa{(r`V`pYeVT&nG;a8#eIli<&b-c?EE0Wh~YBeH67=s^QUC z8dsZ67 z`4!Z~qw&!O{xYQ2#O&K@@enXst$;(+_ns+}qUF|Qx;~6Y#8y79%F2@!AbCT0t8o@A zDGXM_`A0=ArxxfX1FzG5Pp^?$hH0~0+qUSF*P_3OF9(*20L`dgs}5^zQLWmV2cRC4 zgn3PE%-ka`#L;@i8xEtnrVGz?t&DZGDCD9?_|fK72@*lC-QyN+NGq**nbX)}OllOy zXb+oCdvfqtqBLd}Ma^f`y+RPWlU84ZErRMnb^gbK2HIBrBxxyRM%+%T)4+_jG))2+ z{H17wA0RBfJE$S3_l_Q4J-F8GT<=WrYw=7y3%fuf0|K`EIHIY{jrFKVnoVeM{E3o@Zn-_{A|N?jc1cf z)D(ZtH-OnBQmXw+)DbsyyI&G1D)nctKkRVFPmY?6qQZIFOAAGcbdfRH%YbN)&yJe& zDFBx5$W#i9zmsTexqO01sbfEeXC~KAaCD@U%ZD33E&|{bjfM-UmXr#hH!d zMOzkEFyf^OM+t1K%poJNhsF~ZM(fs9dfLAw6{UrgReFMe5Cqj60b}mf((yT!qS0)f z)QO|O9gg2P1o=mwf*hX}?ko;C4<9^v$44*ay3Hd()Mj5{kE2N#GfZGD0%?fP_NY+- zEF7o&TVn&n7 z9-oF6Cvmf(J9TBH`s(|+T#V(~vZ{hkj8d=5l1NXg4&y%(xs9a6C%;EMIgajD4^^xG zRJC8^s|{CZqrxU@9ne1vsM_PGIs+xN?qFO}F7ozf z`+i4;k=#&CssBY%oyYtc$u)jlFF$theyY5iYy4`WPRxNbFRN5`VQ?KePy4Hsv6@Pp zA}u5;MnGSUCrTi5x=Vpn_L)8N_pyG%VTX>&oX`n^sFH5q-OTClJwHAZ^^a9p7u z(?{9GoDY=D+58CcFb0>Sj5YKdsYI8Bfv4lYiVFRFkkklFJolOMX?>Z^^R?eI;%0}X z3(W7*!X(|ex6yn~dtw{RJ~jSuF6Z$!>_yFK*tFEM@}PUEXMI<&L3xt0r5-VtJP>dD`yn}|BT?5^pO@9ZAU-)MAa{NT3CNHsnr4DbV)CzLCo-wVV6Gjgvtg&J$l zsy12{l&N|VoXIXH^eeQueCWejd@#!@pwpW=cB~H>Y>_NCT255s$d5^F+1qj)WHqZ6 z=!RW~3sL-T(&cjfMzY%pxSNE|m$QTV4BwXJWYm@g8y<=;GR*lKGdH2=b#@37l~FY3 zw>Fy3Agy6tdpenQlCZJ}JAPC%tf40rGSUqXOI9rj)B;gYc&pUYpgw|c$RLMvPe+1F zl;_^BvvOnRl2)^LD$Sib{WP>K&%NT*HCh(?;oPHD6)b@MCng`hJmB=SKQG_R7i8J* zgTf%8j_qXWj~!!V=&g#S>OCeLtPt&=@q9_eDhPwrrtwXWTy)$uDYc4j{=FMC(GO# z7)^%@1GT_qFsT2h2)qUd(;jPFRtt>IcVx2H)Y=j^qH4ND)tviX`qTEbu#;h~bKRnz z-=N;_QKR0L4mLilCgF?5uKwLL*p0j|WS#9&{~%Omm%31(;s8=rBw{I+`3pM#fytst zBUfx1RQYfROs{f8=rz7;-f)5%Z@3?Oihe z5YX~IxyEaYU#B)x2L0DufMekDN5L|9Fy7EML%Xr2x6ROQt&0p;b1gt(xM$(Tj1mk{ z57Ubg*fPV!Dkcs>)ehuxs1_Zfyr9&(r4(xkQJUN!On^-bn^gmovKer?T7xX*`vWo? zY78Xf@wV$17y(Oam=jp?!IK;d6yZfNSyXv;fX@C|ZZHpS@STA09E&5g2Uv%oY2GIZJ*bG+BsvR-RsQ`JQfRMgQIKe>=KT1~OC zM3HD^bQ6nEmO_bZMvR`wIIkWX7VLKdZjvzQ1@hBmm%%cOw1uRMlD2_rg@&ua4KXMb zt;e|uE|TjBKBxtz=ic|VczrNn6VPo>cTsql8W;7b#*-nXyiaU+Pr=$x@Ea}HqK0bc zz*}zv)MhRI+Cb6xmv-$u`H;<~MStSH=G9CsZqcEPcce1^gWmM!HZObJ>SmIqZc)axgleh-)kJ{cd*6_o{GhR`h!blFlN4| z{`4mH_FY-9#~C5Cwhi06b*pg8@Rl=3Cf9^)iHq|t;dARwNO%$1o`lC}+t4h=Yt|8| zD~bC|$LZbEr|`aF^x^t|-P6Gas_pPB=TcAmH-MVF$B9V4g}me+^y+|rU1yc>#}@-3 zaY@X)+z;m!;Vt@u)=mhxvjywXPQ+Y@$fgmL_X0Yz+J)H3oJcMhof0wU)Q0g7dhS0% z$_1~W#w1dPIj1uFoK~?lN3VjTW&WzsDYGSl8fOs+D^4(4Zu2wW9yRG*hQjeo;uJju zL%s6c`%9|PGem^YM_zrok+E>BSlG6~FI`_X*Ak%1+<=;;{sub%RD8N;^ZkK`sT4Dx z6BgfS;xWBH@DMqoCJM*rRrm)CpGgt*uM|1lV~>|_s}MvGVw=7$ zPwjK;lhq+MerTjh-S(DHJy05Iaw0YTk<=e7dw>_*H-88m;=76|;U~+;V6Dl|8dXOY z?pMD|OGtj21oQjc3EgE`AEQtbgH?NFlGFs~13O5p|7cr`%9bqL{KPPC4C zjUVxWsDnDee##z6m`#56eJVxPvOoz_SZ<{5 z*BuCN5%#Lh5=c<}+$DWJD3zf&jH&DzQ8skd8>reB3IT;8mes?e-7D{L&u{GN-MW`S=3iB$bIV$IS_Ghw88AKWK%9wc*s9DTut*CR_GL-?yk4B7~WuhTnOSo^5ZwPGC8pzK{hz24nMvpZv zoTyoB^f|@j4$3`97n$r~=~D1V#mrGSHnvLJ;Xdi8EnGJ$aYI-(_`+L+%_Sy<&9FaQ z)g`-pUc;PO8LbOfCW`wUGGU9X6ws>rWmjVk2B%p)rCmULQXUbM{Tqfq+gjG;NGe)2c^8mdmB zh_oLO)Pn!?e0_j~H*Rm>xdML_p47IS8r%;}(+RQN~T>Sv* z$nRZ#og{mg3_;w*fNVJPI6RwWFp~rp7B;UJU)Q5v6pw@2r~M9=KWs?%zy}I}voA4- z93^g6-7Mpnj=>m&wIjlI;|`p{G8%eUd+C7n7Cg9~CBu#q<<;NF=4H6tA?YZt*xl;i zNy%mlF=(26g{eE7A>BL%= zm8scCeKGuE4mD)+T})oYoL0d)Tz`$x@{Aac#4i>JnJtU9By4Je)G{|z8;$|3;tc43 zFTUo{N2^lv#42JQnz0GTgmZhM$!A36-0)=95q{jBdnOz_*K>bdFUN&}<>Ah!3o46_ z8o=eD7UO9j@<$yv=4^19@7C3=zgE;PqovQ%Y|l>9Gn;DB$n+%FR^}kTkxu8D3gh8n6dU}>^oWm&7cdr3Rxn9ZNfLimNB8RNi zGqL;j5`Gb7uii-B?H1Yi*l!tTPea%(LhrbLU%bC<75n1v;&az(f|yXX5#}Rgu!UfD ztJe=Pl6zI@YclANBA@vZ-h$u4rk>#=_*WM85sw?oRzX+Ht(W4n409AN%lKsy9Ef7t zik+AE$Ve}P$L>~_;Yi8v68mB%LjQSLnRSwzx}7>9R}LYhqxg(*sOf_DdEDhVKMn|kvCo;I48RY zb?n4~YQ%6T=lj!t*odeRdi9GRe2VM?_z|l)-=#kmbjim-oN(bkPPm@3g??bS;(N59 zXE`#+c;~bf5EN>qMktb=>B#xseMm+DiWRDmDC8y-35jpP{V*I%A;Xyu19Yj)_ep;w z6zA2%o7RZXJJCNrQw2CD!}BVxWTNte=qFf>e^dGMiG`ut;Ut1T|qChFHtBw0%1m0+{EAf7zAz?Wqs9&o7HD-p~ z93zOo98C_o5~JDcc8AlUe`a^{PRO-vA8a?w+uTNKr8~LCiSwKgIQrj>ujrd1bhGYQ zD(WV~%djQhZ>we5cA58h<6mv7MMvw3-_}-(rGN}+tEGHbys@qJoAR8|R{Je^p4V23 zYlTgKw%W&wc#dtW{T)y9#YAYEuz>8v+kj)sp0}+-9+lfV<#F1!c6pq%txX=|w|z|> zW!sYSaBaJnM{2HjtD7e)S{3m*b&ogek$r2rD0%+?oRPZCovC20Sz{f8@9Hfo)asWc z^nA2xXQVfY!Z{vLo6J(xN5+U**ZMVd`cmuk3A33sPshJgP%8uXx!#ZBSuyRVn~j4S z9J{46!EGA~7B|H{&5)@&q1gHp)I(K`ZNQ$+|?c%zC#JAJgkvO`ELydT%d}4)Z8uA@TXJ*J7JKN=JoX88%mUXGU>1)duAm~ z(Kd7tPB#^0ZsOH+{e~i&P!i1B1DsC+M<1S z;UL0RON`V>IFH<+3*K(MY)ZltYJWSlc}mFhoo9?(U-pt{u0(ihNoUDMhmq^?Jb4M( zsNj#BuVahGed;?OL+yv-lYo+VEYy0;A1~&2RN|)sWK?f`&Xxttp3nt{J4?h(zaK27 z`Qy%Mqr^AT*IAN%SF(Rkb|X7UCDGI=f?PD`3}~K>)5Jx&Tvw;3^~l2b2dzh@B&yrD zCn}`X(Y146Nqo+Z;>b<=VLH*fPi4J$rfI#n*RhZufG{NcmNXxaP$=Q1{ou4w@k?aq zR0_KCJ@u%*>fui2`bLg}bUW9VStbj?g3Zwkft?AEC8#^2$un$%w@_SYgsX=6Sil@> ze@%Fys0(=;dEV4S;Eg&PRG9te~o@L5aaDUqx&^SqvuhieK%3O(O~zgN$l|I zQBhMS3dzCl)dQrkct)BS7Vi>uQAol%`GU=h(~PYG;YoyIuwGV8R8RP9wxxPb`aElzT9|HFxTr%39eIl`<`lPl#xw_Tyy840w7_gS@uky;mG zsfM%&me8!!Izyc1TfCuXh-h4bF5`G%M>mPb%zR${C=3uMywl%;BGc4Q=;<;t`Z zv423rmGGB5Sapnz(s1&aJu$&-2r<>E$&VOzLeKu4-z@&s`N;Ei$hDnnz|T4!VC@lx z*;uRYpaHVwtGajZ#AwkhS;tZ*~l&TI@0BOy_GjY?xV@pO`ps_YWLq9^{cZO-*TwTlGKSZ#`OgTUc~D8g7QUl zT!zDy+>^_WA^3YRBE#)^i3er>mlifV)qn656J#iAL*mjt!xQAZAEJoEe%NZA6SGrW zk4`5<*KWumB-BV+MGXwMPU9HI;b^W47iPHA%Ylg(8G~&iE7exGX&%>7+CPhSS&}!z z%=fb~Be|RU&$LcESJcSdGt;KWFR~OZ)X7#|aZotlm9%bH>g1bv^N42dN?aLD&5(tz z4oW%4hn>ZQ_Wp_%`gJ{R2ZcVnh^pBKmSKVvWjKCF;mi`2kT7JwK9V3}Q(dD?i(SHH zgCmtN1Ri?Zk*r5l+r`pSdIOpg91jj5aA)h*hes71PIPi~&|vwU^-QBn{Y*DE6c1y4 zn{9nFkNc*5;#y)LvMVT|9_9NcUdGGIllsLhpC~U^Nlml7h&04hn+fy2EAv~+XQ#XH zIl9Q82yD?s@bKBtPD<6Kh^|l3g{ZBc^$+{CLd}T_FESw^!DgA~01`7@s|ocDiwAbl zsJT?skpp`q7NmyYp#vDRvl#2aPdxXDRzop1b0`xtK|X6Fv!VpAn1_lR4fe+0iPm(* zQZ*RW3y?300Qn8nniQX>>-dd0q~8Ew?ebVm>DplD-o4=g?SO#IjS_`nf^{cZDL z_9uqe^vv5H^H{W zKOk~Fnyz<8Q}xQyJ8<)%3N2_%Us2JRu2<3YTz@oug;(80QIXlT*afI@PKFMW410|_ zdjea{b&22wMuJN!3>K72yX==ru4@Wh^D!df^K_LiKaG$W^|(xay8IkTKTNt1>?$RN z{?wz;K|ZtKzn#3NM*6=~y~C8t({r))3|?YAAFZ>VKVjcERE-x{smTHI3!toZQGSbX#R>A+EMG#}NKT#4 z!=`Lj-dIi0Q=>ZXEiop|a+j6&;Y=xaQ1X&f2jr#DZ*-^ge@1q?{H*_CfuDb&*gi52 zx0R5~&Rzakq0akR@lZv1Z{HE0$T^RG6eU@$?*@mzrhP914yb=Z^JE4qGuH@YYcAAf z@Vh($8t#vPi@8}L9IQm`yBj;grX&hNP7quFGADz4;e}_WYWi{S0w-qf5H?gUs+Sm6 ziOG?Tb~OWYxX&h%SDDP;7D)+4J&je}!~TJ=XS372(Qc%pc9fS&gCpUItgP3`;f-U} z(Q<(yoKVdY7?RR3AbbIp@7KrlQTi}?t8Q)@66UaL>;)XDil4Pr7)Q<*!S0e?>J!Sl zzhueiZHTu}IBgWE$ru_Eq+wHcvE4ADzacB;Fj>by9Z5umP1WXnSALV-qzzshtEU

cSpB@4DTK1w(;Yn8o=ON9*%{nLC< zYvH-m33feELB8GI}B5^aFoq*tEtF1Nf_yC{GyMv@os+YVS4p& zQKFo~YAOBFJFBwDlB$^h`6&0H=67X&!8i}UexZ%nOz+^!CE5&c6FdfTHn{VHnxuxM z%NOx=zg6EOs?#X77$n8eT>Pop%&+kF&yfpyQR->bDywo4i@-x`?n28BZB*V=;E zbz7)mOEvgYSbo-bu>uciD-e}|PkS*0Y3w!O9jS&xsjDbhe%nLRkicVN%NGeEyrnkD zyjuAi1?%+enffg~zjA46;~&V8SCZRd5}1=v4=|DsJw!E|Is*m|a(lrGB<9mG!D=T_LXxcW=zZHM84W#*ooPdvJ4lXqAaw_Znu z_-XBhOl^`$Wtz`c*N3IrH*`j@{03xn`~(QG8V)K{Uz)s|7z&rFy^Lb1A6fNYwvGWk zFhi;iulKxfXvmLH{zL37wzCt?nwPU<)M>gC2hH+!vc<|gfu8yEJ$qONntna(2B!Ac zmfP)Y_cBt5>q=kxHvw_WqHS$@?JCOv?C@r*+M@FYwB zZWOR01dUr>6=KsdZFwU@gSPaGD`c?K<&&^5H@Y@+dRVOkQp)xIS@d&|dEzj-PyNjr zEPaX}^0H+!BlHHdyjKcyhN(;$R8E%vj`xE0RLyl!bH23JD_#h!?s50ld+nlC+=HNd(Sj@S%9dg=K;%Nsw}VPTK^Xb0{GpAWyzCs-C72K$lUDl` zZ=xdS>!cbQMe2IDXfHY6y}V0H7(r?0pFhn)hVh;+cetb<33w&2x$Uh1EeKr_(x#8I zbR2@!hq2V&@JucIgK1a-b-=yJu)nZqk+S5xg+O)wn^rX=!663sc^22hQezjJHRHcw70JO`2Gd?hjxP}ap#trh?{DrF-nMD zbdqp6G7-`-c1Q%ic)!K5d7)G_JKWNnREGuGdP{b>nXcvJJ`C)tS6$>|>j>3DC$UjA47KfwQkwge}HTrVIh$h1q~u4o}$GGLX6 z{%T41)ogmux=t0bBV#snB_9$c;*-{O0|Y~ui{4^Dx>PCpFPHYTt{c>uALz`(#M7k~ z1PX_|U{%Y(Qp=PPb~pSOZ8sAZ2SiMQAqHhbd32sHP2)(?7U4@wQ1ys?;+VuxFbjsc zTU~jFjPUWkMa{cp7MH=c?!dZM0U!8B^`cHxZVl{iU(H%YTY$KM4D{5}fJ|uCs z&49N0FZM%TZXD-Lf$@CX(ta>`HJ{9z4dq|ctg1v`9WrD#oZz(Ku{P}L?{Lwa@39WS zx__HwGc=WOsb7*+7@xw~6LTMX8!PTe9w}cv0rzAqU4GsVWT`~Dof(m-ZAU9gY_`?0 zt>^H<*Rg!9IUF%xNZ*ZLu{*j?#Vnz#Oreysa@-8wj2ht0wJR7l3^vyVzIQf#+_!jAGs|aNSUu`BoYK zab+wy5W}}AOq8Pz!dGS{RQg>NKaB z%B%1M5M8HTf;r#xc8gH=Wy@89PWI{h=@4XvK8pS!|D?@}wVuori7`|DI!=q6&j|oH zz(nl6r0`kwj&SXRsuEx$hROmm;ul)}F#vklSX`<7F0>hZ*7JQ_EMM&ZY^XULHZxkQ zI|4$9K%{m>r5whe?bUY~MK-RijHOn3`M-ky{TUQHTcq~B0w*$C_t9|7kk*#$RuhtpS$R_lmqGg|OlCpa-^yH}6UzcE6}%`!A?nQv<- zb1RJCd#kRA)UMRM!84WSv!I9IzF0kU_DJjm$CLLPr@sKE4JUVnKiV5k9?B!jDb8My zHBxg42|)!6o7^KLSPFjZ1PXe29--0uGfo5J0qJP_A_l||7ks^+srVFc%I|xK%T{RN z3;ffe|K0rjUYOaZ=LZekwlq?Ae&YJiOpuu`zm+03^>0-phv5Hcg5JnaP-G+XBXdHy z=?;R;2p3=!^ki{RcekD`wL}J@Fi#6w^*nuu;8)FXKIZ9PN6gcgN6gdT_^js%Kg5x< zM&kH?J59mDG-*`xWK?bLmRV!HX|X2zJNfDQ@~5Y(Nj&TegpDBv)*?i(}A*qCXJ;o#f}v*dnlG#H<0E0?tEiu zUxRVhWppsIaoub+6SthfDO+fKn%I#seT&Fzkmcd@Cekn?o z4whJ?6935jC&P;^O1ZsiG_q4_TD$GKkSVt`CE5j2G`R~_novZW#mvJTplOO#eS~Qd z?LOv+=6a3bvG|mjiKelOeUFCPX>QrhfX{Yu1a6-Z>`lBbbu71-92l8Klv&<9or`qz z30{KiF>aH<{oxwI^UsNQ!sZ9!w0;+}%CSYSmRUe)LqtPsmvN06R_0_jdV;}!8hx>VnR{e?NXoh1x5x~3D#9GZ=7o)|X z;59C|G8L$9bi5(EEwSL?#P4f%L{p1w>UwE)1lDtgumEd8c?KD>s;zKwNTX>uZf+3~ z!8ZklcU%~Szu;$>Y2) zl|@0d%AcN@-@J!LmkEuc;*C5|-k4rvk2yY(eZ=w59}^;XI#kAp6zeUc< zMqsh}l5VLO(u&VCQkS!T(BnU2GI~bTPj~}T=Dc$aI-E*R_er@1I;SM&wG{Mgoh8BK*DG4H|w5a7Wve z&Z4u=n|99X(T3;AF75&AS%Hir?o_W?=B6+4=SXK!xOMQ-1Yr)d@L27Q`x;FNRS+S+ zM-9?ohS(ijxK*7e{JnMXoYiH~JhaE`D;(j@UZV<=t?>{GExe_PvWE{gp8R&~^f$S) zfvD?R^`jfb4id%|(&mw*4Ebr2p9S*M9N8ETEK_$-TOT3Yn)!rt0!CTlgh*{-qj=G< zm-32y#N3gsRX4&68UK?$``tRZn)QA5OM`4aV|UUs2{a3-Z$jy!#5W1|>FBaVIOzD% zJh8QQ$PFsz-y=BTRK#m=*2uB>ACV0QBeftdC-H%zo@dVBo=Lc$zThHSbz#7OfTZ_{ z#~7qdJh$*aoHfzU|9c}F69*L+Hqu1H14F%@2<{I{#7o1b5uLtR?|akf6F>7g-{%RR z`7qSw^n7W*OwEAeICaGQc67kiti zMwgMJ%ic%XR%D*7Fb!AoB`nycWnqXJI%zS_wx?y`t)y_EHGgV}FKf-qx4w5oM`0 z;gdtPw^qiTp^dkq{N0MVe`^K*`!hYTf~9p|N?d5v-CB{DI$TQK$9bKtQ}~>6o86&} zw^8r5ip+P(AW+^L+E|%=QcPa0G)Y2~CLZV_&gXs|7;*!Odp5My-LLd$L6LP*K zt2FvWpwEp>Kb28HC@#}}4lF3JTo{n>7w~J6A53|!k>L>Ca^fn4C&QjNF)vIjB3#P{ zIfiT!ri))9zk;)M>R0S{)8I+9kwD1eX@!sZ^*hn3r`cWM6KqJBmR~rQ!4hs&Sy}`z zk~A$H6Z)QZo#kecp5tbzh?@8Kqvl_RefdLsOCfzd$3c_$N21j?Nerv$HcY*@g$RUh z#0h1Vzb9s>c35Eg-6c|TkrkxeVD=Nnww^0o^tCN=dy8D#BG5#2Pk2+t6@XH0m zd7{_8fROnT^oSddGq5n^IQ|e{^5ahu6S#+SC_6zm zp%^D5KmKQeJr>5V&*jU1flg*4{Ji*$v6D7fUb9@39CH$?l5$)NTHX zyLOg&$N3s_h(CkJ6D$VM#`4^5k=eP8305gux4#(VJi-a!_SCwIXt= zZ$3N$Ojb$qRk<#gisir=OpO3?CHRbMQfmhZ7D zl%($pK4^v~9xmeg`c}6$+!?dIksW)us7PM@GdpAMH@MYz2Xc=((nsRZ2p_U1c4sdW z3Y~MJ9x?)9e?;`TKRI~fYIo}>Z+fObi(A5a!$3GBR|9Ws6Cl`YjO~FHyWnp!S;F5I ze%a!0GMk^_Zv}tEay&oQeQuWX{$aTg4-$|*L^^TQbD0LtLnZe~nmg6w=x;Rf5KYR# zTFn8&!H`qm+INiHt(6p1r=2Y0rz0D=X z$HOuqvN*J+ulML>ed0=knUNph|4OA$t-w>Pe5(e$J5~NQpAOCRNMCwo*Fq@AI*Pjz z7vNtwrJdgoVEl*ow%!wF;<^EpI(q7mI4@*_gPixG2%gUKMV3`@nispnSx2^ zZ8EX9G-p(Qd|5}jy;a#d{i4YXP$I@ALoL@D(CM*c(B7Nn@+cqgl}wjYGE?Pmb0k}> z<){+O{9V#9v+Fl@QFwCdYpkAL*BfM77msshi5MwWBj=jZzNg6-7g&q!r*Ef-fH^qoD9*W(;mz(GW;(x>`^B<@6`w_z>Ntnpk4jifmg>X-3XoiOur_;Q|}(u-G`_#I{A4d~h&v2ByB# zNd3Do)70XMrK!b}jP!csR?k*gjtwD|d4b|WI(`W`n45?3G#`!hl@&wk%64`I*#7hv zrPdeVc{;wMOT_|b)GiF15kI^3T6_FV&*m_KVQg^k>Iqfb(_gixCsek^E~#)jJ~67E zPkewFEvh{+L5#}fCv81O{Z=G+tIgq<-e z$M@^at3_q*ZDiSaW9jLo&_(!3;c?tv7>aiP#o6fK=EHsA+7sh5YQJLV3Nz2Hfgx;_D98F6WZ?bTHOh&ewaZ%&qG_6r-H4+j_Z| zmwXTdy4sdU{tUVIubvZ+Y^i~ENuC<7GrN=OtoQ}z6p<6JLc+w)ke}0%gO2P;X7@Q4 zN&fhATE_7(&Kc+C;S#9&d@_%A!>gY4!V)JTRs-`if31HW*5%`mEWSg0De`+m9U zh3)jNn5UtS|Mzg#2796?RV(-67;K=4J;nL%oibbA>lu}+Z_#_M1zlvsc^-+~4-q#* zfg0SrT9c5AfyFo{)_Q4Ob$oI@fPZOSyg_`y>DwF7rJgBTT9>dT{vxub)~L$L6VK~2 zMaI5QKq12JK-}6Lz+;7K@*@~+tYrX)3`S#`5^KTEtAUS4tmrsLQ#?itMWz zEI>3^foDMz?Gt{ct^yk6@oeo;pZtXiqiNy;5(W-!anURXc0LLVdi}iJ2M93#PtUvX z{}F;SD)HcWFy0kc3+C1EhUQ9-dgJxs-Nd8mc;YuE3947N?y~s z`cCADM)2d6V~wg;aVv3411GT5=~oURSuIG`%F4=%#R76XGuP>5^(mjXf8?!newm|) z`I^2_5vTXj#$dN~+$okmXIBR*!;Gl;;3rL+IM*eDcnmiic{=LhfSD9C9W@{Mr0Fmu zfM0btS~-h~{nwiXJD~_qhgN-kFE4=~4_=dqrWZU$LDBfrzQgcvbvONl28k(x>2udpcrtx#s1kB?;gD(g6vzJVTqL#WbMv?u7Si6POO0Ho)UX23F+)24c3zxIz= zd*g_@6ZDw`j`R@?7Zh5bg4!|@Tw`h1B{E)V--SHV#z|^|=p?kj^^phY0{bHp`{=(e z^V=)X5vFoTRljPT2FP!-SFORGCtyzl?0v1mkOBNv5&*u3hk8<6K^kw-dzJ|(yNncX z;%m%JEx$?PC2~_I=btY9B(6i?%OQ2X9ci)3jMd+SUmm}1nqsC;@FD($&=a{jAQMd< z&BZqm4U#wm)cl|VVq-1D#_rv~Hx!iwPL8TunLOzcLMw`iS!h(7`9%?WZ>a zM1oHdWc(oW{q-7|jXXdm#UpJv)UQ9*$P^$H;syEZ1?O=hBP+_cY@6;tJCFb}{s_&@ zHv%AeI{$NwJnd=~mqqYk;-D7iR?JC(Wr52Ld=__%{%7d}#{@t}7b-!7kHak&N)10b zr{Sjvn!*r>;g6(%Zx{HVzt?YFz#|;n*oa}jO#RW2uA>0ueD5`D$3NTp@xYCqc5CN6 z!W0YNxL?SorNT%rcRpqFlXm#9IG{PYHTG(4YsRFl-pH%9ZHVZ@{7t|N zP8Fi1wi0)i%a(0r+ai~BZDr?$aI3l-7#~C=0ay@-ebqVGQ#oH1x%AmDe={(3`>U17 z!JEOGrd&QYX%~G~viu*zplAY_7t>ok}ho|Fw zT}p343<*i%2n<;;#1U9LR-YU*nB9-<9&cq1-ze-6|C{b?S1KIKy%Jd*qj7c+BnPS zAP5yiJwQ7l^6yFI$gmtj7W#dTm=i33TU?Y95YtxmN77 zwShI^&34(AUQR^gn?><6<>zYonJhmz%pCFh2RLmk~dKYUU81++@sBnvAYC!uDJ_WP9edBNk4% z{Z8n%`!%`HFgvPlfs<`wZUZrb-SYQ&1jsn`zg`^LJ^6^0nEkmyJVe6m9a#9~75YtgTvU%rx-5oMGgr|t09njISI1E* zq)M{I76!Tx)_J!qgDjY8=k5<+Rno=;)lQ>klUMAxUiCy6ePOhf{sn32z&_o9osFrx z+)`t^x>{<)z}fI-AD0H)=_?)Te!yD|O?;BklJ$B72c8vwbsX(u?;6nm&){>L43mrc ztQ{Yi;X*SyVUyY2t;!#^$LE;c0mrnWgzIoOHW+_0taODlSCmB|tM|4NFXXlAnc)eW z_9@DrD#`>$!#=WuH~KGd>)l-0z`)g4B;Bw8kGhODM=+S-bt(4?u(X=3oBP4;7Ul-* zCO3)`vc)xnM=X7z?oDkp_-E^GbZHT_&~KO%g_lnmiN0?n`eQWu?i`}9$DQ3~m|q5a zqF~Hoy|F1Kbx3B^5O7U$W6}b#!~NDQ^FcdzE2VHq9CFo0`hKmns()i5vp=V6Wgt8v zY`anCjLRz=LRYarP&;S+II z9WSZ1z|iw4w!1gq<7OR54?4-|zrf8($o%KT(sv${4~1BI4w1;^0MF1}4DAcpC+|`f zuvA2qzQ@3C;Zh1NIGT5=M;Ie;zGaHm1YW6Okp8z}_uqNubHeU7=uE=yxvWV>0oKa^ zH_PVrukD#SaQR|Aj5y*_MA6GniBy33mFWGhY;mw}otuaSSCX$rOTTpm!e$Icac{U1 z%7p+HH#?lvE&m)?? zvKkv)|K%bz<2|89mxuBwrt)_h4K7XSK3W`0UFbkt75SPj>p~OTi`%gua_VMnaQk3! zUopLvs~Aws@c)UPDo=FPfB)>Pe?~u3yQ!_-I!9|4X8AWpOT%TQ>g(sq!gp=3-W@bo zo%3y-Po{3+hv9w9{I0egJXOjIkb8j z{J^q&tyuIsOJ#22Q-xyW4sOyotIK~xD2DdZB>1i z+txi+9k1Ng`FU;awCa4Ot$M58L4EV(FpghjUqDNT^+i=GukOm_fNap~zYE@e5{XFd z1&MKtv!{JLZ-k+rpOHChsQs791gv$Z`oVS~!nqX@^Xf8O@#5i6oQbviO-}s?tMJDp zLJGTlkNSHNLSqNh;^8{_UKUcU?l-jnOK_%;J1Kukj%Rc6(o{)Cv>4B3;+%>w@Gdmg z-4HQnySc~LzP>6P%&a|w9RsRxOUO0`RV+~c&XAVX+NF3W7y7YA;T-i9MhiFk1v4u= zk=j*u@$}SJaMmn)g`A#%>-E*2eHIY1K_G^=f(zs!aOLk7eOET^D~Vu-jf$u81PwO+ zj3&A4s95TvUzWfVx`d&G#J^yOKHnsag|$TB>EfQwu{B-o``0*u)zi5>aA;arqF7%R zA#VfI4)G=or3Xw}Umt8eI*>M8xVW%`tYGTl4XknRfts~}RV|lXJ|w87C)a>MLk;p| z`fnG?tYH&w$7e`fA?G*thxw}zcGAl5G#oWPB3|-v>~#E~Zzsx(zAj+_8T=j+8{bl# zm?Kkk1$nU+E~G9YSRyPYB?Dj_gK2o~V`llQqMP?>+pFr?hR%K+O>p4#+_R_e&DKdQ zts|T&=6;UMD-?}%);rDdkNfiWeJ$2Uea0MqLeO zJ-akj?g zIIq28#Yy>oO#BEV?0|9dj_Bz-)dGkli{b+r?^JVnv9?Uv#yRU^UJ%oTt(!$Uc180M zXoBbqk&8DGfFXX1YEd=(>J&yEbKSZ6L^QnWHZ~u@(8Ycypz!EAmU6c;Ab}(c2&G}@ z@A@7W8*N^S{;coq!c8pxIlWA?bc{B+IKtEZThfIaP$RYHCEQ_)b|*(_@3PAq3(}VL zbj~W0SlX`WM+c+HE(bA~a!Y$NQJ66ye8Id~-6Bp${CcAGtq~r2E66wJk*^b-ch#)6 zZoJJzQP~bMXWeL7&E1Nan&*~=OS_h)99wb_`_j|<5eG$EZ2v>Lv|sw_I4U_t@*kKs z^F=OIy~1Tb#=eg+C^OviV2hfcz*QACO-!(y$r77YP$8rQ6<4C2sx8Cy%BMv&PEVd9 zj6QwQ`|mTW!cCDgCrB25&fr9&t_JtNMU?V4O0}dw^?d$AdLtr53WfxGI_06fCEdAM7)C^HUhNPwB&Lg92Pm zu64!l#?LzALte4o?l8>cp*&(Y$ZduT4b!v`=k6CSBjGNwdw0lqnF2w5?F<29T74NR z(LAEWa&ae_rT|sOi2P|fZT+cekeDTn&7`0{0o#^A2ezpSt32kf?WtBWjH*4#Y2{Ur zC$AquDb$kDQyD8QeOLb}#nZp7B>hKgAnzeK%QZuYqi)uKTovkp?Be63Pvz=Yy6PD9 zU-C?DU~CbIa~NAZVp77T?$=*>QUCh^&(r!dH&PP1rYZppuJ&cLNWh3! z6BO|_Z41FE*VVHHdf5P7Q|^50zKgW$8Y#@IP`<*{1vep`%hgUj%sW+yM#9nosdp0a ziHX(l#~>Jop!XM`53Mij?>WnXtAU7}V5MQ|bdrI3GDMz*cWX|&;)VADfm-h)DKzu0 z3@Q{_mZwlDGTI_h{K6KA=JWD2ityDmsxB(58oj=C45Mm$l1WH!5l4*`U1YkxhfMG2 z7aRIi_>3R~H-Xv}={ZIIwIFWto>lj;wn5`a*@44?Q-$ZG%JI_$4F(l90 z-ZF%NnXa=kJVejo4~me3c(eZc3x8LRt|Va5`{O=UJq&#v(Dk2j61n~N#*V+nxyW*t zS~XSgfr`)#ZyAuq(^_ri+)oe{vP>{2nhS;+IFeLWwmd~; zW~;QiWCgBhE|^x@Cbl_53u+;*{J+;Z*F7^#_B_w;|9(F2|9wC2d&51~{XN%Nue03e zTxYq%=oxEgaAJQb9%)#&D-Z`sqp?MUJ^Zs+>W=J-y@bj1Bq-3Ydg0qp$f$(y>kHE- zl#Lh~YsCcejo9!@i}bqQNZ<*`KC2s=L^tu%Soa&K8?oUn3xB|Nddxq@LX~`L zZ15Rf?`Y}+W37vCrxK4&B{&9QidHm*zhGYy{9m+*Iw7dG^*AgE54w^zNCLJ;9L6I0 zU56==^>E+=l&?F32@s#!FnVi_?WJD5mHU@g%`dZB1=3vIK|i| z=2d07mk(Fd**7&SE9qU&wM z2iCuX{1%~pIN?sb7lqflPE;>ih?EIM5caH4{ZqH=IqEa@Pegly zUWxeV?Xo4j72`el@3KvQ(KEk5w(j?V*meD+JzZf$`cY?BuC5>vcqNVwDp zARj9RA<$wR#)AQv-O#EUa{H|IHv?wU8TDm%Jdle1dhz!tz+c4%e?{AMk-^ovnP112 zMYUYin4P{II z>9!3ciFh>(BjViVZqec>>$-<+PQtsQZAeE{uDCVa zOT=BI@`^`8A|cO)SR!p2qKLF@2q)szFoKA8LkJO{h51G^48K-I1A=m+$q07OY%(uA;d7&1F0vi1rMO~;WI4K(%PP>@m#AG|F0*PWz zM)`ZsYBV3>-i`Yp?poY*)b@9{=^*Z3abLuJ1-JHJ8jTlj9qz8UvvDuQZN>dG?pJZY zkGmT85!|P7|AhNI?(4X{pVMf%;O>Jv1ovyW-^RTi_b0gb<357>B<` zVBaZZoNn*M(O2!Ea>A?Zo@9JTGZP0Po6#K3ID?oE-)F*EebLrmV#HEb+D2TK;AQ)M zKc)aaDnZ)ty1*<_jK8(e_tL+pn;e7AfpxF-SdY)3K*5{}e)SqG?`+v^TZidJEI}ki zJf=5mVL2At^xe@3yAxBCjnLYd`qk)GAt&%^yG)b)i*7TOMe5K-V3YFpMzr?0bf)6E zMq3biil$)1hUPjTw(JA$hYv$7(#iI4JxS~o4JUmLXukax_Gs20c4DC-q=iVI+6H3~ zgNTYnjfkxnqYJedy4XLT6gudh@z$~;!~`E^hhQVESUh*qfU?w#Bf7rE8bkQk!BjS( zaph;nV%yda5wSwTR!G^4L!Ysw#4q3AUc38^Gn_6v-1Uo z_}Dz z9QhHUW|aw9RkO&Q+Oy)bjac?#TS=)7K1ZkyHbq30-Y;*%8chsh$t^|#)94#qn2p3#AiBOFLG-MTw4StmdP)RF zMNqfu0l0$F3#BY(b)99hHO5S)SuhQ6)UChg-}y-jX6s25d(;(D4dx4f<-ruea9EFx zz{u>QCijCVzmObGWszn3XOt*>sAJI`|5UCM`;x#=u9uR)pkF@%7YEvkvfV{|8;hg` zZGcnhK7i5c$`bY~h%yNZTK30hRAl4{iJZ*dZFq*0ls)`#iz7#7wWnYedX}nDx@mp-SeqetG zEO?K)yf~?yY^&!eJ^c$}0t3vj%?Y>9s?~ppX3*iC)T^=nKz+YJ#D{Ffw3J(DDcag* zBB+KA80*$846$u~pThM;+FJGVZQuVP+P*W?MrL41F3JdozJ8q$4H{}$573zh-;P0~ zz&YNqS4M)4JErf~P+#5B3;o}1+6?eJWrVvGw3pMDyWK=b{wJ_>+kAqn<=O|QwX5o9 zE#&LbGGX}F*jMscQcKInKOmN69=Aqr$1I@cDGb1acOP#yrP|`He{b=9RHxL#dVc-j#74)m=|o=e^Z_1P7_5buGvHA9;;seRFXe$6Q7rK1jv_tI#5eD;s`^)Z&*Ciacub3Az$q1iKGV`VM; zD%g&5O`-38lFA3(-zcGc!XeTVhQc9-F{Cky6N)Q)h0vSI-WCy}N`W5;WtFr!8X?6wf%qrhgUR>7Jv^-K*?=AkJqvuYw!ly(%x-p`Oq73z_CD;Z=;%t zt{a4m{t3;cB_d{czwH^`QZ}irZRDF!bXm_>J&$9eLp0;zjY~%&hpW$_JsdsujK!^? zudN)z=Gan?Gc?5WEGzVF=xp_@fP>;HW2xtpZEEv-iHRr;fESfkWx0r7wC@>gm-73-iKz zbwnic=R>K0iTyV4ORR;vXsZ4lD?7(lA`p`*6jAU$&ya0SnH56}tCX(H_u{oNY((Sd6X|nh048D6b%CBb5Qn zTFQ2%`0@?RU%V*o+CH{BZ! zE0yYD=+vMW$9O*Vc=44R&{uzGchZmr5hR6oNkFGOKN0OCq7q3_!P;DG%@xaYeOUxG z5J8Bk4DSoP2orH9(OPi4f@t{>UJZJ&IEh{=CfjvH0{zY@_JC~9za@01$QU|Gk=Ca; zpR!e!`6~7uQ}-9&T)G6uSS*0A{8&AIJOLw8K*V&TFGH5bd=j5}fpqZJm=!R51?MIu zKr2>#9ZN;B@wGg^{HX}1&Jd5X7iV2n@BfHa6K9QXMu#Wn96m3Z;Esd2(x1nA`a5W? z(l$~QOn)UZY)MdoZ!BI98(t(e)pYn97hsL`Elv0kO=YQj={OvZl>UjX_)T|G<$f1i zeH8)T^N00ItdoveU%KqE$X)2)Xn0Tj>M{U2aqAK`V4j`Q(t<7d*A?(5xUc($qGSvk zZE4@Kn@-?rsgA`i%%#6U>#KVhbxAY9y6RW5gAXakH!FAQKHk*FSc-M_Eqf!adxLkE z#%ztZ#ylS%ygxn_pNy!{tuI6W5v>@qZlMUOGY7Td6nV4Ott8qHD`cv1BtUHV(WQSt z>9xEUhAjBPC60@N`tA$c@4e_{HvJWNF&ti?-;4-fK}2jKvL#Ekig5kK(sSr}W1D*n z@+am?D<;;jdFVD&wNyo0>o6*aN2x$DNPvy$bi~nPHVYw-Y{WkAxk?0l)a>k8b@hu!8WmqQ`&6dZ75S8dRfGP+Jky z84=V|w3Ai@<%OVP-T#WQ{v2=3K2X0ALX9u$HEVK9%OvZn21?SnweignP1PQjKI7I# zH!CGEZ7CnUiq2jgR|1ZlPlnHJ8)C6dvwf^J3O#eLjHuCLW0&0)U$#&iA71dhu6QXT zm|#7`)-iucNuKpBN+6|pCAb*PHT0n2l=TZ(6#UUDKZTNq5`acEjDA9C`rZ?fg?x3Q zM#WeUAHAp`Z-J)v~66+zRdYFU=pkt6l$BNpJ@L~P0=o6yzvpg7@YAl}laZ*dmgtE>z zCRo>rx)`OX$%LpdcQAlV&(MNqH5y)+1DP zQYD;JlamP^4gJO=vlJ9lk*85b$T<&uKMV^f8oaQ-a~On#)nj-ICv+pdq6yt0(jU`= zCG^D9+NgbJ@%18pzhjL8t?%5P*coq_ljH_t53A>0(8BS2juD*rRE#yBxHMw8W}8p*)UNK0)HH6m?G$Kw>befbs|`58^&f8__RZG-$D>r>#} zGTv*yyC2@=;(J`CvQ>cwgn_xMyf1mp1)5!{HxL_ef=^MiYsA(zY+h{XvM^M1^g3dh zMyPhY2hQ2RsPnJ9CrAuCv6hWaT{>X0t_!4?lPDr61!d0$QX0`y$NOmkMuxOsA1}?D z$>A_L6mpny^gqc#11-samV>T%6*9msi=n!0ZLoIFEu*;E2VbQa@8yvOR8C za3PjeKq?AF15!D1Q>ozW6CoBXxPVyPzlL~Py4;0gfS$LjY}Sx$p2Nz&zsaT#x+njg zY{ZA>wwniBEgKttBY%i)TU&HerHFhoA`fW&Fy&Y6mJwNs`82GEgn*jcz(IIT z-Z3gHbksE+z$Ao4Z|B7L;2LUVru&iPR|RU&Z;U;D8N+9Ke~aUoY-=$MNvD-rCevwT zmM}Or!tw@T*--?I)}zsw1adzf9gGj_sl-do_4D1Rp?EM9g&$(yRmt zJY>veBf<$3NL0n*@!sxt*9A_a+3-qU+%nT7-|MVR>bg+j1 zgD+Gof@Jz}!j#xPiXn*q%ShOB7=OfLZnU}uJw`w6Hwcn?Ac5$CcwCNZimpS?=~bv_ zQvakmilZ3scz*M>2K^F`X2~t+LaX?&gpZ=Te_WWhICTV$A;S1FhSnTT3(SOTv_u%6 zH{xj=`GX@h2got6ak}%z-o=5XnZvPLyfHo%A8iV7)vY>D-2~{+M~MU?0gx)I|_>Y={~%VIs`=#ns4DA1`cM(b9A zr72Bl#2c(}g4MS!&KiF?sHF?d3z7id2UquTLJh3}U4v)ZP3DbBe%vr5Gx9n z)lx==|EVj!9cEA#o{U7T({01@u9zCzMAR4EwuG7tY#tdMiPkE*wBTe$@nxTaUOP~w zG`0{}5Km;ITV%_AvVeCTYHdRJwY+!hQ8uuRdB7mVGp1I2cMJy(Qqj^w|A_8EZKU;c z+Zp8V1sqaiE%d4QtdWkd@ z!y)+%KZR=Zb6X&G?KZs9+H43mT{b+{+U(C}EJRo;Mjr8pu!|r;Oh|lT>Vq~S2C)=h zq&bE2nFo_x@$%|2BS@sjy6hC99aDQj*8%6>mU&(}MOlSoYO8H}Sj@sBT8JU^b=__* z1-^9L2r)Hm#Wn-$3ET0P5QSpQ>bRs?3}6p7Mc0HGEuKY6cS~=oDePBIq?jsCJ$(Y! z8uMfD0sb^(rsFkFhZvH$#b=a4(YEO|;>)jTFLCPr}84%^D2j*T|UMkC@;^4li(q9<^DdKa=aZv3mt$AB0_wz=LQnOSP>H z9(&bgO8Ml3Ytgtx_xwJM>$M-ULqTIRYPX{GlEudD7%VX@X5l zewotbRcR6mlY6DflhPy%Cex(JYH5Prm0z4R$(JT3m>8u=iZscD$wX-~Nt&2p5-m+4 zq)8!6#z>Q3VPY*p0fAwNH0&x3OQfM*8fv9sxist~4S#)s1}B^k&`V`F5eFGM+ z9iu~>+S%?BcCcXrn=t>7>JzvSx?0Ms8x(z11(YEyP82GikI=XwgHNM{!dnk-u?b;# zd>W5s1)encnP9f96AUZ$tud}#E>S4*y=?kbTP{JX)d*<&8U+W>CVD<5pQ#nL?V;x~ zYcihUd4%*}qeR2lb3$L(`mpCTuKdE*NuqKf3d)0^)mi~)GfVHc&~uu6uB7KA`Alnv zZSmsSzDWp@z**p=leT(J?vEzBmexFpQZDto!>jO4$B7lYu@b)ppOg2}6kH&^hI(P? zhFF1coD{(wp$Mi$J+0P%Z1YLL@j5ZOZSDzWv)ypwokd?3`3VkqE1RsxI`dC-e&&EtW%#IIyydRE7X*V@yThd(xSo0 zFSheIh>ga_1s$B)UaF@JcdOmH=s-QqeTWa1wsZ*#6zzLQj4%2^#x>M1hdqb)1*TPD zll-!L0$l-GdOxUXNPUILCTu5{S}fn1qO{{-*Qsb3TPd2UL93_r`4CnHE2n~NSCFfC zI|Pb;D|J@G#Ox#KlR|9?$RjZEBE|>@M*-17bCDf#KL`?S60^yn;!`$QH+!&PC^tXU zsWFu%Hk$gFY--2NHtQLwjdC=xvN81ygqqLxx(8-W>ue)1T~RI;M15-Wrvlw9=1E}+ zEi3f=GHg^3vwWL4!tPH2Qcp^RS>+THoUrmOY79wzI)X%JkX!Qz+gVyYikX;3n+`8( z3Tg#SCPD09+E$D7FZP7}1}qoDCks#rd~8qPRbeVl#gsB^|Am(q-p*3oq;lq0hdQ2H z>ax$RIAZ_jNYza@xxIXV80scB+SEqhL7}~B)@C{>C!vbxdRl)Z=I4iRvRm!AFudt* zHMO?z`whj!^Ws5LbfRqCC>ToZPiS=6ICsz>^gRzO7yZ*{8m#O@)Nx4AvwbLcJwJjg zq5I)aDv5(PW&QPgLGUhLUxX7Q3dlNDV|(7obBCyfG@6Q&Pob05jJj*RAWkN%7dl$M zIq;#S%dFmz?(bP2GM8B-D+4alu zq+h1}vaDnF%Y+7l#+{SOJV?o0jovx6V%|2=I^uNG_FPWA0i`baE6haSRF|9-pIVLd zU|-FRf&luQOEkXc;^e%(41Sf(2@PRTq?%)el?cEssP?LcXs(e$UC^+W1iem#re-CMo^ul*{ zL3-j2dML89{gov8k2q2vvJ!*c=u*$K$1#|Bk=oytXYt9}fKrb@2*v~Q^Tm`F3<_=^ zeo&{y@#Ej?P3>w)uzJz(2euj8ra&im;J}}!;7>!_Xsc&A9x$qGs?u8SfXf$)o7*lO zJpABx>Ne$BZ#ompN=uw=4^mG8M!r^K;H6r8+s%;+#XQjnkGCh> zg|&6~4)U$0)X8I^7FjtaF@{E`!>e|*L*tz>IxYZ;awJmyUJ{f5D;`B+5qzl@%ONd3 zI|C57Z8v^MX$jA1-xT^F3D!u=Y8}Gifi^!dMgDT%Z2Ua{B@4;g`YHAQRPb(W6k*lXQ)G*UAA zRmZyTZop!d-KOES2(q4Z4o+fotY4-6D4JkwtFNy1&$v{L1*h6Qrs{+K^=pNbdTLYP z(xsg>l@{vd{9&wh77}#2Waf(&=dT zn&{ONJS-h(sWml{*ur+iwk4VJ&E9WAy)e=QAiey=1fZzCM+R}`JH@~FSL{^k92ie+ zK?2RJPYVo)53WtnMV&~GxAyE$FMQgT*EJ(@<@cJ#gtBMwCEFHSKnv4?#&|2-B3v&j ztnj_QVU6g|ILejK>z%fpzV zcqk0AEtH-)NnSoW;!kH^{rz&{_kXQ>tE>~2o%w73EnoWWus;(;g4EqL7hj9DR^yBr zYc)Raj27KJjMj8~7W<)=uXx?ouQFnFIavD8tkXSI3^}6j9P3^)-n#BV{VXjuAsmWq zIU8R(>wI+StjpLAH|u(I$*dNf5IE}sz!fsNu?Kx+x^%rsOAHlA>8y(|zbb6PKj1%s zuaT?2TKREF^{h#H1Pjf3$}@CXSpMIvU*wCg5oj+)=~hH*8}}4Pe7X&dk?z57(fN!I z_uLP(uqz(+;bD$2o{M}Fs))*2WrA5M0gv!(k6)r zB>#hiG82M7TbF>%7S#=&KAJWFq~9(vOjTuxuFFNk3-f897FtNzzOte>d_B zft-c9pYqIu@@$Je&7v@R7fDhsNrfbpl2l1jD@mOo+C~&)aV(EhGvKH%f2+SUXuS)q z2CKQa39Edl{^A_C5zw%SYD&6R=kiLGwbou)_msz5-LP>zTDL9E7ZajW+Q*k_FyulF zGxXB^L4{{TJgF&55|Tc)IB)K1AVxN zqIX48ash99Y7z7(Lj&kR6QEhtE50liN+~!Qe<(iu5H&4bJ{H9=6-~>&8@X<%e7RX} zb=;Qd*6cySk;U_-jg4;2i?G1n>mG{W?6bF*@X`nO6g__KGU8`0>gx(`+>=p-SIj~c zUXhXv^R-3r2lUYjNuREg^u;=&*H@GlN%}~Iq)$~z`a&HjH&LdQyZ_!&>52={(A=c8 z^1qAT0o+>UuJh4wd$dk%w_qmg{qhy-xCK}aGuZ-=89%`++3uE~a^ro9s2z(JjT;ae z2SP-EL>mz=n~E%_rmh?k_`AH$L0;!bUgt<&=l&aconv{OV|krpd7b;;m)D4!%4^xV zzsrlv^im>rd6Ai`yvVGTyq2AFmKPz1ya)-gvApa}YpeE=N}`u+U&nqZ3ieveETI0l zQT@qsYlMX&lo;Kbhw=OG+THtgv|muvJYDV-e>~i!$Io19Z?O?&#bj!NXHgTJ5(@J; zBYvhNlC@;AW(Fgcn&rvVEYG55IYn)jQBEliF0J#1o3zeJc9~Q3QKqQsLq-ybMmpY` z2$e9&BLXCv?09b?RNf?y2vDs=M1blf5+sY8b9?>O`FF&w1@xM9{?Q#8u{F{^w%F$1 zRIRfFKzOv2T;~s-M~6sV2X8EmjSsGZ+9I~PzOChc-5_m4SCKo8_knIuq`z*ExA0Bp zAK#(Qw6dOL0duo%P;4?W@X?w04#wD)gD^~lA-wiSM9?NhnP8Wa6*$_w7*d z7Wo)kT(?3OR4qgHuyl6{uLIWj*2Aht2Y6V}2%9>KyAF;!IGv#Bu#dz-Q)-!ishUQM z*jDy|c8rHl!9BD*s-?y<1zTt)nk2K}hu9B`_=?@xazkEa9zOkBbQ(=WGkz9TnXLFW z8$8&C*%vQ}&TFAAfm-cLwXqoFtlncehLISp-7l`WX?%>+G4j$wZMrtWNZs^%>X$^L zgYRUlw`IblTrg>`0^66CPe-YiL*WM^gR=oJ(hj0AG*-p5hh7_txXjk+nb4&r+q%O# z`4lR4JxCYB53UUJ)M%=78>+3S3i&--iG;kP6TjpdipTIi7}5@CDxk0SBHYdD+I2 zVQEs3?c8ES??8+mIYS3}A)-NaItXGU^=2YUCY!JH#0*c&Cb<>HWS!$>`xCle_>&Be zO%z*<{A_j7EE9p6@FkcqEsoxV)(jV|Pauj9O_URX#z&7KU%Yj7kuaTJNXNDX&Beh@ z&yXhvjAoc?4#XJo4g`rNVvUJLERAzSG}WxZQYs|wap=qOy`(g!ymLs_nhSg}UD-8(oL^HbXv6T-1w zh!f%^$>?oANl*w7WEZ6xN830!|68H5y$v_Tg`t||UbfA6Y%ztxrCyd6Sx!liR&QZi zKWwF1`jB2KE3LL?LLux1Wru| z9Q=U=;!-?L?S&PKFPv(+@L|tkB<25ASF)L^okp{SX*tu)OsklF!Sqk2ZSI%u(h4LE zVmy-RM5gyL&0)HPX$jLOm{u^|&a{^4?@Zg|$#C>cLzzY~oxyZI(?X`}m_E(4lId=y zrVKSdH%~#%la* zjN7sQ%`WcU8d}T4gE2>zDUOvVEkm%F&HWURI?Rg6{n?`4b;inwZB?AI|?7KuV>tz{R?HRrYDiHDnB#h{%n6E<3Pq$E_@qfECm%;z#pyU6~&8LQ>1oUtmOO2)Tv zc&`4NxcsW^n``^zTEAV}gG?9quI0a$!>0*Ey5`K8o06ZNriu?ZH7z|g3pC%7mYR~4 zC9MHlGUw$x4Cc(qNXfT2FOUV46d-oo@bIk`FMQ=oc#eQUAKbI=kv4P9>3Jzg51|m)qw*AtLK+f+o1Sq~ z9SuL;1QsbT=U|85*o#)AkCuBOScr{<%n!bjxlMH zr%am`D`Ek@%vXD<-c5x{;W_7zi{u>AZ)H!F|G%*f`)P}FQnFDs=H<+@%tOnu zBt1``k(;Mi%Z18Mv7{Hyvp5xhXIt>cUU>d(aCUqDRpcn$f0N^X@V`}kw`As}r=&Sm zdyU$j>HGH8!+Qr#&7G67C}kcuqjvML5!8yI?VN^oMIW7>pPDz%>}Vb|`Y38P#$~1C z=R0xrDHgqal{oVw^YT&_vs~GUX&$P2UTWrIC&$8ueq3(0nObQ5q`C9)M$yNk9dimd zIR&qi#o4(9`FiAEdcGM|n!OhBMRP0quL3yzsQo3`T;TSdlgZ;V4R%g4UiB|h6d-wvKb9nE&V6`8n_Fug@UZl8HV69uJdHF`FnJ}%iK z^Jti>7d<5c3$-<#$zHZ5Uz4iI)#Pc?@i$bHrpdyeMl&uqF9!8HF)ueYJwN~E{=u5K z^i0YBe+t(tU8|8bfX@5I9CLi@@|fm=>y}CWwB?c}GGC>EGo*P5(@M5i$CRU_*~IR4 zP3Q2~zW+*z{c)2S_2;P%PB)qN!A<6O zFpgwAj&UL=xtYS|Q<=^LCI4sPrm(5ZkU!bD^|;Nrd*aT+O>r&4O>tex^g&QEFUC#& ztinz1QI9l(a6iZR1yG9TCfpRxE4axXmMw{R9R(%+m$->P3j(A*X#j*GbS=?j0*}*7lu)w(bt6PGL8RZopQfSoJ74~nxco3JXIjHl(=7Q$ zrsYf(_O6@_2 zW)YrqK-HEZ1nokK#)1~Y0!}t)2y*;3{K{Jc?!R}R0e5rZE>HOHaCF#jj+C@e}Rg{NNs$$b{eO)5gkLb{iLlH78IOl8`opQ>;FIX?gFUoPaD z55Gw|ltXIHPza&+0RB^a&Bz&waXS3Uhd-I(jgW$Tr}FSW&oA;L4{6TP+}Fyz8l(Rd zpM1??xJ?zc(IGL_^?#NZxz88jXCPf_3LH}ZPvOfNkcU*LZm9(P_u&@`DatxSKB#3# z_2K5>Q+SR#mjhG)f2h`|buta9S#*;)-P}KNn+iWDCoGUi2z(>CP&vugWPn35R?BAV zoK|Bhi%`0z5cXW#ht@GnLnu_@bCKtB@mqM4c&Y{RzwuAzflM*Q`R}#JktbAA7s3X$ zLc<|#^;#gLmxJG|e>9&9cO)sgC`D=?NcDs0&E-p`W=Jv@ZYYEhNSb7NvsO~ZNzG3+ zj;;OGz!zsZ%l9;eADNea=e(@+Nb|h>$ca;9MIWjY%r8 zo0E`k$;?e7yN*lt+dVXrdNK87>d&+@(*UMDnFcZqWg5oR#B>_dWTsh6%}gy!3z;ru zTEuh}(-NlZn3gl$z;q+i3Z`3_zQ=SIQ`P^yj1MzC$+V7XBhzN4URo)i&P)TC>Y0Wz zH8It)fRh*}GtFdbW?IO!h-nGaa;6nbE16a?J;k(w1RZaOz1|ZWfo}e%$ui~nqtv_mzJ^^+bMD-r7VHHd@_XH9QhiXDLI-u)8}duQu0vG@<11B;t+mZL6#=6;6BaNbhBm>6t)v{7iyx@Q-#NH z1Ks3ea+fC_U<&uCnV|4v9`(23hiJ6m4`K`Rg8K^cSz!S`E$P{F)ANXd3;yR4LKx1* z6rLSNCSZJso34a}VQFc@77Kp~r{Nl+Pr#+8hsC(aSYtAc$<7`_BSd*k7409~qfMkS z`!ryRYB#BuDgBl19a#F4Z?_@+qjue425;Ga>Sa3I zlDQ1eVJ5?+H>1Oh?8PAl6z@CX^IXsbV3J`Z+IgacDTZ;JPNI|-k)Zd1PK7%&=p^x8 z#VH+-!$RO_m{ZP5{|kh#^6G1^zwzcq^Dn+Ub^5EXzxnpNGj(Ub z|KZ1woGyj35%a*VBKb`*n)8YSLmVac_ zxagQW$D3l~Cd4O9oRm0u%G7B%;WTO1T~hx4%>RFc{IzNe5Fq=5qywbOi?O?e8h^%6 z3yLd%F^z}l3Sz9T^9W^3I#s&D7?W<5E+b>oUD9P@Ou9|F5*d>&lCC7iq>H30nK9`w z>B?l>UP6tTu{tkP$k?CxMT|)&OIHcwjuL9h8PnL1u8oX4OQ@+}tgds|!kBchbX77Y z9W-6r8Q&tIri!t;&ZUO2y3V7PG3kEkI>i|CFXF0Wd@ExcV|ATRBV#@Dn;G|Ethq(z zcVEU{jQcV6XB@~lfN_7udd9RyfvzCN10~diGA11~U15w#r%acTaj=9M6XPL_6B&mv zPGUTaaWdnPj58UJVr*s{#<-C2XvRg1{Cnig6fY8{^T8n;DN`?A1%AKb&y@V})@LV*}$b#zw{_#*vJZ z7>{F|$vB#E5#t!f<&5uST)}ue<4VS{jH?*OF|K7ifpHz6>R!Pv-nCgViLcQH<8oX*(Hcmd-g##xNZ8Lwnq!C1>3kV?jME-78=yk=X* zRm}HhT+7&paUEk{#*K{q8Ebk=d30jz&$u&VJ>#y7Lm78xY-HSvaT4Ppj58UBGcIDR zFfM02j&TL!1&p^d)^dlchOr0ZQ;gd(wlVf*+|1aAv6o(^*Ozes6@9GG4$q zk+GILXvvH{7@HZlWn9GAn{hc~AI24oeHm9W?#{T1aX8~z##-*+)iL&9+{n1C+5zk% z)9cOHpRo^PJ!4NFb-#2#aPQ7 z%38+WjO!TtFm7brUG0GOmFW#<9Kcwsr`m)5A7gL5fSNGIK8#JOd&Wts`#@7M0I*vaY=TByUR17#Kh=(Dwmuf~A@ZVf6@u9a)qM_1K4EnnM&&F7 zntYW{<&Dy-hEL@#1W2_{<&Ur$K9xg?k7}RFBgs!qf4=5^;7Ggw`H(j8Re4Z34MA+V znj>`bZ<^gdDz_9r)juk~ls;}I5E}|_x>J6UyrRVVG?D<7YYI=5FRhiK{8PiHavp-% zspC#6?}SzPQMsq^)bvsL4}ooUTub$U(#O+hu)Rd!yYL$iWa08h^~1T`P(2BOD>YqY zF9fAXEq8@NE)KjzxOZTxU(WWao`s;4s_BxeU+%Q$8`V2XhZ^r}kPE?W z{ZP|I?SL~sTjamPe`+6``S~vXQF}r8qWYIA(m5U_N?z2CIOn5mPaHnVc7;mZEa5=5 zFXW>vVfQ0FB$+7AnH*7n9r>Cq@@b-7&ayp<)lBB_WqUHtUSH(=m?M7lKBCZ)*uHE} zqU`xX?;mQf)cPjd2M0e#q~E!HAp40<<%Hxn6(+6Ax2)HWcFegRj1z6Gd~eEf=BWR& zoJHB??Obl0-KlbmLEPz*a*45*Kl%PP+3S;(Q=~oKQcjWfc1z01(T+LS;D9?Bz#>C|BlEVypdMN#5qLvI9uUU4!bU)7Cj;L~0->c-Fa-y|+wH2ZM6>+2N z`KroKZEt0GarSgd`_t^@NZOCI=c}|I?-U>M|1SIbW@%roZEAg+;KV2UiS~9xO^@1- zkp4$HrJtlT-Ci#wKgz{@q6>ery?jXj)mBsXyQ#FP?`_F9Ih6wnZ=%zCpKzi*{f_>= zeA838pi9*otL=Rz>!F9pVxURBU5+bcZ<`!Q%xsOB`KQ>Ri1Am9%Nf7TxPoy7<4VTs z8CNkr#<-U8=ZxzZf5y0x@vDqA!)5$-F!pD>m$9DlH;h9WtMdd##)p}o$oLb+$&71M zKFezmV>9!gVO+v^0b_r*--mGp^HUjnv3#|RE19qM|Ed@-V}32;ZH)cdy*f{5W4>B% zf|&2Y_M4fn_G`RGNcla){7|;<$vA-dYWp0LlFg7t(+v6n0?=nA= z@#~EJS#NLeCoiJU%f#+l4N&bW~Aw~R{| zf5~_w<3o(MFn*u$cE;6=YZ#woe2VcV#x}-pGHzykhOyU3DUZh(2QaQ<9K`rT#$k+) zGBzn+q=7%xf zrTWM5QRiJv%r9ZSIxjkkaT4>@dB{wTuP^g6nXis7irD{Q%r9iVg>enL4`5uv{H=^P zGFHcBX14Fg{4LC%#Ms2)1u@>v{P~PE%o^!c%=~1w zAIx|Q^RpRmXI#v(7;nEt`ahC!Ci9mw-ooxXGcIKQ z491nr@4~o*`7;@BWW15_7RKusZ)f~G;~K_WR6hIPmGLR&Co!&KemBN8=1Xk?4urf# zrf)3so0-3wvDX;Me}!=XYqOnX!Mkl-EH6>csui#GWKG=o^cKH=Q3_)|8He{iuuWm zx3K#WjBU(+j_F5@u9?u<>0zh|7pxSVk&;};kgGJc0~ z3FFroZ)AKY<1LKMjHO{esQ{LKPi^?~N2LJ)h^e>wl6&eqYq9qAotf2q8rK8kko(QZqJKXRUiv;y*y^E9b; zIa6FhAZ1lfa^5BlT5fqsJsJ7uj2-(jB%gkr`Lye|HCEq)svbC-XPe|a(0#0RQ1v@0 z&<@K>>Tl-S<14W`TdCSlwZ~ub9qEyHo;`jN&$s8Z#5peho9*RN^3~ZlH9Xo^;v7Fm zIhXdA*vp~R$5824?UVk6c85CSRODG}?8|2uaPtJVupX#l=q+VN{)uwr3O20aLPMGRzioN{Gc|dhG zTh6ajyP(QT&T}~GtHkQ;yPSUyfyP>Gx8?lVeRg@t`K|@_`XT3)9PwA@eN+>|A!t2R z`N{dQ1@?Aa&MT_3=`^oS^N#8)JI!0u{G6jc%K1TcHl6r1@1%xCnA$=5m6x2?a^}nW z=HL^~wwFIS|3|;_lJmmq>I1nR!MXg%c_MX{0`Vz)XZz&8qdk!Gc#inUd1iH01Dz8> z<$bPQUJ|RT9W;2~$(bOv#Za+Y<5jHINI6ffu9A@JB;>AanLats=ZKG-$8ppji5=;Y zn0{6FH27)UbFXs!7s^k|XCg{{B9c=J}BO&Fo8a=l7?a&r?IE^ip|MV@sIkduf-w8Xt-S*^qB; zIZx}5w>+W38PmMKx=MxCaZu_VfAMVb4vTAg)kH<#4 zeQ93jz^(}p7Lpx&fgSdzi|cb2eytrp{S;jX?O591XUD?3=Ji#A;^%r>^~sr5wzjYN z-36a_!Txr~e3ZU%_oUAZkqBGQT^q=o+fJE^W?KcE82uTN3?R^7taf<-C03YfA*ml zh*r5xe^Jo;`n*K6yu)vT7HPL{B3u{t;LAk)cTCt!)L7j46{1xezY(jl-XY!Ebb(Rfk5lKZ?QsIlmVq#I^^K=vE&c~8*FZ`*Gptm!;Y&}8i%K|?q9+%C-B z7YZ60a7<9+&jWXmd3kn`pcQYO5)|^;N#+%EN(436o)NV2+Q<*dy!OX3K|@P_6x8_3 zm|bLU_IgB6{~v!Av|`!VkI1~}^G5{L{Q9$?71iM%lX>NgazQIz|54DYs<2Ol{U_E5 z8X8h3XvOYZs)YZO#tEAIQI?=ZzE23MiTFfN|4BAM8>2c@ll_W-F@hG=&K0zLzEx2F zbMFcodgnJ1mw4JJNA{=r2-k&k=Go&Mn{No(f` zTGUi7XtH*@pmmMk2^zZ3bFWCp@*#rO_M9ql#bQbEF)iY2lJ*N4y6#s&jhj1)_APXC zn4pR4?iI8=X_cVHcCQI)-gHdjz`q2oUE6&>`ImUgAZV>`x}ZftYb6cdDrjAolaha~ zSy1x}y$+DOx~M2YjfXP?t%$G+8v5i`LH$pk5H!)fSx|k~o;BpIyn|8Ds=rbNt=s*O zpqj<63ySh5Xhp>ZLCqbziuSxRd$gcM@7yh@=IjbV6ED9gsQKzXK`YPM1a17N-60CE zX!%erO#y|MiuijUBEE8k*nZD7i;H5;XDpY(bL;EETkF?6Z;@ zcL`c_`!|BtcDN?Zzw2>~+}A!8E@(y6Y(a}oFA>yy@6&>oAKxKpK)2o}`8bc~=z&p1Kr3TFx0_(P_k<~s`oEvk4(Q2z^02pSr`StWW1#SH9jG*R4zYCh|bzRVkN7{Wx@k#8| zT~Kpbkf8pWQG$jph!M2*=u|;9!E+^EVHVVQWVxWpoyr8&PkBbriqcmFt=q9p((m^P zTIq38(8S)~3tDx1qo8`j4MEM(zM}tD8P#3Ty3s*``VSr{XsiAm;w`TA`gi*7@_8rX zq~Flt^K@@V%=~P*&F}G}5%aR=hDTpK5iwI4`SLG!{Tb2jle1$p$L)>i@bdoTBc+!k zo*&vW@<;8s*K!!J9jIcqw~N{9e`VNl(Qs_T-z3;@*sC zA3Cbhu&}+dr>V{OFXx_(=)5bT_hD@t<&&S9Cf8m3C8FVC_gmX7@m3DFRhdg)s*0F= z05d;B{gw7PM}F7L?xJjc-`zZD&6$Yomqxz3W2V0{>6bQfH4{21Z|-{Gxv$1`RR;fZ z>AhDndMSTgyA)RUPE$llqj~?d`il|o9vb8Ao#&yva?ikX>&?BCb35)|^xT==N^%>Y zS3Vi&rO0@6Rc7Bi-@j>88)d@WrRO`%@2HHKTXggt#asDqLznvFp`Df6D%Sh^FX*L2 zUf9|0*@wC+!Mir!tMzJ$$hvR;k%$ilDMMc0yK9JBuo9jA?Xuc#mm{7V`?Dv%9yjkKkVz=O?i4! z{`FTr9H11hs(XL^pVuQEZPLv+xL`yV@JjN2am^0?FK71(l@_8 zBcZpFI(g(R<-UECkppMlDAYAa9O_xqzhsKPGT)NDZ0h565mG(@%Gk$Ze%(^pMtSw? zzj`#^<*#)4eENVZ-*;0!`SGn!ex2o~ykyAyYM8Z`GHTSA>)&n2317XXFi4roFM`+VGwOlqoN!91G4Eq=XLK z+o5J$fHLyayV`x2rdKv;2d?rR;iG)9c>Vj+AMUO^{jYnU?GsWTG5`7szn@bLO6Szc zr`ta>RPnuI?k(m01}Q7=EetEkyv?aR2aO1HB7 zrM9##Nqm=)MDgBLGIxSxjtlV1KXHQP(a3!(8O0Q2Pt=E_9?x<80 z%ZAl`YZi`CepvI}l>O}^l&wi?>JD!jrab3U_1TN7!j#1zY;}hdg9iL9XgFvp4Pp1_d~shDlhLC9h>*~5as(MWy#~7Vaf>i z=YJV+ewgAt(Aby%(zgRvSP{@MfdE< zmTNP@l&`)FdT^+3s8V+(dP8le^AVe8{n9?wzrWJq%iDE@LkB3wn*F<nr(@%?M`={NU|Qmz%41m z;S0eDV@D|~BkJp3whdOc_UYQVzjCmWG5cGK_0~{j!?X{7&DG<}2Ztx+S>J1n81#Kg zyWYhHW$UiCd!Ovy9P!$=PkUwsDavC$ePi3D1}S$Bd9k`nyOxMMy?dUEnRbV=I=fAC z=+EKG=&aL?XP)Y>tgKxB_UgkUm6|QLYO=?VPG{~;F%O0fR6-x^*1n{}Smnl) z?6PknZd1C|_bvVWca+m_7p!`8)*!_?aOj+bulDC_b@I{=j8|qK`=aa6!Z^j0@zvtH6Qh+rM-qNH z`D(nf>+_{^BbE(PI=!~*^4)Lim2VdwOV!W5O)(98?c&eHgOm;REq|uP8x-ALwxv5i zzEj!0dE)hX&xR|nZ<LwZY##aqe{&lKg$%K5P(y?pZUXSP= z$_CTX-Rb*`%8@%xM|M6iT$vcYVEUt*@*>846Y1`KMX&sM`Uh|G`k_k9S`TYej|s|* zSFfCz^1GsZl>Vc}dqE7h4+%>0_oLRlm}62JU-Uh{HT_P-@~dg<^$x?71IIV5>G5&A zlD=@ujoFp`m0oe(eRdy>RGxpv=riX?q+(7SP%~p>jFQ|p=#Ep%M=77}Foy2x7N}hK zWAOujJZx0zXMQs}_dtR&^H1Mb9`lG-d^f**^o!y+W#`O&e&#Pi6`zl69kUKbD_#2i z@YpRwB9y2H=XC0Gps%vb@>PQJ^gt!}mB@L|H(!gm&Fj3|E3ti)uM1W#o$%&F#phh= zjnH`$m0`c1{&B%31%u?{&lePonS6xp2+U z?iu&e8K-p}cYlf(`SMM5Ny=Lfn;C}pHGpvdAJH8?5nxV0I zk9NHQ<`qLnzdzubp{{DBd)xNc4A!zWzZd^?)o`iJhSK0~uNoduO46_Iy=ti4S>5^a z8&?heS^|P^ef+AyI{d4MtYudXp1*(m!}sY|4U>A<-dj2Gs^MsxQ*owIR}H6&vWz43 zR}Gu`p1y5@_f??-2wV7=rXI?RM-xP`eD~5`m`yLuR-Htb!4bPTpo2s5_HpErF z-uTm+X2Xt0t9A{vG#dieq~O2V(78?FZq1}-!?z#5v*gLK&4!9i4{GBEgDrSy62yl4811ZHM#%ymkfV;E{mK~d&#g=JM__aKfYwx^J|?~-#0HAtfN0z z@Y*w%3^khySH`csWboVnbO*1+mke*eI^xG~?z?2jF3|2iG~<%t&TjWuj>lXwl--Ej z*Er&mVb^zet_TXeWbj`4Y<6L%O9u1Gc6ZmgT{3h_zR)%KLX+XZsBsG}oozCt)T|t^ z`FN9|TUK?~tm-C1ME93`V&85ul;=%7KK6wsLsf0bnDLK*pSiMY=E^2R_xrRXpTED! zaJlc~`M;(#8D5GDUp;9`lOe57yID0+O@{BP)^49Myvbmkt6g-ZUz1_OTmP%QZ-H;B z%GTc}eULtA(+7}Zk$^>uJdCw!RsM4VS^3!71 zwfA0Yzs_rub7sXaGrtn!Czcr+;=U4(%$b>Q{AyS{I3d)%;^468+<)kmHNC^)a@B*g z8ebb0ANXd;o?Xuki@~de8{&M!qG#_Z{pF7ji(iyAX!mU$77cM_!WFh*@%$U~^S-!o zSlnN-@wrD<4U6*g)M4@4O~3wZ#*$(2FR4RC3Fi%q7sa`+`0n&!al**{KaZO-EWWxs z?T^z_v9A~KZ&-Y+_zx@Z9~=_Ti#x*={?m|HJo}>;3VVmdYX6M8yI&s?|9F4+62A|AS>i;^7Zgnno56i5GwA)t+|#ka+jhcW11gGbCPi<+Z2o)C`H4 z(uVY=l(#pHgMCIo6b=uHn53rnj|1*7!lupxAwSk1!}!oZ)!>j;{v9i{Ad$amil> z#7Arw1!sf@M7w^DkoYcmcEiPoUmFn9&b?~u*Dnl+x>-VJ?9&5c+)IPovO5OEC(;kh zod3jt=#1@NVR>*s%m_552O0;&i{EqVWv! z3f_lwV8ICsr( zZ@)ODp`|nat$y*!aQ2buul9>GzgVoh@Yns~r_T+n+4y9?`0&k>%YWtS7oSMO%E%}B z#T_l1ZOgXzi@%@MbbH|5elcC$kfv_v7w>rQq^x^y>KE77F1RPYx?dFXj_9`-`^E96 z)m?Y;ihl96cmLMrtyv1P_DzxiS~A~x;#?1te(5i!lM>GJpk z5%J;0_q#Q}kBH|#arPAp_eI1DKHPQbjju(-8|UA5mn#?%dmf*0=(A@d;?6^JEob;6 zqDxhg*w7vk4Y&K-fB$$yEKLy36Ca9*`%QJf^lpxbcc>O9r`>`5KX=dk#my0M&tI1$ zo?06b52vr+vGe+fc%-9$-^D8<;$3wX`y#Jx+GMnqNYeEfsm zl1bhy;7s6+*BILkJP$kr_yH&I81MjaH*nwU`0fXA3$PBT0ZhOO;4)wdP>Thj zRe%Y&7FZ7Gfu+C#-~xcQkHNe5;=f@5mjbT?e!vcBfaifbfpq`_9s~?P0T4#oXWoQ8 z-~b*3?gVZGt^t+;X9J(T#n>mn+rTTpPQU?d0oDU1V8Pq)377*+1=4^RU^v9sLEr;` zw$a_C@m{>Uy-HyC7Lt5$jK6dQ=g^}^LZf6-QXbKb-ocTq11R6o+vq(5(r8u)4Bu25 zZE~DU?*I#66W#QXPb`~`QC?B2(dASAqqos}0i?0xIL7v9ljCGM(mm>U&nTG;DUawj zx?IXPx~U<=*kWw4=-;%?Ya>_(c4Xo^GQdq(WbMd8UO+k)7_S7L1*}uiPl252jJ0E7 zZq@J5e}G$n24D;D2+#&R3A_Tl0qh4p2fhPx-a-8V4L~DsKcK^F+)#>~|7I+Zwc)LH z8QeAOh74Bi!MK8d42z!)pwK?FBN{^cKKe5Dr44!LAk4*Q*sNg6Q}Zm}&skedHU^$v zvVkr~t7SpxR7uC|a`MiU^0&oCb*P~*%0hW_@pIz3r zm9$@2Qkqj%I3FT2AFsQLboIySmP&Ru=+rnSr(Ie|cFo7zNonoJ>GV=sH*^S|a@N&US=Sn|PxERvUZivVh7B8xD^Z24_c}qYLyL7)&eHjkKjGta zS4sZ#jnEyP}Bb^#LsXb)f zS)|iKH>wZhlg?m9S}ZQ%Y0n`Yby!0pbaIi>JfMeK+DEm}_|zQRuZ74$(DhQ9mHc<( z?U)BJ9_E^PCIM6|)$MAt^i5An^S`wTu>y-tNA{f4CMekiYXX~K9LFZ)$Fm7}Y2nCZ zdF?|FI@-Bu^7y0jABF53wJ*dgSbUyBpOzTQ67!WTu_WG1X)%=8C$JMx&cS`M=kH*D z0`?i^1lmr9Jag1>+CP2Naml|0I7d&8XOr_&*<{PaaHcuKK>4F;SUGHemVtNd$d6d^ zr9`0@QsY@FVo60Tsd-6e0Y4-;K0lF-FNxPDP|VYDRmJmoYa+gvg}LuSDEZd@vAc|0WP-RXSFR<~Oj`{L8{6~aj%!MoA3$ZXRhK*YgqgQC<3>88QQ%q#> zf?3`_T8>o4c$9&%@{?G0-mx;WFz8b7!(kLt*{f(rB$a)FFQrJ6k2EF6?B#gIBc5c$ zlf*JD@Kdc|>ZdZb(K<^({mli2;3u(wANo{UV$D%c6^d9E13wi8!JIW+$);OQj%0@? zht$D|<_QKlA6irM%=x&kxPUS7U74-eUx)q6u}|C6ke?g1pOuloGSH!tBZ*;UC_ac1 z4OBPemjd>86v*eO$8dh3a~18Md>-Z=?9(P>C9*8EK~~;`NJcm}Xpc15#f+iQs>A5`!m6+Wae^)}gutkY1ga0(kANeU-~#s%Zdv4$9}LQid}L`K5o zM2wA0Hus0`iFVml+Fi4GU~TyS@NU8&zJ^=JWZC4FL7L?FIb!Vg|~}D3My*JkAiW z7p5e$Db^%51#LA2Z8ZgLH6<@AGATSEn1-XK=g=oD@ zN@SDr)7hkLstC#oB?gsppON~(c=Q8|<5E73Efd07vl8&%TPxV{qe}Kqi^(Z#);KoV zT86Q;9CoEF>U)zS>O1B~2@uABaUDxM{`IXL|(oP&}TEZGt_=6GI>yx#_{;pL@KTKJa!lr*Z2UJu3I%~&T;e|%iptdlXO zEJ=}sFh76bih}y3mg-4rOI(|j;(8z-*8`)j1=2EcO=CPZKBM(FbsL^R01rj|8Ff7r zwaI!I->?V#QD6AHnKcdLSh$BNBk|$bkRnKasBp13ZKjgVwBE_48t+11yPGNV8*yWa za|U3&UY(hKi-`OQLj_AD3>~52WL4GcH;aE?B{u8ot1>SXF9GuCbN`CayT)h491(|46&Gl z)O&DkFi&7_pw4KU)Wxq4<|MF_N}CiX! z+`a%u>zd}dbcHryj4|;1ZpOX<=$QpE#X?*3zKi67bLlUh(cwPz#7zN3H}v5#j?uFw z+M>^U)R5cGkyOY1i9Qn>Z5PYSBtyE#jc$`~gdC3UeV^&xm$nU@n>e>~4sw2v^AP8F z$`_llPUc*`e-7tMIA6=Tn)3$Ejhr9mZ0Fp~ImG!(&hZwx91UkJ=LMXv;Cwyjdd{0U zKgv14`DM<%oQF6oYvppXIcqr=b1vg-Tk~vT3oX@$4^A((>#>St9*vslnmi2m* zWp#~ZZF%)7Q(;YAwH06BTcE%vHVf-47E^WfOPfoLZ1E^%(fWyO`X)*>Wf-EqrI*2}9a%Bzi4CgY;|Rrm@Z zKHrH{RI(uDW-Yf^>SJy0OPAc`{8e?6RsYpt*98dfgm0Yr@_^iCS#wuJpO^PeA zR$X3wQ)M->V^5$|IRc>aaCPyg;X}%LirSz*IE}@EH${2t0zB3?Y}JGJEilh zkPc7jNrt)f%~H09_FydUy+V77Ehc=EQCLJJ61dTQmO*Ltt4eFAO{n%!!}CYABL9jiyGvM9Um0y_)*DyKKdWkr8nAZ+3c0v)rG>uu z*mh!@EUHJV{DHKTV!qL6G0}4~$1ywd;1Yb&6?WY*OKW+?HI~ID`U)iqGS#B9u>|tC zu-r_#!Wx8yC&y~iT~UcHQO=tg&%u$$Let8+HET?kXxhVZKiI9<#u$WemYdfm$x;)t z$A!h{HicC+wI|6cLci++3jIMR$w<(e!?UH}ANgDhaEt5`86Fn*) zPsT32llgHs`Az*;f8}yDKe+w4e*ItN{=*+8ls)+259j?~dHAm>XRIC}>tug^uv=*U zHU9p{rT&|D6Iz8cx`x_faL(AYe$zCrZ2v?S98Q@Vei1%GlI~WU#BSKX)cRqoMn3$# zzYX8YDjnyY|DgSRZvPDI-)cF~GMW9UrLSeh59fcYLrQU39M+V4cS8kO(3&zUpfaJT4WB0F=Fsl1rCSxcX?;d+SQPlKbeLU(%lPu# z>w`*}b$^rBLq*g5qpZ10P9Ns$tP1~5LP7I)4*m}gDUk0!(}w?OQIzkA=jDGiZPe~t ze$FqrN3N&FhyHi?Honc{`EKU<{=MLs%k6JUo-+>Uh{O2}^TtaQeTA*CfdSW(F&WHGXF3$u12FQke57;mX zYZc%F@f`r^i95Nx7p%ks)B@;<7jn4>{3+HJEQFq(6%XTwC5@0ZSf_CzK=~AdtpLSH z+{k6(W-b#y$>pcPySYq!=E+z-gnSCXYXHi#3H%j6{?NO11=inD{yOjqAPBtyT+U@P z_(m@Kz;6I#zZaaLk@L(1&jCoU1xEmCFXHoXK|}g^;FVl9g1rFw;{$)nG)>c zO85-E51@QDgP#G&zMJ?ISx@{m&;$Ju@HsQk-yj!&FE|y?03go;uLCG;J@^fPV&4n? z7$D!m-~lcVfs;>@?TP7~Ii)4`0GaSD0Dd2!w7p;oOUmm-lf#ThIltn2ASAZ zh72p|*5f|iK@SOnJG=k%b5Hs{j@XY{~yAix~De4pYCh*$; z`SuR@OMv2vfD^G6gXCoJrNCa;lz|@vR9IuN9o%t|+!j9Ye*icY}MlOneZ?hd-)I5EnrCG=V?k^5}ISv_7Qh zQpT=^J@InD4A}sF50J(w_zXST0D3KWEudX3Ff({3&JLB=OT*iL}*?*>mTmD@rCt_F6%#tgm(ApbXm-vp@6Ltw+@nD1ai{A(`n z0@H^`$ewsQmvg{XfYb-TFLGJ69PiEn((Aw;pb7r_z#{v* zE(Uf(-U!aRPVTGO;2s0!G3bdEt583XW5JDhdoeRj%zd6S&2Gd zgK-3Y?RxkIc`x|UDy-FmyaSxI7IgfN|Lh1>(4(Fi{v=YpRGXnrQX>mK+IeG@ovuk1f@ z(tUEgTJTmNOg7+`xPCYIYc3xF&)LlF!6q(OfWuqlKB?Y{Ir)Cf3CO1xJhe&gKN|46 z51>u4o}w509YAfwwqd*j8t6BIJpjd9@E~Jd+hzZI!Kn|)b4UR=3{YC)OCHAjg|vF` zPJrq;1U?K@K|gxk2Ce6)e-!ltJ#jPOgKP)C{uoaSzJ3SV9(pr4*N!%VtOf4|K7vfV ztOYSbE(PC-XE2%@(BHr(x5IzP8gM>9e(J#MfgUNpFa&uB{PP!4 zFOYYD^Mi7K)`7PHG)HX*2LP&b;_8=BF7#&bK|q7G41M5^mobhY`@rj8k;kbWT(TS2 zRnP~(-vGss@&Db}a)9Ec^*wa}_NT3hSsD!0??cg0DlYALr?4LmH`yI8EgT{ zAUA;@1`Ln`;Aa53t`37EKmamv{Ts604h{fibZrTqvrpFRz|R9V)HD4bm4m=S_(n|Y z+({;;HR>c2(^~ToWMW$Dor$uDX+5Uj@HC|ggPXq^=wMa1H`oUjO>YNeHqEbv<8f1 zVp{7(GBK@>A(@!g6p>6!Yj`MjVp?xQdg9Xo($iWQJ=dcKf7*VkfuCyN$Etxat_3hX zX*S720_3egsbGxm*somZlXb-lB+QaRUcIHyKG%l@6=hC|jF0;$# zYI50K0awr!a)n(H7jvuJ8n@Q1bL-s(x7lrTH@WTZfIH|8xx?;=n{}uI*0tVGOeL|xEU)K-S5D-d^mTOXnsK|K2QGDK2=Sn3f?BjRaBL~g{?ji`DMS1%&# zLu@06PS;t~sqZZ7G;~&Unmg+|ZJmvsO`Xl1_D*+aptHL(*xAz=>g??dclLEgI!8KL zm$FOMrS8&n<#uVi3c7S%MP2%?vMxhcMVGm&zRT9t*wxf!?{aqqy1Ki9T|Hf)uHLS2 zS6^48Yov?$m4209?brBo{aSy4U*|9K>-}YZgTKOW_SgGu{ziY3zu9m1yZr%ww?F9b z@rV4q{;KEE%R1*>%EQMX0O}Z?d|dQdi%U1UZqd%%k>rbihO0h3SYgi(bw#A z`?`HSzFuFSZ^WnURCnff7NC~OP($^ooo3WbH)^F9H8RqvL|x>f9*R&06^OsFs~NF( zBko?rJkq5^w7H112vJrb!bU{rMr1vRs*fU4;rak=ue7V}x%L8kk-f}bVXwD0+MDfe zd$+yE-fQo(kJyzCwIkP2;3#sGIVv3Wjz;tpH~LDCqu0^r7;z|D)GfIfLq#oREfp>G z7(vY#LESAqExmXzH`1bPRk!B07PJ<%mbF&2*0(mcHn+Obw|b<0HPWg?kILmessg>K zv8}ny-PYaK)7IP8*EZ6obgG@X&H`tVv&>oHtamm#o1JcFx3kCD>+D0nQ?{$ybK48r zi_rTj(DNG6>)hyZJ?L$H=xIuq+Lh}na22`ATotZ*SEH-h<#u(udR)D(KG%qgY4AK0 z_3-aLmNg!&N9WOd3?8$`=4tZSJpoV96Y_*T5fAgKyc)08tMlr;2Cvy`^EP?y-hemg z4SB=fh?n_PK8;W7)A{s1gU{@<`I>xoU%(gig?wRO#K$^SotjQ z1~Edz7@Z6wQ-e{d!-zCsG}@$*7{DkDVFX4n`cxQsT8uh9Mw}U=tqCJ7fKe902#ffc z7WXkU#Ox}&#;&#N?0UPwZnoR(O?JCIU=P|u_OLx-XAYG^gJ_qQ!i!Z((|YX^>?AZLP*US%8_b7c*lY=Ef1+)6mT~H5YTM0i!;E`=Fn;f4>^|PuWQ0 AbpQYW literal 0 HcmV?d00001 diff --git a/ZDistA_komp.h b/ZDistA_komp.h index a04201a..736ceec 100644 --- a/ZDistA_komp.h +++ b/ZDistA_komp.h @@ -11,7 +11,7 @@ //#include "pawel_usun_to.h" -#include "../tdefs.h" +#include "tdefs.h" #include "helper.h" #include "ZDistL.h" @@ -91,8 +91,8 @@ float Xp; // zasieg dla poligonu i srodek okregu dla kolowej wspolrzedna X dla powrotu float Rp; // zasieg dla dla poligonu i srodek okregu dla kolowej wspolrzedna R dla powrotu float Zp; // srednica kola charakterystyki kolowej dla powrotu - float Rrtanfi1;// wartoœæ rozruchowa prostej nachylonej k¹tem linii - float Rptanfi1;// wartoœæ powrotowa prostej nachylonej k¹tem linii + float Rrtanfi1;// warto�� rozruchowa prostej nachylonej k�tem linii + float Rptanfi1;// warto�� powrotowa prostej nachylonej k�tem linii }; struct Wyjscia_ZDistA_komp // struktura wyjsc przekaznika @@ -128,27 +128,27 @@ float Igrp; // wartosc kwadratu pradu granicznego warunku dzialania przekaznika (powrot) float Zgr;// minimalna wartosc impedancji kryteriunkierunkowego w kwadracie - float kp;//wspó³czynnik powrotu dla niedomiarówek - float kpp;//wspó³czynnik powrotu dla nadmiarówek - float kpk; //kwadrat wspó³czynnika powrotu + float kp;//wsp�czynnik powrotu dla niedomiar�wek + float kpp;//wsp�czynnik powrotu dla nadmiar�wek + float kpk; //kwadrat wsp�czynnika powrotu short kierunek[6]; //A2 float XKR; // wspolczynnik prostej R kierunku kryterium impedancyjnego float XKX; // wspolczynnik prostej R kierunku kryterium impedancyjnego - float KL;//wspolczynnik prostej dla blokady od obszaru obcia¿enia + float KL;//wspolczynnik prostej dla blokady od obszaru obcia�enia float RLf; // wartosc rozruchowa blokady Load do przodu float RLr; // wartosc rozruchowa blokady Load do tylu float Iogr; // wartosc graniczna skladowej zerowej identyfikacji zwarc z udzialem ziemi float khio; // wspolczynnik stabilizacji dla przekaznika identyfikacji zwarc z udzialem ziemi - float Uomin; ///< minimalna wartoœæ napiêcia sk³adowej zerowej + float Uomin; ///< minimalna warto�� napi�cia sk�adowej zerowej float ReKrown; // skladowa rzeczywista wspolczynnika dla linii rownoleglej float ImKrown;// skladowa urojona wspolczynnika dla linii rownoleglej float Krown_ignac; float kp_obc; //wspolczynnik powrotu krzywych obciazenia - float kpp_obc; //kwadrat wspó³czynnika powrotu krzywych obciazenia + float kpp_obc; //kwadrat wsp�czynnika powrotu krzywych obciazenia }; @@ -383,64 +383,64 @@ struct ZDistA_komp_params double fi1; ///< Kat linii double fi2; ///< Kat nachylenia prostej korekcji strefy pierwszej dla zwarc jednofazowych z ziemia double R1W_Zf1W_LE; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do przodu” wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do przodu� wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia double R1W_Zf1W_LL; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do przodu” wyduzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do przodu� wyduzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe double R1_Zf1_LE; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do przodu” strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do przodu� strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia double R1_Zf1_LL; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do przodu” strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do przodu� strefy 1 charakterystyki kolowej zwarcie miedzyfazowe double R2_Zf2_LE; ///< Zasieg rezystancyjny strefy 2 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do przodu” strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do przodu� strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia double R2_Zf2_LL; ///< Zasieg rezystancyjny strefy 2 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do przodu” strefy 2 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do przodu� strefy 2 charakterystyki kolowej zwarcie miedzyfazowe double R3_Zf3_LE; ///< Zasieg rezystancyjny strefy 3 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do przodu” strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do przodu� strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia double R3_Zf3_LL; ///< Zasieg rezystancyjny strefy 3 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do przodu” strefy 3 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do przodu� strefy 3 charakterystyki kolowej zwarcie miedzyfazowe double R4_Zf4_LE; ///< Zasieg rezystancyjny strefy 4 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do przodu” strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do przodu� strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia double R4_Zf4_LL; ///< Zasieg rezystancyjny strefy 4 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do przodu” strefy 4 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do przodu� strefy 4 charakterystyki kolowej zwarcie miedzyfazowe double R5_Zf5_LE; ///< Zasieg rezystancyjny strefy 5 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do przodu” strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do przodu� strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia double R5_Zf5_LL; ///< Zasieg rezystancyjny strefy 5 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do przodu” strefy 5 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do przodu� strefy 5 charakterystyki kolowej zwarcie miedzyfazowe double X1W_Zr1W_LE; ///< Zasieg reaktancyjny wydluzenie strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do tylu” wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do tylu� wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia double X1W_Zr1W_LL; ///< Zasieg reaktancyjny wydluzenie strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do tylu” wydluzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do tylu� wydluzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe double X1_Zr1_LE; ///< Zasieg reaktancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do tylu” strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do tylu� strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia double X1_Zr1_LL; ///< Zasieg reaktancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do tylu” strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do tylu� strefy 1 charakterystyki kolowej zwarcie miedzyfazowe double X2_Zr2_LE; ///< Zasieg reaktancyjny strefy 2 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do tylu” strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do tylu� strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia double X2_Zr2_LL; ///< Zasieg reaktancyjny strefy 2 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do tylu” strefy 2 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do tylu� strefy 2 charakterystyki kolowej zwarcie miedzyfazowe double X3_Zr3_LE; ///< Zasieg reaktancyjny strefy 3 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do tylu” strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do tylu� strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia double X3_Zr3_LL; ///< Zasieg reaktancyjny strefy 3 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do tylu” strefy 3 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do tylu� strefy 3 charakterystyki kolowej zwarcie miedzyfazowe double X4_Zr4_LE; ///< Zasieg reaktancyjny strefy 4 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do tylu” strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do tylu� strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia double X4_Zr4_LL; ///< Zasieg reaktancyjny strefy 4 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do tylu” strefy 4 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do tylu� strefy 4 charakterystyki kolowej zwarcie miedzyfazowe double X5_Zr5_LE; ///< Zasieg reaktancyjny strefy 5 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia - ///< Zasieg „do tylu” strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia + ///< Zasieg �do tylu� strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia double X5_Zr5_LL; ///< Zasieg reaktancyjny strefy 5 dla charakterystyki poligonalnej zwarcie miedzyfazowe - ///< Zasieg „do tylu” strefy 5 charakterystyki kolowej zwarcie miedzyfazowe + ///< Zasieg �do tylu� strefy 5 charakterystyki kolowej zwarcie miedzyfazowe double kp; ///< wspolczynnik powrotu //A2 // double U_min; ///< Minimalna wartosc napiecia double fi3; ///< Kat kierunkowy od osi X impedancyjnego kryterium kierunku double fi4; ///< Kat kierunkowy od osi R impedancyjnego kryterium kierunku - double RLf; ///< Zasieg „ do przodu” kryterium blokady dzialania w zakresie pradow obcia¿enia - double RLr; ///< Zasieg „ do tylu” kryterium blokady dzialania w zakresie pradow obcia¿enia - double fi5; ///< Kat kierunkowy blokady dzialania w zakresie pradow obcia¿enia + double RLf; ///< Zasieg � do przodu� kryterium blokady dzialania w zakresie pradow obcia�enia + double RLr; ///< Zasieg � do tylu� kryterium blokady dzialania w zakresie pradow obcia�enia + double fi5; ///< Kat kierunkowy blokady dzialania w zakresie pradow obcia�enia double Iomin; ///< Minimalna wartosc rozruchowa pradu zerowego przekaznika identyfikacji zwarc z udzialem ziemi double Iokh; ///< Wspolczynnik stabilizacji charakterystyki przekaznika identyfikacji zwarc z udzialem ziemi - double Uomin; ///< minimalna wartoœæ napiêcia sk³adowej zerowej + double Uomin; ///< minimalna warto�� napi�cia sk�adowej zerowej long Typ0; ///< Typ charakterystyki strefa 1W long K0; ///< Kierunek dzialania strefa 1W @@ -448,7 +448,7 @@ struct ZDistA_komp_params double Krown; ///< Wspolczynnik kompensacji linii rown double Krown_kat; ///< Kat wektora kompensacji lnii rown - double kp_obc; ///< Wspó³czynnik powrotu dla blokady od pr¹du obci¹¿enia + double kp_obc; ///< Wsp�czynnik powrotu dla blokady od pr�du obci��enia }__attribute__((__packed__)); diff --git a/ZDistA_wrapper.c b/ZDistA_wrapper.c new file mode 100644 index 0000000..a729b1d --- /dev/null +++ b/ZDistA_wrapper.c @@ -0,0 +1,202 @@ +#include "ZDistA_wrapper.h" +#include "ZDistA_komp.h" +#include +#include +#include + +// Definicja "uchwytu". Ta struktura przechowuje wszystko, co jest potrzebne do dziaÅ‚ania algorytmu. +// Jest niewidoczna dla Pythona. +struct ZDistA_handle { + // Główna struktura logiczna z oryginalnego kodu + struct ZDistA_komp_logic logic; + + // Pamięć na dane wejÅ›ciowe (fazory) - wskaźniki w 'logic' bÄ™dÄ… wskazywać tutaj + float u_re[3], u_im[3]; + float i_re[3], i_im[3]; + float u12_re, u12_im, u23_re, u23_im, u31_re, u31_im; + float u_mag[3], i_mag[3], u12_mag, u23_mag, u31_mag; + + // SkÅ‚adowe symetryczne + float su1_re, su1_im, su1_mag; + float su0_re, su0_im, su0_mag; + float su2_re, su2_im, su2_mag; + float si1_re, si1_im, si1_mag; + float si0_re, si0_im, si0_mag; + float si2_re, si2_im, si2_mag; + + // Dummy analog_in_params, ponieważ nie mamy peÅ‚nego Å›rodowiska + struct analog_in_params param_i; + struct analog_in_params param_u; + + // Struktura z pustymi wskaźnikami, bo nie jest używana w tym trybie + struct ZDistA_komp_args args; +}; + +// Funkcja pomocnicza do ustawiania wskaźników w strukturze 'logic' +static void setup_pointers(ZDistA_handle* h) { + struct ZDistA_komp_logic* log = &h->logic; + + // Ustawienie wskaźników na dane wejÅ›ciowe + log->U1_orta = &h->u_re[0]; log->U1_ortb = &h->u_im[0]; log->U1 = &h->u_mag[0]; + log->U2_orta = &h->u_re[1]; log->U2_ortb = &h->u_im[1]; log->U2 = &h->u_mag[1]; + log->U3_orta = &h->u_re[2]; log->U3_ortb = &h->u_im[2]; log->U3 = &h->u_mag[2]; + + log->I1_orta = &h->i_re[0]; log->I1_ortb = &h->i_im[0]; log->I1 = &h->i_mag[0]; + log->I2_orta = &h->i_re[1]; log->I2_ortb = &h->i_im[1]; log->I2 = &h->i_mag[1]; + log->I3_orta = &h->i_re[2]; log->I3_ortb = &h->i_im[2]; log->I3 = &h->i_mag[2]; + + // NapiÄ™cia miÄ™dzyfazowe + log->U12_orta = &h->u12_re; log->U12_ortb = &h->u12_im; log->U12 = &h->u12_mag; + log->U23_orta = &h->u23_re; log->U23_ortb = &h->u23_im; log->U23 = &h->u23_mag; + log->U31_orta = &h->u31_re; log->U31_ortb = &h->u31_im; log->U31 = &h->u31_mag; + + // SkÅ‚adowe symetryczne + log->sU1_orta = &h->su1_re; log->sU1_ortb = &h->su1_im; log->sU1 = &h->su1_mag; + log->sU0_orta = &h->su0_re; log->sU0_ortb = &h->su0_im; log->sU0 = &h->su0_mag; + log->sU2_orta = &h->su2_re; log->sU2_ortb = &h->su2_im; log->sU2 = &h->su2_mag; + + log->sI1_orta = &h->si1_re; log->sI1_ortb = &h->si1_im; log->sI1 = &h->si1_mag; + log->sI0_orta = &h->si0_re; log->sI0_ortb = &h->si0_im; log->sI0 = &h->si0_mag; + log->sI2_orta = &h->si2_re; log->sI2_ortb = &h->si2_im; log->sI2 = &h->si2_mag; + + // "Dummy" wskaźniki, aby uniknąć bÅ‚Ä™dów + log->param_I = &h->param_i; + log->param_U = &h->param_u; + + // Inne wskaźniki, które muszÄ… być zainicjowane + log->dw.log_ptr = log; + log->nast_.wyjscie.Zdist_dw = &log->dw; + log->nast_.wyjscie.on = &log->nast_.on_; +} + +DLL_EXPORT ZDistA_handle* ZDistA_init(double z1_r, double z1_x, double line_angle_deg, int kierunek) { + ZDistA_handle* h = (ZDistA_handle*)malloc(sizeof(ZDistA_handle)); + if (!h) return NULL; + + memset(h, 0, sizeof(ZDistA_handle)); + setup_pointers(h); + + struct Nastawy_przeliczone_ZDistA_komp* nast = &h->logic.nast_; + nast->on_ = 1; // WÅ‚Ä…czamy logikÄ™ + + // Ustawiamy parametry przekÅ‚adni (hardkodowane, jak w tester.py) + h->param_u.znam_wtor = 1.0; // WartoÅ›ci znormalizowane + h->param_i.znam_wtor = 1.0; + nast->wyjscie.przekladnia = 1.0; + + // Ustawienia charakterystyk - wzorowane na logice z distance_algorithm_zimba.py + nast->tanfi1 = tan(line_angle_deg * (3.14159265 / 180.0)); + nast->tanfi2 = tan(85.0 * (3.14159265 / 180.0)); // Typowa wartość + + // Strefa 1 + nast->typ[1] = 0; // Poligonalna + nast->kierunek[1] = kierunek; + nast->n_pol[1][0].Rr = z1_r; // LE + nast->n_pol[1][0].Xr = z1_x; // LE + nast->n_pol[1][1].Rr = z1_r; // LL + nast->n_pol[1][1].Xr = z1_x; // LL + + // Strefy 2-5 z rozsÄ…dnymi wartoÅ›ciami domyÅ›lnymi + for (int i = 2; i < 6; ++i) { + nast->typ[i] = 0; + nast->kierunek[i] = kierunek; + nast->n_pol[i][0].Rr = z1_r * (1.0 + (i-1)*0.5); + nast->n_pol[i][0].Xr = z1_x * (1.0 + (i-1)*0.5); + nast->n_pol[i][1].Rr = z1_r * (1.0 + (i-1)*0.5); + nast->n_pol[i][1].Xr = z1_x * (1.0 + (i-1)*0.5); + } + + // Inne ważne parametry + nast->Igr = 0.1 * 0.1; // Próg prÄ…dowy + nast->kp = 1.05; // Współczynnik powrotu + nast->kpk = 1.05 * 1.05; + nast->Igrp = nast->Igr * nast->kpk; + + // Przeliczenie wartoÅ›ci powrotnych i tangensów + for(int i = 0; i<6; ++i) { + for(int j=0; j<2; ++j) { + nast->n_pol[i][j].Rp = nast->n_pol[i][j].Rr / nast->kp; + nast->n_pol[i][j].Xp = nast->n_pol[i][j].Xr / nast->kp; + nast->n_pol[i][j].Rrtanfi1 = nast->n_pol[i][j].Rr * nast->tanfi1; + nast->n_pol[i][j].Rptanfi1 = nast->n_pol[i][j].Rp * nast->tanfi1; + } + } + nast->Xr1f = nast->n_pol[1][0].Xr * (1 + nast->tanfi2 / nast->tanfi1); + nast->Xr1fp = nast->Xr1f / nast->kp; + + return h; +} + +DLL_EXPORT int ZDistA_step( + ZDistA_handle* handle, + const float u_re[3], const float u_im[3], + const float i_re[3], const float i_im[3] +) { + if (!handle) return 0; + + // 1. Skopiuj dane wejÅ›ciowe do wewnÄ™trznych buforów uchwytu + for (int i = 0; i < 3; ++i) { + handle->u_re[i] = u_re[i]; handle->u_im[i] = u_im[i]; + handle->i_re[i] = i_re[i]; handle->i_im[i] = i_im[i]; + handle->u_mag[i] = sqrtf(u_re[i]*u_re[i] + u_im[i]*u_im[i]); + handle->i_mag[i] = sqrtf(i_re[i]*i_re[i] + i_im[i]*i_im[i]); + } + + // 2. Oblicz wartoÅ›ci, które w oryginale pochodziÅ‚y z innych modułów + // NapiÄ™cia miÄ™dzyfazowe + handle->u12_re = u_re[0] - u_re[1]; handle->u12_im = u_im[0] - u_im[1]; + handle->u23_re = u_re[1] - u_re[2]; handle->u23_im = u_im[1] - u_im[2]; + handle->u31_re = u_re[2] - u_re[0]; handle->u31_im = u_im[2] - u_im[0]; + handle->u12_mag = sqrtf(handle->u12_re*handle->u12_re + handle->u12_im*handle->u12_im); + handle->u23_mag = sqrtf(handle->u23_re*handle->u23_re + handle->u23_im*handle->u23_im); + handle->u31_mag = sqrtf(handle->u31_re*handle->u31_re + handle->u31_im*handle->u31_im); + + // SkÅ‚adowe symetryczne (staÅ‚e 'a' i 'a2' dla uproszczenia) + const float a_re = -0.5, a_im = 0.8660254; // sqrt(3)/2 + + // I0 = (I1+I2+I3)/3 + handle->si0_re = (i_re[0] + i_re[1] + i_re[2]) / 3.0f; + handle->si0_im = (i_im[0] + i_im[1] + i_im[2]) / 3.0f; + + // I1_zg = (I1 + a*I2 + a2*I3)/3 + handle->si1_re = (i_re[0] + (i_re[1]*a_re - i_im[1]*a_im) + (i_re[2]*a_re - i_im[2]*-a_im)) / 3.0f; + handle->si1_im = (i_im[0] + (i_re[1]*a_im + i_im[1]*a_re) + (i_re[2]*-a_im + i_im[2]*a_re)) / 3.0f; + + // I2_pr = (I1 + a2*I2 + a*I3)/3 + handle->si2_re = (i_re[0] + (i_re[1]*a_re - i_im[1]*-a_im) + (i_re[2]*a_re - i_im[2]*a_im)) / 3.0f; + handle->si2_im = (i_im[0] + (i_re[1]*-a_im + i_im[1]*a_re) + (i_re[2]*a_im + i_im[2]*a_re)) / 3.0f; + + // To samo dla napięć + handle->su0_re = (u_re[0] + u_re[1] + u_re[2]) / 3.0f; + handle->su0_im = (u_im[0] + u_im[1] + u_im[2]) / 3.0f; + handle->su1_re = (u_re[0] + (u_re[1]*a_re - u_im[1]*a_im) + (u_re[2]*a_re - u_im[2]*-a_im)) / 3.0f; + handle->su1_im = (u_im[0] + (u_re[1]*a_im + u_im[1]*a_re) + (u_re[2]*-a_im + u_im[2]*a_re)) / 3.0f; + handle->su2_re = (u_re[0] + (u_re[1]*a_re - u_im[1]*-a_im) + (u_re[2]*a_re - u_im[2]*a_im)) / 3.0f; + handle->su2_im = (u_im[0] + (u_re[1]*-a_im + u_im[1]*a_re) + (u_re[2]*a_im + u_im[2]*a_re)) / 3.0f; + + // Magnitudy skÅ‚adowych symetrycznych + handle->si0_mag = sqrtf(handle->si0_re*handle->si0_re + handle->si0_im*handle->si0_im); + handle->si1_mag = sqrtf(handle->si1_re*handle->si1_re + handle->si1_im*handle->si1_im); + handle->si2_mag = sqrtf(handle->si2_re*handle->si2_re + handle->si2_im*handle->si2_im); + handle->su0_mag = sqrtf(handle->su0_re*handle->su0_re + handle->su0_im*handle->su0_im); + handle->su1_mag = sqrtf(handle->su1_re*handle->su1_re + handle->su1_im*handle->su1_im); + handle->su2_mag = sqrtf(handle->su2_re*handle->su2_re + handle->su2_im*handle->su2_im); + + // 3. WywoÅ‚aj głównÄ… funkcjÄ™ logiki + ZDistA_komp(NULL, &handle->logic); + + // 4. Sprawdź, czy którekolwiek z wyjść jest aktywne + struct ZDistA_komp_logic* log = &handle->logic; + if (log->P1_L1E.val || log->P1_L2E.val || log->P1_L3E.val || + log->P1_L1L2.val || log->P1_L2L3.val || log->P1_L3L1.val) { + return 1; + } + + return 0; +} + +DLL_EXPORT void ZDistA_cleanup(ZDistA_handle* handle) { + if (handle) { + free(handle); + } +} diff --git a/ZDistA_wrapper.h b/ZDistA_wrapper.h new file mode 100644 index 0000000..400749f --- /dev/null +++ b/ZDistA_wrapper.h @@ -0,0 +1,50 @@ +#ifndef ZDISTA_WRAPPER_H +#define ZDISTA_WRAPPER_H + +// Na systemach Windows, musimy jawnie eksportować symbole, aby byÅ‚y widoczne w DLL. +#if defined(_WIN32) || defined(_WIN64) + #define DLL_EXPORT __declspec(dllexport) +#else + #define DLL_EXPORT +#endif + +// Deklaracja nieprzezroczystej struktury (opaque struct). +// DziÄ™ki temu, wewnÄ™trzna budowa naszego "uchwytu" jest ukryta przed kodem Python. +struct ZDistA_handle; +typedef struct ZDistA_handle ZDistA_handle; + +/** + * @brief Inicjalizuje logikÄ™ przekaźnika i wszystkie wewnÄ™trzne struktury. + * + * @param z1_r Rezystancja nastawcza strefy 1. + * @param z1_x Reaktancja nastawcza strefy 1. + * @param line_angle_deg KÄ…t linii w stopniach. + * @param kierunek Kierunkowość (0: bezkierunkowy, 1: do linii, 2: do szyn). + * @return Wskaźnik do "uchwytu" (handle) przechowujÄ…cego stan przekaźnika, lub NULL w przypadku bÅ‚Ä™du. + */ +DLL_EXPORT ZDistA_handle* ZDistA_init(double z1_r, double z1_x, double line_angle_deg, int kierunek); + +/** + * @brief Wykonuje jeden krok obliczeniowy algorytmu. + * + * @param handle Uchwyt zwrócony przez ZDistA_init. + * @param u_re Tablica 3 floatów (U1, U2, U3 - części rzeczywiste). + * @param u_im Tablica 3 floatów (U1, U2, U3 - części urojone). + * @param i_re Tablica 3 floatów (I1, I2, I3 - części rzeczywiste). + * @param i_im Tablica 3 floatów (I1, I2, I3 - części urojone). + * @return Wartość > 0, jeÅ›li nastÄ…piÅ‚o zadziaÅ‚anie (trip), 0 w przeciwnym razie. + */ +DLL_EXPORT int ZDistA_step( + ZDistA_handle* handle, + const float u_re[3], const float u_im[3], + const float i_re[3], const float i_im[3] +); + +/** + * @brief Zwalnia pamięć zaalokowanÄ… dla uchwytu. + * + * @param handle Uchwyt do zwolnienia. + */ +DLL_EXPORT void ZDistA_cleanup(ZDistA_handle* handle); + +#endif // ZDISTA_WRAPPER_H diff --git a/ZDistL_komp.h b/ZDistL_komp.h index 1680e9c..7956322 100644 --- a/ZDistL_komp.h +++ b/ZDistL_komp.h @@ -10,14 +10,14 @@ #define ZDISTL_H_KOMP_ //#include "pawel_usun_to.h" -#include "../tdefs.h" +#include "tdefs.h" #include "helper.h" #include "ZDistL.h" struct Wyjscie1_zdistl_komp { - u8 dbl_Com[7]; ///deblokowanie dzia³ania stref w zale¿noœci od stanu ³¹cza + u8 dbl_Com[7]; ///deblokowanie dzia�ania stref w zale�no�ci od stanu ��cza u8 PSPZ;//pobudzenie SPZ }; @@ -368,8 +368,8 @@ struct ZDistL_params_komp long tryb_zezw; ///nul 2>&1 -git remote add origin %REPO_URL% - -echo [5/6] Przygotowanie plikow i pierwszy commit... -git add . -git commit -m "Pierwszy commit - konfiguracja automatyczna" - -echo [6/6] Wysylanie kodu na serwer... -git branch -M main -git push -u origin main - -echo. -echo === GOTOWE! Twoj projekt jest polaczony z Gitea === -pause \ No newline at end of file +echo Installing required Python packages... +pip install -r requirements.txt +echo Done. +pause diff --git a/mocks.c b/mocks.c index 453d61b..d246257 100644 --- a/mocks.c +++ b/mocks.c @@ -1,352 +1,11 @@ -// Symulacja funkcji z helper.h -static inline int set_float_ptr(float *src, float **dst) -{ - *dst = src; - return 0; +#include "mocks.h" + +// Mock implementation of Task_sleep +void Task_sleep(unsigned int ticks) { + // This is a mock, so we do nothing. + // In a real scenario, you might use a platform-specific sleep function. } -static inline int set_bit_ptr_struct(struct binary_io *src, struct binary_io **dst) -{ - *dst = src; - return 0; -} - -static inline int set_pointer_in_ptr(u32 *src, u32 **dst) -{ - *dst = src; - return 0; -} - -static inline int set_pointer_out_ptr(u32 *src, u32 **dst) -{ - *dst = src; - return 0; -} - -static inline int check_struct(struct binary_io *ptr) -{ - return (ptr && ptr->val) ? 1 : 0; -} - -static inline void check_and_set_struct(u8 condition, struct binary_io **ptr) -{ - if (*ptr) - (*ptr)->val = condition ? 1 : 0; -} - -static inline void clear_struct(struct binary_io **ptr) -{ - if (*ptr) - (*ptr)->val = 0; -} - -// Funkcja testowa (zwraca 0 w normalnym trybie) -static inline int czy_test_R() { return 0; } -// Szybka odwrotność pierwiastka (Reciprocal Square Root) -static inline float _rcpsp(float x) -{ - if (x <= 1e-9f) - return 0.0f; // Zabezpieczenie przed dzieleniem przez zero - return 1.0f / sqrtf(x); -} - -// Funkcje kÄ…towe (e_phi.h) -static inline float get_phase(float y, float x) -{ - return atan2f(y, x) * 180.0f / 3.14159265f; -} - -static inline float get_phase_diff(float angle1, float angle2) -{ - float diff = angle1 - angle2; - while (diff <= -180.0f) - diff += 360.0f; - while (diff > 180.0f) - diff -= 360.0f; - return diff; -} - -// Timer / UkÅ‚ad opóźniajÄ…cy (filtr P) -// Liczy czas w pÄ™tlach programu -static inline void sprawdz_P(u8 *out, u8 cond_start, u8 cond_reset, short *counter, int time_set, int time_reset) -{ - if (cond_reset) - { - *counter = 0; - *out = 0; - } - else if (cond_start) - { - if (*counter < time_set) - (*counter)++; - if (*counter >= time_set) - *out = 1; - } - else - { - if (*counter > 0) - (*counter)--; - if (*counter == 0) - *out = 0; - } -} - -// Struktura dla pojedynczej strefy wielokÄ…tnej (poligonalnej) -struct n_pol_struct -{ - float Rr, Xr; // Granice Rezystancji i Reaktancji (prawo/góra) - float Rp, Xp; // Granice (lewo/dół lub po przesuniÄ™ciu kp) - float Z, Zp; // Impedancja graniczna (moduÅ‚) - float Rrtanfi1, Rptanfi1; // Granice nachylone -}; - -// Struktura wyjÅ›cia -struct wyjscie_struct -{ - float przekladnia; - void *Zdist_dw; // Wskaźnik void na log->dw - int *on; // Wskaźnik na log->nast_.on_ - u8 SOTF_zwrotnie; -}; - -// Struktura obliczonych nastaw (log->nast_) -struct nast_struct -{ - int on_; - int bl_; - int Bl_L[6]; // Blokady Load - int z6_kolo; - - float ReK1, ImK1; - float ReKr, ImKr; - float ReKrown, ImKrown; - float Krown_ignac; - - float kp, kpp, kpk; - float kp_obc, kpp_obc; - - int typ[6]; // Typy stref - float tanfi1, tanfi2; - - struct n_pol_struct n_pol[6][2]; // [strefa][typ pÄ™tli: 0=fazowa, 1=miedzyfazowa] - - float Xr1f, Xr1fp; - float Xr1Wf, Xr1Wfp; - float Igr, Igrp; - float Zgr; - int kierunek[6]; - - float XKR, XKX, KL; - float RLf, RLr; - float Iogr, khio, Uomin; - - struct wyjscie_struct wyjscie; -}; - -// Struktura zmiennych wewnÄ™trznych (pomiary, flagi, liczniki) -> log->dw -struct Z_measurement -{ - float R[9], X[9], Z[9], E[9]; // 9 pÄ™tli pomiarowych - float absR[9], absX[9]; - float XRtanfi1_noabs[9], XRtanfi2[9]; -}; - -struct dw_struct -{ - float I1, I2, I3, U1, U2, U3; - struct Z_measurement Z; - u8 Igr[6]; // Warunki prÄ…dowe - - void *log_ptr; // Wskaźnik zwrotny na logic - - u8 XKp[6][9], XKm[6][9]; // Kierunkowość - u8 BL_Load[9]; // Blokada od obciążenia - u8 Zm[9]; // Zmienne pomocnicze - - // Liczniki czasu (timery) - short liczpIo; - short liczp11[6][9], liczp12[6][9], liczp10[9]; - short liczps[6][9]; - short liczKdod; - - u8 Iogr; // Przekroczenie I0 granicznego - u8 Kp, Km; // Kierunek (+/-) - - u8 P_s[6][9]; // Pobudzenia surowe - u8 P_sss[6][9]; // Pobudzenia super surowe? - u8 Pbk[6][9]; // Pobudzenia bezkierunkowe - u8 P[6][9]; // Pobudzenia finalne (kierunkowe) -}; - -// WejÅ›cia IO - wskaźniki na zmienne zewnÄ™trzne -struct ZDistA_komp_io -{ - struct binary_io *bl_in; - struct binary_io *bl_k_in; - - float *i1_orta_float_in, *i1_ortb_float_in, *i1_float_in; - float *i2_orta_float_in, *i2_ortb_float_in, *i2_float_in; - float *i3_orta_float_in, *i3_ortb_float_in, *i3_float_in; - - float *u1_orta_float_in, *u1_ortb_float_in, *u1_float_in; - float *u2_orta_float_in, *u2_ortb_float_in, *u2_float_in; - float *u3_orta_float_in, *u3_ortb_float_in, *u3_float_in; - - float *u12_orta_float_in, *u12_ortb_float_in, *u12_float_in; - float *u23_orta_float_in, *u23_ortb_float_in, *u23_float_in; - float *u31_orta_float_in, *u31_ortb_float_in, *u31_float_in; - - // SkÅ‚adowe symetryczne / dodatkowe - float *i1_zg_orta_float_in, *i1_zg_ortb_float_in, *i1_zg_float_in; // I1 (zgodna) - float *i2_pr_orta_float_in, *i2_pr_ortb_float_in, *i2_pr_float_in; // I2 (przeciwna) - float *io_orta_float_in, *io_ortb_float_in, *io_float_in; // I0 (zerowa) - - float *u1_zg_orta_float_in, *u1_zg_ortb_float_in, *u1_zg_float_in; - float *u2_pr_orta_float_in, *u2_pr_ortb_float_in, *u2_pr_float_in; - float *uo_orta_float_in, *uo_ortb_float_in, *uo_float_in; - - u32 *i_param_an_ptr_in; - u32 *u_param_an_ptr_in; - - struct binary_io *test_in; - struct binary_io *deakt_in; - - // Kompensacja linii równolegÅ‚ej - float *i_rown_orta_float_in; - float *i_rown_ortb_float_in; - u32 *i_rown_an_ptr_in; - - struct binary_io *wyl_in; - - // WyjÅ›cia (Pobudzenia) - wskaźniki na struct binary_io - struct binary_io *P1W_L1E_out, *P1W_L2E_out, *P1W_L3E_out; - struct binary_io *P1W_L1L2_out, *P1W_L2L3_out, *P1W_L3L1_out; - - struct binary_io *P1_L1E_out, *P1_L2E_out, *P1_L3E_out; - struct binary_io *P1_L1L2_out, *P1_L2L3_out, *P1_L3L1_out; - - struct binary_io *P2_L1E_out, *P2_L2E_out, *P2_L3E_out; - struct binary_io *P2_L1L2_out, *P2_L2L3_out, *P2_L3L1_out; - - struct binary_io *P3_L1E_out, *P3_L2E_out, *P3_L3E_out; - struct binary_io *P3_L1L2_out, *P3_L2L3_out, *P3_L3L1_out; - - struct binary_io *P4_L1E_out, *P4_L2E_out, *P4_L3E_out; - struct binary_io *P4_L1L2_out, *P4_L2L3_out, *P4_L3L1_out; - - struct binary_io *P5_L1E_out, *P5_L2E_out, *P5_L3E_out; - struct binary_io *P5_L1L2_out, *P5_L2L3_out, *P5_L3L1_out; - - u32 *wy_ptr_out; - - // Debug - float *z1_float_out, *z2_float_out, *z3_float_out; - float *z4_float_out, *z5_float_out, *z6_float_out; -}; - -// Struktura parametrów (Nastawy) -struct ZDistA_komp_params -{ - long bity; - float Kk1, Kk1_kat; - float KkC, KkC_kat; - float kp, kp_obc; - int Typ0, Typ1, Typ2, Typ3, Typ4, Typ5; - float fi1, fi2, fi3, fi4, fi5; - - // Rezystancje/Reaktancje dla pÄ™tli (dużo zmiennych) - // ZastÄ™pujemy je blokiem lub listÄ… na podstawie kodu init - double R1W_Zf1W_LE, R1W_Zf1W_LL; - double R1_Zf1_LE, R1_Zf1_LL; - double R2_Zf2_LE, R2_Zf2_LL; - double R3_Zf3_LE, R3_Zf3_LL; - double R4_Zf4_LE, R4_Zf4_LL; - double R5_Zf5_LE, R5_Zf5_LL; - - double X1W_Zr1W_LE, X1W_Zr1W_LL; - double X1_Zr1_LE, X1_Zr1_LL; - double X2_Zr2_LE, X2_Zr2_LL; - double X3_Zr3_LE, X3_Zr3_LL; - double X4_Zr4_LE, X4_Zr4_LL; - double X5_Zr5_LE, X5_Zr5_LL; - - float I_min, Iomin; - int K0, K1, K2, K3, K4, K5; - - float RLf, RLr, Iokh, Uomin; - float Krown, Krown_kat; -}; - -struct ZDistA_komp_args -{ - struct ZDistA_komp_io io; - struct ZDistA_komp_params params; -}; - -// Główna struktura stanu logicznego (Instance Data) -struct ZDistA_komp_logic -{ - struct binary_io stan_bl; - struct binary_io Bl_K; - - // Wskaźniki na pomiary - float *I1_orta, *I1_ortb, *I1; - float *I2_orta, *I2_ortb, *I2; - float *I3_orta, *I3_ortb, *I3; - - float *U1_orta, *U1_ortb, *U1; - float *U2_orta, *U2_ortb, *U2; - float *U3_orta, *U3_ortb, *U3; - - float *U12_orta, *U12_ortb, *U12; - float *U23_orta, *U23_ortb, *U23; - float *U31_orta, *U31_ortb, *U31; - - float *sI1_orta, *sI1_ortb, *sI1; // zgodna - float *sI2_orta, *sI2_ortb, *sI2; // przeciwna - float *sI0_orta, *sI0_ortb, *sI0; // zerowa - - float *sU1_orta, *sU1_ortb, *sU1; - float *sU2_orta, *sU2_ortb, *sU2; - float *sU0_orta, *sU0_ortb, *sU0; - - struct param_I_struct *param_I; - struct param_U_struct *param_U; - - struct binary_io test; - struct binary_io deakt; - - float *I_row_orta, *I_row_ortb; - struct param_I_struct *param_I_rown; - - struct binary_io wyl; - - // WyjÅ›cia logiczne (wskaźniki na struktury binary_io) - // Uwaga: w kodzie init jest: set_bit_ptr_struct(..., &log->P1W_L1E) - // To sugeruje, że w strukturze logic te pola to WSKAŹNIKI (struct binary_io*) - struct binary_io *P1W_L1E, *P1W_L2E, *P1W_L3E; - struct binary_io *P1W_L1L2, *P1W_L2L3, *P1W_L3L1; - - struct binary_io *P1_L1E, *P1_L2E, *P1_L3E; - struct binary_io *P1_L1L2, *P1_L2L3, *P1_L3L1; - - struct binary_io *P2_L1E, *P2_L2E, *P2_L3E; - struct binary_io *P2_L1L2, *P2_L2L3, *P2_L3L1; - - struct binary_io *P3_L1E, *P3_L2E, *P3_L3E; - struct binary_io *P3_L1L2, *P3_L2L3, *P3_L3L1; - - struct binary_io *P4_L1E, *P4_L2E, *P4_L3E; - struct binary_io *P4_L1L2, *P4_L2L3, *P4_L3L1; - - struct binary_io *P5_L1E, *P5_L2E, *P5_L3E; - struct binary_io *P5_L1L2, *P5_L2L3, *P5_L3L1; - - float *z[6]; // WyjÅ›cia debugowe - - struct nast_struct nast_; - struct dw_struct dw; - - int l_nieustalony; -}; - +// Mock definitions for global variables +Semaphore_Handle spi_semaphore; +Swi_Handle swi_notify; \ No newline at end of file diff --git a/mocks.h b/mocks.h index 3b1f853..3830c83 100644 --- a/mocks.h +++ b/mocks.h @@ -1,28 +1,36 @@ +#ifndef MOCKS_H_ +#define MOCKS_H_ + #include -#include -#include // do memset -// Definicje typów z tdefs.h -typedef uint8_t u8; -typedef uint16_t u16; -typedef uint32_t u32; +// Mock for Semaphore_Handle +typedef void* Semaphore_Handle; -// Prosta struktura dla sygnałów binarnych (wejÅ›cia/wyjÅ›cia logiczne) -struct binary_io -{ - int val; // 0 lub 1 -}; +// Mock for Swi_Handle +typedef void* Swi_Handle; -// Struktury parametrów analogowych (np. przekÅ‚adniki) -struct param_I_struct -{ - float znam_wtor; // np. 1.0 lub 5.0 A - float znam_pierw; // np. 400.0 A -}; +// Mock for Task_sleep +void Task_sleep(unsigned int ticks); -struct param_U_struct -{ - float znam_wtor; // np. 100.0 V - float znam_pierw; // np. 110000.0 V -}; +// Mock for MessageQ +typedef struct { + int reserved; +} MessageQ_MsgHeader; +// Mock for UInt32 +typedef uint32_t UInt32; + +// Mock for UInt16 +typedef uint16_t UInt16; + +// Mock for Timer_Handle +typedef void* Timer_Handle; + +// Mock for UArg +typedef intptr_t UArg; + +// Mock for Void +typedef void Void; + + +#endif /* MOCKS_H_ */ \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 6ce692a..1354324 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,3 @@ numpy matplotlib -comtrade +comtrade \ No newline at end of file diff --git a/src/21g.c b/src/21g.c new file mode 100644 index 0000000..9c7193d --- /dev/null +++ b/src/21g.c @@ -0,0 +1,278 @@ +/* + * 21g.c + * + * Created on: 07-05-2018 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "21g.h" +#include "helper.h" +#include "analog_in.h" +#include "config.h" +#include + +int r21g_initlog(void *arguments, void *logic) +{ + struct r21g_args *args = (struct r21g_args *)arguments; + struct r21g_logic *log = (struct r21g_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_float_ptr(args->io.U_esk_float_in,&log->U_esk)) + return -1; + if(set_float_ptr(args->io.U_orta_float_in,&log->U_orta)) + return -1; + if(set_float_ptr(args->io.U_ortb_float_in,&log->U_ortb)) + return -1; + + if(set_float_ptr(args->io.I_esk_float_in,&log->I_esk)) + return -1; + if(set_float_ptr(args->io.I_orta_float_in,&log->I_orta)) + return -1; + if(set_float_ptr(args->io.I_ortb_float_in,&log->I_ortb)) + return -1; + + if(set_pointer_in_ptr(args->io.U_param_ptr_in,(u32 *)&log->U_param)) + return -1; + + if(set_pointer_in_ptr(args->io.I_param_ptr_in,(u32 *)&log->I_param)) + return -1; + + if(set_bit_ptr_struct(args->io.p_out,&log->P)) + return -1; + if(set_bit_ptr_struct(args->io.z_out,&log->Z)) + return -1; + if(set_bit_ptr_struct(args->io.w_out,&log->W)) + return -1; + + if(set_float_ptr(args->io.y_float_out,&log->y)) + return -1; + if(set_float_ptr(args->io.g_float_out,&log->g)) + return -1; + if(set_float_ptr(args->io.b_float_out,&log->b)) + return -1; + + log->przek = log->I_param->znam_wtor / log->U_param->znam_wtor; + log->y_rozr = (float)0.001 * 0.001 * args->params.yr * args->params.yr;// * log->przek * log->przek; + log->y_powr = log->y_rozr * args->params.kp * args->params.kp; + + log->g_rozr = (float)0.001 * args->params.gr;// * log->przek; + log->g_powr = log->g_rozr * args->params.kp; + + log->b_rozr = (float)0.001 * args->params.br;// * log->przek; + log->b_powr = log->b_rozr * args->params.kp; + + log->umin_rozr = args->params.umin * args->params.umin; + log->umin_powr = log->umin_rozr * args->params.kp * args->params.kp; + + log->imin_rozr = args->params.imin * args->params.imin; + log->imin_powr = log->imin_rozr * args->params.kp * args->params.kp; + + log->td=(u32)(args->params.td*1000); + + + log->cos_shift = cosf(args->params.phi_corr*PI/180); + log->sin_shift = sinf(args->params.phi_corr*PI/180); + + return 0; +} + +void r21g(void *arguments, void *logic) +{ + struct r21g_logic *log = (struct r21g_logic *)logic; + struct r21g_args *args = (struct r21g_args *)arguments; + float y,g,b,m,g_new; + + if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked + { + if(!*log->U_esk) + y=*log->I_esk / 0.000000001; + else + y=*log->I_esk / *log->U_esk; + + m=((*log->U_ortb * *log->U_ortb) + (*log->U_orta * *log->U_orta)); + + if(!m) + m=0.000000001; + + g=((*log->I_ortb * *log->U_ortb) + (*log->I_orta * *log->U_orta))/m; + b=((*log->I_orta * *log->U_ortb) - (*log->I_ortb * *log->U_orta))/m; +//sprawdzic dzielenie przez zero + + y*=log->przek*log->przek; + g*=log->przek; + b*=log->przek; + + g_new=(log->cos_shift*g) - (log->sin_shift*b); + b=(log->sin_shift*g) + (log->cos_shift*b); + g=g_new; + + *log->y=y; + *log->g=g; + *log->b=b; + //g= + + sprawdz_P(&log->pobi, + *log->I_esk>log->imin_rozr, + *log->I_eskimin_powr, + &log->lpi, + 15,3); + sprawdz_P(&log->pobu, + *log->U_esk>log->umin_rozr, + *log->U_eskumin_powr, + &log->lpu, + 15,3); + + if(log->pobu && log->pobi) + { + sprawdz_P(&log->poby, + y>log->y_rozr, + yy_powr, + &log->lpy, + 20,3); + + switch(args->params.g_enum) + { + case S_FWD: + sprawdz_P(&log->pobg, + g>log->g_rozr, + gg_powr, + &log->lpg, + 20,3); + break; + case S_BKW: + g*=-1; + sprawdz_P(&log->pobg, + g>log->g_rozr, + gg_powr, + &log->lpg, + 20,3); + break; + case S_BID: + if(g<0) + g*=-1; + sprawdz_P(&log->pobg, + g>log->g_rozr, + gg_powr, + &log->lpg, + 20,3); + break; + default: + log->pobg=0; + break; + } + + switch(args->params.b_enum) + { + case S_FWD: + sprawdz_P(&log->pobb, + b>log->b_rozr, + bb_powr, + &log->lpb, + 20,3); + break; + case S_BKW: + b*=-1; + sprawdz_P(&log->pobb, + b>log->b_rozr, + bb_powr, + &log->lpb, + 20,3); + break; + case S_BID: + if(b<0) + b*=-1; + sprawdz_P(&log->pobb, + b>log->b_rozr, + bb_powr, + &log->lpb, + 20,3); + break; + default: + log->pobb=0; + break; + } + } + else + { + log->poby=0; + log->pobg=0; + log->pobb=0; + log->lpy=0; + log->lpg=0; + log->lpb=0; + } + + if(!args->params.log_enum) + { + log->p=1; + + if(args->params.y_enum) + log->p&=log->poby; + if(args->params.g_enum) + log->p&=log->pobg; + if(args->params.b_enum) + log->p&=log->pobb; + } + else + { + log->p=0; + + if(args->params.y_enum) + log->p|=log->poby; + if(args->params.g_enum) + log->p|=log->pobg; + if(args->params.b_enum) + log->p|=log->pobb; + } + + if(!args->params.y_enum&&!args->params.g_enum&&!args->params.b_enum) + log->p=0; + + if(log->p) + { + if(log->td_cnt>log->td) + log->z=1; + else + log->td_cnt+=LOOP_CYCLE_MS; + } + else + { + log->td_cnt=0; + log->z=0; + } + + if(czy_test_R() && check_struct(&log->test)) + { + log->p=1; + log->z=1; + } + + check_and_set_struct(log->p,&log->P); + check_and_set_struct(log->z,&log->Z); + + if(args->params.bity & 0x00000004) // na wylacz + { + check_and_set_struct(log->z,&log->W); + } + } + else + { + log->poby=0; + log->pobg=0; + log->pobb=0; + log->lpy=0; + log->lpg=0; + log->lpb=0; + + clear_struct(&log->P); + clear_struct(&log->Z); + clear_struct(&log->W); + } +} diff --git a/src/21g.h b/src/21g.h new file mode 100644 index 0000000..cc14b8c --- /dev/null +++ b/src/21g.h @@ -0,0 +1,123 @@ +/* + * 21g.h + * + * Created on: 07-03-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef R21G_H_ +#define R21G_H_ + +#include "tdefs.h" +#include "helper.h" +#include "analog_in.h" + +#define S_OFF 0 +#define S_FWD 1 +#define S_BKW 2 +#define S_BID 3 + +struct r21g_logic +{ + struct binary_io stan_bl; + struct binary_io test; + float *U_esk; + float *U_orta; + float *U_ortb; + float *I_esk; + float *I_orta; + float *I_ortb; + + struct analog_in_params *U_param; + struct analog_in_params *I_param; + + struct binary_io P; + struct binary_io Z; + struct binary_io W; + + float y_rozr; + float y_powr; + float g_rozr; + float g_powr; + float b_rozr; + float b_powr; + float u_rozr; + float u_powr; + float umin_rozr; + float umin_powr; + float imin_rozr; + float imin_powr; + u8 p; + u8 z; + + u8 pobi; + u8 pobu; + u8 poby; + u8 pobg; + u8 pobb; + + short lpi; + short lpu; + short lpy; + short lpg; + short lpb; + + float *y; + float *g; + float *b; + + float przek; + u32 td; + u32 td_cnt; + float cos_shift; + float sin_shift; +}; + +struct r21g_io +{ + u32 stan_bl_in; + u32 test_in; + u32 U_esk_float_in; + u32 U_orta_float_in; + u32 U_ortb_float_in; + u32 U_param_ptr_in; + u32 I_esk_float_in; + u32 I_orta_float_in; + u32 I_ortb_float_in; + u32 I_param_ptr_in; + u32 p_out; + u32 z_out; + u32 w_out; + u32 y_float_out; + u32 g_float_out; + u32 b_float_out; +}__attribute__((__packed__)); + +struct r21g_params +{ + u32 bity; + u32 y_enum; + double yr; + u32 g_enum; + double gr; + u32 b_enum; + double br; + double phi_corr; + double kp; + double td; + double umin; + double imin; + u32 log_enum; +}__attribute__((__packed__)); + +struct r21g_args +{ + struct r21g_io io; + struct r21g_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int r21g_initlog(void *arguments, void *logic); +extern void r21g(void *arguments, void *logic); + +#endif /* 21G_H_ */ diff --git a/src/CBR.c b/src/CBR.c new file mode 100644 index 0000000..710c2c8 --- /dev/null +++ b/src/CBR.c @@ -0,0 +1,371 @@ +/* + * CBR.c + * + * Created on: 16-02-2017 + * Author: PS + */ + + +#include "CBR.h" +#include "helper.h" +#include "config.h" + +int CBR_initlog(void *arguments, void *logic) +{ + struct CBR_args *args = (struct CBR_args *)arguments; + struct CBR_logic *log = (struct CBR_logic *)logic; + + if(set_bit_ptr_mask(args->io.we_W1_ON,&log->we_W1_ON,&log->we_W1_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_W2_ON,&log->we_W2_ON,&log->we_W2_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_W3_ON,&log->we_W3_ON,&log->we_W3_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_W1_OFF,&log->we_W1_OFF,&log->we_W1_OFF_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_W2_OFF,&log->we_W2_OFF,&log->we_W2_OFF_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_W3_OFF,&log->we_W3_OFF,&log->we_W3_OFF_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_W1_ON,&log->wy_W1_ON,&log->wy_W1_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_W2_ON,&log->wy_W2_ON,&log->wy_W2_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_W3_ON,&log->wy_W3_ON,&log->wy_W3_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.zd_W_ON,&log->zd_W_ON,&log->zd_W_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.zd_W_OFF,&log->zd_W_OFF,&log->zd_W_OFF_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.niezg_L1,&log->niezg_L1,&log->niezg_L1_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.niezg_L2,&log->niezg_L2,&log->niezg_L2_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.niezg_L3,&log->niezg_L3,&log->niezg_L3_bit_mask)) + return -1; + + log->nast_.licz_p = ((args->params.to) * 1000); + log->nast_.rodzaj = args->params.rodzaj; + + log->licz_t1 = 0; // wyzerowanie czasu + log->licz_t2 = 0; // wyzerowanie czasu + log->licz_t3 = 0; // wyzerowanie czasu + + + + + return 0; +} + +void CBR(void *arguments, void *logic) +{ + struct CBR_logic *log = (struct CBR_logic *)logic; + + u8 t_W1_ON = ((*log->we_W1_ON & log->we_W1_ON_bit_mask) != 0) ? 1: 0; + u8 t_W2_ON = ((*log->we_W2_ON & log->we_W2_ON_bit_mask) != 0) ? 1: 0; + u8 t_W3_ON = ((*log->we_W3_ON & log->we_W3_ON_bit_mask) != 0) ? 1: 0; + u8 t_W1_OFF = ((*log->we_W1_OFF & log->we_W1_OFF_bit_mask) != 0) ? 1: 0; + u8 t_W2_OFF = ((*log->we_W2_OFF & log->we_W2_OFF_bit_mask) != 0) ? 1: 0; + u8 t_W3_OFF = ((*log->we_W3_OFF & log->we_W3_OFF_bit_mask) != 0) ? 1: 0; + + u8 o_W1_ON = ((*log->wy_W1_ON & log->wy_W1_ON_bit_mask) != 0) ? 1: 0; + u8 o_W2_ON = ((*log->wy_W2_ON & log->wy_W2_ON_bit_mask) != 0) ? 1: 0; + u8 o_W3_ON = ((*log->wy_W3_ON & log->wy_W3_ON_bit_mask) != 0) ? 1: 0; + + u8 o_zd_ON = ((*log->zd_W_ON & log->zd_W_ON_bit_mask) != 0) ? 1: 0; + u8 o_zd_OFF = ((*log->zd_W_OFF & log->zd_W_OFF_bit_mask) != 0) ? 1: 0; + u8 o_n_L1 = ((*log->niezg_L1 & log->niezg_L1_bit_mask) != 0) ? 1: 0; + u8 o_n_L2 = ((*log->niezg_L2 & log->niezg_L2_bit_mask) != 0) ? 1: 0; + u8 o_n_L3 = ((*log->niezg_L3 & log->niezg_L3_bit_mask) != 0) ? 1: 0; + + + switch(log->nast_.rodzaj) + { + case 0: //jednobitowo + + o_n_L1 = 0; + o_n_L2 = 0; + o_n_L3 = 0; + + if (t_W1_ON) + { + o_W1_ON = 1; + o_W2_ON = 1; + o_W3_ON = 1; + o_zd_ON = 1; + o_zd_OFF = 0; + } + else + { + o_W1_ON = 0; + o_W2_ON = 0; + o_W3_ON = 0; + o_zd_ON = 0; + o_zd_OFF = 1; + } + break; + +//***************************************************************** + + case 1: //jednobitowo pofazowo + + o_n_L1 = 0; + o_n_L2 = 0; + o_n_L3 = 0; + + if (t_W1_ON) + o_W1_ON = 1; + else + o_W1_ON = 0; + + if (t_W2_ON) + o_W2_ON = 1; + else + o_W2_ON = 0; + + if (t_W3_ON) + o_W3_ON = 1; + else + o_W3_ON = 0; + + if (t_W1_ON && t_W2_ON && t_W3_ON) + { + o_zd_ON = 1; + o_zd_OFF = 0; + } + else if (!t_W1_ON && !t_W2_ON && !t_W3_ON) + { + o_zd_OFF = 1; + o_zd_ON = 0; + } + else + { + o_zd_ON = 0; + o_zd_OFF = 0; + } + break; + +//***************************************************************** + + case 2: //dwubitowo + + if (t_W1_ON && !t_W1_OFF) + { + o_W1_ON = 1; + o_W2_ON = 1; + o_W3_ON = 1; + o_zd_ON = 1; + o_zd_OFF = 0; + o_n_L1 = 0; + o_n_L2 = 0; + o_n_L3 = 0; + log->licz_t1 = 0; + } + else if (!t_W1_ON && t_W1_OFF) + { + o_W1_ON = 0; + o_W2_ON = 0; + o_W3_ON = 0; + o_zd_ON = 0; + o_zd_OFF = 1; + o_n_L1 = 0; + o_n_L2 = 0; + o_n_L3 = 0; + log->licz_t1 = 0; + } + else if (!t_W1_ON && !t_W1_OFF) + { + //pamieta stan - nie zmienia + log->licz_t1+=LOOP_CYCLE_MS; + } + else if (t_W1_ON && t_W1_OFF) + { + o_W1_ON = 1; + o_W2_ON = 1; + o_W3_ON = 1; + o_zd_ON = 1; + o_zd_OFF = 0; + log->licz_t1+=LOOP_CYCLE_MS; + } + + if (log->licz_t1 > log->nast_.licz_p) + { + o_n_L1 = 1; + o_n_L2 = 1; + o_n_L3 = 1; + log->licz_t1 = log->nast_.licz_p + (5*LOOP_CYCLE_MS); + } + + break; + +//***************************************************************** + + case 3: //dwubitowo pofazowo + + // FAZA L1 + if (t_W1_ON && !t_W1_OFF) + { + o_W1_ON = 1; + o_n_L1 = 0; + log->licz_t1 = 0; + } + else if (!t_W1_ON && t_W1_OFF) + { + o_W1_ON = 0; + o_n_L1 = 0; + log->licz_t1 = 0; + } + else if (!t_W1_ON && !t_W1_OFF) + { + //pamieta stan - nie zmienia + log->licz_t1+=LOOP_CYCLE_MS; + } + else if (t_W1_ON && t_W1_OFF) + { + o_W1_ON = 1; + log->licz_t1+=LOOP_CYCLE_MS; + } + + if (log->licz_t1 > log->nast_.licz_p) + { + o_n_L1 = 1; + log->licz_t1 = log->nast_.licz_p + (5*LOOP_CYCLE_MS); + } + + // FAZA L2 + if (t_W2_ON && !t_W2_OFF) + { + o_W2_ON = 1; + o_n_L2 = 0; + log->licz_t2 = 0; + } + else if (!t_W2_ON && t_W2_OFF) + { + o_W2_ON = 0; + o_n_L2 = 0; + log->licz_t2 = 0; + } + else if (!t_W2_ON && !t_W2_OFF) + { + //pamieta stan - nie zmienia + log->licz_t2+=LOOP_CYCLE_MS; + } + else if (t_W2_ON && t_W2_OFF) + { + o_W2_ON = 1; + log->licz_t2+=LOOP_CYCLE_MS; + } + + if (log->licz_t2 > log->nast_.licz_p) + { + o_n_L2 = 1; + log->licz_t2 = log->nast_.licz_p + (5*LOOP_CYCLE_MS); + } + + // FAZA L3 + if (t_W3_ON && !t_W3_OFF) + { + o_W3_ON = 1; + o_n_L3 = 0; + log->licz_t3 = 0; + } + else if (!t_W3_ON && t_W3_OFF) + { + o_W3_ON = 0; + o_n_L3 = 0; + log->licz_t3 = 0; + } + else if (!t_W3_ON && !t_W3_OFF) + { + //pamieta stan - nie zmienia + log->licz_t3+=LOOP_CYCLE_MS; + } + else if (t_W3_ON && t_W3_OFF) + { + o_W3_ON = 1; + log->licz_t3+=LOOP_CYCLE_MS; + } + + if (log->licz_t3 > log->nast_.licz_p) + { + o_n_L3 = 1; + log->licz_t3 = log->nast_.licz_p + (5*LOOP_CYCLE_MS); + } + + if (o_W1_ON && o_W2_ON && o_W3_ON) + { + o_zd_ON = 1; + o_zd_OFF = 0; + } + else if (!o_W1_ON && !o_W2_ON && !o_W3_ON) + { + o_zd_ON = 0; + o_zd_OFF = 1; + } + else + { + o_zd_ON = 0; + o_zd_OFF = 0; + } + + break; + default: + break; + + } + + if (o_W1_ON) + *log->wy_W1_ON |= log->wy_W1_ON_bit_mask; + else + *log->wy_W1_ON &= ~log->wy_W1_ON_bit_mask; + + if (o_W2_ON) + *log->wy_W2_ON |= log->wy_W2_ON_bit_mask; + else + *log->wy_W2_ON &= ~log->wy_W2_ON_bit_mask; + + if (o_W3_ON) + *log->wy_W3_ON |= log->wy_W3_ON_bit_mask; + else + *log->wy_W3_ON &= ~log->wy_W3_ON_bit_mask; + + if (o_zd_ON) + *log->zd_W_ON |= log->zd_W_ON_bit_mask; + else + *log->zd_W_ON &= ~log->zd_W_ON_bit_mask; + + if (o_zd_OFF) + *log->zd_W_OFF |= log->zd_W_OFF_bit_mask; + else + *log->zd_W_OFF &= ~log->zd_W_OFF_bit_mask; + + if (o_n_L1) + *log->niezg_L1 |= log->niezg_L1_bit_mask; + else + *log->niezg_L1 &= ~log->niezg_L1_bit_mask; + + if (o_n_L2) + *log->niezg_L2 |= log->niezg_L2_bit_mask; + else + *log->niezg_L2 &= ~log->niezg_L2_bit_mask; + + if (o_n_L3) + *log->niezg_L3 |= log->niezg_L3_bit_mask; + else + *log->niezg_L3 &= ~log->niezg_L3_bit_mask; + + +} diff --git a/src/CBR.h b/src/CBR.h new file mode 100644 index 0000000..f6eaba3 --- /dev/null +++ b/src/CBR.h @@ -0,0 +1,96 @@ +/* + * CBR.h + * + * Created on: 16-02-2017 + * Author: PS + */ + +#ifndef CBR_H_ +#define CBR_H_ + + +//#include +#include "tdefs.h" + +struct Nastawy_przeliczone_CBR +{ + u32 licz_p; + u32 rodzaj; +}; + +struct CBR_logic +{ + u8* we_W1_ON; + u8 we_W1_ON_bit_mask; + u8* we_W2_ON; + u8 we_W2_ON_bit_mask; + u8* we_W3_ON; + u8 we_W3_ON_bit_mask; + u8* we_W1_OFF; + u8 we_W1_OFF_bit_mask; + u8* we_W2_OFF; + u8 we_W2_OFF_bit_mask; + u8* we_W3_OFF; + u8 we_W3_OFF_bit_mask; + + u8* wy_W1_ON; + u8 wy_W1_ON_bit_mask; + u8* wy_W2_ON; + u8 wy_W2_ON_bit_mask; + u8* wy_W3_ON; + u8 wy_W3_ON_bit_mask; + u8* zd_W_ON; + u8 zd_W_ON_bit_mask; + u8* zd_W_OFF; + u8 zd_W_OFF_bit_mask; + u8* niezg_L1; + u8 niezg_L1_bit_mask; + u8* niezg_L2; + u8 niezg_L2_bit_mask; + u8* niezg_L3; + u8 niezg_L3_bit_mask; + + struct Nastawy_przeliczone_CBR nast_;// struktura z parametramio + + u32 licz_t1; + u32 licz_t2; + u32 licz_t3; + +}; + + +struct CBR_params +{ + u32 rodzaj; + double to; ///< nastawa czasy dzialania +}__attribute__((__packed__)); + +struct CBR_io +{ + u32 we_W1_ON; + u32 we_W2_ON; + u32 we_W3_ON; + u32 we_W1_OFF; + u32 we_W2_OFF; + u32 we_W3_OFF; + + u32 wy_W1_ON; + u32 wy_W2_ON; + u32 wy_W3_ON; + u32 zd_W_ON; + u32 zd_W_OFF; + u32 niezg_L1; + u32 niezg_L2; + u32 niezg_L3; +}__attribute__((__packed__)); + +struct CBR_args +{ + struct CBR_io io; + struct CBR_params params; +}__attribute__((__packed__)); + +extern void CBR(void *args, void *logic); +extern int CBR_initlog(void *arguments, void *logic); + +#endif /* CBR_H_ */ diff --git a/src/Diff.c b/src/Diff.c new file mode 100644 index 0000000..f553535 --- /dev/null +++ b/src/Diff.c @@ -0,0 +1,1465 @@ +/* + * Diff.c + * + * Created on: 12-04-2017 + * Author: Krzysztof Jakubczyk + */ + +#include +#include + +#include "tdefs.h" +#include "misc.h" + +#include "Diff.h" +#include "helper.h" +#include "analog_in.h" + + + uint16_t pY_ZPrAE[25] = { 0,1,0,2,0,3,0,4,0,5,0,6,0,0,7,0,8,0,9,0,10,0,11,0,12 }; // off,y0,y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11,d0,d1,d2,d3,d4,d5,d6,d7,d8,d9,d10,d11 + uint16_t pD_ZPrAE[25] = { 0,0,1,0,2,0,3,0,4,0,5,0,6,7,0,8,0,9,0,10,0,11,0,12,0 }; + + struct kom1_zprae Y2w_ZPrAE = { + zero,zero,zero, // off + A,A,zero, // y0 + A,O,zero, // y2 + A,V,zero, // y4 + A,B,zero, // y6 + A,N,zero, // y8 + A,U,zero, // y10 + A,C,zero, // yd1 + A,W,zero, // yd3 + A,Z,zero, // yd5 + A,Y,zero, // yd7 + A,X,zero, // yd9 + A,E,zero // yd11 + }; + + struct kom2_zprae Y3w_ZPrAE = { + zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, zero,zero,zero, + zero,zero,zero, A,A,A, A,A,O, A,A,V, A,A,B, A,A,N, A,A,U, A,A,C, A,A,W, A,A,Z, A,A,Y, A,A,X, A,A,E, // Yy0xx + zero,zero,zero, A,O,A, A,O,O, A,O,V, A,O,B, A,O,N, A,O,U, A,O,C, A,O,W, A,O,Z, A,O,Y, A,O,X, A,O,E, // Yy2xx + zero,zero,zero, A,V,A, A,V,O, A,V,V, A,V,B, A,V,N, A,V,U, A,V,C, A,V,W, A,V,Z, A,V,Y, A,V,X, A,V,E, // Yy4xx + zero,zero,zero, A,B,A, A,B,O, A,B,V, A,B,B, A,B,N, A,B,U, A,B,C, A,B,W, A,B,Z, A,B,Y, A,B,X, A,B,E, // Yy6xx + zero,zero,zero, A,N,A, A,N,O, A,N,V, A,N,B, A,N,N, A,N,U, A,N,C, A,N,W, A,N,Z, A,N,Y, A,N,X, A,N,E, // Yy8xx + zero,zero,zero, A,U,A, A,U,O, A,U,V, A,U,B, A,U,N, A,U,U, A,U,C, A,U,W, A,U,Z, A,U,Y, A,U,X, A,U,E, // Yy10xx + zero,zero,zero, A,C,A, A,C,O, A,C,V, A,C,B, A,C,N, A,C,U, A,C,C, A,C,W, A,C,Z, A,C,Y, A,C,X, A,C,E, // Yd1xx + zero,zero,zero, A,W,A, A,W,O, A,W,V, A,W,B, A,W,N, A,W,U, A,W,C, A,W,W, A,W,Z, A,W,Y, A,W,X, A,W,E, // Yd3xx + zero,zero,zero, A,Z,A, A,Z,O, A,Z,V, A,Z,B, A,Z,N, A,Z,U, A,Z,C, A,Z,W, A,Z,Z, A,Z,Y, A,Z,X, A,Z,E, // Yd5xx + zero,zero,zero, A,Y,A, A,Y,O, A,Y,V, A,Y,B, A,Y,N, A,Y,U, A,Y,C, A,Y,W, A,Y,Z, A,Y,Y, A,Y,X, A,Y,E, // Yd7xx + zero,zero,zero, A,X,A, A,X,O, A,X,V, A,X,B, A,X,N, A,X,U, A,X,C, A,X,W, A,X,Z, A,X,Y, A,X,X, A,X,E, // Yd9xx + zero,zero,zero, A,E,A, A,E,O, A,E,V, A,E,B, A,E,N, A,E,U, A,E,C, A,E,W, A,E,Z, A,E,Y, A,E,X, A,E,E // Yd11xx + }; + + struct kom1_zprae D2w_ZPrAE = { + zero,zero,zero, + A,C,zero, // dy1 + A,W,zero, // dy3 + A,F,zero, // dy5 + A,D,zero, // dy7 + A,X,zero, // dy9 + A,E,zero, // dy11 + A,A,zero, // dd0 + A,O,zero, // dd2 + A,V,zero, // dd4 + A,B,zero, // dd6 + A,N,zero, // dd8 + A,U,zero // dd10 + }; + + struct kom1_zprae D3w_ZPrAE = { + zero,zero,zero, + A,C,C, + A,W,W, + A,F,F, + A,D,D, + A,X,X, + A,E,E, + A,A,A, + A,O,O, + A,V,V, + A,B,B, + A,N,N, + A,U,U + }; + +int diff_init_io(struct diff_io *io, struct diff_logic *log) +{ + if(set_bit_ptr_struct(io->stan_bl_in,&log->stan_bl)) + return -1; + if(set_pointer_in_ptr(io->ia_l1_ptr_in,(u32 *)&log->I1_l1)) + return -1; + if(set_pointer_in_ptr(io->ia_l1_params_ptr_in,(u32 *)&log->I1_l1_params)) + return -1; + if(set_pointer_in_ptr(io->ia_l2_ptr_in,(u32 *)&log->I1_l2)) + return -1; + if(set_pointer_in_ptr(io->ia_l2_params_ptr_in,(u32 *)&log->I1_l2_params)) + return -1; + if(set_pointer_in_ptr(io->ia_l3_ptr_in,(u32 *)&log->I1_l3)) + return -1; + if(set_pointer_in_ptr(io->ia_l3_params_ptr_in,(u32 *)&log->I1_l3_params)) + return -1; + + if(set_pointer_in_ptr(io->ib_l1_ptr_in,(u32 *)&log->I2_l1)) + return -1; + if(set_pointer_in_ptr(io->ib_l1_params_ptr_in,(u32 *)&log->I2_l1_params)) + return -1; + if(set_pointer_in_ptr(io->ib_l2_ptr_in,(u32 *)&log->I2_l2)) + return -1; + if(set_pointer_in_ptr(io->ib_l2_params_ptr_in,(u32 *)&log->I2_l2_params)) + return -1; + if(set_pointer_in_ptr(io->ib_l3_ptr_in,(u32 *)&log->I2_l3)) + return -1; + if(set_pointer_in_ptr(io->ib_l3_params_ptr_in,(u32 *)&log->I2_l3_params)) + return -1; + + if(set_pointer_in_ptr(io->ic_l1_ptr_in,(u32 *)&log->I3_l1)) + return -1; + if(set_pointer_in_ptr(io->ic_l1_params_ptr_in,(u32 *)&log->I3_l1_params)) + return -1; + if(set_pointer_in_ptr(io->ic_l2_ptr_in,(u32 *)&log->I3_l2)) + return -1; + if(set_pointer_in_ptr(io->ic_l2_params_ptr_in,(u32 *)&log->I3_l2_params)) + return -1; + if(set_pointer_in_ptr(io->ic_l3_ptr_in,(u32 *)&log->I3_l3)) + return -1; + if(set_pointer_in_ptr(io->ic_l3_params_ptr_in,(u32 *)&log->I3_l3_params)) + return -1; + + if(set_pointer_in_ptr(io->id_l1_ptr_in,(u32 *)&log->I4_l1)) + return -1; + if(set_pointer_in_ptr(io->id_l1_params_ptr_in,(u32 *)&log->I4_l1_params)) + return -1; + if(set_pointer_in_ptr(io->id_l2_ptr_in,(u32 *)&log->I4_l2)) + return -1; + if(set_pointer_in_ptr(io->id_l2_params_ptr_in,(u32 *)&log->I4_l2_params)) + return -1; + if(set_pointer_in_ptr(io->id_l3_ptr_in,(u32 *)&log->I4_l3)) + return -1; + if(set_pointer_in_ptr(io->id_l3_params_ptr_in,(u32 *)&log->I4_l3_params)) + return -1; + + if(set_bit_ptr_struct(io->test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(io->w_out,&log->W)) + return -1; + + if(set_bit_ptr_struct(io->z_out,&log->Z)) + return -1; + + if(set_bit_ptr_struct(io->p_out,&log->P)) + return -1; + + if(set_bit_ptr_struct(io->pl1_out,&log->Pf1)) + return -1; + + if(set_bit_ptr_struct(io->pl2_out,&log->Pf2)) + return -1; + + if(set_bit_ptr_struct(io->pl3_out,&log->Pf3)) + return -1; + + if(set_bit_ptr_struct(io->bl_2h_L1_out,&log->BL2h1)) + return -1; + + if(set_bit_ptr_struct(io->bl_2h_L2_out,&log->BL2h2)) + return -1; + + if(set_bit_ptr_struct(io->bl_2h_L3_out,&log->BL2h3)) + return -1; + + if(set_bit_ptr_struct(io->bl_5h_L1_out,&log->BL5h1)) + return -1; + + if(set_bit_ptr_struct(io->bl_5h_L2_out,&log->BL5h2)) + return -1; + + if(set_bit_ptr_struct(io->bl_5h_L3_out,&log->BL5h3)) + return -1; + + if(set_bit_ptr_struct(io->zzew1_out,&log->Zw1)) + return -1; + + if(set_bit_ptr_struct(io->zzew2_out,&log->Zw2)) + return -1; + + if(set_bit_ptr_struct(io->zzew3_out,&log->Zw3)) + return -1; + + if(set_bit_ptr_struct(io->nieobs_out,&log->nieobs)) + return -1; + + if(set_float_ptr(io->di_l1_float_out,&log->Ir1)) + return -1; + if(set_float_ptr(io->di_l2_float_out,&log->Ir2)) + return -1; + if(set_float_ptr(io->di_l3_float_out,&log->Ir3)) + return -1; + + if(set_float_ptr(io->ih_l1_float_out,&log->Ih1)) + return -1; + if(set_float_ptr(io->ih_l2_float_out,&log->Ih2)) + return -1; + if(set_float_ptr(io->ih_l3_float_out,&log->Ih3)) + return -1; + if(set_float_ptr(io->i2h_l1_float_out,&log->I2h1)) + return -1; + if(set_float_ptr(io->i2h_l2_float_out,&log->I2h2)) + return -1; + if(set_float_ptr(io->i2h_l3_float_out,&log->I2h3)) + return -1; + if(set_float_ptr(io->i5h_l1_float_out,&log->I5h1)) + return -1; + if(set_float_ptr(io->i5h_l2_float_out,&log->I5h2)) + return -1; + if(set_float_ptr(io->i5h_l3_float_out,&log->I5h3)) + return -1; + + if(set_float_ptr(io->ir_l1_ch_float_out,&log->Ir_L1_ortb)) + return -1; + if(set_float_ptr(io->ir_l2_ch_float_out,&log->Ir_L2_ortb)) + return -1; + if(set_float_ptr(io->ir_l3_ch_float_out,&log->Ir_L3_ortb)) + return -1; + +/* if(set_float_ptr(io->ih_l1_ch_float_out,&log->Ih_L1_ortb)) + return -1; + if(set_float_ptr(io->ih_l2_ch_float_out,&log->Ih_L2_ortb)) + return -1; + if(set_float_ptr(io->ih_l3_ch_float_out,&log->Ih_L3_ortb)) + return -1;*/ + + log->an_struct_i1_l1.samples_buf=log->I1_l1; + log->an_struct_i1_l1.params=log->I1_l1_params; + log->an_struct_i1_l2.samples_buf=log->I1_l2; + log->an_struct_i1_l2.params=log->I1_l2_params; + log->an_struct_i1_l3.samples_buf=log->I1_l3; + log->an_struct_i1_l3.params=log->I1_l3_params; + + log->an_struct_i2_l1.samples_buf=log->I2_l1; + log->an_struct_i2_l1.params=log->I2_l1_params; + log->an_struct_i2_l2.samples_buf=log->I2_l2; + log->an_struct_i2_l2.params=log->I2_l2_params; + log->an_struct_i2_l3.samples_buf=log->I2_l3; + log->an_struct_i2_l3.params=log->I2_l3_params; + + log->an_struct_i3_l1.samples_buf=log->I3_l1; + log->an_struct_i3_l1.params=log->I3_l1_params; + log->an_struct_i3_l2.samples_buf=log->I3_l2; + log->an_struct_i3_l2.params=log->I3_l2_params; + log->an_struct_i3_l3.samples_buf=log->I3_l3; + log->an_struct_i3_l3.params=log->I3_l3_params; + + log->an_struct_i4_l1.samples_buf=log->I4_l1; + log->an_struct_i4_l1.params=log->I4_l1_params; + log->an_struct_i4_l2.samples_buf=log->I4_l2; + log->an_struct_i4_l2.params=log->I4_l2_params; + log->an_struct_i4_l3.samples_buf=log->I4_l3; + log->an_struct_i4_l3.params=log->I4_l3_params; + + + return 0; +} + +/** +Ustawia wskazniki na prady po skompensowaniu +\param typ wskazuje na podstawie nastawy odpowiednia konfiguracje pradow +\param nr_pradu okreslenie numeru przetwarzanago pradu +*/ + +void ustaw_kompensacje(struct diff_logic *log, enum typ_kompensacji typ, u16 nr_pradu) +{ + struct we_an_diff *a0,*a1,*a2; +/* + u16 *temp; + temp = ((uint16_t *)&nast_.adr.I1_L1 + 3 * nr_pradu); + const WA::Wyjscie* a0 = (const WA::Wyjscie*)wskazniki[*temp];U + if (*temp == 0) a0 = &nast_.wejscie_zero; + const WA::Wyjscie* a1 = (const WA::Wyjscie*)wskazniki[*(temp + 1)]; + if (*(temp + 1) == 0) a1 = &nast_.wejscie_zero; + const WA::Wyjscie* a2 = (const WA::Wyjscie*)wskazniki[*(temp + 2)]; + if (*(temp + 2) == 0) a2 = &nast_.wejscie_zero; +*/ + switch(nr_pradu) + { + case 0: + if((u32)log->I1_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i1_l1; + + + if((u32)log->I1_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i1_l2; + + if((u32)log->I1_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i1_l3; + break; + case 1: + if((u32)log->I2_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i2_l1; + + + if((u32)log->I2_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i2_l2; + + if((u32)log->I2_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i2_l3; + break; + case 2: + if((u32)log->I3_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i3_l1; + + + if((u32)log->I3_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i3_l2; + + if((u32)log->I3_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i3_l3; + break; + case 3: + if((u32)log->I4_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i4_l1; + + + if((u32)log->I4_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i4_l2; + + if((u32)log->I4_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i4_l3; + break; + default: + if((u32)log->I1_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i1_l1; + + + if((u32)log->I1_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i1_l2; + + if((u32)log->I1_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i1_l3; + break; + } + + switch (typ) + { + case zero: + log->nast_.L[0].Ip[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 0; + break; + case A: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 1; + break; + case B: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a1;// + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = -1; + break; + case C: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case D: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = -0.57735f; + break; + case E: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case F: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = -0.57735f; + break; + case N: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 1; + break; + case O: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = -1; + break; + + case U: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = -1; + break; + + case V: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 1; + break; + + + case W: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case X: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case Y: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case Z: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + break; + } +} + +void przelicz_nastawy1(struct diff_logic *log,struct nast_trafo n) +{ + int k; + + memset(&log->dw, 0x00, sizeof(log->dw)); // zerowanie przekaznika + memset(&log->l, 0x00, sizeof(log->l)); + memset(&log->P_, 0x00, sizeof(log->P_)); + + status_rej = oczekiwanie; + +#ifdef IZAZ600 + //sprawdzenie adresu danej analogowej dla rejestratora 3kHz + if ((nast_.adr.Ir_L1_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ir_L2_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ir_L3_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ih_L1_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ih_L2_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ih_L3_ortb < ilosc_kanalow_rej3kHz) + ) + nast_.Rej3kHz_adr_wej_OK = 1; else nast_.Rej3kHz_adr_wej_OK = 0; +#endif + + + //wyznaczenie adresow wejsc do rownan kompensacji + //OFF,y0,y1,y5,y6,y7,y11,d0,d1,d5,d7,d6,d11 + memset(&log->nast_.wejscie_zero, 0x00, sizeof(log->nast_.wejscie_zero)); + for (k = 0; k < SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2; k++) + log->nast_.buf_zero[k] = 32767; + + log->nast_.wejscie_zero.samples_buf = log->nast_.buf_zero; + log->nast_.wejscie_zero.params = &log->nast_.n_zero; + //log->nast_.wejscie_zero.nr_probki = &bufnr_probki; + log->nast_.n_zero.multiplier = 0; + log->nast_.n_zero.znam_pierw = 1; + log->nast_.n_zero.offset = 0; + memset(&log->nast_.a, 0x00, 4 * sizeof(float)); + + + if (n.GrP1) // jesli gorne uzwojenie trafo D + { + if (!n.GrP3) //jesli dwuuzwojeniowy trafo + { + log->jaki_uklad = tylko_T2w_D; + } + else + { //jesli trojuzwojeniowy trafo + log->jaki_uklad = tylko_T3w_D; + } + } + else + { //jesli gorne uzwojenie trafo Y + if (!n.GrP3) //jesli dwuuzwojeniowy trafo + { + log->jaki_uklad = tylko_T2w_Y; + } + else + { //jesli trojuzwojeniowy trafo + log->jaki_uklad = tylko_T3w_Y; + } + } + + //----------------------------------------- + + if(n.GrP2>12) + log->nast_.p2d=1; + else + log->nast_.p2d=0; + + if(n.GrP3>12) + log->nast_.p3d=1; + else + log->nast_.p3d=0; + + //ustawienie skladnikow kompensacji pradow + switch (log->jaki_uklad) + { + case tylko_T2w_Y: + ustaw_kompensacje(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje(log,zero, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + break; + case tylko_T2w_D: + ustaw_kompensacje(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje(log,zero, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + + break; + case tylko_T3w_Y: + ustaw_kompensacje(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Ig, 0); + ustaw_kompensacje(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Id, 1); + ustaw_kompensacje(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]||!pY_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + log->nast_.a[2] *= log->nast_.L[0].Ip[2]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT13 + ; + break; + case tylko_T3w_D: + ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]||!pD_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + log->nast_.a[2] *= log->nast_.L[0].Ip[2]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT13 + ; + break; + } + //----------------------------------------- + + //przeliczenie wspolczynnikow kompensacji o korekcje i ofset + uint16_t i; + + for (i = 0; i < 3; i++) + { + + log->nast_.L[i].a_p[0][0] = log->nast_.a[0] * log->nast_.L[i].Ip[0]->params->nast.mnoznik; + log->nast_.L[i].a_p[0][1] = log->nast_.L[i].Ip[0]->params->nast.offset + 32767; + log->nast_.L[i].a_m[0][0] = log->nast_.a[0] * log->nast_.L[i].Im[0]->params->nast.mnoznik; + log->nast_.L[i].a_m[0][1] = log->nast_.L[i].Im[0]->params->nast.offset + 32767; + log->nast_.L[i].a_p[1][0] = log->nast_.a[1] * log->nast_.L[i].Ip[1]->params->nast.mnoznik; + log->nast_.L[i].a_p[1][1] = log->nast_.L[i].Ip[1]->params->nast.offset + 32767; + log->nast_.L[i].a_m[1][0] = log->nast_.a[1] * log->nast_.L[i].Im[1]->params->nast.mnoznik; + log->nast_.L[i].a_m[1][1] = log->nast_.L[i].Im[1]->params->nast.offset + 32767; + log->nast_.L[i].a_p[2][0] = log->nast_.a[2] * log->nast_.L[i].Ip[2]->params->nast.mnoznik; + log->nast_.L[i].a_p[2][1] = log->nast_.L[i].Ip[2]->params->nast.offset + 32767; + log->nast_.L[i].a_m[2][0] = log->nast_.a[2] * log->nast_.L[i].Im[2]->params->nast.mnoznik; + log->nast_.L[i].a_m[2][1] = log->nast_.L[i].Im[2]->params->nast.offset + 32767; + log->nast_.L[i].a_p[3][0] = log->nast_.a[3] * log->nast_.L[i].Ip[3]->params->nast.mnoznik; + log->nast_.L[i].a_p[3][1] = log->nast_.L[i].Ip[3]->params->nast.offset + 32767; + log->nast_.L[i].a_m[3][0] = log->nast_.a[3] * log->nast_.L[i].Im[3]->params->nast.mnoznik; + log->nast_.L[i].a_m[3][1] = log->nast_.L[i].Im[3]->params->nast.offset + 32767; + + } + + // wyznaczenie wspolczynnikow filtru + + + for (i = 0; i < SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS; i++) + { + log->nast_.wsp_filtr_sin_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_cos_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_sin2h_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_cos2h_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_sin5h_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_cos5h_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + } + + //---------------------------------------------------------------------------------- + + memset(&log->w, 0x00, sizeof(log->w)); +} + +/** +Wylicza pobudzenie blokad od drugiej i piatej harmonicznej +\param *licz wskaznik na licznik pobudzen +\param pobudzenie warunek pobudzenia przekaznika +\param powrot warunek powrotu przekaznika +\param st stan przekaznika z poprzedniej iteracji +\param powz ilosc powtorzen przy zadzialaniu +\param powp ilosc powtorzen przy powrocie +\return aktualny stan przekaznika +*/ + +u8 kryterium1(short *licz, u8 pobudzenie, u8 powrot, u8 st, unsigned short powz, unsigned short powp) +{ + u8 stp; + stp = st; + if (st) + { + if (powrot) + { + *(licz + 1) += LOOP_CYCLE_MS; + if (*(licz + 1) > powp) + { + stp = 0; + *(licz) = 0; + *(licz + 1) = powp; + } + } + else { + *(licz + 1) -= LOOP_CYCLE_MS; + if (*(licz + 1) < 0) *(licz + 1) = 0; + } + } + else { + if (pobudzenie) + { + *(licz) += LOOP_CYCLE_MS; + if (*(licz) > powz) + { + stp = 1; + *(licz) = powz; + *(licz + 1) = 0; + } + } + else { + *(licz) -= LOOP_CYCLE_MS; + if (*(licz) < 0) *(licz) = 0; + } + } + return(stp); +} +//------------------ + + + +/** +Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji +\param Ir wartoscs pradu roznicowego +\param Ih wartosc pradu stabilizacji +*/ +void kryterium2(struct diff_logic *log,float Ir, float Ih, u8 *pob) +{ + + u16 n = 1; + + if (Ih > log->nast_.Ih1) n = 2; + if (Ih > log->nast_.Ihp) n = 3; + if ((Ih > log->nast_.Ihg) && log->nast_.h) n = 4; + *pob = 0; + *(pob + 1) = 0; + *(pob + 2) = 1; + switch (n) + { + case 1: + *pob = Ir > log->nast_.Iror; + *(pob + 1) = Ir < log->nast_.Irop; + break; + case 2: + *pob = Ir > Ih * log->nast_.kh1r; + *(pob + 1) = Ir < Ih * log->nast_.kh1p; + break; + case 3: + *pob = Ir > (Ih * log->nast_.kh2r + log->nast_.Ir3c); + *(pob + 1) = Ir < (Ih * log->nast_.kh2p + log->nast_.Ip3c); + break; + case 4: + *pob = Ir > log->nast_.Ir4; + *(pob + 1) = Ir < log->nast_.Ip4; + //*(pob + 2) = *(pob + 1); zakomentowane 11.04.20222 po bledach z blokada czasowa zgloszonymi przez NC + + break; + } +} + +/** +Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji +\param Ir wartosc pradu roznicowego +\param Ih wartosc pradu stabilizacji +*/ +void kryterium3(struct diff_logic *log,float Ir, float Ih, u8 *pob) +{ + + *pob = 0; + *(pob + 1) = 0; + *(pob + 2) = 1; + + if ((Ih > log->nast_.Ihg) && log->nast_.h) + { + *pob = Ir > log->nast_.Ir4; + *(pob + 1) = Ir < log->nast_.Ip4; + *(pob + 2) = *(pob + 1); + } + else { + *pob = Ir > (log->nast_.Iror + log->nast_.kh1r2 * Ih); + *(pob + 1) = Ir < (log->nast_.Irop + log->nast_.kh1p2 * Ih); + } +} + +void przelicz_1h_ip(struct diff_logic *log, uint16_t Lnr) +{ + int16_t i, i1,j; + float orta; + float ortb; + float pr; + + log->w.Ir[Lnr]=0; + + for(j=0;j<3;j++) + { + orta=0; + ortb=0; + i1 = log->w.nr_probki; + + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_ip[i1][((Lnr-3)*3)+j]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] += sqrtf(pr / 2); + } + + log->w.Ir[Lnr]/=2; + log->w.Ir[Lnr]*=log->w.Ir[Lnr]; +} + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void przelicz_1h(struct diff_logic *log, uint16_t Lnr) +{ + int16_t i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + log->w.orta[Lnr] = orta; + log->w.ortb[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] = pr / 2; +} + + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void przelicz_2h(struct diff_logic *log,uint16_t Lnr) +{ + + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin2h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos2h_[i]; + } + + log->w.orta2h[Lnr] = orta; + log->w.ortb2h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I2h[Lnr] = pr / 2; + +} + + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void przelicz_5h(struct diff_logic *log,uint16_t Lnr) +{ + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin5h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos5h_[i]; + } + + log->w.orta5h[Lnr] = orta; + log->w.ortb5h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I5h[Lnr] = pr / 2; +} + + +/** +Sprawdza kryterium dzialania przekaznika +Wywolywana powinna byc po kazdej nowej probce z czestotliwoscia 1200Hz +Ustawia zmienne zawarte w structurze "stan_"" + +*/ + +void przelicz_1200Hz_1(struct diff_logic *log) +{ + uint16_t i; + float local1, local2,b,c; + float i0[3]; + // wyliczenie pradu ronicowego i stabilizacji + float ich[3]; + + while (log->w.nr_probki != bus_an_cur_sample_num) + { + log->w.nr_probki++; + if (log->w.nr_probki > ((SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1)) log->w.nr_probki = 0; + + i0[0]=log->nast_.L[0].a_p[0][0] * ((float)log->nast_.L[0].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[0][1]) + + log->nast_.L[1].a_p[0][0] * ((float)log->nast_.L[1].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[0][1]) + + log->nast_.L[2].a_p[0][0] * ((float)log->nast_.L[2].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[0][1]); + + i0[1]=log->nast_.L[0].a_p[1][0] * ((float)log->nast_.L[0].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[1][1]) + + log->nast_.L[1].a_p[1][0] * ((float)log->nast_.L[1].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[1][1]) + + log->nast_.L[2].a_p[1][0] * ((float)log->nast_.L[2].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[1][1]); + + i0[2]=log->nast_.L[0].a_p[2][0] * ((float)log->nast_.L[0].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[2][1]) + + log->nast_.L[1].a_p[2][0] * ((float)log->nast_.L[1].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[2][1]) + + log->nast_.L[2].a_p[2][0] * ((float)log->nast_.L[2].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[2][1]); + + i0[0]/=3; + i0[1]/=3; + i0[2]/=3; + + for (i = 0; i < 3; i++) + { + + + local1 = log->nast_.L[i].a_p[0][0] * ((float)log->nast_.L[i].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[0][1]) + - log->nast_.L[i].a_m[0][0] * ((float)log->nast_.L[i].Im[0]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[0][1]); + + b = log->nast_.L[i].a_p[1][0] * ((float)log->nast_.L[i].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[1][1]) + - log->nast_.L[i].a_m[1][0] * ((float)log->nast_.L[i].Im[1]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[1][1]); + + c = log->nast_.L[i].a_p[2][0] * ((float)log->nast_.L[i].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[2][1]) + - log->nast_.L[i].a_m[2][0] * ((float)log->nast_.L[i].Im[2]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[2][1]); + + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + local1-=i0[0]; + if(!log->nast_.p2d) + b-=i0[1]; + if(!log->nast_.p3d) + c-=i0[2]; + } + + local2 = b + c + + log->nast_.L[i].a_p[3][0] * ((float)log->nast_.L[i].Ip[3]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[3][1]) + - log->nast_.L[i].a_m[3][0] * ((float)log->nast_.L[i].Im[3]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[3][1]); + + //log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2; + log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2; //zmienino 09.11.2020 + log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) / 2; + ich[i] = fabs(log->w.buf_Lr[log->w.nr_probki][0 + i]); + + log->w.buf_ip[log->w.nr_probki][i*3 + 0] = local1; + log->w.buf_ip[log->w.nr_probki][i*3 + 1] = b; + log->w.buf_ip[log->w.nr_probki][i*3 + 2] = c; + } + + + if (log->nast_.r) // jesli wlaczone wylaczenie bezwarunkowe + { + + log->P_.Irr1 = kryterium1(&log->l.Ir1[0], + (ich[0] > log->nast_.Irr), + (ich[0]nast_.Ipr), + log->P_.Irr1, + c1r, c1p); + log->P_.Irr2 = kryterium1(&log->l.Ir2[0], + (ich[1]>log->nast_.Irr), + (ich[1]nast_.Ipr), + log->P_.Irr2, + c1r, c1p); + log->P_.Irr3 = kryterium1(&log->l.Ir3[0], + (ich[2]>log->nast_.Irr), + (ich[2] < log->nast_.Ipr), + log->P_.Irr3, + c1r, c1p); + } + else log->P_.Irr1 = log->P_.Irr2 = log->P_.Irr3 = 0; + + } + + //---------------------------------------------------------------------------------------- + + //filtracja przebiegow + + + // 1h pradu rozruchowego + + + przelicz_1h(log,0); + przelicz_1h(log,1); + przelicz_1h(log,2); + + przelicz_1h_ip(log,3); + przelicz_1h_ip(log,4); + przelicz_1h_ip(log,5); + + //----------------------------------------------------------------------------------------- + + u8 war1 = log->w.Ir[0] > 0.0016; + u8 war2 = log->w.Ir[1] > 0.0016; + u8 war3 = log->w.Ir[2] > 0.0016; + + + + log->Ir1_ = sqrtf(log->w.Ir[0]); + log->Ir2_ = sqrtf(log->w.Ir[1]); + log->Ir3_ = sqrtf(log->w.Ir[2]); + log->Ih1_ = sqrtf(log->w.Ir[3]); + log->Ih2_ = sqrtf(log->w.Ir[4]); + log->Ih3_ = sqrtf(log->w.Ir[5]); + + if (log->nast_.bl2h) //jesli aktywna blokada od drugiej harmonicznej + { + + // druga harmoniczna + przelicz_2h(log,0); + przelicz_2h(log,1); + przelicz_2h(log,2); + + + log->P_.Bl2h_L1 = kryterium1(&log->l.l2h1[0], + ((log->w.I2h[0] > log->nast_.khr_2h*log->w.Ir[0]) && war1), + ((log->w.I2h[0]nast_.khp_2h*log->w.Ir[0]) || !war1), + log->P_.Bl2h_L1, + c2r, c2p); + log->P_.Bl2h_L2 = kryterium1(&log->l.l2h2[0], + ((log->w.I2h[1]>log->nast_.khr_2h*log->w.Ir[1]) && war2), + ((log->w.I2h[1]nast_.khp_2h*log->w.Ir[1]) || !war2), + log->P_.Bl2h_L2, + c2r, c2p); + log->P_.Bl2h_L3 = kryterium1(&log->l.l2h3[0], + ((log->w.I2h[2]>log->nast_.khr_2h*log->w.Ir[2]) && war3), + ((log->w.I2h[2]nast_.khp_2h*log->w.Ir[2]) || !war3), + log->P_.Bl2h_L3, + c2r, c2p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl2h_L1 || log->P_.Bl2h_L2 || log->P_.Bl2h_L3) + { + log->P_.Bl2h_L1 = 1; + log->P_.Bl2h_L2 = 1; + log->P_.Bl2h_L3 = 1; + } + else + { + log->P_.Bl2h_L1 = 0; + log->P_.Bl2h_L2 = 0; + log->P_.Bl2h_L3 = 0; + } + } + } + else log->P_.Bl2h_L1 = log->P_.Bl2h_L2 = log->P_.Bl2h_L3 = 0; + + if (log->nast_.bl5h) //jesli aktywna blokada od piatej harmonicznej + { + // piata harmoniczna + przelicz_5h(log,0); + przelicz_5h(log,1); + przelicz_5h(log,2); + + log->P_.Bl5h_L1 = kryterium1(&log->l.l5h1[0], + ((log->w.I5h[0]>log->nast_.khr_5h*log->w.Ir[0]) && war1), + ((log->w.I5h[0]nast_.khp_5h*log->w.Ir[0]) || !war1), + log->P_.Bl5h_L1, + c3r, c3p); + log->P_.Bl5h_L2 = kryterium1(&log->l.l5h2[0], + ((log->w.I5h[1]>log->nast_.khr_5h*log->w.Ir[1]) && war2), + ((log->w.I5h[1]nast_.khp_5h*log->w.Ir[1]) || !war2), + log->P_.Bl5h_L2, + c3r, c3p); + log->P_.Bl5h_L3 = kryterium1(&log->l.l5h3[0], + ((log->w.I5h[2]>log->nast_.khr_5h*log->w.Ir[2]) && war3), + ((log->w.I5h[2] < log->nast_.khp_5h*log->w.Ir[2]) || !war3), + log->P_.Bl5h_L3, + c3r, c3p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl5h_L1 || log->P_.Bl5h_L2 || log->P_.Bl5h_L3) + { + log->P_.Bl5h_L1 = 1; + log->P_.Bl5h_L2 = 1; + log->P_.Bl5h_L3 = 1; + } + else + { + log->P_.Bl5h_L1 = 0; + log->P_.Bl5h_L2 = 0; + log->P_.Bl5h_L3 = 0; + } + } + + } + else log->P_.Bl5h_L1 = log->P_.Bl5h_L2 = log->P_.Bl5h_L3 = 0; +} + + +/** +Sprawdza kryterium dzialania przekaznika +Wywolywana powinna byc po kazdej nowej probce z czestotliwoscia 1200Hz +Ustawia zmienne zawarte w structurze "stan_"" +\param w struktura danych wejsciowych algorytmlw dzialania +*/ +void przelicz_1200Hz_2(struct diff_logic *log) +{ + if (log->nast_.sbc) //jesli wwlaczona czasowa blokada dzialania + { + log->dw.sbc_P = (log->Ih1_ > log->nast_.Ihdr) && (log->Ir1_ < (log->Ih1_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw1 && kryterium1(&log->l.sbc1[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw1), + c4r, log->nast_.t_sbc), + &log->Zw1); + + log->dw.sbc_P = (log->Ih2_ > log->nast_.Ihdr) && (log->Ir2_ < (log->Ih2_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw2 && kryterium1(&log->l.sbc2[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw2), + c4r, log->nast_.t_sbc), + &log->Zw2); + + log->dw.sbc_P = (log->Ih3_ > log->nast_.Ihdr) && (log->Ir3_ < (log->Ih3_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw3 && kryterium1(&log->l.sbc3[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw3), + c4r, log->nast_.t_sbc), + &log->Zw3); + } + else + { + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + } + + log->P_.Psf1 = kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, c5r, c5p); + log->P_.Psf2 = kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, c5r, c5p); + log->P_.Psf3 = kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, c5r, c5p); + + //--------------------------------------------------------- + + //ustawienie wyjcia + + check_and_set_struct(log->P_.Irr1 || //pobudzenie bezwarunkowe + (log->P_.Psf1 && //prad w strefie dzialania + !check_struct(&log->Zw1) && //brak blokady czasowej + !log->P_.Bl2h_L1 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L1),//brak blokady od piatej harmonicznej) + &log->Pf1); + + check_and_set_struct(log->P_.Irr2 || //pobudzenie bezwarunkowe + (log->P_.Psf2 && //prad w strefie dzialania + !check_struct(&log->Zw2) && //brak blokady czasowej + !log->P_.Bl2h_L2 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L2),//brak blokady od piatej harmonicznej + &log->Pf2); + + check_and_set_struct(log->P_.Irr3 || //pobudzenie bezwarunkowe + (log->P_.Psf3 && //prad w strefie dzialania + !check_struct(&log->Zw3) && //brak blokady czasowej + !log->P_.Bl2h_L3 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L3),//brak blokady od piatej harmonicznej + &log->Pf3); + +//------------------- + + check_and_set_struct(check_struct(&log->Pf1)||check_struct(&log->Pf2)||check_struct(&log->Pf3),&log->P); + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + check_and_set_struct(check_struct(&log->test),&log->P); + check_and_set_struct(check_struct(&log->test),&log->Pf1); + check_and_set_struct(check_struct(&log->test),&log->Pf2); + check_and_set_struct(check_struct(&log->test),&log->Pf3); + } + + // realizacja blokady dzialania przekaznika + + if (((log->nast_.bl && check_struct(&log->stan_bl))||check_struct(&log->nieobs))/* || czy_start()*/) + { + clear_struct(&log->P); + clear_struct(&log->Pf1); + clear_struct(&log->Pf2); + clear_struct(&log->Pf3); + } + // + + // realizacja przekaznika czasowego + if (check_struct(&log->P)) + log->dw.licz_t+=LOOP_CYCLE_MS; + else + log->dw.licz_t = 0; // odliczanie czasu dzialania + + if (log->dw.licz_t > log->nast_.tz) + { // sprawdzenie przekroczenia nastawionego czasu + //if (!check_struct(&log->Z)) log->dw.czas_ = time; + set_struct(&log->Z); // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t-=LOOP_CYCLE_MS; + } + else clear_struct(&log->Z); + // + + check_and_set_struct(check_struct(&log->Z) && log->nast_.w,&log->W); //wyprowadzenie wylaczenia zaleznie od nastawy + + +//wystawienie pomiarow + + if(!check_struct(&log->nieobs)) + { + *log->Ir1 = log->w.Ir[0]; + *log->Ir2 = log->w.Ir[1]; + *log->Ir3 = log->w.Ir[2]; + *log->Ih1 = log->w.Ir[3]; + *log->Ih2 = log->w.Ir[4]; + *log->Ih3 = log->w.Ir[5]; + *log->Ir_L1_ortb = log->w.buf_Lr[log->w.nr_probki][0]; + *log->Ir_L2_ortb = log->w.buf_Lr[log->w.nr_probki][1]; + *log->Ir_L3_ortb = log->w.buf_Lr[log->w.nr_probki][2]; + *log->I2h1 = log->w.I2h[0]; + *log->I2h2 = log->w.I2h[1]; + *log->I2h3 = log->w.I2h[2]; + *log->I5h1 = log->w.I5h[0]; + *log->I5h2 = log->w.I5h[1]; + *log->I5h3 = log->w.I5h[2]; + } + else + { + *log->Ir1 = 0; + *log->Ir2 = 0; + *log->Ir3 = 0; + *log->Ih1 = 0; + *log->Ih2 = 0; + *log->Ih3 = 0; + *log->Ir_L1_ortb = 0; + *log->Ir_L2_ortb = 0; + *log->Ir_L3_ortb = 0; + *log->I2h1 = 0; + *log->I2h2 = 0; + *log->I2h3 = 0; + *log->I5h1 = 0; + *log->I5h2 = 0; + *log->I5h3 = 0; + } + +#ifdef IZAZ600 + int nr_pr; + if (nast_.Rej3kHz_adr_wej_OK) { + for (int i = 0; i < 3; i++) + { + int j = 2 - i; + nr_pr = w.nr_probki; + nr_pr = nr_pr - j; + if (nr_pr < 0) nr_pr += 120; + + dane_analogowe_3[nast_.adr.Ir_L1_ortb][i] = w.buf_Lr[nr_pr][0]; + dane_analogowe_3[nast_.adr.Ir_L2_ortb][i] = w.buf_Lr[nr_pr][1]; + dane_analogowe_3[nast_.adr.Ir_L3_ortb][i] = w.buf_Lr[nr_pr][2]; + dane_analogowe_3[nast_.adr.Ih_L1_ortb][i] = w.buf_Lr[nr_pr][3]; + dane_analogowe_3[nast_.adr.Ih_L2_ortb][i] = w.buf_Lr[nr_pr][4]; + dane_analogowe_3[nast_.adr.Ih_L3_ortb][i] = w.buf_Lr[nr_pr][5]; + } + } +#endif + +//---------------------------------------------- + check_and_set_struct(log->P_.Bl2h_L1 && log->P_.Psf1,&log->BL2h1); + check_and_set_struct(log->P_.Bl2h_L2 && log->P_.Psf2,&log->BL2h2); + check_and_set_struct(log->P_.Bl2h_L3 && log->P_.Psf3,&log->BL2h3); + check_and_set_struct(log->P_.Bl5h_L1 && log->P_.Psf1,&log->BL5h1); + check_and_set_struct(log->P_.Bl5h_L2 && log->P_.Psf2,&log->BL5h2); + check_and_set_struct(log->P_.Bl5h_L3 && log->P_.Psf3,&log->BL5h3); +} + + + +/** +Funkcja drugiego poziomu. +Funkcja wywolywana z czestotliwoscia drugiego poziomu (w iZAZ400...800 to 100Hz) +Wszystkie funkcje tego typu (z calej konfiguracji) powinny sie bezwzglednie wykonac w kadym cyklu. +Funkcje powinny byc wykonywane z priorytetem srednim +(przerywane tylko obsluga przetwornika AC i funkcja przelicz_1()) +Przekroczenie 60% czasu petli powinno generowac zdarzenie systemowe typu WARRNING, +a kilkukrotne przekroczenie 100% czasu petli powinno przelaczyc zabezpieczenie w stan ERROR.. +*/ +void przelicz_2(struct diff_logic *log) +{ +if(log->nast_.on) // jesli zabezpieczenie aktywne +{ + + + // realizacja rejestratora parametrow ostatniego zaklocenia + + if (log->nast_.w) + switch(status_rej) + { + case oczekiwanie: + if (check_struct(&log->P)) + { + log->dw.maxIr1 = log->Ir1_; + log->dw.maxIr2 = log->Ir2_; + log->dw.maxIr3 = log->Ir3_; + log->dw.maxIh1 = log->Ih1_; + log->dw.maxIh2 = log->Ih2_; + log->dw.maxIh3 = log->Ih3_; + log->dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + if (check_struct(&log->Z)) + { + status_rej = sledzenie_zad; + } + } + break; + case sledzenie: + if (check_struct(&log->Z)) + { + status_rej = sledzenie_zad; + //log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (check_struct(&log->P)) + { + if(log->dw.maxIr1 < log->Ir1_) + { + log->dw.maxIr1 = log->Ir1_; + log->dw.maxIh1 = log->Ih1_; + } + if(log->dw.maxIr2 < log->Ir2_) + { + log->dw.maxIr2 = log->Ir2_; + log->dw.maxIh2 = log->Ih2_; + } + if(log->dw.maxIr3 < log->Ir3_) + { + log->dw.maxIr3 = log->Ir3_; + log->dw.maxIh3 = log->Ih3_; + } + log->dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!check_struct(&log->P)) + { + log->dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + log->dw.dl_++; + if(log->dw.maxIr1 < log->Ir1_) + { + log->dw.maxIr1 = log->Ir1_; + log->dw.maxIh1 = log->Ih1_; + } + if(log->dw.maxIr2 < log->Ir2_) + { + log->dw.maxIr2 = log->Ir2_; + log->dw.maxIh2 = log->Ih2_; + } + if(log->dw.maxIr3 < log->Ir3_) + { + log->dw.maxIr3 = log->Ir3_; + log->dw.maxIh3 = log->Ih3_; + } + } + break; + case zapis: + if (!log->dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + } +} +} + +/** +Funkcja piatego poziomu +Funkcja wywolywana bez priorytetu w czasie wolnym procesora ale nie rzadziej ni 1 s. +*/ +void przelicz_5(struct diff_logic *log) +{ + if(log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + { + // log->dw.R.max_L1_ = log->dw.maxIr1; // wyliczenie maksymalnej wartosci faza 1 + // log->dw.R.Ih1 = log->dw.maxIh1; + // log->dw.R.max_L2_ = log->dw.maxIr2; // wyliczenie maksymalnej wartosci faza 2 + // log->dw.R.Ih2 = log->dw.maxIh2; + // log->dw.R.max_L3_ = log->dw.maxIr3; // wyliczenie maksymalnej wartosci faza 3 + //log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // log->dw.R.Ih3 = log->dw.maxIh3; + //log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // log->dw.R.dlugosc = (float)log->dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia + // log->dw.R.czy_nowa = 1; // sygnalizacja powstania nowej rejestracji + log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + } +} + + diff --git a/src/Diff.h b/src/Diff.h new file mode 100644 index 0000000..e8562af --- /dev/null +++ b/src/Diff.h @@ -0,0 +1,431 @@ +/* + * Diff.h + * + * Created on: 12-04-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DIFF_H_ +#define DIFF_H_ + +#include "tdefs.h" +#include "analog_in.h" +#include "helper.h" +#include "config.h" + +extern u16 pY_ZPrAE[25]; +extern u16 pD_ZPrAE[25]; +extern struct kom1_zprae Y2w_ZPrAE; +extern struct kom2_zprae Y3w_ZPrAE; +extern struct kom1_zprae D2w_ZPrAE; +extern struct kom1_zprae D3w_ZPrAE; + +#define c1r 5 //ilosc powtorzen zadzialania bezwarunkowego +#define c1p 30 //ilosc powtorzen odpadu bezwarunkowego +#define c2r 2 //ilosc powtorzen zadzialania blokady 2h +#define c2p 7 //ilosc powtorzen odpadu blokady 2h +#define c3r 2 //ilosc powtorzen zadzialania blokady 5h +#define c3p 10 //ilosc powtorzen odpadu blokady 5h +#define c4r 5 //ilosc powtorzen zadzialania czasowej blokady +#define c5r 20 //ilosc powtorzen zadzialania stabilizowanej strefy dzialania !!!!!!!!Pawel bylo 2 +#define c5p 2 //ilosc powtorzen odpadu stabilizowanej strefy dzialania + + struct we_an_diff + { + u16 *samples_buf; + struct analog_in_params_extended *params; + }; + + struct licz + { + short l2h1[2]; + short l2h2[2]; + short l2h3[2]; + short l5h1[2]; + short l5h2[2]; + short l5h3[2]; + short Ir1[2]; + short Ir2[2]; + short Ir3[2]; + short sbc1[2]; + short sbc2[2]; + short sbc3[2]; + short chs1[2]; + short chs2[2]; + short chs3[2]; + + }; + + struct pobudzenia_czastkowe + { + u8 Irr1; + u8 Irr2; + u8 Irr3; + u8 Pb1; + u8 Ob1; + u8 BlZw1; + u8 Pb2; + u8 Ob2; + u8 BlZw2; + u8 Pb3; + u8 Ob3; + u8 BlZw3; + u8 Psf1; + u8 Psf2; + u8 Psf3; + u8 Bl2h_L1; + u8 Bl2h_L2; + u8 Bl2h_L3; + u8 Bl5h_L1; + u8 Bl5h_L2; + u8 Bl5h_L3; + }; + + struct wskazniki_prad_do_kompensacji + { + struct we_an_diff *Ip[4]; + struct we_an_diff *Im[4]; + + float a_p[4][2]; + float a_m[4][2]; + }; + + struct Nastawy_przeliczone + { +// Adresy adr; + + float khr_2h;///< wartosc rozruchowa blokady od 2 harmonicznej + float khp_2h;///< wartosc powrotowa blokady od 2 harmonicznej + float khr_5h;///< wartosc rozruchowa blokady od 5 harmonicznej + float khp_5h;///< wartosc powrotowa blokady od 5 harmonicznej + float Irr;///< wartosc rozruchowa dzialania bezwarunkowego + float Ipr;///< wartosc powrotowa dzialania bezwarunkowego + float Ihdr;/// + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "Diff.h" +#include "Diff_1.h" + +int diff_1_initlog(void *arguments, void *logic) +{ + struct diff_1_args *args = (struct diff_1_args *)arguments; + struct diff_logic *log = (struct diff_logic *)logic; + + if(diff_init_io(&args->io,log)) + return -1; + + log->nast_.on = (args->params.bity & 0x0001)?1:0; + log->nast_.w = (args->params.bity & 0x0002)?1:0; + log->nast_.bl = (args->params.bity & 0x0004)?1:0; + log->nast_.bl2h = (args->params.bity & 0x0008)?1:0; + log->nast_.bl5h = (args->params.bity & 0x0010)?1:0; + log->nast_.sbc = (args->params.bity & 0x0020)?1:0; + log->nast_.h = (args->params.bity & 0x0040)?1:0; + log->nast_.r = (args->params.bity & 0x0080)?1:0; + log->nast_.cross_block = (args->params.bity & 0x0100)?1:0; + + //ustawienie wartosci rozruchowych + log->nast_.khr_2h = args->params.k_2h * args->params.k_2h; + log->nast_.khp_2h = log->nast_.khr_2h * 0.98;//args->params.kp *args->params.kp; + log->nast_.khr_5h = args->params.k_5h * args->params.k_5h; + log->nast_.khp_5h = log->nast_.khr_5h * 0.98;//args->params.kp *args->params.kp; + log->nast_.Irr = args->params.Ir * 1.3577f; + log->nast_.Ipr = args->params.Ir * args->params.kp; + log->nast_.Ihdr = args->params.Ihd; + log->nast_.Irdr = args->params.rd; + log->nast_.kh1r = args->params.kh1; + log->nast_.kh1p = args->params.kh1 * args->params.kp; + log->nast_.kh2r = args->params.kh2; + log->nast_.kh2p = args->params.kh2 * args->params.kp; + log->nast_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ip3c = log->nast_.Ir3c * args->params.kp; + log->nast_.Iror = args->params.Iro; + log->nast_.Irop = args->params.Iro * args->params.kp; + log->nast_.Ir4 = args->params.Ihg * args->params.kh2 + args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ip4 = log->nast_.Ir4 * args->params.kp; + log->nast_.t_sbc = args->params.tbld * 1000; + log->nast_.Ih1 = args->params.Iro / args->params.kh1; + log->nast_.Ihp = args->params.Ihp; + log->nast_.Ihg = args->params.Ihg; + + log->nast_.tz = args->params.t * 1000; + +// log->nast_.ih_siemens = (args->params.Ih_typ == IH_SUM_IP) ? 1 : 0; +//-------- + + struct nast_trafo n1; + n1.GrP1 = args->params.GrP1; + n1.GrP2 = args->params.GrP2; + n1.GrP3 = args->params.GrP3; + n1.PT12 = args->params.PT12; + n1.PT13 = args->params.PT13; +// n1.Tr_zas = args->params.Tr_zas; + + przelicz_nastawy1(log,n1); + + return 0; +} + +void diff_1(void *arguments, void *logic) +{ + struct diff_logic *log = (struct diff_logic *)logic; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + przelicz_1200Hz_1(log); + + //Sprawdzenie pobudzenia w stabilizowanej strefie dzialania + kryterium2(log,log->Ir1_,log->Ih1_,&log->P_.Pb1); + kryterium2(log,log->Ir2_,log->Ih2_,&log->P_.Pb2); + kryterium2(log,log->Ir3_,log->Ih3_,&log->P_.Pb3); + + przelicz_1200Hz_2(log); + } else { + *log->Ir1=0; + *log->Ir2=0; + *log->Ir3=0; + *log->Ih1=0; + *log->Ih2=0; + *log->Ih3=0; + *log->Ir_L1_ortb=0; + *log->Ir_L2_ortb=0; + *log->Ir_L3_ortb=0; + // *log->Ih_L1_ortb=0; + //*log->Ih_L2_ortb=0; + //*log->Ih_L3_ortb=0; + *log->I2h1=0; + *log->I2h2=0; + *log->I2h3=0; + *log->I5h1=0; + *log->I5h2=0; + *log->I5h3=0; + clear_struct(&log->P); + clear_struct(&log->Pf1); + clear_struct(&log->Pf2); + clear_struct(&log->Pf3); + clear_struct(&log->W); + clear_struct(&log->Z); + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + clear_struct(&log->BL2h1); + clear_struct(&log->BL2h2); + clear_struct(&log->BL2h3); + clear_struct(&log->BL5h1); + clear_struct(&log->BL5h2); + clear_struct(&log->BL5h3); + } +} diff --git a/src/Diff_1.h b/src/Diff_1.h new file mode 100644 index 0000000..f013856 --- /dev/null +++ b/src/Diff_1.h @@ -0,0 +1,52 @@ +/* + * Diff_1.h + * + * Created on: 01-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DIFF_1_H_ +#define DIFF_1_H_ + +#include "Diff.h" + +#include "tdefs.h" + +struct diff_1_params +{ + u32 bity; ///< nastawy bitowe; + +// u32 Tr_zas;/// + +#include "tdefs.h" +#include "misc.h" +#include "helper.h" + + + +#include "Diff.h" +#include "Diff_1_bis.h" + +#define c1r_nowe 2 //ilosc powtorzen zadzialania bezwarunkowego +#define c1p_nowe 5 //ilosc powtorzen odpadu bezwarunkowego + +void ustaw_kompensacje_bis(struct diff_1_bis_logic *log, enum typ_kompensacji typ, u16 nr_pradu) +{ + struct we_an_diff *a0,*a1,*a2; + + switch(nr_pradu) + { + case 0: + if((u32)log->I1_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i1_l1; + + + if((u32)log->I1_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i1_l2; + + if((u32)log->I1_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i1_l3; + break; + case 1: + if((u32)log->I2_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i2_l1; + + + if((u32)log->I2_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i2_l2; + + if((u32)log->I2_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i2_l3; + break; + case 2: + if((u32)log->I3_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i3_l1; + + + if((u32)log->I3_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i3_l2; + + if((u32)log->I3_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i3_l3; + break; + case 3: + + a0=&log->nast_.wejscie_zero; + a1=&log->nast_.wejscie_zero; + a2=&log->nast_.wejscie_zero; + + break; + default: + if((u32)log->I1_l1==(u32)&log_manager.nets_data[0]) + a0=&log->nast_.wejscie_zero; + else + a0 = &log->an_struct_i1_l1; + + + if((u32)log->I1_l2==(u32)&log_manager.nets_data[0]) + a1=&log->nast_.wejscie_zero; + else + a1 = &log->an_struct_i1_l2; + + if((u32)log->I1_l3==(u32)&log_manager.nets_data[0]) + a2=&log->nast_.wejscie_zero; + else + a2 = &log->an_struct_i1_l3; + break; + } + + switch (typ) + { + case zero: + log->nast_.L[0].Ip[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 0; + break; + case A: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 1; + break; + case B: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a1;// + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = -1; + break; + case C: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case D: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = -0.57735f; + break; + case E: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case F: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = -0.57735f; + break; + case N: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 1; + break; + case O: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = -1; + break; + + case U: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = -1; + break; + + case V: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.wejscie_zero; + log->nast_.a[nr_pradu] = 1; + break; + + + case W: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case X: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case Y: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + break; + case Z: //Pawel + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + break; + } +} + + +void przelicz_nastawy_bis(struct diff_1_bis_logic *log,struct nast_trafo n) +{ + int k; + + memset(&log->dw, 0x00, sizeof(log->dw)); // zerowanie przekaznika + memset(&log->l, 0x00, sizeof(log->l)); + memset(&log->P_, 0x00, sizeof(log->P_)); + + //status_rej = oczekiwanie; + +#ifdef IZAZ600 + //sprawdzenie adresu danej analogowej dla rejestratora 3kHz + if ((nast_.adr.Ir_L1_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ir_L2_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ir_L3_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ih_L1_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ih_L2_ortb < ilosc_kanalow_rej3kHz) + & (nast_.adr.Ih_L3_ortb < ilosc_kanalow_rej3kHz) + ) + nast_.Rej3kHz_adr_wej_OK = 1; else nast_.Rej3kHz_adr_wej_OK = 0; +#endif + + + //wyznaczenie adresow wejsc do rownan kompensacji + //OFF,y0,y1,y5,y6,y7,y11,d0,d1,d5,d7,d6,d11 + memset(&log->nast_.wejscie_zero, 0x00, sizeof(log->nast_.wejscie_zero)); + for (k = 0; k < SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2; k++) + log->nast_.buf_zero[k] = 32767; + + log->nast_.wejscie_zero.samples_buf = log->nast_.buf_zero; + log->nast_.wejscie_zero.params = &log->nast_.n_zero; + //log->nast_.wejscie_zero.nr_probki = &bufnr_probki; + log->nast_.n_zero.multiplier = 0; + log->nast_.n_zero.znam_pierw = 1; + log->nast_.n_zero.offset = 0; + memset(&log->nast_.a, 0x00, 4 * sizeof(float)); + + + if (n.GrP1) // jesli gorne uzwojenie trafo D + { + if (!n.GrP3) //jesli dwuuzwojeniowy trafo + { + log->jaki_uklad = tylko_T2w_D; + } + else + { //jesli trojuzwojeniowy trafo + log->jaki_uklad = tylko_T3w_D; + } + } + else + { //jesli gorne uzwojenie trafo Y + if (!n.GrP3) //jesli dwuuzwojeniowy trafo + { + log->jaki_uklad = tylko_T2w_Y; + } + else + { //jesli trojuzwojeniowy trafo + log->jaki_uklad = tylko_T3w_Y; + } + } + + //----------------------------------------- + + if(n.GrP2>12) + log->nast_.p2d=1; + else + log->nast_.p2d=0; + + if(n.GrP3>12) + log->nast_.p3d=1; + else + log->nast_.p3d=0; + + //ustawienie skladnikow kompensacji pradow + switch (log->jaki_uklad) + { + case tylko_T2w_Y: + ustaw_kompensacje_bis(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje_bis(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje_bis(log,zero, 2); + ustaw_kompensacje_bis(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + break; + case tylko_T2w_D: + ustaw_kompensacje_bis(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje_bis(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje_bis(log,zero, 2); + ustaw_kompensacje_bis(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + + break; + case tylko_T3w_Y: + ustaw_kompensacje_bis(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Ig, 0); + ustaw_kompensacje_bis(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Id, 1); + ustaw_kompensacje_bis(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje_bis(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]||!pY_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + log->nast_.a[2] *= log->nast_.L[0].Ip[2]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT13 + ; + break; + case tylko_T3w_D: + ustaw_kompensacje_bis(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje_bis(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje_bis(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje_bis(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]||!pD_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] *= log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT12 + ; + log->nast_.a[2] *= log->nast_.L[0].Ip[2]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / n.PT13 + ; + break; + } + //----------------------------------------- + + //przeliczenie wspolczynnikow kompensacji o korekcje i ofset + uint16_t i; + + for (i = 0; i < 3; i++) + { + + log->nast_.L[i].a_p[0][0] = log->nast_.a[0] * log->nast_.L[i].Ip[0]->params->nast.mnoznik; + log->nast_.L[i].a_p[0][1] = log->nast_.L[i].Ip[0]->params->nast.offset + 32767; + log->nast_.L[i].a_m[0][0] = log->nast_.a[0] * log->nast_.L[i].Im[0]->params->nast.mnoznik; + log->nast_.L[i].a_m[0][1] = log->nast_.L[i].Im[0]->params->nast.offset + 32767; + log->nast_.L[i].a_p[1][0] = log->nast_.a[1] * log->nast_.L[i].Ip[1]->params->nast.mnoznik; + log->nast_.L[i].a_p[1][1] = log->nast_.L[i].Ip[1]->params->nast.offset + 32767; + log->nast_.L[i].a_m[1][0] = log->nast_.a[1] * log->nast_.L[i].Im[1]->params->nast.mnoznik; + log->nast_.L[i].a_m[1][1] = log->nast_.L[i].Im[1]->params->nast.offset + 32767; + log->nast_.L[i].a_p[2][0] = log->nast_.a[2] * log->nast_.L[i].Ip[2]->params->nast.mnoznik; + log->nast_.L[i].a_p[2][1] = log->nast_.L[i].Ip[2]->params->nast.offset + 32767; + log->nast_.L[i].a_m[2][0] = log->nast_.a[2] * log->nast_.L[i].Im[2]->params->nast.mnoznik; + log->nast_.L[i].a_m[2][1] = log->nast_.L[i].Im[2]->params->nast.offset + 32767; + log->nast_.L[i].a_p[3][0] = log->nast_.a[3] * log->nast_.L[i].Ip[3]->params->nast.mnoznik; + log->nast_.L[i].a_p[3][1] = log->nast_.L[i].Ip[3]->params->nast.offset + 32767; + log->nast_.L[i].a_m[3][0] = log->nast_.a[3] * log->nast_.L[i].Im[3]->params->nast.mnoznik; + log->nast_.L[i].a_m[3][1] = log->nast_.L[i].Im[3]->params->nast.offset + 32767; + + } + + // wyznaczenie wspolczynnikow filtru + + + for (i = 0; i < SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS; i++) + { + log->nast_.wsp_filtr_sin_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_cos_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_sin2h_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_cos2h_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_sin5h_[i] = sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + log->nast_.wsp_filtr_cos5h_[i] = cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + } + + //---------------------------------------------------------------------------------- + + memset(&log->w, 0x00, sizeof(log->w)); +} + +int diff_1_bis_initlog(void *arguments, void *logic) +{ + struct diff_1_bis_args *args = (struct diff_1_bis_args *)arguments; + struct diff_1_bis_logic *log = (struct diff_1_bis_logic *)logic; + + if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl)) + return -1; + if(set_pointer_in_ptr(args->io.ia_l1_ptr_in,(u32 *)&log->I1_l1)) + return -1; + if(set_pointer_in_ptr(args->io.ia_l1_params_ptr_in,(u32 *)&log->I1_l1_params)) + return -1; + if(set_pointer_in_ptr(args->io.ia_l2_ptr_in,(u32 *)&log->I1_l2)) + return -1; + if(set_pointer_in_ptr(args->io.ia_l2_params_ptr_in,(u32 *)&log->I1_l2_params)) + return -1; + if(set_pointer_in_ptr(args->io.ia_l3_ptr_in,(u32 *)&log->I1_l3)) + return -1; + if(set_pointer_in_ptr(args->io.ia_l3_params_ptr_in,(u32 *)&log->I1_l3_params)) + return -1; + + if(set_pointer_in_ptr(args->io.ib_l1_ptr_in,(u32 *)&log->I2_l1)) + return -1; + if(set_pointer_in_ptr(args->io.ib_l1_params_ptr_in,(u32 *)&log->I2_l1_params)) + return -1; + if(set_pointer_in_ptr(args->io.ib_l2_ptr_in,(u32 *)&log->I2_l2)) + return -1; + if(set_pointer_in_ptr(args->io.ib_l2_params_ptr_in,(u32 *)&log->I2_l2_params)) + return -1; + if(set_pointer_in_ptr(args->io.ib_l3_ptr_in,(u32 *)&log->I2_l3)) + return -1; + if(set_pointer_in_ptr(args->io.ib_l3_params_ptr_in,(u32 *)&log->I2_l3_params)) + return -1; + + if(set_pointer_in_ptr(args->io.ic_l1_ptr_in,(u32 *)&log->I3_l1)) + return -1; + if(set_pointer_in_ptr(args->io.ic_l1_params_ptr_in,(u32 *)&log->I3_l1_params)) + return -1; + if(set_pointer_in_ptr(args->io.ic_l2_ptr_in,(u32 *)&log->I3_l2)) + return -1; + if(set_pointer_in_ptr(args->io.ic_l2_params_ptr_in,(u32 *)&log->I3_l2_params)) + return -1; + if(set_pointer_in_ptr(args->io.ic_l3_ptr_in,(u32 *)&log->I3_l3)) + return -1; + if(set_pointer_in_ptr(args->io.ic_l3_params_ptr_in,(u32 *)&log->I3_l3_params)) + return -1; +/* + if(set_pointer_in_ptr(args->io.id_l1_ptr_in,(u32 *)&log->I4_l1)) + return -1; + if(set_pointer_in_ptr(args->io.id_l1_params_ptr_in,(u32 *)&log->I4_l1_params)) + return -1; + if(set_pointer_in_ptr(args->io.id_l2_ptr_in,(u32 *)&log->I4_l2)) + return -1; + if(set_pointer_in_ptr(args->io.id_l2_params_ptr_in,(u32 *)&log->I4_l2_params)) + return -1; + if(set_pointer_in_ptr(args->io.id_l3_ptr_in,(u32 *)&log->I4_l3)) + return -1; + if(set_pointer_in_ptr(args->io.id_l3_params_ptr_in,(u32 *)&log->I4_l3_params)) + return -1; +*/ + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.w_out,&log->W)) + return -1; + + if(set_bit_ptr_struct(args->io.z_out,&log->Z)) + return -1; + + if(set_bit_ptr_struct(args->io.p_out,&log->P)) + return -1; + + if(set_bit_ptr_struct(args->io.pl1_out,&log->Pf1)) + return -1; + + if(set_bit_ptr_struct(args->io.pl2_out,&log->Pf2)) + return -1; + + if(set_bit_ptr_struct(args->io.pl3_out,&log->Pf3)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L1_out,&log->BL2h1)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L2_out,&log->BL2h2)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L3_out,&log->BL2h3)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L1_out,&log->BL5h1)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L2_out,&log->BL5h2)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L3_out,&log->BL5h3)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew1_out,&log->Zw1)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew2_out,&log->Zw2)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew3_out,&log->Zw3)) + return -1; + + if(set_bit_ptr_struct(args->io.nieobs_out,&log->nieobs)) + return -1; + + if(set_bit_ptr_struct(args->io.pob_bezwar_out,&log->pob_bezwar)) + return -1; + + if(set_float_ptr(args->io.di_l1_float_out,&log->Ir1)) + return -1; + if(set_float_ptr(args->io.di_l2_float_out,&log->Ir2)) + return -1; + if(set_float_ptr(args->io.di_l3_float_out,&log->Ir3)) + return -1; + + if(set_float_ptr(args->io.ih_l1_float_out,&log->Ih1)) + return -1; + if(set_float_ptr(args->io.ih_l2_float_out,&log->Ih2)) + return -1; + if(set_float_ptr(args->io.ih_l3_float_out,&log->Ih3)) + return -1; + if(set_float_ptr(args->io.i2h_l1_float_out,&log->I2h1)) + return -1; + if(set_float_ptr(args->io.i2h_l2_float_out,&log->I2h2)) + return -1; + if(set_float_ptr(args->io.i2h_l3_float_out,&log->I2h3)) + return -1; + if(set_float_ptr(args->io.i5h_l1_float_out,&log->I5h1)) + return -1; + if(set_float_ptr(args->io.i5h_l2_float_out,&log->I5h2)) + return -1; + if(set_float_ptr(args->io.i5h_l3_float_out,&log->I5h3)) + return -1; + + if(set_float_ptr(args->io.ir_l1_ch_float_out,&log->Ir_L1_ortb)) + return -1; + if(set_float_ptr(args->io.ir_l2_ch_float_out,&log->Ir_L2_ortb)) + return -1; + if(set_float_ptr(args->io.ir_l3_ch_float_out,&log->Ir_L3_ortb)) + return -1; + + log->an_struct_i1_l1.samples_buf=log->I1_l1; + log->an_struct_i1_l1.params=log->I1_l1_params; + log->an_struct_i1_l2.samples_buf=log->I1_l2; + log->an_struct_i1_l2.params=log->I1_l2_params; + log->an_struct_i1_l3.samples_buf=log->I1_l3; + log->an_struct_i1_l3.params=log->I1_l3_params; + + log->an_struct_i2_l1.samples_buf=log->I2_l1; + log->an_struct_i2_l1.params=log->I2_l1_params; + log->an_struct_i2_l2.samples_buf=log->I2_l2; + log->an_struct_i2_l2.params=log->I2_l2_params; + log->an_struct_i2_l3.samples_buf=log->I2_l3; + log->an_struct_i2_l3.params=log->I2_l3_params; + + log->an_struct_i3_l1.samples_buf=log->I3_l1; + log->an_struct_i3_l1.params=log->I3_l1_params; + log->an_struct_i3_l2.samples_buf=log->I3_l2; + log->an_struct_i3_l2.params=log->I3_l2_params; + log->an_struct_i3_l3.samples_buf=log->I3_l3; + log->an_struct_i3_l3.params=log->I3_l3_params; + + log->nast_.on = (args->params.bity & 0x0001)?1:0; + log->nast_.w = (args->params.bity & 0x0002)?1:0; + log->nast_.bl = (args->params.bity & 0x0004)?1:0; + log->nast_.bl2h = (args->params.bity & 0x0008)?1:0; + log->nast_.bl5h = (args->params.bity & 0x0010)?1:0; + log->nast_.sbc = (args->params.bity & 0x0020)?1:0; + log->nast_.h = (args->params.bity & 0x0040)?1:0; + log->nast_.r = (args->params.bity & 0x0080)?1:0; + log->nast_.cross_block = (args->params.bity & 0x0100)?1:0; + log->algorytm = (args->params.bity & 0x0200)?1:0; + + //ustawienie wartosci rozruchowych + log->nast_.khr_2h = args->params.k_2h * args->params.k_2h; + log->nast_.khp_2h = log->nast_.khr_2h * 0.98;//args->params.kp *args->params.kp; + log->nast_.khr_5h = args->params.k_5h * args->params.k_5h; + log->nast_.khp_5h = log->nast_.khr_5h * 0.98;//args->params.kp *args->params.kp; + log->nast_.Irr = args->params.Ir * 1.3577f; + log->Irr2 = args->params.Ir * args->params.Ir; + log->Irr2_p = log->Irr2 * args->params.kp; + log->nast_.Ipr = args->params.Ir * args->params.kp; + log->nast_.Ihdr = args->params.Ihd; + log->nast_.Irdr = args->params.rd; + log->nast_.kh1r = args->params.kh1; + log->nast_.kh1p = args->params.kh1 * args->params.kp; + log->nast_.kh2r = args->params.kh2; + log->nast_.kh2p = args->params.kh2 * args->params.kp; + log->nast_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ip3c = log->nast_.Ir3c * args->params.kp; + log->nast_.Iror = args->params.Iro; + log->nast_.Irop = args->params.Iro * args->params.kp; + log->nast_.Ir4 = args->params.Ihg * args->params.kh2 + args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ip4 = log->nast_.Ir4 * args->params.kp; + log->nast_.t_sbc = args->params.tbld * 1000; + log->nast_.Ih1 = args->params.Iro / args->params.kh1; + log->nast_.Ihp = args->params.Ihp; + log->nast_.Ihg = args->params.Ihg; + + log->nast_.tz = args->params.t * 1000; + +// log->nast_.ih_siemens = (args->params.Ih_typ == IH_SUM_IP) ? 1 : 0; +//-------- + + struct nast_trafo n1; + n1.GrP1 = args->params.GrP1; + n1.GrP2 = args->params.GrP2; + n1.GrP3 = args->params.GrP3; + n1.PT12 = args->params.PT12; + n1.PT13 = args->params.PT13; +// n1.Tr_zas = args->params.Tr_zas; + + przelicz_nastawy_bis(log,n1); + + return 0; +} + + + +/** +Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji +\param Ir wartoscs pradu roznicowego +\param Ih wartosc pradu stabilizacji +*/ +void kryterium2_nowe(struct diff_1_bis_logic *log,float Ir, float Ih, u8 *pob) +{ + + u16 n = 1; + + if (Ih > log->nast_.Ih1) n = 2; + if (Ih > log->nast_.Ihp) n = 3; + if ((Ih > log->nast_.Ihg) && log->nast_.h) n = 4; + *pob = 0; + *(pob + 1) = 0; + *(pob + 2) = 1; + switch (n) + { + case 1: + *pob = Ir > log->nast_.Iror; + *(pob + 1) = Ir < log->nast_.Irop; + break; + case 2: + *pob = Ir > Ih * log->nast_.kh1r; + *(pob + 1) = Ir < Ih * log->nast_.kh1p; + break; + case 3: + *pob = Ir > (Ih * log->nast_.kh2r + log->nast_.Ir3c); + *(pob + 1) = Ir < (Ih * log->nast_.kh2p + log->nast_.Ip3c); + break; + case 4: + *pob = Ir > log->nast_.Ir4; + *(pob + 1) = Ir < log->nast_.Ip4; + //*(pob + 2) = *(pob + 1); zakomentowane 11.04.20222 po bledach z blokada czasowa zgloszonymi przez NC + + break; + } +} + +/** +Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji +\param Ir wartosc pradu roznicowego +\param Ih wartosc pradu stabilizacji +*/ +void kryterium3_nowe(struct diff_1_bis_logic *log,float Ir, float Ih, u8 *pob) +{ + + *pob = 0; + *(pob + 1) = 0; + *(pob + 2) = 1; + + if ((Ih > log->nast_.Ihg) && log->nast_.h) + { + *pob = Ir > log->nast_.Ir4; + *(pob + 1) = Ir < log->nast_.Ip4; + *(pob + 2) = *(pob + 1); + } + else { + *pob = Ir > (log->nast_.Iror + log->nast_.kh1r2 * Ih); + *(pob + 1) = Ir < (log->nast_.Irop + log->nast_.kh1p2 * Ih); + } +} + +void przelicz_1h_ip_nowe(struct diff_1_bis_logic *log, uint16_t Lnr) +{ + int16_t i, i1,j; + float orta; + float ortb; + float pr; + + log->w.Ir[Lnr]=0; + + for(j=0;j<3;j++) + { + orta=0; + ortb=0; + i1 = log->w.nr_probki; + + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_ip[i1][((Lnr-3)*3)+j]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] += sqrtf(pr / 2); + } + + log->w.Ir[Lnr]/=2; + log->w.Ir[Lnr]*=log->w.Ir[Lnr]; +} + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void przelicz_1h_nowe(struct diff_1_bis_logic *log, uint16_t Lnr) +{ + int16_t i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + log->w.orta[Lnr] = orta; + log->w.ortb[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] = pr / 2; +} + + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void przelicz_2h_nowe(struct diff_1_bis_logic *log,uint16_t Lnr) +{ + + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin2h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos2h_[i]; + } + + log->w.orta2h[Lnr] = orta; + log->w.ortb2h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I2h[Lnr] = pr / 2; + +} + + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void przelicz_5h_nowe(struct diff_1_bis_logic *log,uint16_t Lnr) +{ + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); i++) + { + i1--; + if (i1 < 0) i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin5h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos5h_[i]; + } + + log->w.orta5h[Lnr] = orta; + log->w.ortb5h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I5h[Lnr] = pr / 2; +} + + + +void przelicz_1200Hz_1_nowe(struct diff_1_bis_logic *log) +{ + uint16_t i; + float local1, local2,b,c; + float i0[3]; + // wyliczenie pradu ronicowego i stabilizacji + float ich[3]; + + while (log->w.nr_probki != bus_an_cur_sample_num) + { + log->w.nr_probki++; + if (log->w.nr_probki > ((SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1)) log->w.nr_probki = 0; + + i0[0]=log->nast_.L[0].a_p[0][0] * ((float)log->nast_.L[0].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[0][1]) + + log->nast_.L[1].a_p[0][0] * ((float)log->nast_.L[1].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[0][1]) + + log->nast_.L[2].a_p[0][0] * ((float)log->nast_.L[2].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[0][1]); + + i0[1]=log->nast_.L[0].a_p[1][0] * ((float)log->nast_.L[0].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[1][1]) + + log->nast_.L[1].a_p[1][0] * ((float)log->nast_.L[1].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[1][1]) + + log->nast_.L[2].a_p[1][0] * ((float)log->nast_.L[2].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[1][1]); + + i0[2]=log->nast_.L[0].a_p[2][0] * ((float)log->nast_.L[0].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[0].a_p[2][1]) + + log->nast_.L[1].a_p[2][0] * ((float)log->nast_.L[1].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[1].a_p[2][1]) + + log->nast_.L[2].a_p[2][0] * ((float)log->nast_.L[2].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[2].a_p[2][1]); + + i0[0]/=3; + i0[1]/=3; + i0[2]/=3; + + for (i = 0; i < 3; i++) + { + + + local1 = log->nast_.L[i].a_p[0][0] * ((float)log->nast_.L[i].Ip[0]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[0][1]) + - log->nast_.L[i].a_m[0][0] * ((float)log->nast_.L[i].Im[0]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[0][1]); + + b = log->nast_.L[i].a_p[1][0] * ((float)log->nast_.L[i].Ip[1]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[1][1]) + - log->nast_.L[i].a_m[1][0] * ((float)log->nast_.L[i].Im[1]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[1][1]); + + c = log->nast_.L[i].a_p[2][0] * ((float)log->nast_.L[i].Ip[2]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[2][1]) + - log->nast_.L[i].a_m[2][0] * ((float)log->nast_.L[i].Im[2]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[2][1]); + + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + local1-=i0[0]; + if(!log->nast_.p2d) + b-=i0[1]; + if(!log->nast_.p3d) + c-=i0[2]; + } + + local2 = b + c + + log->nast_.L[i].a_p[3][0] * ((float)log->nast_.L[i].Ip[3]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_p[3][1]) + - log->nast_.L[i].a_m[3][0] * ((float)log->nast_.L[i].Im[3]->samples_buf[log->w.nr_probki] - log->nast_.L[i].a_m[3][1]); + + //log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2; + log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2; //zmienino 09.11.2020 + log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) / 2; + ich[i] = fabs(log->w.buf_Lr[log->w.nr_probki][0 + i]); + + log->w.buf_ip[log->w.nr_probki][i*3 + 0] = local1; + log->w.buf_ip[log->w.nr_probki][i*3 + 1] = b; + log->w.buf_ip[log->w.nr_probki][i*3 + 2] = c; + } + + + if (log->nast_.r) // jesli wlaczone wylaczenie bezwarunkowe + { + if (log->algorytm == 0) + { + log->P_.Irr1 = kryterium1(&log->l.Ir1[0], + (ich[0] > log->nast_.Irr), + (ich[0]nast_.Ipr), + log->P_.Irr1, + c1r, c1p); + log->P_.Irr2 = kryterium1(&log->l.Ir2[0], + (ich[1]>log->nast_.Irr), + (ich[1]nast_.Ipr), + log->P_.Irr2, + c1r, c1p); + log->P_.Irr3 = kryterium1(&log->l.Ir3[0], + (ich[2]>log->nast_.Irr), + (ich[2] < log->nast_.Ipr), + log->P_.Irr3, + c1r, c1p); + } + else + { + log->P_.Irr1 = kryterium1(&log->l.Ir1[0], + (log->w.Ir[0] >log->Irr2), + (log->w.Ir[0]Irr2_p), + log->P_.Irr1, + c1r_nowe, c1p_nowe); + log->P_.Irr2 = kryterium1(&log->l.Ir2[0], + (log->w.Ir[1]>log->Irr2), + (log->w.Ir[1]Irr2_p), + log->P_.Irr2, + c1r_nowe, c1p_nowe); + log->P_.Irr3 = kryterium1(&log->l.Ir3[0], + (log->w.Ir[2]>log->Irr2), + (log->w.Ir[2]Irr2_p), + log->P_.Irr3, + c1r_nowe, c1p_nowe); + } + } + else log->P_.Irr1 = log->P_.Irr2 = log->P_.Irr3 = 0; + + check_and_set_struct((log->P_.Irr1 ||log->P_.Irr2 || log->P_.Irr3),&log->pob_bezwar); + + + } + + //---------------------------------------------------------------------------------------- + + //filtracja przebiegow + + + // 1h pradu rozruchowego + + + przelicz_1h_nowe(log,0); + przelicz_1h_nowe(log,1); + przelicz_1h_nowe(log,2); + + przelicz_1h_ip_nowe(log,3); + przelicz_1h_ip_nowe(log,4); + przelicz_1h_ip_nowe(log,5); + + //----------------------------------------------------------------------------------------- + + u8 war1 = log->w.Ir[0] > 0.0016; + u8 war2 = log->w.Ir[1] > 0.0016; + u8 war3 = log->w.Ir[2] > 0.0016; + + + + log->Ir1_ = sqrtf(log->w.Ir[0]); + log->Ir2_ = sqrtf(log->w.Ir[1]); + log->Ir3_ = sqrtf(log->w.Ir[2]); + log->Ih1_ = sqrtf(log->w.Ir[3]); + log->Ih2_ = sqrtf(log->w.Ir[4]); + log->Ih3_ = sqrtf(log->w.Ir[5]); + + if (log->nast_.bl2h) //jesli aktywna blokada od drugiej harmonicznej + { + + // druga harmoniczna + przelicz_2h_nowe(log,0); + przelicz_2h_nowe(log,1); + przelicz_2h_nowe(log,2); + + + log->P_.Bl2h_L1 = kryterium1(&log->l.l2h1[0], + ((log->w.I2h[0] > log->nast_.khr_2h*log->w.Ir[0]) && war1), + ((log->w.I2h[0]nast_.khp_2h*log->w.Ir[0]) || !war1), + log->P_.Bl2h_L1, + c2r, c2p); + log->P_.Bl2h_L2 = kryterium1(&log->l.l2h2[0], + ((log->w.I2h[1]>log->nast_.khr_2h*log->w.Ir[1]) && war2), + ((log->w.I2h[1]nast_.khp_2h*log->w.Ir[1]) || !war2), + log->P_.Bl2h_L2, + c2r, c2p); + log->P_.Bl2h_L3 = kryterium1(&log->l.l2h3[0], + ((log->w.I2h[2]>log->nast_.khr_2h*log->w.Ir[2]) && war3), + ((log->w.I2h[2]nast_.khp_2h*log->w.Ir[2]) || !war3), + log->P_.Bl2h_L3, + c2r, c2p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl2h_L1 || log->P_.Bl2h_L2 || log->P_.Bl2h_L3) + { + log->P_.Bl2h_L1 = 1; + log->P_.Bl2h_L2 = 1; + log->P_.Bl2h_L3 = 1; + } + else + { + log->P_.Bl2h_L1 = 0; + log->P_.Bl2h_L2 = 0; + log->P_.Bl2h_L3 = 0; + } + } + } + else log->P_.Bl2h_L1 = log->P_.Bl2h_L2 = log->P_.Bl2h_L3 = 0; + + if (log->nast_.bl5h) //jesli aktywna blokada od piatej harmonicznej + { + // piata harmoniczna + przelicz_5h_nowe(log,0); + przelicz_5h_nowe(log,1); + przelicz_5h_nowe(log,2); + + log->P_.Bl5h_L1 = kryterium1(&log->l.l5h1[0], + ((log->w.I5h[0]>log->nast_.khr_5h*log->w.Ir[0]) && war1), + ((log->w.I5h[0]nast_.khp_5h*log->w.Ir[0]) || !war1), + log->P_.Bl5h_L1, + c3r, c3p); + log->P_.Bl5h_L2 = kryterium1(&log->l.l5h2[0], + ((log->w.I5h[1]>log->nast_.khr_5h*log->w.Ir[1]) && war2), + ((log->w.I5h[1]nast_.khp_5h*log->w.Ir[1]) || !war2), + log->P_.Bl5h_L2, + c3r, c3p); + log->P_.Bl5h_L3 = kryterium1(&log->l.l5h3[0], + ((log->w.I5h[2]>log->nast_.khr_5h*log->w.Ir[2]) && war3), + ((log->w.I5h[2] < log->nast_.khp_5h*log->w.Ir[2]) || !war3), + log->P_.Bl5h_L3, + c3r, c3p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl5h_L1 || log->P_.Bl5h_L2 || log->P_.Bl5h_L3) + { + log->P_.Bl5h_L1 = 1; + log->P_.Bl5h_L2 = 1; + log->P_.Bl5h_L3 = 1; + } + else + { + log->P_.Bl5h_L1 = 0; + log->P_.Bl5h_L2 = 0; + log->P_.Bl5h_L3 = 0; + } + } + + } + else log->P_.Bl5h_L1 = log->P_.Bl5h_L2 = log->P_.Bl5h_L3 = 0; +} + +void przelicz_1200Hz_2_nowe(struct diff_1_bis_logic *log) +{ + if (log->nast_.sbc) //jesli wwlaczona czasowa blokada dzialania + { + log->dw.sbc_P = (log->Ih1_ > log->nast_.Ihdr) && (log->Ir1_ < (log->Ih1_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw1 && kryterium1(&log->l.sbc1[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw1), + c4r, log->nast_.t_sbc), + &log->Zw1); + + log->dw.sbc_P = (log->Ih2_ > log->nast_.Ihdr) && (log->Ir2_ < (log->Ih2_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw2 && kryterium1(&log->l.sbc2[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw2), + c4r, log->nast_.t_sbc), + &log->Zw2); + + log->dw.sbc_P = (log->Ih3_ > log->nast_.Ihdr) && (log->Ir3_ < (log->Ih3_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw3 && kryterium1(&log->l.sbc3[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw3), + c4r, log->nast_.t_sbc), + &log->Zw3); + } + else + { + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + } + + log->P_.Psf1 = kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, c5r, c5p); + log->P_.Psf2 = kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, c5r, c5p); + log->P_.Psf3 = kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, c5r, c5p); + + //--------------------------------------------------------- + + //ustawienie wyjcia + + check_and_set_struct(log->P_.Irr1 || //pobudzenie bezwarunkowe + (log->P_.Psf1 && //prad w strefie dzialania + !check_struct(&log->Zw1) && //brak blokady czasowej + !log->P_.Bl2h_L1 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L1),//brak blokady od piatej harmonicznej) + &log->Pf1); + + check_and_set_struct(log->P_.Irr2 || //pobudzenie bezwarunkowe + (log->P_.Psf2 && //prad w strefie dzialania + !check_struct(&log->Zw2) && //brak blokady czasowej + !log->P_.Bl2h_L2 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L2),//brak blokady od piatej harmonicznej + &log->Pf2); + + check_and_set_struct(log->P_.Irr3 || //pobudzenie bezwarunkowe + (log->P_.Psf3 && //prad w strefie dzialania + !check_struct(&log->Zw3) && //brak blokady czasowej + !log->P_.Bl2h_L3 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L3),//brak blokady od piatej harmonicznej + &log->Pf3); + +//------------------- + + check_and_set_struct(check_struct(&log->Pf1)||check_struct(&log->Pf2)||check_struct(&log->Pf3),&log->P); + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + check_and_set_struct(check_struct(&log->test),&log->P); + check_and_set_struct(check_struct(&log->test),&log->Pf1); + check_and_set_struct(check_struct(&log->test),&log->Pf2); + check_and_set_struct(check_struct(&log->test),&log->Pf3); + } + + // realizacja blokady dzialania przekaznika + + if (((log->nast_.bl && check_struct(&log->stan_bl))||check_struct(&log->nieobs))/* || czy_start()*/) + { + clear_struct(&log->P); + clear_struct(&log->Pf1); + clear_struct(&log->Pf2); + clear_struct(&log->Pf3); + clear_struct(&log->pob_bezwar); + } + // + + // realizacja przekaznika czasowego + if (check_struct(&log->P)) + log->dw.licz_t+=LOOP_CYCLE_MS; + else + log->dw.licz_t = 0; // odliczanie czasu dzialania + + if (log->dw.licz_t > log->nast_.tz) + { // sprawdzenie przekroczenia nastawionego czasu + //if (!check_struct(&log->Z)) log->dw.czas_ = time; + set_struct(&log->Z); // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t-=LOOP_CYCLE_MS; + } + else clear_struct(&log->Z); + // + + check_and_set_struct(check_struct(&log->Z) && log->nast_.w,&log->W); //wyprowadzenie wylaczenia zaleznie od nastawy + + +//wystawienie pomiarow + + if(!check_struct(&log->nieobs)) + { + *log->Ir1 = log->w.Ir[0]; + *log->Ir2 = log->w.Ir[1]; + *log->Ir3 = log->w.Ir[2]; + *log->Ih1 = log->w.Ir[3]; + *log->Ih2 = log->w.Ir[4]; + *log->Ih3 = log->w.Ir[5]; + *log->Ir_L1_ortb = log->w.buf_Lr[log->w.nr_probki][0]; + *log->Ir_L2_ortb = log->w.buf_Lr[log->w.nr_probki][1]; + *log->Ir_L3_ortb = log->w.buf_Lr[log->w.nr_probki][2]; + *log->I2h1 = log->w.I2h[0]; + *log->I2h2 = log->w.I2h[1]; + *log->I2h3 = log->w.I2h[2]; + *log->I5h1 = log->w.I5h[0]; + *log->I5h2 = log->w.I5h[1]; + *log->I5h3 = log->w.I5h[2]; + } + else + { + *log->Ir1 = 0; + *log->Ir2 = 0; + *log->Ir3 = 0; + *log->Ih1 = 0; + *log->Ih2 = 0; + *log->Ih3 = 0; + *log->Ir_L1_ortb = 0; + *log->Ir_L2_ortb = 0; + *log->Ir_L3_ortb = 0; + *log->I2h1 = 0; + *log->I2h2 = 0; + *log->I2h3 = 0; + *log->I5h1 = 0; + *log->I5h2 = 0; + *log->I5h3 = 0; + } + +//---------------------------------------------- + check_and_set_struct(log->P_.Bl2h_L1 && log->P_.Psf1,&log->BL2h1); + check_and_set_struct(log->P_.Bl2h_L2 && log->P_.Psf2,&log->BL2h2); + check_and_set_struct(log->P_.Bl2h_L3 && log->P_.Psf3,&log->BL2h3); + check_and_set_struct(log->P_.Bl5h_L1 && log->P_.Psf1,&log->BL5h1); + check_and_set_struct(log->P_.Bl5h_L2 && log->P_.Psf2,&log->BL5h2); + check_and_set_struct(log->P_.Bl5h_L3 && log->P_.Psf3,&log->BL5h3); +} + + +void diff_1_bis(void *arguments, void *logic) +{ + struct diff_1_bis_logic *log = (struct diff_1_bis_logic *)logic; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + przelicz_1200Hz_1_nowe(log); + + //Sprawdzenie pobudzenia w stabilizowanej strefie dzialania + kryterium2_nowe(log,log->Ir1_,log->Ih1_,&log->P_.Pb1); + kryterium2_nowe(log,log->Ir2_,log->Ih2_,&log->P_.Pb2); + kryterium2_nowe(log,log->Ir3_,log->Ih3_,&log->P_.Pb3); + + przelicz_1200Hz_2_nowe(log); + } else { + *log->Ir1=0; + *log->Ir2=0; + *log->Ir3=0; + *log->Ih1=0; + *log->Ih2=0; + *log->Ih3=0; + *log->Ir_L1_ortb=0; + *log->Ir_L2_ortb=0; + *log->Ir_L3_ortb=0; + // *log->Ih_L1_ortb=0; + //*log->Ih_L2_ortb=0; + //*log->Ih_L3_ortb=0; + *log->I2h1=0; + *log->I2h2=0; + *log->I2h3=0; + *log->I5h1=0; + *log->I5h2=0; + *log->I5h3=0; + clear_struct(&log->P); + clear_struct(&log->Pf1); + clear_struct(&log->Pf2); + clear_struct(&log->Pf3); + clear_struct(&log->W); + clear_struct(&log->Z); + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + clear_struct(&log->BL2h1); + clear_struct(&log->BL2h2); + clear_struct(&log->BL2h3); + clear_struct(&log->BL5h1); + clear_struct(&log->BL5h2); + clear_struct(&log->BL5h3); + clear_struct(&log->pob_bezwar); + } +} diff --git a/src/Diff_1_bis.h b/src/Diff_1_bis.h new file mode 100644 index 0000000..9930016 --- /dev/null +++ b/src/Diff_1_bis.h @@ -0,0 +1,249 @@ +/* + * Diff_1.h + * + * Created on: 01-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DIFF_1_BIS_H_ +#define DIFF_1_BIS_H_ + +#include "Diff.h" + +//#include "pawel_usun_to.h" + + +#include "tdefs.h" + + +extern u16 pY_ZPrAE[25]; +extern u16 pD_ZPrAE[25]; +extern struct kom1_zprae Y2w_ZPrAE; +extern struct kom2_zprae Y3w_ZPrAE; +extern struct kom1_zprae D2w_ZPrAE; +extern struct kom1_zprae D3w_ZPrAE; + +#define c1r 5 //ilosc powtorzen zadzialania bezwarunkowego +#define c1p 30 //ilosc powtorzen odpadu bezwarunkowego +#define c2r 2 //ilosc powtorzen zadzialania blokady 2h +#define c2p 7 //ilosc powtorzen odpadu blokady 2h +#define c3r 2 //ilosc powtorzen zadzialania blokady 5h +#define c3p 10 //ilosc powtorzen odpadu blokady 5h +#define c4r 5 //ilosc powtorzen zadzialania czasowej blokady +#define c5r 20 //ilosc powtorzen zadzialania stabilizowanej strefy dzialania !!!!!!!!Pawel bylo 2 +#define c5p 2 //ilosc powtorzen odpadu stabilizowanej strefy dzialania + +struct diff_1_bis_io +{ + u32 stan_bl_in; + u32 ia_l1_ptr_in; + u32 ia_l1_params_ptr_in; + u32 ia_l2_ptr_in; + u32 ia_l2_params_ptr_in; + u32 ia_l3_ptr_in; + u32 ia_l3_params_ptr_in; + u32 ib_l1_ptr_in; + u32 ib_l1_params_ptr_in; + u32 ib_l2_ptr_in; + u32 ib_l2_params_ptr_in; + u32 ib_l3_ptr_in; + u32 ib_l3_params_ptr_in; + u32 ic_l1_ptr_in; + u32 ic_l1_params_ptr_in; + u32 ic_l2_ptr_in; + u32 ic_l2_params_ptr_in; + u32 ic_l3_ptr_in; + u32 ic_l3_params_ptr_in; + + /* + u32 id_l1_ptr_in; + u32 id_l1_params_ptr_in; + u32 id_l2_ptr_in; + u32 id_l2_params_ptr_in; + u32 id_l3_ptr_in; + u32 id_l3_params_ptr_in; + */ + + u32 test_in; + + u32 w_out; + u32 z_out; + u32 p_out; + u32 pl1_out; + u32 pl2_out; + u32 pl3_out; + u32 bl_2h_L1_out; + u32 bl_2h_L2_out; + u32 bl_2h_L3_out; + u32 bl_5h_L1_out; + u32 bl_5h_L2_out; + u32 bl_5h_L3_out; + u32 zzew1_out; + u32 zzew2_out; + u32 zzew3_out; + u32 di_l1_float_out; + u32 di_l2_float_out; + u32 di_l3_float_out; + u32 ih_l1_float_out; + u32 ih_l2_float_out; + u32 ih_l3_float_out; + u32 i2h_l1_float_out; + u32 i2h_l2_float_out; + u32 i2h_l3_float_out; + u32 i5h_l1_float_out; + u32 i5h_l2_float_out; + u32 i5h_l3_float_out; + u32 ir_l1_ch_float_out; + u32 ir_l2_ch_float_out; + u32 ir_l3_ch_float_out; + u32 nieobs_out; + u32 pob_bezwar_out; +// u32 ih_l1_ch_float_out; +// u32 ih_l2_ch_float_out; +// u32 ih_l3_ch_float_out; +}__attribute__((__packed__)); + +struct diff_1_bis_logic +{ + struct binary_io stan_bl; + u16 *I1_l1; + struct analog_in_params_extended *I1_l1_params; + u16 *I1_l2; + struct analog_in_params_extended *I1_l2_params; + u16 *I1_l3; + struct analog_in_params_extended *I1_l3_params; + u16 *I2_l1; + struct analog_in_params_extended *I2_l1_params; + u16 *I2_l2; + struct analog_in_params_extended *I2_l2_params; + u16 *I2_l3; + struct analog_in_params_extended *I2_l3_params; + u16 *I3_l1; + struct analog_in_params_extended *I3_l1_params; + u16 *I3_l2; + struct analog_in_params_extended *I3_l2_params; + u16 *I3_l3; + struct analog_in_params_extended *I3_l3_params; + /* + u16 *I4_l1; + struct analog_in_params_extended *I4_l1_params; + u16 *I4_l2; + struct analog_in_params_extended *I4_l2_params; + u16 *I4_l3; + struct analog_in_params_extended *I4_l3_params; + */ + struct binary_io test; + + struct binary_io W; + struct binary_io Z; + struct binary_io P; + struct binary_io Pf1; + struct binary_io Pf2; + struct binary_io Pf3; + struct binary_io BL2h1; + struct binary_io BL2h2; + struct binary_io BL2h3; + struct binary_io BL5h1; + struct binary_io BL5h2; + struct binary_io BL5h3; + struct binary_io Zw1; + struct binary_io Zw2; + struct binary_io Zw3; + struct binary_io nieobs; + struct binary_io pob_bezwar; + float *Ir1; + float *Ir2; + float *Ir3; + float *Ih1; + float *Ih2; + float *Ih3; + float *I2h1; + float *I2h2; + float *I2h3; + float *I5h1; + float *I5h2; + float *I5h3; + float *Ir_L1_ortb; + float *Ir_L2_ortb; + float *Ir_L3_ortb; +// float *Ih_L1_ortb; +// float *Ih_L2_ortb; +// float *Ih_L3_ortb; + + enum jaki_uklad jaki_uklad; + struct dane_wewnetrzne_Diff dw; + struct licz l; + struct pobudzenia_czastkowe P_; + struct wejscie_wew w; + struct Nastawy_przeliczone nast_; // struktura z parametrami rozruchowymi + + float Irr2; + float Irr2_p; + float Ir1_; + float Ir2_; + float Ir3_; + float Ih1_; + float Ih2_; + float Ih3_; + + u8 algorytm; + + struct we_an_diff an_struct_i1_l1; + struct we_an_diff an_struct_i1_l2; + struct we_an_diff an_struct_i1_l3; + struct we_an_diff an_struct_i2_l1; + struct we_an_diff an_struct_i2_l2; + struct we_an_diff an_struct_i2_l3; + struct we_an_diff an_struct_i3_l1; + struct we_an_diff an_struct_i3_l2; + struct we_an_diff an_struct_i3_l3; + struct we_an_diff an_struct_i4_l1; + struct we_an_diff an_struct_i4_l2; + struct we_an_diff an_struct_i4_l3; + +// float mul_I1[3]; +// float mul_I2[3]; +// float mul_I3[3]; + // struct Nastawy_przeliczone_Diff_1 nast_; +}; + + +struct diff_1_bis_params +{ + u32 bity; ///< nastawy bitowe; + +// u32 Tr_zas;/// + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "Diff.h" +#include "Diff_2.h" + +int diff_2_initlog(void *arguments, void *logic) +{ + struct diff_2_args *args = (struct diff_2_args *)arguments; + struct diff_logic *log = (struct diff_logic *)logic; + + if(diff_init_io(&args->io,log)) + return -1; + + log->nast_.on = (args->params.bity & 0x0001)?1:0; + log->nast_.w = (args->params.bity & 0x0002)?1:0; + log->nast_.bl = (args->params.bity & 0x0004)?1:0; + log->nast_.bl2h = (args->params.bity & 0x0008)?1:0; + log->nast_.bl5h = (args->params.bity & 0x0010)?1:0; + log->nast_.sbc = (args->params.bity & 0x0020)?1:0; + log->nast_.h = (args->params.bity & 0x0040)?1:0; + log->nast_.r = (args->params.bity & 0x0080)?1:0; + + //ustawienie wartosci rozruchowych + log->nast_.khr_2h = args->params.k_2h * args->params.k_2h; + log->nast_.khp_2h = log->nast_.khr_2h * args->params.kp *args->params.kp; + log->nast_.khr_5h = args->params.k_5h * args->params.k_5h; + log->nast_.khp_5h = log->nast_.khr_5h * args->params.kp *args->params.kp; + log->nast_.Irr = args->params.Ir * 1.3577f; + log->nast_.Ipr = args->params.Ir * args->params.kp; + log->nast_.Ihdr = args->params.Ihd; + log->nast_.Irdr = args->params.rd; + log->nast_.kh1r = args->params.kh1; + log->nast_.kh1p = log->nast_.kh1r * args->params.kp; + + log->nast_.kh1r2 = args->params.kh1 * args->params.kh1; + log->nast_.kh1p2 = log->nast_.kh1r2 * args->params.kp * args->params.kp; + log->nast_.Ir4 = args->params.Ihg * args->params.kh1 * args->params.Ihg * args->params.kh1 + args->params.Iro * args->params.Iro; + log->nast_.Ip4 = log->nast_.Ir4 * args->params.kp * args->params.kp; + log->nast_.Iror = args->params.Iro * args->params.Iro; + log->nast_.Irop = log->nast_.Iror * args->params.kp * args->params.kp; + log->nast_.t_sbc = args->params.tbld * 1000; + log->nast_.Ihg = args->params.Ihg * args->params.Ihg; + + log->nast_.tz = args->params.t * 1000; + + + //-------- + + struct nast_trafo n1; + n1.GrP1 = args->params.GrP1; + n1.GrP2 = args->params.GrP2; + n1.GrP3 = args->params.GrP3; + n1.PT12 = args->params.PT12; + n1.PT13 = args->params.PT13; + //n1.Tr_zas = args->params.Tr_zas; + + przelicz_nastawy1(log,n1); + + return 0; +} + +void diff_2(void *arguments, void *logic) +{ + struct diff_logic *log = (struct diff_logic *)logic; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + przelicz_1200Hz_1(log); + + //Sprawdzenie pobudzenia w stabilizowanej strefie dzialania + + + kryterium3(log,log->Ir1_,log->Ih1_,&log->P_.Pb1); + kryterium3(log,log->Ir2_,log->Ih2_,&log->P_.Pb2); + kryterium3(log,log->Ir3_,log->Ih3_,&log->P_.Pb3); + + przelicz_1200Hz_2(log); + } else { + *log->Ir1=0; + *log->Ir2=0; + *log->Ir3=0; + *log->Ih1=0; + *log->Ih2=0; + *log->Ih3=0; + *log->Ir_L1_ortb=0; + *log->Ir_L2_ortb=0; + *log->Ir_L3_ortb=0; + //*log->Ih_L1_ortb=0; + //*log->Ih_L2_ortb=0; + //*log->Ih_L3_ortb=0; + *log->I2h1=0; + *log->I2h2=0; + *log->I2h3=0; + *log->I5h1=0; + *log->I5h2=0; + *log->I5h3=0; + clear_struct(&log->P); + clear_struct(&log->Pf1); + clear_struct(&log->Pf2); + clear_struct(&log->Pf3); + clear_struct(&log->W); + clear_struct(&log->Z); + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + clear_struct(&log->BL2h1); + clear_struct(&log->BL2h2); + clear_struct(&log->BL2h3); + clear_struct(&log->BL5h1); + clear_struct(&log->BL5h2); + clear_struct(&log->BL5h3); + } +} diff --git a/src/Diff_2.h b/src/Diff_2.h new file mode 100644 index 0000000..2da3d65 --- /dev/null +++ b/src/Diff_2.h @@ -0,0 +1,49 @@ +/* + * Diff_2.h + * + * Created on: 08-05-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DIFF_2_H_ +#define DIFF_2_H_ + +#include "Diff.h" + +#include "tdefs.h" + +struct diff_2_params +{ + u32 bity; ///< nastawy bitowe; + + u32 Tr_zas;/// + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "LMZ.h" + + +int LMZ_initlog(void *arguments, void *logic) +{ + struct LMZ_args *args = (struct LMZ_args *)arguments; + struct LMZ_logic *log = (struct LMZ_logic *)logic; + struct analog_in_params *pwi,*pwu,*pwio2; + + if(set_pointer_in_ptr(args->io.we_zdist_ptr_in,(u32 *)&log->nast_.WYA)) + return -1; + + if(set_bit_ptr_struct(args->io.we_zdistl_p1_in,&log->WE_ZDistL_Pzf[0])) + return -1; + if(set_bit_ptr_struct(args->io.we_zdistl_p1w_in,&log->WE_ZDistL_Pzf[1])) + return -1; + + if(set_float_ptr(args->io.ioii_orta_float_in,&log->IoII_orta)) + return -1; + if(set_float_ptr(args->io.ioii_ortb_float_in,&log->IoII_ortb)) + return -1; + if(set_pointer_in_ptr(args->io.ioii_params_ptr_in,(u32 *)&log->IoII_params)) + return -1; + + if(set_bit_ptr_struct(args->io.clr_in,&log->clr)) + return -1; + if(set_bit_ptr_struct(args->io.p_out,&log->P)) + return -1; + if(set_float_ptr(args->io.distance_float_out,&log->distance)) + return -1; + + #ifdef LMZ_DEBUG + if(set_float_ptr(args->io.debug_out,&log->debug)) + return -1; + if(set_float_ptr(args->io.debug1_out,&log->debug1)) + return -1; + if(set_float_ptr(args->io.debug2_out,&log->debug2)) + return -1; + if(set_float_ptr(args->io.debug3_out,&log->debug3)) + return -1; + if(set_float_ptr(args->io.debug4_out,&log->debug4)) + return -1; + #endif + log->nast_.on_ = (args->params.bity & 0x0001) ? 1 : 0; + log->nast_.L2t = (args->params.bity & 0x0002) ? 1 : 0; + + + pwu = log->nast_.WYA->Zdist_dw->log_ptr->param_U; + pwi = log->nast_.WYA->Zdist_dw->log_ptr->param_I; + pwio2 = log->IoII_params; + + + log->nast_.a.r = args->params.R1 * pwi->znam_pierw / pwu->znam_pierw; + log->nast_.a.i = -args->params.X1 * pwi->znam_pierw / pwu->znam_pierw; + log->nast_.b.r = (args->params.R0 - args->params.R1) * pwi->znam_pierw / pwu->znam_pierw; + log->nast_.b.i = -(args->params.X0 - args->params.X1) * pwi->znam_pierw / pwu->znam_pierw; + log->nast_.c.r = args->params.Rm * pwio2->znam_pierw / pwu->znam_pierw; + log->nast_.c.i = -args->params.Xm * pwio2->znam_pierw / pwu->znam_pierw; + + log->nast_.kziem.r = ((args->params.R0 / args->params.R1)-1)/3; + log->nast_.kziem.i = -((args->params.X0 / args->params.X1)-1)/3; + + if(log->nast_.L2t==0) + { + log->nast_.c.r = 0; + log->nast_.c.i = 0; + } + + + return 0; +} + +/** +Wylicza iloczyn dwoch liczb zespolonych +*/ +struct zespolona p_z(struct zespolona *a, + struct zespolona *b) +{ + struct zespolona result; + result.r = a->r * b->r - a->i * b->i; + result.i = a->r * b->i + a->i * b->r; + return(result); +} + +/** +Wylicza iloczyn dwoch liczb zespolonych +*/ +struct zespolona dziel(struct zespolona *a, + struct zespolona *b) +{ + struct zespolona result; + result.r = ((a->r * b->r) + (a->i * b->i))/((b->r * b->r) +(b->i * b->i)); + result.i = ((a->i * b->r) - (a->r * b->i))/((b->r * b->r) +(b->i * b->i)); + return(result); +} + +/** +Wylicza dane dla odlegoci w ptli jednofazowej +*/ +float +wylicz_dane_1f(struct LMZ_logic *log, + u16 nr_pr, //indeks pooenia wektora wyliczanych danych w buforze + u16 nr_I //numer prdu wejciowego +) +{ + struct zespolona temp; + struct zespolona loc1,loc2,loc3; + struct zespolona *i1,*ip; + float loc4; + + struct zespolona imped; + struct zespolona k3io; + + + i1 = &log->dw.dp[nr_pr].IL1 + nr_I; + ip = &log->dw.Im[nr_pr % (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)].IL1 + nr_I; + + loc4 = (float)1.0 / (i1->r * i1->r + i1->i * i1->i); + loc1.r = i1->r * loc4; + loc1.i = (-i1->i) * loc4; + + loc2 = p_z(&log->dw.dp[nr_pr].Io,&log->nast_.b); + loc3 = p_z(&log->dw.dp[nr_pr].Io2,&log->nast_.c); + loc2.r += loc3.r; + loc2.i += loc3.i; + loc3 = p_z(&loc2,&loc1); + log->dw.zz.r = log->nast_.a.r - loc3.r; + log->dw.zz.i = log->nast_.a.i - loc3.i; + + log->dw.zi = *i1; + + log->dw.zibis.r = i1->r - ip->r; + log->dw.zibis.i = i1->i - ip->i; + + float ignac = 1; + + temp = p_z(&log->dw.zz,&log->dw.zi); + + float wynik = ( ignac * (log->dw.zu.i * log->dw.zibis.r - log->dw.zu.r * log->dw.zibis.i ) + / (temp.i * log->dw.zibis.r - temp.r * log->dw.zibis.i )); + + return wynik; + +} + + +/** +Wylicza dane dla odlegoci w ptli midzyfazowej +*/ +float wylicz_dane_mf(struct LMZ_logic *log, + u16 nr_pr, //indeks pooenia wektora wyliczanych danych w buforze + u16 nr_I1, //numer prdu wejciowego 1 + u16 nr_I2 //numer prdu wejciowego 2 +) +{ + struct zespolona temp; + struct zespolona *i1,*ip1; + struct zespolona *i2,*ip2; + + i1 = &log->dw.dp[nr_pr].IL1 + nr_I1; + ip1 = &log->dw.Im[nr_pr % (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)].IL1 + nr_I1; + i2 = &log->dw.dp[nr_pr].IL1 + nr_I2; + ip2 = &log->dw.Im[nr_pr % (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)].IL1 + nr_I2; + + log->dw.zz = log->nast_.a; + + log->dw.zi.r = i1->r - i2->r; + log->dw.zi.i = i1->i - i2->i; + + log->dw.zibis.r = i1->r - i2->r - ip1->r + ip2->r; + log->dw.zibis.i = i1->i - i2->i - ip1->i + ip2->i; + + temp = p_z(&log->dw.zz,&log->dw.zi); + + return( (log->dw.zu.i * log->dw.zibis.r - log->dw.zu.r * log->dw.zibis.i ) + / (temp.i * log->dw.zibis.r - temp.r * log->dw.zibis.i )); + +} + +void zapisz_p(struct LMZ_logic *log) +{ + float *wsk; + int i; + + log->dw.wsk_buf++; + + for(i=0;i<6;i++) + { + log->dw.P[log->dw.wsk_buf][i] = (log->nast_.WYA->Zdist_dw->P[0][i] || log->nast_.WYA->Zdist_dw->P[1][i])?1:0; + } + + wsk = &log->dw.dp[log->dw.wsk_buf].IL1.r; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_ortb; + wsk++; + *wsk = -*log->nast_.WYA->Zdist_dw->log_ptr->sI0_orta;//poprawiono + wsk++; + *wsk = -*log->nast_.WYA->Zdist_dw->log_ptr->sI0_ortb;//poprawiono + wsk++; + *wsk = *log->IoII_orta; + wsk++; + *wsk = *log->IoII_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U1_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U1_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U2_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U2_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U3_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U3_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U12_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U12_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U23_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U23_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U31_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->U31_ortb; +} + +void zapisz_pre(struct LMZ_logic *log) +{ + float *wsk; + + wsk = &log->dw.Ip[log->dw.wsk_buf_pre].IL1.r; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I1_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I2_ortb; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_orta; + wsk++; + *wsk = *log->nast_.WYA->Zdist_dw->log_ptr->I3_ortb; + + log->dw.wsk_buf_pre++; + if(log->dw.wsk_buf_pre>MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-1) log->dw.wsk_buf_pre = 0; +} + + +void LMZ(void *arguments, void *logic) +{ + struct LMZ_logic *log = (struct LMZ_logic *)logic; + u16 local; + int k; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + zapisz_pre(log); + switch(log->status_rej_LMZ) + { + case oczekiwanie: + + if(check_struct(&log->WE_ZDistL_Pzf[0]) || check_struct(&log->WE_ZDistL_Pzf[1])) + { + log->status_rej_LMZ = sledzenie;// stan_nieustalony; + log->dw.wsk_buf = 0; //dodane + zapisz_p(log);//dodane + + //dw.czas_ = time; + log->dw.licz1 = 10;//20; + + if (log->dw.wsk_buf_pre == 0) + local = MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-2; + else if (log->dw.wsk_buf_pre == 1) + local = MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-1; + else + local = log->dw.wsk_buf_pre - 2 ; + + + for(k=0;kMAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3-1) local -= MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3; + log->dw.Im[k] = log->dw.Ip[local]; + local++; + } + } + break; + + case stan_nieustalony: + if(log->dw.licz1>LOOP_CYCLE_MS) + log->dw.licz1-=LOOP_CYCLE_MS; + else + log->dw.licz1=0; + + if(!log->dw.licz1) + { + if(check_struct(&log->WE_ZDistL_Pzf[0]) || check_struct(&log->WE_ZDistL_Pzf[1])) + { + log->status_rej_LMZ = sledzenie; + log->dw.wsk_buf = 0; + zapisz_p(log); + log->dw.licz1 = 10; + } else { + log->status_rej_LMZ = oczekiwanie; + } + } + break; + + case sledzenie: + + zapisz_p(log); + + + if (log->dw.wsk_buf==MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*5-1) + { + log->status_rej_LMZ = blokada; + log->dw.wsk_rej = 1; + } else { + if(!check_struct(&log->WE_ZDistL_Pzf[0]) && !check_struct(&log->WE_ZDistL_Pzf[1])) + { + if(log->dw.licz1>LOOP_CYCLE_MS) + log->dw.licz1-=LOOP_CYCLE_MS; + else + log->dw.licz1=0; + } else { + log->dw.licz1 = 10; + } + if(!log->dw.licz1) + { + if (log->dw.wsk_bufstatus_rej_LMZ = oczekiwanie; + } else { + log->status_rej_LMZ = blokada; + log->dw.wsk_rej = 1; + } + + } + + } + break; + + case blokada: + if (!log->dw.wsk_rej && !(check_struct(&log->WE_ZDistL_Pzf[0]) || check_struct(&log->WE_ZDistL_Pzf[1]))) + { + log->status_rej_LMZ = oczekiwanie; + } + break; + + default: + break; + } + } +} + +void LMZ_20hz(void *arguments, void *logic) +{ + struct LMZ_logic *log = (struct LMZ_logic *)logic; + u16 licz[6], temp, temp1; + u16 czas_mf, czas_1f, zwarcie_mf, zwarcie_1f, roznica; + u16 czas_4, czas_5, czas_6; + float local; + u16 k; + int i; + u8 startbuf = 0; + float sr1, sr2, sr3, iloraz; + + if(log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli zebra dostatecznie wielk ilo danych + { + //wyliczanie petli dla strefy 1 + + licz[0]=0; + licz[1]=0; + licz[2]=0; + licz[3]=0; + licz[4]=0; + licz[5]=0; + + for(i=0;idw.wsk_buf;i++) + { + //okrelenie ktra ptla bya pobudzona najduej (kryterium wyboru) + for(k=0;k<6;k++) + { + if(log->dw.P[i][k]) licz[k]++; + } + } + temp = 0; + temp1 = 0; + czas_1f = 0; + czas_mf = 0; + zwarcie_mf = 0; + zwarcie_1f = 0; + + //zwarcia miedzyfazowe + for(k=3;k<6;k++) + { + if(licz[k]>temp1) + { + temp1 = licz[k]; + temp = k+1; + czas_mf = temp1; + zwarcie_mf = k+1; + } + } + + czas_4 = licz[3]; + czas_5 = licz[4]; + czas_6 = licz[5]; + + //zwarcia jednofazowe + for(k=0;k<3;k++) + { + if(licz[k]>czas_1f) + { + czas_1f = licz[k]; + zwarcie_1f = k+1; + } + } + + for(k=0;k<3;k++) + { + if(licz[k]>temp1) + { + temp1 = licz[k]; + temp = k+1; + } + } + + if (czas_1f > czas_mf) + { + roznica = czas_1f - czas_mf; + } + else + { + roznica = czas_mf - czas_1f; + } + + //wyznaczanie przedzialu z ktorego beda wyioczane dane + if(log->dw.wsk_buf<25) + { + startbuf=0; + log->dw.wsk_buf-=(log->dw.wsk_buf-20); + } + else if(log->dw.wsk_buf>=25 && log->dw.wsk_buf<35) + { + startbuf=(log->dw.wsk_buf-25); + log->dw.wsk_buf-=5; + } + else if(log->dw.wsk_buf>=35 && log->dw.wsk_buf<40) + { + startbuf=5; + log->dw.wsk_buf-=10; + } + else if(log->dw.wsk_buf>=40 && log->dw.wsk_buf<50) + { + startbuf=10; + log->dw.wsk_buf-=10; + } + else if(log->dw.wsk_buf>=50 && log->dw.wsk_buf<=60) + { + startbuf=15; + log->dw.wsk_buf-=15; + } + else + { + startbuf=20; + log->dw.wsk_buf-=20; + } + + sr1 = 0; + sr2 = 0; + sr3 = 0; + + //wyliczenie sredniej wartosci pradow w oknie zwarcia + for(i=0;i<(log->dw.wsk_buf - startbuf);i++) + { + float r = log->dw.dp[i+startbuf].IL1.r; + float ii = log->dw.dp[i+startbuf].IL1.i; + float esk = sqrt((r * r + ii * ii) * 0.5); + sr1 +=esk; + + r = log->dw.dp[i+startbuf].IL2.r; + ii = log->dw.dp[i+startbuf].IL2.i; + esk = sqrt((r * r + ii * ii) * 0.5); + sr2 +=esk; + + r = log->dw.dp[i+startbuf].IL3.r; + ii = log->dw.dp[i+startbuf].IL3.i; + esk = sqrt((r * r + ii * ii) * 0.5); + sr3 +=esk; + } + sr1 = sr1 / (log->dw.wsk_buf - startbuf); + sr2 = sr2 / (log->dw.wsk_buf - startbuf); + sr3 = sr3 / (log->dw.wsk_buf - startbuf); + + float procent = 1; + + #ifdef LMZ_DEBUG + *log->debug1 = (float)czas_1f; + *log->debug2 = (float)czas_4; + *log->debug3 = (float)czas_5; + *log->debug4 = (float)czas_6; + #endif + + float d1 = 1; + float d2 = 1; + + //gdy zwarcie jest miedzyfazowe i sa pobudzone jednoczesnie dwie petle, wybierz wlasciwa + //na podstawie pradow + if (czas_mf > 20/LOOP_CYCLE_MS) //zwarcie miedzyfazowe + { + if ((czas_4 > 20/LOOP_CYCLE_MS) && (czas_5 > 20/LOOP_CYCLE_MS) && (czas_6 > 20/LOOP_CYCLE_MS)) + { + //zwarcie 3 fazowe nic nie rob + } + else + { + if ((czas_4 > 20/LOOP_CYCLE_MS) && (czas_5 > 20/LOOP_CYCLE_MS)) //L1L2 i L2L3 + { + if (czas_4 > czas_5) + procent = (float)czas_5 / (float)czas_4; + else + procent = (float)czas_4 / (float)czas_5; + + if (procent > 0.7) + { + float iloraz12 = 1; + float iloraz23 = 1; + d1 = sr1; + d2 = sr2; + if (d1 > d2) + iloraz12 = d1 / d2; + else + iloraz12 = d2 / d1; + + d1 = sr2; + d2 = sr3; + if (d1 > d2) + iloraz23 = d1 / d2; + else + iloraz23 = d2 / d1; + + if (iloraz12 > iloraz23) + { + temp = 5; + zwarcie_mf = 5; + } + else + { + temp = 4; + zwarcie_mf = 4; + } + } + } + + if ((czas_4 > 20/LOOP_CYCLE_MS) && (czas_6 > 20/LOOP_CYCLE_MS)) //L1L2 i L3L1 + { + if (czas_4 > czas_6) + procent = (float)czas_6 / (float)czas_4; + else + procent = (float)czas_4 / (float)czas_6; + + if (procent > 0.7) + { + float iloraz12 = 1; + float iloraz31 = 1; + d1 = sr1; + d2 = sr2; + if (d1 > d2) + iloraz12 = d1 / d2; + else + iloraz12 = d2 / d1; + + d1 = sr3; + d2 = sr1; + if (d1 > d2) + iloraz31 = d1 / d2; + else + iloraz31 = d2 / d1; + + if (iloraz12 > iloraz31) + { + temp = 6; + zwarcie_mf = 6; + } + else + { + temp = 4; + zwarcie_mf = 4; + } + } + } + + if ((czas_5 > 20/LOOP_CYCLE_MS) && (czas_6 > 20/LOOP_CYCLE_MS)) //L2L3 i L3L1 + { + if (czas_5 > czas_6) + procent = (float)czas_6 / (float)czas_5; + else + procent = (float)czas_5 / (float)czas_6; + + if (procent > 0.7) + { + float iloraz23 = 1; + float iloraz31 = 1; + + d1 = sr2; + d2 = sr3; + + if (d1 > d2) + iloraz23 = d1 / d2; + else + iloraz23 = d2 / d1; + + d1 = sr3; + d2 = sr1; + + if (d1 > d2) + iloraz31 = d1 / d2; + else + iloraz31 = d2 / d1; + + if (iloraz23 > iloraz31) + { + temp = 6; + zwarcie_mf = 6; + } + else + { + temp = 5; + zwarcie_mf = 5; + } + } + } + } + } + + if (zwarcie_mf == 4) + { + d1 = sr1; + d2 = sr2; + } + else if (zwarcie_mf == 5) + { + d1 = sr2; + d2 = sr3; + } + if (zwarcie_mf == 6) + { + d1 = sr3; + d2 = sr1; + } + + if (d1 > d2) + iloraz = d1 / d2; + else + iloraz = d2 / d1; + + procent = 1; + + if (czas_1f != 0) + procent = (float)czas_mf / (float)czas_1f; + + if ((czas_mf > 20/LOOP_CYCLE_MS) && (czas_1f > 20/LOOP_CYCLE_MS) && (procent > 0.5) && (zwarcie_mf != 0)) + { + if (iloraz < 3) + temp = zwarcie_mf; + else + temp = zwarcie_1f; + } + + #ifdef LMZ_DEBUG + *log->debug = temp; + #endif + + switch(temp) + { + case 1://zwarcie RE + for(i=0;i<(log->dw.wsk_buf - startbuf);i++) + { + log->dw.zu = log->dw.dp[i+startbuf].UL1; + log->dw.odl_b[i] = wylicz_dane_1f(log,i+startbuf,0); + } + break; + case 2://zwarcie SE + for(i=0;i<(log->dw.wsk_buf - startbuf);i++) + { + log->dw.zu = log->dw.dp[i+startbuf].UL2; + log->dw.odl_b[i] = wylicz_dane_1f(log,i+startbuf,1); + } + break; + case 3://zwarcie TE + for(i=0;i<(log->dw.wsk_buf-startbuf);i++) + { + log->dw.zu = log->dw.dp[i+startbuf].UL3; + log->dw.odl_b[i] = wylicz_dane_1f(log,i+startbuf,2); + } + break; + case 4://zwarcie RS + for(i=0;i<(log->dw.wsk_buf-startbuf);i++) + { + log->dw.zu = log->dw.dp[i+startbuf].UL12; + log->dw.odl_b[i] = wylicz_dane_mf(log,i+startbuf,0,1); + } + break; + case 5://zwarcie ST + for(i=0;i<(log->dw.wsk_buf-startbuf);i++) + { + log->dw.zu = log->dw.dp[i+startbuf].UL23; + log->dw.odl_b[i] = wylicz_dane_mf(log,i+startbuf,1,2); + } + break; + case 6://zwarcie TE + for(i=0;i<(log->dw.wsk_buf-startbuf);i++) + { + log->dw.zu = log->dw.dp[i+startbuf].UL31; + log->dw.odl_b[i] = wylicz_dane_mf(log,i+startbuf,2,0); + } + break; + case 0: //error + break; + } + +////--------------------------- + + //szukanie wyniku - mediana z wyliczonych wartoci + for(i=0;i<(log->dw.wsk_buf-startbuf);i++) + { + temp1 = i+1; + while(temp1) + { + temp1--; + if(log->dw.odl_b[temp1+1]>log->dw.odl_b[temp1]) + { + local = log->dw.odl_b[temp1]; + log->dw.odl_b[temp1] = log->dw.odl_b[temp1+1]; + log->dw.odl_b[temp1+1] = local; + } + else + { + temp1 = 0; + } + } + } + + if(temp) + { + set_struct(&log->P); + if((log->dw.wsk_buf-startbuf) % 2) + { + *log->distance=log->dw.odl_b[(log->dw.wsk_buf-startbuf)/2 + 1]; + } + else + { + *log->distance=(log->dw.odl_b[(log->dw.wsk_buf-startbuf)/2 + 1] + log->dw.odl_b[(log->dw.wsk_buf-startbuf)/2]) / 2; + } + //------------------------------------------------ + } + + log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + } + + if(check_struct(&log->clr)) + { + *log->distance=0; + clear_struct(&log->P); + + } +} diff --git a/src/LMZ.h b/src/LMZ.h new file mode 100644 index 0000000..53e9554 --- /dev/null +++ b/src/LMZ.h @@ -0,0 +1,143 @@ +/* + * LMZ.h + * + * Created on: 23-05-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef LMZ_H_ +#define LMZ_H_ + +#include "tdefs.h" +#include "ZDistA.h" +#include "analog_in.h" + +struct zespolona +{ + float r; + float i; +}; + +struct Nastawy_przeliczone_LMZ +{ + struct Wyjscia_ZDistA *WYA; + //R_ZDistL::dane_wewnetrzne *WYL; + struct zespolona a; //wspólczynnik + struct zespolona b; //wspólczynnik + struct zespolona c; //wspólczynnik + struct zespolona kziem; //wspólczynnik + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 L2t; ///< linia dwutorowa +}; + +struct dane_probek +{ + struct zespolona IL1; // dane pr¹du faza 1 + struct zespolona IL2; // dane pr¹du faza 2 + struct zespolona IL3; // dane pr¹du faza 3 + struct zespolona Io; // dane pr¹du zerpwego + struct zespolona Io2; // dane pr¹du zerowego toru równoleg³ego + struct zespolona UL1; //napiêcie faza 1 + struct zespolona UL2; //napiêcie faza 2 + struct zespolona UL3; //napiêcie faza 3 + struct zespolona UL12; //napiêcie miedzyfazowe 1-2 + struct zespolona UL23; //napiêcie miêdzyfazowe 2-3 + struct zespolona UL31; //napiêcie miedzyfazowe 3-1 +}; + +struct prad_pz +{ + struct zespolona IL1;//bufor pradu z przed zwarcia faza 1 + struct zespolona IL2;//bufor pradu z przed zwarcia faza 2 + struct zespolona IL3;//bufor pradu z przed zwarcia faza 3 +}; + +struct dane_wewnetrzne +{ +// Wyjscia_rejestratora R; ///< dane ostatniego zak³ócenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + short wsk_buf;//aktualne polozenie w buforze próbek (w trakcie rejestracji) + short wsk_buf_pre;//aktualne polozenie w buforze próbek z przed zwarcia + short licz1; + u16 wsk_p; //wskaznik do okreslenia pradu + float dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + float odl_b[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*5];//odleg³oœæ pojedynczej iteracji + struct dane_probek dp[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*5]; // ortogonalne kana³ow wejsciowych + struct prad_pz Ip[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*3]; //bufor biezacy pamietania probek z przed zwarcia + struct prad_pz Im[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS]; //bufor pamietania probek z przed zwarcia "zatrzaœniêta" + u8 P[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*5][6]; + struct zespolona zz; + struct zespolona zi; + struct zespolona zu; + struct zespolona zibis; + u8 wsk_rej; +}; + +struct LMZ_logic +{ + struct binary_io WE_ZDistL_Pzf[2]; + float *IoII_orta; + float *IoII_ortb; + struct analog_in_params *IoII_params; + struct binary_io clr; + struct binary_io P; + float *distance; + + #ifdef LMZ_DEBUG + float *debug; + float *debug1; + float *debug2; + float *debug3; + float *debug4; + #endif + + struct dane_wewnetrzne dw; + struct Nastawy_przeliczone_LMZ nast_;// struktura z parametrami + enum rej_ost status_rej_LMZ; +}; + +struct LMZ_io +{ + u32 we_zdist_ptr_in; + u32 we_zdistl_p1_in; + u32 we_zdistl_p1w_in; + u32 ioii_orta_float_in; + u32 ioii_ortb_float_in; + u32 ioii_params_ptr_in; + u32 clr_in; + u32 p_out; + u32 distance_float_out; + + #ifdef LMZ_DEBUG + u32 debug_out; + u32 debug1_out; + u32 debug2_out; + u32 debug3_out; + u32 debug4_out; + #endif + +}__attribute__((__packed__)); + +struct LMZ_params +{ + u32 bity; ///< nastawy bitowe; + double R1;///< Rezystancja jednostkowa dla sk³adowej zgodnej + double X1;///ret_I_L1 = 0; + log->ret_I_L2 = 0; + log->ret_I_L3 = 0; + log->ret_W_L1 = 0; + log->ret_W_L2 = 0; + log->ret_W_L3 = 0; + log->ret_I_3f = 0; + log->ret_W_3f = 0; + log->wy_T1 = 0; + log->wy_T2 = 0; + log->wy_T2_I = 0; + log->wy_T2_W = 0; + + log->pob_I_L1 = 0; + log->pob_I_L2 = 0; + log->pob_I_L3 = 0; + log->pob_W_L1 = 0; + log->pob_W_L2 = 0; + log->pob_W_L3 = 0; + log->pob_I_3f = 0; + log->pob_W_3f = 0; + log->pob_T2_I = 0; + log->pob_T2_W = 0; + log->wy_pob = 0; + + log->t_I_L1 = 0; + log->t_I_L2 = 0; + log->t_I_L3 = 0; + log->t_W_L1 = 0; + log->t_W_L2 = 0; + log->t_W_L3 = 0; + log->t_I_3f = 0; + log->t_W_3f = 0; + log->t_I_2 = 0; + log->t_W_2 = 0; + +} + +int LRW_initlog(void *arguments, void *logic) +{ + struct LRW_args *args = (struct LRW_args *)arguments; + struct LRW_logic *log = (struct LRW_logic *)logic; + + if(set_bit_ptr_struct(args->io.stan_bl,&log->stan_bl)) + return -1; + + if(set_bit_ptr_struct(args->io.w_on_L1_in,&log->w_on_L1)) + return -1; + if(set_bit_ptr_struct(args->io.w_on_L2_in,&log->w_on_L2)) + return -1; + if(set_bit_ptr_struct(args->io.w_on_L3_in,&log->w_on_L3)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_L1_in,&log->wyl_L1)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_L2_in,&log->wyl_L2)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_L3_in,&log->wyl_L3)) + return -1; + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.retrip_I_L1_out,&log->retrip_I_L1)) + return -1; + if(set_bit_ptr_struct(args->io.retrip_I_L2_out,&log->retrip_I_L2)) + return -1; + if(set_bit_ptr_struct(args->io.retrip_I_L3_out,&log->retrip_I_L3)) + return -1; + if(set_bit_ptr_struct(args->io.retrip_W_L1_out,&log->retrip_W_L1)) + return -1; + if(set_bit_ptr_struct(args->io.retrip_W_L2_out,&log->retrip_W_L2)) + return -1; + if(set_bit_ptr_struct(args->io.retrip_W_L3_out,&log->retrip_W_L3)) + return -1; + + if(set_bit_ptr_struct(args->io.retrip_I_3f_out,&log->retrip_I_3f)) + return -1; + if(set_bit_ptr_struct(args->io.retrip_W_3f_out,&log->retrip_W_3f)) + return -1; + + if(set_bit_ptr_struct(args->io.T2_I_out,&log->T2_I)) + return -1; + if(set_bit_ptr_struct(args->io.T2_W_out,&log->T2_W)) + return -1; + + if(set_bit_ptr_struct(args->io.T1_out,&log->T1)) + return -1; + if(set_bit_ptr_struct(args->io.T2_out,&log->T2)) + return -1; + + if(set_bit_ptr_struct(args->io.pob_out,&log->pob)) + return -1; + + if(set_float_ptr(args->io.I1_float_in,&log->I1)) + return -1; + + if(set_float_ptr(args->io.I2_float_in,&log->I2)) + return -1; + + if(set_float_ptr(args->io.I3_float_in,&log->I3)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->kryt_pr = (args->params.bity & 0x0002)?1:0; + log->kryt_wyl = (args->params.bity & 0x0004)?1:0; + log->nast_.full_period = (args->params.bity & 0x0008)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + + log->t1_limit = (u32)((args->params.t1_) * 1000 / LOOP_CYCLE_MS); + log->t2_limit = (u32)((args->params.t2_) * 1000 / LOOP_CYCLE_MS); + + set_default(log); + + return 0; +} + +void LRW(void *arguments, void *logic) +{ + struct LRW_logic *log = (struct LRW_logic *)logic; + + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + u8 blokada = check_struct(&log->stan_bl)?1:0; + u8 w_on_L1 = check_struct(&log->w_on_L1)?1:0; + u8 w_on_L2 = check_struct(&log->w_on_L2)?1:0; + u8 w_on_L3 = check_struct(&log->w_on_L3)?1:0; + u8 wyl_L1 = check_struct(&log->wyl_L1)?1:0; + u8 wyl_L2 = check_struct(&log->wyl_L2)?1:0; + u8 wyl_L3 = check_struct(&log->wyl_L3)?1:0; + u8 test = check_struct(&log->test)?1:0; + + + sprawdz_P(&log->dw.pob1, + *log->I1>log->nast_.limit1_, + *log->I1nast_.limit2_, + &log->dw.lp1, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob2, + *log->I2>log->nast_.limit1_, + *log->I2nast_.limit2_, + &log->dw.lp2, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 2 + + sprawdz_P(&log->dw.pob3, + *log->I3>log->nast_.limit1_, + *log->I3nast_.limit2_, + &log->dw.lp3, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 3 + + + log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3; + + log->pob_I_L1 = wyl_L1 && !wyl_L2 && !wyl_L3 && log->dw.pob1 && log->kryt_pr; + log->pob_I_L2 = !wyl_L1 && wyl_L2 && !wyl_L3 && log->dw.pob2 && log->kryt_pr; + log->pob_I_L3 = !wyl_L1 && !wyl_L2 && wyl_L3 && log->dw.pob3 && log->kryt_pr; + + log->pob_W_L1 = wyl_L1 && !wyl_L2 && !wyl_L3 && w_on_L1 && log->kryt_wyl; + log->pob_W_L2 = !wyl_L1 && wyl_L2 && !wyl_L3 && w_on_L2 && log->kryt_wyl; + log->pob_W_L3 = !wyl_L1 && !wyl_L2 && wyl_L3 && w_on_L3 && log->kryt_wyl; + + u8 min_2faz = (wyl_L1 && wyl_L2) || (wyl_L2 && wyl_L3) || (wyl_L1 && wyl_L3); + u8 suma_on = w_on_L1 || w_on_L2 || w_on_L3; + + log->pob_I_3f = min_2faz && log->dw.pob && log->kryt_pr; + log->pob_W_3f = min_2faz && suma_on && log->kryt_wyl; + + log->pob_T2_I = log->pob_I_L1 || log->pob_I_L2 || log->pob_I_L3 || log->pob_I_3f; + log->pob_T2_W = log->pob_W_L1 || log->pob_W_L2 || log->pob_W_L3 || log->pob_W_3f; + + //obsluga wejscia testu przekaznika + if (czy_test_R() && test) + { + log->pob_I_L1 = 1; + log->pob_I_L2 = 1; + log->pob_I_L3 = 1; + log->pob_W_L1 = 1; + log->pob_W_L2 = 1; + log->pob_W_L3 = 1; + + log->pob_I_3f = 1; + log->pob_W_3f = 1; + + log->pob_T2_I = 1; + log->pob_T2_W = 1; + } + + //realizacja blokady dzialania przekaznika + if(blokada) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0; + log->pob_I_L1 = 0; + log->pob_I_L2 = 0; + log->pob_I_L3 = 0; + log->pob_W_L1 = 0; + log->pob_W_L2 = 0; + log->pob_W_L3 = 0; + + log->pob_I_3f = 0; + log->pob_W_3f = 0; + + log->pob_T2_I = 0; + log->pob_T2_W = 0; + } + + log->wy_pob = log->pob_T2_I || log->pob_T2_W; + //sekcja timerow + + if (log->pob_I_L1) + { + if (++log->t_I_L1 > log->t1_limit) + { + log->ret_I_L1 = 1; + log->t_I_L1 = log->t1_limit + 10; + } + } + else + { + log->t_I_L1 = 0; + log->ret_I_L1 = 0; + } + + if (log->pob_I_L2) + { + if (++log->t_I_L2 > log->t1_limit) + { + log->ret_I_L2 = 1; + log->t_I_L2 = log->t1_limit + 10; + } + } + else + { + log->t_I_L2 = 0; + log->ret_I_L2 = 0; + } + + if (log->pob_I_L3) + { + if (++log->t_I_L3 > log->t1_limit) + { + log->ret_I_L3 = 1; + log->t_I_L3 = log->t1_limit + 10; + } + } + else + { + log->t_I_L3 = 0; + log->ret_I_L3 = 0; + } + + if (log->pob_W_L1) + { + if (++log->t_W_L1 > log->t1_limit) + { + log->ret_W_L1 = 1; + log->t_W_L1 = log->t1_limit + 10; + } + } + else + { + log->t_W_L1 = 0; + log->ret_W_L1 = 0; + } + + if (log->pob_W_L2) + { + if (++log->t_W_L2 > log->t1_limit) + { + log->ret_W_L2 = 1; + log->t_W_L2 = log->t1_limit + 10; + } + } + else + { + log->t_W_L2 = 0; + log->ret_W_L2 = 0; + } + + if (log->pob_W_L3) + { + if (++log->t_W_L3 > log->t1_limit) + { + log->ret_W_L3 = 1; + log->t_W_L3 = log->t1_limit + 10; + } + } + else + { + log->t_W_L3 = 0; + log->ret_W_L3 = 0; + } + + if (log->pob_I_3f) + { + if (++log->t_I_3f > log->t1_limit) + { + log->ret_I_3f = 1; + log->t_I_3f = log->t1_limit + 10; + } + } + else + { + log->t_I_3f = 0; + log->ret_I_3f = 0; + } + + if (log->pob_W_3f) + { + if (++log->t_W_3f > log->t1_limit) + { + log->ret_W_3f = 1; + log->t_W_3f = log->t1_limit + 10; + } + } + else + { + log->t_W_3f = 0; + log->ret_W_3f = 0; + } + + if (log->pob_T2_I) + { + if (++log->t_I_2 > log->t2_limit) + { + log->wy_T2_I = 1; + log->t_I_2 = log->t2_limit + 10; + } + } + else + { + log->wy_T2_I = 0; + log->t_I_2 = 0; + } + + if (log->pob_T2_W) + { + if (++log->t_W_2 > log->t2_limit) + { + log->wy_T2_W = 1; + log->t_W_2 = log->t2_limit + 10; + } + } + else + { + log->wy_T2_W = 0; + log->t_W_2 = 0; + } + + log->wy_T1 = log->ret_I_L1 || log->ret_I_L2 || log->ret_I_L3 || log->ret_W_L1 || log->ret_W_L2 || + log->ret_W_L3 || log->ret_I_3f || log->ret_W_3f; + + log->wy_T2 = log->wy_T2_I || log->wy_T2_W; + + check_and_set_struct(log->ret_I_L1,&log->retrip_I_L1); + check_and_set_struct(log->ret_I_L2,&log->retrip_I_L2); + check_and_set_struct(log->ret_I_L3,&log->retrip_I_L3); + check_and_set_struct(log->ret_W_L1,&log->retrip_W_L1); + check_and_set_struct(log->ret_W_L2,&log->retrip_W_L2); + check_and_set_struct(log->ret_W_L3,&log->retrip_W_L3); + check_and_set_struct(log->ret_I_3f,&log->retrip_I_3f); + check_and_set_struct(log->ret_W_3f,&log->retrip_W_3f); + check_and_set_struct(log->wy_T2_I,&log->T2_I); + check_and_set_struct(log->wy_T2_W,&log->T2_W); + check_and_set_struct(log->wy_T1,&log->T1); + check_and_set_struct(log->wy_T2,&log->T2); + check_and_set_struct(log->wy_pob,&log->pob); + + } + else + { + set_default(log); + + clear_struct(&log->retrip_I_L1); + clear_struct(&log->retrip_I_L2); + clear_struct(&log->retrip_I_L3); + clear_struct(&log->retrip_W_L1); + clear_struct(&log->retrip_W_L2); + clear_struct(&log->retrip_W_L3); + clear_struct(&log->retrip_I_3f); + clear_struct(&log->retrip_W_3f); + clear_struct(&log->T1); + clear_struct(&log->T2); + clear_struct(&log->T2_I); + clear_struct(&log->T2_W); + } +} + + + diff --git a/src/LRW.h b/src/LRW.h new file mode 100644 index 0000000..852a996 --- /dev/null +++ b/src/LRW.h @@ -0,0 +1,167 @@ +/* + * LRW.h + * + * Created on: 29-11-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef LRW_H__ +#define LRW_H__ + +//#include "pawel_usun_to.h" + +#include "tdefs.h" + + struct Nastawy_przeliczone_lrw + { + float limit1_; ///< wartosc rozruchowa + float limit2_; ///< wartosc powrotu + u32 t_; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 full_period; + }; + + struct dane_wewnetrzne_lrw + { +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + short lp1; /// +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "R87L.h" +#include "analog_in.h" +#include "Diff.h" + +/** +Ustawia wskazniki na prady po skompensowaniu +\param typ wskazuje na podstawie nastawy odpowiednia konfiguracje pradow +\param nr_pradu okreslenie numeru przetwarzanago pradu +*/ + +static void ustaw_kompensacje(struct r87l_logic *log, enum typ_kompensacji typ, u16 nr_pradu) +{ + float *a0,*a1,*a2; + float *a0_prev,*a1_prev,*a2_prev; + switch(nr_pradu) + { + case 0: + a0 = &log->A[0][0];//&log->stream[0]->A[0]; + a1 = &log->A[0][1];//stream[0]->A[1]; + a2 = &log->A[0][2];//stream[0]->A[2]; + a0_prev = &log->A_prev[0][0];//&log->stream[0]->A[0]; + a1_prev = &log->A_prev[0][1];//stream[0]->A[1]; + a2_prev = &log->A_prev[0][2];//stream[0]->A[2]; + break; + + case 1: + a0 = &log->A[1][0];//&log->stream[1]->A[0]; + a1 = &log->A[1][1];//stream[1]->A[1]; + a2 = &log->A[1][2];//stream[1]->A[2]; + a0_prev = &log->A_prev[1][0];//&log->stream[1]->A[0]; + a1_prev = &log->A_prev[1][1];//stream[1]->A[1]; + a2_prev = &log->A_prev[1][2];//stream[1]->A[2]; + break; + default: // 2 and others + a0 = &log->A[2][0];//&log->stream[0]->A[0]; + a1 = &log->A[2][1];//stream[0]->A[1]; + a2 = &log->A[2][2];//stream[0]->A[2]; + a0_prev = &log->A_prev[2][0];//&log->stream[0]->A[0]; + a1_prev = &log->A_prev[2][1];//stream[0]->A[1]; + a2_prev = &log->A_prev[2][2];//stream[0]->A[2]; + break; + } + + switch (typ) + { + case zero: + log->nast_.L[0].Ip[nr_pradu] = &log->nast_.zero; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 0; + + log->nast_.L[0].Ip_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + + break; + case A: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + + break; + case B: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a1;// + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = -1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;// + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + case C: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a0_prev; + break; + case D: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = -0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a0_prev; + break; + case E: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a1_prev; + break; + case F: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = -0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a1_prev; + + break; + case N: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + case O: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = -1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + + case U: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = -1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + + case V: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + + + case W: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a0_prev; + break; + + case X: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a1_prev; + break; + case Y: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a2_prev; + break; + case Z: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a2_prev; + break; + } +} + +void przelicz_nastawy1_r87l(struct r87l_logic *log,struct nast_trafo n) +{ + if (n.GrP1) // jesli gorne uzwojenie trafo D + { + if (!n.GrP3) //jesli dwuuzwojeniowy trafo + { + log->jaki_uklad = tylko_T2w_D; + } + else + { //jesli trojuzwojeniowy trafo + log->jaki_uklad = tylko_T3w_D; + } + } + else + { //jesli gorne uzwojenie trafo Y + if (!n.GrP3) //jesli dwuuzwojeniowy trafo + { + log->jaki_uklad = tylko_T2w_Y; + } + else + { //jesli trojuzwojeniowy trafo + log->jaki_uklad = tylko_T3w_Y; + } + } + + //----------------------------------------- + + if(n.GrP2>6) + log->nast_.p2d=1; + else + log->nast_.p2d=0; + + if(n.GrP3>6) + log->nast_.p3d=1; + else + log->nast_.p3d=0; + + //ustawienie skladnikow kompensacji pradow + switch (log->jaki_uklad) + { + case tylko_T2w_Y: + ustaw_kompensacje(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje(log,zero, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= /*log->nast_.L[0].Ip[1]->params->znam_pierw + / log->nast_.L[0].Ip[0]->params->znam_pierw + / */n.PT12 + ; + log->nast_.stream_cnt=2; + break; + case tylko_T2w_D: + ustaw_kompensacje(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje(log,zero, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= n.PT12; + log->nast_.stream_cnt=2; + + break; + case tylko_T3w_Y: + ustaw_kompensacje(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Ig, 0); + ustaw_kompensacje(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Id, 1); + ustaw_kompensacje(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]||!pY_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= n.PT12; + log->nast_.a[2] /= n.PT13; + log->nast_.stream_cnt=3; + + break; + case tylko_T3w_D: + ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]||!pD_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= n.PT12; + log->nast_.a[2] /= n.PT13; + log->nast_.stream_cnt=3; + + break; + } + //----------------------------------------- + + //przeliczenie wspolczynnikow kompensacji o korekcje i ofset + uint16_t i; + + for (i = 0; i < 3; i++) + { + + log->nast_.L[i].a_p[0][0] = log->nast_.a[0] * 1;//log->nast_.L[i].Ip[0]->params->nast.mnoznik; + //log->nast_.L[i].a_p[0][1] = /*log->nast_.L[i].Ip[0]->params->nast.offset + */32767; + log->nast_.L[i].a_m[0][0] = log->nast_.a[0] * 1;//log->nast_.L[i].Im[0]->params->nast.mnoznik; + //log->nast_.L[i].a_m[0][1] =/* log->nast_.L[i].Im[0]->params->nast.offset + */32767; + log->nast_.L[i].a_p[1][0] = log->nast_.a[1] * 1;//log->nast_.L[i].Ip[1]->params->nast.mnoznik; + //log->nast_.L[i].a_p[1][1] =/* log->nast_.L[i].Ip[1]->params->nast.offset + */32767; + log->nast_.L[i].a_m[1][0] = log->nast_.a[1] * 1;//log->nast_.L[i].Im[1]->params->nast.mnoznik; + //log->nast_.L[i].a_m[1][1] =/* log->nast_.L[i].Im[1]->params->nast.offset + */32767; + log->nast_.L[i].a_p[2][0] = log->nast_.a[2] * 1;//log->nast_.L[i].Ip[2]->params->nast.mnoznik; + //log->nast_.L[i].a_p[2][1] =/* log->nast_.L[i].Ip[2]->params->nast.offset + */32767; + log->nast_.L[i].a_m[2][0] = log->nast_.a[2] * 1;//log->nast_.L[i].Im[2]->params->nast.mnoznik; + //log->nast_.L[i].a_m[2][1] =/* log->nast_.L[i].Im[2]->params->nast.offset + */32767; + log->nast_.L[i].a_p[3][0] = log->nast_.a[3] * 1;//log->nast_.L[i].Ip[3]->params->nast.mnoznik; + //log->nast_.L[i].a_p[3][1] =/* log->nast_.L[i].Ip[3]->params->nast.offset + */32767; + log->nast_.L[i].a_m[3][0] = log->nast_.a[3] * 1;//log->nast_.L[i].Im[3]->params->nast.mnoznik; + //log->nast_.L[i].a_m[3][1] =/* log->nast_.L[i].Im[3]->params->nast.offset + */32767; + + log->nast_.L[i].a_pi0[0] = log->nast_.a[0] * 0.333333f;//log->nast_.L[i].Ip[0]->params->nast.mnoznik; + log->nast_.L[i].a_pi0[1] = log->nast_.a[1] * 0.333333f;//log->nast_.L[i].Ip[1]->params->nast.mnoznik; + log->nast_.L[i].a_pi0[2] = log->nast_.a[2] * 0.333333f;//log->nast_.L[i].Ip[2]->params->nast.mnoznik; + } +/* + uint16_t j; + + if(log->nast_.test_1str) + { + for(j=0;j<3;j++) + { + if(j==log->my_addr) + continue; + + for(i=0;i<3;i++) + { + log->nast_.L[i].a_p[j][0]=0; + log->nast_.L[i].a_m[j][0]=0; + log->nast_.L[i].a_pi0[j]=0; + } + } + } +*/ + // wyznaczenie wspolczynnikow filtru + + + for (i = 0; i < MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2; i++) + { + log->nast_.wsp_filtr_sin_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_cos_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_sin2h_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_cos2h_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_sin5h_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/5*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_cos5h_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/5*2)) * i) / (10/LOOP_CYCLE_MS*2); + } + + //---------------------------------------------------------------------------------- + + memset(&log->w, 0x00, sizeof(log->w)); +} + +/** +Wylicza pobudzenie blokad od drugiej i piatej harmonicznej +\param *licz wskaznik na licznik pobudzen +\param pobudzenie warunek pobudzenia przekaznika +\param powrot warunek powrotu przekaznika +\param st stan przekaznika z poprzedniej iteracji +\param powz ilosc powtorzen przy zadzialaniu +\param powp ilosc powtorzen przy powrocie +\return aktualny stan przekaznika +*/ + +u8 r87l_kryterium1(short *licz, u8 pobudzenie, u8 powrot, u8 st, unsigned short powz, unsigned short powp) +{ + u8 stp; + stp = st; + if (st) + { + if (powrot) + { + *(licz + 1) += LOOP_CYCLE_MS; + if (*(licz + 1) > powp) + { + stp = 0; + *(licz) = 0; + *(licz + 1) = powp; + } + } + else { + *(licz + 1) -= LOOP_CYCLE_MS; + if (*(licz + 1) < 0) *(licz + 1) = 0; + } + } + else { + if (pobudzenie) + { + *(licz) += LOOP_CYCLE_MS; + if (*(licz) > powz) + { + stp = 1; + *(licz) = powz; + *(licz + 1) = 0; + } + } + else { + *(licz) -= LOOP_CYCLE_MS; + if (*(licz) < 0) *(licz) = 0; + } + } + return(stp); +} +//------------------ +/** +Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji +\param Ir wartoscs pradu roznicowego +\param Ih wartosc pradu stabilizacji +*/ + +void r87l_kryterium2(struct r87l_logic *log,float Ir, float Ih, u8 *pob, u8 idx) +{ + u16 n = 2;// 1; + +// if (Ih > log->nast_.Ih1) n = 2; + if (Ih > log->nast_.Ihp) n = 3; + if ((Ih > log->nast_.Ihg) && log->nast_.h) n = 4; + + *pob = 0; + *(pob + 1) = 0; + *(pob + 2) = 1; + + log->n[idx]=n; + + switch (n) + { + case 1: + *pob = Ir > log->nast_.Iror; + *(pob + 1) = Ir < log->nast_.Irop; + break; + case 2: +// *pob = Ir > Ih * log->nast_.kh1r; +// *(pob + 1) = Ir < Ih * log->nast_.kh1p; + *pob = Ir > ((Ih * log->nast_.kh1r)+log->nast_.Iror); + *(pob + 1) = Ir < ((Ih * log->nast_.kh1p)+log->nast_.Irop); + + break; + case 3: + *pob = Ir > (Ih * log->nast_.kh2r + log->nast_.Ir3c); + *(pob + 1) = Ir < (Ih * log->nast_.kh2p + log->nast_.Ip3c); + break; + case 4: + *pob = Ir > log->nast_.Ir4; + *(pob + 1) = Ir < log->nast_.Ip4; + *(pob + 2) = *(pob + 1); + + break; + } +} + +/** +Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji +\param Ir wartosc pradu roznicowego +\param Ih wartosc pradu stabilizacji +*/ +void r87l_kryterium3(struct r87l_logic *log,float Ir, float Ih, u8 *pob) +{ + + *pob = 0; + *(pob + 1) = 0; + *(pob + 2) = 1; + + if ((Ih > log->nast_.Ihg) && log->nast_.h) + { + *pob = Ir > log->nast_.Ir4; + *(pob + 1) = Ir < log->nast_.Ip4; + *(pob + 2) = *(pob + 1); + } + else { + *pob = Ir > (log->nast_.Iror + log->nast_.kh1r2 * Ih); + *(pob + 1) = Ir < (log->nast_.Irop + log->nast_.kh1p2 * Ih); + } +} + +void r87l_przelicz_1h_ip(struct r87l_logic *log, uint16_t Lnr) +{ + int16_t i, i1,j; + float orta; + float ortb; + float pr; + + log->w.Ir[Lnr]=0; + + for(j=0;j<3;j++) + { + orta=0; + ortb=0; + i1 = log->w.nr_probki; + + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2; + pr = log->w.buf_ip[i1][((Lnr-3)*3)+j]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] += sqrtf(pr * 0.5f); + } + + log->w.Ir[Lnr]*=0.5f; + log->w.Ir[Lnr]*=log->w.Ir[Lnr]; +} + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void r87l_przelicz_1h(struct r87l_logic *log, uint16_t Lnr) +{ + int16_t i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + log->w.orta[Lnr] = orta; + log->w.ortb[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] = pr * 0.5f; +} + + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void r87l_przelicz_2h(struct r87l_logic *log,uint16_t Lnr) +{ + + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin2h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos2h_[i]; + } + + log->w.orta2h[Lnr] = orta; + log->w.ortb2h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I2h[Lnr] = pr * 0.5f; + +} + + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void r87l_przelicz_5h(struct r87l_logic *log,uint16_t Lnr) +{ + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin5h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos5h_[i]; + } + + log->w.orta5h[Lnr] = orta; + log->w.ortb5h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I5h[Lnr] = pr * 0.5f; +} + +int R87L_initlog(void *arguments, void *logic) +{ + struct r87l_args *args = (struct r87l_args *)arguments; + struct r87l_logic *log = (struct r87l_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_pointer_in_ptr(args->io.stream1_ptr_in,(u32*)&log->stream[0])) + return -1; + if(set_pointer_in_ptr(args->io.stream2_ptr_in,(u32*)&log->stream[1])) + return -1; + if(set_pointer_in_ptr(args->io.stream3_ptr_in,(u32*)&log->stream[2])) + return -1; + + if(set_bit_ptr_struct(args->io.streams_ok_in,&log->streams_ok)) + return -1; + + if(set_bit_ptr_struct(args->io.zezw_w1f_in,&log->zezw_w1f)) + return -1; + + if(set_bit_ptr_struct(args->io.przeglad_in,&log->przeglad)) + return -1; + + if(set_bit_ptr_struct(0/*args->io.test_1str_in*/,&log->test_1str)) // poprawka wycofujaca test1str + return -1; + +// if(set_pointer_in_ptr(args->io.stream1_ptr_in/*args->io.ownstream_ptr_in*/,(u32*)&log->ownstream)) // poprawka wycofujaca test1str +// return -1; + + if(set_bit_ptr_struct(args->io.W_out,&log->W)) + return -1; + if(set_bit_ptr_struct(args->io.WL1_out,&log->Wf[0])) + return -1; + if(set_bit_ptr_struct(args->io.WL2_out,&log->Wf[1])) + return -1; + if(set_bit_ptr_struct(args->io.WL3_out,&log->Wf[2])) + return -1; + + if(set_bit_ptr_struct(args->io.Z_out,&log->Z)) + return -1; + if(set_bit_ptr_struct(args->io.ZL1_out,&log->Zf[0])) + return -1; + if(set_bit_ptr_struct(args->io.ZL2_out,&log->Zf[1])) + return -1; + if(set_bit_ptr_struct(args->io.ZL3_out,&log->Zf[2])) + return -1; + + if(set_bit_ptr_struct(args->io.P_out,&log->P)) + return -1; + + if(set_bit_ptr_struct(args->io.PL1_out,&log->Pf[0])) + return -1; + + if(set_bit_ptr_struct(args->io.PL2_out,&log->Pf[1])) + return -1; + + if(set_bit_ptr_struct(args->io.PL3_out,&log->Pf[2])) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L1_out,&log->BL2h1)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L2_out,&log->BL2h2)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L3_out,&log->BL2h3)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L1_out,&log->BL5h1)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L2_out,&log->BL5h2)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L3_out,&log->BL5h3)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew1_out,&log->Zw1)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew2_out,&log->Zw2)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew3_out,&log->Zw3)) + return -1; + + if(set_float_ptr(args->io.di_l1_float_out,&log->Ir1)) + return -1; + if(set_float_ptr(args->io.di_l2_float_out,&log->Ir2)) + return -1; + if(set_float_ptr(args->io.di_l3_float_out,&log->Ir3)) + return -1; + + if(set_float_ptr(args->io.ih_l1_float_out,&log->Ih1)) + return -1; + if(set_float_ptr(args->io.ih_l2_float_out,&log->Ih2)) + return -1; + if(set_float_ptr(args->io.ih_l3_float_out,&log->Ih3)) + return -1; + if(set_float_ptr(args->io.i2h_l1_float_out,&log->I2h1)) + return -1; + if(set_float_ptr(args->io.i2h_l2_float_out,&log->I2h2)) + return -1; + if(set_float_ptr(args->io.i2h_l3_float_out,&log->I2h3)) + return -1; + if(set_float_ptr(args->io.i5h_l1_float_out,&log->I5h1)) + return -1; + if(set_float_ptr(args->io.i5h_l2_float_out,&log->I5h2)) + return -1; + if(set_float_ptr(args->io.i5h_l3_float_out,&log->I5h3)) + return -1; + + if(set_float_ptr(args->io.ir_l1_ch_float_out,&log->Ir_L1_ortb)) + return -1; + if(set_float_ptr(args->io.ir_l2_ch_float_out,&log->Ir_L2_ortb)) + return -1; + if(set_float_ptr(args->io.ir_l3_ch_float_out,&log->Ir_L3_ortb)) + return -1; + + if(set_bit_ptr_struct(args->io.nieobs_out,&log->nieobs)) + return -1; + + if(set_bit_ptr_struct(args->io.blok_out,&log->blok)) + return -1; + + if(set_float_ptr(args->io.comm_bits_float_out,(float *)&log->comm_bits)) + return -1; + + if(set_bit_ptr_struct(args->io.blok_w_out,&log->blok_w)) + return -1; + + log->nast_.on = (args->params.bity & 0x0001)?1:0; + log->nast_.w = (args->params.bity & 0x0002)?1:0; + log->nast_.bl = (args->params.bity & 0x0004)?1:0; + log->nast_.bl2h = (args->params.bity & 0x0008)?1:0; + log->nast_.bl5h = (args->params.bity & 0x0010)?1:0; + log->nast_.sbc = (args->params.bity & 0x0020)?1:0; + log->nast_.h = (args->params.bity & 0x0040)?1:0; + log->nast_.r = (args->params.bity & 0x0080)?1:0; + log->nast_.cross_block = (args->params.bity & 0x0100)?1:0; + log->nast_.przyspiesz_dzialanie = (args->params.bity & 0x0200)?1:0; + log->nast_.przyjmuj_zadzialania = (args->params.bity & 0x0400)?1:0; + log->nast_.rozwijajace = (args->params.bity & 0x0800)?1:0; + //log->nast_.test_1str = (args->params.bity & 0x1000)?1:0; + //ustawienie wartosci rozruchowych + log->nast_.khr_2h = args->params.k_2h * args->params.k_2h; + log->nast_.khp_2h = log->nast_.khr_2h * 0.98;//args->params.kp *args->params.kp; + log->nast_.khr_5h = args->params.k_5h * args->params.k_5h; + log->nast_.khp_5h = log->nast_.khr_5h * 0.98;//args->params.kp *args->params.kp; + log->nast_.Irr = args->params.Ir * args->params.Ir;// * 0.93f // 1.3577f; + log->nast_.Ipr = args->params.Ir * args->params.kp * args->params.kp; + log->nast_.Ihdr = args->params.Ihd; + log->nast_.Irdr = args->params.rd; + log->nast_.kh1r = args->params.kh1; + log->nast_.kh1p = args->params.kh1 * args->params.kp; + log->nast_.kh2r = args->params.kh2; + log->nast_.kh2p = args->params.kh2 * args->params.kp; +// log->nast_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2)+ args->params.Iro; + + log->nast_.Ip3c = log->nast_.Ir3c * args->params.kp; + log->nast_.Iror = args->params.Iro; + log->nast_.I2h_min = log->nast_.Iror * log->nast_.Iror * log->nast_.khr_2h; + log->nast_.I5h_min = log->nast_.Iror * log->nast_.Iror * log->nast_.khr_5h; + log->nast_.Irop = args->params.Iro * args->params.kp; +// log->nast_.Ir4 = args->params.Ihg * args->params.kh2 + args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ir4 = args->params.Ihp * args->params.kh1 + args->params.kh2 * (args->params.Ihg - args->params.Ihp)+ args->params.Iro; + log->nast_.Ip4 = log->nast_.Ir4 * args->params.kp; + log->nast_.t_sbc = args->params.tbld * 1000; + log->nast_.Ih1 = args->params.Iro / args->params.kh1; + log->nast_.Ihp = args->params.Ihp; + log->nast_.Ihg = args->params.Ihg; + + log->nast_.tz = args->params.t * 1000; + log->nast_.zero_frame.A[0] = 0.0f; + log->nast_.zero_frame.A[1] = 0.0f; + log->nast_.zero_frame.A[2] = 0.0f; + log->nast_.zero=0.0f; + + // + struct nast_trafo n1; + n1.GrP1 = args->params.GrP1; + n1.GrP2 = args->params.GrP2; + n1.GrP3 = args->params.GrP3; + n1.PT12 = args->params.PT12; + n1.PT13 = args->params.PT13; +// n1.Tr_zas = args->params.Tr_zas; + + przelicz_nastawy1_r87l(log,n1); + *log->comm_bits=0; + + log->my_addr=args->params.adres; + log->original_stream=log->stream[log->my_addr]; + log->ownstream=log->original_stream; // poprawka usuwajaca 1stronne testowanie + + log->startup_timer=R87L_START_DELAY; + log->no_stream_timer=R87L_NO_STREAM_BLOK_WYL_TIME; + + return 0; +} + +void r87l_przelicz_1(struct r87l_logic *log) +{ + uint16_t i; + float local1, local2,b,c; + float i0[3]; + // wyliczenie pradu ronicowego i stabilizacji + //float ich[3]; + + if(log->startup_timer) + *log->comm_bits|=R87L_COMM_BLOK_WYL; + else + *log->comm_bits&=~R87L_COMM_BLOK_WYL; + +// while (log->w.nr_probki != bus_an_cur_sample_num) + { + log->w.nr_probki++; + log->w.nr_probki%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2); + + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + i0[0]=log->nast_.L[0].a_pi0[0] * (*log->nast_.L[0].Ip_prev[0] - log->nast_.L[0].a_p[0][1]) + + log->nast_.L[1].a_pi0[0] * (*log->nast_.L[1].Ip_prev[0] - log->nast_.L[1].a_p[0][1]) + + log->nast_.L[2].a_pi0[0] * (*log->nast_.L[2].Ip_prev[0] - log->nast_.L[2].a_p[0][1]); + + i0[1]=log->nast_.L[0].a_pi0[1] * (*log->nast_.L[0].Ip_prev[1] - log->nast_.L[0].a_p[1][1]); + i0[1]+=log->nast_.L[1].a_pi0[1] * (*log->nast_.L[1].Ip_prev[1] - log->nast_.L[1].a_p[1][1]); + i0[1]+=log->nast_.L[2].a_pi0[1] * (*log->nast_.L[2].Ip_prev[1] - log->nast_.L[2].a_p[1][1]); + + i0[2]=log->nast_.L[0].a_pi0[2] * (*log->nast_.L[0].Ip_prev[2] - log->nast_.L[0].a_p[2][1]) + + log->nast_.L[1].a_pi0[2] * (*log->nast_.L[1].Ip_prev[2] - log->nast_.L[1].a_p[2][1]) + + log->nast_.L[2].a_pi0[2] * (*log->nast_.L[2].Ip_prev[2] - log->nast_.L[2].a_p[2][1]); + + if(check_struct(&log->test_1str)) + { + for(i=0;i<3;i++) + { + if(i==log->my_addr) + continue; + i0[i]=0; + } + } + } + + + for (i = 0; i < 3; i++) + { + + + local1 = log->nast_.L[i].a_p[0][0] * (*log->nast_.L[i].Ip_prev[0] - log->nast_.L[i].a_p[0][1]) + - log->nast_.L[i].a_m[0][0] * (*log->nast_.L[i].Im_prev[0] - log->nast_.L[i].a_m[0][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=0) + local1=0; + + b = log->nast_.L[i].a_p[1][0] * (*log->nast_.L[i].Ip_prev[1] - log->nast_.L[i].a_p[1][1]) + - log->nast_.L[i].a_m[1][0] * (*log->nast_.L[i].Im_prev[1] - log->nast_.L[i].a_m[1][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=1) + b=0; + + c = log->nast_.L[i].a_p[2][0] * (*log->nast_.L[i].Ip_prev[2] - log->nast_.L[i].a_p[2][1]) + - log->nast_.L[i].a_m[2][0] * (*log->nast_.L[i].Im_prev[2] - log->nast_.L[i].a_m[2][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=2) + c=0; + + + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + local1-=i0[0]; + if(!log->nast_.p2d) + b-=i0[1]; + if(!log->nast_.p3d) + c-=i0[2]; + } + + local2 = b + c; + // + log->nast_.L[i].a_p[3][0] * (*log->nast_.L[i].Ip[3] - log->nast_.L[i].a_p[3][1]) + // - log->nast_.L[i].a_m[3][0] * (*log->nast_.L[i].Im[3] - log->nast_.L[i].a_m[3][1]); + +// przed zmianami plus na minus w roznicowym +// log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2; +// log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f; // /2; + + log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2; + log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f; // /2; + + + // ich[i] = fabs(log->w.buf_Lr[log->w.nr_probki][0 + i]); + + log->w.buf_ip[log->w.nr_probki][i*3 + 0] = local1; + log->w.buf_ip[log->w.nr_probki][i*3 + 1] = b; + log->w.buf_ip[log->w.nr_probki][i*3 + 2] = c; + } + +////// druga probka + log->w.nr_probki++; + log->w.nr_probki%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2); + + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + i0[0]=log->nast_.L[0].a_pi0[0] * (*log->nast_.L[0].Ip[0] - log->nast_.L[0].a_p[0][1]) + + log->nast_.L[1].a_pi0[0] * (*log->nast_.L[1].Ip[0] - log->nast_.L[1].a_p[0][1]) + + log->nast_.L[2].a_pi0[0] * (*log->nast_.L[2].Ip[0] - log->nast_.L[2].a_p[0][1]); + + i0[1]=log->nast_.L[0].a_pi0[1] * (*log->nast_.L[0].Ip[1] - log->nast_.L[0].a_p[1][1]); + i0[1]+=log->nast_.L[1].a_pi0[1] * (*log->nast_.L[1].Ip[1] - log->nast_.L[1].a_p[1][1]); + i0[1]+=log->nast_.L[2].a_pi0[1] * (*log->nast_.L[2].Ip[1] - log->nast_.L[2].a_p[1][1]); + + i0[2]=log->nast_.L[0].a_pi0[2] * (*log->nast_.L[0].Ip[2] - log->nast_.L[0].a_p[2][1]) + + log->nast_.L[1].a_pi0[2] * (*log->nast_.L[1].Ip[2] - log->nast_.L[1].a_p[2][1]) + + log->nast_.L[2].a_pi0[2] * (*log->nast_.L[2].Ip[2] - log->nast_.L[2].a_p[2][1]); + + if(check_struct(&log->test_1str)) + { + for(i=0;i<3;i++) + { + if(i==log->my_addr) + continue; + i0[i]=0; + } + } + + } + + for (i = 0; i < 3; i++) + { + + + local1 = log->nast_.L[i].a_p[0][0] * (*log->nast_.L[i].Ip[0] - log->nast_.L[i].a_p[0][1]) + - log->nast_.L[i].a_m[0][0] * (*log->nast_.L[i].Im[0] - log->nast_.L[i].a_m[0][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=0) + local1=0; + + b = log->nast_.L[i].a_p[1][0] * (*log->nast_.L[i].Ip[1] - log->nast_.L[i].a_p[1][1]) + - log->nast_.L[i].a_m[1][0] * (*log->nast_.L[i].Im[1] - log->nast_.L[i].a_m[1][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=1) + b=0; + + c = log->nast_.L[i].a_p[2][0] * (*log->nast_.L[i].Ip[2] - log->nast_.L[i].a_p[2][1]) + - log->nast_.L[i].a_m[2][0] * (*log->nast_.L[i].Im[2] - log->nast_.L[i].a_m[2][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=2) + c=0; + + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + local1-=i0[0]; + if(!log->nast_.p2d) + b-=i0[1]; + if(!log->nast_.p3d) + c-=i0[2]; + } + + local2 = b + c; + // + log->nast_.L[i].a_p[3][0] * (*log->nast_.L[i].Ip[3] - log->nast_.L[i].a_p[3][1]) + // - log->nast_.L[i].a_m[3][0] * (*log->nast_.L[i].Im[3] - log->nast_.L[i].a_m[3][1]); + +// przed zmianami plus na minus w roznicowym +// log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2; +// log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f; // /2; + + log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2; + log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f; // /2; + + + // ich[i] = fabs(log->w.buf_Lr[log->w.nr_probki][0 + i]); + + log->w.buf_ip[log->w.nr_probki][i*3 + 0] = local1; + log->w.buf_ip[log->w.nr_probki][i*3 + 1] = b; + log->w.buf_ip[log->w.nr_probki][i*3 + 2] = c; + } +//// druga probka + + if (log->nast_.r && (check_struct(&log->streams_ok) || check_struct(&log->test_1str))) // jesli wlaczone wylaczenie bezwarunkowe + { +/* + log->P_.Irr1 = r87l_kryterium1(&log->l.Ir1[0], + (ich[0] > log->nast_.Irr), + (ich[0]nast_.Ipr), + log->P_.Irr1, + c1r, c1p); + log->P_.Irr2 = r87l_kryterium1(&log->l.Ir2[0], + (ich[1]>log->nast_.Irr), + (ich[1]nast_.Ipr), + log->P_.Irr2, + c1r, c1p); + log->P_.Irr3 = r87l_kryterium1(&log->l.Ir3[0], + (ich[2]>log->nast_.Irr), + (ich[2] < log->nast_.Ipr), + log->P_.Irr3, + c1r, c1p); + */ + log->P_.Irr1 = r87l_kryterium1(&log->l.Ir1[0], + (log->w.Ir[0] > log->nast_.Irr), + (log->w.Ir[0]nast_.Ipr), + log->P_.Irr1, + c1r, c1p); + log->P_.Irr2 = r87l_kryterium1(&log->l.Ir2[0], + (log->w.Ir[1]>log->nast_.Irr), + (log->w.Ir[1]nast_.Ipr), + log->P_.Irr2, + c1r, c1p); + log->P_.Irr3 = r87l_kryterium1(&log->l.Ir3[0], + (log->w.Ir[2]>log->nast_.Irr), + (log->w.Ir[2]< log->nast_.Ipr), + log->P_.Irr3, + c1r, c1p); + + + } + else if(check_struct(&log->streams_ok) || check_struct(&log->test_1str)) + log->P_.Irr1 = log->P_.Irr2 = log->P_.Irr3 = 0; + + } + + //---------------------------------------------------------------------------------------- + + //filtracja przebiegow + + + // 1h pradu rozruchowego + + r87l_przelicz_1h(log,0); + r87l_przelicz_1h(log,1); + r87l_przelicz_1h(log,2); + + r87l_przelicz_1h_ip(log,3); + r87l_przelicz_1h_ip(log,4); + r87l_przelicz_1h_ip(log,5); + + //----------------------------------------------------------------------------------------- + + u8 war1 = log->w.Ir[0] > 0.0016 && (log->w.I2h[0] >= log->nast_.I2h_min); + u8 war2 = log->w.Ir[1] > 0.0016 && (log->w.I2h[1] >= log->nast_.I2h_min); + u8 war3 = log->w.Ir[2] > 0.0016 && (log->w.I2h[2] >= log->nast_.I2h_min); + + + log->Ir1_ = sqrtf(log->w.Ir[0]); + log->Ir2_ = sqrtf(log->w.Ir[1]); + log->Ir3_ = sqrtf(log->w.Ir[2]); + log->Ih1_ = sqrtf(log->w.Ir[3]); + log->Ih2_ = sqrtf(log->w.Ir[4]); + log->Ih3_ = sqrtf(log->w.Ir[5]); + + if (log->nast_.bl2h) + { + // druga harmoniczna + r87l_przelicz_2h(log,0); + r87l_przelicz_2h(log,1); + r87l_przelicz_2h(log,2); + } + + if (log->nast_.bl2h && (check_struct(&log->streams_ok)||check_struct(&log->test_1str))) //jesli aktywna blokada od drugiej harmonicznej + { + log->P_.Bl2h_L1 = r87l_kryterium1(&log->l.l2h1[0], + ((log->w.I2h[0] > log->nast_.khr_2h*log->w.Ir[0]) && war1), + ((log->w.I2h[0]nast_.khp_2h*log->w.Ir[0]) || !war1), + log->P_.Bl2h_L1, + c2r, c2p); + log->P_.Bl2h_L2 = r87l_kryterium1(&log->l.l2h2[0], + ((log->w.I2h[1]>log->nast_.khr_2h*log->w.Ir[1]) && war2), + ((log->w.I2h[1]nast_.khp_2h*log->w.Ir[1]) || !war2), + log->P_.Bl2h_L2, + c2r, c2p); + log->P_.Bl2h_L3 = r87l_kryterium1(&log->l.l2h3[0], + ((log->w.I2h[2]>log->nast_.khr_2h*log->w.Ir[2]) && war3), + ((log->w.I2h[2]nast_.khp_2h*log->w.Ir[2]) || !war3), + log->P_.Bl2h_L3, + c2r, c2p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl2h_L1 || log->P_.Bl2h_L2 || log->P_.Bl2h_L3) + { + log->P_.Bl2h_L1 = 1; + log->P_.Bl2h_L2 = 1; + log->P_.Bl2h_L3 = 1; + } + else + { + log->P_.Bl2h_L1 = 0; + log->P_.Bl2h_L2 = 0; + log->P_.Bl2h_L3 = 0; + } + } + } + else if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + log->P_.Bl2h_L1 = log->P_.Bl2h_L2 = log->P_.Bl2h_L3 = 0; + + war1 = log->w.Ir[0] > 0.0016 && (log->w.I5h[0] >= log->nast_.I5h_min); + war2 = log->w.Ir[1] > 0.0016 && (log->w.I5h[1] >= log->nast_.I5h_min); + war3 = log->w.Ir[2] > 0.0016 && (log->w.I5h[2] >= log->nast_.I5h_min); + + if (log->nast_.bl5h) + { + // piata harmoniczna + r87l_przelicz_5h(log,0); + r87l_przelicz_5h(log,1); + r87l_przelicz_5h(log,2); + } + if (log->nast_.bl5h && (check_struct(&log->streams_ok)||check_struct(&log->test_1str))) //jesli aktywna blokada od piatej harmonicznej + { + log->P_.Bl5h_L1 = r87l_kryterium1(&log->l.l5h1[0], + ((log->w.I5h[0]>log->nast_.khr_5h*log->w.Ir[0]) && war1), + ((log->w.I5h[0]nast_.khp_5h*log->w.Ir[0]) || !war1), + log->P_.Bl5h_L1, + c3r, c3p); + log->P_.Bl5h_L2 = r87l_kryterium1(&log->l.l5h2[0], + ((log->w.I5h[1]>log->nast_.khr_5h*log->w.Ir[1]) && war2), + ((log->w.I5h[1]nast_.khp_5h*log->w.Ir[1]) || !war2), + log->P_.Bl5h_L2, + c3r, c3p); + log->P_.Bl5h_L3 = r87l_kryterium1(&log->l.l5h3[0], + ((log->w.I5h[2]>log->nast_.khr_5h*log->w.Ir[2]) && war3), + ((log->w.I5h[2] < log->nast_.khp_5h*log->w.Ir[2]) || !war3), + log->P_.Bl5h_L3, + c3r, c3p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl5h_L1 || log->P_.Bl5h_L2 || log->P_.Bl5h_L3) + { + log->P_.Bl5h_L1 = 1; + log->P_.Bl5h_L2 = 1; + log->P_.Bl5h_L3 = 1; + } + else + { + log->P_.Bl5h_L1 = 0; + log->P_.Bl5h_L2 = 0; + log->P_.Bl5h_L3 = 0; + } + } + + } + else if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + log->P_.Bl5h_L1 = log->P_.Bl5h_L2 = log->P_.Bl5h_L3 = 0; +} + +/** +Sprawdza kryterium dzialania przekaznika +Wywolywana powinna byc po kazdej nowej probce z czestotliwoscia 1200Hz +Ustawia zmienne zawarte w structurze "stan_"" +\param w struktura danych wejsciowych algorytmlw dzialania +*/ +void r87l_przelicz_2(struct r87l_logic *log) +{ + //u8 stream_ok=1; + u8 i,j,zmiana_przeglad=0; + + if (log->nast_.sbc && (check_struct(&log->streams_ok) || check_struct(&log->test_1str))) //jesli wwlaczona czasowa blokada dzialania + { + log->dw.sbc_P = (log->Ih1_ > log->nast_.Ihdr) && (log->Ir1_ < (log->Ih1_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw1 && r87l_kryterium1(&log->l.sbc1[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw1), + c4r, log->nast_.t_sbc), + &log->Zw1); + + log->dw.sbc_P = (log->Ih2_ > log->nast_.Ihdr) && (log->Ir2_ < (log->Ih2_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw2 && r87l_kryterium1(&log->l.sbc2[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw2), + c4r, log->nast_.t_sbc), + &log->Zw2); + + log->dw.sbc_P = (log->Ih3_ > log->nast_.Ihdr) && (log->Ir3_ < (log->Ih3_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw3 && r87l_kryterium1(&log->l.sbc3[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw3), + c4r, log->nast_.t_sbc), + &log->Zw3); + } + else if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + { + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + } + + if(log->nast_.przyspiesz_dzialanie) + { + log->P_.Psf1 = r87l_kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, log->n[0]==2 ? 12 : c5r, c5p); + log->P_.Psf2 = r87l_kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, log->n[1]==2 ? 12 : c5r, c5p); + log->P_.Psf3 = r87l_kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, log->n[2]==2 ? 12 : c5r, c5p); + } + else + { + log->P_.Psf1 = r87l_kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, c5r, c5p); + log->P_.Psf2 = r87l_kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, c5r, c5p); + log->P_.Psf3 = r87l_kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, c5r, c5p); + } + //--------------------------------------------------------- + + //ustawienie wyjcia + if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + { + check_and_set_struct(log->P_.Irr1 || //pobudzenie bezwarunkowe + (log->P_.Psf1 && //prad w strefie dzialania + !check_struct(&log->Zw1) && //brak blokady czasowej + !log->P_.Bl2h_L1 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L1),//brak blokady od piatej harmonicznej) + &log->Pf[0]); + + check_and_set_struct(log->P_.Irr2 || //pobudzenie bezwarunkowe + (log->P_.Psf2 && //prad w strefie dzialania + !check_struct(&log->Zw2) && //brak blokady czasowej + !log->P_.Bl2h_L2 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L2),//brak blokady od piatej harmonicznej + &log->Pf[1]); + + check_and_set_struct(log->P_.Irr3 || //pobudzenie bezwarunkowe + (log->P_.Psf3 && //prad w strefie dzialania + !check_struct(&log->Zw3) && //brak blokady czasowej + !log->P_.Bl2h_L3 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L3),//brak blokady od piatej harmonicznej + &log->Pf[2]); + } + + u8 pob=0; + for(i=0;i<3;i++) + if(check_struct(&log->Pf[i])) + pob++; +/*if(pob>=2) zmiana 2024.07.11 zeby nie zapalal pobudzen w nieswoich fazach + for(i=0;i<3;i++) + set_struct(&log->Pf[i]); +*/ +//------------------- + + check_and_set_struct(check_struct(&log->Pf[0])||check_struct(&log->Pf[1])||check_struct(&log->Pf[2]),&log->P); + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + //*log->comm_bits|=R87L_COMM_TEST; + check_and_set_struct(check_struct(&log->test),&log->P); + check_and_set_struct(check_struct(&log->test),&log->Pf[0]); + check_and_set_struct(check_struct(&log->test),&log->Pf[1]); + check_and_set_struct(check_struct(&log->test),&log->Pf[2]); + } + else + { + /*if(*log->comm_bits & R87L_COMM_TEST) + log->startup_timer=R87L_START_DELAY; + + *log->comm_bits&=~R87L_COMM_TEST;*/ + } + + // realizacja blokady dzialania przekaznika +// if(!check_struct(&log->streams_ok)) // przeniesione z ora ponizej zeby nie rozsylac blokady przy utracie probki +// { +// clear_struct(&log->P); +// clear_struct(&log->Pf[0]); +// clear_struct(&log->Pf[1]); +// clear_struct(&log->Pf[2]); +// } + if (((log->nast_.bl && check_struct(&log->stan_bl))||check_struct(&log->nieobs))/* || czy_start()*/) + { + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + *log->comm_bits|=R87L_COMM_BLOK; + } + else + { + clear_struct(&log->blok); + if(!check_struct(&log->test_1str)) + *log->comm_bits&=~R87L_COMM_BLOK; + else + *log->comm_bits|=R87L_COMM_BLOK; + } + + //blokada z innej jednostki + u8 blok_z_zew=0; + u8 zmiana_blok_wyl=0; + + if(check_struct(&log->streams_ok)) + { + for(j=0;jnast_.stream_cnt;j++) + { + if(j==log->my_addr) + continue; + if(log->stream[j]->bits & (R87L_COMM_BLOK/*|R87L_COMM_TEST*/)) + { + blok_z_zew=1; + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + } + + + if((log->stream[j]->bits & R87L_COMM_PRZEGLAD) != (log->bits_prev[j] & R87L_COMM_PRZEGLAD)) + zmiana_przeglad=1; + if((log->stream[j]->bits & R87L_COMM_BLOK_WYL) && !(log->bits_prev[j] & R87L_COMM_BLOK_WYL) && !(log->stream[j]->bits & R87L_COMM_PRZEGLAD)) + zmiana_blok_wyl=1; + + log->bits_prev[j]=log->stream[j]->bits; + + } + + log->blok_z_zew_prev=blok_z_zew; + } + else if(log->blok_z_zew_prev) + { + blok_z_zew=1; + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + } + + if(check_struct(&log->przeglad)) + { + blok_z_zew=1; + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + + if(!(*log->comm_bits & R87L_COMM_PRZEGLAD)) + { + *log->comm_bits|=R87L_COMM_PRZEGLAD; + zmiana_przeglad=1; + } + } + else + { + if(*log->comm_bits & R87L_COMM_PRZEGLAD) + { + *log->comm_bits&=~R87L_COMM_PRZEGLAD; + zmiana_przeglad=1; + } + } + + // + + // realizacja przekaznika czasowego +//rozwijajace tutaj dopisac + + for(i=0;i<3;i++) + { + if (check_struct(&log->Pf[i])||pob>=2) + log->licz_t[i]+=LOOP_CYCLE_MS; + else + log->licz_t[i] = 0; // odliczanie czasu dzialania + +// zmieniono 08.03.2021 po uwagach z PSE + u8 bylo_zadz=0; + for(j=0;j<3;j++) + { + if(j==i) + continue; + if(check_struct(&log->Zf[j])) + bylo_zadz=1; + } +// + //sprawdzenie wlasnego zadzialania + if ((log->licz_t[i] > log->nast_.tz) || (bylo_zadz && log->nast_.rozwijajace && log->licz_t[i])) + { // sprawdzenie przekroczenia nastawionego czasu + set_struct(&log->Zf[i]); // ustawienie zadzialania po odliczeniu czasu + log->licz_t[i]-=LOOP_CYCLE_MS; + + if(bylo_zadz && log->nast_.rozwijajace) + { + set_struct(&log->Zf[0]); + set_struct(&log->Zf[1]); + set_struct(&log->Zf[2]); + log->licz_t[0]=log->nast_.tz; + log->licz_t[1]=log->nast_.tz; + log->licz_t[2]=log->nast_.tz; + } + } + else + { + clear_struct(&log->Zf[i]); + } + } + + + for(i=0;i<3;i++) + { + u8 bylo_wyl=0; + if(check_struct(&log->streams_ok) && !check_struct(&log->blok)) + { + for(j=0;jnast_.stream_cnt;j++) + { + if(j==log->my_addr) + continue; + if(log->stream[j]->bits & (R87L_COMM_W_L1<nast_.przyjmuj_zadzialania) + bylo_wyl=1; + } + } + if(check_struct(&log->Zf[i])&&log->nast_.w && !log->startup_timer) + *log->comm_bits|=(R87L_COMM_W_L1<comm_bits&=~(R87L_COMM_W_L1<Zf[i])||bylo_wyl) && log->nast_.w && !log->startup_timer,&log->Wf[i]); + } + + if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + log->no_stream_timer=0; + else + if(log->no_stream_timer<=R87L_NO_STREAM_BLOK_WYL_TIME) + log->no_stream_timer+=LOOP_CYCLE_MS; + + if(!blok_z_zew && !check_struct(&log->stan_bl) && !zmiana_przeglad && !zmiana_blok_wyl && log->no_stream_timerstartup_timer) + { + if(!check_struct(&log->P)) + { + if(log->startup_timerstartup_timer=0; + else + log->startup_timer-=LOOP_CYCLE_MS; + } + else + log->startup_timer=R87L_START_DELAY; + } + } + else + { + log->startup_timer=R87L_START_DELAY; + } + + if(log->startup_timer) + *log->comm_bits|=R87L_COMM_BLOK_WYL; + else + *log->comm_bits&=~R87L_COMM_BLOK_WYL; + + check_and_set_struct(log->startup_timer && !check_struct(&log->blok),&log->blok_w); + + if(!check_struct(&log->zezw_w1f)) + { + for(i=0;i<3;i++) + if(check_struct(&log->Wf[i])) + { + for(j=0;j<3;j++) + set_struct(&log->Wf[j]); + break; + } + } +// if (check_struct(&log->P)) +// log->dw.licz_t++; +// else +// log->dw.licz_t = 0; // odliczanie czasu dzialania + +// if (log->dw.licz_t > log->nast_.tz) +// { // sprawdzenie przekroczenia nastawionego czasu + //if (!check_struct(&log->Z)) log->dw.czas_ = time; +// set_struct(&log->Z); // ustawienie zadzialania po odliczeniu czasu +// log->dw.licz_t--; +// } +// else clear_struct(&log->Z); + // + + check_and_set_struct(check_struct(&log->Zf[0])||check_struct(&log->Zf[1])||check_struct(&log->Zf[2]),&log->Z); + check_and_set_struct(check_struct(&log->Wf[0])||check_struct(&log->Wf[1])||check_struct(&log->Wf[2]),&log->W); + + +//wystawienie pomiarow + + if(!check_struct(&log->nieobs) && (check_struct(&log->streams_ok)||check_struct(&log->test_1str))) + { + *log->Ir1 = log->w.Ir[0]; + *log->Ir2 = log->w.Ir[1]; + *log->Ir3 = log->w.Ir[2]; + *log->Ih1 = log->w.Ir[3]; + *log->Ih2 = log->w.Ir[4]; + *log->Ih3 = log->w.Ir[5]; + *log->Ir_L1_ortb = log->w.buf_Lr[log->w.nr_probki][0]; + *log->Ir_L2_ortb = log->w.buf_Lr[log->w.nr_probki][1]; + *log->Ir_L3_ortb = log->w.buf_Lr[log->w.nr_probki][2]; + *log->I2h1 = log->w.I2h[0]; + *log->I2h2 = log->w.I2h[1]; + *log->I2h3 = log->w.I2h[2]; + *log->I5h1 = log->w.I5h[0]; + *log->I5h2 = log->w.I5h[1]; + *log->I5h3 = log->w.I5h[2]; + } + else + { + *log->Ir1 = 32768.0f; + *log->Ir2 = 32768.0f; + *log->Ir3 = 32768.0f; + *log->Ih1 = 32768.0f; + *log->Ih2 = 32768.0f; + *log->Ih3 = 32768.0f; + *log->Ir_L1_ortb = 32768.0f; + *log->Ir_L2_ortb = 32768.0f; + *log->Ir_L3_ortb = 32768.0f; + *log->I2h1 = 32768.0f; + *log->I2h2 = 32768.0f; + *log->I2h3 = 32768.0f; + *log->I5h1 = 32768.0f; + *log->I5h2 = 32768.0f; + *log->I5h3 = 32768.0f; + } + +//---------------------------------------------- + check_and_set_struct(log->P_.Bl2h_L1 && log->P_.Psf1 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h1); + check_and_set_struct(log->P_.Bl2h_L2 && log->P_.Psf2 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h2); + check_and_set_struct(log->P_.Bl2h_L3 && log->P_.Psf3 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h3); + check_and_set_struct(log->P_.Bl5h_L1 && log->P_.Psf1 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h1); + check_and_set_struct(log->P_.Bl5h_L2 && log->P_.Psf2 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h2); + check_and_set_struct(log->P_.Bl5h_L3 && log->P_.Psf3 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h3); +} + + +void R87L(void *arguments, void *logic) +{ + struct r87l_logic *log = (struct r87l_logic *)logic; +// struct r87l_args *args = (struct r87l_args *)arguments; + u8 i,j; + + if(check_struct(&log->test_1str)) + log->stream[log->my_addr]=log->ownstream; + else + log->stream[log->my_addr]=log->original_stream; + + for(i=0;inast_.stream_cnt;i++) + { + for(j=0;j<3;j++) + { + if(log->stream[i]->A[j]!=32768.0f) + log->A[i][j]=log->stream[i]->A[j]; + if(log->stream[i]->A_prev[j]!=32768.0f) + log->A_prev[i][j]=log->stream[i]->A_prev[j]; + } + } + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + r87l_przelicz_1(log); + + if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + { + //Sprawdzenie pobudzenia w stabilizowanej strefie dzialania + r87l_kryterium2(log,log->Ir1_,log->Ih1_,&log->P_.Pb1,0); + r87l_kryterium2(log,log->Ir2_,log->Ih2_,&log->P_.Pb2,1); + r87l_kryterium2(log,log->Ir3_,log->Ih3_,&log->P_.Pb3,2); + } + + r87l_przelicz_2(log); + } + else + { + *log->Ir1=0; + *log->Ir2=0; + *log->Ir3=0; + *log->Ih1=0; + *log->Ih2=0; + *log->Ih3=0; + *log->Ir_L1_ortb=0; + *log->Ir_L2_ortb=0; + *log->Ir_L3_ortb=0; + *log->I2h1=0; + *log->I2h2=0; + *log->I2h3=0; + *log->I5h1=0; + *log->I5h2=0; + *log->I5h3=0; + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + clear_struct(&log->W); + clear_struct(&log->Wf[0]); + clear_struct(&log->Wf[1]); + clear_struct(&log->Wf[2]); + + clear_struct(&log->Z); + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + clear_struct(&log->BL2h1); + clear_struct(&log->BL2h2); + clear_struct(&log->BL2h3); + clear_struct(&log->BL5h1); + clear_struct(&log->BL5h2); + clear_struct(&log->BL5h3); + } + +} diff --git a/src/R87L.h b/src/R87L.h new file mode 100644 index 0000000..2c07e4f --- /dev/null +++ b/src/R87L.h @@ -0,0 +1,289 @@ +/* + * R87L.h + * + * Created on: 20-05-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef R87L_H_ +#define R87L_H_ + +#include "tdefs.h" +#include "analog_in.h" +#include "mkstream.h" +#include "Diff.h" + +#define R87L_START_DELAY 100 +#define R87L_NO_STREAM_BLOK_WYL_TIME 10000 // 10s + +#define R87L_COMM_W_L1 (1<<0) +#define R87L_COMM_W_L2 (1<<1) +#define R87L_COMM_W_L3 (1<<2) +#define R87L_COMM_BLOK (1<<3) +#define R87L_COMM_PRZEGLAD (1<<4) +#define R87L_COMM_BLOK_WYL (1<<5) + + + struct wskazniki_prad_do_kompensacji_87l + { + float /*struct we_an_diff*/ *Ip[4]; + float /*struct we_an_diff*/ *Im[4]; + float /*struct we_an_diff*/ *Ip_prev[4]; + float /*struct we_an_diff*/ *Im_prev[4]; + + float a_p[4][2]; + float a_m[4][2]; + float a_pi0[3]; + }; + + struct Nastawy_przeliczone_87 + { + float khr_2h;///< wartosc rozruchowa blokady od 2 harmonicznej + float khp_2h;///< wartosc powrotowa blokady od 2 harmonicznej + float khr_5h;///< wartosc rozruchowa blokady od 5 harmonicznej + float khp_5h;///< wartosc powrotowa blokady od 5 harmonicznej + float Irr;///< wartosc rozruchowa dzialania bezwarunkowego + float Ipr;///< wartosc powrotowa dzialania bezwarunkowego + float Ihdr;/// +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "R87L.h" +#include "R87L_8.h" +#include "analog_in.h" +#include "Diff.h" + +///// +///// +///// DOROBI PODTRZYMANIE COMM_BITS[3..7]!!! +///// +///// + +/** +Ustawia wskazniki na prady po skompensowaniu +\param typ wskazuje na podstawie nastawy odpowiednia konfiguracje pradow +\param nr_pradu okreslenie numeru przetwarzanago pradu +*/ + +static void ustaw_kompensacje_8(struct r87l_8_logic *log, enum typ_kompensacji typ, u16 nr_pradu) +{ + float *a0,*a1,*a2; + float *a0_prev,*a1_prev,*a2_prev; + switch(nr_pradu) + { + case 0: + a0 = &log->A[0][0];//&log->stream[0]->A[0]; + a1 = &log->A[0][1];//stream[0]->A[1]; + a2 = &log->A[0][2];//stream[0]->A[2]; + a0_prev = &log->A_prev[0][0];//&log->stream[0]->A[0]; + a1_prev = &log->A_prev[0][1];//stream[0]->A[1]; + a2_prev = &log->A_prev[0][2];//stream[0]->A[2]; + break; + + case 1: + a0 = &log->A[1][0];//&log->stream[1]->A[0]; + a1 = &log->A[1][1];//stream[1]->A[1]; + a2 = &log->A[1][2];//stream[1]->A[2]; + a0_prev = &log->A_prev[1][0];//&log->stream[1]->A[0]; + a1_prev = &log->A_prev[1][1];//stream[1]->A[1]; + a2_prev = &log->A_prev[1][2];//stream[1]->A[2]; + break; + case 2: + a0 = &log->A[2][0];//&log->stream[1]->A[0]; + a1 = &log->A[2][1];//stream[1]->A[1]; + a2 = &log->A[2][2];//stream[1]->A[2]; + a0_prev = &log->A_prev[2][0];//&log->stream[1]->A[0]; + a1_prev = &log->A_prev[2][1];//stream[1]->A[1]; + a2_prev = &log->A_prev[2][2];//stream[1]->A[2]; + break; + case 3: + a0 = &log->A[3][0];//&log->stream[1]->A[0]; + a1 = &log->A[3][1];//stream[1]->A[1]; + a2 = &log->A[3][2];//stream[1]->A[2]; + a0_prev = &log->A_prev[3][0];//&log->stream[1]->A[0]; + a1_prev = &log->A_prev[3][1];//stream[1]->A[1]; + a2_prev = &log->A_prev[3][2];//stream[1]->A[2]; + break; + case 4: + a0 = &log->A[4][0];//&log->stream[1]->A[0]; + a1 = &log->A[4][1];//stream[1]->A[1]; + a2 = &log->A[4][2];//stream[1]->A[2]; + a0_prev = &log->A_prev[4][0];//&log->stream[1]->A[0]; + a1_prev = &log->A_prev[4][1];//stream[1]->A[1]; + a2_prev = &log->A_prev[4][2];//stream[1]->A[2]; + break; + case 5: + a0 = &log->A[5][0];//&log->stream[1]->A[0]; + a1 = &log->A[5][1];//stream[1]->A[1]; + a2 = &log->A[5][2];//stream[1]->A[2]; + a0_prev = &log->A_prev[5][0];//&log->stream[1]->A[0]; + a1_prev = &log->A_prev[5][1];//stream[1]->A[1]; + a2_prev = &log->A_prev[5][2];//stream[1]->A[2]; + break; + case 6: + a0 = &log->A[6][0];//&log->stream[1]->A[0]; + a1 = &log->A[6][1];//stream[1]->A[1]; + a2 = &log->A[6][2];//stream[1]->A[2]; + a0_prev = &log->A_prev[6][0];//&log->stream[1]->A[0]; + a1_prev = &log->A_prev[6][1];//stream[1]->A[1]; + a2_prev = &log->A_prev[6][2];//stream[1]->A[2]; + break; + + default: // 2 and others + a0 = &log->A[7][0];//&log->stream[0]->A[0]; + a1 = &log->A[7][1];//stream[0]->A[1]; + a2 = &log->A[7][2];//stream[0]->A[2]; + a0_prev = &log->A_prev[7][0];//&log->stream[0]->A[0]; + a1_prev = &log->A_prev[7][1];//stream[0]->A[1]; + a2_prev = &log->A_prev[7][2];//stream[0]->A[2]; + break; + } + + switch (typ) + { + case zero: + log->nast_.L[0].Ip[nr_pradu] = &log->nast_.zero; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 0; + + log->nast_.L[0].Ip_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + + break; + case A: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + + break; + case B: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a1;// + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = -1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev;// + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + case C: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a0_prev; + break; + case D: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = -0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a0_prev; + break; + case E: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a1_prev; + break; + case F: + log->nast_.L[0].Ip[nr_pradu] = a0; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a1; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a2; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = -0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a1_prev; + + break; + case N: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + case O: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = -1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + + case U: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = -1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + + case V: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = &log->nast_.zero; + log->nast_.a[nr_pradu] = 1; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = &log->nast_.zero; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = &log->nast_.zero; + break; + + + case W: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a1; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a2; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a0; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a0_prev; + break; + + case X: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a2; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a0; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a1; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a1_prev; + break; + case Y: + log->nast_.L[0].Ip[nr_pradu] = a1; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a2; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a0; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a2_prev; + break; + case Z: + log->nast_.L[0].Ip[nr_pradu] = a2; + log->nast_.L[0].Im[nr_pradu] = a0; + log->nast_.L[1].Ip[nr_pradu] = a0; + log->nast_.L[1].Im[nr_pradu] = a1; + log->nast_.L[2].Ip[nr_pradu] = a1; + log->nast_.L[2].Im[nr_pradu] = a2; + log->nast_.a[nr_pradu] = 0.57735f; + + log->nast_.L[0].Ip_prev[nr_pradu] = a2_prev; + log->nast_.L[0].Im_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Ip_prev[nr_pradu] = a0_prev; + log->nast_.L[1].Im_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Ip_prev[nr_pradu] = a1_prev; + log->nast_.L[2].Im_prev[nr_pradu] = a2_prev; + break; + } +} + +void przelicz_nastawy1_r87l_8(struct r87l_8_logic *log) +{ + u16 i; + + log->jaki_uklad = tylko_T3w_D; + log->nast_.p2d=1; + log->nast_.p3d=1; + + //ustawienie skladnikow kompensacji pradow + + ustaw_kompensacje_8(log,A,0); + ustaw_kompensacje_8(log,A,1); + for(i=2;inast_.stream_cnt;i++) + ustaw_kompensacje_8(log,A,i); + for(i=log->nast_.stream_cnt;i<8;i++) + ustaw_kompensacje_8(log,zero,i); + + +/* switch (log->jaki_uklad) + { + case tylko_T2w_Y: + ustaw_kompensacje_8(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje_8(log,Y2w_ZPrAE.k[pY_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje_8(log,zero, 2); + ustaw_kompensacje_8(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= n.PT12; + log->nast_.stream_cnt=2; + break; + case tylko_T2w_D: + ustaw_kompensacje_8(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje_8(log,D2w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje_8(log,zero, 2); + ustaw_kompensacje_8(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= n.PT12; + log->nast_.stream_cnt=2; + + break; + case tylko_T3w_Y: + ustaw_kompensacje_8(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Ig, 0); + ustaw_kompensacje_8(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Id, 1); + ustaw_kompensacje_8(log,Y3w_ZPrAE.k[pY_ZPrAE[n.GrP2]][pY_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje_8(log,zero, 3); + + if(!pY_ZPrAE[n.GrP2]||!pY_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= n.PT12; + log->nast_.a[2] /= n.PT13; + log->nast_.stream_cnt=3; + + break; + case tylko_T3w_D: + ustaw_kompensacje_8(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Ig, 0); + ustaw_kompensacje_8(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP2]].Id, 1); + ustaw_kompensacje_8(log,D3w_ZPrAE.k[pD_ZPrAE[n.GrP3]].Io, 2); + ustaw_kompensacje_8(log,zero, 3); + + if(!pD_ZPrAE[n.GrP2]||!pD_ZPrAE[n.GrP3]) + set_struct(&log->nieobs); + + log->nast_.a[1] /= n.PT12; + log->nast_.a[2] /= n.PT13; + log->nast_.stream_cnt=3; + + break; + }*/ + + + //----------------------------------------- + + //przeliczenie wspolczynnikow kompensacji o korekcje i ofset + + for (i = 0; i < 3; i++) + { + + log->nast_.L[i].a_p[0][0] = log->nast_.a[0] * 1;//log->nast_.L[i].Ip[0]->params->nast.mnoznik; + //log->nast_.L[i].a_p[0][1] = /*log->nast_.L[i].Ip[0]->params->nast.offset + */32767; + log->nast_.L[i].a_m[0][0] = log->nast_.a[0] * 1;//log->nast_.L[i].Im[0]->params->nast.mnoznik; + //log->nast_.L[i].a_m[0][1] =/* log->nast_.L[i].Im[0]->params->nast.offset + */32767; + log->nast_.L[i].a_p[1][0] = log->nast_.a[1] * 1;//log->nast_.L[i].Ip[1]->params->nast.mnoznik; + //log->nast_.L[i].a_p[1][1] =/* log->nast_.L[i].Ip[1]->params->nast.offset + */32767; + log->nast_.L[i].a_m[1][0] = log->nast_.a[1] * 1;//log->nast_.L[i].Im[1]->params->nast.mnoznik; + //log->nast_.L[i].a_m[1][1] =/* log->nast_.L[i].Im[1]->params->nast.offset + */32767; + log->nast_.L[i].a_p[2][0] = log->nast_.a[2] * 1;//log->nast_.L[i].Ip[2]->params->nast.mnoznik; + //log->nast_.L[i].a_p[2][1] =/* log->nast_.L[i].Ip[2]->params->nast.offset + */32767; + log->nast_.L[i].a_m[2][0] = log->nast_.a[2] * 1;//log->nast_.L[i].Im[2]->params->nast.mnoznik; + //log->nast_.L[i].a_m[2][1] =/* log->nast_.L[i].Im[2]->params->nast.offset + */32767; + log->nast_.L[i].a_p[3][0] = log->nast_.a[3] * 1;//log->nast_.L[i].Ip[3]->params->nast.mnoznik; + //log->nast_.L[i].a_p[3][1] =/* log->nast_.L[i].Ip[3]->params->nast.offset + */32767; + log->nast_.L[i].a_m[3][0] = log->nast_.a[3] * 1;//log->nast_.L[i].Im[3]->params->nast.mnoznik; + //log->nast_.L[i].a_m[3][1] =/* log->nast_.L[i].Im[3]->params->nast.offset + */32767; + + + log->nast_.L[i].a_p[4][0] = log->nast_.a[4] * 1;//log->nast_.L[i].Ip[3]->params->nast.mnoznik; + //log->nast_.L[i].a_p[3][1] =/* log->nast_.L[i].Ip[3]->params->nast.offset + */32767; + log->nast_.L[i].a_m[4][0] = log->nast_.a[4] * 1;//log->nast_.L[i].Im[3]->params->nast.mnoznik; + //log->nast_.L[i].a_m[3][1] =/* log->nast_.L[i].Im[3]->params->nast.offset + */32767; + log->nast_.L[i].a_p[5][0] = log->nast_.a[5] * 1;//log->nast_.L[i].Ip[3]->params->nast.mnoznik; + //log->nast_.L[i].a_p[3][1] =/* log->nast_.L[i].Ip[3]->params->nast.offset + */32767; + log->nast_.L[i].a_m[5][0] = log->nast_.a[5] * 1;//log->nast_.L[i].Im[3]->params->nast.mnoznik; + //log->nast_.L[i].a_m[3][1] =/* log->nast_.L[i].Im[3]->params->nast.offset + */32767; + log->nast_.L[i].a_p[6][0] = log->nast_.a[6] * 1;//log->nast_.L[i].Ip[3]->params->nast.mnoznik; + //log->nast_.L[i].a_p[3][1] =/* log->nast_.L[i].Ip[3]->params->nast.offset + */32767; + log->nast_.L[i].a_m[6][0] = log->nast_.a[6] * 1;//log->nast_.L[i].Im[3]->params->nast.mnoznik; + //log->nast_.L[i].a_m[3][1] =/* log->nast_.L[i].Im[3]->params->nast.offset + */32767; + log->nast_.L[i].a_p[7][0] = log->nast_.a[7] * 1;//log->nast_.L[i].Ip[3]->params->nast.mnoznik; + //log->nast_.L[i].a_p[3][1] =/* log->nast_.L[i].Ip[3]->params->nast.offset + */32767; + log->nast_.L[i].a_m[7][0] = log->nast_.a[7] * 1;//log->nast_.L[i].Im[3]->params->nast.mnoznik; + //log->nast_.L[i].a_m[3][1] =/* log->nast_.L[i].Im[3]->params->nast.offset + */32767; + + + log->nast_.L[i].a_pi0[0] = log->nast_.a[0] * 0.333333f;//log->nast_.L[i].Ip[0]->params->nast.mnoznik; + log->nast_.L[i].a_pi0[1] = log->nast_.a[1] * 0.333333f;//log->nast_.L[i].Ip[1]->params->nast.mnoznik; + log->nast_.L[i].a_pi0[2] = log->nast_.a[2] * 0.333333f;//log->nast_.L[i].Ip[2]->params->nast.mnoznik; + } + + // wyznaczenie wspolczynnikow filtru + + + for (i = 0; i < MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2; i++) + { + log->nast_.wsp_filtr_sin_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_cos_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_sin2h_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_cos2h_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_sin5h_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/5*2)) * i) / (10/LOOP_CYCLE_MS*2); + log->nast_.wsp_filtr_cos5h_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/5*2)) * i) / (10/LOOP_CYCLE_MS*2); + } + + //---------------------------------------------------------------------------------- + + memset(&log->w, 0x00, sizeof(log->w)); +} + +void r87l_8_przelicz_1h_ip(struct r87l_8_logic *log, uint16_t Lnr) // prad hamujacy +{ + int16_t i, i1,j; + float orta; + float ortb; + float pr; + + log->w.Ir[Lnr]=0; + + for(j=0;j<8;j++) + { + orta=0; + ortb=0; + i1 = log->w.nr_probki; + + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2; + pr = log->w.buf_ip[i1][((Lnr-3)*8)+j]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] += sqrtf(pr * 0.5f); + } + + log->w.Ir[Lnr]*=0.5f; + log->w.Ir[Lnr]*=log->w.Ir[Lnr]; +} +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void r87l_8_przelicz_2h(struct r87l_8_logic *log,uint16_t Lnr) +{ + + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin2h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos2h_[i]; + } + + log->w.orta2h[Lnr] = orta; + log->w.ortb2h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I2h[Lnr] = pr * 0.5f; + +} + +/** +Wylicza wartosci chwilowe odfiltrowanego przebiegu 2h (ortogonalna a(sinus) i b(cosinus)) +Wpisuje wartoci ortogonalnych a do stan_.orta +Wpisuje wartoci ortogonalnych b do stan_.ortb_ +Wpisuje kwadrat amplitudy d zmiennej stan_.kwampl +\param: Lnr - numer fazy +*/ +void r87l_8_przelicz_5h(struct r87l_8_logic *log,uint16_t Lnr) +{ + short i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin5h_[i]; + ortb += pr * log->nast_.wsp_filtr_cos5h_[i]; + } + + log->w.orta5h[Lnr] = orta; + log->w.ortb5h[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.I5h[Lnr] = pr * 0.5f; +} + +int R87L_8_initlog(void *arguments, void *logic) +{ + struct r87l_8_args *args = (struct r87l_8_args *)arguments; + struct r87l_8_logic *log = (struct r87l_8_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_pointer_in_ptr(args->io.stream1_ptr_in,(u32*)&log->stream[0])) + return -1; + if(set_pointer_in_ptr(args->io.stream2_ptr_in,(u32*)&log->stream[1])) + return -1; + if(set_pointer_in_ptr(args->io.stream3_ptr_in,(u32*)&log->stream[2])) + return -1; + if(set_pointer_in_ptr(args->io.stream4_ptr_in,(u32*)&log->stream[3])) + return -1; + if(set_pointer_in_ptr(args->io.stream5_ptr_in,(u32*)&log->stream[4])) + return -1; + if(set_pointer_in_ptr(args->io.stream6_ptr_in,(u32*)&log->stream[5])) + return -1; + if(set_pointer_in_ptr(args->io.stream7_ptr_in,(u32*)&log->stream[6])) + return -1; + if(set_pointer_in_ptr(args->io.stream8_ptr_in,(u32*)&log->stream[7])) + return -1; + + + if(set_bit_ptr_struct(args->io.streams_ok_in,&log->streams_ok)) + return -1; + + if(set_bit_ptr_struct(args->io.zezw_w1f_in,&log->zezw_w1f)) + return -1; + + if(set_bit_ptr_struct(args->io.przeglad_in,&log->przeglad)) + return -1; + + if(set_bit_ptr_struct(args->io.test_1str_in,&log->test_1str)) + return -1; + + if(set_pointer_in_ptr(args->io.ownstream_ptr_in,(u32*)&log->ownstream)) + return -1; + + if(set_bit_ptr_struct(args->io.W_out,&log->W)) + return -1; + if(set_bit_ptr_struct(args->io.WL1_out,&log->Wf[0])) + return -1; + if(set_bit_ptr_struct(args->io.WL2_out,&log->Wf[1])) + return -1; + if(set_bit_ptr_struct(args->io.WL3_out,&log->Wf[2])) + return -1; + + if(set_bit_ptr_struct(args->io.Z_out,&log->Z)) + return -1; + if(set_bit_ptr_struct(args->io.ZL1_out,&log->Zf[0])) + return -1; + if(set_bit_ptr_struct(args->io.ZL2_out,&log->Zf[1])) + return -1; + if(set_bit_ptr_struct(args->io.ZL3_out,&log->Zf[2])) + return -1; + + if(set_bit_ptr_struct(args->io.P_out,&log->P)) + return -1; + + if(set_bit_ptr_struct(args->io.PL1_out,&log->Pf[0])) + return -1; + + if(set_bit_ptr_struct(args->io.PL2_out,&log->Pf[1])) + return -1; + + if(set_bit_ptr_struct(args->io.PL3_out,&log->Pf[2])) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L1_out,&log->BL2h1)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L2_out,&log->BL2h2)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_L3_out,&log->BL2h3)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L1_out,&log->BL5h1)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L2_out,&log->BL5h2)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_5h_L3_out,&log->BL5h3)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew1_out,&log->Zw1)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew2_out,&log->Zw2)) + return -1; + + if(set_bit_ptr_struct(args->io.zzew3_out,&log->Zw3)) + return -1; + + if(set_float_ptr(args->io.di_l1_float_out,&log->Ir1)) + return -1; + if(set_float_ptr(args->io.di_l2_float_out,&log->Ir2)) + return -1; + if(set_float_ptr(args->io.di_l3_float_out,&log->Ir3)) + return -1; + + if(set_float_ptr(args->io.ih_l1_float_out,&log->Ih1)) + return -1; + if(set_float_ptr(args->io.ih_l2_float_out,&log->Ih2)) + return -1; + if(set_float_ptr(args->io.ih_l3_float_out,&log->Ih3)) + return -1; + if(set_float_ptr(args->io.i2h_l1_float_out,&log->I2h1)) + return -1; + if(set_float_ptr(args->io.i2h_l2_float_out,&log->I2h2)) + return -1; + if(set_float_ptr(args->io.i2h_l3_float_out,&log->I2h3)) + return -1; + if(set_float_ptr(args->io.i5h_l1_float_out,&log->I5h1)) + return -1; + if(set_float_ptr(args->io.i5h_l2_float_out,&log->I5h2)) + return -1; + if(set_float_ptr(args->io.i5h_l3_float_out,&log->I5h3)) + return -1; + + if(set_float_ptr(args->io.ir_l1_ch_float_out,&log->Ir_L1_ortb)) + return -1; + if(set_float_ptr(args->io.ir_l2_ch_float_out,&log->Ir_L2_ortb)) + return -1; + if(set_float_ptr(args->io.ir_l3_ch_float_out,&log->Ir_L3_ortb)) + return -1; + + if(set_bit_ptr_struct(args->io.nieobs_out,&log->nieobs)) + return -1; + + if(set_bit_ptr_struct(args->io.blok_out,&log->blok)) + return -1; + + if(set_float_ptr(args->io.comm_bits_float_out,(float *)&log->comm_bits)) + return -1; + + if(set_bit_ptr_struct(args->io.blok_w_out,&log->blok_w)) + return -1; + + log->nast_.on = (args->params.bity & 0x0001)?1:0; + log->nast_.w = (args->params.bity & 0x0002)?1:0; + log->nast_.bl = (args->params.bity & 0x0004)?1:0; + log->nast_.bl2h = (args->params.bity & 0x0008)?1:0; + log->nast_.bl5h = (args->params.bity & 0x0010)?1:0; + log->nast_.sbc = (args->params.bity & 0x0020)?1:0; + log->nast_.h = (args->params.bity & 0x0040)?1:0; + log->nast_.r = (args->params.bity & 0x0080)?1:0; + log->nast_.cross_block = (args->params.bity & 0x0100)?1:0; + log->nast_.przyspiesz_dzialanie = (args->params.bity & 0x0200)?1:0; + log->nast_.przyjmuj_zadzialania = (args->params.bity & 0x0400)?1:0; + log->nast_.rozwijajace = (args->params.bity & 0x0800)?1:0; + //ustawienie wartosci rozruchowych + log->nast_.khr_2h = args->params.k_2h * args->params.k_2h; + log->nast_.khp_2h = log->nast_.khr_2h * 0.98;//args->params.kp *args->params.kp; + log->nast_.khr_5h = args->params.k_5h * args->params.k_5h; + log->nast_.khp_5h = log->nast_.khr_5h * 0.98;//args->params.kp *args->params.kp; + log->nast_.Irr = args->params.Ir * args->params.Ir;// * 0.93f // 1.3577f; + log->nast_.Ipr = args->params.Ir * args->params.kp * args->params.kp; + log->nast_.Ihdr = args->params.Ihd; + log->nast_.Irdr = args->params.rd; + log->nast_.kh1r = args->params.kh1; + log->nast_.kh1p = args->params.kh1 * args->params.kp; + log->nast_.kh2r = args->params.kh2; + log->nast_.kh2p = args->params.kh2 * args->params.kp; +// log->nast_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ir3c = args->params.Ihp * (args->params.kh1 - args->params.kh2)+ args->params.Iro; + + log->nast_.Ip3c = log->nast_.Ir3c * args->params.kp; + log->nast_.Iror = args->params.Iro; + log->nast_.I2h_min = log->nast_.Iror * log->nast_.Iror * log->nast_.khr_2h; + log->nast_.I5h_min = log->nast_.Iror * log->nast_.Iror * log->nast_.khr_5h; + log->nast_.Irop = args->params.Iro * args->params.kp; +// log->nast_.Ir4 = args->params.Ihg * args->params.kh2 + args->params.Ihp * (args->params.kh1 - args->params.kh2); + log->nast_.Ir4 = args->params.Ihp * args->params.kh1 + args->params.kh2 * (args->params.Ihg - args->params.Ihp)+ args->params.Iro; + log->nast_.Ip4 = log->nast_.Ir4 * args->params.kp; + log->nast_.t_sbc = args->params.tbld * 1000; + log->nast_.Ih1 = args->params.Iro / args->params.kh1; + log->nast_.Ihp = args->params.Ihp; + log->nast_.Ihg = args->params.Ihg; + + log->nast_.tz = args->params.t * 1000; + log->nast_.zero_frame.A[0] = 0.0f; + log->nast_.zero_frame.A[1] = 0.0f; + log->nast_.zero_frame.A[2] = 0.0f; + log->nast_.zero=0.0f; + + log->nast_.stream_cnt=args->params.liczba_jednostek+2; + if(log->nast_.stream_cnt>8) + log->nast_.stream_cnt=0; + + // +// struct nast_trafo n1; +/* n1.GrP1 = args->params.GrP1; + n1.GrP2 = args->params.GrP2; + n1.GrP3 = args->params.GrP3; + n1.PT12 = args->params.PT12; + n1.PT13 = args->params.PT13;*/ +// n1.Tr_zas = args->params.Tr_zas; + + przelicz_nastawy1_r87l_8(log); + *log->comm_bits=0; + + log->my_addr=args->params.adres; + log->original_stream=log->stream[log->my_addr]; + + log->startup_timer=R87L_START_DELAY; + log->no_stream_timer=R87L_NO_STREAM_BLOK_WYL_TIME; + + return 0; +} + +void r87l_8_przelicz_1h(struct r87l_8_logic *log, uint16_t Lnr) +{ + int16_t i, i1; + float orta = 0; + float ortb = 0; + float pr; + i1 = log->w.nr_probki; + for (i = 0; i < (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2); i++) + { + i1--; + if (i1 < 0) i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2)*2; + pr = log->w.buf_Lr[i1][Lnr]; + orta += pr * log->nast_.wsp_filtr_sin_[i]; + ortb += pr * log->nast_.wsp_filtr_cos_[i]; + } + + log->w.orta[Lnr] = orta; + log->w.ortb[Lnr] = ortb; + + pr = orta * orta + ortb * ortb; + log->w.Ir[Lnr] = pr * 0.5f; +} + +void r87l_8_przelicz_1(struct r87l_8_logic *log) +{ + uint16_t i,j; + float local1, local2,b,c,rest[5]; +// float i0[3]; + // wyliczenie pradu ronicowego i stabilizacji + //float ich[3]; + + if(log->startup_timer) + *log->comm_bits|=R87L_COMM_BLOK_WYL; + else + *log->comm_bits&=~R87L_COMM_BLOK_WYL; + +// while (log->w.nr_probki != bus_an_cur_sample_num) + { + log->w.nr_probki++; + log->w.nr_probki%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2); +/* + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + i0[0]=log->nast_.L[0].a_pi0[0] * (*log->nast_.L[0].Ip_prev[0] - log->nast_.L[0].a_p[0][1]) + + log->nast_.L[1].a_pi0[0] * (*log->nast_.L[1].Ip_prev[0] - log->nast_.L[1].a_p[0][1]) + + log->nast_.L[2].a_pi0[0] * (*log->nast_.L[2].Ip_prev[0] - log->nast_.L[2].a_p[0][1]); + + i0[1]=log->nast_.L[0].a_pi0[1] * (*log->nast_.L[0].Ip_prev[1] - log->nast_.L[0].a_p[1][1]); + i0[1]+=log->nast_.L[1].a_pi0[1] * (*log->nast_.L[1].Ip_prev[1] - log->nast_.L[1].a_p[1][1]); + i0[1]+=log->nast_.L[2].a_pi0[1] * (*log->nast_.L[2].Ip_prev[1] - log->nast_.L[2].a_p[1][1]); + + i0[2]=log->nast_.L[0].a_pi0[2] * (*log->nast_.L[0].Ip_prev[2] - log->nast_.L[0].a_p[2][1]) + + log->nast_.L[1].a_pi0[2] * (*log->nast_.L[1].Ip_prev[2] - log->nast_.L[1].a_p[2][1]) + + log->nast_.L[2].a_pi0[2] * (*log->nast_.L[2].Ip_prev[2] - log->nast_.L[2].a_p[2][1]); + }*/ + + for (i = 0; i < 3; i++) + { + + + local1 = log->nast_.L[i].a_p[0][0] * (*log->nast_.L[i].Ip_prev[0] - log->nast_.L[i].a_p[0][1]) + - log->nast_.L[i].a_m[0][0] * (*log->nast_.L[i].Im_prev[0] - log->nast_.L[i].a_m[0][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=0) + local1=0; + + b = log->nast_.L[i].a_p[1][0] * (*log->nast_.L[i].Ip_prev[1] - log->nast_.L[i].a_p[1][1]) + - log->nast_.L[i].a_m[1][0] * (*log->nast_.L[i].Im_prev[1] - log->nast_.L[i].a_m[1][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=1) + b=0; + + c = log->nast_.L[i].a_p[2][0] * (*log->nast_.L[i].Ip_prev[2] - log->nast_.L[i].a_p[2][1]) + - log->nast_.L[i].a_m[2][0] * (*log->nast_.L[i].Im_prev[2] - log->nast_.L[i].a_m[2][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=2) + c=0; + + // rest + for(j=3;j<8;j++) // dosumowac reszte + { + rest[j-3]=log->nast_.L[i].a_p[j][0] * (*log->nast_.L[i].Ip_prev[j] - log->nast_.L[i].a_p[j][1]) + - log->nast_.L[i].a_m[j][0] * (*log->nast_.L[i].Im_prev[j] - log->nast_.L[i].a_m[j][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=j) + rest[j-3]=0; + } +/* + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + local1-=i0[0]; + if(!log->nast_.p2d) + b-=i0[1]; + if(!log->nast_.p3d) + c-=i0[2]; + }*/ + + local2 = b + c; + // + log->nast_.L[i].a_p[3][0] * (*log->nast_.L[i].Ip[3] - log->nast_.L[i].a_p[3][1]) + // - log->nast_.L[i].a_m[3][0] * (*log->nast_.L[i].Im[3] - log->nast_.L[i].a_m[3][1]); + +// przed zmianami plus na minus w roznicowym +// log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2; +// log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f; // /2; + + log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2; + for(j=0;j<5;j++) + log->w.buf_Lr[log->w.nr_probki][0 + i] += rest[j]; + + log->w.buf_Lr[log->w.nr_probki][3 + i] = log->w.buf_Lr[log->w.nr_probki][0 + i] * 0.5f; // /2; + + + // ich[i] = fabs(log->w.buf_Lr[log->w.nr_probki][0 + i]); + + log->w.buf_ip[log->w.nr_probki][i*8 + 0] = local1; + log->w.buf_ip[log->w.nr_probki][i*8 + 1] = b; + log->w.buf_ip[log->w.nr_probki][i*8 + 2] = c; + log->w.buf_ip[log->w.nr_probki][i*8 + 3] = rest[0]; + log->w.buf_ip[log->w.nr_probki][i*8 + 4] = rest[1]; + log->w.buf_ip[log->w.nr_probki][i*8 + 5] = rest[2]; + log->w.buf_ip[log->w.nr_probki][i*8 + 6] = rest[3]; + log->w.buf_ip[log->w.nr_probki][i*8 + 7] = rest[4]; + } + +////// druga probka + log->w.nr_probki++; + log->w.nr_probki%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2*2); +/* + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + i0[0]=log->nast_.L[0].a_pi0[0] * (*log->nast_.L[0].Ip[0] - log->nast_.L[0].a_p[0][1]) + + log->nast_.L[1].a_pi0[0] * (*log->nast_.L[1].Ip[0] - log->nast_.L[1].a_p[0][1]) + + log->nast_.L[2].a_pi0[0] * (*log->nast_.L[2].Ip[0] - log->nast_.L[2].a_p[0][1]); + + i0[1]=log->nast_.L[0].a_pi0[1] * (*log->nast_.L[0].Ip[1] - log->nast_.L[0].a_p[1][1]); + i0[1]+=log->nast_.L[1].a_pi0[1] * (*log->nast_.L[1].Ip[1] - log->nast_.L[1].a_p[1][1]); + i0[1]+=log->nast_.L[2].a_pi0[1] * (*log->nast_.L[2].Ip[1] - log->nast_.L[2].a_p[1][1]); + + i0[2]=log->nast_.L[0].a_pi0[2] * (*log->nast_.L[0].Ip[2] - log->nast_.L[0].a_p[2][1]) + + log->nast_.L[1].a_pi0[2] * (*log->nast_.L[1].Ip[2] - log->nast_.L[1].a_p[2][1]) + + log->nast_.L[2].a_pi0[2] * (*log->nast_.L[2].Ip[2] - log->nast_.L[2].a_p[2][1]); + } +*/ + for (i = 0; i < 3; i++) + { + + + local1 = log->nast_.L[i].a_p[0][0] * (*log->nast_.L[i].Ip[0] - log->nast_.L[i].a_p[0][1]) + - log->nast_.L[i].a_m[0][0] * (*log->nast_.L[i].Im[0] - log->nast_.L[i].a_m[0][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=0) + local1=0; + + b = log->nast_.L[i].a_p[1][0] * (*log->nast_.L[i].Ip[1] - log->nast_.L[i].a_p[1][1]) + - log->nast_.L[i].a_m[1][0] * (*log->nast_.L[i].Im[1] - log->nast_.L[i].a_m[1][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=1) + b=0; + + c = log->nast_.L[i].a_p[2][0] * (*log->nast_.L[i].Ip[2] - log->nast_.L[i].a_p[2][1]) + - log->nast_.L[i].a_m[2][0] * (*log->nast_.L[i].Im[2] - log->nast_.L[i].a_m[2][1]); + + if(check_struct(&log->test_1str) && log->my_addr!=2) + c=0; + + // rest + for(j=3;j<8;j++) // dosumowac reszte + { + rest[j-3]=log->nast_.L[i].a_p[j][0] * (*log->nast_.L[i].Ip[j] - log->nast_.L[i].a_p[j][1]) + - log->nast_.L[i].a_m[j][0] * (*log->nast_.L[i].Im[j] - log->nast_.L[i].a_m[j][1]); + if(check_struct(&log->test_1str) && log->my_addr!=j) + rest[j-3]=0; + + } + + /* + if(log->jaki_uklad==tylko_T2w_Y || log->jaki_uklad==tylko_T3w_Y) + { + local1-=i0[0]; + if(!log->nast_.p2d) + b-=i0[1]; + if(!log->nast_.p3d) + c-=i0[2]; + } +*/ + local2 = b + c; + // + log->nast_.L[i].a_p[3][0] * (*log->nast_.L[i].Ip[3] - log->nast_.L[i].a_p[3][1]) + // - log->nast_.L[i].a_m[3][0] * (*log->nast_.L[i].Im[3] - log->nast_.L[i].a_m[3][1]); + +// przed zmianami plus na minus w roznicowym +// log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 - local2; +// log->w.buf_Lr[log->w.nr_probki][3 + i] = (local1 + local2) * 0.5f; // /2; + + log->w.buf_Lr[log->w.nr_probki][0 + i] = local1 + local2; + for(j=0;j<5;j++) + log->w.buf_Lr[log->w.nr_probki][0 + i] += rest[j]; + + log->w.buf_Lr[log->w.nr_probki][3 + i] = log->w.buf_Lr[log->w.nr_probki][0 + i] * 0.5f; // /2; + + + // ich[i] = fabs(log->w.buf_Lr[log->w.nr_probki][0 + i]); + + log->w.buf_ip[log->w.nr_probki][i*8 + 0] = local1; + log->w.buf_ip[log->w.nr_probki][i*8 + 1] = b; + log->w.buf_ip[log->w.nr_probki][i*8 + 2] = c; + log->w.buf_ip[log->w.nr_probki][i*8 + 3] = rest[0]; + log->w.buf_ip[log->w.nr_probki][i*8 + 4] = rest[1]; + log->w.buf_ip[log->w.nr_probki][i*8 + 5] = rest[2]; + log->w.buf_ip[log->w.nr_probki][i*8 + 6] = rest[3]; + log->w.buf_ip[log->w.nr_probki][i*8 + 7] = rest[4]; + + } +//// druga probka + + if (log->nast_.r && (check_struct(&log->streams_ok)|| check_struct(&log->test_1str)) ) // jesli wlaczone wylaczenie bezwarunkowe + { +/* + log->P_.Irr1 = r87l_kryterium1(&log->l.Ir1[0], + (ich[0] > log->nast_.Irr), + (ich[0]nast_.Ipr), + log->P_.Irr1, + c1r, c1p); + log->P_.Irr2 = r87l_kryterium1(&log->l.Ir2[0], + (ich[1]>log->nast_.Irr), + (ich[1]nast_.Ipr), + log->P_.Irr2, + c1r, c1p); + log->P_.Irr3 = r87l_kryterium1(&log->l.Ir3[0], + (ich[2]>log->nast_.Irr), + (ich[2] < log->nast_.Ipr), + log->P_.Irr3, + c1r, c1p); + */ + log->P_.Irr1 = r87l_kryterium1(&log->l.Ir1[0], + (log->w.Ir[0] > log->nast_.Irr), + (log->w.Ir[0]nast_.Ipr), + log->P_.Irr1, + c1r, c1p); + log->P_.Irr2 = r87l_kryterium1(&log->l.Ir2[0], + (log->w.Ir[1]>log->nast_.Irr), + (log->w.Ir[1]nast_.Ipr), + log->P_.Irr2, + c1r, c1p); + log->P_.Irr3 = r87l_kryterium1(&log->l.Ir3[0], + (log->w.Ir[2]>log->nast_.Irr), + (log->w.Ir[2]< log->nast_.Ipr), + log->P_.Irr3, + c1r, c1p); + + + } + else if(check_struct(&log->streams_ok) || check_struct(&log->test_1str)) + log->P_.Irr1 = log->P_.Irr2 = log->P_.Irr3 = 0; + + } + + //---------------------------------------------------------------------------------------- + + //filtracja przebiegow + + + // 1h pradu rozruchowego + + r87l_8_przelicz_1h(log,0); + r87l_8_przelicz_1h(log,1); + r87l_8_przelicz_1h(log,2); + + r87l_8_przelicz_1h_ip(log,3); + r87l_8_przelicz_1h_ip(log,4); + r87l_8_przelicz_1h_ip(log,5); + + //----------------------------------------------------------------------------------------- + + u8 war1 = log->w.Ir[0] > 0.0016 && (log->w.I2h[0] >= log->nast_.I2h_min); + u8 war2 = log->w.Ir[1] > 0.0016 && (log->w.I2h[1] >= log->nast_.I2h_min); + u8 war3 = log->w.Ir[2] > 0.0016 && (log->w.I2h[2] >= log->nast_.I2h_min); + + + log->Ir1_ = sqrtf(log->w.Ir[0]); + log->Ir2_ = sqrtf(log->w.Ir[1]); + log->Ir3_ = sqrtf(log->w.Ir[2]); + log->Ih1_ = sqrtf(log->w.Ir[3]); + log->Ih2_ = sqrtf(log->w.Ir[4]); + log->Ih3_ = sqrtf(log->w.Ir[5]); + + if (log->nast_.bl2h) + { + // druga harmoniczna + r87l_8_przelicz_2h(log,0); + r87l_8_przelicz_2h(log,1); + r87l_8_przelicz_2h(log,2); + } + + if (log->nast_.bl2h && (check_struct(&log->streams_ok)|| check_struct(&log->test_1str))) //jesli aktywna blokada od drugiej harmonicznej + { + log->P_.Bl2h_L1 = r87l_kryterium1(&log->l.l2h1[0], + ((log->w.I2h[0] > log->nast_.khr_2h*log->w.Ir[0]) && war1), + ((log->w.I2h[0]nast_.khp_2h*log->w.Ir[0]) || !war1), + log->P_.Bl2h_L1, + c2r, c2p); + log->P_.Bl2h_L2 = r87l_kryterium1(&log->l.l2h2[0], + ((log->w.I2h[1]>log->nast_.khr_2h*log->w.Ir[1]) && war2), + ((log->w.I2h[1]nast_.khp_2h*log->w.Ir[1]) || !war2), + log->P_.Bl2h_L2, + c2r, c2p); + log->P_.Bl2h_L3 = r87l_kryterium1(&log->l.l2h3[0], + ((log->w.I2h[2]>log->nast_.khr_2h*log->w.Ir[2]) && war3), + ((log->w.I2h[2]nast_.khp_2h*log->w.Ir[2]) || !war3), + log->P_.Bl2h_L3, + c2r, c2p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl2h_L1 || log->P_.Bl2h_L2 || log->P_.Bl2h_L3) + { + log->P_.Bl2h_L1 = 1; + log->P_.Bl2h_L2 = 1; + log->P_.Bl2h_L3 = 1; + } + else + { + log->P_.Bl2h_L1 = 0; + log->P_.Bl2h_L2 = 0; + log->P_.Bl2h_L3 = 0; + } + } + } + else if(check_struct(&log->streams_ok)|| check_struct(&log->test_1str)) + log->P_.Bl2h_L1 = log->P_.Bl2h_L2 = log->P_.Bl2h_L3 = 0; + + war1 = log->w.Ir[0] > 0.0016 && (log->w.I5h[0] >= log->nast_.I5h_min); + war2 = log->w.Ir[1] > 0.0016 && (log->w.I5h[1] >= log->nast_.I5h_min); + war3 = log->w.Ir[2] > 0.0016 && (log->w.I5h[2] >= log->nast_.I5h_min); + + if (log->nast_.bl5h) + { + // piata harmoniczna + r87l_8_przelicz_5h(log,0); + r87l_8_przelicz_5h(log,1); + r87l_8_przelicz_5h(log,2); + } + if (log->nast_.bl5h && (check_struct(&log->streams_ok)|| check_struct(&log->test_1str))) //jesli aktywna blokada od piatej harmonicznej + { + log->P_.Bl5h_L1 = r87l_kryterium1(&log->l.l5h1[0], + ((log->w.I5h[0]>log->nast_.khr_5h*log->w.Ir[0]) && war1), + ((log->w.I5h[0]nast_.khp_5h*log->w.Ir[0]) || !war1), + log->P_.Bl5h_L1, + c3r, c3p); + log->P_.Bl5h_L2 = r87l_kryterium1(&log->l.l5h2[0], + ((log->w.I5h[1]>log->nast_.khr_5h*log->w.Ir[1]) && war2), + ((log->w.I5h[1]nast_.khp_5h*log->w.Ir[1]) || !war2), + log->P_.Bl5h_L2, + c3r, c3p); + log->P_.Bl5h_L3 = r87l_kryterium1(&log->l.l5h3[0], + ((log->w.I5h[2]>log->nast_.khr_5h*log->w.Ir[2]) && war3), + ((log->w.I5h[2] < log->nast_.khp_5h*log->w.Ir[2]) || !war3), + log->P_.Bl5h_L3, + c3r, c3p); + + if (log->nast_.cross_block != 0) + { + if (log->P_.Bl5h_L1 || log->P_.Bl5h_L2 || log->P_.Bl5h_L3) + { + log->P_.Bl5h_L1 = 1; + log->P_.Bl5h_L2 = 1; + log->P_.Bl5h_L3 = 1; + } + else + { + log->P_.Bl5h_L1 = 0; + log->P_.Bl5h_L2 = 0; + log->P_.Bl5h_L3 = 0; + } + } + + } + else if(check_struct(&log->streams_ok)|| check_struct(&log->test_1str)) + log->P_.Bl5h_L1 = log->P_.Bl5h_L2 = log->P_.Bl5h_L3 = 0; +} + +/** +Sprawdza kryterium dzialania przekaznika +Wywolywana powinna byc po kazdej nowej probce z czestotliwoscia 1200Hz +Ustawia zmienne zawarte w structurze "stan_"" +\param w struktura danych wejsciowych algorytmlw dzialania +*/ +void r87l_8_przelicz_2(struct r87l_8_logic *log) +{ + //u8 stream_ok=1; + u8 i,j,zmiana_przeglad=0; + + if (log->nast_.sbc && (check_struct(&log->streams_ok)|| check_struct(&log->test_1str))) //jesli wwlaczona czasowa blokada dzialania + { + log->dw.sbc_P = (log->Ih1_ > log->nast_.Ihdr) && (log->Ir1_ < (log->Ih1_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw1 && r87l_kryterium1(&log->l.sbc1[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw1), + c4r, log->nast_.t_sbc), + &log->Zw1); + + log->dw.sbc_P = (log->Ih2_ > log->nast_.Ihdr) && (log->Ir2_ < (log->Ih2_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw2 && r87l_kryterium1(&log->l.sbc2[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw2), + c4r, log->nast_.t_sbc), + &log->Zw2); + + log->dw.sbc_P = (log->Ih3_ > log->nast_.Ihdr) && (log->Ir3_ < (log->Ih3_ * log->nast_.kh1r - log->nast_.Irdr)); + check_and_set_struct(log->P_.BlZw3 && r87l_kryterium1(&log->l.sbc3[0], + log->dw.sbc_P, + !log->dw.sbc_P, + check_struct(&log->Zw3), + c4r, log->nast_.t_sbc), + &log->Zw3); + } + else if(check_struct(&log->streams_ok)|| check_struct(&log->test_1str)) + { + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + } + + if(log->nast_.przyspiesz_dzialanie) + { + log->P_.Psf1 = r87l_kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, log->n[0]==2 ? 12 : c5r, c5p); + log->P_.Psf2 = r87l_kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, log->n[1]==2 ? 12 : c5r, c5p); + log->P_.Psf3 = r87l_kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, log->n[2]==2 ? 12 : c5r, c5p); + } + else + { + log->P_.Psf1 = r87l_kryterium1(&log->l.chs1[0], log->P_.Pb1, log->P_.Ob1, log->P_.Psf1, c5r, c5p); + log->P_.Psf2 = r87l_kryterium1(&log->l.chs2[0], log->P_.Pb2, log->P_.Ob2, log->P_.Psf2, c5r, c5p); + log->P_.Psf3 = r87l_kryterium1(&log->l.chs3[0], log->P_.Pb3, log->P_.Ob3, log->P_.Psf3, c5r, c5p); + } + //--------------------------------------------------------- + + //ustawienie wyjcia + if(check_struct(&log->streams_ok)|| check_struct(&log->test_1str)) + { + check_and_set_struct(log->P_.Irr1 || //pobudzenie bezwarunkowe + (log->P_.Psf1 && //prad w strefie dzialania + !check_struct(&log->Zw1) && //brak blokady czasowej + !log->P_.Bl2h_L1 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L1),//brak blokady od piatej harmonicznej) + &log->Pf[0]); + + check_and_set_struct(log->P_.Irr2 || //pobudzenie bezwarunkowe + (log->P_.Psf2 && //prad w strefie dzialania + !check_struct(&log->Zw2) && //brak blokady czasowej + !log->P_.Bl2h_L2 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L2),//brak blokady od piatej harmonicznej + &log->Pf[1]); + + check_and_set_struct(log->P_.Irr3 || //pobudzenie bezwarunkowe + (log->P_.Psf3 && //prad w strefie dzialania + !check_struct(&log->Zw3) && //brak blokady czasowej + !log->P_.Bl2h_L3 && //brak blokady od drugiej harmonicznej + !log->P_.Bl5h_L3),//brak blokady od piatej harmonicznej + &log->Pf[2]); + } + + u8 pob=0; + for(i=0;i<3;i++) + if(check_struct(&log->Pf[i])) + pob++; +// if(pob>=2) +// for(i=0;i<3;i++) +// set_struct(&log->Pf[i]); + +//------------------- + + check_and_set_struct(check_struct(&log->Pf[0])||check_struct(&log->Pf[1])||check_struct(&log->Pf[2]),&log->P); + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + //*log->comm_bits|=R87L_COMM_TEST; + check_and_set_struct(check_struct(&log->test),&log->P); + check_and_set_struct(check_struct(&log->test),&log->Pf[0]); + check_and_set_struct(check_struct(&log->test),&log->Pf[1]); + check_and_set_struct(check_struct(&log->test),&log->Pf[2]); + } + else + { + /*if(*log->comm_bits & R87L_COMM_TEST) + log->startup_timer=R87L_START_DELAY; + + *log->comm_bits&=~R87L_COMM_TEST;*/ + } + + // realizacja blokady dzialania przekaznika +// if(!check_struct(&log->streams_ok)) // przeniesione z ora ponizej zeby nie rozsylac blokady przy utracie probki +// { +// clear_struct(&log->P); +// clear_struct(&log->Pf[0]); +// clear_struct(&log->Pf[1]); +// clear_struct(&log->Pf[2]); +// } + if (((log->nast_.bl && check_struct(&log->stan_bl))||check_struct(&log->nieobs))/* || czy_start()*/) + { + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + *log->comm_bits|=R87L_COMM_BLOK; + } + else + { + clear_struct(&log->blok); + if(!check_struct(&log->test_1str)) + *log->comm_bits&=~R87L_COMM_BLOK; + else + *log->comm_bits|=R87L_COMM_BLOK; + } + + //blokada z innej jednostki + u8 blok_z_zew=0; + u8 zmiana_blok_wyl=0; + + if(check_struct(&log->streams_ok)) + { + for(j=0;jnast_.stream_cnt;j++) + { + if(j==log->my_addr) + continue; + if(log->stream[j]->bits & (R87L_COMM_BLOK/*|R87L_COMM_TEST*/)) + { + blok_z_zew=1; + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + } + + + if((log->stream[j]->bits & R87L_COMM_PRZEGLAD) != (log->bits_prev[j] & R87L_COMM_PRZEGLAD)) + zmiana_przeglad=1; + if((log->stream[j]->bits & R87L_COMM_BLOK_WYL) && !(log->bits_prev[j] & R87L_COMM_BLOK_WYL) && !(log->stream[j]->bits & R87L_COMM_PRZEGLAD)) + zmiana_blok_wyl=1; + + log->bits_prev[j]=log->stream[j]->bits; + + } + + log->blok_z_zew_prev=blok_z_zew; + } + else if(log->blok_z_zew_prev) + { + blok_z_zew=1; + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + } + + if(check_struct(&log->przeglad)) + { + blok_z_zew=1; + set_struct(&log->blok); + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + pob=0; + + if(!(*log->comm_bits & R87L_COMM_PRZEGLAD)) + { + *log->comm_bits|=R87L_COMM_PRZEGLAD; + zmiana_przeglad=1; + } + } + else + { + if(*log->comm_bits & R87L_COMM_PRZEGLAD) + { + *log->comm_bits&=~R87L_COMM_PRZEGLAD; + zmiana_przeglad=1; + } + } + + // + + // realizacja przekaznika czasowego +//rozwijajace tutaj dopisac + + for(i=0;i<3;i++) + { + if (check_struct(&log->Pf[i]) || pob>=2) + log->licz_t[i]+=LOOP_CYCLE_MS; + else + log->licz_t[i] = 0; // odliczanie czasu dzialania + +// zmieniono 08.03.2021 po uwagach z PSE + u8 bylo_zadz=0; + for(j=0;j<3;j++) + { + if(j==i) + continue; + if(check_struct(&log->Zf[j])) + bylo_zadz=1; + } +// + //sprawdzenie wlasnego zadzialania + if ((log->licz_t[i] > log->nast_.tz) || (bylo_zadz && log->nast_.rozwijajace && log->licz_t[i])) + { // sprawdzenie przekroczenia nastawionego czasu + set_struct(&log->Zf[i]); // ustawienie zadzialania po odliczeniu czasu + log->licz_t[i]-=LOOP_CYCLE_MS; + + if(bylo_zadz && log->nast_.rozwijajace) + { + set_struct(&log->Zf[0]); + set_struct(&log->Zf[1]); + set_struct(&log->Zf[2]); + log->licz_t[0]=log->nast_.tz; + log->licz_t[1]=log->nast_.tz; + log->licz_t[2]=log->nast_.tz; + } + } + else + { + clear_struct(&log->Zf[i]); + } + } + + + for(i=0;i<3;i++) + { + u8 bylo_wyl=0; + if(check_struct(&log->streams_ok) && !check_struct(&log->blok)) + { + for(j=0;jnast_.stream_cnt;j++) + { + if(j==log->my_addr) + continue; + if(log->stream[j]->bits & (R87L_COMM_W_L1<nast_.przyjmuj_zadzialania) + bylo_wyl=1; + } + } + if(check_struct(&log->Zf[i])&&log->nast_.w && !log->startup_timer) + *log->comm_bits|=(R87L_COMM_W_L1<comm_bits&=~(R87L_COMM_W_L1<Zf[i])||bylo_wyl) && log->nast_.w && !log->startup_timer,&log->Wf[i]); + } + + if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + log->no_stream_timer=0; + else + if(log->no_stream_timer<=R87L_NO_STREAM_BLOK_WYL_TIME) + log->no_stream_timer+=LOOP_CYCLE_MS; + + if(!blok_z_zew && !check_struct(&log->stan_bl) && !zmiana_przeglad && !zmiana_blok_wyl && log->no_stream_timerstartup_timer) + { + if(!check_struct(&log->P)) + { + if(log->startup_timerstartup_timer=0; + else + log->startup_timer-=LOOP_CYCLE_MS; + } + else + log->startup_timer=R87L_START_DELAY; + } + } + else + { + log->startup_timer=R87L_START_DELAY; + } + + if(log->startup_timer) + *log->comm_bits|=R87L_COMM_BLOK_WYL; + else + *log->comm_bits&=~R87L_COMM_BLOK_WYL; + + check_and_set_struct(log->startup_timer && !check_struct(&log->blok),&log->blok_w); + + if(!check_struct(&log->zezw_w1f)) + { + for(i=0;i<3;i++) + if(check_struct(&log->Wf[i])) + { + for(j=0;j<3;j++) + set_struct(&log->Wf[j]); + break; + } + } +// if (check_struct(&log->P)) +// log->dw.licz_t++; +// else +// log->dw.licz_t = 0; // odliczanie czasu dzialania + +// if (log->dw.licz_t > log->nast_.tz) +// { // sprawdzenie przekroczenia nastawionego czasu + //if (!check_struct(&log->Z)) log->dw.czas_ = time; +// set_struct(&log->Z); // ustawienie zadzialania po odliczeniu czasu +// log->dw.licz_t--; +// } +// else clear_struct(&log->Z); + // + + check_and_set_struct(check_struct(&log->Zf[0])||check_struct(&log->Zf[1])||check_struct(&log->Zf[2]),&log->Z); + check_and_set_struct(check_struct(&log->Wf[0])||check_struct(&log->Wf[1])||check_struct(&log->Wf[2]),&log->W); + + +//wystawienie pomiarow + + if(!check_struct(&log->nieobs) && (check_struct(&log->streams_ok)||check_struct(&log->test_1str))) + { + *log->Ir1 = log->w.Ir[0]; + *log->Ir2 = log->w.Ir[1]; + *log->Ir3 = log->w.Ir[2]; + *log->Ih1 = log->w.Ir[3]; + *log->Ih2 = log->w.Ir[4]; + *log->Ih3 = log->w.Ir[5]; + *log->Ir_L1_ortb = log->w.buf_Lr[log->w.nr_probki][0]; + *log->Ir_L2_ortb = log->w.buf_Lr[log->w.nr_probki][1]; + *log->Ir_L3_ortb = log->w.buf_Lr[log->w.nr_probki][2]; + *log->I2h1 = log->w.I2h[0]; + *log->I2h2 = log->w.I2h[1]; + *log->I2h3 = log->w.I2h[2]; + *log->I5h1 = log->w.I5h[0]; + *log->I5h2 = log->w.I5h[1]; + *log->I5h3 = log->w.I5h[2]; + } + else + { + *log->Ir1 = 32768.0f; + *log->Ir2 = 32768.0f; + *log->Ir3 = 32768.0f; + *log->Ih1 = 32768.0f; + *log->Ih2 = 32768.0f; + *log->Ih3 = 32768.0f; + *log->Ir_L1_ortb = 32768.0f; + *log->Ir_L2_ortb = 32768.0f; + *log->Ir_L3_ortb = 32768.0f; + *log->I2h1 = 32768.0f; + *log->I2h2 = 32768.0f; + *log->I2h3 = 32768.0f; + *log->I5h1 = 32768.0f; + *log->I5h2 = 32768.0f; + *log->I5h3 = 32768.0f; + } + +//---------------------------------------------- + check_and_set_struct(log->P_.Bl2h_L1 && log->P_.Psf1 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h1); + check_and_set_struct(log->P_.Bl2h_L2 && log->P_.Psf2 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h2); + check_and_set_struct(log->P_.Bl2h_L3 && log->P_.Psf3 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL2h3); + check_and_set_struct(log->P_.Bl5h_L1 && log->P_.Psf1 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h1); + check_and_set_struct(log->P_.Bl5h_L2 && log->P_.Psf2 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h2); + check_and_set_struct(log->P_.Bl5h_L3 && log->P_.Psf3 && (!(*log->comm_bits & R87L_COMM_BLOK)||check_struct(&log->streams_ok)) && !blok_z_zew,&log->BL5h3); +} + +//------------------ +/** +Sprawdza kryterium pobudzenia i odpadu dla charakterystyki stabilizacji +\param Ir wartoscs pradu roznicowego +\param Ih wartosc pradu stabilizacji +*/ + +void r87l_8_kryterium2(struct r87l_8_logic *log,float Ir, float Ih, u8 *pob, u8 idx) +{ + u16 n = 2;// 1; + +// if (Ih > log->nast_.Ih1) n = 2; + if (Ih > log->nast_.Ihp) n = 3; + if ((Ih > log->nast_.Ihg) && log->nast_.h) n = 4; + + *pob = 0; + *(pob + 1) = 0; + *(pob + 2) = 1; + + log->n[idx]=n; + + switch (n) + { + case 1: + *pob = Ir > log->nast_.Iror; + *(pob + 1) = Ir < log->nast_.Irop; + break; + case 2: +// *pob = Ir > Ih * log->nast_.kh1r; +// *(pob + 1) = Ir < Ih * log->nast_.kh1p; + *pob = Ir > ((Ih * log->nast_.kh1r)+log->nast_.Iror); + *(pob + 1) = Ir < ((Ih * log->nast_.kh1p)+log->nast_.Irop); + + break; + case 3: + *pob = Ir > (Ih * log->nast_.kh2r + log->nast_.Ir3c); + *(pob + 1) = Ir < (Ih * log->nast_.kh2p + log->nast_.Ip3c); + break; + case 4: + *pob = Ir > log->nast_.Ir4; + *(pob + 1) = Ir < log->nast_.Ip4; + *(pob + 2) = *(pob + 1); + + break; + } +} + + + +void R87L_8(void *arguments, void *logic) +{ + struct r87l_8_logic *log = (struct r87l_8_logic *)logic; +// struct r87l_args *args = (struct r87l_args *)arguments; + u8 i,j; + + if(check_struct(&log->test_1str)) + log->stream[log->my_addr]=log->ownstream; + else + log->stream[log->my_addr]=log->original_stream; + + for(i=0;inast_.stream_cnt;i++) + { + for(j=0;j<3;j++) + { + if(log->stream[i]->A[j]!=32768.0f) + log->A[i][j]=log->stream[i]->A[j]; + if(log->stream[i]->A_prev[j]!=32768.0f) + log->A_prev[i][j]=log->stream[i]->A_prev[j]; + } + } + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + r87l_8_przelicz_1(log); + + if(check_struct(&log->streams_ok)||check_struct(&log->test_1str)) + { + //Sprawdzenie pobudzenia w stabilizowanej strefie dzialania + r87l_8_kryterium2(log,log->Ir1_,log->Ih1_,&log->P_.Pb1,0); + r87l_8_kryterium2(log,log->Ir2_,log->Ih2_,&log->P_.Pb2,1); + r87l_8_kryterium2(log,log->Ir3_,log->Ih3_,&log->P_.Pb3,2); + } + + r87l_8_przelicz_2(log); + } + else + { + *log->Ir1=0; + *log->Ir2=0; + *log->Ir3=0; + *log->Ih1=0; + *log->Ih2=0; + *log->Ih3=0; + *log->Ir_L1_ortb=0; + *log->Ir_L2_ortb=0; + *log->Ir_L3_ortb=0; + *log->I2h1=0; + *log->I2h2=0; + *log->I2h3=0; + *log->I5h1=0; + *log->I5h2=0; + *log->I5h3=0; + clear_struct(&log->P); + clear_struct(&log->Pf[0]); + clear_struct(&log->Pf[1]); + clear_struct(&log->Pf[2]); + clear_struct(&log->W); + clear_struct(&log->Wf[0]); + clear_struct(&log->Wf[1]); + clear_struct(&log->Wf[2]); + + clear_struct(&log->Z); + clear_struct(&log->Zw1); + clear_struct(&log->Zw2); + clear_struct(&log->Zw3); + clear_struct(&log->BL2h1); + clear_struct(&log->BL2h2); + clear_struct(&log->BL2h3); + clear_struct(&log->BL5h1); + clear_struct(&log->BL5h2); + clear_struct(&log->BL5h3); + } + +} diff --git a/src/R87L_8.h b/src/R87L_8.h new file mode 100644 index 0000000..710feb4 --- /dev/null +++ b/src/R87L_8.h @@ -0,0 +1,269 @@ +/* + * R87L_8.h + * + * Created on: 24-05-2024 + * Author: Krzysztof Jakubczyk + */ + +#ifndef R87L_8_H_ +#define R87L_8_H_ + +#include "tdefs.h" +#include "analog_in.h" +#include "mkstream.h" +#include "Diff.h" + + struct wskazniki_prad_do_kompensacji_87l_8 + { + float /*struct we_an_diff*/ *Ip[8]; + float /*struct we_an_diff*/ *Im[8]; + float /*struct we_an_diff*/ *Ip_prev[8]; + float /*struct we_an_diff*/ *Im_prev[8]; + + float a_p[8][2]; + float a_m[8][2]; + float a_pi0[8]; + }; + + struct Nastawy_przeliczone_87_8 + { + float khr_2h;///< wartosc rozruchowa blokady od 2 harmonicznej + float khp_2h;///< wartosc powrotowa blokady od 2 harmonicznej + float khr_5h;///< wartosc rozruchowa blokady od 5 harmonicznej + float khp_5h;///< wartosc powrotowa blokady od 5 harmonicznej + float Irr;///< wartosc rozruchowa dzialania bezwarunkowego + float Ipr;///< wartosc powrotowa dzialania bezwarunkowego + float Ihdr;/// + + +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; + +} diff --git a/src/RNT.h b/src/RNT.h new file mode 100644 index 0000000..afd5594 --- /dev/null +++ b/src/RNT.h @@ -0,0 +1,324 @@ +/* + * RNT.h + * + * Created on: 14-11-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef RNT_H_ +#define RNT_H_ + +#include "tdefs.h" +#include "helper.h" + +#define T_STER_RECZNEGO 1 +#define TP_OP_BLOKADY_NAP 500 + +enum _stan_man_rnt +{ + CZEKANIE_NA_STEROWANIE_MANUAL = 0, + CZEKANIE_NA_STEROWANIE_NR_POZYCJI = 1, + CZEKANIE_NA_STEROWANIE_AUTO = 2, + + STEROWANIE_W_GORE = 5, + IMPULS_W_GORE = 6, + SPRAWDZENIE_ODP_PRZE_ZACZEPOW_GORA = 7, + + STEROWANIE_W_DOL = 11, + IMPULS_W_DOL = 12, + SPRAWDZENIE_ODP_PRZE_ZACZEPOW_DOL = 13, + + KONIEC_STEROWANIA = 20 +}; + + +struct Nastawy_przeliczone_rnt +{ + float R1; ///< wartosc rezystancji linii po str. pierwotnej + float X1; ///< wartosc reaktancji linii po str. pierwotnej + float R2; ///< wartosc rezystancji linii po str. pierwotnej + float X2; ///< wartosc reaktancji linii po str. pierwotnej + short tryb; ///< nastawa trybu dzia³ania 0-U1 1-U2 2-œredniaU1/U2 3-maxU1/U2 + int t1; ///< opoznienie czasu zadzialania przekaznika + int t2; ///< opoznienie czasu zadzialania przekaznika + + float dUzr; ///< wartosc rozruchowa do napiêcia nieczu³oœci + float dUzp; ///< wartosc powrotu do napiêcia nieczu³oœci + + float Uz1; ///< wartosc napiêcia zadanego nr 1 + float Uz2; ///< wartosc napiêcia zadanego nr 2 + float Uz3; ///< wartosc napiêcia zadanego nr 3 + float Uz4; ///< wartosc napiêcia zadanego nr 4 + + int t_imp_ster; /// czas implusu steruj¹cego gora/do³ + int t_ster_zacz; ///< czas maks sterowania + int t_poz_zacz; /// czas odpowiedzi pozycji zaczepu + + float kp; ///< wspolczynnik powrotu dla Uz + + u8 on; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 kon_biegu; /// kontrola biegu sterowania prze³¹cznikiem zaczepów (wg dodatkowego wejscia BIEG) + u8 kon_poz_ster; /// kontrola pozycji zaczepu przy sterowaniu (porównanie nr zaczepu przed i po sterowaniu) + u8 akt_kon_U1; /// kontrola zabezpieczenia pomiaru U1 + u8 akt_kon_U2; /// kontrola zabezpieczenia pomiaru U2 + u8 akt_kon_Uzacz; /// kontrola zabezpieczenia napiêcia zaczepu + u8 akt_blok_biegu; /// aktywnosc blokady od biegu prze³¹cznika zaczepów + u8 akt_blok_pozycja; /// aktywnosc blokady od niewlasciwej pozycji zaczepu po sterowaniu + u8 akt_mfazowe; /// aktywne pomiary miêdzy fazowe + + u8 akt_komp1; ///< czy kompensacja 1 jest aktywana 1-aktywana + u8 akt_komp2; ///< czy kompensacja 2 jest aktywana 1-aktywana + + u8 bl; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna +}; + +struct Wejscie_rnt +{ + float orta_U1_; ///< ortogonalna a napiêcia wejsciowego U1 + float ortb_U1_; ///< ortogonalna b napiêcia wejsciowego U1 + float kwampl_U1_; ///< kwadrat amplitudy pr¹du + float orta_I1_;///< ortogonalna a pr¹du wejsciowego I1 + float ortb_I1_;///< ortogonalna b pr¹du wejsciowego I1 + float kwampl_I1_;///< kwadrat amplitudy pr¹du + float orta_U2_; ///< ortogonalna a napiêcia wejsciowego U2 + float ortb_U2_; ///< ortogonalna b napiêcia wejsciowego U2 + float kwampl_U2_; ///< kwadrat amplitudy napiêcia + float orta_I2_;///< ortogonalna a pr¹du wejsciowego I1 + float ortb_I2_;///< ortogonalna b pr¹du wejsciowego I1 + float kwampl_I2_;///< kwadrat amplitudy pr¹du + + float nr_zacz_;///nr zaczepu z prze³¹cznika trafo + float nr_zacz_zad;///nr zaczepu z prze³¹cznika trafo + +}; + +struct dane_wewnetrzne_rnt +{ + + u8 zacz_w_gore; + u8 zacz_w_dol; + u8 blokada_trw; + u8 blokada_ster_gora; + u8 blokada_ster_dol; + u8 pob_G; //pobudzenie automatyki G + u8 pob_D; //pobudzenie automatyki D + short lp_G; //licznik pobudzenia G + short lp_D; //licznik pobudzenia D + u8 byl_bieg; + int timer_blokad_pod_nap1; + int timer_blokad_pod_nap2; +}; + +struct rnt_logic +{ + struct binary_io stan_bl_; + struct binary_io man_; + struct binary_io auto_; + + float *orta_U1_; + float *ortb_U1_; + float *kwampl_U1_; + struct binary_io kon_U1_; + + float *orta_I1_; + float *ortb_I1_; + float *kwampl_I1_; + + + float *orta_U2_; + float *ortb_U2_; + float *kwampl_U2_; + struct binary_io kon_U2_; + + float *orta_I2_; + float *ortb_I2_; + float *kwampl_I2_; + + float *nr_zacz_; + struct binary_io kon_u_nr_zacz_; + struct binary_io zo_zacz_; + + struct binary_io gora_; + struct binary_io dol_; + struct binary_io bieg_; + struct binary_io aktU1z_; + struct binary_io aktU2z_; + struct binary_io aktU3z_; + struct binary_io aktU4z_; + + struct binary_io bl_gora_; + struct binary_io bl_dol_; + + struct binary_io test_; + struct binary_io kas_; + + struct binary_io blok_trw_; + + + + struct binary_io gora_out_; + struct binary_io dol_out_; + + struct binary_io e_bieg_out_; + struct binary_io e_poz_out_; + struct binary_io zaczep_min; + struct binary_io zaczep_max; + + struct binary_io pob_G; + struct binary_io pob_D; + + struct binary_io blok_out_; + struct binary_io blok_trw_out_; + struct binary_io blad_trybu_out_; + + struct binary_io ster_poz_; + + float *nr_zacz_zad; + + float *U_odn;//napiêcie odniesienia + float *tg_out;//czas do prze³¹czenia w górê + float *td_out;//czas do prze³¹czenia w dó³ + + struct Nastawy_przeliczone_rnt nast_; + struct dane_wewnetrzne_rnt dw; + struct Wejscie_rnt w; + + enum _stan_man_rnt _stan_man; + u32 licznik_ster_gora; + u32 licznik_ster_dol; + u32 licznik_poz_gora; + u32 licznik_poz_dol; + u32 licznik_ster_biegu; + + u8 nr_zaczepu_przed_ster; //nr zaczepu przed sterowaniem + float licz_t1G; + int licz_t2G; + float licz_t1D; + int licz_t2D; + + struct analog_in_params *I_params[2]; + struct analog_in_params *U_params[2]; + + float mul[2]; + + +}; + +struct rnt_io +{ + u32 stan_bl_in; + u32 man_in; + u32 auto_in; + + u32 orta_U1_float_in; + u32 ortb_U1_float_in; + u32 kwampl_U1_float_in; + u32 U1_params_ptr_in; + u32 kon_U1_in; + u32 orta_I1_float_in; + u32 ortb_I1_float_in; + u32 kwampl_I1_float_in; + u32 I1_params_ptr_in; + + u32 orta_U2_float_in; + u32 ortb_U2_float_in; + u32 kwampl_U2_float_in; + u32 U2_params_ptr_in; + u32 kon_U2_in; + u32 orta_I2_float_in; + u32 ortb_I2_float_in; + u32 kwampl_I2_float_in; + u32 I2_params_ptr_in; + + u32 nr_zacz_in; + u32 zo_zacz_in; + u32 kon_u_nr_zacz_in; + + u32 bieg_in; + + u32 gora_in; + u32 dol_in; + + u32 aktU1z_in; + u32 aktU2z_in; + u32 aktU3z_in; + u32 aktU4z_in; + + u32 bl_gora_in; + u32 bl_dol_in; + + u32 test_in; + u32 kas_in; + + u32 ster_poz_in; + u32 zad_zacz_in; + u32 blok_trw_in; + + u32 gora_out; + u32 dol_out; + u32 e_bieg_out; + u32 e_poz_out; + + u32 pob_G_out; + u32 pob_D_out; + u32 Uodn_float_out; + u32 tg_out; + u32 td_out; + u32 blok_out; + u32 blok_trw_out; + u32 blad_trybu_out; + +}__attribute__((__packed__)); + +struct rnt_params +{ + u32 akt; ///< nastawy bitowe; + u32 tryb; ///< wybór trybu regulacji (0=U1/1=U2/2=œrednia U1 z U2/3=maksymalna U1 lub U2) + u32 akt_komp1; ///< nastawy bitowe; + double R1; ///< rezystancja linii 1 w wart. pier + double X1; ///< reaktanacja linii 1 w wart. pier + u32 akt_komp2; ///< nastawy bitowe; + double R2; ///< rezystancja linii 2 w wart. pier + double X2; ///< reaktanacja linii 2 w wart. pier + + double t1; ///< wartosc nastawy czasu dzialania ch-ki zale¿nej w sekundach stopieñ 1 + double t2; ///< wartosc nastawy czasu dzialania ch-ki niezale¿nej w sekundach stopieñ 2 + double dUz; ///< dopuszczalne napiêcie nieczu³oœci + double kp; ///< wspó³czynnik powrotu dla dUz + + double Uz1; ///< zadane napiêcie nr 1 + double Uz2; ///< zadane napiêcie nr 2 + double Uz3; ///< zadane napiêcie nr 3 + double Uz4; ///< zadane napiêcie nr 4 + + double t_imp_ster; ///< wartosc nastawy impulsu steruj¹cego + double t_ster_zacz; ///< wartosc nastawy max. czasu sterowania zaczepu + double t_poz_zacz; ///< wartosc nastawy max. czasu rekacji nr zaczepu + + +}__attribute__((__packed__)); + +struct rnt_args +{ + struct rnt_io io; + struct rnt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void rnt(void *args, void *logic); +extern void rnt_100hz(void *args, void *logic); +extern void rnt_20hz(void *args, void *logic); +extern int rnt_initlog(void *arguments, void *logic); + + + +enum _tryb_pracy_rnt +{ + U1 = 0, + U2 = 1, + sredniaU1U2 = 2, + maxU1U2 = 3 +}; + + +#endif /* RNT_H_ */ diff --git a/src/R_67N_LWN.c b/src/R_67N_LWN.c new file mode 100644 index 0000000..d7bc4c9 --- /dev/null +++ b/src/R_67N_LWN.c @@ -0,0 +1,507 @@ +/* + * R_67N_LWN.c + * + * Created on: 20-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "r_67n_lwn.h" +#include "analog_in.h" +#include "helper.h" +#include "ZDistL.h" + +#include + +int r_67n_lwn_initlog(void *arguments, void *logic) +{ + struct r_67n_lwn_args *args = (struct r_67n_lwn_args *)arguments; + struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic; + + if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl_)) + return -1; + + if(set_float_ptr(args->io.orta_I_float_in,&log->orta_I_)) + return -1; + + if(set_float_ptr(args->io.ortb_I_float_in,&log->ortb_I_)) + return -1; + + if(set_float_ptr(args->io.kwampl_I_float_in,&log->kwampl_I_)) + return -1; + + if(set_float_ptr(args->io.orta_U_float_in,&log->orta_U_)) + return -1; + + if(set_float_ptr(args->io.ortb_U_float_in,&log->ortb_U_)) + return -1; + + if(set_float_ptr(args->io.kwampl_U_float_in,&log->kwampl_U_)) + return -1; + + if(set_float_ptr(args->io.IL1_float_in,&log->IL1)) + return -1; + + if(set_float_ptr(args->io.IL2_float_in,&log->IL2)) + return -1; + + if(set_float_ptr(args->io.IL3_float_in,&log->IL3)) + return -1; + + if(set_float_ptr(args->io.Io2h_float_in,&log->Io2h)) + return -1; + + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.l_odb_in,&log->l_odb)) + return -1; + + if(set_bit_ptr_struct(args->io.l_ok_in,&log->l_ok)) + return -1; + + if(set_bit_ptr_struct(args->io.w_out,&log->W)) + return -1; + + if(set_bit_ptr_struct(args->io.z1_out,&log->Z1)) + return -1; + + if(set_bit_ptr_struct(args->io.z2_out,&log->Z2)) + return -1; + + if(set_bit_ptr_struct(args->io.p1_out,&log->P1)) + return -1; + + if(set_bit_ptr_struct(args->io.p2_out,&log->P2)) + return -1; + + if(set_bit_ptr_struct(args->io.kier_out,&log->kier)) + return -1; + + if(set_bit_ptr_struct(args->io.NRL_out,&log->NRL)) + return -1; + + + //// + //status_rej = oczekiwanie; + + log->nast_.on_ = args->params.bity & 0x0001; + log->nast_.w1 = args->params.bity & 0x0002; + log->nast_.w2 = args->params.bity & 0x0004; + log->nast_.Bl_2h1_ON = (args->params.bity & 0x0008)?1:0; + log->nast_.Bl_2h2_ON = (args->params.bity & 0x0010)?1:0; + log->nast_.bl_ = args->params.bity & 0x0020; + + + log->nast_.dir = args->params.dir; + + log->nast_.Ir1 = args->params.I1 * args->params.I1; // wyliczenie wartosci rozruchowej pradu + log->nast_.Ir2 = args->params.I2 * args->params.I2; // wyliczenie wartosci rozruchowej pradu + + log->nast_.Ip1 = log->nast_.Ir1 * args->params.kp * args->params.kp; // wyliczenie wartoci powrotu algorytmu + log->nast_.Ip2 = log->nast_.Ir2 * args->params.kp * args->params.kp; // wyliczenie wartoci powrotu algorytmu + + log->nast_.kh1 = args->params.kh1 * args->params.kh1; + log->nast_.khp1 = log->nast_.kh1 * args->params.kp * args->params.kp; + log->nast_.kh2 = args->params.kh2 * args->params.kh2; + log->nast_.khp2 = log->nast_.kh2 * args->params.kp * args->params.kp; + + log->nast_.U_echo = args->params.L_U_echo * args->params.L_U_echo; // wyliczenie wartosci rozruchowej blokady napieciowej + log->nast_.Ur = args->params.U * args->params.U; // wyliczenie wartosci rozruchowej blokady napieciowej + log->nast_.Up = log->nast_.Ur * args->params.kp * args->params.kp; // wyliczenie wartosci powrotu blokady napieciowej + log->nast_.orta = cos(args->params.kat * (3.14159f / 180)); // wartosc ortogonalnej a wektora przesuniecia dla kata charakterystycznego + log->nast_.ortb = sin(args->params.kat * (3.14159f / 180)); // wartosc ortogonalnej b wektora przesuniecia dla kata charakterystycznego + + log->nast_.I1_2hr = args->params.Io2h1 * args->params.Io2h1; + log->nast_.I1_2hr_powr = (0.95 * args->params.Io2h1) * (0.95 * args->params.Io2h1); + + + log->nast_.t1 = ((args->params.t1 - 0.005f) * 100); //czas dzialania + if (log->nast_.t1) log->nast_.t0_1 = 0; else log->nast_.t0_1 = 1; + log->nast_.t2 = ((args->params.t2 - 0.005f) * 100); //czas dzialania + if (log->nast_.t2) log->nast_.t0_2 = 0; else log->nast_.t0_2 = 1; + + log->nast_.l_nad_time=(u32)(args->params.L_t_nad * 1000); + log->nast_.l_t_blok=(u32)(args->params.L_T_odwr * 1000); + log->nast_.t1_min=(u32)((args->params.t1_min - 0.005f) * 100); + log->nast_.t2_min=(u32)((args->params.t2_min - 0.005f) * 100); +// *((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = log->nast_.on_; + + return 0; +} + + +void r_67n_lwn(void *arguments, void *logic) +{ + struct r_67n_lwn_args *args = (struct r_67n_lwn_args *)arguments; + struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic; + float local_orta_U_ ,local_ortb_U_, ILmax; + u8 wk,l_nrl; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + log->w.kwampl_U_ = *log->kwampl_U_; + log->w.kwampl_I_ = *log->kwampl_I_; + log->w.orta_I_ = *log->orta_I_; + log->w.orta_U_ = *log->orta_U_; + log->w.ortb_I_ = *log->ortb_I_; + log->w.ortb_U_ = *log->ortb_U_; + log->w.IL1 = *log->IL1; + log->w.IL2 = *log->IL2; + log->w.IL3 = *log->IL3; + log->w.Io2h = *log->Io2h; + + // przesuniecie napiecia o wektor kata nastawy + local_orta_U_ = log->w.orta_U_ * log->nast_.orta - log->w.ortb_U_ * log->nast_.ortb; + local_ortb_U_ = log->w.orta_U_ * log->nast_.ortb + log->w.ortb_U_ * log->nast_.orta; + + // okreslenie kierunku + log->dw.kierunek = (log->w.orta_I_ * local_orta_U_ + log->w.ortb_I_ * local_ortb_U_) > 0; + if(!czy_test_R()) // w tescie nie uzywaj kierunku + check_and_set_struct(log->dw.kierunek,&log->kier); + + switch(log->nast_.dir) //okeslenie kierunku stopnia 1 + { + case 0: + wk = log->dw.kierunek; + break; + case 1: + wk = !log->dw.kierunek; + break; + case 2: + wk = 1; + break; + } + + //sprawdzenie warunku napiciowego + log->dw.P_U |= log->w.kwampl_U_ > log->nast_.Ur; + log->dw.P_U &= log->w.kwampl_U_ > log->nast_.Up; + + if(log->dw.Bl_2h1) + log->dw.Bl_2h1 = log->w.Io2h > log->nast_.I1_2hr_powr * log->w.kwampl_I_; + else + log->dw.Bl_2h1 = log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_; + + if(log->dw.Bl_2h2) + log->dw.Bl_2h2 = log->w.Io2h > log->nast_.I1_2hr_powr * log->w.kwampl_I_; + else + log->dw.Bl_2h2 = log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_; + + log->dw.Bl_2h1 &= log->nast_.Bl_2h1_ON; + log->dw.Bl_2h2 &= log->nast_.Bl_2h2_ON; + + //Sprawdzenie przekaznikw pradowych + //szukanie maksymalnego pradu + ILmax = log->w.IL1; + if (ILmax < log->w.IL2) ILmax = log->w.IL2; + if (ILmax < log->w.IL3) ILmax = log->w.IL3; + + //sprawdzenie pobudzenia stopnia 1 + sprawdz_P(&log->dw.P1, + wk && log->dw.P_U && !log->dw.Bl_2h1 && log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > (log->nast_.kh1 * ILmax), + !wk || !log->dw.P_U || log->dw.Bl_2h1 || log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < (log->nast_.khp1 * ILmax), + &log->dw.lp1, + 11,3 + ); + + //sprawdzenie pobudzenia stopnia 2 + sprawdz_P(&log->dw.P2, + wk && log->dw.P_U && !log->dw.Bl_2h2 && log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > log->nast_.kh2 * ILmax, + !wk || !log->dw.P_U || log->dw.Bl_2h2 || log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < log->nast_.khp2 * ILmax, + &log->dw.lp2, + 11,3 + ); + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.P1 = log->dw.P2 = check_struct(&log->test)?1:0; + } + // + + // realizacja blokady dzialania przekaznika + if(log->nast_.bl_ && check_struct(&log->stan_bl_) /*| czy_start()*/) + log->dw.P1 = log->dw.P2 = 0; + // + + log->dw.Z1 |= (log->dw.P1 && log->nast_.t0_1); // jesli dziaanie bezzwoczne stopnia 1 + log->dw.Z2 |= log->dw.P2 && log->nast_.t0_2; // jesli dziaanie bezzwoczne stopnia 2 + log->dw.Z = log->dw.Z1 || log->dw.Z2; + + if (log->dw.Z && !log->dw.Z_last) //zapisanie momentu zadziaania + { + log->dw.Z_last = 1; +// log->dw.czas_ = time; + } + + check_and_set_struct(log->dw.P1,&log->P1); + check_and_set_struct(log->dw.P2,&log->P2); + check_and_set_struct(log->dw.Z1,&log->Z1); + check_and_set_struct(log->dw.Z2,&log->Z2); + + check_and_set_struct(((log->dw.Z1 && log->nast_.w1) || (log->dw.Z2 && log->nast_.w2) || (log->l_echo && (args->params.bity & R67N_L_ECHO_WYL))) && !log->l_blok_wyl && !log->dw.Podwr,&log->W); + + if(args->params.bity & R67N_L_AKT) + { + //nadawanie + l_nrl=0; + + if(log->dw.P1 && args->params.bity & R67N_L_NADP1) + { + l_nrl=1; + log->l_nad_cnt=0; + } + + if(log->dw.P2 && args->params.bity & R67N_L_NADP2) + { + l_nrl=1; + log->l_nad_cnt=0; + } + //odbior + if(!(args->params.bity & R67N_L_BLOK)) // gdy tryb zezwalajacy + { + if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.Podwr) + { + if(log->dw.P1) + log->skr_t1=1; + else + log->skr_t1=0; + + if(log->dw.P2) + log->skr_t2=1; + else + log->skr_t2=0; + } + else + { + log->skr_t1=0; + log->skr_t2=0; + } + } + else // tryb blokujacy + { + if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.Podwr) + log->l_blok_wyl=1; + else + log->l_blok_wyl=0; + } + // + + // echo + if(args->params.bity & R67N_L_ECHO) + { + sprawdz_P(&log->dw.PI0, + (log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > log->nast_.kh1 * ILmax)||(log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > log->nast_.kh2 * ILmax), + (log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < log->nast_.khp1 * ILmax)&&(log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < log->nast_.khp2 * ILmax), + &log->dw.lp3, + 5,200 + ); + + if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.PI0 && log->w.kwampl_U_ > log->nast_.U_echo && !log->dw.Podwr) + { + l_nrl=1; + log->l_nad_cnt=0; + log->l_echo=1; + } + else + log->l_echo=0; + } + + + // LRC + if(args->params.bity & R67N_L_ODWR_PR) + { + sprawdz_P(&log->dw.Podwr, + !wk && log->dw.P_U && ((!log->dw.Bl_2h1 && log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > (log->nast_.kh1 * ILmax))||(!log->dw.Bl_2h2 && log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > (log->nast_.kh2 * ILmax))), + wk || !log->dw.P_U || ((log->dw.Bl_2h1 || log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < (log->nast_.khp1 * ILmax))&&(log->dw.Bl_2h2 || log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < (log->nast_.khp2 * ILmax))), + &log->dw.lp4, + 40,log->nast_.l_t_blok + ); + + if(log->dw.Podwr) + { + l_nrl=0; + clear_struct(&log->NRL); + log->l_nad_cnt=0; + } + } + + // wyslanie impulsu nadawania + if(l_nrl && !log->l_nad_cnt) + set_struct(&log->NRL); + + if(log->l_nad_cnt>=log->nast_.l_nad_time) + { + clear_struct(&log->NRL); + if(!l_nrl) + log->l_nad_cnt=0; + } + else if(check_struct(&log->NRL)) + log->l_nad_cnt++; + + + } + } + else + { + clear_struct(&log->Z1); + clear_struct(&log->Z2); + clear_struct(&log->W); + clear_struct(&log->P1); + clear_struct(&log->P2); + + clear_struct(&log->NRL); + } +} + +void r_67n_lwn_100hz(void *arguments, void *logic) +{ + struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic; + int t; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + // realizacja przekaznika czasowego + if (log->dw.P1) log->dw.lt1++; else log->dw.lt1 = 0; + if (log->dw.P2) log->dw.lt2++; else log->dw.lt2 = 0; + + if(log->skr_t1 && log->nast_.t1_minnast_.t1) + t=log->nast_.t1_min; + else + t=log->nast_.t1; + + if (log->dw.lt1 > t) + { + log->dw.Z1 = 1; + log->dw.lt1--; + } + + if(log->skr_t2 && log->nast_.t2_minnast_.t2) + t=log->nast_.t2_min; + else + t=log->nast_.t2; + + if (log->dw.lt2 > t) + { + log->dw.Z2 = 1; + log->dw.lt2--; + } + + // + //kasowanie zadziaa po odpadniciu pobudze + log->dw.P = log->dw.P1 || log->dw.P2; + + if(!log->dw.P1) + log->dw.Z1=0; + if(!log->dw.P2) + log->dw.Z2=0; + if(!log->dw.P) + log->dw.Z=0; + + log->dw.Z_last = log->dw.Z; + + // realizacja rejestratora parametrow ostatniego zaklocenia + switch(status_rej) + { + case oczekiwanie: + if (log->dw.P) + { + log->dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + log->dw.max_orta_I = log->w.orta_I_; + log->dw.max_ortb_I = log->w.ortb_I_; + log->dw.max_orta_U = log->w.orta_U_; + log->dw.max_ortb_U = log->w.ortb_U_; + log->dw.max_I = log->w.kwampl_I_; + log->dw.max_U = log->w.kwampl_U_; + log->dw.I1 = log->w.IL1; + log->dw.I2 = log->w.IL2; + log->dw.I3 = log->w.IL3; + } + break; + case sledzenie: + if (log->dw.Z) + { + status_rej = sledzenie_zad; + //log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (log->dw.P) + { + if(log->w.kwampl_I_>log->dw.max_I) + { + log->dw.max_orta_I = log->w.orta_I_; + log->dw.max_ortb_I = log->w.ortb_I_; + log->dw.max_orta_U = log->w.orta_U_; + log->dw.max_ortb_U = log->w.ortb_U_; + log->dw.max_I = log->w.kwampl_I_; + log->dw.max_U = log->w.kwampl_U_; + log->dw.I1 = log->w.IL1; + log->dw.I2 = log->w.IL2; + log->dw.I3 = log->w.IL3; + log->dw.Io2h = log->w.Io2h; + } + + log->dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!log->dw.P) + { + log->dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + + if(log->w.kwampl_I_>log->dw.max_I) + { + log->dw.max_orta_I = log->w.orta_I_; + log->dw.max_ortb_I = log->w.ortb_I_; + log->dw.max_orta_U = log->w.orta_U_; + log->dw.max_ortb_U = log->w.ortb_U_; + log->dw.max_I = log->w.kwampl_I_; + log->dw.max_U = log->w.kwampl_U_; + log->dw.I1 = log->w.IL1; + log->dw.I2 = log->w.IL2; + log->dw.I3 = log->w.IL3; + log->dw.Io2h = log->w.Io2h; + } + + + log->dw.dl_++; + } + break; + case zapis: + if (!log->dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + } + + } +} + +void r_67n_lwn_20hz(void *arguments, void *logic) +{ + struct r_67n_lwn_logic *log = (struct r_67n_lwn_logic *)logic; + + // czesc ktora miala byc wykonywana max co 1s w wolnym czasie + if(log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + { + log->dw.R.max_I_ = sqrt(log->dw.max_I); // wyliczenie maksymalnej wartosci pradu + log->dw.R.max_U_ = sqrt(log->dw.max_U); // wyliczenie wartosci napiecia + log->dw.R.I1 = sqrt(log->dw.I1); + log->dw.R.I2 = sqrt(log->dw.I2); + log->dw.R.I3 = sqrt(log->dw.I3); + log->dw.R.Io2h = sqrt(log->dw.Io2h); + log->dw.R.max_kat_ = (180 / 3.14159f) * (atan2(log->dw.max_ortb_I,log->dw.max_orta_I) - atan2(log->dw.max_ortb_U,log->dw.max_orta_U));// wyliczenie kata + // log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // log->dw.R.dlugosc = (float)log->dw.dl_ * 0.01f; // / 100; // wpisanie czasu trwania zwarcia + // log->dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + } + +} diff --git a/src/R_67N_LWN.h b/src/R_67N_LWN.h new file mode 100644 index 0000000..83823be --- /dev/null +++ b/src/R_67N_LWN.h @@ -0,0 +1,220 @@ +/* + * R_67N_LWN.h + * + * Created on: 19-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef R_67N_LWN_H_ +#define R_67N_LWN_H_ + +#include "tdefs.h" +#include "helper.h" + +#define R67N_L_AKT 0x0040 +#define R67N_L_NADP1 0x0080 +#define R67N_L_NADP2 0x0100 +#define R67N_L_ECHO 0x0200 +#define R67N_L_ECHO_WYL 0x0400 +#define R67N_L_BLOK 0x0800 +#define R67N_L_ODWR_PR 0x1000 + +struct Wyjscia_rejestratora_lwn +{ + float max_I_;///< maksymalna watosc pradu w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_U_;///< watosc napiecia w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_kat_; ///< watosc kata w czasie zaklocenia w stopniach + float I1; ///< prad fazy L1 + float I2; ///< prad fazy L2 + float I3; ///< prad fazy L3 + float Io2h; ///< pr¹d drugiej harmonicznej +}; + + +struct Nastawy_przeliczone_lwn +{ + float Ir1; ///< wartosc rozruchowa pradu stopieñ 1 + float Ip1; ///< wartosc powrotu pradu rozruchowego stopieñ 1 + float Ir2; ///< wartosc rozruchowa pradu stopieñ 2 + float Ip2; ///< wartosc powrotu pradu rozruchowego stopieñ 2 + float kh1; ///< wspólczynnik stabilizacji zadzia³ania stopieñ 1 + float kh2; ///< wspólczynnik stabilizacji zadzia³ania stopieñ 2 + float khp1; ///< wspolczynnik stabilizacji dla powrotu stopieñ 1 + float khp2; ///< wspolczynnik stabilizacji dla powrotu stopieñ 2 + float I1_2hr; ///< wartoœæ rozruchowa blokady drugiej harmonicznej stopieñ 1 + float I1_2hr_powr; ///< wartoœæ rozruchowa blokady drugiej harmonicznej stopieñ 1 + float U_echo; ///< wartosc rozruchowa echa + float Ur; ///< wartosc rozruchowa napiecia blokady + float Up; ///< wartosc powrotu napiecia blokady + float orta; ///< ortogonalna a dla wektora nasatwy kata charakterystycznego + float ortb; ///< ortogonalna b dla wektora nasatwy kata charakterystycznego + short dir; ///< nastawa kierunku 2 - do przodu; 1 - w obu kierunkach; 0 - do tylu + int t1; ///< opoznienie czasu zadzialania przekaznika + int t2; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 w1; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 w2; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 t0_1; ///< przekaŸnik bezzw³oczny stopieñ 1 + u8 t0_2; ///< przekaŸnik bezzw³oczny stopieñ 2 + u8 Bl_2h1_ON; ///< W³¹czenie blokady od drugiej harmonicznej w stopniu 1 + u8 Bl_2h2_ON; ///< W³¹czenie blokady od drugiej harmonicznej w stopniu 2 + u32 l_nad_time; + u32 t1_min; + u32 t2_min; + u32 l_t_blok; +}; + +struct Wejscie_lwn +{ + float orta_I_; ///< ortogonalna a pradu wejsciowego Io + float ortb_I_; ///< ortogonalna b pradu wejsciowego Io + float kwampl_I_; ///< kwadrat amplitudy pr¹du + float orta_U_; ///< ortogonalna b napiecia wejsciowego Uo + float ortb_U_; ///< ortogonalna b napiecia wejsciowego Uo + float kwampl_U_; ///< kwadrat amplitudy napiecia + float IL1; ///< kwadrat amplitudy pr¹du faza L1 + float IL2; ///< kwadrat amplitudy pr¹du faza L2 + float IL3; ///< kwadrat amplitudy pr¹du faza L3 + float Io2h;///< kwadrat amplitudy drugiej harmonicznej sk³adowej zerowej pr¹du +}; + +struct dane_wewnetrzne_lwn +{ + struct Wyjscia_rejestratora_lwn R; ///< dane ostatniego zaklocenia + long long czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + u32 dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + u8 wsk_rej; // nowy zapis rejestracji parametrow zaklocenia (spelnienie kryterium identyfikacji) + + float max_I;///< maksymalna watosc pradu w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_U;///< watosc napiecia w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_kat; ///< watosc kata w czasie zaklocenia w stopniach + float I1; ///< prad fazy L1 + float I2; ///< prad fazy L2 + float I3; ///< prad fazy L3 + float Io2h;///< prad drugiej harmonicznej + + float max_orta_I; + float max_ortb_I; + float max_orta_U; + float max_ortb_U; + + short lp1;///< licznikpobudzen stopieñ 1 + short lp2;///< licznikpobudzen stopieñ 2 + short lp3;///< licznikpobudzen I0 do echa + short lp4;///< licznikpobudzen w LRC + int lt1; + int lt2; + u8 Podwr; + u8 PI0; // pobudzenie i0 do echa + u8 P; // pobudzenie i0 do echa + u8 P1; + u8 P2; + u8 Z; + u8 Z1; + u8 Z2; + u8 P_U; + u8 Bl_2h1; + u8 Bl_2h2; + u8 kierunek; + u8 Z_last; +}; + +struct r_67n_lwn_logic +{ + struct binary_io stan_bl_; + + float *orta_I_; + float *ortb_I_; + float *kwampl_I_; + + float *orta_U_; + float *ortb_U_; + float *kwampl_U_; + + float *IL1; + float *IL2; + float *IL3; + float *Io2h; + + struct binary_io test; + struct binary_io l_odb; + struct binary_io l_ok; + struct binary_io W; + struct binary_io Z1; + struct binary_io Z2; + struct binary_io P1; + struct binary_io P2; + struct binary_io kier; + struct binary_io NRL; + + struct Nastawy_przeliczone_lwn nast_; + struct dane_wewnetrzne_lwn dw; + struct Wejscie_lwn w; + + u8 l_echo; + u8 skr_t1; + u8 skr_t2; + u8 l_blok_wyl; + u32 l_nad_cnt; +}; + +struct r_67n_lwn_io +{ + u32 stan_bl_in; + u32 orta_I_float_in; + u32 ortb_I_float_in; + u32 kwampl_I_float_in; + u32 orta_U_float_in; + u32 ortb_U_float_in; + u32 kwampl_U_float_in; + u32 IL1_float_in; + u32 IL2_float_in; + u32 IL3_float_in; + u32 Io2h_float_in; + u32 test_in; + u32 l_odb_in; + u32 l_ok_in; + u32 w_out; + u32 z1_out; + u32 z2_out; + u32 p1_out; + u32 p2_out; + u32 kier_out; + u32 NRL_out; +}__attribute__((__packed__)); + +struct r_67n_lwn_params +{ + u32 bity; ///< nastawy bitowe; + u32 dir; ///< nastawa kierunku dzialania przekaznika + double U; ///< napiecie rozruchowe przekaznika w krotnosciach wartosci znamionowej + double kat; ///< wartosc nastawy kata charakterystycznego w stopniach + double I1; ///< prad rozruchowy przekaznika stopien 1 + double I2; ///< prad rozruchowy przekaznika stopien 2 + double kh1; ///< wspó³czynnik stabilizacji stopieñ 1 + double kh2; ///< wspó³czynnik stabilizacji stopieñ 2 + double t1; ///< wartosc nastawy czasu dzialania w sekundach stopiñ 1 + double t2; ///< wartosc nastawy czasu dzialania w sekundach stopiñ 2 + double t1_min; // czas t1 przy skróceniu + double t2_min; // czas t1 przy skróceniu + double Io2h1; /// + +int r_67n_lwn2_initlog(void *arguments, void *logic) +{ + struct r_67n_lwn2_args *args = (struct r_67n_lwn2_args *)arguments; + struct r_67n_lwn2_logic *log = (struct r_67n_lwn2_logic *)logic; + + if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl_)) + return -1; + + if(set_float_ptr(args->io.orta_I_float_in,&log->orta_I_)) + return -1; + + if(set_float_ptr(args->io.ortb_I_float_in,&log->ortb_I_)) + return -1; + + if(set_float_ptr(args->io.kwampl_I_float_in,&log->kwampl_I_)) + return -1; + + if(set_float_ptr(args->io.orta_U_float_in,&log->orta_U_)) + return -1; + + if(set_float_ptr(args->io.ortb_U_float_in,&log->ortb_U_)) + return -1; + + if(set_float_ptr(args->io.kwampl_U_float_in,&log->kwampl_U_)) + return -1; + + if(set_float_ptr(args->io.IL1_float_in,&log->IL1)) + return -1; + + if(set_float_ptr(args->io.IL2_float_in,&log->IL2)) + return -1; + + if(set_float_ptr(args->io.IL3_float_in,&log->IL3)) + return -1; + + if(set_float_ptr(args->io.Io2h_float_in,&log->Io2h)) + return -1; + + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.l_odb_in,&log->l_odb)) + return -1; + + if(set_bit_ptr_struct(args->io.l_ok_in,&log->l_ok)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_st1_in,&log->bl_st1)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_st2_in,&log->bl_st2)) + return -1; + + if(set_bit_ptr_struct(args->io.w_out,&log->W)) + return -1; + + if(set_bit_ptr_struct(args->io.z1_out,&log->Z1)) + return -1; + + if(set_bit_ptr_struct(args->io.z2_out,&log->Z2)) + return -1; + + if(set_bit_ptr_struct(args->io.p1_out,&log->P1)) + return -1; + + if(set_bit_ptr_struct(args->io.p2_out,&log->P2)) + return -1; + + if(set_bit_ptr_struct(args->io.kier_out,&log->kier)) + return -1; + + if(set_bit_ptr_struct(args->io.NRL_out,&log->NRL)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_st1_out,&log->bl_st1_wy)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_st2_out,&log->bl_st2_wy)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_st1_out,&log->bl_2h_st1_wy)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_st2_out,&log->bl_2h_st2_wy)) + return -1; + + if(set_bit_ptr_struct(args->io.l_blok_out,&log->l_blok)) + return -1; + if(set_bit_ptr_struct(args->io.l_zezw_out,&log->l_zezw)) + return -1; + if(set_bit_ptr_struct(args->io.l_echo_out,&log->l_echo_wy)) + return -1; + if(set_bit_ptr_struct(args->io.l_rc_out,&log->l_rc)) + return -1; + + //// + //status_rej = oczekiwanie; + + log->nast_.on_ = args->params.bity & 0x0001; + log->nast_.w1 = args->params.bity & 0x0002; + log->nast_.w2 = args->params.bity & 0x0004; + log->nast_.Bl_2h1_ON = (args->params.bity & 0x0008)?1:0; + log->nast_.Bl_2h2_ON = (args->params.bity & 0x0010)?1:0; + log->nast_.bl_ = args->params.bity & 0x0020; + + + log->nast_.dir = args->params.dir; + log->nast_.dir2 = args->params.dir2; + + log->nast_.Ir1 = args->params.I1 * args->params.I1; // wyliczenie wartosci rozruchowej pradu + log->nast_.Ir2 = args->params.I2 * args->params.I2; // wyliczenie wartosci rozruchowej pradu + + log->nast_.Ip1 = log->nast_.Ir1 * args->params.kp * args->params.kp; // wyliczenie wartoci powrotu algorytmu + log->nast_.Ip2 = log->nast_.Ir2 * args->params.kp * args->params.kp; // wyliczenie wartoci powrotu algorytmu + + log->nast_.kh1 = args->params.kh1 * args->params.kh1; + log->nast_.khp1 = log->nast_.kh1 * args->params.kp * args->params.kp; + log->nast_.kh2 = args->params.kh2 * args->params.kh2; + log->nast_.khp2 = log->nast_.kh2 * args->params.kp * args->params.kp; + + log->nast_.U_echo = args->params.L_U_echo * args->params.L_U_echo; // wyliczenie wartosci rozruchowej blokady napieciowej + log->nast_.Ur = args->params.U * args->params.U; // wyliczenie wartosci rozruchowej blokady napieciowej + log->nast_.Up = log->nast_.Ur * args->params.kp * args->params.kp; // wyliczenie wartosci powrotu blokady napieciowej + log->nast_.orta = cos(args->params.kat * (3.14159f / 180)); // wartosc ortogonalnej a wektora przesuniecia dla kata charakterystycznego + log->nast_.ortb = sin(args->params.kat * (3.14159f / 180)); // wartosc ortogonalnej b wektora przesuniecia dla kata charakterystycznego + + log->nast_.I1_2hr = args->params.Io2h1 * args->params.Io2h1; + log->nast_.I1_2hr_powr = (0.95 * args->params.Io2h1) * (0.95 * args->params.Io2h1); + + log->nast_.I2h_min1 = log->nast_.I1_2hr * log->nast_.Ir1; + log->nast_.I2h_min2 = log->nast_.I1_2hr * log->nast_.Ir2; + + log->nast_.t1 = ((args->params.t1 - 0.005f) * 100); //czas dzialania + if (log->nast_.t1) log->nast_.t0_1 = 0; else log->nast_.t0_1 = 1; + log->nast_.t2 = ((args->params.t2 - 0.005f) * 100); //czas dzialania + if (log->nast_.t2) log->nast_.t0_2 = 0; else log->nast_.t0_2 = 1; + + log->nast_.l_nad_time=(u32)(args->params.L_t_nad * 1000); + log->nast_.l_nad_time_max=(u32)(args->params.L_t_nad_max * 1000); + log->nast_.l_t_blok=(u32)(args->params.L_T_odwr * 1000); + log->nast_.t1_min=(u32)((args->params.t1_min - 0.005f) * 100); + log->nast_.t2_min=(u32)((args->params.t2_min - 0.005f) * 100); +// *((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = log->nast_.on_; + + return 0; +} + + +void r_67n_lwn2(void *arguments, void *logic) +{ + struct r_67n_lwn2_args *args = (struct r_67n_lwn2_args *)arguments; + struct r_67n_lwn2_logic *log = (struct r_67n_lwn2_logic *)logic; + float local_orta_U_ ,local_ortb_U_, ILmax; + u8 wk,wk2,l_nrl; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + log->w.kwampl_U_ = *log->kwampl_U_; + log->w.kwampl_I_ = *log->kwampl_I_; + log->w.orta_I_ = *log->orta_I_; + log->w.orta_U_ = *log->orta_U_; + log->w.ortb_I_ = *log->ortb_I_; + log->w.ortb_U_ = *log->ortb_U_; + log->w.IL1 = *log->IL1; + log->w.IL2 = *log->IL2; + log->w.IL3 = *log->IL3; + log->w.Io2h = *log->Io2h; + + // przesuniecie napiecia o wektor kata nastawy + local_orta_U_ = log->w.orta_U_ * log->nast_.orta - log->w.ortb_U_ * log->nast_.ortb; + local_ortb_U_ = log->w.orta_U_ * log->nast_.ortb + log->w.ortb_U_ * log->nast_.orta; + + // okreslenie kierunku + log->dw.kierunek = ((log->w.orta_I_ * local_orta_U_ + log->w.ortb_I_ * local_ortb_U_) > 0) ? 1 : 0; + if(!czy_test_R()) // w tescie nie uzywaj kierunku + check_and_set_struct(log->dw.kierunek,&log->kier); + + switch(log->nast_.dir) //okeslenie kierunku stopnia 1 + { + case 0: + wk = log->dw.kierunek; + break; + case 1: + wk = !log->dw.kierunek; + break; + case 2: + wk = 1; + default: + break; + } + + switch(log->nast_.dir2) //okeslenie kierunku stopnia 2 + { + case 0: + wk2 = log->dw.kierunek; + break; + case 1: + wk2 = !log->dw.kierunek; + break; + case 2: + wk2 = 1; + break; + default: + break; + } + + check_and_set_struct(check_struct(&log->bl_st1)||log->nast_.dir==3,&log->bl_st1_wy); + check_and_set_struct(check_struct(&log->bl_st2)||log->nast_.dir2==3,&log->bl_st2_wy); + + //sprawdzenie warunku napiciowego + log->dw.P_U |= log->w.kwampl_U_ > log->nast_.Ur; + log->dw.P_U &= log->w.kwampl_U_ > log->nast_.Up; + + u8 war1 = log->w.Io2h > log->nast_.I2h_min1; + u8 war2 = log->w.Io2h > log->nast_.I2h_min2; + + sprawdz_P(&log->dw.P2h1, + (log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_) && war1, + (log->w.Io2h < log->nast_.I1_2hr_powr * log->w.kwampl_I_) || !war1, + &log->dw.lp2h1,2,10); + + sprawdz_P(&log->dw.P2h2, + (log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_ && war2), + (log->w.Io2h < log->nast_.I1_2hr_powr * log->w.kwampl_I_) || !war2, + &log->dw.lp2h2,2,10); +/* + if(log->dw.Bl_2h1) + log->dw.Bl_2h1 = log->w.Io2h > log->nast_.I1_2hr_powr * log->w.kwampl_I_; + else + log->dw.Bl_2h1 = log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_; + + + if(log->dw.Bl_2h2) + log->dw.Bl_2h2 = log->w.Io2h > log->nast_.I1_2hr_powr * log->w.kwampl_I_; + else + log->dw.Bl_2h2 = log->w.Io2h > log->nast_.I1_2hr * log->w.kwampl_I_; + + + log->dw.Bl_2h1 &= log->nast_.Bl_2h1_ON; + log->dw.Bl_2h2 &= log->nast_.Bl_2h2_ON; +*/ + log->dw.Bl_2h1 = log->dw.P2h1 && log->nast_.Bl_2h1_ON; + log->dw.Bl_2h2 = log->dw.P2h2 && log->nast_.Bl_2h2_ON; + + + //Sprawdzenie przekaznikw pradowych + //szukanie maksymalnego pradu + ILmax = log->w.IL1; + if (ILmax < log->w.IL2) ILmax = log->w.IL2; + if (ILmax < log->w.IL3) ILmax = log->w.IL3; + + //sprawdzenie pobudzenia stopnia 1 + sprawdz_P(&log->dw.P1_pre, + !check_struct(&log->bl_st1) && wk && log->dw.P_U && /*!log->dw.Bl_2h1 &&*/ log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > (log->nast_.kh1 * ILmax), + check_struct(&log->bl_st1) || !wk || !log->dw.P_U ||/* log->dw.Bl_2h1 ||*/ log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < (log->nast_.khp1 * ILmax), + &log->dw.lp1_pre, + 20,3 + ); + + //sprawdzenie pobudzenia stopnia 2 + sprawdz_P(&log->dw.P2_pre, + !check_struct(&log->bl_st2) && wk2 && log->dw.P_U && /*!log->dw.Bl_2h2 &&*/ log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > log->nast_.kh2 * ILmax, + check_struct(&log->bl_st2) || !wk2 || !log->dw.P_U ||/* log->dw.Bl_2h2 ||*/ log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < log->nast_.khp2 * ILmax, + &log->dw.lp2_pre, + 20,3 + ); + + sprawdz_P(&log->dw.P1, + log->dw.P1_pre && !log->dw.Bl_2h1, + !log->dw.P1_pre || log->dw.Bl_2h1, + &log->dw.lp1, + 1,1 + ); + + sprawdz_P(&log->dw.P2, + log->dw.P2_pre && !log->dw.Bl_2h2, + !log->dw.P2_pre || log->dw.Bl_2h2, + &log->dw.lp2, + 1,1 + ); + + check_and_set_struct(log->dw.P1_pre && log->dw.Bl_2h1, &log->bl_2h_st1_wy); + check_and_set_struct(log->dw.P2_pre && log->dw.Bl_2h2, &log->bl_2h_st2_wy); + + //// + // blokady 2h po pobudzeniach + //// + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.P1 = log->dw.P2 = check_struct(&log->test)?1:0; + } + // + + // realizacja blokady dzialania przekaznika + if(log->nast_.bl_ && check_struct(&log->stan_bl_) /*| czy_start()*/) + log->dw.P1 = log->dw.P2 = 0; + // + + sprawdz_P(&log->dw.Ptyl1, + !wk && log->dw.P_U && ((!log->dw.Bl_2h1 && log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > (log->nast_.kh1 * ILmax))), + wk || !log->dw.P_U || ((log->dw.Bl_2h1 || log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < (log->nast_.khp1 * ILmax))), + &log->dw.lp5, + 20,3 + ); + sprawdz_P(&log->dw.Ptyl2, + !wk2 && log->dw.P_U && ((!log->dw.Bl_2h2 && log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > (log->nast_.kh2 * ILmax))), + wk2 || !log->dw.P_U || ((log->dw.Bl_2h2 || log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < (log->nast_.khp2 * ILmax))), + &log->dw.lp6, + 20,3 + ); + + if(log->nast_.dir == 3 || check_struct(&log->bl_st1)) + { + log->dw.P1=0; + log->dw.Ptyl1=0; + log->dw.P1_pre=0; + } + + if(log->nast_.dir2 == 3 || check_struct(&log->bl_st2)) + { + log->dw.P2=0; + log->dw.Ptyl2=0; + log->dw.P2_pre=0; + } + + log->dw.Z1 |= (log->dw.P1 && log->nast_.t0_1); // jesli dziaanie bezzwoczne stopnia 1 + log->dw.Z2 |= log->dw.P2 && log->nast_.t0_2; // jesli dziaanie bezzwoczne stopnia 2 + log->dw.Z = log->dw.Z1 || log->dw.Z2; + + if (log->dw.Z && !log->dw.Z_last) //zapisanie momentu zadziaania + { + log->dw.Z_last = 1; +// log->dw.czas_ = time; + } + + check_and_set_struct(log->dw.P1,&log->P1); + check_and_set_struct(log->dw.P2,&log->P2); + check_and_set_struct(log->dw.Z1,&log->Z1); + check_and_set_struct(log->dw.Z2,&log->Z2); + + check_and_set_struct(((log->dw.Z1 && log->nast_.w1) || (log->dw.Z2 && log->nast_.w2) || (log->l_echo && (args->params.bity & R67N_L_ECHO_WYL))) && !log->l_blok_wyl && !log->dw.Podwr,&log->W); + + if(args->params.bity & R67N_L_AKT) + { + //nadawanie + l_nrl=0; + + //odbior + if(!(args->params.bity & R67N_L_BLOK)) // gdy tryb zezwalajacy + { + if(log->dw.P1 && args->params.bity & R67N_L_NADP1) + { + l_nrl=1; + //log->l_nad_cnt=0; + } + + if(log->dw.P2 && args->params.bity & R67N_L_NADP2) + { + l_nrl=1; + //log->l_nad_cnt=0; + } + + if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.Podwr) + { + if(log->dw.P1) + log->skr_t1=1; + else + log->skr_t1=0; + + if(log->dw.P2) + log->skr_t2=1; + else + log->skr_t2=0; + + set_struct(&log->l_zezw); + } + else + { + clear_struct(&log->l_zezw); + if(!log->dw.P1 && !log->dw.P2) + { + log->skr_t1=0; + log->skr_t2=0; + } + } + } + else // tryb blokujacy + { + if(log->dw.P1 && !log->dw.P1_prev) + log->skr_t1=1; + + if(log->dw.P2 && !log->dw.P2_prev) + log->skr_t2=1; + + log->dw.P1_prev=log->dw.P1; + log->dw.P2_prev=log->dw.P2; + + if(log->dw.Ptyl1 && args->params.bity & R67N_L_NADP1) + { + l_nrl=1; + //log->l_nad_cnt=0; + } + + if(log->dw.Ptyl2 && args->params.bity & R67N_L_NADP2) + { + l_nrl=1; + //log->l_nad_cnt=0; + } + + if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.Podwr) + { + set_struct(&log->l_blok); + //log->l_blok_wyl=1; + + //dodane ponizej + log->skr_t1=0; + log->skr_t2=0; + } + else + { + if(!check_struct(&log->l_ok)) + { + log->skr_t1=0; + log->skr_t2=0; + } + clear_struct(&log->l_blok); + //log->l_blok_wyl=0; + } + } + // + + // echo + if(args->params.bity & R67N_L_ECHO) + { + if(!check_struct(&log->stan_bl_)) + { + sprawdz_P(&log->dw.PI0, + (log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > log->nast_.kh1 * ILmax)||(log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > log->nast_.kh2 * ILmax), + (log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < log->nast_.khp1 * ILmax)&&(log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < log->nast_.khp2 * ILmax), + &log->dw.lp3, + 5,200 + ); + + sprawdz_P(&log->dw.PU0, + (log->w.kwampl_U_ > log->nast_.U_echo), + (log->w.kwampl_U_ < (0.99 * log->nast_.U_echo)), + &log->dw.lp7, + 25,3 + ); + + + if(check_struct(&log->l_odb) && check_struct(&log->l_ok) && !log->dw.PI0 && log->dw.PU0 && !log->dw.Podwr) + { + l_nrl=1; + //log->l_nad_cnt=0; + log->l_echo=1; + set_struct(&log->l_echo_wy); + } + else + { + clear_struct(&log->l_echo_wy); + log->l_echo=0; + } + } + else + { + log->l_echo=0; + clear_struct(&log->l_echo_wy); + } + } + + // LRC + if(args->params.bity & R67N_L_ODWR_PR) + { + sprawdz_P(&log->dw.Podwr, + !wk && log->dw.P_U && ((!log->dw.Bl_2h1 && log->w.kwampl_I_ > log->nast_.Ir1 && log->w.kwampl_I_ > (log->nast_.kh1 * ILmax))||(!log->dw.Bl_2h2 && log->w.kwampl_I_ > log->nast_.Ir2 && log->w.kwampl_I_ > (log->nast_.kh2 * ILmax))), + wk || !log->dw.P_U || ((log->dw.Bl_2h1 || log->w.kwampl_I_ < log->nast_.Ip1 || log->w.kwampl_I_ < (log->nast_.khp1 * ILmax))&&(log->dw.Bl_2h2 || log->w.kwampl_I_ < log->nast_.Ip2 || log->w.kwampl_I_ < (log->nast_.khp2 * ILmax))), + &log->dw.lp4, + 40,log->nast_.l_t_blok + ); + + if(log->dw.Podwr) + { + set_struct(&log->l_rc); + l_nrl=0; + clear_struct(&log->NRL); + //log->l_nad_cnt=0; + } + else + clear_struct(&log->l_rc); + } + + // wyslanie impulsu nadawania + if(l_nrl && !log->l_nad_cnt) + set_struct(&log->NRL); + + if((log->l_nad_cnt>=log->nast_.l_nad_time && !l_nrl)|| log->l_nad_cnt>=log->nast_.l_nad_time_max) + { + clear_struct(&log->NRL); + + if(!l_nrl) + log->l_nad_cnt=0; + } + else if(check_struct(&log->NRL)) + log->l_nad_cnt+=LOOP_CYCLE_MS; + // + + + } + } + else + { + clear_struct(&log->Z1); + clear_struct(&log->Z2); + clear_struct(&log->W); + clear_struct(&log->P1); + clear_struct(&log->P2); + + clear_struct(&log->NRL); + clear_struct(&log->bl_2h_st1_wy); + clear_struct(&log->bl_2h_st2_wy); + } +} + +void r_67n_lwn2_100hz(void *arguments, void *logic) +{ + struct r_67n_lwn2_logic *log = (struct r_67n_lwn2_logic *)logic; + int t; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + // realizacja przekaznika czasowego + if (log->dw.P1) log->dw.lt1++; else log->dw.lt1 = 0; + if (log->dw.P2) log->dw.lt2++; else log->dw.lt2 = 0; + + if(log->skr_t1 && log->nast_.t1_minnast_.t1) + t=log->nast_.t1_min; + else + t=log->nast_.t1; + + if (log->dw.lt1 > t) + { + log->dw.Z1 = 1; + log->dw.lt1--; + } + + if(log->skr_t2 && log->nast_.t2_minnast_.t2) + t=log->nast_.t2_min; + else + t=log->nast_.t2; + + if (log->dw.lt2 > t) + { + log->dw.Z2 = 1; + log->dw.lt2--; + } + + // + //kasowanie zadziaa po odpadniciu pobudze + log->dw.P = log->dw.P1 || log->dw.P2; + + if(!log->dw.P1) + log->dw.Z1=0; + if(!log->dw.P2) + log->dw.Z2=0; + if(!log->dw.P) + log->dw.Z=0; + + log->dw.Z_last = log->dw.Z; + + // realizacja rejestratora parametrow ostatniego zaklocenia + switch(status_rej) + { + case oczekiwanie: + if (log->dw.P) + { + log->dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + log->dw.max_orta_I = log->w.orta_I_; + log->dw.max_ortb_I = log->w.ortb_I_; + log->dw.max_orta_U = log->w.orta_U_; + log->dw.max_ortb_U = log->w.ortb_U_; + log->dw.max_I = log->w.kwampl_I_; + log->dw.max_U = log->w.kwampl_U_; + log->dw.I1 = log->w.IL1; + log->dw.I2 = log->w.IL2; + log->dw.I3 = log->w.IL3; + } + break; + case sledzenie: + if (log->dw.Z) + { + status_rej = sledzenie_zad; + //log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (log->dw.P) + { + if(log->w.kwampl_I_>log->dw.max_I) + { + log->dw.max_orta_I = log->w.orta_I_; + log->dw.max_ortb_I = log->w.ortb_I_; + log->dw.max_orta_U = log->w.orta_U_; + log->dw.max_ortb_U = log->w.ortb_U_; + log->dw.max_I = log->w.kwampl_I_; + log->dw.max_U = log->w.kwampl_U_; + log->dw.I1 = log->w.IL1; + log->dw.I2 = log->w.IL2; + log->dw.I3 = log->w.IL3; + log->dw.Io2h = log->w.Io2h; + } + + log->dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!log->dw.P) + { + log->dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + + if(log->w.kwampl_I_>log->dw.max_I) + { + log->dw.max_orta_I = log->w.orta_I_; + log->dw.max_ortb_I = log->w.ortb_I_; + log->dw.max_orta_U = log->w.orta_U_; + log->dw.max_ortb_U = log->w.ortb_U_; + log->dw.max_I = log->w.kwampl_I_; + log->dw.max_U = log->w.kwampl_U_; + log->dw.I1 = log->w.IL1; + log->dw.I2 = log->w.IL2; + log->dw.I3 = log->w.IL3; + log->dw.Io2h = log->w.Io2h; + } + + + log->dw.dl_++; + } + break; + case zapis: + if (!log->dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + } + + } +} + +void r_67n_lwn2_20hz(void *arguments, void *logic) +{ + struct r_67n_lwn2_logic *log = (struct r_67n_lwn2_logic *)logic; + + // czesc ktora miala byc wykonywana max co 1s w wolnym czasie + if(log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + { + log->dw.R.max_I_ = sqrt(log->dw.max_I); // wyliczenie maksymalnej wartosci pradu + log->dw.R.max_U_ = sqrt(log->dw.max_U); // wyliczenie wartosci napiecia + log->dw.R.I1 = sqrt(log->dw.I1); + log->dw.R.I2 = sqrt(log->dw.I2); + log->dw.R.I3 = sqrt(log->dw.I3); + log->dw.R.Io2h = sqrt(log->dw.Io2h); + log->dw.R.max_kat_ = (180 / 3.14159f) * (atan2(log->dw.max_ortb_I,log->dw.max_orta_I) - atan2(log->dw.max_ortb_U,log->dw.max_orta_U));// wyliczenie kata + // log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // log->dw.R.dlugosc = (float)log->dw.dl_ * 0.01f; // / 100; // wpisanie czasu trwania zwarcia + // log->dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + } + +} diff --git a/src/R_67N_LWN2.h b/src/R_67N_LWN2.h new file mode 100644 index 0000000..4544600 --- /dev/null +++ b/src/R_67N_LWN2.h @@ -0,0 +1,262 @@ +/* + * R_67N_LWN2.h + * + * Created on: 14-11-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef R_67N_LWN2_H_ +#define R_67N_LWN2_H_ + +#include "tdefs.h" +#include "helper.h" + +#define R67N_L_AKT 0x0040 +#define R67N_L_NADP1 0x0080 +#define R67N_L_NADP2 0x0100 +#define R67N_L_ECHO 0x0200 +#define R67N_L_ECHO_WYL 0x0400 +#define R67N_L_BLOK 0x0800 +#define R67N_L_ODWR_PR 0x1000 + +struct Wyjscia_rejestratora_lwn2 +{ + float max_I_;///< maksymalna watosc pradu w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_U_;///< watosc napiecia w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_kat_; ///< watosc kata w czasie zaklocenia w stopniach + float I1; ///< prad fazy L1 + float I2; ///< prad fazy L2 + float I3; ///< prad fazy L3 + float Io2h; ///< pr¹d drugiej harmonicznej +}; + + +struct Nastawy_przeliczone_lwn2 +{ + float Ir1; ///< wartosc rozruchowa pradu stopieñ 1 + float Ip1; ///< wartosc powrotu pradu rozruchowego stopieñ 1 + float Ir2; ///< wartosc rozruchowa pradu stopieñ 2 + float Ip2; ///< wartosc powrotu pradu rozruchowego stopieñ 2 + float kh1; ///< wspólczynnik stabilizacji zadzia³ania stopieñ 1 + float kh2; ///< wspólczynnik stabilizacji zadzia³ania stopieñ 2 + float khp1; ///< wspolczynnik stabilizacji dla powrotu stopieñ 1 + float khp2; ///< wspolczynnik stabilizacji dla powrotu stopieñ 2 + float I1_2hr; ///< wartoœæ rozruchowa blokady drugiej harmonicznej stopieñ 1 + float I1_2hr_powr; ///< wartoœæ rozruchowa blokady drugiej harmonicznej stopieñ 1 + float U_echo; ///< wartosc rozruchowa echa + float Ur; ///< wartosc rozruchowa napiecia blokady + float Up; ///< wartosc powrotu napiecia blokady + float orta; ///< ortogonalna a dla wektora nasatwy kata charakterystycznego + float ortb; ///< ortogonalna b dla wektora nasatwy kata charakterystycznego + short dir; ///< nastawa kierunku 2 - do przodu; 1 - w obu kierunkach; 0 - do tylu + int t1; ///< opoznienie czasu zadzialania przekaznika + int t2; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 w1; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 w2; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 t0_1; ///< przekaŸnik bezzw³oczny stopieñ 1 + u8 t0_2; ///< przekaŸnik bezzw³oczny stopieñ 2 + u8 Bl_2h1_ON; ///< W³¹czenie blokady od drugiej harmonicznej w stopniu 1 + u8 Bl_2h2_ON; ///< W³¹czenie blokady od drugiej harmonicznej w stopniu 2 + u32 l_nad_time; + u32 l_nad_time_max; + u32 t1_min; + u32 t2_min; + u32 l_t_blok; + short dir2; ///< nastawa kierunku stop. 2, 2 - do przodu; 1 - w obu kierunkach; 0 - do tylu + float I2h_min1; + float I2h_min2; +}; + +struct Wejscie_lwn2 +{ + float orta_I_; ///< ortogonalna a pradu wejsciowego Io + float ortb_I_; ///< ortogonalna b pradu wejsciowego Io + float kwampl_I_; ///< kwadrat amplitudy pr¹du + float orta_U_; ///< ortogonalna b napiecia wejsciowego Uo + float ortb_U_; ///< ortogonalna b napiecia wejsciowego Uo + float kwampl_U_; ///< kwadrat amplitudy napiecia + float IL1; ///< kwadrat amplitudy pr¹du faza L1 + float IL2; ///< kwadrat amplitudy pr¹du faza L2 + float IL3; ///< kwadrat amplitudy pr¹du faza L3 + float Io2h;///< kwadrat amplitudy drugiej harmonicznej sk³adowej zerowej pr¹du +}; + +struct dane_wewnetrzne_lwn2 +{ + struct Wyjscia_rejestratora_lwn2 R; ///< dane ostatniego zaklocenia + long long czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + u32 dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + u8 wsk_rej; // nowy zapis rejestracji parametrow zaklocenia (spelnienie kryterium identyfikacji) + + float max_I;///< maksymalna watosc pradu w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_U;///< watosc napiecia w czasie zaklocenia w krotnosciach warotosci znamionowej + float max_kat; ///< watosc kata w czasie zaklocenia w stopniach + float I1; ///< prad fazy L1 + float I2; ///< prad fazy L2 + float I3; ///< prad fazy L3 + float Io2h;///< prad drugiej harmonicznej + + float max_orta_I; + float max_ortb_I; + float max_orta_U; + float max_ortb_U; + + short lp1;///< licznikpobudzen stopieñ 1 + short lp2;///< licznikpobudzen stopieñ 2 + short lp1_pre;///< licznikpobudzen stopieñ 1 + short lp2_pre;///< licznikpobudzen stopieñ 2 + short lp3;///< licznikpobudzen I0 do echa + short lp4;///< licznikpobudzen w LRC + short lp5;/// + short lp6;/// + short lp7; + int lt1; + int lt2; + u8 Podwr; + u8 Ptyl1; + u8 P1_prev; + u8 Ptyl2; + u8 P2_prev; + u8 PI0; // pobudzenie i0 do echa + u8 P; // pobudzenie i0 do echa + u8 P1; + u8 P2; + u8 P1_pre; + u8 P2_pre; + u8 Z; + u8 Z1; + u8 Z2; + u8 P_U; + u8 Bl_2h1; + u8 Bl_2h2; + u8 kierunek; + u8 Z_last; + u8 P2h1; + u8 P2h2; + u8 PU0; //pobudzenie dla echa + short lp2h1; + short lp2h2; +}; + +struct r_67n_lwn2_logic +{ + struct binary_io stan_bl_; + + float *orta_I_; + float *ortb_I_; + float *kwampl_I_; + + float *orta_U_; + float *ortb_U_; + float *kwampl_U_; + + float *IL1; + float *IL2; + float *IL3; + float *Io2h; + + struct binary_io test; + struct binary_io l_odb; + struct binary_io l_ok; + struct binary_io W; + struct binary_io Z1; + struct binary_io Z2; + struct binary_io P1; + struct binary_io P2; + struct binary_io kier; + struct binary_io NRL; + struct binary_io bl_st1; + struct binary_io bl_st2; + struct binary_io bl_st1_wy; + struct binary_io bl_st2_wy; + struct binary_io bl_2h_st1_wy; + struct binary_io bl_2h_st2_wy; + struct binary_io l_blok; + struct binary_io l_zezw; + struct binary_io l_echo_wy; + struct binary_io l_rc; + + struct Nastawy_przeliczone_lwn2 nast_; + struct dane_wewnetrzne_lwn2 dw; + struct Wejscie_lwn2 w; + + u8 l_echo; + u8 skr_t1; + u8 skr_t2; + u8 l_blok_wyl; + u32 l_nad_cnt; +}; + +struct r_67n_lwn2_io +{ + u32 stan_bl_in; + u32 orta_I_float_in; + u32 ortb_I_float_in; + u32 kwampl_I_float_in; + u32 orta_U_float_in; + u32 ortb_U_float_in; + u32 kwampl_U_float_in; + u32 IL1_float_in; + u32 IL2_float_in; + u32 IL3_float_in; + u32 Io2h_float_in; + u32 test_in; + u32 l_odb_in; + u32 l_ok_in; + u32 bl_st1_in; + u32 bl_st2_in; + u32 w_out; + u32 z1_out; + u32 z2_out; + u32 p1_out; + u32 p2_out; + u32 kier_out; + u32 NRL_out; + u32 bl_st1_out; + u32 bl_st2_out; + u32 bl_2h_st1_out; + u32 bl_2h_st2_out; + u32 l_blok_out; + u32 l_zezw_out; + u32 l_echo_out; + u32 l_rc_out; +}__attribute__((__packed__)); + +struct r_67n_lwn2_params +{ + u32 bity; ///< nastawy bitowe; + u32 dir; ///< nastawa kierunku dzialania przekaznika + double U; ///< napiecie rozruchowe przekaznika w krotnosciach wartosci znamionowej + double kat; ///< wartosc nastawy kata charakterystycznego w stopniach + double I1; ///< prad rozruchowy przekaznika stopien 1 + double I2; ///< prad rozruchowy przekaznika stopien 2 + double kh1; ///< wspó³czynnik stabilizacji stopieñ 1 + double kh2; ///< wspó³czynnik stabilizacji stopieñ 2 + double t1; ///< wartosc nastawy czasu dzialania w sekundach stopiñ 1 + double t2; ///< wartosc nastawy czasu dzialania w sekundach stopiñ 2 + double t1_min; // czas t1 przy skróceniu + double t2_min; // czas t1 przy skróceniu + double Io2h1; /// + + +#include "tdefs.h" +#include "misc.h" +#include "helper.h" +#include "analog_in.h" + +#include "ZDistA.h" + +#define RnaS (3.141593 / 180) +#define filtr_P 5,5 + +struct nast_pomoc +{ + float R1W_Zf1W_LE; + float R1W_Zf1W_LL; + float R1_Zf1_LE; + float R1_Zf1_LL; + float R2_Zf2_LE; + float R2_Zf2_LL; + float R3_Zf3_LE; + float R3_Zf3_LL; + float R4_Zf4_LE; + float R4_Zf4_LL; + float R5_Zf5_LE; + float R5_Zf5_LL; + float X1W_Zr1W_LE; + float X1W_Zr1W_LL; + float X1_Zr1_LE; + float X1_Zr1_LL; + float X2_Zr2_LE; + float X2_Zr2_LL; + float X3_Zr3_LE; + float X3_Zr3_LL; + float X4_Zr4_LE; + float X4_Zr4_LL; + float X5_Zr5_LE; + float X5_Zr5_LL; +}; + +int ZDistA_initlog(void *arguments, void *logic) +{ + struct ZDistA_args *args = (struct ZDistA_args *)arguments; + struct ZDistA_logic *log = (struct ZDistA_logic *)logic; + float locX1,locX2,locR1,locR2; + float sink,cosk; + u16 i; + + if(set_bit_ptr_struct(args->io.bl_in,&log->stan_bl)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_k_in,&log->Bl_K)) + return -1; + + if(set_float_ptr(args->io.i1_orta_float_in,&log->I1_orta)) + return -1; + if(set_float_ptr(args->io.i1_ortb_float_in,&log->I1_ortb)) + return -1; + if(set_float_ptr(args->io.i1_float_in,&log->I1)) + return -1; + if(set_float_ptr(args->io.i2_orta_float_in,&log->I2_orta)) + return -1; + if(set_float_ptr(args->io.i2_ortb_float_in,&log->I2_ortb)) + return -1; + if(set_float_ptr(args->io.i2_float_in,&log->I2)) + return -1; + if(set_float_ptr(args->io.i3_orta_float_in,&log->I3_orta)) + return -1; + if(set_float_ptr(args->io.i3_ortb_float_in,&log->I3_ortb)) + return -1; + if(set_float_ptr(args->io.i3_float_in,&log->I3)) + return -1; + + if(set_float_ptr(args->io.u1_orta_float_in,&log->U1_orta)) + return -1; + if(set_float_ptr(args->io.u1_ortb_float_in,&log->U1_ortb)) + return -1; + if(set_float_ptr(args->io.u1_float_in,&log->U1)) + return -1; + if(set_float_ptr(args->io.u2_orta_float_in,&log->U2_orta)) + return -1; + if(set_float_ptr(args->io.u2_ortb_float_in,&log->U2_ortb)) + return -1; + if(set_float_ptr(args->io.u2_float_in,&log->U2)) + return -1; + if(set_float_ptr(args->io.u3_orta_float_in,&log->U3_orta)) + return -1; + if(set_float_ptr(args->io.u3_ortb_float_in,&log->U3_ortb)) + return -1; + if(set_float_ptr(args->io.u3_float_in,&log->U3)) + return -1; + + if(set_float_ptr(args->io.u12_orta_float_in,&log->U12_orta)) + return -1; + if(set_float_ptr(args->io.u12_ortb_float_in,&log->U12_ortb)) + return -1; + if(set_float_ptr(args->io.u12_float_in,&log->U12)) + return -1; + if(set_float_ptr(args->io.u23_orta_float_in,&log->U23_orta)) + return -1; + if(set_float_ptr(args->io.u23_ortb_float_in,&log->U23_ortb)) + return -1; + if(set_float_ptr(args->io.u23_float_in,&log->U23)) + return -1; + if(set_float_ptr(args->io.u31_orta_float_in,&log->U31_orta)) + return -1; + if(set_float_ptr(args->io.u31_ortb_float_in,&log->U31_ortb)) + return -1; + if(set_float_ptr(args->io.u31_float_in,&log->U31)) + return -1; + + if(set_float_ptr(args->io.i1_zg_orta_float_in,&log->sI1_orta)) + return -1; + if(set_float_ptr(args->io.i1_zg_ortb_float_in,&log->sI1_ortb)) + return -1; + if(set_float_ptr(args->io.i1_zg_float_in,&log->sI1)) + return -1; + + if(set_float_ptr(args->io.i2_pr_orta_float_in,&log->sI2_orta)) + return -1; + if(set_float_ptr(args->io.i2_pr_ortb_float_in,&log->sI2_ortb)) + return -1; + if(set_float_ptr(args->io.i2_pr_float_in,&log->sI2)) + return -1; + + if(set_float_ptr(args->io.io_orta_float_in,&log->sI0_orta)) + return -1; + if(set_float_ptr(args->io.io_ortb_float_in,&log->sI0_ortb)) + return -1; + if(set_float_ptr(args->io.io_float_in,&log->sI0)) + return -1; + + if(set_float_ptr(args->io.u1_zg_orta_float_in,&log->sU1_orta)) + return -1; + if(set_float_ptr(args->io.u1_zg_ortb_float_in,&log->sU1_ortb)) + return -1; + if(set_float_ptr(args->io.u1_zg_float_in,&log->sU1)) + return -1; + + if(set_float_ptr(args->io.u2_pr_orta_float_in,&log->sU2_orta)) + return -1; + if(set_float_ptr(args->io.u2_pr_ortb_float_in,&log->sU2_ortb)) + return -1; + if(set_float_ptr(args->io.u2_pr_float_in,&log->sU2)) + return -1; + + if(set_float_ptr(args->io.uo_orta_float_in,&log->sU0_orta)) + return -1; + if(set_float_ptr(args->io.uo_ortb_float_in,&log->sU0_ortb)) + return -1; + if(set_float_ptr(args->io.uo_float_in,&log->sU0)) + return -1; + + if(set_pointer_in_ptr(args->io.i_param_an_ptr_in,(u32 *)&log->param_I)) + return -1; + + if(set_pointer_in_ptr(args->io.u_param_an_ptr_in,(u32 *)&log->param_U)) + return -1; + + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.deakt_in,&log->deakt)) + return -1; + + if(set_bit_ptr_struct(args->io.P1W_L1E_out,&log->P1W_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L2E_out,&log->P1W_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L3E_out,&log->P1W_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P1W_L1L2_out,&log->P1W_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L2L3_out,&log->P1W_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L3L1_out,&log->P1W_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P1_L1E_out,&log->P1_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L2E_out,&log->P1_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L3E_out,&log->P1_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P1_L1L2_out,&log->P1_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L2L3_out,&log->P1_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L3L1_out,&log->P1_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P2_L1E_out,&log->P2_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L2E_out,&log->P2_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L3E_out,&log->P2_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P2_L1L2_out,&log->P2_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L2L3_out,&log->P2_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L3L1_out,&log->P2_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P3_L1E_out,&log->P3_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L2E_out,&log->P3_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L3E_out,&log->P3_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P3_L1L2_out,&log->P3_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L2L3_out,&log->P3_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L3L1_out,&log->P3_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P4_L1E_out,&log->P4_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L2E_out,&log->P4_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L3E_out,&log->P4_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P4_L1L2_out,&log->P4_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L2L3_out,&log->P4_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L3L1_out,&log->P4_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P5_L1E_out,&log->P5_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L2E_out,&log->P5_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L3E_out,&log->P5_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P5_L1L2_out,&log->P5_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L2L3_out,&log->P5_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L3L1_out,&log->P5_L3L1)) + return -1; + + if(set_pointer_out_ptr(args->io.wy_ptr_out,(u32 *)&log->nast_.wyjscie)) + return -1; + + //debug + if(set_float_ptr(args->io.z1_float_out,&log->z[0])) + return -1; + if(set_float_ptr(args->io.z2_float_out,&log->z[1])) + return -1; + if(set_float_ptr(args->io.z3_float_out,&log->z[2])) + return -1; + if(set_float_ptr(args->io.z4_float_out,&log->z[3])) + return -1; + if(set_float_ptr(args->io.z5_float_out,&log->z[4])) + return -1; + if(set_float_ptr(args->io.z6_float_out,&log->z[5])) + return -1; + + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0002)?1:0; + log->nast_.Bl_L[1] = (args->params.bity & 0x0004)?1:0; + log->nast_.Bl_L[2] = (args->params.bity & 0x0008)?1:0; + log->nast_.Bl_L[3] = (args->params.bity & 0x0010)?1:0; + log->nast_.Bl_L[4] = (args->params.bity & 0x0020)?1:0; + log->nast_.Bl_L[5] = (args->params.bity & 0x0040)?1:0; + log->nast_.Bl_L[0] = (args->params.bity & 0x0080)?1:0; + + //wyliczenie przekladni impedancyjnej + log->nast_.wyjscie.przekladnia = log->param_I->znam_wtor / log->param_U->znam_wtor; + log->nast_.wyjscie.Zdist_dw = &log->dw; + log->nast_.wyjscie.on = &log->nast_.on_; + + //wyliczenie wektorow kompensacji ziemnozwarciowej + log->nast_.ReK1 = 3 * args->params.Kk1 * cos(RnaS * -args->params.Kk1_kat); + log->nast_.ImK1 = 3 * args->params.Kk1 * sin(RnaS * -args->params.Kk1_kat); + log->nast_.ReKr = 3 * args->params.KkC * cos(RnaS * -args->params.KkC_kat); + log->nast_.ImKr = 3 * args->params.KkC * sin(RnaS * -args->params.KkC_kat); + + log->nast_.kp = args->params.kp; + log->nast_.kpp = 1/args->params.kp; + log->nast_.kpk = log->nast_.kpp * log->nast_.kpp; + + log->nast_.typ[0] = args->params.Typ0; + log->nast_.typ[1] = args->params.Typ1; + log->nast_.typ[2] = args->params.Typ2; + log->nast_.typ[3] = args->params.Typ3; + log->nast_.typ[4] = args->params.Typ4; + log->nast_.typ[5] = args->params.Typ5; + + sink = sin(RnaS * args->params.fi1); + cosk = cos(RnaS * args->params.fi1); + + log->nast_.tanfi2 = tan(RnaS * args->params.fi2); + log->nast_.tanfi1 = tan(RnaS * args->params.fi1); + + + struct nast_pomoc n_pomoc; + + n_pomoc.R1W_Zf1W_LE = (float)args->params.R1W_Zf1W_LE; + n_pomoc.R1W_Zf1W_LL = (float)args->params.R1W_Zf1W_LL; + n_pomoc.R1_Zf1_LE = (float)args->params.R1_Zf1_LE; + n_pomoc.R1_Zf1_LL = (float)args->params.R1_Zf1_LL; + n_pomoc.R2_Zf2_LE = (float)args->params.R2_Zf2_LE; + n_pomoc.R2_Zf2_LL = (float)args->params.R2_Zf2_LL; + n_pomoc.R3_Zf3_LE = (float)args->params.R3_Zf3_LE; + n_pomoc.R3_Zf3_LL = (float)args->params.R3_Zf3_LL; + n_pomoc.R4_Zf4_LE = (float)args->params.R4_Zf4_LE; + n_pomoc.R4_Zf4_LL = (float)args->params.R4_Zf4_LL; + n_pomoc.R5_Zf5_LE = (float)args->params.R5_Zf5_LE; + n_pomoc.R5_Zf5_LL = (float)args->params.R5_Zf5_LL; + n_pomoc.X1W_Zr1W_LE = (float)args->params.X1W_Zr1W_LE; + n_pomoc.X1W_Zr1W_LL = (float)args->params.X1W_Zr1W_LL; + n_pomoc.X1_Zr1_LE = (float)args->params.X1_Zr1_LE; + n_pomoc.X1_Zr1_LL = (float)args->params.X1_Zr1_LL; + n_pomoc.X2_Zr2_LE = (float)args->params.X2_Zr2_LE; + n_pomoc.X2_Zr2_LL = (float)args->params.X2_Zr2_LL; + n_pomoc.X3_Zr3_LE = (float)args->params.X3_Zr3_LE; + n_pomoc.X3_Zr3_LL = (float)args->params.X3_Zr3_LL; + n_pomoc.X4_Zr4_LE = (float)args->params.X4_Zr4_LE; + n_pomoc.X4_Zr4_LL = (float)args->params.X4_Zr4_LL; + n_pomoc.X5_Zr5_LE = (float)args->params.X5_Zr5_LE; + n_pomoc.X5_Zr5_LL = (float)args->params.X5_Zr5_LL; + + for (i=0;i<12;i++) + { + if (log->nast_.typ[i/2]) + { + log->nast_.n_pol[i/2][i%2].Z = (*(float*)(&n_pomoc.R1W_Zf1W_LE + i) + *(float*)(&n_pomoc.X1W_Zr1W_LE + i)) * log->nast_.wyjscie.przekladnia / 2; + log->nast_.n_pol[i/2][i%2].Zp = log->nast_.n_pol[i/2][i%2].Z * args->params.kp; + log->nast_.n_pol[i/2][i%2].Z *= log->nast_.n_pol[i/2][i%2].Z; + log->nast_.n_pol[i/2][i%2].Zp *= log->nast_.n_pol[i/2][i%2].Zp; + locR1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * cosk; + locR2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * cosk; + locX1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * sink; + locX2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * sink; + log->nast_.n_pol[i/2][i%2].Rr = (locR1 - (locR1 + locR2 ) / 2) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Xr = (locX1 - (locX1 + locX2 ) / 2) * log->nast_.wyjscie.przekladnia; + locR1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * cosk * args->params.kp; + locR2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * cosk * args->params.kp; + locX1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * sink * args->params.kp; + locX2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * sink * args->params.kp; + log->nast_.n_pol[i/2][i%2].Rp = (locR1 - (locR1 + locR2 ) / 2) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Xp = (locX1 - (locX1 + locX2 ) / 2) * log->nast_.wyjscie.przekladnia; + + + } else { + log->nast_.n_pol[i/2][i%2].Rr = *(float *)(&n_pomoc.R1W_Zf1W_LE + i) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Xr = *(float *)(&n_pomoc.X1W_Zr1W_LE + i) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Rp = log->nast_.n_pol[i/2][i%2].Rr * args->params.kp; + log->nast_.n_pol[i/2][i%2].Xp = log->nast_.n_pol[i/2][i%2].Xr * args->params.kp; + + log->nast_.n_pol[i/2][i%2].Rrtanfi1 = log->nast_.n_pol[i/2][i%2].Rr * log->nast_.tanfi1; + log->nast_.n_pol[i/2][i%2].Rptanfi1 = log->nast_.n_pol[i/2][i%2].Rp * log->nast_.tanfi1; + + } + } + + + + log->nast_.Xr1f = args->params.X1_Zr1_LE * (1 + log->nast_.tanfi2 / log->nast_.tanfi1) * log->nast_.wyjscie.przekladnia; + log->nast_.Xr1fp = log->nast_.Xr1f * args->params.kp; + log->nast_.Xr1Wf = args->params.X1W_Zr1W_LE * (1 + log->nast_.tanfi2 / log->nast_.tanfi1) * log->nast_.wyjscie.przekladnia; + log->nast_.Xr1Wfp = log->nast_.Xr1Wf * args->params.kp; + log->nast_.Igr = args->params.I_min * args->params.I_min; + log->nast_.Igrp = log->nast_.Igr * log->nast_.kpk; + + log->nast_.Zgr = 0.01f * log->nast_.wyjscie.przekladnia * log->nast_.wyjscie.przekladnia; + log->nast_.kierunek[0] = args->params.K0; + log->nast_.kierunek[1] = args->params.K1; + log->nast_.kierunek[2] = args->params.K2; + log->nast_.kierunek[3] = args->params.K3; + log->nast_.kierunek[4] = args->params.K4; + log->nast_.kierunek[5] = args->params.K5; + +//A2 + + log->nast_.XKR = -tan(RnaS * args->params.fi4); + log->nast_.XKX = -tan(RnaS * (90 - args->params.fi3)); + log->nast_.KL = tan(RnaS * args->params.fi5); + log->nast_.RLf = args->params.RLf; + log->nast_.RLr = - args->params.RLr; + log->nast_.Iogr = args->params.Iomin * args->params.Iomin * 0.11111f; + log->nast_.khio = args->params.Iokh * args->params. Iokh * 0.11111f; + log->nast_.Uomin = args->params.Uomin * args->params.Uomin * 0.11111f; + + log->dw.log_ptr = log; + log->nast_.wyjscie.SOTF_zwrotnie = 0xFF; + + log->l_nieustalony = 5; + + return 0; +} + +void ZDistA(void *arguments, void *logic) +{ + struct ZDistA_logic *log = (struct ZDistA_logic *)logic; + float KIoorta,KIoortb,Iorta,Iortb,I,Uorta,Uortb; + u8 wk1,wk2,wk1p,wk2p; + u16 i,j; + float local,localp,local0,local0p; + float local1,local1p,local2,local2p,local3,local3p; + + log->dw.I1 = *log->I1; + log->dw.I2 = *log->I2; + log->dw.I3 = *log->I3; + log->dw.U1 = *log->U1; + log->dw.U2 = *log->U2; + log->dw.U3 = *log->U3; + + if(log->nast_.on_ && !check_struct(&log->deakt)) // jesli zabezpieczenie aktywne + { + + //wyliczenie impedancji petli zwarciowych + //-------------------------------------------------------------------------------- + //wyliczenie K * 3Io dla strefy pierwszej + KIoorta = *log->sI0_orta * log->nast_.ReK1 - + *log->sI0_ortb * log->nast_.ImK1; + KIoortb = *log->sI0_orta * log->nast_.ImK1 + + *log->sI0_ortb * log->nast_.ReK1; + + + //petla zwarciowa L1-E strefa pierwsza + // I = I1 - k * 3 Io + Iorta = *log->I1_orta + KIoorta; + Iortb = *log->I1_ortb + KIoortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - k * 3Io) + log->dw.Z.R[0] = (*log->U1_orta * Iorta + + *log->U1_ortb * Iortb) * I; + log->dw.Z.X[0] = (*log->U1_orta * Iortb - + *log->U1_ortb * Iorta) * I; + log->dw.Z.Z[0] = log->dw.Z.X[0] * log->dw.Z.X[0] + log->dw.Z.R[0] * log->dw.Z.R[0]; + + //petla zwarciowa L2-E strefa pierwsza + // I = I2 - k * 3 Io + Iorta = *log->I2_orta + KIoorta; + Iortb = *log->I2_ortb + KIoortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I2 - k * 3Io) + log->dw.Z.R[1] = (*log->U2_orta * Iorta + + *log->U2_ortb * Iortb) * I; + log->dw.Z.X[1] = (*log->U2_orta * Iortb - + *log->U2_ortb * Iorta) * I; + log->dw.Z.Z[1] = log->dw.Z.X[1] * log->dw.Z.X[1] + log->dw.Z.R[1] * log->dw.Z.R[1]; + + //petla zwarciowa L3-E strefa pierwsza + // I = I3 - k * 3 Io + Iorta = *log->I3_orta + KIoorta; + Iortb = *log->I3_ortb + KIoortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I3 - k * 3Io) + log->dw.Z.R[2] = (*log->U3_orta * Iorta + + *log->U3_ortb * Iortb) * I; + log->dw.Z.X[2] = (*log->U3_orta * Iortb - + *log->U3_ortb * Iorta) * I; + log->dw.Z.Z[2] = log->dw.Z.X[2] * log->dw.Z.X[2] + log->dw.Z.R[2] * log->dw.Z.R[2]; + + + //wyliczenie K * 3Io dla pozostalych stref + KIoorta = *log->sI0_orta * log->nast_.ReKr - + *log->sI0_ortb * log->nast_.ImKr; + KIoortb = *log->sI0_orta * log->nast_.ImKr + + *log->sI0_ortb * log->nast_.ReKr; + + //petla zwarciowa L1-E pozostale strefy + // I = I1 - k * 3 Io + Iorta = *log->I1_orta + KIoorta; + Iortb = *log->I1_ortb + KIoortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - k * 3Io) + log->dw.Z.R[3] = (*log->U1_orta * Iorta + + *log->U1_ortb * Iortb) * I; + log->dw.Z.X[3] = (*log->U1_orta * Iortb - + *log->U1_ortb * Iorta) * I; + log->dw.Z.Z[3] = log->dw.Z.X[3] * log->dw.Z.X[3] + log->dw.Z.R[3] * log->dw.Z.R[3]; + + //petla zwarciowa L2-E pozostale strefy + // I = I2 - k * 3 Io + Iorta = *log->I2_orta + KIoorta; + Iortb = *log->I2_ortb + KIoortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I2 - k * 3Io) + log->dw.Z.R[4] = (*log->U2_orta * Iorta + + *log->U2_ortb * Iortb) * I; + log->dw.Z.X[4] = (*log->U2_orta * Iortb - + *log->U2_ortb * Iorta) * I; + log->dw.Z.Z[4] = log->dw.Z.X[4] * log->dw.Z.X[4] + log->dw.Z.R[4] * log->dw.Z.R[4]; + + //petla zwarciowa L3-E pozostale strefy + // I = I3 - k * 3 Io + Iorta = *log->I3_orta + KIoorta; + Iortb = *log->I3_ortb + KIoortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I3 - k * 3Io) + log->dw.Z.R[5] = (*log->U3_orta * Iorta + + *log->U3_ortb * Iortb) * I; + log->dw.Z.X[5] = (*log->U3_orta * Iortb - + *log->U3_ortb * Iorta) * I; + log->dw.Z.Z[5] = log->dw.Z.X[5] * log->dw.Z.X[5] + log->dw.Z.R[5] * log->dw.Z.R[5]; + + //petla zwarciowa L1-L2 wszystkie strefy + // I = I1 - I2 + Iorta = *log->I1_orta - *log->I2_orta; + Iortb = *log->I1_ortb - *log->I2_ortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - I2) + log->dw.Z.R[6] = (*log->U12_orta * Iorta + + *log->U12_ortb * Iortb) * I; + log->dw.Z.X[6] = (*log->U12_orta * Iortb - + *log->U12_ortb * Iorta) * I; + log->dw.Z.Z[6] = log->dw.Z.X[6] * log->dw.Z.X[6] + log->dw.Z.R[6] * log->dw.Z.R[6]; + + //*log->z[0]=log->dw.Z.R[6]; + //*log->z[1]=log->dw.Z.X[6]; + //petla zwarciowa L2-L3 wszystkie strefy + // I = I2 - I3 + Iorta = *log->I2_orta - *log->I3_orta; + Iortb = *log->I2_ortb - *log->I3_ortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - I2) + log->dw.Z.R[7] = (*log->U23_orta * Iorta + + *log->U23_ortb * Iortb) * I; + log->dw.Z.X[7] = (*log->U23_orta * Iortb - + *log->U23_ortb * Iorta) * I; + log->dw.Z.Z[7] = log->dw.Z.X[7] * log->dw.Z.X[7] + log->dw.Z.R[7] * log->dw.Z.R[7]; + //*log->z[2]=log->dw.Z.R[7]; + //*log->z[3]=log->dw.Z.X[7]; + + //petla zwarciowa L3-L1 wszystkie strefy + // I = I3 - I1 + Iorta = *log->I3_orta - *log->I1_orta; + Iortb = *log->I3_ortb - *log->I1_ortb; + I = _rcpsp(Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - I2) + log->dw.Z.R[8] = (*log->U31_orta * Iorta + + *log->U31_ortb * Iortb) * I; + log->dw.Z.X[8] = (*log->U31_orta * Iortb - + *log->U31_ortb * Iorta) * I; + log->dw.Z.Z[8] = log->dw.Z.X[8] * log->dw.Z.X[8] + log->dw.Z.R[8] * log->dw.Z.R[8]; + + //*log->z[4]=log->dw.Z.R[8]; + //*log->z[5]=log->dw.Z.X[8]; + + //-------------------------------------------------------------------------------- + + + //Sprawdzenie wielkosci pradow fazowych + + if (log->dw.I1 > log->nast_.Igr) log->dw.Igr[0] = 1; + if (log->dw.I1 < log->nast_.Igrp) log->dw.Igr[0] = 0; + + if (log->dw.I2 > log->nast_.Igr) log->dw.Igr[1] = 1; + if (log->dw.I2 < log->nast_.Igrp) log->dw.Igr[1] = 0; + + if (log->dw.I3 > log->nast_.Igr) log->dw.Igr[2] = 1; + if (log->dw.I3 < log->nast_.Igrp) log->dw.Igr[2] = 0; + + + log->dw.Igr[3] = log->dw.Igr[0] && log->dw.Igr[1]; + log->dw.Igr[4] = log->dw.Igr[1] && log->dw.Igr[2]; + log->dw.Igr[5] = log->dw.Igr[2] && log->dw.Igr[0]; + + //dodatkowe kryterium blokady po pojawieniu sie pradu +// if ((log->dw.I1 < 0.1) && (log->dw.I2 < 0.1) && (log->dw.I3 < 0.1)) + if ((log->dw.I1 < 0.01) && (log->dw.I2 < 0.01) && (log->dw.I3 < 0.01)) + { + log->l_nieustalony = 5; + } + else + { + if (log->l_nieustalony > 0) + log->l_nieustalony--; + } + + //identyfikacja zwarc z udzialem ziemi + local = log->nast_.Iogr; + localp = local * log->nast_.kpk; + wk1p = ((*log->sI0 > local) + && (*log->sU0 > log->nast_.Uomin))?1:0; + + wk2p = ((*log->sI0 < localp) + || (*log->sU0 < log->nast_.Uomin))?1:0; + + + + if (log->dw.I1 > log->dw.I2) local = log->dw.I1; + else local = log->dw.I2; + if (log->dw.I3 > local) local = log->dw.I3; + + + local = local * log->nast_.khio; + localp = local * log->nast_.kpk; + + + wk1 = (wk1p && + (*log->sI0 > local))?1:0; + wk2 = (wk2p || + (*log->sI0 < localp))?1:0; + + + sprawdz_P( + &log->dw.Iogr, + wk1, + wk2, + &log->dw.liczpIo, + 3,3 + ); + + + // impedancyjne kryterium kierunkowe i sprawdzenie obszaru obciazenia + + local = log->nast_.XKR; + localp = local * log->nast_.kp; + local0 = log->nast_.XKX; + local0p = local0 * log->nast_.kpp; + + + + local1 = log->nast_.RLf * log->nast_.wyjscie.przekladnia; + local1p = local1 * log->nast_.kpp; + local2 = log->nast_.RLr * log->nast_.wyjscie.przekladnia; + local2p = local2 * log->nast_.kpp; + local3 = log->nast_.KL; + local3p = local3 * log->nast_.kp; + + for(i=0;i<9;i++) + { + + wk1 = log->dw.Z.X[i] > local * log->dw.Z.R[i]; + wk1p = log->dw.Z.X[i] < localp * log->dw.Z.R[i]; + wk2 = log->dw.Z.X[i] > local0 * log->dw.Z.R[i]; + wk2p = log->dw.Z.X[i] < local0p * log->dw.Z.R[i]; + + for (j = 0; j < 6; j++) + { + int k = 0; + if (i < 3 ) + k = i; + else + k = i - 3; + + sprawdz_P( //kierunek do przodu + &log->dw.XKp[j][i], + wk1 && wk2 && log->dw.P_sss[j][k], + wk1p || wk2p, + &log->dw.liczp11[j][i], + 7, + 3 + ); + + + sprawdz_P( //kierunek do tylu + &log->dw.XKm[j][i], + !wk1 && !wk2 && log->dw.P_sss[j][k], + !wk1p || !wk2p, + &log->dw.liczp12[j][i], + 7, + 3 + ); + } + + sprawdz_P( // blokada od pradow obcia¿enia + &log->dw.BL_Load[i], + (log->dw.Z.R[i] > local1 || log->dw.Z.R[i] < local2) && + fabs(log->dw.Z.X[i]) < fabs(local3 * log->dw.Z.R[i]), + (log->dw.Z.R[i] < local1p && log->dw.Z.R[i] > local2p) || + fabs(log->dw.Z.X[i]) > fabs(local3p * log->dw.Z.R[i]), + &log->dw.liczp10[i], + filtr_P + ); + + + } + + + //-------------------------------------------------------------------------------- + // identyfikacja bardzo bliskich zwarc + + log->dw.Zm[0] = log->dw.Z.Z[0] < log->nast_.Zgr || log->dw.U1 < 0.0001f ; + log->dw.Zm[1] = log->dw.Z.Z[1] < log->nast_.Zgr || log->dw.U2 < 0.0001f ; + log->dw.Zm[2] = log->dw.Z.Z[2] < log->nast_.Zgr || log->dw.U3 < 0.0001f ; + log->dw.Zm[3] = log->dw.Z.Z[3] < log->nast_.Zgr || log->dw.U1 < 0.0001f ; + log->dw.Zm[4] = log->dw.Z.Z[4] < log->nast_.Zgr || log->dw.U2 < 0.0001f ; + log->dw.Zm[5] = log->dw.Z.Z[5] < log->nast_.Zgr || log->dw.U3 < 0.0001f ; + log->dw.Zm[6] = log->dw.Z.Z[6] < log->nast_.Zgr || *log->U12 < 0.0001f ; + log->dw.Zm[7] = log->dw.Z.Z[7] < log->nast_.Zgr || *log->U23 < 0.0001f ; + log->dw.Zm[8] = log->dw.Z.Z[8] < log->nast_.Zgr || *log->U31 < 0.0001f ; + + + + //-------------------------------------------------------------------------------- + //Wyliczenie wartosci rozruchowych dla charakterystyki poligonalnej + + i = 0; + while(i<3) + { + log->dw.Z.absR[i] = fabs(log->dw.Z.R[i]); + log->dw.Z.absX[i] = fabs(log->dw.Z.X[i]); + log->dw.Z.XRtanfi1_noabs[i] = log->dw.Z.X[i] - log->dw.Z.R[i] * log->nast_.tanfi1; + // log->dw.Z.XRtanfi1[i] = fabs(log->dw.Z.XRtanfi1_noabs[i]); + log->dw.Z.XRtanfi2[i] = log->dw.Z.X[i] + log->dw.Z.R[i] * log->nast_.tanfi2; + i++; + } + + while(i<9) + { + log->dw.Z.absR[i] = fabs(log->dw.Z.R[i]); + log->dw.Z.absX[i] = fabs(log->dw.Z.X[i]); + log->dw.Z.XRtanfi1_noabs[i] = log->dw.Z.X[i] - log->dw.Z.R[i] * log->nast_.tanfi1; + // log->dw.Z.XRtanfi1[i] = fabs(log->dw.Z.XRtanfi1_noabs[i]); + i++; + } + + + + //----------------------------------------------------------------------------- + // sprawdzenie spelnienia warunku impedancji strefy + //----------------------------------------------------------------------------- + // sprawdzenie strefy 1 zwarcia jednofazowe z ziemia strefa 1W + int l_powt = 5; + if (log->nast_.kierunek[0] == 0) + l_powt = 10; + else + l_powt = 5; + + if(log->nast_.wyjscie.SOTF_zwrotnie == 0) + l_powt = 5; + + + for(i=0;i<3;i++) + { + if (log->nast_.typ[0]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[0][0].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[0][0].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = I < log->nast_.n_pol[0][0].Z && log->dw.Igr[i]; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[0][0].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[0][0].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = I > log->nast_.n_pol[0][0].Zp || !log->dw.Igr[i]; + } else { + wk1 = (/*log->dw.Z.absR[i] < log->nast_.n_pol[0][0].Rr ||*/ + ((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] > -log->nast_.n_pol[0][0].Rrtanfi1) && (log->dw.Z.R[i] > - log->nast_.n_pol[0][0].Rr)):((log->dw.Z.XRtanfi1_noabs[i] < log->nast_.n_pol[0][0].Rrtanfi1)&&(log->dw.Z.R[i] < log->nast_.n_pol[0][0].Rr))) ) //here + && log->dw.Z.XRtanfi2[i] < log->nast_.Xr1Wf + && log->dw.Igr[i] + && log->dw.Z.absX[i] < log->nast_.n_pol[0][0].Xr + && log->l_nieustalony == 0; + + wk2 = ( + ((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] < -log->nast_.n_pol[0][0].Rptanfi1) || (log->dw.Z.R[i] < - log->nast_.n_pol[0][0].Rp)):((log->dw.Z.XRtanfi1_noabs[i] > log->nast_.n_pol[0][0].Rptanfi1)||(log->dw.Z.R[i] > log->nast_.n_pol[0][0].Rp))) + /*&& log->dw.Z.absR[i] > log->nast_.n_pol[0][0].Rp*/) + || !log->dw.Igr[i] + || log->dw.Z.XRtanfi2[i] > log->nast_.Xr1Wfp + || log->dw.Z.absX[i] > log->nast_.n_pol[0][0].Xp + || log->l_nieustalony != 0; + } + sprawdz_P( + &log->dw.P_s[0][i], + wk1, + wk2, + &log->dw.liczps[0][i], + l_powt, 5 + ); + log->dw.Pbk[0][i] = log->dw.P_s[0][i] && log->dw.Iogr && !(log->nast_.Bl_L[0] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + log->dw.P_sss[0][i]= wk1; + } + + // sprawdzenie strefy 1 zwarcia jednofazowe z ziemia strefa 1 + if (log->nast_.kierunek[1] == 0) + l_powt = 10; + else + l_powt = 5; + + if (log->nast_.wyjscie.SOTF_zwrotnie == 1) + l_powt = 5; + + for(i=0;i<3;i++) + { + if (log->nast_.typ[1]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[1][0].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[1][0].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = I < log->nast_.n_pol[1][0].Z && log->dw.Igr[i]; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[1][0].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[1][0].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = I > log->nast_.n_pol[1][0].Zp || !log->dw.Igr[i]; + } else { + wk1 = (((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] > -log->nast_.n_pol[1][0].Rrtanfi1) && (log->dw.Z.R[i] > - log->nast_.n_pol[1][0].Rr)):((log->dw.Z.XRtanfi1_noabs[i] < log->nast_.n_pol[1][0].Rrtanfi1)&&(log->dw.Z.R[i] < log->nast_.n_pol[1][0].Rr))) ) + +// (log->dw.Z.absR[i] < log->nast_.n_pol[1][0].Rr || log->dw.Z.XRtanfi1[i] < log->nast_.n_pol[1][0].Rrtanfi1) + && log->dw.Z.XRtanfi2[i] < log->nast_.Xr1f + && log->dw.Igr[i] + && log->dw.Z.absX[i] < log->nast_.n_pol[1][0].Xr + && log->l_nieustalony == 0; + wk2 = ((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] < -log->nast_.n_pol[1][0].Rptanfi1) || (log->dw.Z.R[i] < - log->nast_.n_pol[1][0].Rp)):((log->dw.Z.XRtanfi1_noabs[i] > log->nast_.n_pol[1][0].Rptanfi1)||(log->dw.Z.R[i] > log->nast_.n_pol[1][0].Rp))) + + //(log->dw.Z.absR[i] > log->nast_.n_pol[1][0].Rp && log->dw.Z.XRtanfi1[i] > log->nast_.n_pol[1][0].Rptanfi1) + || log->dw.Z.XRtanfi2[i] > log->nast_.Xr1fp + || !log->dw.Igr[i] + || log->dw.Z.absX[i] > log->nast_.n_pol[1][0].Xp + || log->l_nieustalony != 0; + } + sprawdz_P( + &log->dw.P_s[1][i], + wk1, + wk2, + &log->dw.liczps[1][i], + l_powt, 5 + ); + log->dw.Pbk[1][i] = log->dw.P_s[1][i] && log->dw.Iogr && !(log->nast_.Bl_L[1] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + log->dw.P_sss[1][i]= wk1; + } + + + //sprawdzenie pozostalych stref zwarcie jednofazowe z ziemia + for(j=2;j<6;j++) + { + if (log->nast_.kierunek[j] == 0) + l_powt = 10; + else + l_powt = 5; + + if (log->nast_.wyjscie.SOTF_zwrotnie == j) + l_powt = 5; + + for(i=3;i<6;i++) + { + if (log->nast_.typ[j]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][0].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][0].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = I < log->nast_.n_pol[j][0].Z && log->dw.Igr[i-3]; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][0].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][0].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = I > log->nast_.n_pol[j][0].Zp || !log->dw.Igr[i-3]; + } else { + wk1 = //(log->dw.Z.absR[i] < log->nast_.n_pol[j][0].Rr || log->dw.Z.XRtanfi1[i] < log->nast_.n_pol[j][0].Rrtanfi1) + (((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] > -log->nast_.n_pol[j][0].Rrtanfi1) && (log->dw.Z.R[i] > - log->nast_.n_pol[j][0].Rr)):((log->dw.Z.XRtanfi1_noabs[i] < log->nast_.n_pol[j][0].Rrtanfi1)&&(log->dw.Z.R[i] < log->nast_.n_pol[j][0].Rr))) ) + + && log->dw.Igr[i-3] + && log->dw.Z.absX[i] < log->nast_.n_pol[j][0].Xr + && log->l_nieustalony == 0; + wk2 =// (log->dw.Z.absR[i] > log->nast_.n_pol[j][0].Rp && log->dw.Z.XRtanfi1[i] > log->nast_.n_pol[j][0].Rptanfi1) + ((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] < -log->nast_.n_pol[j][0].Rptanfi1) || (log->dw.Z.R[i] < - log->nast_.n_pol[j][0].Rp)):((log->dw.Z.XRtanfi1_noabs[i] > log->nast_.n_pol[j][0].Rptanfi1)||(log->dw.Z.R[i] > log->nast_.n_pol[j][0].Rp))) + + || !log->dw.Igr[i-3] + || log->dw.Z.absX[i] > log->nast_.n_pol[j][0].Xp + || log->l_nieustalony != 0; + } + sprawdz_P( + &log->dw.P_s[j][i-3], + wk1, + wk2, + &log->dw.liczps[j][i-3], + l_powt, 5 + ); + + log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && log->dw.Iogr && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + log->dw.P_sss[j][i-3]= wk1; + } + + } + + //sprawdzenie petli miedzyfazowych + for(j=0;j<6;j++) + { + if (log->nast_.kierunek[j] == 0) + l_powt = 10; + else + l_powt = 5; + + if (log->nast_.wyjscie.SOTF_zwrotnie == j) + l_powt = 5; + + for(i=6;i<9;i++) + { + if (log->nast_.typ[j]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][1].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][1].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = (I < log->nast_.n_pol[j][1].Z && log->dw.Igr[i-3])?1:0; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][1].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][1].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = (I > log->nast_.n_pol[j][1].Zp || !log->dw.Igr[i-3])?1:0; + } else { + wk1 = //(log->dw.Z.absR[i] < log->nast_.n_pol[j][1].Rr || log->dw.Z.XRtanfi1[i] < log->nast_.n_pol[j][1].Rrtanfi1) + ( ((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] > -log->nast_.n_pol[j][1].Rrtanfi1) && (log->dw.Z.R[i] > - log->nast_.n_pol[j][1].Rr)):((log->dw.Z.XRtanfi1_noabs[i] < log->nast_.n_pol[j][1].Rrtanfi1)&&(log->dw.Z.R[i] < log->nast_.n_pol[j][1].Rr))) ) + && log->dw.Igr[i-3] + && log->dw.Z.absX[i] < log->nast_.n_pol[j][1].Xr + && log->l_nieustalony == 0; + wk2 = //(log->dw.Z.absR[i] > log->nast_.n_pol[j][1].Rp && log->dw.Z.XRtanfi1[i] > log->nast_.n_pol[j][1].Rptanfi1) + ((log->dw.Z.X[i]>=0.0f)?((log->dw.Z.XRtanfi1_noabs[i] < -log->nast_.n_pol[j][1].Rptanfi1) || (log->dw.Z.R[i] < - log->nast_.n_pol[j][1].Rp)):((log->dw.Z.XRtanfi1_noabs[i] > log->nast_.n_pol[j][1].Rptanfi1)||(log->dw.Z.R[i] > log->nast_.n_pol[j][1].Rp))) + + || !log->dw.Igr[i-3] + || log->dw.Z.absX[i] > log->nast_.n_pol[j][1].Xp + || log->l_nieustalony != 0; + } + sprawdz_P( + &log->dw.P_s[j][i-3], + wk1, + wk2, + &log->dw.liczps[j][i-3], + l_powt, 5 + ); + log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz minimalnej wartosci pradu + log->dw.P_sss[j][i-3]= wk1; + } + } + + + //----------------------------------------------------------------------------- + // sprawdzenie dodatkowego kryterium kierunkowego + + if (*log->sU2 > 0.001f) + { + // kryterium skladowych przeciwnych + //przesuniecia skladowych napiecia o 45 stopni + if (*log->sI2 > 0.0003f) + { + Uorta = (*log->sU2_orta - *log->sU2_ortb) * 0.7071f; // + Uortb = (*log->sU2_orta + *log->sU2_ortb) * 0.7071f; + I = *log->sI2_orta * Uorta + + *log->sI2_ortb * Uortb; + + wk1 = I < 0; + sprawdz_P ( + &log->dw.Kp, + wk1, + !wk1, + &log->dw.liczKdod, + 3,3 + ); + log->dw.Km = !log->dw.Kp; + } else { + log->dw.Km = log->dw.Kp = 0; + } + } else { + // kryterium skladowych zgodnych + //przesuniecia skladowych napiecia o 45 stopni + if ((*log->sI1 > 0.0005f) && + (*log->sU1 > 0.00001f)) + { + Uorta = (*log->sU1_orta - *log->sU1_ortb) * 0.7071f; // + Uortb = (*log->sU1_orta + *log->sU1_ortb) * 0.7071f; + I = *log->sI1_orta * Uorta + + *log->sI1_ortb * Uortb; + wk1 = I > 0; + sprawdz_P ( + &log->dw.Kp, + wk1, + !wk1, + &log->dw.liczKdod, + 3,3 + ); + log->dw.Km = !log->dw.Kp; + } else { + log->dw.Km = log->dw.Kp = 0; + } + } + + + //----------------------------------------------------------------------------- + //Ustalenie pobudzen stref + //okreslenie warunkow kierunkowych + //zwarcie jednofazowe z ziemia strefa 1 i 1W + + if (!check_struct(&log->Bl_K))//jeœli nie ma blokady kierunku + { + for(j=0;j<2;j++) + { + + switch(log->nast_.kierunek[j]) + { + case 0: //bezkierunku + for(i=0;i<3;i++) + { + log->dw.P[j][i] = log->dw.Pbk[j][i]; + } + break; + case 1: //do linii + for(i=0;i<3;i++) + { + log->dw.P[j][i] = log->dw.Pbk[j][i] && log->dw.Kp && (log->dw.XKp[j][i] || log->dw.Zm[i]); + } + break; + case 2: // do szyn + for(i=0;i<3;i++) + { + log->dw.P[j][i] = log->dw.Pbk[j][i] && log->dw.Km && (log->dw.XKm[j][i] || log->dw.Zm[i]); + } + break; + } + } + + //zwarcie jednofazowe z ziemia - pozostale + for(j=2;j<6;j++) + { + switch(log->nast_.kierunek[j]) + { + case 0: //bezkierunku + for(i=3;i<6;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3]; + } + break; + case 1: //do linii + for(i=3;i<6;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Kp && (log->dw.XKp[j][i] || log->dw.Zm[i]); + } + break; + case 2: // do szyn + for(i=3;i<6;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Km && (log->dw.XKm[j][i] || log->dw.Zm[i]); + } + break; + } + } + + //zwarcie miedzyfazowe + for(j=0;j<6;j++) + { + switch(log->nast_.kierunek[j]) + { + case 0: //bezkierunku + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3]; + } + break; + case 1: //do linii + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Kp && (log->dw.XKp[j][i] || log->dw.Zm[i]); + } + break; + case 2: // do szyn + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Km && (log->dw.XKm[j][i] || log->dw.Zm[i]); + } + break; + } + } + } else { + for(j=0;j<6;j++) + { + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3]; //poprawiony strzal z boku? + } + + } + } + + //----------------------------------------------------------------------------- + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + for(i=0;i<36;i++) + { + *(&log->dw.P[0][0] + i) = check_struct(&log->test)?1:0; + } + } + // + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && check_struct(&log->stan_bl))/* | czy_start()*/) + { + for(i=0;i<36;i++) + { + *(&log->dw.P[0][0] + i) = 0; + } + } + + /*for(i=0;i<36;i++) + { + check_and_set_struct(log->dw.P[i/6][i%6],(struct binary_io *)((u8 *)&log->P1W_L1E+(sizeof(struct binary_io)*i))); + }*/ + + check_and_set_struct(log->dw.P[0][0], &log->P1W_L1E); + check_and_set_struct(log->dw.P[0][1], &log->P1W_L2E); + check_and_set_struct(log->dw.P[0][2], &log->P1W_L3E); + check_and_set_struct(log->dw.P[0][3], &log->P1W_L1L2); + check_and_set_struct(log->dw.P[0][4], &log->P1W_L2L3); + check_and_set_struct(log->dw.P[0][5], &log->P1W_L3L1); + check_and_set_struct(log->dw.P[1][0], &log->P1_L1E); + check_and_set_struct(log->dw.P[1][1], &log->P1_L2E); + check_and_set_struct(log->dw.P[1][2], &log->P1_L3E); + check_and_set_struct(log->dw.P[1][3], &log->P1_L1L2); + check_and_set_struct(log->dw.P[1][4], &log->P1_L2L3); + check_and_set_struct(log->dw.P[1][5], &log->P1_L3L1); + check_and_set_struct(log->dw.P[2][0], &log->P2_L1E); + check_and_set_struct(log->dw.P[2][1], &log->P2_L2E); + check_and_set_struct(log->dw.P[2][2], &log->P2_L3E); + check_and_set_struct(log->dw.P[2][3], &log->P2_L1L2); + check_and_set_struct(log->dw.P[2][4], &log->P2_L2L3); + check_and_set_struct(log->dw.P[2][5], &log->P2_L3L1); + check_and_set_struct(log->dw.P[3][0], &log->P3_L1E); + check_and_set_struct(log->dw.P[3][1], &log->P3_L2E); + check_and_set_struct(log->dw.P[3][2], &log->P3_L3E); + check_and_set_struct(log->dw.P[3][3], &log->P3_L1L2); + check_and_set_struct(log->dw.P[3][4], &log->P3_L2L3); + check_and_set_struct(log->dw.P[3][5], &log->P3_L3L1); + check_and_set_struct(log->dw.P[4][0], &log->P4_L1E); + check_and_set_struct(log->dw.P[4][1], &log->P4_L2E); + check_and_set_struct(log->dw.P[4][2], &log->P4_L3E); + check_and_set_struct(log->dw.P[4][3], &log->P4_L1L2); + check_and_set_struct(log->dw.P[4][4], &log->P4_L2L3); + check_and_set_struct(log->dw.P[4][5], &log->P4_L3L1); + check_and_set_struct(log->dw.P[5][0], &log->P5_L1E); + check_and_set_struct(log->dw.P[5][1], &log->P5_L2E); + check_and_set_struct(log->dw.P[5][2], &log->P5_L3E); + check_and_set_struct(log->dw.P[5][3], &log->P5_L1L2); + check_and_set_struct(log->dw.P[5][4], &log->P5_L2L3); + check_and_set_struct(log->dw.P[5][5], &log->P5_L3L1); + } + else + { + /*for(i=0;i<36;i++) + { + //clear_struct((struct binary_io *)((u8 *)&log->P1W_L1E+(sizeof(struct binary_io)*i))); + }*/ + clear_struct(&log->P1W_L1E); + clear_struct(&log->P1W_L2E); + clear_struct(&log->P1W_L3E); + clear_struct(&log->P1W_L1L2); + clear_struct(&log->P1W_L2L3); + clear_struct(&log->P1W_L3L1); + clear_struct(&log->P1_L1E); + clear_struct(&log->P1_L2E); + clear_struct(&log->P1_L3E); + clear_struct(&log->P1_L1L2); + clear_struct(&log->P1_L2L3); + clear_struct(&log->P1_L3L1); + clear_struct(&log->P2_L1E); + clear_struct(&log->P2_L2E); + clear_struct(&log->P2_L3E); + clear_struct(&log->P2_L1L2); + clear_struct(&log->P2_L2L3); + clear_struct(&log->P2_L3L1); + clear_struct(&log->P3_L1E); + clear_struct(&log->P3_L2E); + clear_struct(&log->P3_L3E); + clear_struct(&log->P3_L1L2); + clear_struct(&log->P3_L2L3); + clear_struct(&log->P3_L3L1); + clear_struct(&log->P4_L1E); + clear_struct(&log->P4_L2E); + clear_struct(&log->P4_L3E); + clear_struct(&log->P4_L1L2); + clear_struct(&log->P4_L2L3); + clear_struct(&log->P4_L3L1); + clear_struct(&log->P5_L1E); + clear_struct(&log->P5_L2E); + clear_struct(&log->P5_L3E); + clear_struct(&log->P5_L1L2); + clear_struct(&log->P5_L2L3); + clear_struct(&log->P5_L3L1); + + } +} diff --git a/src/ZDistA.h b/src/ZDistA.h new file mode 100644 index 0000000..46e8476 --- /dev/null +++ b/src/ZDistA.h @@ -0,0 +1,444 @@ +/* + * ZDistA.h + * + * + * Created on: 09-03-2017 + * Author: KJ + */ + +#ifndef ZDISTA_H_ +#define ZDISTA_H_ + +/* +#include +#define u8 uint8_t +#define u16 uint16_t +#define u32 uint32_t + +struct binary_io +{ + int yyyy; +}; +*/ + +#include "tdefs.h" +#include "helper.h" + + struct impedancja_ZDistA + { + float R[9]; // czesc rzeczywista impedancji + float X[9]; // czesc urojona impedancji + float Z[9]; //kwadrat impedancji + float absR[9]; // wartosc bezwzgledna impedancji czesc rzeczywista + float absX[9]; // wartosc bezwzgledna impedancji czesc urojona + float XRtanfi2[9]; // abs(X+R*tan(fi2)) + // float XRtanfi1[9]; // abs(X-R*tan(fi1)) + //[0] petla L1-E strefa 1 + //[1] petla L2-E strefa 1 + //[2] petla L3-E strefa 1 + //[3] petla L1-E strefa 2,3,4,5 + //[4] petla L2-E strefa 2,3,4,5 + //[5] petla L3-E strefa 2,3,4,5 + //[6] petla L1-L2 + //[7] petla L2-L3 + //[8] petla L3-L1 + float XRtanfi1_noabs[9]; + }; + + struct dane_wewnetrzne_ZDistA + { + struct impedancja_ZDistA Z; + u8 Km; + u8 Kp; + u8 Igr[6]; // przekroczenie wartosci granicznej pradu + u8 Iogr; // zwarcie z udzialem ziemi + u8 P_s[6][6];//pobudzenie kryterium impedacyjnego (obszar) [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] + u8 P_sss[6][6]; + u8 Pbk[6][6];//pobudzenie w strefie [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] bezkierunkowe + u8 P[6][6];//pobudzenie w strefie [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] + u8 BL_Load[9]; ///< impedancja w obszrze pradow roboczych + u8 XKp[6][9]; ///< impedancyjne kryterium kierunku "do przodu" + u8 XKm[6][9]; ///< impedancyjne kryterium kierunku "do tylu" + u8 Zm[9];///< impedancyjne kryterium bardzo bliskiego zwarcia + short liczp10[9]; /// licznik pobudzen + short liczp11[6][9]; /// licznik pobudzen + short liczp12[6][9]; /// licznik pobudzen + //[0] petla L1-E strefa 1 + //[1] petla L2-E strefa 1 + //[2] petla L3-E strefa 1 + //[3] petla L1-E strefa 2,3,4,5 + //[4] petla L2-E strefa 2,3,4,5 + //[5] petla L3-E strefa 2,3,4,5 + //[6] petla L1-L2 + //[7] petla L2-L3 + //[8] petla L3-L1 + short liczps[6][6]; //liczniki pobudzen dla pobudzen stref [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] + short liczpIo; //licznik pobudzen dla identyfikacji zwarc z udzialem ziemi + short liczKdod; + float U1; + float U2; + float U3; + float I1; + float I2; + float I3; + struct ZDistA_logic *log_ptr; + }; + + struct nast_w_poligon + { + float Xr; // zasieg dla poligonu i srodek okregu dla kolowej wspolrzedna X + float Rr; // zasieg dla dla poligonu i srodek okregu dla kolowej wspolrzedna R + float Z; // srednica kola charakterystyki kolowej + float Xp; // zasieg dla poligonu i srodek okregu dla kolowej wspolrzedna X dla powrotu + float Rp; // zasieg dla dla poligonu i srodek okregu dla kolowej wspolrzedna R dla powrotu + float Zp; // srednica kola charakterystyki kolowej dla powrotu + float Rrtanfi1;// wartoœæ rozruchowa prostej nachylonej k¹tem linii + float Rptanfi1;// wartoœæ powrotowa prostej nachylonej k¹tem linii + }; + + struct Wyjscia_ZDistA // struktura wyjsc przekaznika + { + struct dane_wewnetrzne_ZDistA *Zdist_dw; + float przekladnia; + u8 *on; + u8 SOTF_zwrotnie; + }; + + + struct Nastawy_przeliczone_ZDistA + { + struct Wyjscia_ZDistA wyjscie; + u8 on_; + u8 bl_; + u8 Bl_L[6]; + u8 typ[6];// typ charakterystyki 0 - poligonalna, 1- kolowa + float ReK1; // skladowa rzeczywista wspolczynnika kompensacji ziemnozwarciowej dla strefy 1 + float ImK1;// skladowa urojona wspolczynnika kompensacji ziemnozwarciowej dla strefy 1 + float ReKr;// skladowa rzeczywista wspolczynnika kompensacji ziemnozwarciowej dla stref 2,3,4,5 + float ImKr;// skladowa urojona wspolczynnika kompensacji ziemnozwarciowej dla stref 2,3,4,5 + + struct nast_w_poligon n_pol[6][2]; // nastawy stref dla charakterystyki poligonalnej [numer strefy][LE/LL] + float tanfi1; // tangens kata linii + float tanfi2; //tangens kata dodatkowej prostej dla charakterystyki poligonalnej strefa 1 + float Xr1f; // wartosc rozruchowa dodatkowej prostej dla charakterystyki poligonalnej + float Xr1fp; // wartosc powrotowa dodatkowej prostej dla charakterystyki poligonalnej + float Xr1Wf; // wartosc rozruchowa dodatkowej prostej dla charakterystyki poligonalnej + float Xr1Wfp; // wartosc powrotowa dodatkowej prostej dla charakterystyki poligonalnej + float Igr; // wartosc kwadratu pradu granicznego warunku dzialania przekaznika + float Igrp; // wartosc kwadratu pradu granicznego warunku dzialania przekaznika (powrot) + + float Zgr;// minimalna wartosc impedancji kryteriunkierunkowego w kwadracie + float kp;//wspó³czynnik powrotu dla niedomiarówek + float kpp;//wspó³czynnik powrotu dla nadmiarówek + float kpk; //kwadrat wspó³czynnika powrotu + short kierunek[6]; + + //A2 + float XKR; // wspolczynnik prostej R kierunku kryterium impedancyjnego + float XKX; // wspolczynnik prostej R kierunku kryterium impedancyjnego + float KL;//wspolczynnik prostej dla blokady od obszaru obcia¿enia + float RLf; // wartosc rozruchowa blokady Load do przodu + float RLr; // wartosc rozruchowa blokady Load do tylu + float Iogr; // wartosc graniczna skladowej zerowej identyfikacji zwarc z udzialem ziemi + float khio; // wspolczynnik stabilizacji dla przekaznika identyfikacji zwarc z udzialem ziemi + float Uomin; ///< minimalna wartoœæ napiêcia sk³adowej zerowej + + }; + +struct ZDistA_logic +{ + struct binary_io stan_bl; + struct binary_io Bl_K; + float *I1_orta; ///< Skladowa ortogonalna a pradu faza L1 + float *I1_ortb; ///< Skladowa ortogonalna b pradu faza L1 + float *I1; ///< Prad mierzony faza 1 + float *I2_orta; ///< Skladowa ortogonalna a pradu faza L2 + float *I2_ortb; ///< Skladowa ortogonalna b pradu faza L2 + float *I2; ///< Prad mierzony faza 2 + float *I3_orta; ///< Skladowa ortogonalna a pradu faza L3 + float *I3_ortb; ///< Skladowa ortogonalna b pradu faza L3 + float *I3; ///< Prad mierzony faza 3 + float *U1_orta; ///< Skladowa ortogonalna a napiecia faza L1 + float *U1_ortb; ///< Skladowa ortogonalna b napiecia faza L1 + float *U1; ///< Napiecie mierzone faza 1 + float *U2_orta; ///< Skladowa ortogonalna a napiecia faza L2 + float *U2_ortb; ///< Skladowa ortogonalna b napiecia faza L2 + float *U2; ///< Napiecie mierzone faza 2 + float *U3_orta; ///< Skladowa ortogonalna a napiecia faza L3 + float *U3_ortb; ///< Skladowa ortogonalna b napiecia faza L3 + float *U3; ///< Napiecie mierzone faza 3 + float *U12_orta; ///< Skladowa ortogonalna a napiecia L1-L2 + float *U12_ortb; ///< Skladowa ortogonalna b napiecia L1-L2 + float *U12; ///< Napiecie mierzone L1-L2 + float *U23_orta; ///< Skladowa ortogonalna a napiecia L2-L3 + float *U23_ortb; ///< Skladowa ortogonalna b napiecia L2-L3 + float *U23; ///< Napiecie mierzone L2-L3 + float *U31_orta; ///< Skladowa ortogonalna a napiecia L3-L1 + float *U31_ortb; ///< Skladowa ortogonalna b napiecia L3-L1 + float *U31; ///< Napiecie mierzone L3-L1 + float *sI1_orta; ///< Skladowa ortogonalna a skladowej zgodnej pradu + float *sI1_ortb; ///< Skladowa ortogonalna b skladowej zgodnej pradu + float *sI1; ///< Skladowa zgodna pradu + float *sI2_orta; ///< Skladowa ortogonalna a skladowej przeciwnej pradu + float *sI2_ortb; ///< Skladowa ortogonalna b skladowej przeciwnej pradu + float *sI2; ///< Skladowa przeciwna pradu + float *sI0_orta; ///< Skladowa ortogonalna a skladowej zerowej pradu + float *sI0_ortb; ///< Skladowa ortogonalna b skladowej zerowej pradu + float *sI0; ///< Skladowa zerowa pradu + float *sU1_orta; ///< Skladowa ortogonalna a skladowej zgodnej napiecia + float *sU1_ortb; ///< Skladowa ortogonalna b skladowej zgodnej napiecia + float *sU1; ///< Skladowa zgodna napiecia + float *sU2_orta; ///< Skladowa ortogonalna a skladowej przeciwnej napiecia + float *sU2_ortb; ///< Skladowa ortogonalna b skladowej przeciwnej napiecia + float *sU2; ///< Skladowa przeciwna napiecia + float *sU0_orta; ///< Skladowa ortogonalna a skladowej zerowej napiecia + float *sU0_ortb; ///< Skladowa ortogonalna b skladowej zerowej napiecia + float *sU0; ///< Skladowa zerowa napiecia + struct analog_in_params *param_I; ///< Parametry wejscia pradowego + struct analog_in_params *param_U; /// < Parametry kanalu napieciowego + struct binary_io test; + struct binary_io P1W_L1E; ///< Pobudzenie strefy 1W zwarcie L1-E + struct binary_io P1W_L2E; ///< Pobudzenie strefy 1W zwarcie L2-E + struct binary_io P1W_L3E; ///< Pobudzenie strefy 1W zwarcie L3-E + struct binary_io P1W_L1L2; ///< Pobudzenie strefy 1W zwarcie L1-L2 + struct binary_io P1W_L2L3; ///< Pobudzenie strefy 1W zwarcie L2-L3 + struct binary_io P1W_L3L1; ///< Pobudzenie strefy 1W zwarcie L3-L1 + struct binary_io P1_L1E; ///< Pobudzenie strefy 1 zwarcie L1-E + struct binary_io P1_L2E; ///< Pobudzenie strefy 1 zwarcie L2-E + struct binary_io P1_L3E; ///< Pobudzenie strefy 1 zwarcie L3-E + struct binary_io P1_L1L2; ///< Pobudzenie strefy 1 zwarcie L1-L2 + struct binary_io P1_L2L3; ///< Pobudzenie strefy 1 zwarcie L2-L3 + struct binary_io P1_L3L1; ///< Pobudzenie strefy 1 zwarcie L3-L1 + struct binary_io P2_L1E; ///< Pobudzenie strefy 2 zwarcie L1-E + struct binary_io P2_L2E; ///< Pobudzenie strefy 2 zwarcie L2-E + struct binary_io P2_L3E; ///< Pobudzenie strefy 2 zwarcie L3-E + struct binary_io P2_L1L2; ///< Pobudzenie strefy 2 zwarcie L1-L2 + struct binary_io P2_L2L3; ///< Pobudzenie strefy 2 zwarcie L2-L3 + struct binary_io P2_L3L1; ///< Pobudzenie strefy 2 zwarcie L3-L1 + struct binary_io P3_L1E; ///< Pobudzenie strefy 3 zwarcie L1-E + struct binary_io P3_L2E; ///< Pobudzenie strefy 3 zwarcie L2-E + struct binary_io P3_L3E; ///< Pobudzenie strefy 3 zwarcie L3-E + struct binary_io P3_L1L2; ///< Pobudzenie strefy 3 zwarcie L1-L2 + struct binary_io P3_L2L3; ///< Pobudzenie strefy 3 zwarcie L2-L3 + struct binary_io P3_L3L1; ///< Pobudzenie strefy 3 zwarcie L3-L1 + struct binary_io P4_L1E; ///< Pobudzenie strefy 4 zwarcie L1-E + struct binary_io P4_L2E; ///< Pobudzenie strefy 4 zwarcie L2-E + struct binary_io P4_L3E; ///< Pobudzenie strefy 4 zwarcie L3-E + struct binary_io P4_L1L2; ///< Pobudzenie strefy 4 zwarcie L1-L2 + struct binary_io P4_L2L3; ///< Pobudzenie strefy 4 zwarcie L2-L3 + struct binary_io P4_L3L1; ///< Pobudzenie strefy 4 zwarcie L3-L1 + struct binary_io P5_L1E; ///< Pobudzenie strefy 5 zwarcie L1-E + struct binary_io P5_L2E; ///< Pobudzenie strefy 5 zwarcie L2-E + struct binary_io P5_L3E; ///< Pobudzenie strefy 5 zwarcie L3-E + struct binary_io P5_L1L2; ///< Pobudzenie strefy 5 zwarcie L1-L2 + struct binary_io P5_L2L3; ///< Pobudzenie strefy 5 zwarcie L2-L3 + struct binary_io P5_L3L1; ///< Pobudzenie strefy 5 zwarcie L3-L1 + struct binary_io deakt; /// deaktywacja + + struct dane_wewnetrzne_ZDistA dw; + struct Nastawy_przeliczone_ZDistA nast_;// struktura z parametrami + + u8 l_nieustalony; + + float *z[6]; /// debug +}; + +struct ZDistA_io +{ + u32 bl_in; + u32 bl_k_in; + u32 i1_orta_float_in; + u32 i1_ortb_float_in; + u32 i1_float_in; + u32 i2_orta_float_in; + u32 i2_ortb_float_in; + u32 i2_float_in; + u32 i3_orta_float_in; + u32 i3_ortb_float_in; + u32 i3_float_in; + u32 u1_orta_float_in; + u32 u1_ortb_float_in; + u32 u1_float_in; + u32 u2_orta_float_in; + u32 u2_ortb_float_in; + u32 u2_float_in; + u32 u3_orta_float_in; + u32 u3_ortb_float_in; + u32 u3_float_in; + u32 u12_orta_float_in; + u32 u12_ortb_float_in; + u32 u12_float_in; + u32 u23_orta_float_in; + u32 u23_ortb_float_in; + u32 u23_float_in; + u32 u31_orta_float_in; + u32 u31_ortb_float_in; + u32 u31_float_in; + u32 i1_zg_orta_float_in; + u32 i1_zg_ortb_float_in; + u32 i1_zg_float_in; + u32 i2_pr_orta_float_in; + u32 i2_pr_ortb_float_in; + u32 i2_pr_float_in; + u32 io_orta_float_in; + u32 io_ortb_float_in; + u32 io_float_in; + u32 u1_zg_orta_float_in; + u32 u1_zg_ortb_float_in; + u32 u1_zg_float_in; + u32 u2_pr_orta_float_in; + u32 u2_pr_ortb_float_in; + u32 u2_pr_float_in; + u32 uo_orta_float_in; + u32 uo_ortb_float_in; + u32 uo_float_in; + u32 i_param_an_ptr_in; + u32 u_param_an_ptr_in; + u32 test_in; + u32 deakt_in; + u32 wy_ptr_out; //zainicjowac + u32 P1W_L1E_out; + u32 P1W_L2E_out; + u32 P1W_L3E_out; + u32 P1W_L1L2_out; + u32 P1W_L2L3_out; + u32 P1W_L3L1_out; + u32 P1_L1E_out; + u32 P1_L2E_out; + u32 P1_L3E_out; + u32 P1_L1L2_out; + u32 P1_L2L3_out; + u32 P1_L3L1_out; + u32 P2_L1E_out; + u32 P2_L2E_out; + u32 P2_L3E_out; + u32 P2_L1L2_out; + u32 P2_L2L3_out; + u32 P2_L3L1_out; + u32 P3_L1E_out; + u32 P3_L2E_out; + u32 P3_L3E_out; + u32 P3_L1L2_out; + u32 P3_L2L3_out; + u32 P3_L3L1_out; + u32 P4_L1E_out; + u32 P4_L2E_out; + u32 P4_L3E_out; + u32 P4_L1L2_out; + u32 P4_L2L3_out; + u32 P4_L3L1_out; + u32 P5_L1E_out; + u32 P5_L2E_out; + u32 P5_L3E_out; + u32 P5_L1L2_out; + u32 P5_L2L3_out; + u32 P5_L3L1_out; + + //debug + u32 z1_float_out; + u32 z2_float_out; + u32 z3_float_out; + u32 z4_float_out; + u32 z5_float_out; + u32 z6_float_out; + +}__attribute__((__packed__)); + +struct ZDistA_params +{ + u32 bity; ///< nastawy bitowe; + + long Typ1; ///< Typ charakterystyki strefa 1 + long Typ2; ///< Typ charakterystyki strefa 2 + long Typ3; ///< Typ charakterystyki strefa 3 + long Typ4; ///< Typ charakterystyki strefa 4 + long Typ5; ///< Typ charakterystyki strefa 5 + long K1; ///< Kierunek dzialania strefa 1 + long K2; ///< Kierunek dzialania strefa 2 + long K3; ///< Kierunek dzialania strefa 3 + long K4; ///< Kierunek dzialania strefa 4 + long K5; ///< Kierunek dzialania strefa 5 + + double I_min; ///< Graniczna wartosc pradu + double Kk1; ///< Wspolczynnik kompensacji ziemnozwarciowej strefa 1 + double Kk1_kat; ///< Kat wektora kompensacji ziemnozwarciowej strefa 1 + double KkC; ///< Wspolczynnik kompensacji ziemnozwarciowej stref 2,3,4,5 + double KkC_kat; ///< Kat wektora kompensacji ziemnozwarciowej stref 2,3,4,5 + double fi1; ///< Kat linii + double fi2; ///< Kat nachylenia prostej korekcji strefy pierwszej dla zwarc jednofazowych z ziemia + double R1W_Zf1W_LE; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R1W_Zf1W_LL; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” wyduzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double R1_Zf1_LE; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R1_Zf1_LL; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double R2_Zf2_LE; ///< Zasieg rezystancyjny strefy 2 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R2_Zf2_LL; ///< Zasieg rezystancyjny strefy 2 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 2 charakterystyki kolowej zwarcie miedzyfazowe + double R3_Zf3_LE; ///< Zasieg rezystancyjny strefy 3 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R3_Zf3_LL; ///< Zasieg rezystancyjny strefy 3 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 3 charakterystyki kolowej zwarcie miedzyfazowe + double R4_Zf4_LE; ///< Zasieg rezystancyjny strefy 4 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R4_Zf4_LL; ///< Zasieg rezystancyjny strefy 4 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 4 charakterystyki kolowej zwarcie miedzyfazowe + double R5_Zf5_LE; ///< Zasieg rezystancyjny strefy 5 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R5_Zf5_LL; ///< Zasieg rezystancyjny strefy 5 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 5 charakterystyki kolowej zwarcie miedzyfazowe + double X1W_Zr1W_LE; ///< Zasieg reaktancyjny wydluzenie strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X1W_Zr1W_LL; ///< Zasieg reaktancyjny wydluzenie strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” wydluzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double X1_Zr1_LE; ///< Zasieg reaktancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X1_Zr1_LL; ///< Zasieg reaktancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double X2_Zr2_LE; ///< Zasieg reaktancyjny strefy 2 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X2_Zr2_LL; ///< Zasieg reaktancyjny strefy 2 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 2 charakterystyki kolowej zwarcie miedzyfazowe + double X3_Zr3_LE; ///< Zasieg reaktancyjny strefy 3 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X3_Zr3_LL; ///< Zasieg reaktancyjny strefy 3 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 3 charakterystyki kolowej zwarcie miedzyfazowe + double X4_Zr4_LE; ///< Zasieg reaktancyjny strefy 4 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X4_Zr4_LL; ///< Zasieg reaktancyjny strefy 4 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 4 charakterystyki kolowej zwarcie miedzyfazowe + double X5_Zr5_LE; ///< Zasieg reaktancyjny strefy 5 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X5_Zr5_LL; ///< Zasieg reaktancyjny strefy 5 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 5 charakterystyki kolowej zwarcie miedzyfazowe + double kp; ///< wspolczynnik powrotu +//A2 +// double U_min; ///< Minimalna wartosc napiecia + double fi3; ///< Kat kierunkowy od osi X impedancyjnego kryterium kierunku + double fi4; ///< Kat kierunkowy od osi R impedancyjnego kryterium kierunku + double RLf; ///< Zasieg „ do przodu” kryterium blokady dzialania w zakresie pradow obcia¿enia + double RLr; ///< Zasieg „ do tylu” kryterium blokady dzialania w zakresie pradow obcia¿enia + double fi5; ///< Kat kierunkowy blokady dzialania w zakresie pradow obcia¿enia + double Iomin; ///< Minimalna wartosc rozruchowa pradu zerowego przekaznika identyfikacji zwarc z udzialem ziemi + double Iokh; ///< Wspolczynnik stabilizacji charakterystyki przekaznika identyfikacji zwarc z udzialem ziemi + double Uomin; ///< minimalna wartoœæ napiêcia sk³adowej zerowej + + long Typ0; ///< Typ charakterystyki strefa 1W + long K0; ///< Kierunek dzialania strefa 1W + +}__attribute__((__packed__)); + +struct ZDistA_args +{ + struct ZDistA_io io; + struct ZDistA_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void ZDistA(void *args, void *logic); +extern int ZDistA_initlog(void *arguments, void *logic); + +#endif /* ZDISTA_H_ */ diff --git a/src/ZDistA_kopia.c_ b/src/ZDistA_kopia.c_ new file mode 100644 index 0000000..27cd647 --- /dev/null +++ b/src/ZDistA_kopia.c_ @@ -0,0 +1,1124 @@ +/* + * ZDistA.c + * + * Created on: 09-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + + +#include "../tdefs.h" +#include "../misc.h" +#include "helper.h" +#include "analog_in.h" + +#include "ZDistA.h" + +#define RnaS (3.141593 / 180) +#define filtr_P 5,5 + +struct nast_pomoc +{ + float R1W_Zf1W_LE; + float R1W_Zf1W_LL; + float R1_Zf1_LE; + float R1_Zf1_LL; + float R2_Zf2_LE; + float R2_Zf2_LL; + float R3_Zf3_LE; + float R3_Zf3_LL; + float R4_Zf4_LE; + float R4_Zf4_LL; + float R5_Zf5_LE; + float R5_Zf5_LL; + float X1W_Zr1W_LE; + float X1W_Zr1W_LL; + float X1_Zr1_LE; + float X1_Zr1_LL; + float X2_Zr2_LE; + float X2_Zr2_LL; + float X3_Zr3_LE; + float X3_Zr3_LL; + float X4_Zr4_LE; + float X4_Zr4_LL; + float X5_Zr5_LE; + float X5_Zr5_LL; +}; + +int ZDistA_initlog(void *arguments, void *logic) +{ + struct ZDistA_args *args = (struct ZDistA_args *)arguments; + struct ZDistA_logic *log = (struct ZDistA_logic *)logic; + float locX1,locX2,locR1,locR2; + float sink,cosk; + u16 i; + + if(set_bit_ptr_struct(args->io.bl_in,&log->stan_bl)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_k_in,&log->Bl_K)) + return -1; + + if(set_float_ptr(args->io.i1_orta_float_in,&log->I1_orta)) + return -1; + if(set_float_ptr(args->io.i1_ortb_float_in,&log->I1_ortb)) + return -1; + if(set_float_ptr(args->io.i1_float_in,&log->I1)) + return -1; + if(set_float_ptr(args->io.i2_orta_float_in,&log->I2_orta)) + return -1; + if(set_float_ptr(args->io.i2_ortb_float_in,&log->I2_ortb)) + return -1; + if(set_float_ptr(args->io.i2_float_in,&log->I2)) + return -1; + if(set_float_ptr(args->io.i3_orta_float_in,&log->I3_orta)) + return -1; + if(set_float_ptr(args->io.i3_ortb_float_in,&log->I3_ortb)) + return -1; + if(set_float_ptr(args->io.i3_float_in,&log->I3)) + return -1; + + if(set_float_ptr(args->io.u1_orta_float_in,&log->U1_orta)) + return -1; + if(set_float_ptr(args->io.u1_ortb_float_in,&log->U1_ortb)) + return -1; + if(set_float_ptr(args->io.u1_float_in,&log->U1)) + return -1; + if(set_float_ptr(args->io.u2_orta_float_in,&log->U2_orta)) + return -1; + if(set_float_ptr(args->io.u2_ortb_float_in,&log->U2_ortb)) + return -1; + if(set_float_ptr(args->io.u2_float_in,&log->U2)) + return -1; + if(set_float_ptr(args->io.u3_orta_float_in,&log->U3_orta)) + return -1; + if(set_float_ptr(args->io.u3_ortb_float_in,&log->U3_ortb)) + return -1; + if(set_float_ptr(args->io.u3_float_in,&log->U3)) + return -1; + + if(set_float_ptr(args->io.u12_orta_float_in,&log->U12_orta)) + return -1; + if(set_float_ptr(args->io.u12_ortb_float_in,&log->U12_ortb)) + return -1; + if(set_float_ptr(args->io.u12_float_in,&log->U12)) + return -1; + if(set_float_ptr(args->io.u23_orta_float_in,&log->U23_orta)) + return -1; + if(set_float_ptr(args->io.u23_ortb_float_in,&log->U23_ortb)) + return -1; + if(set_float_ptr(args->io.u23_float_in,&log->U23)) + return -1; + if(set_float_ptr(args->io.u31_orta_float_in,&log->U31_orta)) + return -1; + if(set_float_ptr(args->io.u31_ortb_float_in,&log->U31_ortb)) + return -1; + if(set_float_ptr(args->io.u31_float_in,&log->U31)) + return -1; + + if(set_float_ptr(args->io.i1_zg_orta_float_in,&log->sI1_orta)) + return -1; + if(set_float_ptr(args->io.i1_zg_ortb_float_in,&log->sI1_ortb)) + return -1; + if(set_float_ptr(args->io.i1_zg_float_in,&log->sI1)) + return -1; + + if(set_float_ptr(args->io.i2_pr_orta_float_in,&log->sI2_orta)) + return -1; + if(set_float_ptr(args->io.i2_pr_ortb_float_in,&log->sI2_ortb)) + return -1; + if(set_float_ptr(args->io.i2_pr_float_in,&log->sI2)) + return -1; + + if(set_float_ptr(args->io.io_orta_float_in,&log->sI0_orta)) + return -1; + if(set_float_ptr(args->io.io_ortb_float_in,&log->sI0_ortb)) + return -1; + if(set_float_ptr(args->io.io_float_in,&log->sI0)) + return -1; + + if(set_float_ptr(args->io.u1_zg_orta_float_in,&log->sU1_orta)) + return -1; + if(set_float_ptr(args->io.u1_zg_ortb_float_in,&log->sU1_ortb)) + return -1; + if(set_float_ptr(args->io.u1_zg_float_in,&log->sU1)) + return -1; + + if(set_float_ptr(args->io.u2_pr_orta_float_in,&log->sU2_orta)) + return -1; + if(set_float_ptr(args->io.u2_pr_ortb_float_in,&log->sU2_ortb)) + return -1; + if(set_float_ptr(args->io.u2_pr_float_in,&log->sU2)) + return -1; + + if(set_float_ptr(args->io.uo_orta_float_in,&log->sU0_orta)) + return -1; + if(set_float_ptr(args->io.uo_ortb_float_in,&log->sU0_ortb)) + return -1; + if(set_float_ptr(args->io.uo_float_in,&log->sU0)) + return -1; + + if(set_pointer_in_ptr(args->io.i_param_an_ptr_in,(u32 *)&log->param_I)) + return -1; + + if(set_pointer_in_ptr(args->io.u_param_an_ptr_in,(u32 *)&log->param_U)) + return -1; + + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.P1W_L1E_out,&log->P1W_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L2E_out,&log->P1W_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L3E_out,&log->P1W_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P1W_L1L2_out,&log->P1W_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L2L3_out,&log->P1W_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_L3L1_out,&log->P1W_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P1_L1E_out,&log->P1_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L2E_out,&log->P1_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L3E_out,&log->P1_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P1_L1L2_out,&log->P1_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L2L3_out,&log->P1_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P1_L3L1_out,&log->P1_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P2_L1E_out,&log->P2_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L2E_out,&log->P2_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L3E_out,&log->P2_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P2_L1L2_out,&log->P2_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L2L3_out,&log->P2_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P2_L3L1_out,&log->P2_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P3_L1E_out,&log->P3_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L2E_out,&log->P3_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L3E_out,&log->P3_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P3_L1L2_out,&log->P3_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L2L3_out,&log->P3_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P3_L3L1_out,&log->P3_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P4_L1E_out,&log->P4_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L2E_out,&log->P4_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L3E_out,&log->P4_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P4_L1L2_out,&log->P4_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L2L3_out,&log->P4_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P4_L3L1_out,&log->P4_L3L1)) + return -1; + + if(set_bit_ptr_struct(args->io.P5_L1E_out,&log->P5_L1E)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L2E_out,&log->P5_L2E)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L3E_out,&log->P5_L3E)) + return -1; + + if(set_bit_ptr_struct(args->io.P5_L1L2_out,&log->P5_L1L2)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L2L3_out,&log->P5_L2L3)) + return -1; + if(set_bit_ptr_struct(args->io.P5_L3L1_out,&log->P5_L3L1)) + return -1; + + if(set_pointer_out_ptr(args->io.wy_ptr_out,(u32 *)&log->nast_.wyjscie)) + return -1; + + //debug + if(set_float_ptr(args->io.z1_float_out,&log->z[0])) + return -1; + if(set_float_ptr(args->io.z2_float_out,&log->z[1])) + return -1; + if(set_float_ptr(args->io.z3_float_out,&log->z[2])) + return -1; + if(set_float_ptr(args->io.z4_float_out,&log->z[3])) + return -1; + if(set_float_ptr(args->io.z5_float_out,&log->z[4])) + return -1; + if(set_float_ptr(args->io.z6_float_out,&log->z[5])) + return -1; + + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0002)?1:0; + log->nast_.Bl_L[1] = (args->params.bity & 0x0004)?1:0; + log->nast_.Bl_L[2] = (args->params.bity & 0x0008)?1:0; + log->nast_.Bl_L[3] = (args->params.bity & 0x0010)?1:0; + log->nast_.Bl_L[4] = (args->params.bity & 0x0020)?1:0; + log->nast_.Bl_L[5] = (args->params.bity & 0x0040)?1:0; + log->nast_.Bl_L[0] = (args->params.bity & 0x0080)?1:0; + + //wyliczenie przekladni impedancyjnej + log->nast_.wyjscie.przekladnia = log->param_I->znam_wtor / log->param_U->znam_wtor; + log->nast_.wyjscie.Zdist_dw = &log->dw; + log->nast_.wyjscie.on = &log->nast_.on_; + + //wyliczenie wektorow kompensacji ziemnozwarciowej + log->nast_.ReK1 = 3 * args->params.Kk1 * cos(RnaS * -args->params.Kk1_kat); + log->nast_.ImK1 = 3 * args->params.Kk1 * sin(RnaS * -args->params.Kk1_kat); + log->nast_.ReKr = 3 * args->params.KkC * cos(RnaS * -args->params.KkC_kat); + log->nast_.ImKr = 3 * args->params.KkC * sin(RnaS * -args->params.KkC_kat); + + log->nast_.kp = args->params.kp; + log->nast_.kpp = 1/args->params.kp; + log->nast_.kpk = log->nast_.kpp * log->nast_.kpp; + + log->nast_.typ[0] = args->params.Typ0; + log->nast_.typ[1] = args->params.Typ1; + log->nast_.typ[2] = args->params.Typ2; + log->nast_.typ[3] = args->params.Typ3; + log->nast_.typ[4] = args->params.Typ4; + log->nast_.typ[5] = args->params.Typ5; + + sink = sin(RnaS * args->params.fi1); + cosk = cos(RnaS * args->params.fi1); + + log->nast_.tanfi2 = tan(RnaS * args->params.fi2); + log->nast_.tanfi1 = tan(RnaS * args->params.fi1); + + + struct nast_pomoc n_pomoc; + + n_pomoc.R1W_Zf1W_LE = (float)args->params.R1W_Zf1W_LE; + n_pomoc.R1W_Zf1W_LL = (float)args->params.R1W_Zf1W_LL; + n_pomoc.R1_Zf1_LE = (float)args->params.R1_Zf1_LE; + n_pomoc.R1_Zf1_LL = (float)args->params.R1_Zf1_LL; + n_pomoc.R2_Zf2_LE = (float)args->params.R2_Zf2_LE; + n_pomoc.R2_Zf2_LL = (float)args->params.R2_Zf2_LL; + n_pomoc.R3_Zf3_LE = (float)args->params.R3_Zf3_LE; + n_pomoc.R3_Zf3_LL = (float)args->params.R3_Zf3_LL; + n_pomoc.R4_Zf4_LE = (float)args->params.R4_Zf4_LE; + n_pomoc.R4_Zf4_LL = (float)args->params.R4_Zf4_LL; + n_pomoc.R5_Zf5_LE = (float)args->params.R5_Zf5_LE; + n_pomoc.R5_Zf5_LL = (float)args->params.R5_Zf5_LL; + n_pomoc.X1W_Zr1W_LE = (float)args->params.X1W_Zr1W_LE; + n_pomoc.X1W_Zr1W_LL = (float)args->params.X1W_Zr1W_LL; + n_pomoc.X1_Zr1_LE = (float)args->params.X1_Zr1_LE; + n_pomoc.X1_Zr1_LL = (float)args->params.X1_Zr1_LL; + n_pomoc.X2_Zr2_LE = (float)args->params.X2_Zr2_LE; + n_pomoc.X2_Zr2_LL = (float)args->params.X2_Zr2_LL; + n_pomoc.X3_Zr3_LE = (float)args->params.X3_Zr3_LE; + n_pomoc.X3_Zr3_LL = (float)args->params.X3_Zr3_LL; + n_pomoc.X4_Zr4_LE = (float)args->params.X4_Zr4_LE; + n_pomoc.X4_Zr4_LL = (float)args->params.X4_Zr4_LL; + n_pomoc.X5_Zr5_LE = (float)args->params.X5_Zr5_LE; + n_pomoc.X5_Zr5_LL = (float)args->params.X5_Zr5_LL; + + for (i=0;i<12;i++) + { + if (log->nast_.typ[i/2]) + { + log->nast_.n_pol[i/2][i%2].Z = (*(float*)(&n_pomoc.R1W_Zf1W_LE + i) + *(float*)(&n_pomoc.X1W_Zr1W_LE + i)) * log->nast_.wyjscie.przekladnia / 2; + log->nast_.n_pol[i/2][i%2].Zp = log->nast_.n_pol[i/2][i%2].Z * args->params.kp; + log->nast_.n_pol[i/2][i%2].Z *= log->nast_.n_pol[i/2][i%2].Z; + log->nast_.n_pol[i/2][i%2].Zp *= log->nast_.n_pol[i/2][i%2].Zp; + locR1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * cosk; + locR2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * cosk; + locX1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * sink; + locX2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * sink; + log->nast_.n_pol[i/2][i%2].Rr = (locR1 - (locR1 + locR2 ) / 2) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Xr = (locX1 - (locX1 + locX2 ) / 2) * log->nast_.wyjscie.przekladnia; + locR1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * cosk * args->params.kp; + locR2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * cosk * args->params.kp; + locX1 = *(float*)(&n_pomoc.R1W_Zf1W_LE + i) * sink * args->params.kp; + locX2 = *(float*)(&n_pomoc.X1W_Zr1W_LE + i) * sink * args->params.kp; + log->nast_.n_pol[i/2][i%2].Rp = (locR1 - (locR1 + locR2 ) / 2) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Xp = (locX1 - (locX1 + locX2 ) / 2) * log->nast_.wyjscie.przekladnia; + + + } else { + log->nast_.n_pol[i/2][i%2].Rr = *(float *)(&n_pomoc.R1W_Zf1W_LE + i) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Xr = *(float *)(&n_pomoc.X1W_Zr1W_LE + i) * log->nast_.wyjscie.przekladnia; + log->nast_.n_pol[i/2][i%2].Rp = log->nast_.n_pol[i/2][i%2].Rr * args->params.kp; + log->nast_.n_pol[i/2][i%2].Xp = log->nast_.n_pol[i/2][i%2].Xr * args->params.kp; + + log->nast_.n_pol[i/2][i%2].Rrtanfi1 = log->nast_.n_pol[i/2][i%2].Rr * log->nast_.tanfi1; + log->nast_.n_pol[i/2][i%2].Rptanfi1 = log->nast_.n_pol[i/2][i%2].Rp * log->nast_.tanfi1; + + } + } + + + + log->nast_.Xr1f = args->params.X1_Zr1_LE * (1 + log->nast_.tanfi2 / log->nast_.tanfi1) * log->nast_.wyjscie.przekladnia; + log->nast_.Xr1fp = log->nast_.Xr1f * args->params.kp; + log->nast_.Xr1Wf = args->params.X1W_Zr1W_LE * (1 + log->nast_.tanfi2 / log->nast_.tanfi1) * log->nast_.wyjscie.przekladnia; + log->nast_.Xr1Wfp = log->nast_.Xr1Wf * args->params.kp; + log->nast_.Igr = args->params.I_min * args->params.I_min; + log->nast_.Igrp = log->nast_.Igr * log->nast_.kpk; + + log->nast_.Zgr = 0.01f * log->nast_.wyjscie.przekladnia * log->nast_.wyjscie.przekladnia; + log->nast_.kierunek[0] = args->params.K0; + log->nast_.kierunek[1] = args->params.K1; + log->nast_.kierunek[2] = args->params.K2; + log->nast_.kierunek[3] = args->params.K3; + log->nast_.kierunek[4] = args->params.K4; + log->nast_.kierunek[5] = args->params.K5; + +//A2 + + log->nast_.XKR = -tan(RnaS * args->params.fi4); + log->nast_.XKX = -tan(RnaS * (90 - args->params.fi3)); + log->nast_.KL = tan(RnaS * args->params.fi5); + log->nast_.RLf = args->params.RLf; + log->nast_.RLr = - args->params.RLr; + log->nast_.Iogr = args->params.Iomin * args->params.Iomin * 0.11111f; + log->nast_.khio = args->params.Iokh * args->params. Iokh * 0.11111f; + log->nast_.Uomin = args->params.Uomin * args->params.Uomin * 0.11111f; + + log->dw.log_ptr = log; + + return 0; +} + +void ZDistA(void *arguments, void *logic) +{ + struct ZDistA_logic *log = (struct ZDistA_logic *)logic; + float KIoorta,KIoortb,Iorta,Iortb,I,Uorta,Uortb; + u8 wk1,wk2,wk1p,wk2p; + u16 i,j; + float local,localp,local0,local0p; + float local1,local1p,local2,local2p,local3,local3p; + + log->dw.I1 = *log->I1; + log->dw.I2 = *log->I2; + log->dw.I3 = *log->I3; + log->dw.U1 = *log->U1; + log->dw.U2 = *log->U2; + log->dw.U3 = *log->U3; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + + //wyliczenie impedancji petli zwarciowych + //-------------------------------------------------------------------------------- + //wyliczenie K * 3Io dla strefy pierwszej + KIoorta = *log->sI0_orta * log->nast_.ReK1 - + *log->sI0_ortb * log->nast_.ImK1; + KIoortb = *log->sI0_orta * log->nast_.ImK1 + + *log->sI0_ortb * log->nast_.ReK1; + + + //petla zwarciowa L1-E strefa pierwsza + // I = I1 - k * 3 Io + Iorta = *log->I1_orta + KIoorta; + Iortb = *log->I1_ortb + KIoortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - k * 3Io) + log->dw.Z.R[0] = (*log->U1_orta * Iorta + + *log->U1_ortb * Iortb) * I; + log->dw.Z.X[0] = (*log->U1_orta * Iortb - + *log->U1_ortb * Iorta) * I; + log->dw.Z.Z[0] = log->dw.Z.X[0] * log->dw.Z.X[0] + log->dw.Z.R[0] * log->dw.Z.R[0]; + + //petla zwarciowa L2-E strefa pierwsza + // I = I2 - k * 3 Io + Iorta = *log->I2_orta + KIoorta; + Iortb = *log->I2_ortb + KIoortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I2 - k * 3Io) + log->dw.Z.R[1] = (*log->U2_orta * Iorta + + *log->U2_ortb * Iortb) * I; + log->dw.Z.X[1] = (*log->U2_orta * Iortb - + *log->U2_ortb * Iorta) * I; + log->dw.Z.Z[1] = log->dw.Z.X[1] * log->dw.Z.X[1] + log->dw.Z.R[1] * log->dw.Z.R[1]; + + //petla zwarciowa L3-E strefa pierwsza + // I = I3 - k * 3 Io + Iorta = *log->I3_orta + KIoorta; + Iortb = *log->I3_ortb + KIoortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I3 - k * 3Io) + log->dw.Z.R[2] = (*log->U3_orta * Iorta + + *log->U3_ortb * Iortb) * I; + log->dw.Z.X[2] = (*log->U3_orta * Iortb - + *log->U3_ortb * Iorta) * I; + log->dw.Z.Z[2] = log->dw.Z.X[2] * log->dw.Z.X[2] + log->dw.Z.R[2] * log->dw.Z.R[2]; + + + //wyliczenie K * 3Io dla pozostalych stref + KIoorta = *log->sI0_orta * log->nast_.ReKr - + *log->sI0_ortb * log->nast_.ImKr; + KIoortb = *log->sI0_orta * log->nast_.ImKr + + *log->sI0_ortb * log->nast_.ReKr; + + //petla zwarciowa L1-E pozostale strefy + // I = I1 - k * 3 Io + Iorta = *log->I1_orta + KIoorta; + Iortb = *log->I1_ortb + KIoortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - k * 3Io) + log->dw.Z.R[3] = (*log->U1_orta * Iorta + + *log->U1_ortb * Iortb) * I; + log->dw.Z.X[3] = (*log->U1_orta * Iortb - + *log->U1_ortb * Iorta) * I; + log->dw.Z.Z[3] = log->dw.Z.X[3] * log->dw.Z.X[3] + log->dw.Z.R[3] * log->dw.Z.R[3]; + + //petla zwarciowa L2-E pozostale strefy + // I = I2 - k * 3 Io + Iorta = *log->I2_orta + KIoorta; + Iortb = *log->I2_ortb + KIoortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I2 - k * 3Io) + log->dw.Z.R[4] = (*log->U2_orta * Iorta + + *log->U2_ortb * Iortb) * I; + log->dw.Z.X[4] = (*log->U2_orta * Iortb - + *log->U2_ortb * Iorta) * I; + log->dw.Z.Z[4] = log->dw.Z.X[4] * log->dw.Z.X[4] + log->dw.Z.R[4] * log->dw.Z.R[4]; + + //petla zwarciowa L3-E pozostale strefy + // I = I3 - k * 3 Io + Iorta = *log->I3_orta + KIoorta; + Iortb = *log->I3_ortb + KIoortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I3 - k * 3Io) + log->dw.Z.R[5] = (*log->U3_orta * Iorta + + *log->U3_ortb * Iortb) * I; + log->dw.Z.X[5] = (*log->U3_orta * Iortb - + *log->U3_ortb * Iorta) * I; + log->dw.Z.Z[5] = log->dw.Z.X[5] * log->dw.Z.X[5] + log->dw.Z.R[5] * log->dw.Z.R[5]; + + //petla zwarciowa L1-L2 wszystkie strefy + // I = I1 - I2 + Iorta = *log->I1_orta - *log->I2_orta; + Iortb = *log->I1_ortb - *log->I2_ortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - I2) + log->dw.Z.R[6] = (*log->U12_orta * Iorta + + *log->U12_ortb * Iortb) * I; + log->dw.Z.X[6] = (*log->U12_orta * Iortb - + *log->U12_ortb * Iorta) * I; + log->dw.Z.Z[6] = log->dw.Z.X[6] * log->dw.Z.X[6] + log->dw.Z.R[6] * log->dw.Z.R[6]; + + *log->z[0]=log->dw.Z.R[6]; + *log->z[1]=log->dw.Z.X[6]; + //petla zwarciowa L2-L3 wszystkie strefy + // I = I2 - I3 + Iorta = *log->I2_orta - *log->I3_orta; + Iortb = *log->I2_ortb - *log->I3_ortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - I2) + log->dw.Z.R[7] = (*log->U23_orta * Iorta + + *log->U23_ortb * Iortb) * I; + log->dw.Z.X[7] = (*log->U23_orta * Iortb - + *log->U23_ortb * Iorta) * I; + log->dw.Z.Z[7] = log->dw.Z.X[7] * log->dw.Z.X[7] + log->dw.Z.R[7] * log->dw.Z.R[7]; + *log->z[2]=log->dw.Z.R[7]; + *log->z[3]=log->dw.Z.X[7]; + + //petla zwarciowa L3-L1 wszystkie strefy + // I = I3 - I1 + Iorta = *log->I3_orta - *log->I1_orta; + Iortb = *log->I3_ortb - *log->I1_ortb; + I = 1 / (Iorta * Iorta + Iortb * Iortb); + // Z = U / (I1 - I2) + log->dw.Z.R[8] = (*log->U31_orta * Iorta + + *log->U31_ortb * Iortb) * I; + log->dw.Z.X[8] = (*log->U31_orta * Iortb - + *log->U31_ortb * Iorta) * I; + log->dw.Z.Z[8] = log->dw.Z.X[8] * log->dw.Z.X[8] + log->dw.Z.R[8] * log->dw.Z.R[8]; + + *log->z[4]=log->dw.Z.R[8]; + *log->z[5]=log->dw.Z.X[8]; + + //-------------------------------------------------------------------------------- + + + //Sprawdzenie wielkosci pradow fazowych + + if (log->dw.I1 > log->nast_.Igr) log->dw.Igr[0] = 1; + if (log->dw.I1 < log->nast_.Igrp) log->dw.Igr[0] = 0; + + if (log->dw.I2 > log->nast_.Igr) log->dw.Igr[1] = 1; + if (log->dw.I2 < log->nast_.Igrp) log->dw.Igr[1] = 0; + + if (log->dw.I3 > log->nast_.Igr) log->dw.Igr[2] = 1; + if (log->dw.I3 < log->nast_.Igrp) log->dw.Igr[2] = 0; + + + log->dw.Igr[3] = log->dw.Igr[0] && log->dw.Igr[1]; + log->dw.Igr[4] = log->dw.Igr[1] && log->dw.Igr[2]; + log->dw.Igr[5] = log->dw.Igr[2] && log->dw.Igr[0]; + + + //identyfikacja zwarc z udzialem ziemi + local = log->nast_.Iogr; + localp = local * log->nast_.kpk; + wk1p = ((*log->sI0 > local) + && (*log->sU0 > log->nast_.Uomin))?1:0; + + wk2p = ((*log->sI0 < localp) + || (*log->sU0 < log->nast_.Uomin))?1:0; + + + + if (log->dw.I1 > log->dw.I2) local = log->dw.I1; + else local = log->dw.I2; + if (log->dw.I3 > local) local = log->dw.I3; + + + local = local * log->nast_.khio; + localp = local * log->nast_.kpk; + + + wk1 = (wk1p && + (*log->sI0 > local))?1:0; + wk2 = (wk2p || + (*log->sI0 < localp))?1:0; + + + sprawdz_P( + &log->dw.Iogr, + wk1, + wk2, + &log->dw.liczpIo, + 3,3 + ); + + + // impedancyjne kryterium kierunkowe i sprawdzenie obszaru obciazenia + + local = log->nast_.XKR; + localp = local * log->nast_.kp; + local0 = log->nast_.XKX; + local0p = local0 * log->nast_.kp; + + + + local1 = log->nast_.RLf * log->nast_.wyjscie.przekladnia; + local1p = local1 * log->nast_.kpp; + local2 = log->nast_.RLr * log->nast_.wyjscie.przekladnia; + local2p = local2 * log->nast_.kpp; + local3 = log->nast_.KL; + local3p = local3 * log->nast_.kp; + + for(i=0;i<9;i++) + { + + wk1 = log->dw.Z.X[i] > local * log->dw.Z.R[i]; + wk1p = log->dw.Z.X[i] < localp * log->dw.Z.R[i]; + wk2 = log->dw.Z.X[i] > local0 * log->dw.Z.R[i]; + wk2p = log->dw.Z.X[i] < local0p * log->dw.Z.R[i]; + + int licz_p = 5; + + /* + if (((log->dw.Z.X[i] < 0) && (log->dw.Z.R[i] > 0)) || ((log->dw.Z.X[i] > 0) && (log->dw.Z.R[i] < 0))) + licz_p = 20; + else + licz_p = 5; + */ + + sprawdz_P( //kierunek do przodu + &log->dw.XKp[i], + wk1 && wk2, + wk1p || wk2p, + &log->dw.liczp11[i], + 8, + 4 + ); + + + sprawdz_P( //kierunek do tylu + &log->dw.XKm[i], + !wk1 && !wk2, + !wk1p || !wk2p, + &log->dw.liczp12[i], + 8, + 4 + ); + + + sprawdz_P( // blokada od pradow obcia¿enia + &log->dw.BL_Load[i], + (log->dw.Z.R[i] > local1 || log->dw.Z.R[i] < local2) && + fabs(log->dw.Z.X[i]) < fabs(local3 * log->dw.Z.R[i]), + (log->dw.Z.R[i] < local1p && log->dw.Z.R[i] > local2p) || + fabs(log->dw.Z.X[i]) > fabs(local3p * log->dw.Z.R[i]), + &log->dw.liczp10[i], + filtr_P + ); + + + } + + + //-------------------------------------------------------------------------------- + // identyfikacja bardzo bliskich zwarc + + log->dw.Zm[0] = log->dw.Z.Z[0] < log->nast_.Zgr || log->dw.U1 < 0.0001f ; + log->dw.Zm[1] = log->dw.Z.Z[1] < log->nast_.Zgr || log->dw.U2 < 0.0001f ; + log->dw.Zm[2] = log->dw.Z.Z[2] < log->nast_.Zgr || log->dw.U3 < 0.0001f ; + log->dw.Zm[3] = log->dw.Z.Z[3] < log->nast_.Zgr || log->dw.U1 < 0.0001f ; + log->dw.Zm[4] = log->dw.Z.Z[4] < log->nast_.Zgr || log->dw.U2 < 0.0001f ; + log->dw.Zm[5] = log->dw.Z.Z[5] < log->nast_.Zgr || log->dw.U3 < 0.0001f ; + log->dw.Zm[6] = log->dw.Z.Z[6] < log->nast_.Zgr || *log->U12 < 0.0001f ; + log->dw.Zm[7] = log->dw.Z.Z[7] < log->nast_.Zgr || *log->U23 < 0.0001f ; + log->dw.Zm[8] = log->dw.Z.Z[8] < log->nast_.Zgr || *log->U31 < 0.0001f ; + + + + //-------------------------------------------------------------------------------- + //Wyliczenie wartosci rozruchowych dla charakterystyki poligonalnej + + i = 0; + while(i<3) + { + log->dw.Z.absR[i] = fabs(log->dw.Z.R[i]); + log->dw.Z.absX[i] = fabs(log->dw.Z.X[i]); + log->dw.Z.XRtanfi1[i] = fabs(log->dw.Z.X[i] - log->dw.Z.R[i] * log->nast_.tanfi1); + log->dw.Z.XRtanfi2[i] = log->dw.Z.X[i] + log->dw.Z.R[i] * log->nast_.tanfi2; + i++; + } + + while(i<9) + { + log->dw.Z.absR[i] = fabs(log->dw.Z.R[i]); + log->dw.Z.absX[i] = fabs(log->dw.Z.X[i]); + log->dw.Z.XRtanfi1[i] = fabs(log->dw.Z.X[i] - log->dw.Z.R[i] * log->nast_.tanfi1); + i++; + } + + + + //----------------------------------------------------------------------------- + // sprawdzenie spelnienia warunku impedancji strefy + //----------------------------------------------------------------------------- + // sprawdzenie strefy 1 zwarcia jednofazowe z ziemia strefa 1W + for(i=0;i<3;i++) + { + if (log->nast_.typ[0]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[0][0].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[0][0].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = I < log->nast_.n_pol[0][0].Z && log->dw.Igr[i]; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[0][0].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[0][0].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = I > log->nast_.n_pol[0][0].Zp || !log->dw.Igr[i]; + } else { + wk1 = (log->dw.Z.absR[i] < log->nast_.n_pol[0][0].Rr || log->dw.Z.XRtanfi1[i] < log->nast_.n_pol[0][0].Rrtanfi1) + && log->dw.Z.XRtanfi2[i] < log->nast_.Xr1Wf + && log->dw.Igr[i] + && log->dw.Z.absX[i] < log->nast_.n_pol[0][0].Xr; + wk2 = (log->dw.Z.XRtanfi1[i] > log->nast_.n_pol[0][0].Rptanfi1 && log->dw.Z.absR[i] > log->nast_.n_pol[0][0].Rp) + || !log->dw.Igr[i] + || log->dw.Z.XRtanfi2[i] > log->nast_.Xr1Wfp + || log->dw.Z.absX[i] > log->nast_.n_pol[0][0].Xp; + } + sprawdz_P( + &log->dw.P_s[0][i], + wk1, + wk2, + &log->dw.liczps[0][i], + filtr_P + ); + log->dw.Pbk[0][i] = log->dw.P_s[0][i] && log->dw.Iogr && !(log->nast_.Bl_L[0] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + } + + // sprawdzenie strefy 1 zwarcia jednofazowe z ziemia strefa 1 + for(i=0;i<3;i++) + { + if (log->nast_.typ[1]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[1][0].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[1][0].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = I < log->nast_.n_pol[1][0].Z && log->dw.Igr[i]; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[1][0].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[1][0].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = I > log->nast_.n_pol[1][0].Zp || !log->dw.Igr[i]; + } else { + wk1 = (log->dw.Z.absR[i] < log->nast_.n_pol[1][0].Rr || log->dw.Z.XRtanfi1[i] < log->nast_.n_pol[1][0].Rrtanfi1) + && log->dw.Z.XRtanfi2[i] < log->nast_.Xr1f + && log->dw.Igr[i] + && log->dw.Z.absX[i] < log->nast_.n_pol[1][0].Xr; + wk2 = (log->dw.Z.absR[i] > log->nast_.n_pol[1][0].Rp && log->dw.Z.XRtanfi1[i] > log->nast_.n_pol[1][0].Rptanfi1) + || log->dw.Z.XRtanfi2[i] > log->nast_.Xr1fp + || !log->dw.Igr[i] + || log->dw.Z.absX[i] > log->nast_.n_pol[1][0].Xp; + } + sprawdz_P( + &log->dw.P_s[1][i], + wk1, + wk2, + &log->dw.liczps[1][i], + filtr_P + ); + log->dw.Pbk[1][i] = log->dw.P_s[1][i] && log->dw.Iogr && !(log->nast_.Bl_L[1] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + } + + + //sprawdzenie pozostalych stref zwarcie jednofazowe z ziemia + for(j=2;j<6;j++) + { + for(i=3;i<6;i++) + { + if (log->nast_.typ[j]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][0].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][0].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = I < log->nast_.n_pol[j][0].Z && log->dw.Igr[i-3]; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][0].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][0].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = I > log->nast_.n_pol[j][0].Zp || !log->dw.Igr[i-3]; + } else { + wk1 = (log->dw.Z.absR[i] < log->nast_.n_pol[j][0].Rr || log->dw.Z.XRtanfi1[i] < log->nast_.n_pol[j][0].Rrtanfi1) + && log->dw.Igr[i-3] + && log->dw.Z.absX[i] < log->nast_.n_pol[j][0].Xr; + wk2 = (log->dw.Z.absR[i] > log->nast_.n_pol[j][0].Rp && log->dw.Z.XRtanfi1[i] > log->nast_.n_pol[j][0].Rptanfi1) + || !log->dw.Igr[i-3] + || log->dw.Z.absX[i] > log->nast_.n_pol[j][0].Xp; + } + sprawdz_P( + &log->dw.P_s[j][i-3], + wk1, + wk2, + &log->dw.liczps[j][i-3], + filtr_P + ); + + log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && log->dw.Iogr && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz kryterium zwarc z udzialem ziemi + } + + } + + //sprawdzenie petli miedzyfazowych + for(j=0;j<6;j++) + { + for(i=6;i<9;i++) + { + if (log->nast_.typ[j]) + { + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][1].Rr; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][1].Xr; + I = Iorta * Iorta + Iortb * Iortb; + wk1 = (I < log->nast_.n_pol[j][1].Z && log->dw.Igr[i-3])?1:0; + Iorta = log->dw.Z.R[i] - log->nast_.n_pol[j][1].Rp; + Iortb = log->dw.Z.X[i] - log->nast_.n_pol[j][1].Xp; + I = Iorta * Iorta + Iortb * Iortb; + wk2 = (I > log->nast_.n_pol[j][1].Zp || !log->dw.Igr[i-3])?1:0; + } else { + wk1 = (log->dw.Z.absR[i] < log->nast_.n_pol[j][1].Rr || log->dw.Z.XRtanfi1[i] < log->nast_.n_pol[j][1].Rrtanfi1) + && log->dw.Igr[i-3] + && log->dw.Z.absX[i] < log->nast_.n_pol[j][1].Xr; + wk2 = (log->dw.Z.absR[i] > log->nast_.n_pol[j][1].Rp && log->dw.Z.XRtanfi1[i] > log->nast_.n_pol[j][1].Rptanfi1) + || !log->dw.Igr[i-3] + || log->dw.Z.absX[i] > log->nast_.n_pol[j][1].Xp; + } + sprawdz_P( + &log->dw.P_s[j][i-3], + wk1, + wk2, + &log->dw.liczps[j][i-3], + filtr_P + ); + log->dw.Pbk[j][i-3] = log->dw.P_s[j][i-3] && !(log->nast_.Bl_L[j] && log->dw.BL_Load[i]); // uwzglednienie blokady od pradow obcia¿enia oraz minimalnej wartosci pradu + + } + + + + + + } + + + + + //----------------------------------------------------------------------------- + // sprawdzenie dodatkowego kryterium kierunkowego + + if (*log->sU2 > 0.001f) + { + // kryterium skladowych przeciwnych + //przesuniecia skladowych napiecia o 45 stopni + if (*log->sI2 > 0.0003f) + { + Uorta = (*log->sU2_orta - *log->sU2_ortb) * 0.7071f; // + Uortb = (*log->sU2_orta + *log->sU2_ortb) * 0.7071f; + I = *log->sI2_orta * Uorta + + *log->sI2_ortb * Uortb; + + wk1 = I < 0; + sprawdz_P ( + &log->dw.Kp, + wk1, + !wk1, + &log->dw.liczKdod, + 3,3 + ); + log->dw.Km = !log->dw.Kp; + } else { + log->dw.Km = log->dw.Kp = 0; + } + } else { + // kryterium skladowych zgodnych + //przesuniecia skladowych napiecia o 45 stopni + if ((*log->sI1 > 0.0005f) && + (*log->sU1 > 0.00001f)) + { + Uorta = (*log->sU1_orta - *log->sU1_ortb) * 0.7071f; // + Uortb = (*log->sU1_orta + *log->sU1_ortb) * 0.7071f; + I = *log->sI1_orta * Uorta + + *log->sI1_ortb * Uortb; + wk1 = I > 0; + sprawdz_P ( + &log->dw.Kp, + wk1, + !wk1, + &log->dw.liczKdod, + 3,3 + ); + log->dw.Km = !log->dw.Kp; + } else { + log->dw.Km = log->dw.Kp = 0; + } + } + + + + + + //----------------------------------------------------------------------------- + //Ustalenie pobudzen stref + //okreslenie warunkow kierunkowych + //zwarcie jednofazowe z ziemia strefa 1 i 1W + + if (!check_struct(&log->Bl_K))//jeœli nie ma blokady kierunku + { + + switch(log->nast_.kierunek[0]) + { + case 0: //bezkierunku + for(i=0;i<3;i++) + { + log->dw.P[0][i] = log->dw.Pbk[0][i]; + log->dw.P[1][i] = log->dw.Pbk[1][i]; + } + break; + case 1: //do linii + for(i=0;i<3;i++) + { + wk1 = log->dw.Kp && (log->dw.XKp[i] || log->dw.Zm[i]); + log->dw.P[0][i] = log->dw.Pbk[0][i] && wk1; + log->dw.P[1][i] = log->dw.Pbk[1][i] && wk1; + } + break; + case 2: // do szyn + for(i=0;i<3;i++) + { + wk1 = log->dw.Km && (log->dw.XKm[i] || log->dw.Zm[i]); + log->dw.P[0][i] = log->dw.Pbk[0][i] && wk1; + log->dw.P[1][i] = log->dw.Pbk[1][i] && wk1; + } + break; + } + + + //zwarcie jednofazowe z ziemia - pozostale + for(j=2;j<6;j++) + { + switch(log->nast_.kierunek[j]) + { + case 0: //bezkierunku + for(i=3;i<6;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3]; + } + break; + case 1: //do linii + for(i=3;i<6;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Kp && (log->dw.XKp[i] || log->dw.Zm[i]); + } + break; + case 2: // do szyn + for(i=3;i<6;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Km && (log->dw.XKm[i] || log->dw.Zm[i]); + } + break; + } + } + + //zwarcie miedzyfazowe + for(j=0;j<6;j++) + { + switch(log->nast_.kierunek[j]) + { + case 0: //bezkierunku + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3]; + } + break; + case 1: //do linii + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Kp && (log->dw.XKp[i] || log->dw.Zm[i]); + } + break; + case 2: // do szyn + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3] && log->dw.Km && (log->dw.XKm[i] || log->dw.Zm[i]); + } + break; + } + } + } else { + for(j=0;j<6;j++) + { + for(i=6;i<9;i++) + { + log->dw.P[j][i-3] = log->dw.Pbk[j][i-3]; //poprawiony strzal z boku? + } + + } + } + + //----------------------------------------------------------------------------- + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + for(i=0;i<36;i++) + { + *(&log->dw.P[0][0] + i) = check_struct(&log->test)?1:0; + } + } + // + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && check_struct(&log->stan_bl))/* | czy_start()*/) + { + for(i=0;i<36;i++) + { + *(&log->dw.P[0][0] + i) = 0; + } + } + + /*for(i=0;i<36;i++) + { + check_and_set_struct(log->dw.P[i/6][i%6],(struct binary_io *)((u8 *)&log->P1W_L1E+(sizeof(struct binary_io)*i))); + }*/ + + check_and_set_struct(log->dw.P[0][0], &log->P1W_L1E); + check_and_set_struct(log->dw.P[0][1], &log->P1W_L2E); + check_and_set_struct(log->dw.P[0][2], &log->P1W_L3E); + check_and_set_struct(log->dw.P[0][3], &log->P1W_L1L2); + check_and_set_struct(log->dw.P[0][4], &log->P1W_L2L3); + check_and_set_struct(log->dw.P[0][5], &log->P1W_L3L1); + check_and_set_struct(log->dw.P[1][0], &log->P1_L1E); + check_and_set_struct(log->dw.P[1][1], &log->P1_L2E); + check_and_set_struct(log->dw.P[1][2], &log->P1_L3E); + check_and_set_struct(log->dw.P[1][3], &log->P1_L1L2); + check_and_set_struct(log->dw.P[1][4], &log->P1_L2L3); + check_and_set_struct(log->dw.P[1][5], &log->P1_L3L1); + check_and_set_struct(log->dw.P[2][0], &log->P2_L1E); + check_and_set_struct(log->dw.P[2][1], &log->P2_L2E); + check_and_set_struct(log->dw.P[2][2], &log->P2_L3E); + check_and_set_struct(log->dw.P[2][3], &log->P2_L1L2); + check_and_set_struct(log->dw.P[2][4], &log->P2_L2L3); + check_and_set_struct(log->dw.P[2][5], &log->P2_L3L1); + check_and_set_struct(log->dw.P[3][0], &log->P3_L1E); + check_and_set_struct(log->dw.P[3][1], &log->P3_L2E); + check_and_set_struct(log->dw.P[3][2], &log->P3_L3E); + check_and_set_struct(log->dw.P[3][3], &log->P3_L1L2); + check_and_set_struct(log->dw.P[3][4], &log->P3_L2L3); + check_and_set_struct(log->dw.P[3][5], &log->P3_L3L1); + check_and_set_struct(log->dw.P[4][0], &log->P4_L1E); + check_and_set_struct(log->dw.P[4][1], &log->P4_L2E); + check_and_set_struct(log->dw.P[4][2], &log->P4_L3E); + check_and_set_struct(log->dw.P[4][3], &log->P4_L1L2); + check_and_set_struct(log->dw.P[4][4], &log->P4_L2L3); + check_and_set_struct(log->dw.P[4][5], &log->P4_L3L1); + check_and_set_struct(log->dw.P[5][0], &log->P5_L1E); + check_and_set_struct(log->dw.P[5][1], &log->P5_L2E); + check_and_set_struct(log->dw.P[5][2], &log->P5_L3E); + check_and_set_struct(log->dw.P[5][3], &log->P5_L1L2); + check_and_set_struct(log->dw.P[5][4], &log->P5_L2L3); + check_and_set_struct(log->dw.P[5][5], &log->P5_L3L1); + } + else + { + /*for(i=0;i<36;i++) + { + //clear_struct((struct binary_io *)((u8 *)&log->P1W_L1E+(sizeof(struct binary_io)*i))); + }*/ + clear_struct(&log->P1W_L1E); + clear_struct(&log->P1W_L2E); + clear_struct(&log->P1W_L3E); + clear_struct(&log->P1W_L1L2); + clear_struct(&log->P1W_L2L3); + clear_struct(&log->P1W_L3L1); + clear_struct(&log->P1_L1E); + clear_struct(&log->P1_L2E); + clear_struct(&log->P1_L3E); + clear_struct(&log->P1_L1L2); + clear_struct(&log->P1_L2L3); + clear_struct(&log->P1_L3L1); + clear_struct(&log->P2_L1E); + clear_struct(&log->P2_L2E); + clear_struct(&log->P2_L3E); + clear_struct(&log->P2_L1L2); + clear_struct(&log->P2_L2L3); + clear_struct(&log->P2_L3L1); + clear_struct(&log->P3_L1E); + clear_struct(&log->P3_L2E); + clear_struct(&log->P3_L3E); + clear_struct(&log->P3_L1L2); + clear_struct(&log->P3_L2L3); + clear_struct(&log->P3_L3L1); + clear_struct(&log->P4_L1E); + clear_struct(&log->P4_L2E); + clear_struct(&log->P4_L3E); + clear_struct(&log->P4_L1L2); + clear_struct(&log->P4_L2L3); + clear_struct(&log->P4_L3L1); + clear_struct(&log->P5_L1E); + clear_struct(&log->P5_L2E); + clear_struct(&log->P5_L3E); + clear_struct(&log->P5_L1L2); + clear_struct(&log->P5_L2L3); + clear_struct(&log->P5_L3L1); + + } +} diff --git a/src/ZDistA_kopia.h_ b/src/ZDistA_kopia.h_ new file mode 100644 index 0000000..b31a2b5 --- /dev/null +++ b/src/ZDistA_kopia.h_ @@ -0,0 +1,425 @@ +/* + * ZDistA.h + * + * + * Created on: 09-03-2017 + * Author: KJ + */ + +#ifndef ZDISTA_H_ +#define ZDISTA_H_ + +#include "../tdefs.h" +#include "helper.h" + + struct impedancja_ZDistA + { + float R[9]; // czesc rzeczywista impedancji + float X[9]; // czesc urojona impedancji + float Z[9]; //kwadrat impedancji + float absR[9]; // wartosc bezwzgledna impedancji czesc rzeczywista + float absX[9]; // wartosc bezwzgledna impedancji czesc urojona + float XRtanfi2[9]; // abs(X+R*tan(fi2)) + float XRtanfi1[9]; // abs(X-R*tan(fi1)) + //[0] petla L1-E strefa 1 + //[1] petla L2-E strefa 1 + //[2] petla L3-E strefa 1 + //[3] petla L1-E strefa 2,3,4,5 + //[4] petla L2-E strefa 2,3,4,5 + //[5] petla L3-E strefa 2,3,4,5 + //[6] petla L1-L2 + //[7] petla L2-L3 + //[8] petla L3-L1 + }; + + struct dane_wewnetrzne_ZDistA + { + struct impedancja_ZDistA Z; + u8 Km; + u8 Kp; + u8 Igr[6]; // przekroczenie wartosci granicznej pradu + u8 Iogr; // zwarcie z udzialem ziemi + u8 P_s[6][6];//pobudzenie kryterium impedacyjnego (obszar) [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] + u8 Pbk[6][6];//pobudzenie w strefie [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] bezkierunkowe + u8 P[6][6];//pobudzenie w strefie [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] + u8 BL_Load[9]; ///< impedancja w obszrze pradow roboczych + u8 XKp[9]; ///< impedancyjne kryterium kierunku "do przodu" + u8 XKm[9]; ///< impedancyjne kryterium kierunku "do tylu" + u8 Zm[9];///< impedancyjne kryterium bardzo bliskiego zwarcia + short liczp10[9]; /// licznik pobudzen + short liczp11[9]; /// licznik pobudzen + short liczp12[9]; /// licznik pobudzen + //[0] petla L1-E strefa 1 + //[1] petla L2-E strefa 1 + //[2] petla L3-E strefa 1 + //[3] petla L1-E strefa 2,3,4,5 + //[4] petla L2-E strefa 2,3,4,5 + //[5] petla L3-E strefa 2,3,4,5 + //[6] petla L1-L2 + //[7] petla L2-L3 + //[8] petla L3-L1 + short liczps[6][6]; //liczniki pobudzen dla pobudzen stref [numer strefy][L1E,L2E,L3E,L1L2,L2L3,L3L1] + short liczpIo; //licznik pobudzen dla identyfikacji zwarc z udzialem ziemi + short liczKdod; + float U1; + float U2; + float U3; + float I1; + float I2; + float I3; + struct ZDistA_logic *log_ptr; + }; + + struct nast_w_poligon + { + float Xr; // zasieg dla poligonu i srodek okregu dla kolowej wspolrzedna X + float Rr; // zasieg dla dla poligonu i srodek okregu dla kolowej wspolrzedna R + float Z; // srednica kola charakterystyki kolowej + float Xp; // zasieg dla poligonu i srodek okregu dla kolowej wspolrzedna X dla powrotu + float Rp; // zasieg dla dla poligonu i srodek okregu dla kolowej wspolrzedna R dla powrotu + float Zp; // srednica kola charakterystyki kolowej dla powrotu + float Rrtanfi1;// wartoœæ rozruchowa prostej nachylonej k¹tem linii + float Rptanfi1;// wartoœæ powrotowa prostej nachylonej k¹tem linii + }; + + struct Wyjscia_ZDistA // struktura wyjsc przekaznika + { + struct dane_wewnetrzne_ZDistA *Zdist_dw; + float przekladnia; + u8 *on; + }; + + + struct Nastawy_przeliczone_ZDistA + { + struct Wyjscia_ZDistA wyjscie; + u8 on_; + u8 bl_; + u8 Bl_L[6]; + u8 typ[6];// typ charakterystyki 0 - poligonalna, 1- kolowa + float ReK1; // skladowa rzeczywista wspolczynnika kompensacji ziemnozwarciowej dla strefy 1 + float ImK1;// skladowa urojona wspolczynnika kompensacji ziemnozwarciowej dla strefy 1 + float ReKr;// skladowa rzeczywista wspolczynnika kompensacji ziemnozwarciowej dla stref 2,3,4,5 + float ImKr;// skladowa urojona wspolczynnika kompensacji ziemnozwarciowej dla stref 2,3,4,5 + + struct nast_w_poligon n_pol[6][2]; // nastawy stref dla charakterystyki poligonalnej [numer strefy][LE/LL] + float tanfi1; // tangens kata linii + float tanfi2; //tangens kata dodatkowej prostej dla charakterystyki poligonalnej strefa 1 + float Xr1f; // wartosc rozruchowa dodatkowej prostej dla charakterystyki poligonalnej + float Xr1fp; // wartosc powrotowa dodatkowej prostej dla charakterystyki poligonalnej + float Xr1Wf; // wartosc rozruchowa dodatkowej prostej dla charakterystyki poligonalnej + float Xr1Wfp; // wartosc powrotowa dodatkowej prostej dla charakterystyki poligonalnej + float Igr; // wartosc kwadratu pradu granicznego warunku dzialania przekaznika + float Igrp; // wartosc kwadratu pradu granicznego warunku dzialania przekaznika (powrot) + + float Zgr;// minimalna wartosc impedancji kryteriunkierunkowego w kwadracie + float kp;//wspó³czynnik powrotu dla niedomiarówek + float kpp;//wspó³czynnik powrotu dla nadmiarówek + float kpk; //kwadrat wspó³czynnika powrotu + short kierunek[6]; + + //A2 + float XKR; // wspolczynnik prostej R kierunku kryterium impedancyjnego + float XKX; // wspolczynnik prostej R kierunku kryterium impedancyjnego + float KL;//wspolczynnik prostej dla blokady od obszaru obcia¿enia + float RLf; // wartosc rozruchowa blokady Load do przodu + float RLr; // wartosc rozruchowa blokady Load do tylu + float Iogr; // wartosc graniczna skladowej zerowej identyfikacji zwarc z udzialem ziemi + float khio; // wspolczynnik stabilizacji dla przekaznika identyfikacji zwarc z udzialem ziemi + float Uomin; ///< minimalna wartoœæ napiêcia sk³adowej zerowej + + }; + +struct ZDistA_logic +{ + struct binary_io stan_bl; + struct binary_io Bl_K; + float *I1_orta; ///< Skladowa ortogonalna a pradu faza L1 + float *I1_ortb; ///< Skladowa ortogonalna b pradu faza L1 + float *I1; ///< Prad mierzony faza 1 + float *I2_orta; ///< Skladowa ortogonalna a pradu faza L2 + float *I2_ortb; ///< Skladowa ortogonalna b pradu faza L2 + float *I2; ///< Prad mierzony faza 2 + float *I3_orta; ///< Skladowa ortogonalna a pradu faza L3 + float *I3_ortb; ///< Skladowa ortogonalna b pradu faza L3 + float *I3; ///< Prad mierzony faza 3 + float *U1_orta; ///< Skladowa ortogonalna a napiecia faza L1 + float *U1_ortb; ///< Skladowa ortogonalna b napiecia faza L1 + float *U1; ///< Napiecie mierzone faza 1 + float *U2_orta; ///< Skladowa ortogonalna a napiecia faza L2 + float *U2_ortb; ///< Skladowa ortogonalna b napiecia faza L2 + float *U2; ///< Napiecie mierzone faza 2 + float *U3_orta; ///< Skladowa ortogonalna a napiecia faza L3 + float *U3_ortb; ///< Skladowa ortogonalna b napiecia faza L3 + float *U3; ///< Napiecie mierzone faza 3 + float *U12_orta; ///< Skladowa ortogonalna a napiecia L1-L2 + float *U12_ortb; ///< Skladowa ortogonalna b napiecia L1-L2 + float *U12; ///< Napiecie mierzone L1-L2 + float *U23_orta; ///< Skladowa ortogonalna a napiecia L2-L3 + float *U23_ortb; ///< Skladowa ortogonalna b napiecia L2-L3 + float *U23; ///< Napiecie mierzone L2-L3 + float *U31_orta; ///< Skladowa ortogonalna a napiecia L3-L1 + float *U31_ortb; ///< Skladowa ortogonalna b napiecia L3-L1 + float *U31; ///< Napiecie mierzone L3-L1 + float *sI1_orta; ///< Skladowa ortogonalna a skladowej zgodnej pradu + float *sI1_ortb; ///< Skladowa ortogonalna b skladowej zgodnej pradu + float *sI1; ///< Skladowa zgodna pradu + float *sI2_orta; ///< Skladowa ortogonalna a skladowej przeciwnej pradu + float *sI2_ortb; ///< Skladowa ortogonalna b skladowej przeciwnej pradu + float *sI2; ///< Skladowa przeciwna pradu + float *sI0_orta; ///< Skladowa ortogonalna a skladowej zerowej pradu + float *sI0_ortb; ///< Skladowa ortogonalna b skladowej zerowej pradu + float *sI0; ///< Skladowa zerowa pradu + float *sU1_orta; ///< Skladowa ortogonalna a skladowej zgodnej napiecia + float *sU1_ortb; ///< Skladowa ortogonalna b skladowej zgodnej napiecia + float *sU1; ///< Skladowa zgodna napiecia + float *sU2_orta; ///< Skladowa ortogonalna a skladowej przeciwnej napiecia + float *sU2_ortb; ///< Skladowa ortogonalna b skladowej przeciwnej napiecia + float *sU2; ///< Skladowa przeciwna napiecia + float *sU0_orta; ///< Skladowa ortogonalna a skladowej zerowej napiecia + float *sU0_ortb; ///< Skladowa ortogonalna b skladowej zerowej napiecia + float *sU0; ///< Skladowa zerowa napiecia + struct analog_in_params *param_I; ///< Parametry wejscia pradowego + struct analog_in_params *param_U; /// < Parametry kanalu napieciowego + struct binary_io test; + struct binary_io P1W_L1E; ///< Pobudzenie strefy 1W zwarcie L1-E + struct binary_io P1W_L2E; ///< Pobudzenie strefy 1W zwarcie L2-E + struct binary_io P1W_L3E; ///< Pobudzenie strefy 1W zwarcie L3-E + struct binary_io P1W_L1L2; ///< Pobudzenie strefy 1W zwarcie L1-L2 + struct binary_io P1W_L2L3; ///< Pobudzenie strefy 1W zwarcie L2-L3 + struct binary_io P1W_L3L1; ///< Pobudzenie strefy 1W zwarcie L3-L1 + struct binary_io P1_L1E; ///< Pobudzenie strefy 1 zwarcie L1-E + struct binary_io P1_L2E; ///< Pobudzenie strefy 1 zwarcie L2-E + struct binary_io P1_L3E; ///< Pobudzenie strefy 1 zwarcie L3-E + struct binary_io P1_L1L2; ///< Pobudzenie strefy 1 zwarcie L1-L2 + struct binary_io P1_L2L3; ///< Pobudzenie strefy 1 zwarcie L2-L3 + struct binary_io P1_L3L1; ///< Pobudzenie strefy 1 zwarcie L3-L1 + struct binary_io P2_L1E; ///< Pobudzenie strefy 2 zwarcie L1-E + struct binary_io P2_L2E; ///< Pobudzenie strefy 2 zwarcie L2-E + struct binary_io P2_L3E; ///< Pobudzenie strefy 2 zwarcie L3-E + struct binary_io P2_L1L2; ///< Pobudzenie strefy 2 zwarcie L1-L2 + struct binary_io P2_L2L3; ///< Pobudzenie strefy 2 zwarcie L2-L3 + struct binary_io P2_L3L1; ///< Pobudzenie strefy 2 zwarcie L3-L1 + struct binary_io P3_L1E; ///< Pobudzenie strefy 3 zwarcie L1-E + struct binary_io P3_L2E; ///< Pobudzenie strefy 3 zwarcie L2-E + struct binary_io P3_L3E; ///< Pobudzenie strefy 3 zwarcie L3-E + struct binary_io P3_L1L2; ///< Pobudzenie strefy 3 zwarcie L1-L2 + struct binary_io P3_L2L3; ///< Pobudzenie strefy 3 zwarcie L2-L3 + struct binary_io P3_L3L1; ///< Pobudzenie strefy 3 zwarcie L3-L1 + struct binary_io P4_L1E; ///< Pobudzenie strefy 4 zwarcie L1-E + struct binary_io P4_L2E; ///< Pobudzenie strefy 4 zwarcie L2-E + struct binary_io P4_L3E; ///< Pobudzenie strefy 4 zwarcie L3-E + struct binary_io P4_L1L2; ///< Pobudzenie strefy 4 zwarcie L1-L2 + struct binary_io P4_L2L3; ///< Pobudzenie strefy 4 zwarcie L2-L3 + struct binary_io P4_L3L1; ///< Pobudzenie strefy 4 zwarcie L3-L1 + struct binary_io P5_L1E; ///< Pobudzenie strefy 5 zwarcie L1-E + struct binary_io P5_L2E; ///< Pobudzenie strefy 5 zwarcie L2-E + struct binary_io P5_L3E; ///< Pobudzenie strefy 5 zwarcie L3-E + struct binary_io P5_L1L2; ///< Pobudzenie strefy 5 zwarcie L1-L2 + struct binary_io P5_L2L3; ///< Pobudzenie strefy 5 zwarcie L2-L3 + struct binary_io P5_L3L1; ///< Pobudzenie strefy 5 zwarcie L3-L1 + + struct dane_wewnetrzne_ZDistA dw; + struct Nastawy_przeliczone_ZDistA nast_;// struktura z parametrami + + float *z[6]; /// debug +}; + +struct ZDistA_io +{ + u32 bl_in; + u32 bl_k_in; + u32 i1_orta_float_in; + u32 i1_ortb_float_in; + u32 i1_float_in; + u32 i2_orta_float_in; + u32 i2_ortb_float_in; + u32 i2_float_in; + u32 i3_orta_float_in; + u32 i3_ortb_float_in; + u32 i3_float_in; + u32 u1_orta_float_in; + u32 u1_ortb_float_in; + u32 u1_float_in; + u32 u2_orta_float_in; + u32 u2_ortb_float_in; + u32 u2_float_in; + u32 u3_orta_float_in; + u32 u3_ortb_float_in; + u32 u3_float_in; + u32 u12_orta_float_in; + u32 u12_ortb_float_in; + u32 u12_float_in; + u32 u23_orta_float_in; + u32 u23_ortb_float_in; + u32 u23_float_in; + u32 u31_orta_float_in; + u32 u31_ortb_float_in; + u32 u31_float_in; + u32 i1_zg_orta_float_in; + u32 i1_zg_ortb_float_in; + u32 i1_zg_float_in; + u32 i2_pr_orta_float_in; + u32 i2_pr_ortb_float_in; + u32 i2_pr_float_in; + u32 io_orta_float_in; + u32 io_ortb_float_in; + u32 io_float_in; + u32 u1_zg_orta_float_in; + u32 u1_zg_ortb_float_in; + u32 u1_zg_float_in; + u32 u2_pr_orta_float_in; + u32 u2_pr_ortb_float_in; + u32 u2_pr_float_in; + u32 uo_orta_float_in; + u32 uo_ortb_float_in; + u32 uo_float_in; + u32 i_param_an_ptr_in; + u32 u_param_an_ptr_in; + u32 test_in; + u32 wy_ptr_out; //zainicjowac + u32 P1W_L1E_out; + u32 P1W_L2E_out; + u32 P1W_L3E_out; + u32 P1W_L1L2_out; + u32 P1W_L2L3_out; + u32 P1W_L3L1_out; + u32 P1_L1E_out; + u32 P1_L2E_out; + u32 P1_L3E_out; + u32 P1_L1L2_out; + u32 P1_L2L3_out; + u32 P1_L3L1_out; + u32 P2_L1E_out; + u32 P2_L2E_out; + u32 P2_L3E_out; + u32 P2_L1L2_out; + u32 P2_L2L3_out; + u32 P2_L3L1_out; + u32 P3_L1E_out; + u32 P3_L2E_out; + u32 P3_L3E_out; + u32 P3_L1L2_out; + u32 P3_L2L3_out; + u32 P3_L3L1_out; + u32 P4_L1E_out; + u32 P4_L2E_out; + u32 P4_L3E_out; + u32 P4_L1L2_out; + u32 P4_L2L3_out; + u32 P4_L3L1_out; + u32 P5_L1E_out; + u32 P5_L2E_out; + u32 P5_L3E_out; + u32 P5_L1L2_out; + u32 P5_L2L3_out; + u32 P5_L3L1_out; + + //debug + u32 z1_float_out; + u32 z2_float_out; + u32 z3_float_out; + u32 z4_float_out; + u32 z5_float_out; + u32 z6_float_out; + +}__attribute__((__packed__)); + +struct ZDistA_params +{ + u32 bity; ///< nastawy bitowe; + + long Typ1; ///< Typ charakterystyki strefa 1 + long Typ2; ///< Typ charakterystyki strefa 2 + long Typ3; ///< Typ charakterystyki strefa 3 + long Typ4; ///< Typ charakterystyki strefa 4 + long Typ5; ///< Typ charakterystyki strefa 5 + long K1; ///< Kierunek dzialania strefa 1 + long K2; ///< Kierunek dzialania strefa 2 + long K3; ///< Kierunek dzialania strefa 3 + long K4; ///< Kierunek dzialania strefa 4 + long K5; ///< Kierunek dzialania strefa 5 + + double I_min; ///< Graniczna wartosc pradu + double Kk1; ///< Wspolczynnik kompensacji ziemnozwarciowej strefa 1 + double Kk1_kat; ///< Kat wektora kompensacji ziemnozwarciowej strefa 1 + double KkC; ///< Wspolczynnik kompensacji ziemnozwarciowej stref 2,3,4,5 + double KkC_kat; ///< Kat wektora kompensacji ziemnozwarciowej stref 2,3,4,5 + double fi1; ///< Kat linii + double fi2; ///< Kat nachylenia prostej korekcji strefy pierwszej dla zwarc jednofazowych z ziemia + double R1W_Zf1W_LE; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R1W_Zf1W_LL; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” wyduzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double R1_Zf1_LE; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R1_Zf1_LL; ///< Zasieg rezystancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double R2_Zf2_LE; ///< Zasieg rezystancyjny strefy 2 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R2_Zf2_LL; ///< Zasieg rezystancyjny strefy 2 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 2 charakterystyki kolowej zwarcie miedzyfazowe + double R3_Zf3_LE; ///< Zasieg rezystancyjny strefy 3 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R3_Zf3_LL; ///< Zasieg rezystancyjny strefy 3 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 3 charakterystyki kolowej zwarcie miedzyfazowe + double R4_Zf4_LE; ///< Zasieg rezystancyjny strefy 4 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R4_Zf4_LL; ///< Zasieg rezystancyjny strefy 4 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 4 charakterystyki kolowej zwarcie miedzyfazowe + double R5_Zf5_LE; ///< Zasieg rezystancyjny strefy 5 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do przodu” strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia + double R5_Zf5_LL; ///< Zasieg rezystancyjny strefy 5 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do przodu” strefy 5 charakterystyki kolowej zwarcie miedzyfazowe + double X1W_Zr1W_LE; ///< Zasieg reaktancyjny wydluzenie strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” wydluzenie strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X1W_Zr1W_LL; ///< Zasieg reaktancyjny wydluzenie strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” wydluzenie strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double X1_Zr1_LE; ///< Zasieg reaktancyjny strefy 1 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 1 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X1_Zr1_LL; ///< Zasieg reaktancyjny strefy 1 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 1 charakterystyki kolowej zwarcie miedzyfazowe + double X2_Zr2_LE; ///< Zasieg reaktancyjny strefy 2 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 2 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X2_Zr2_LL; ///< Zasieg reaktancyjny strefy 2 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 2 charakterystyki kolowej zwarcie miedzyfazowe + double X3_Zr3_LE; ///< Zasieg reaktancyjny strefy 3 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 3 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X3_Zr3_LL; ///< Zasieg reaktancyjny strefy 3 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 3 charakterystyki kolowej zwarcie miedzyfazowe + double X4_Zr4_LE; ///< Zasieg reaktancyjny strefy 4 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 4 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X4_Zr4_LL; ///< Zasieg reaktancyjny strefy 4 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 4 charakterystyki kolowej zwarcie miedzyfazowe + double X5_Zr5_LE; ///< Zasieg reaktancyjny strefy 5 dla charakterystyki poligonalnej zwarcie jednofazowe z ziemia + ///< Zasieg „do tylu” strefy 5 charakterystyki kolowej zwarcie jednofazowe z ziemia + double X5_Zr5_LL; ///< Zasieg reaktancyjny strefy 5 dla charakterystyki poligonalnej zwarcie miedzyfazowe + ///< Zasieg „do tylu” strefy 5 charakterystyki kolowej zwarcie miedzyfazowe + double kp; ///< wspolczynnik powrotu +//A2 +// double U_min; ///< Minimalna wartosc napiecia + double fi3; ///< Kat kierunkowy od osi X impedancyjnego kryterium kierunku + double fi4; ///< Kat kierunkowy od osi R impedancyjnego kryterium kierunku + double RLf; ///< Zasieg „ do przodu” kryterium blokady dzialania w zakresie pradow obcia¿enia + double RLr; ///< Zasieg „ do tylu” kryterium blokady dzialania w zakresie pradow obcia¿enia + double fi5; ///< Kat kierunkowy blokady dzialania w zakresie pradow obcia¿enia + double Iomin; ///< Minimalna wartosc rozruchowa pradu zerowego przekaznika identyfikacji zwarc z udzialem ziemi + double Iokh; ///< Wspolczynnik stabilizacji charakterystyki przekaznika identyfikacji zwarc z udzialem ziemi + double Uomin; ///< minimalna wartoœæ napiêcia sk³adowej zerowej + + long Typ0; ///< Typ charakterystyki strefa 1W + long K0; ///< Kierunek dzialania strefa 1W + +}__attribute__((__packed__)); + +struct ZDistA_args +{ + struct ZDistA_io io; + struct ZDistA_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void ZDistA(void *args, void *logic); +extern int ZDistA_initlog(void *arguments, void *logic); + +#endif /* ZDISTA_H_ */ diff --git a/src/ZDistL.c b/src/ZDistL.c new file mode 100644 index 0000000..daea5a1 --- /dev/null +++ b/src/ZDistL.c @@ -0,0 +1,1424 @@ +/* + * ZDistL.c + * + * Created on: 07-03-2017 + * Author: Krzysztof Jakubczyk + */ + + + + +#include + + +#include "tdefs.h" +#include "misc.h" +#include "helper.h" + +#include "ZDistL.h" +#include "ZDistA.h" + + +struct ZDistL_nast +{ + double ts0LE;///< Czas wy³¹czenia strefy 1W zwarcia jednofazowe z ziemi¹ + double ts0LL;///< Czas wy³¹czenia strefy 1W zwarcia miedzyfazowe + double ts1LE;///< Czas wy³¹czenia strefy 1 zwarcia jednofazowe z ziemi¹ + double ts1LL;///< Czas wy³¹czenia strefy 1 zwarcia miedzyfazowe + double ts2LE;///< Czas wy³¹czenia strefy 2 zwarcia jednofazowe z ziemi¹ + double ts2LL;///< Czas wy³¹czenia strefy 2 zwarcia miedzyfazowe + double ts3LE;///< Czas wy³¹czenia strefy 3 zwarcia jednofazowe z ziemi¹ + double ts3LL;///< Czas wy³¹czenia strefy 3 zwarcia miedzyfazowe + double ts4LE;///< Czas wy³¹czenia strefy 4 zwarcia jednofazowe z ziemi¹ + double ts4LL;///< Czas wy³¹czenia strefy 4 zwarcia miedzyfazowe + double ts5LE;///< Czas wy³¹czenia strefy 5 zwarcia jednofazowe z ziemi¹ + double ts5LL;///< Czas wy³¹czenia strefy 5 zwarcia miedzyfazowe + + long Stf0; ///< Sposób dzia³ania strefy 1W + long Stf1; ///< Sposób dzia³ania strefy 1 + long Stf2; ///< Sposób dzia³ania strefy 2 + long Stf3; ///< Sposób dzia³ania strefy 3 + long Stf4; ///< Sposób dzia³ania strefy 4 + long Stf5; ///< Sposób dzia³ania strefy 5 +}; + +int ZDistL_initlog(void *arguments, void *logic) +{ + struct ZDistL_args *args = (struct ZDistL_args *)arguments; + struct ZDistL_logic *log = (struct ZDistL_logic *)logic; + struct ZDistL_nast znast; + + if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl)) + return -1; + if(set_pointer_in_ptr(args->io.we_zdist_ptr_in,(u32 *)&log->WE_Zdist)) + return -1; + if(set_bit_ptr_struct(args->io.Bl_PS_in,&log->Bl_PS)) + return -1; + if(set_bit_ptr_struct(args->io.lacze_OK_in,&log->lacze_OK)) + return -1; + if(set_bit_ptr_struct(args->io.lacze_odb_in,&log->lacze_odb)) + return -1; + if(set_bit_ptr_struct(args->io.W_ON_in,&log->W_ON)) + return -1; + if(set_bit_ptr_struct(args->io.zgoda_1f_in,&log->zgoda_1f)) + return -1; + if(set_bit_ptr_struct(args->io.Z_in,&log->Z)) + return -1; + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + if(set_bit_ptr_struct(args->io.blok_1W_in,&log->blok_1W)) + return -1; + if(set_bit_ptr_struct(args->io.deakt_in,&log->deakt)) + return -1; + + if(set_bit_ptr_struct(args->io.P_out,&log->P)) + return -1; + if(set_bit_ptr_struct(args->io.P1W_out,&log->P1W)) + return -1; + if(set_bit_ptr_struct(args->io.P1_out,&log->P1)) + return -1; + if(set_bit_ptr_struct(args->io.P2_out,&log->P2)) + return -1; + if(set_bit_ptr_struct(args->io.P3_out,&log->P3)) + return -1; + if(set_bit_ptr_struct(args->io.P4_out,&log->P4)) + return -1; + if(set_bit_ptr_struct(args->io.P5_out,&log->P5)) + return -1; + if(set_bit_ptr_struct(args->io.R_out,&log->R)) + return -1; + if(set_bit_ptr_struct(args->io.S_out,&log->S)) + return -1; + if(set_bit_ptr_struct(args->io.T_out,&log->T)) + return -1; + if(set_bit_ptr_struct(args->io.E_out,&log->E)) + return -1; + if(set_bit_ptr_struct(args->io.Z1W_out,&log->Z1W)) + return -1; + if(set_bit_ptr_struct(args->io.Z1_out,&log->Z1)) + return -1; + if(set_bit_ptr_struct(args->io.Z2_out,&log->Z2)) + return -1; + if(set_bit_ptr_struct(args->io.Z3_out,&log->Z3)) + return -1; + if(set_bit_ptr_struct(args->io.Z4_out,&log->Z4)) + return -1; + if(set_bit_ptr_struct(args->io.Z5_out,&log->Z5)) + return -1; + if(set_bit_ptr_struct(args->io.W_out,&log->W)) + return -1; + if(set_bit_ptr_struct(args->io.W1_out,&log->W1)) + return -1; + if(set_bit_ptr_struct(args->io.W2_out,&log->W2)) + return -1; + if(set_bit_ptr_struct(args->io.W3_out,&log->W3)) + return -1; + if(set_bit_ptr_struct(args->io.bl_LRC_out,&log->bl_LRC)) + return -1; + if(set_bit_ptr_struct(args->io.log_odbl_out,&log->log_odbl)) + return -1; + if(set_bit_ptr_struct(args->io.lacze_nad_out,&log->lacze_nad)) + return -1; + if(set_bit_ptr_struct(args->io.zzw_akt_out,&log->zzw_akt)) + return -1; + if(set_bit_ptr_struct(args->io.blk_lacz_out,&log->blk_lacz)) + return -1; + if(set_bit_ptr_struct(args->io.zadz_echa_out,&log->zadz_echa)) + return -1; + if(set_bit_ptr_struct(args->io.zezw_lacz_out,&log->zezw_lacz)) + return -1; + if(set_bit_ptr_struct(args->io.skr_czas_lacz_out,&log->skr_czas_lacz)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_lacz_out,&log->wyl_lacz)) + return -1; + if(set_bit_ptr_struct(args->io.blok_lacz_out,&log->blok_lacz)) + return -1; + if(set_bit_ptr_struct(args->io.wydl_czas_lacz_out,&log->wydl_czas_lacz)) + return -1; + if(set_bit_ptr_struct(args->io.z_zwarcie_out,&log->z_zwarcie)) + return -1; + + log->nast_.echo_on = (args->params.bity & 0x0001)?1:0; + log->nast_.wyl_od_echa = (args->params.bity & 0x0002)?1:0; + log->nast_.log_odbl_on = (args->params.bity & 0x0004)?1:0; + log->nast_.LCR_on = (args->params.bity & 0x0008)?1:0; + log->nast_.Bl_PS[1] = (args->params.bity & 0x0010)?1:0; + log->nast_.Bl_PS[2] = (args->params.bity & 0x0020)?1:0; + log->nast_.Bl_PS[3] = (args->params.bity & 0x0040)?1:0; + log->nast_.Bl_PS[4] = (args->params.bity & 0x0080)?1:0; + log->nast_.Bl_PS[5] = (args->params.bity & 0x0100)?1:0; + log->nast_.ZZw_ON = (args->params.bity & 0x0200)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0400)?1:0; + log->nast_.LRC_s[0] = (args->params.bity & 0x0800)?1:0; + log->nast_.LRC_s[1] = (args->params.bity & 0x1000)?1:0; + log->nast_.LRC_s[2] = (args->params.bity & 0x2000)?1:0; + log->nast_.LRC_s[3] = (args->params.bity & 0x4000)?1:0; + log->nast_.LRC_s[4] = (args->params.bity & 0x8000)?1:0; + log->nast_.LRC_s[5] = (args->params.bity & 0x10000)?1:0; + log->nast_.lacze_on = (args->params.bity & 0x20000)?1:0; + log->nast_.zgoda_1faz_s[0] = (args->params.bity & 0x40000)?1:0; + log->nast_.zgoda_1faz_s[1] = (args->params.bity & 0x80000)?1:0; + log->nast_.zgoda_1faz_s[2] = (args->params.bity & 0x100000)?1:0; + log->nast_.zgoda_1faz_s[3] = (args->params.bity & 0x200000)?1:0; + log->nast_.zgoda_1faz_s[4] = (args->params.bity & 0x400000)?1:0; + log->nast_.zgoda_1faz_s[5] = (args->params.bity & 0x800000)?1:0; + log->nast_.Bl_PS[0] = (args->params.bity & 0x1000000)?1:0; + log->nast_.rozwij = (args->params.bity & 0x2000000)?1:0; + + log->nast_.ZdistA = log->WE_Zdist; + + unsigned short i; + + znast.ts0LE=args->params.ts1LE;///< Czas wy³¹czenia strefy 1W zwarcia jednofazowe z ziemi¹ + znast.ts0LL=args->params.ts1LL;///< Czas wy³¹czenia strefy 1W zwarcia miedzyfazowe + znast.ts1LE=args->params.ts1LE;///< Czas wy³¹czenia strefy 1 zwarcia jednofazowe z ziemi¹ + znast.ts1LL=args->params.ts1LL;///< Czas wy³¹czenia strefy 1 zwarcia miedzyfazowe + znast.ts2LE=args->params.ts2LE;///< Czas wy³¹czenia strefy 2 zwarcia jednofazowe z ziemi¹ + znast.ts2LL=args->params.ts2LL;///< Czas wy³¹czenia strefy 2 zwarcia miedzyfazowe + znast.ts3LE=args->params.ts3LE;///< Czas wy³¹czenia strefy 3 zwarcia jednofazowe z ziemi¹ + znast.ts3LL=args->params.ts3LL;///< Czas wy³¹czenia strefy 3 zwarcia miedzyfazowe + znast.ts4LE=args->params.ts4LE;///< Czas wy³¹czenia strefy 4 zwarcia jednofazowe z ziemi¹ + znast.ts4LL=args->params.ts4LL;///< Czas wy³¹czenia strefy 4 zwarcia miedzyfazowe + znast.ts5LE=args->params.ts5LE;///< Czas wy³¹czenia strefy 5 zwarcia jednofazowe z ziemi¹ + znast.ts5LL=args->params.ts5LL;///< Czas wy³¹czenia strefy 5 zwarcia miedzyfazowe + + znast.Stf0=args->params.Stf0; + znast.Stf1=args->params.Stf1; + znast.Stf2=args->params.Stf2; + znast.Stf3=args->params.Stf3; + znast.Stf4=args->params.Stf4; + znast.Stf5=args->params.Stf5; + + log->nast_.tryb_lacza = (enum _tryb_lacza)args->params.tryb_lacza; + log->nast_.tryb_zezw = (enum _tryb_zezwalajacy)args->params.tryb_zezw; + log->nast_.tryb_blok = (enum _tryb_blokujacy)args->params.tryb_blokujacy; + + log->nast_.ktora_strefa = args->params.strefa_dzial; + log->nast_.ktora_strefa_nad = args->params.strefa_nad; + log->nast_.strefa_pradu_wstecznego = args->params.lrc_strefa; + + log->dw.blok_lacza[0] = 0; + log->dw.blok_lacza[1] = 0; + log->dw.blok_lacza[2] = 0; + log->dw.blok_lacza[3] = 0; + log->dw.blok_lacza[4] = 0; + log->dw.blok_lacza[5] = 0; + + log->dw.wydl_czas = 0; + log->dw.wylacz_od_lacza = 0; + log->dw.wylacz_od_echa = 0; + log->dw.wylacz_od_lacza_suma = 0; + + log->dw.wy_bl_LRC = 0; + log->dw.wy_log_odbl = 0; + log->dw.wy_lacze_nad = 0; + + log->dw.wy_blk_lacz = 0; + + log->dw.wy_zadz_echa = 0; + log->dw.wy_zezw_lacz = 0; + log->dw.wy_skr_czas_lacz = 0; + log->dw.wy_wyl_lacz = 0; + log->dw.wy_blok_lacz = 0; + log->dw.wy_wydl_czas_lacz = 0; + + log->dw.dodaj_rozw[0] = 0; + log->dw.dodaj_rozw[1] = 0; + log->dw.dodaj_rozw[2] = 0; + log->dw.dodaj_rozw[3] = 0; + log->dw.dodaj_rozw[4] = 0; + log->dw.dodaj_rozw[5] = 0; + + log->dw.ZZwP = 0; + + if (log->nast_.tryb_lacza == ZEZWALAJACY && log->nast_.tryb_zezw == ZGODA) + { + log->dw.blok_lacza[log->nast_.ktora_strefa] = 1; + } + + if (args->params.ts0LE < 0.01) + { + log->nast_.Zbz[0][0] = 1; + log->nast_.t_stf[0][0] = 0; + + } + else + { + log->nast_.Zbz[0][0] = 0; + log->nast_.t_stf[0][0] = (int16_t)(100 * (args->params.ts0LE - 0.01f)); + + if (log->nast_.t_stf[0][0] < 0) log->nast_.t_stf[0][0] = 0; + } + + if (args->params.ts0LL < 0.01) + { + log->nast_.Zbz[0][1] = 1; + log->nast_.t_stf[0][1] = 0; + } + else + { + log->nast_.Zbz[0][1] = 0; + log->nast_.t_stf[0][1] = (int16_t)(100 * (args->params.ts0LL - 0.01f)); + + if (log->nast_.t_stf[0][1] < 0) log->nast_.t_stf[0][1] = 0; + } + + + if (args->params.ts1LE < 0.01) + { + log->nast_.Zbz[1][0] = 1; + log->nast_.t_stf[1][0] = 0; + } + else + { + log->nast_.Zbz[1][0] = 0; + log->nast_.t_stf[1][0] = (int16_t)(100 * (args->params.ts1LE - 0.01f)); + + if (log->nast_.t_stf[1][0] < 0) log->nast_.t_stf[1][0] = 0; + } + + if (args->params.ts1LL < 0.01) + { + log->nast_.Zbz[1][1] = 1; + log->nast_.t_stf[1][1] = 0; + } + else + { + log->nast_.Zbz[1][1] = 0; + log->nast_.t_stf[1][1] = (int16_t)(100 * (args->params.ts1LL - 0.01f)); + + if (log->nast_.t_stf[1][1] < 0) log->nast_.t_stf[1][1] = 0; + } + + + if (args->params.ts2LE < 0.01) + { + log->nast_.Zbz[2][0] = 1; + log->nast_.t_stf[2][0] = 0; + } + else + { + log->nast_.Zbz[2][0] = 0; + log->nast_.t_stf[2][0] = (int16_t)(100 * (args->params.ts2LE - 0.01f)); + + if (log->nast_.t_stf[2][0] < 0) log->nast_.t_stf[2][0] = 0; + } + + if (args->params.ts2LL < 0.01) + { + log->nast_.Zbz[2][1] = 1; + log->nast_.t_stf[2][1] = 0; + } + else + { + log->nast_.Zbz[2][1] = 0; + log->nast_.t_stf[2][1] = (int16_t)(100 * (args->params.ts2LL - 0.01f)); + + if (log->nast_.t_stf[2][1] < 0) log->nast_.t_stf[2][1] = 0; + } + + if (args->params.ts3LE < 0.01) + { + log->nast_.Zbz[3][0] = 1; + log->nast_.t_stf[3][0] = 0; + } + else + { + log->nast_.Zbz[3][0] = 0; + log->nast_.t_stf[3][0] = (int16_t)(100 * (args->params.ts3LE - 0.01f)); + + if (log->nast_.t_stf[3][0] < 0) log->nast_.t_stf[3][0] = 0; + } + + if (args->params.ts3LL < 0.01) + { + log->nast_.Zbz[3][1] = 1; + log->nast_.t_stf[3][1] = 0; + } + else + { + log->nast_.Zbz[3][1] = 0; + log->nast_.t_stf[3][1] = (int16_t)(100 * (args->params.ts3LL - 0.01f)); + + if (log->nast_.t_stf[3][1] < 0) log->nast_.t_stf[3][1] = 0; + } + + if (args->params.ts4LE < 0.01) + { + log->nast_.Zbz[4][0] = 1; + log->nast_.t_stf[4][0] = 0; + } + else + { + log->nast_.Zbz[4][0] = 0; + log->nast_.t_stf[4][0] = (int16_t)(100 * (args->params.ts4LE - 0.01f)); + + if (log->nast_.t_stf[4][0] < 0) log->nast_.t_stf[4][0] = 0; + } + + if (args->params.ts4LL < 0.01) + { + log->nast_.Zbz[4][1] = 1; + log->nast_.t_stf[4][1] = 0; + } + else + { + log->nast_.Zbz[4][1] = 0; + log->nast_.t_stf[4][1] = (int16_t)(100 * (args->params.ts4LL - 0.01f)); + + if (log->nast_.t_stf[4][1] < 0) log->nast_.t_stf[4][1] = 0; + } + + + if (args->params.ts5LE < 0.01) + { + log->nast_.Zbz[5][0] = 1; + log->nast_.t_stf[5][0] = 0; + } + else + { + log->nast_.Zbz[5][0] = 0; + log->nast_.t_stf[5][0] = (int16_t)(100 * (args->params.ts5LE - 0.01f)); + + if (log->nast_.t_stf[5][0] < 0) log->nast_.t_stf[5][0] = 0; + } + + if (args->params.ts5LL < 0.01) + { + log->nast_.Zbz[5][1] = 1; + log->nast_.t_stf[5][1] = 0; + } + else + { + log->nast_.Zbz[5][1] = 0; + log->nast_.t_stf[5][1] = (int16_t)(100 * (args->params.ts5LL - 0.01f)); + + if (log->nast_.t_stf[5][1] < 0) log->nast_.t_stf[5][1] = 0; + } + + + log->nast_.ZZw_st = args->params.ZZw_st; + + switch(log->nast_.ZZw_st) + { + case 0: + log->PZZw=log->P1W; + log->ZZZw=log->Z1W; + break; + case 1: + log->PZZw=log->P1; + log->ZZZw=log->Z1; + break; + case 2: + log->PZZw=log->P2; + log->ZZZw=log->Z2; + break; + case 3: + log->PZZw=log->P3; + log->ZZZw=log->Z3; + break; + case 4: + log->PZZw=log->P4; + log->ZZZw=log->Z4; + break; + case 5: + log->PZZw=log->P5; + log->ZZZw=log->Z5; + break; + default: + log->PZZw=log->P1W; + log->ZZZw=log->Z1W; + break; + } + + long *temp_e = &znast.Stf0; + + for (i=0;i<6;i++) + { + + //ustawienie bitow sposobu dzialania stref + switch (*(temp_e)) + { + case 0: + log->nast_.Stf_ON[i] = 0; + log->nast_.Stf_W[i] = 0; + break; + case 1: + log->nast_.Stf_ON[i] = 1; + log->nast_.Stf_W[i] = 0; + break; + case 2: + log->nast_.Stf_ON[i] = 1; + log->nast_.Stf_W[i] = 1; + break; + default: + break; + } + temp_e++; + + //-------------------------- + } + + log->nast_.ZZw_ta = (short)(100 * (args->params.ZZw_ta + 0.005f)); + log->nast_.ZZw_to = (short)(100 * (args->params.ZZw_to + 0.005f)); + + log->nast_.ZZw_Ir = args->params.ZZw_Ir * args->params.ZZw_Ir; + log->nast_.ZZw_Ur = args->params.ZZw_Ur * args->params.ZZw_Ur; + + log->nast_.Uecho_pob = args->params.U_echa * args->params.U_echa; + log->nast_.Uecho_odp = 1.02 * 1.02 * log->nast_.Uecho_pob; + + log->nast_.t_LRC = (int16_t)(100 * (args->params.t_LRC + 0.005f)); + log->nast_.t_lacze_nad = (int16_t)(100 * (args->params.t_lacze_nad + 0.005f)); + log->nast_.t_pod_odbl = (int16_t)(100 * (args->params.t_pod_odbl + 0.005f)); + + log->nast_.ts_plus = (int16_t)(100 * (args->params.ts_plus - 0.01f)); + if (log->nast_.ts_plus < 0) log->nast_.ts_plus = 0; + + log->nast_.czas_wyl_echa = (int16_t)(100 * (args->params.czas_wyl_echa + 0.005f));; + log->nast_.max_t_nadawania = (int16_t)(100 * (args->params.max_t_nadawania + 0.005f)); + + for (i = 0; i < 6; i++) + { + log->dw.Pf[i][0] = 0; + log->dw.Pf[i][1] = 0; + log->dw.Zf[i][0] = 0; + log->dw.Zf[i][1] = 0; + + log->dw.Pzf[i] = 0; + log->dw.Z[i] = 0; + log->dw.Ws[i] = 0; + + log->dw.blok_LRC[i] = 0; + + log->dw.liczt_eldz[i][0] = 0; + log->dw.liczt_eldz[i][1] = 0; + + log->dw.liczts_rozwij[i] = 0; + log->dw.Zf_rozwij[i] = 0; + log->dw.liczt_eldz_rozwij[i] = 0; + //liczts_rozwij + + } + log->dw.lacze_odb_old = 0; + log->dw.skrocil_strefa = 0; + log->dw.skrocil_echo = 0; + + for (i = 0; i < 6; i++) + { + log->P_L1_lub_L2[i] = 0; + log->P_L2_lub_L3[i] = 0; + log->P_L3_lub_L1[i] = 0; + } + + + + + return 0; +} + +/** +Funkcja okraœlania petli zwarciwej przy wy³aczeniu +\param nr_strefy - numer strefy wy³aczaj¹cej +*/ +void wybiornik_f(struct ZDistL_logic *log,unsigned short nr_strefy) +{ + log->dw.R = log->pobudzenia[nr_strefy][0] + || log->pobudzenia[nr_strefy][3] + || log->pobudzenia[nr_strefy][5]; + + log->dw.S = log->pobudzenia[nr_strefy][1] + || log->pobudzenia[nr_strefy][3] + || log->pobudzenia[nr_strefy][4]; + + log->dw.T = log->pobudzenia[nr_strefy][2] + || log->pobudzenia[nr_strefy][4] + || log->pobudzenia[nr_strefy][5]; + + log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr; + + log->dw.jedfaz = (log->dw.R && !(log->dw.S||log->dw.T)) + || (log->dw.S && !(log->dw.T||log->dw.R)) + || (log->dw.T && !(log->dw.R||log->dw.S)) + || (!log->dw.R && !log->dw.S && !log->dw.T); //zeby po wyl 1 fazowym nie robil na krotko 3 faz +} + +/** +Funkcja okraœlania petli zwarciwej przy wy³aczeniu - dzia³anie bezkierunkowe +\param nr_strefy - numer strefy wy³aczaj¹cej +*/ +void wybiornik_fbk(struct ZDistL_logic *log, unsigned short nr_strefy) +{ + log->dw.Rbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][0] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5]; + + log->dw.Sbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][1] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4]; + + log->dw.Tbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][2] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5]; + + log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr; +} + + +void +wylacz_P(struct ZDistL_logic *log, u8 zgoda_1f) +{ +int i; + + for(i=0;i<6;i++) + { + if (log->dw.Ws[i]) + { + wybiornik_f(log,i); + + if(log->dw.jedfaz && zgoda_1f && log->nast_.zgoda_1faz_s[i]) + { + if(log->dw.R) + log->dw.W1 = 1; + if(log->dw.S) + log->dw.W2 = 1; + if(log->dw.T) + log->dw.W3 = 1; + } + else + { + log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.W = 1; + } + } + } + + if (log->dw.wylacz_od_lacza_suma == 1) + { + log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.W = 1; + } +} + + +void ZDistL(void *arguments, void *logic) +{ + struct ZDistL_logic *log = (struct ZDistL_logic *)logic; + + u16 i,j; + log->dw.P = 0; + log->dw.Zz = 0; + + u8 temp_Bl,temp_Zezwolenie, pob_zab_echo; + + u8 we_stan_bl = check_struct(&log->stan_bl)?1:0; + u8 we_Bl_PS = check_struct(&log->Bl_PS)?1:0; + u8 we_lacze_OK = check_struct(&log->lacze_OK)?1:0; + u8 we_lacze_odb = check_struct(&log->lacze_odb)?1:0; + u8 we_zgoda_1f = check_struct(&log->zgoda_1f)?1:0; + u8 we_test = check_struct(&log->test)?1:0; + u8 we_Z = check_struct(&log->Z)?1:0; + u8 b_1W = check_struct(&log->blok_1W)?1:0; + + if (*log->nast_.ZdistA->on && !(check_struct(&log->deakt))) + { + + //**************\/********* Poprawka na bledne wylaczenie 3 fazowe przy zwarciu 1 fazowym ************\/*************************** + + #define L1 0 + #define L2 1 + #define L3 2 + #define L1L2 3 + #define L2L3 4 + #define L3L1 5 + #define faz_0_85 0.24f + #define st_075kw 0.5625f + #define st_133kw 1.7689f + u8 UL1, UL2, UL3; + u8 IL1L2, IL2L3, IL3L1; + + + //kryteria podanpiêciowe + if (log->nast_.ZdistA->Zdist_dw->U1 < faz_0_85) + UL1 = 1; + else + UL1 = 0; + + if (log->nast_.ZdistA->Zdist_dw->U2 < faz_0_85) + UL2 = 1; + else + UL2 = 0; + + if (log->nast_.ZdistA->Zdist_dw->U3 < faz_0_85) + UL3 = 1; + else + UL3 = 0; + + //kryteria pr¹dowe + if (log->nast_.ZdistA->Zdist_dw->I1 > 0.1) //zeby nie dzielic przez 0 + { + float k = log->nast_.ZdistA->Zdist_dw->I2/ log->nast_.ZdistA->Zdist_dw->I1; + if ((k > st_075kw) && (k < st_133kw)) + IL1L2 = 1; + else + IL1L2 = 0; + } + else + IL1L2 = 0; + + if (log->nast_.ZdistA->Zdist_dw->I2 > 0.1) //zeby nie dzielic przez 0 + { + float k = log->nast_.ZdistA->Zdist_dw->I3/ log->nast_.ZdistA->Zdist_dw->I2; + if ((k > st_075kw) && (k < st_133kw)) + IL2L3 = 1; + else + IL2L3 = 0; + } + else + IL2L3 = 0; + + if (log->nast_.ZdistA->Zdist_dw->I3 > 0.1) //zeby nie dzielic przez 0 + { + float k = log->nast_.ZdistA->Zdist_dw->I1/ log->nast_.ZdistA->Zdist_dw->I3; + if ((k > st_075kw) && (k < st_133kw)) + IL3L1 = 1; + else + IL3L1 = 0; + } + else + IL3L1 = 0; + + + //kryteria pobudzenia doziemnego jednofazowego w opoznieniem odpadu + for (i=0; i<6; i++) + { + + if((log->nast_.ZdistA->Zdist_dw->P[i][L1]) || (log->nast_.ZdistA->Zdist_dw->P[i][L2])) + { + log->P_L1_lub_L2[i] = 3; + } + else + { + if (log->P_L1_lub_L2[i] > 0) + log->P_L1_lub_L2[i]--; + + } + + if ((log->nast_.ZdistA->Zdist_dw->P[i][L2]) || (log->nast_.ZdistA->Zdist_dw->P[i][L3])) + { + log->P_L2_lub_L3[i] = 3; + } + else + { + if (log->P_L2_lub_L3[i] > 0) + log->P_L2_lub_L3[i]--; + } + + if ((log->nast_.ZdistA->Zdist_dw->P[i][L3]) || (log->nast_.ZdistA->Zdist_dw->P[i][L1])) + { + log->P_L3_lub_L1[i] = 3; + } + else + { + if (log->P_L3_lub_L1[i] > 0) + log->P_L3_lub_L1[i]--; + } + + for (j=0; j<3; j++) + { + log->pobudzenia[i][j] = log->nast_.ZdistA->Zdist_dw->P[i][j]; + } + + //blokowanie pobudzen miedzyfazowych gdy strefa moze wylaczyc 1-fazowo a napiecia nie przysiadly + if ((we_zgoda_1f && log->nast_.zgoda_1faz_s[i]) && (log->nast_.ZdistA->Zdist_dw->P[i][L1L2]) && (log->P_L1_lub_L2[i] > 0)) + { + if ((UL1 && UL2) || (IL1L2)) + log->pobudzenia[i][L1L2] = 1; + else + log->pobudzenia[i][L1L2] = 0; + } + else + log->pobudzenia[i][L1L2] = log->nast_.ZdistA->Zdist_dw->P[i][L1L2]; + + + if ((we_zgoda_1f && log->nast_.zgoda_1faz_s[i]) && (log->nast_.ZdistA->Zdist_dw->P[i][L2L3]) && (log->P_L2_lub_L3[i] > 0)) + { + if ((UL2 && UL3) || (IL2L3)) + log->pobudzenia[i][L2L3] = 1; + else + log->pobudzenia[i][L2L3] = 0; + } + else + log->pobudzenia[i][L2L3] = log->nast_.ZdistA->Zdist_dw->P[i][L2L3]; + + + if ((we_zgoda_1f && log->nast_.zgoda_1faz_s[i]) && (log->nast_.ZdistA->Zdist_dw->P[i][L3L1]) && (log->P_L3_lub_L1[i] > 0)) + { + if ((UL3 && UL1) || (IL3L1)) + log->pobudzenia[i][L3L1] = 1; + else + log->pobudzenia[i][L3L1] = 0; + } + else + log->pobudzenia[i][L3L1] = log->nast_.ZdistA->Zdist_dw->P[i][L3L1]; + + } + //*************/\******** Poprawka na bledne wylaczenie 3 fazowe przy zwarciu 1 fazowym ***************/\*********************** + + + //Lacze + + if ((log->nast_.tryb_lacza == BLOKUJACY) && (log->nast_.tryb_blok == BLOKUJ_PRZEKAZNIK) && (we_lacze_odb == 1) && (we_lacze_OK)) + { + log->dw.wy_blk_lacz = 1; + } + else + { + log->dw.wy_blk_lacz = 0; + } + + + if (log->nast_.tryb_lacza == ZEZWALAJACY) + { + + if (log->nast_.tryb_zezw == ZGODA ) + { + if (((we_lacze_odb == 1)&& (we_lacze_OK)) || (log->dw.logika_dblokowania)) + { + log->dw.blok_lacza[log->nast_.ktora_strefa] = 0; + log->dw.wy_zezw_lacz = 1; + } + else + { + log->dw.blok_lacza[log->nast_.ktora_strefa] = 1; + log->dw.wy_zezw_lacz = 0; + } + } + else if (log->nast_.tryb_zezw == SKROC_CZAS) + { + if ((we_lacze_odb == 1)&& (we_lacze_OK)) + { + log->dw.liczts[log->nast_.ktora_strefa][0] = 0; + log->dw.liczts[log->nast_.ktora_strefa][1] = 0; + log->dw.wy_skr_czas_lacz = 1; + } + else + { + log->dw.wy_skr_czas_lacz = 0; + } + } + else if (log->nast_.tryb_zezw == WYLACZ) + { + if ((we_lacze_odb == 1)&& (we_lacze_OK) && !we_stan_bl) + { + log->dw.wylacz_od_lacza = 1; + log->dw.wy_wyl_lacz = 1; + } + else + { + log->dw.wylacz_od_lacza = 0; + log->dw.wy_wyl_lacz = 0; + } + + } + } + else if (log->nast_.tryb_lacza == BLOKUJACY)//tryb blokuj¹cy + { + if (log->nast_.tryb_blok == BLOKUJ) + { + if ((we_lacze_odb == 1)&& (we_lacze_OK)) + { + log->dw.blok_lacza[log->nast_.ktora_strefa] = 1; + log->dw.wy_blok_lacz = 1; + } + else + { + log-> dw.blok_lacza[log->nast_.ktora_strefa] = 0; + log->dw.wy_blok_lacz = 0; + } + } + else if (log->nast_.tryb_blok == WYDLUZ_CZAS) + { + if ((we_lacze_odb == 1)&& (we_lacze_OK)) + { + log->dw.wy_wydl_czas_lacz = 1; + } + else + { + log->dw.wy_wydl_czas_lacz = 0; + } + + //funkcja wykonywana jest w kodzie ponizej + } + else if (log->nast_.tryb_blok == BLOKUJ_PRZEKAZNIK) + { + + } + } + else //tryb wylaczony + { + log->dw.blok_lacza[0] = 0; + log->dw.blok_lacza[1] = 0; + log->dw.blok_lacza[2] = 0; + log->dw.blok_lacza[3] = 0; + log->dw.blok_lacza[4] = 0; + log->dw.blok_lacza[5] = 0; + } + //------------------------------------------------------------------------- + + //logika pradu wstecznego + + if ((log->dw.Pzf[log->nast_.strefa_pradu_wstecznego] == 1) && (log->nast_.LCR_on)) + { + log->dw.blokada_pradu_wstecznego = 1; + log->dw.t_pod_blok_wst = log->nast_.t_LRC; + log->dw.wy_bl_LRC = 1; + } + + if (log->dw.blokada_pradu_wstecznego == 1) + { + for (i = 0; i < 6; i++) + { + if (log->nast_.LRC_s[i] == 1) + log->dw.blok_LRC[i] = 1; + } + } + else + { + for (i = 0; i < 6; i++) + log->dw.blok_LRC[i] = 0; + } + + //------------------------------------------------------------------------- + //logika s³abego zasialnia - echa + + pob_zab_echo = 0; + + u8 U_echo_pob_b = log->nast_.ZdistA->Zdist_dw->U1 < log->nast_.Uecho_pob || + log->nast_.ZdistA->Zdist_dw->U2 nast_.Uecho_pob || + log->nast_.ZdistA->Zdist_dw->U3 nast_.Uecho_pob; + + u8 U_echo_odp_b = log->nast_.ZdistA->Zdist_dw->U1 > log->nast_.Uecho_odp && + log->nast_.ZdistA->Zdist_dw->U2 > log->nast_.Uecho_odp && + log->nast_.ZdistA->Zdist_dw->U3 > log->nast_.Uecho_odp; + + sprawdz_P(&log->dw.pob_echo, U_echo_pob_b, U_echo_odp_b, &log->dw.licznik_Uecho, 3, 3); + + for (i=0;i<6;i++) + { + if (log->dw.Pzf[i] != 0) + pob_zab_echo = 1; + } + + if (log->nast_.echo_on && !we_stan_bl) + { + if (!pob_zab_echo && log->dw.pob_echo && (we_lacze_odb == 1) && (we_lacze_OK == 1)) + { + if ((log->nast_.wyl_od_echa) && (log->dw.wylacz_od_echa == 0)) //poprawka hazardu + { + log->dw.wylacz_od_echa = 1; + log->dw.t_wyl_od_echa = log->nast_.czas_wyl_echa; + } + // i odbij echo + if (log->dw.skrocil_echo == 0) + { + log->dw.lacze_nad = 1; + log->dw.t_lacze_nad = log->nast_.t_lacze_nad; + log->dw.wy_lacze_nad = 1; + } + } + + if (log->dw.wylacz_od_echa) + { + log->dw.wy_zadz_echa = 1; + } + else + { + log->dw.wy_zadz_echa = 0; + } + + if (we_lacze_odb == 0) + log->dw.skrocil_echo = 0; + + } + + log->dw.wylacz_od_lacza_suma = log->dw.wylacz_od_lacza || log->dw.wylacz_od_echa; + //------------------------------------------------------------------------- + //logika odblokowania + + if ((log->nast_.log_odbl_on) && (log->nast_.tryb_lacza == ZEZWALAJACY) && (log->nast_.tryb_zezw == ZGODA)) + { + if ((log->dw.kopia_lacze_ok == 1) && (we_lacze_OK == 0)) //urwanie lacza ok + { + log->dw.wy_log_odbl = 1; + log->dw.logika_dblokowania = 1; + log->dw.t_log_odblokowania = log->nast_.t_pod_odbl; + } + } + + log->dw.kopia_lacze_ok = we_lacze_OK; + + + temp_Bl = (we_stan_bl & log->nast_.bl_); + + for (i=0;i<6;i++) + { + if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona + { + temp_Zezwolenie = !(temp_Bl | (log->nast_.Bl_PS[i] & we_Bl_PS)); + + if ((i == 0) && (b_1W == 1)) + temp_Zezwolenie = 0; + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.Pf[i][0] = log->dw.Pf[i][1] = temp_Zezwolenie & we_test; + } + else + { + + //ustawienie pobudzen + log->dw.Pf[i][0] = (log->pobudzenia[i][0] | + log->pobudzenia[i][1] | + log->pobudzenia[i][2]) + & temp_Zezwolenie; + log->dw.Pf[i][1] = (log->pobudzenia[i][3] | + log->pobudzenia[i][4] | + log->pobudzenia[i][5]) + & temp_Zezwolenie; + } + log->dw.Pzf[i] = log->dw.Pf[i][0] | log->dw.Pf[i][1]; + + //ustawienie zadzialan jesli bezzwloczny + + + + if ((log->nast_.tryb_lacza == BLOKUJACY) && (log->nast_.tryb_blok == WYDLUZ_CZAS) && (log->nast_.ktora_strefa == i)) + { + if ((log->dw.Pzf[log->nast_.ktora_strefa] == 1) && (log->dw.wydl_czas == 0) && ( we_lacze_odb == 1) && (we_lacze_OK)) + { + log->dw.wydl_czas = 1; + log->dw.liczts[log->nast_.ktora_strefa][0] += log->nast_.ts_plus; + log->dw.liczts[log->nast_.ktora_strefa][1] += log->nast_.ts_plus; + log->nast_.Zbz[log->nast_.ktora_strefa][0] = 0; + log->nast_.Zbz[log->nast_.ktora_strefa][1] = 0; + log->dw.dodaj_rozw[log->nast_.ktora_strefa] = log->nast_.ts_plus; + } + + if (log->dw.Pzf[log->nast_.ktora_strefa] == 0) + { + log->dw.wydl_czas = 0; + + if (log->nast_.t_stf[log->nast_.ktora_strefa][0] < 0.01) + log->nast_.Zbz[log->nast_.ktora_strefa][0] = 1; + + if (log->nast_.t_stf[log->nast_.ktora_strefa][1] < 0.01) + log->nast_.Zbz[log->nast_.ktora_strefa][1] = 1; + } + } + + + if (log->nast_.Zbz[i][0]) + { + if (log->dw.Pf[i][0] != 0) + log->dw.Zf[i][0] = 1; + + + } + if (log->nast_.Zbz[i][1]) + { + if (log->dw.Pf[i][1] != 0) + log->dw.Zf[i][1] = 1; + } + + + + //ustawienie zadzialania strefy + log->dw.Z[i] = log->dw.Zf[i][0] | log->dw.Zf[i][1]; + + + } else { + log->dw.Pzf[i] = 0; + log->dw.Z[i] = 0; + } + + log->dw.P |= log->dw.Pzf[i]; + log->dw.Zz |= log->dw.Z[i]; //zadzialanie zabezpieczenia + } + + //lacze nadawanie + if (log->nast_.lacze_on == 0) + { + log->dw.wy_lacze_nad = 0; + } + else + { + if (log->dw.skrocil_strefa == 0) + { + if ((log->dw.Pzf[log->nast_.ktora_strefa_nad] == 1) && !log->dw.blokada_pradu_wstecznego) + { + log->dw.lacze_nad = 1; + log->dw.t_lacze_nad = log->nast_.t_lacze_nad; + log->dw.wy_lacze_nad = 1; + } + } + + if (log->dw.Pzf[log->nast_.ktora_strefa_nad] == 0) + { + log->dw.skrocil_strefa = 0; //kasowanie flagi skrocil czas gdy zanikl odpad + } + } + + //------------------------------------------------------------------------- + + log->dw.lacze_odb_old = we_lacze_odb && we_lacze_OK; //jesli jest jedae ale nie ok to zero + + //------------------------------------------------------------------------------------------------------- + //automatyka za³aczenia na zwarcie + + if (we_Z && log->nast_.ZZw_ON) { + log->dw.ZZw |= 1; + } + if (log->dw.ZZw && log->nast_.ZZw_ON) //jeseli jest w³aczony i jest spe³nione kryterium za³¹czenia na zwarcie + { + wybiornik_fbk(log, log->nast_.ZZw_st); + + log->dw.bl_WSPZ = 1; + + if (log->dw.Rbk | log->dw.Sbk | log->dw.Tbk) + { + if (!temp_Bl) + { + log->dw.W |= 1; + log->dw.ZZwP = 1; + } + } else log->dw.ZZwP = 0; + log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.W; + + } else { + log->dw.Rbk = log->dw.Sbk = log->dw.Tbk = 0; + log->dw.ZZwP = 0; + } + + //------------------------------------------------------------------------------------------------------- + + for(i=0;i<6;i++) + { + log->dw.Ws[i] = log->dw.Z[i] && log->nast_.Stf_W[i] && !log->dw.blok_lacza[i] && !log->dw.blok_LRC[i]; + } + + wylacz_P(log, we_zgoda_1f); + + //------------------------------------------------------------------------------------------------------- + + + if (log->dw.ZZw && log->nast_.ZZw_ON) + log->nast_.ZdistA->SOTF_zwrotnie = log->nast_.ZZw_st; + else + log->nast_.ZdistA->SOTF_zwrotnie = 0xFF; + + + //wyprowadzenie wyjœæ + check_and_set_struct(log->dw.P,&log->P); + check_and_set_struct(log->dw.Pzf[0],&log->P1W); + check_and_set_struct(log->dw.Pzf[1],&log->P1); + check_and_set_struct(log->dw.Pzf[2],&log->P2); + check_and_set_struct(log->dw.Pzf[3],&log->P3); + check_and_set_struct(log->dw.Pzf[4],&log->P4); + check_and_set_struct(log->dw.Pzf[5],&log->P5); + + check_and_set_struct(log->dw.Z[0],&log->Z1W); + check_and_set_struct(log->dw.Z[1],&log->Z1); + check_and_set_struct(log->dw.Z[2],&log->Z2); + check_and_set_struct(log->dw.Z[3],&log->Z3); + check_and_set_struct(log->dw.Z[4],&log->Z4); + check_and_set_struct(log->dw.Z[5],&log->Z5); + + check_and_set_struct(log->dw.R || log->dw.Rbk,&log->R); + check_and_set_struct(log->dw.S || log->dw.Sbk,&log->S); + check_and_set_struct(log->dw.T || log->dw.Tbk,&log->T); + check_and_set_struct(log->dw.E,&log->E); + + check_and_set_struct(log->dw.W,&log->W); + check_and_set_struct(log->dw.W1,&log->W1); + check_and_set_struct(log->dw.W2,&log->W2); + check_and_set_struct(log->dw.W3,&log->W3); + + if(log->dw.ZZwP) + { + set_struct(&log->ZZZw); + set_struct(&log->PZZw); + } + + check_and_set_struct(log->dw.wy_bl_LRC, &log->bl_LRC); + check_and_set_struct(log->dw.wy_log_odbl,&log->log_odbl); + check_and_set_struct(log->dw.wy_lacze_nad,&log->lacze_nad); + check_and_set_struct(log->dw.ZZw,&log->zzw_akt); + check_and_set_struct(log->dw.wy_blk_lacz,&log->blk_lacz); + + check_and_set_struct(log->dw.wy_zadz_echa,&log->zadz_echa); + check_and_set_struct(log->dw.wy_zezw_lacz,&log->zezw_lacz); + check_and_set_struct(log->dw.wy_skr_czas_lacz,&log->skr_czas_lacz); + check_and_set_struct(log->dw.wy_wyl_lacz,&log->wyl_lacz); + check_and_set_struct(log->dw.wy_blok_lacz,&log->blok_lacz); + check_and_set_struct(log->dw.wy_wydl_czas_lacz,&log->wydl_czas_lacz); + check_and_set_struct(log->dw.ZZwP,&log->z_zwarcie); + } + else + { + clear_struct(&log->P); + clear_struct(&log->P1W); + clear_struct(&log->P1); + clear_struct(&log->P2); + clear_struct(&log->P3); + clear_struct(&log->P4); + clear_struct(&log->P5); + clear_struct(&log->Z1W); + clear_struct(&log->Z1); + clear_struct(&log->Z2); + clear_struct(&log->Z3); + clear_struct(&log->Z4); + clear_struct(&log->Z5); + clear_struct(&log->R); + clear_struct(&log->S); + clear_struct(&log->T); + clear_struct(&log->E); + clear_struct(&log->W); + clear_struct(&log->W1); + clear_struct(&log->W2); + clear_struct(&log->W3); + + clear_struct(&log->PZZw); + clear_struct(&log->ZZZw); + clear_struct(&log->bl_LRC); + clear_struct(&log->log_odbl); + clear_struct(&log->lacze_nad); + clear_struct(&log->blk_lacz); + + clear_struct(&log->zadz_echa); + clear_struct(&log->zezw_lacz); + clear_struct(&log->skr_czas_lacz); + clear_struct(&log->wyl_lacz); + clear_struct(&log->blok_lacz); + clear_struct(&log->wydl_czas_lacz); + clear_struct(&log->z_zwarcie); + } + +} + +void ZDistL_100hz(void *arguments, void *logic) +{ +struct ZDistL_logic *log = (struct ZDistL_logic *)logic; + +u16 i; +u8 tempbool = 0; + +u8 we_W_ON = check_struct(&log->W_ON)?1:0; +u8 we_Z = check_struct(&log->Z)?1:0; + + if (*log->nast_.ZdistA->on) + { + //lacze nad + if (log->dw.lacze_nad == 1) + { + if (--log->dw.t_lacze_nad == 0) + { + log->dw.wy_lacze_nad = 0; + log->dw.lacze_nad = 0; + } + } + //logika pradu wstecznego + if (log->dw.blokada_pradu_wstecznego == 1) + { + if (--log->dw.t_pod_blok_wst == 0) + { + log->dw.wy_bl_LRC = 0; + log->dw.blokada_pradu_wstecznego = 0; + } + } + + //logika odblokowania + if (log->dw.logika_dblokowania == 1) + { + if (--log->dw.t_log_odblokowania == 0) + { + log->dw.logika_dblokowania = 0; + log->dw.wy_log_odbl = 0; + } + } + + //echo + if (log->dw.wylacz_od_echa == 1) + { + if (--log->dw.t_wyl_od_echa == 0) + log->dw.wylacz_od_echa = 0; + } + + //urwanie imp nadawania + if (log->dw.lacze_nad == 1) + { + if (++log->dw.trwanie_nad > log->nast_.max_t_nadawania) + { + log->dw.wy_lacze_nad = 0; + log->dw.lacze_nad = 0; + log->dw.skrocil_strefa = 1; + log->dw.skrocil_echo = 1; + } + } + else + { + log->dw.trwanie_nad = 0; + } + + //wczytanie danych o stanie wy³¹cznika + log->dw.W_ON = we_W_ON; + + if (we_W_ON) + log->dw.W_OFF = 2; + else + if (log->dw.W_OFF > 0) + log->dw.W_OFF--; + + for (i=0;i<6;i++) + { + + //realizacja przekaznikow czasowych + if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona + { + + if (log->dw.Pf[i][0]) + { + if(!log->nast_.Zbz[i][0]) // jesli nie bezzwloczny + { + if (!log->dw.liczts[i][0]) + { + log->dw.Zf[i][0] = 1; + log->dw.liczts[i][0]++; + } + log->dw.liczts[i][0]--; + } + log->dw.liczt_eldz[i][0] = 2; + } + else//wylaczenie zadzialania (odpad) + { + if (log->dw.liczt_eldz[i][0] > 0) + log->dw.liczt_eldz[i][0]--; + else + { + log->dw.liczts[i][0] = log->nast_.t_stf[i][0]; + log->dw.Zf[i][0] = 0; + } + } + if (log->dw.Pf[i][1]) + { + if(!log->nast_.Zbz[i][1]) // jesli nie bezzwloczny + { + if (!log->dw.liczts[i][1]) + { + log->dw.Zf[i][1] = 1; + log->dw.liczts[i][1]++; + } + log->dw.liczts[i][1]--; + } + log->dw.liczt_eldz[i][1] = 2; + } + else//wylaczenie zadzialania (odpad) + { + if (log->dw.liczt_eldz[i][1] > 0) + log->dw.liczt_eldz[i][1]--; + else + { + log->dw.liczts[i][1] = log->nast_.t_stf[i][1]; + log->dw.Zf[i][1] = 0; + } + } + } + //----------------------------------------------------------- + + + if ((log->dw.Pf[i][0]) || (log->dw.Pf[i][1])) + { + log->dw.liczts_rozwij[i]++; + log->dw.liczt_eldz_rozwij[i] = 2; + if ((log->dw.Pf[i][0]) && (log->dw.liczts_rozwij[i] > (log->nast_.t_stf[i][0] + log->dw.dodaj_rozw[i]))) + { + log->dw.Zf_rozwij[i] = 1; + } + + if ((log->dw.Pf[i][1]) && (log->dw.liczts_rozwij[i] > (log->nast_.t_stf[i][1]+ log->dw.dodaj_rozw[i]))) + { + log->dw.Zf_rozwij[i] = 1; + } + } + else + { + if (log->dw.liczt_eldz_rozwij[i] > 0) + log->dw.liczt_eldz_rozwij[i]--; + else + { + log->dw.dodaj_rozw[i] = 0; + log->dw.Zf_rozwij[i] = 0; + log->dw.liczts_rozwij[i] = 0; + } + } + + if (log->nast_.rozwij) + { + log->dw.Zf[i][1] |= log->dw.Zf_rozwij[i]; + log->dw.Zf[i][0] |= log->dw.Zf_rozwij[i]; + } + + + } + + //odpad wylaczen + + for(i=0;i<6;i++) + { + tempbool |= log->dw.Ws[i]; + } + tempbool |= log->dw.wylacz_od_lacza_suma; + + if (!tempbool) log->dw.W = log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.R = log->dw.S = log->dw.T = log->dw.E = 0; + + + //automatyka za³aczenia na zwarcie - identyfikacja stanu za³¹czenia na zwarcie + + if (log->nast_.ZZw_ON) + { + //w³aczenie od otwarcia wy³¹cznika + + tempbool = (log->nast_.ZdistA->Zdist_dw->I1< log->nast_.ZZw_Ir) & + (log->nast_.ZdistA->Zdist_dw->I2 < log->nast_.ZZw_Ir) & + (log->nast_.ZdistA->Zdist_dw->I3 < log->nast_.ZZw_Ir) & + (log->nast_.ZdistA->Zdist_dw->U1 < log->nast_.ZZw_Ur) & + (log->nast_.ZdistA->Zdist_dw->U2 < log->nast_.ZZw_Ur) & + (log->nast_.ZdistA->Zdist_dw->U3 < log->nast_.ZZw_Ur); + + sprawdz_P_100(&log->dw.ZZw_PIU,tempbool,!tempbool,&log->dw.licznik_PIU,log->nast_.ZZw_to,2); + + log->dw.ZZw_start = log->dw.ZZw_PIU | //w³¹czenie ZZw jeœli brak napiêcia i pr¹du + we_Z | //w³¹czenie ZZw jeœli aktywny sygna³ zamkniêcia wy³¹cznika + (log->dw.W_OFF == 0); //w³¹czenie ZZw jeœli otwarte styki + + log->dw.ZZw |= log->dw.ZZw_start; + if(log->dw.ZZw_start) + { + log->dw.licznik_ZZwa = 0; + } + else + { + log->dw.licznik_ZZwa++; + if(log->dw.licznik_ZZwa > log->nast_.ZZw_ta) + { + if (!log->dw.ZZwP) log->dw.ZZw = 0; + log->dw.licznik_ZZwa--; + } + } + } + else + { + log->dw.ZZw = 0; + } + } + +} diff --git a/src/ZDistL.h b/src/ZDistL.h new file mode 100644 index 0000000..929c777 --- /dev/null +++ b/src/ZDistL.h @@ -0,0 +1,409 @@ +/* + * ZDistL.h + * + * + * Created on: 29-11-2016 + * Author: KJ + */ + +#ifndef ZDISTL_H_ +#define ZDISTL_H_ +/* +#include +#define u8 uint8_t +#define u16 uint16_t +#define u32 uint32_t + +struct binary_io +{ + int yyyy; +}; + +*/ + +#include "tdefs.h" +#include "helper.h" + +struct Wyjscie1_zdistl +{ + u8 dbl_Com[7]; ///deblokowanie dzia�ania stref w zale�no�ci od stanu ��cza + u8 PSPZ;//pobudzenie SPZ +}; + +enum Wylaczenie +{ + start_jednofazowy, + start_trojfazowy, + start_kombinowany, + wyl_definitywny + +}; + + + enum _tryb_lacza + { + WYL = 0, + ZEZWALAJACY = 1, + BLOKUJACY = 2 + }; + + enum _tryb_zezwalajacy + { + ZGODA = 0, + SKROC_CZAS = 1, + WYLACZ = 2 + }; + + enum _tryb_blokujacy + { + BLOKUJ = 0, + WYDLUZ_CZAS = 1, + BLOKUJ_PRZEKAZNIK = 2, + SKROC_ZASIEG = 3 + }; + + + +struct dane_wewnetrzne_ZDistL +{ + u8 blok_lacza[6]; + u8 wydl_czas; + u8 wylacz_od_lacza; + u8 wylacz_od_echa; + u8 wylacz_od_lacza_suma; + //lacze nad + u16 t_lacze_nad; + u8 lacze_nad; + + u16 trwanie_nad; + u8 skrocil_strefa; + u8 skrocil_echo; + + + //logika pradu wstecznego + u8 blokada_pradu_wstecznego; + u16 t_pod_blok_wst; + u8 kopia_lacze_ok; + u8 lacze_odb_old; + + + //logika odblokowania + u8 logika_dblokowania; + u16 t_log_odblokowania; + + u8 blok_LRC[6]; + + u16 t_wyl_od_echa; + + float RL1Em; ///< Rezystancja pÄ™tli zwarciowej fazy L1 z ziemia (rejestracja) + float XL1Em; ///< Rezaktancja pÄ™tli zwarciowej fazy L1 z ziemia (rejestracja) + float RL2Em; ///< Rezystancja pÄ™tli zwarciowej fazy L2 z ziemia (rejestracja) + float XL2Em; ///< Rezaktancja pÄ™tli zwarciowej fazy L2 z ziemia (rejestracja) + float RL3Em; ///< Rezystancja pÄ™tli zwarciowej fazy L3 z ziemia (rejestracja) + float XL3Em; ///< Rezaktancja pÄ™tli zwarciowej fazy L3 z ziemia (rejestracja) + float RL1L2m; ///< Rezystancja pÄ™tli zwarciowej fazy L1 z L2 (rejestracja) + float XL1L2m; ///< Rezaktancja pÄ™tli zwarciowej fazy L1 z L2 (rejestracja) + float RL2L3m; ///< Rezystancja pÄ™tli zwarciowej fazy L2 z L3 (rejestracja) + float XL2L3m; ///< Rezaktancja pÄ™tli zwarciowej fazy L2 z L3 (rejestracja) + float RL3L1m; ///< Rezystancja pÄ™tli zwarciowej fazy L3 z L1 (rejestracja) + float XL3L1m; ///< Rezaktancja pÄ™tli zwarciowej fazy L3 z L1 (rejestracja) + short liczts[6][2]; /// + +#include "../tdefs.h" +#include "../misc.h" + +#include "helper.h" +#include "ZDistL.h" +#include "ZDistA.h" + +struct ZDistL_nast +{ + double ts1LE;///< Czas wy³¹czenia strefy 1 zwarcia jednofazowe z ziemi¹ + double ts1LL;///< Czas wy³¹czenia strefy 1 zwarcia miedzyfazowe + double ts2LE;///< Czas wy³¹czenia strefy 2 zwarcia jednofazowe z ziemi¹ + double ts2LL;///< Czas wy³¹czenia strefy 2 zwarcia miedzyfazowe + double ts3LE;///< Czas wy³¹czenia strefy 3 zwarcia jednofazowe z ziemi¹ + double ts3LL;///< Czas wy³¹czenia strefy 3 zwarcia miedzyfazowe + double ts4LE;///< Czas wy³¹czenia strefy 4 zwarcia jednofazowe z ziemi¹ + double ts4LL;///< Czas wy³¹czenia strefy 4 zwarcia miedzyfazowe + double ts5LE;///< Czas wy³¹czenia strefy 5 zwarcia jednofazowe z ziemi¹ + double ts5LL;///< Czas wy³¹czenia strefy 5 zwarcia miedzyfazowe + + long Stf1; ///< Sposób dzia³ania strefy 1 + long Stf2; ///< Sposób dzia³ania strefy 2 + long Stf3; ///< Sposób dzia³ania strefy 3 + long Stf4; ///< Sposób dzia³ania strefy 4 + long Stf5; ///< Sposób dzia³ania strefy 5 +}; + +int ZDistL_initlog(void *arguments, void *logic) +{ + struct ZDistL_args *args = (struct ZDistL_args *)arguments; + struct ZDistL_logic *log = (struct ZDistL_logic *)logic; + struct ZDistL_nast znast; + + if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl)) + return -1; + + if(set_pointer_in_ptr(args->io.we_zdist_ptr_in,(u32 *)&log->WE_Zdist)) + return -1; + if(set_pointer_in_ptr(args->io.we_l1_ptr_in,(u32 *)&log->WE_L1)) + return -1; + if(set_pointer_in_ptr(args->io.we_l2_ptr_in,(u32 *)&log->WE_L2)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_ps_in,&log->Bl_PS)) + return -1; + if(set_bit_ptr_struct(args->io.bl_ufail_in,&log->Bl_Ufail)) + return -1; + if(set_bit_ptr_struct(args->io.bl_sck_in,&log->Bl_SCH)) + return -1; + if(set_bit_ptr_struct(args->io.wm_ok_in,&log->WM_OK)) + return -1; + if(set_bit_ptr_struct(args->io.w_on_l1_in,&log->W_ON_L1)) + return -1; + if(set_bit_ptr_struct(args->io.w_off_l1_in,&log->W_OFF_L1)) + return -1; + if(set_bit_ptr_struct(args->io.w_on_l2_in,&log->W_ON_L2)) + return -1; + if(set_bit_ptr_struct(args->io.w_off_l2_in,&log->W_OFF_L2)) + return -1; + if(set_bit_ptr_struct(args->io.w_on_l3_in,&log->W_ON_L3)) + return -1; + if(set_bit_ptr_struct(args->io.w_off_l3_in,&log->W_OFF_L3)) + return -1; + if(set_bit_ptr_struct(args->io.start_spz_in,&log->Start_SPZ)) + return -1; + if(set_bit_ptr_struct(args->io.bl_spz_in,&log->BL_SPZ)) + return -1; + if(set_bit_ptr_struct(args->io.z_in,&log->Z)) + return -1; + if(set_bit_ptr_struct(args->io.kas_syg_in,&log->Kas_Syg)) + return -1; + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.p_out,&log->P)) + return -1; + if(set_bit_ptr_struct(args->io.p1w_out,&log->P1W)) + return -1; + if(set_bit_ptr_struct(args->io.p1_out,&log->P1)) + return -1; + if(set_bit_ptr_struct(args->io.p2_out,&log->P2)) + return -1; + if(set_bit_ptr_struct(args->io.p3_out,&log->P3)) + return -1; + if(set_bit_ptr_struct(args->io.p4_out,&log->P4)) + return -1; + if(set_bit_ptr_struct(args->io.p5_out,&log->P5)) + return -1; + if(set_bit_ptr_struct(args->io.r_out,&log->R)) + return -1; + if(set_bit_ptr_struct(args->io.s_out,&log->S)) + return -1; + if(set_bit_ptr_struct(args->io.t_out,&log->T)) + return -1; + if(set_bit_ptr_struct(args->io.e_out,&log->E)) + return -1; + if(set_bit_ptr_struct(args->io.z1w_out,&log->Z1W)) + return -1; + if(set_bit_ptr_struct(args->io.z1_out,&log->Z1)) + return -1; + if(set_bit_ptr_struct(args->io.z2_out,&log->Z2)) + return -1; + if(set_bit_ptr_struct(args->io.z3_out,&log->Z3)) + return -1; + if(set_bit_ptr_struct(args->io.z4_out,&log->Z4)) + return -1; + if(set_bit_ptr_struct(args->io.z5_out,&log->Z5)) + return -1; + if(set_bit_ptr_struct(args->io.w_out,&log->W)) + return -1; + if(set_bit_ptr_struct(args->io.w1_out,&log->W1)) + return -1; + if(set_bit_ptr_struct(args->io.w2_out,&log->W2)) + return -1; + if(set_bit_ptr_struct(args->io.w3_out,&log->W3)) + return -1; + if(set_bit_ptr_struct(args->io.zal_out,&log->zal)) + return -1; + if(set_bit_ptr_struct(args->io.p_spz_out,&log->P_SPZ)) + return -1; + if(set_bit_ptr_struct(args->io.spz_p1_out,&log->SPZ_P1)) + return -1; + if(set_bit_ptr_struct(args->io.spz_p2_out,&log->SPZ_P2)) + return -1; + if(set_bit_ptr_struct(args->io.spz_p3_out,&log->SPZ_P3)) + return -1; + if(set_bit_ptr_struct(args->io.spz_p4_out,&log->SPZ_P4)) + return -1; + if(set_bit_ptr_struct(args->io.spz_p5_out,&log->SPZ_P5)) + return -1; + if(set_bit_ptr_struct(args->io.spz_wz_out,&log->WZ)) + return -1; + if(set_bit_ptr_struct(args->io.spz_wzw_out,&log->WZW)) + return -1; + if(set_bit_ptr_struct(args->io.wd_out,&log->WD)) + return -1; + if(set_bit_ptr_struct(args->io.err_spz_out,&log->err_SPZ)) + return -1; + if(set_bit_ptr_struct(args->io.spz1f_bli0_out,&log->SPZ_BlIo)) + return -1; + if(set_bit_ptr_struct(args->io.tps_SPZ_out,&log->tps_SPZ)) + return -1; + if(set_bit_ptr_struct(args->io.t_bzn_out,&log->t_bzn)) + return -1; + + if(set_pointer_out_ptr(args->io.wyj_ptr_out,(u32 *)&log->dw)) + return -1; + + + log->nast_.SPZ_ON = (args->params.bity & 0x0001)?1:0; + log->nast_.SPZ_s[0] = (args->params.bity & 0x0002)?1:0; + log->nast_.SPZ_s[1] = (args->params.bity & 0x0004)?1:0; + log->nast_.SPZ_s[2] = (args->params.bity & 0x0008)?1:0; + log->nast_.SPZ_s[3] = (args->params.bity & 0x0010)?1:0; + log->nast_.SPZ_s[4] = (args->params.bity & 0x0020)?1:0; + log->nast_.SPZ_zew = (args->params.bity & 0x0040)?1:0; + log->nast_.Bl_SCH = (args->params.bity & 0x0080)?1:0; + log->nast_.Bl_PS[1] = log->nast_.Bl_PS[0] = (args->params.bity & 0x0100)?1:0; + log->nast_.Bl_PS[2] = (args->params.bity & 0x0200)?1:0; + log->nast_.Bl_PS[3] = (args->params.bity & 0x0400)?1:0; + log->nast_.Bl_PS[4] = (args->params.bity & 0x0800)?1:0; + log->nast_.Bl_PS[5] = (args->params.bity & 0x1000)?1:0; + log->nast_.ZZw_ON = (args->params.bity & 0x2000)?1:0; + log->nast_.bl_ = (args->params.bity & 0x4000)?1:0; + + log->nast_.ZdistA = log->WE_Zdist;//(R_ZdistA::Wyjscia *)wskazniki[log->nast_.adr.WE_Zdist]; + + unsigned short i; + short temp_ti; + + znast.ts1LE=args->params.ts1LE;///< Czas wy³¹czenia strefy 1 zwarcia jednofazowe z ziemi¹ + znast.ts1LL=args->params.ts1LL;///< Czas wy³¹czenia strefy 1 zwarcia miedzyfazowe + znast.ts2LE=args->params.ts2LE;///< Czas wy³¹czenia strefy 2 zwarcia jednofazowe z ziemi¹ + znast.ts2LL=args->params.ts2LL;///< Czas wy³¹czenia strefy 2 zwarcia miedzyfazowe + znast.ts3LE=args->params.ts3LE;///< Czas wy³¹czenia strefy 3 zwarcia jednofazowe z ziemi¹ + znast.ts3LL=args->params.ts3LL;///< Czas wy³¹czenia strefy 3 zwarcia miedzyfazowe + znast.ts4LE=args->params.ts4LE;///< Czas wy³¹czenia strefy 4 zwarcia jednofazowe z ziemi¹ + znast.ts4LL=args->params.ts4LL;///< Czas wy³¹czenia strefy 4 zwarcia miedzyfazowe + znast.ts5LE=args->params.ts5LE;///< Czas wy³¹czenia strefy 5 zwarcia jednofazowe z ziemi¹ + znast.ts5LL=args->params.ts5LL;///< Czas wy³¹czenia strefy 5 zwarcia miedzyfazowe + znast.Stf1=args->params.Stf1; + znast.Stf2=args->params.Stf2; + znast.Stf3=args->params.Stf3; + znast.Stf4=args->params.Stf4; + znast.Stf5=args->params.Stf5; + + double *temp_tf = &znast.ts1LE; + + for (i=2;i<12;i++) + { + + //przeliczenie czasow stref + temp_ti = (100 * (*temp_tf - 0.005f)); + temp_tf++; + if (temp_ti) + { + log->nast_.Zbz[i/2][i%2] = 0; + log->nast_.t_stf[i/2][i%2] = temp_ti; + } else { + log->nast_.Zbz[i/2][i%2] = 1; + } + //-------------------------- + } + log->nast_.Zbz[0][0] = 0; + log->nast_.Zbz[0][1] = 0; + log->nast_.t_stf[0][0] = log->nast_.t_stf[1][0]; + log->nast_.t_stf[0][1] = log->nast_.t_stf[1][1]; + //------------------------- + + log->nast_.ZZw_st = args->params.ZZw_st; + + switch(log->nast_.ZZw_st) + { + case 0: + log->PZZw=log->P1W; + log->ZZZw=log->Z1W; + break; + case 1: + log->PZZw=log->P1; + log->ZZZw=log->Z1; + break; + case 2: + log->PZZw=log->P2; + log->ZZZw=log->Z2; + break; + case 3: + log->PZZw=log->P3; + log->ZZZw=log->Z3; + break; + case 4: + log->PZZw=log->P4; + log->ZZZw=log->Z4; + break; + case 5: + log->PZZw=log->P5; + log->ZZZw=log->Z5; + break; + default: + log->PZZw=log->P1W; + log->ZZZw=log->Z1W; + break; + } + + long *temp_e = &znast.Stf1; + + for (i=1;i<6;i++) + { + + //ustawienie bitow sposobu dzialania stref + switch (*(temp_e)) + { + case 0: + log->nast_.Stf_ON[i] = 0; + log->nast_.Stf_W[i] = 0; + break; + case 1: + log->nast_.Stf_ON[i] = 1; + log->nast_.Stf_W[i] = 0; + break; + case 2: + log->nast_.Stf_ON[i] = 1; + log->nast_.Stf_W[i] = 1; + break; + } + temp_e++; + log->nast_.Stf_W_SPZ[i] = log->nast_.Stf_W[i] && log->nast_.SPZ_s[i-1]; + log->nast_.Stf_WD[i] = log->nast_.Stf_W[i] && !(log->nast_.SPZ_s[i-1] && log->nast_.SPZ_ON); + //-------------------------- + } + log->nast_.Stf_ON[0] = log->nast_.Stf_ON[1]; + log->nast_.Stf_W[0] = log->nast_.Stf_W[1]; + log->nast_.Stf_WD[0] = 0; + log->nast_.Stf_W_SPZ[0] = log->nast_.Stf_W_SPZ[1]; + //------------------------- + + log->nast_.SPZst1 = (enum Wylaczenie)args->params.SPZst1; //ustawienie sposobu startu SPZ + + log->nast_.tWSPZ = 1000 * args->params.tWSPZ; //nastawa dodatkowego opóŸnienia wy³¹czenia w SPZ + + log->nast_.tSPZ_1f = (short)(100 * (args->params.tSPZ_1f + 0.005f)); + log->nast_.tSPZ_3f = (short)(100 * (args->params.tSPZ_3f + 0.005f)); + log->nast_.tSPZ_zr = (short)(100 * (args->params.tSPZ_zr + 0.005f)); + log->nast_.tSPZ_st2 = (short)(100 * (args->params.tSPZ_st2 + 0.005f)); + log->nast_.tSPZ_st3 = (short)(100 * (args->params.tSPZ_st3 + 0.005f)); + log->nast_.tSPZ_st4 = (short)(100 * (args->params.tSPZ_st4 + 0.005f)); + log->nast_.tSPZ_st5 = (short)(100 * (args->params.tSPZ_st5 + 0.005f)); + log->nast_.tSPZow = (short)(100 * (args->params.tSPZow + 0.005f)); + log->nast_.tSPZSCH = (short)(100 * (args->params.tSPZSCH + 0.005f)); + log->nast_.tSPZ_zal = (short)(100 * (args->params.tSPZ_zal + 0.005f)); + log->nast_.tSPZ_bl = (short)(100 * (args->params.tSPZCON + 0.005f)); + log->nast_.ZZw_ta = (short)(100 * (args->params.ZZw_ta + 0.005f)); + log->nast_.ZZw_to = (short)(100 * (args->params.ZZw_to + 0.005f)); + log->nast_.SPZilst = (short)(args->params.SPZilst + 0.5f); + + log->nast_.ZZw_Ir = args->params.ZZw_Ir * args->params.ZZw_Ir; + log->nast_.ZZw_Ur = args->params.ZZw_Ur * args->params.ZZw_Ur; + + + for(i=0;i<7;i++) + log->nast_.wy.dbl_Com[i] = 1; + + log->nast_.wy.PSPZ = 0; + + if(log->WE_L1) log->nast_.COM1 = log->WE_L1; else log->nast_.COM1 = &log->nast_.wy; + if(log->WE_L2) log->nast_.COM2 = log->WE_L2; else log->nast_.COM2 = &log->nast_.wy; + + return 0; +} + +/** +Funkcja okraœlania petli zwarciwej przy wy³aczeniu +\param nr_strefy - numer strefy wy³aczaj¹cej +*/ +void wybiornik_f(struct ZDistL_logic *log,unsigned short nr_strefy) +{ + log->dw.R = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][0] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][5]; + + log->dw.S = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][1] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][4]; + + log->dw.T = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][2] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][4] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][5]; + + log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr; + + log->dw.jedfaz = (log->dw.R && !(log->dw.S||log->dw.T)) + || (log->dw.S && !(log->dw.T||log->dw.R)) + || (log->dw.T && !(log->dw.R||log->dw.S)); +} + +/** +Funkcja okraœlania petli zwarciwej przy wy³aczeniu - dzia³anie bezkierunkowe +\param nr_strefy - numer strefy wy³aczaj¹cej +*/ +void wybiornik_fbk(struct ZDistL_logic *log, unsigned short nr_strefy) +{ + log->dw.Rbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][0] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5]; + + log->dw.Sbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][1] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4]; + + log->dw.Tbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][2] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5]; + + log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr; +} + + +void czy_zw_rozwojowe(struct ZDistL_logic *log) +{ + log->dw.R1 |= log->dw.R; + log->dw.S1 |= log->dw.S; + log->dw.T1 |= log->dw.T; + + if ( + log->dw.R1 && log->dw.S1 || + log->dw.S1 && log->dw.T1 || + log->dw.R1 && log->dw.T1 + ) + { + log->dw.W = log->dw.W1 = log->dw.W2 = log->dw.W3 = 1; + } +} + +/** +Funkcja wykonuje wy³aczenie w zaleznoœci od nastawy +\param wyl - tryb wylaczenia +*/ +void wylacz(struct ZDistL_logic *log, enum Wylaczenie wyl) +{ + short i; + + if(!log->dw.W) + { + switch(wyl) + { + + case start_jednofazowy: + + for(i=0;i<6;i++) + { + if (log->dw.Ws[i]) + { + + wybiornik_f(log,i); + + if(log->dw.jedfaz) + { + log->dw.W1 = log->dw.R; + log->dw.W2 = log->dw.S; + log->dw.W3 = log->dw.T; + log->dw.blIo = 1; + log->dw.P_SPZD = 1; + } + czy_zw_rozwojowe(log); + if (log->dw.W) + set_struct(&log->WD); + break; + + } + } + break; + + case start_trojfazowy: + + for(i=0;i<6;i++) + { + if (log->dw.Ws[i]) + { + log->dw.W = 1; + wybiornik_f(log,i); + log->dw.P_SPZD = 1; + break; + } + } + log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.W; + break; + + case start_kombinowany: + + for(i=0;i<6;i++) + { + if (log->dw.Ws[i]) + { + log->dw.P_SPZD = 1; + wybiornik_f(log,i); + if(log->dw.jedfaz) + { + log->dw.W1 = log->dw.R; + log->dw.W2 = log->dw.S; + log->dw.W3 = log->dw.T; + log->dw.blIo = 1; + } else { + log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.W = 1; + } + czy_zw_rozwojowe(log); + break; + + } + + } + + + break; + + case wyl_definitywny: + for(i=1;i<6;i++) + { + if (log->dw.Ws[i]) + { + log->dw.W = 1; + wybiornik_f(log,i); + set_struct(&log->WD); + break; + } + } + log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.W; + + break; + default: + break; + } + } +} + +void ZDistL(void *arguments, void *logic) +{ + struct ZDistL_logic *log = (struct ZDistL_logic *)logic; + unsigned short i; + + log->dw.P = 0; + log->dw.Zz = 0; + + u8 temp_Bl,temp_Zezwolenie; + u8 temp_b; + + + if (*log->nast_.ZdistA->on) + { + temp_Bl = check_struct(&log->Bl_Ufail) || (check_struct(&log->stan_bl) && log->nast_.bl_);// | czy_start(); + + for (i=0;i<6;i++) + { + if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona + { + temp_Zezwolenie = !(temp_Bl || (log->nast_.Bl_PS[i] && check_struct(&log->Bl_PS))); + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.Pf[i][0] = log->dw.Pf[i][1] = temp_Zezwolenie && check_struct(&log->test); + } + else + { + + //ustawienie pobudzen + log->dw.Pf[i][0] = (log->nast_.ZdistA->Zdist_dw->P[i][0] || + log->nast_.ZdistA->Zdist_dw->P[i][1] || + log->nast_.ZdistA->Zdist_dw->P[i][2]) + && temp_Zezwolenie; + log->dw.Pf[i][1] = (log->nast_.ZdistA->Zdist_dw->P[i][3] || + log->nast_.ZdistA->Zdist_dw->P[i][4] || + log->nast_.ZdistA->Zdist_dw->P[i][5]) + && temp_Zezwolenie; + } + log->dw.Pzf[i] = log->dw.Pf[i][0] || log->dw.Pf[i][1]; + + //ustawienie zadzialan jesli bezzwloczny + if (log->nast_.Zbz[i][0]) + { + log->dw.Zf[i][0] = log->dw.Pf[i][0]; + } + if (log->nast_.Zbz[i][1]) + { + log->dw.Zf[i][1] = log->dw.Pf[i][1]; + } + //ustawienie zadzialania strefy + log->dw.Z[i] = log->dw.Zf[i][0] || log->dw.Zf[i][1]; + log->dw.Ws[i] = log->dw.Z[i] && log->nast_.Stf_WD[i]; + + + } else { + log->dw.Pzf[i] = 0; + log->dw.Z[i] = 0; + } + + log->dw.P |= log->dw.Pzf[i]; + log->dw.Zz |= log->dw.Z[i]; //zadzialanie zabezpieczenia + } + + + wylacz(log,wyl_definitywny); + + + + //------------------------------------------------------------------------------------------------------- + //automatyka za³aczenia na zwarcie + + if (check_struct(&log->Z)) + { + log->dw.ZZw |= 1; + } + + if (log->dw.ZZw && log->nast_.ZZw_ON) //jeseli jest w³aczony i jest spe³nione kryterium za³¹czenia na zwarcie + { + wybiornik_fbk(log,log->nast_.ZZw_st); + + log->dw.bl_WSPZ = 1; + + if (log->dw.Rbk || log->dw.Sbk || log->dw.Tbk) + { + log->dw.W |= 1; + set_struct(&log->WD); + log->dw.ZZwP = 1; + } else log->dw.ZZwP = 0; + log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.W; + + } else { + log->dw.Rbk = log->dw.Sbk = log->dw.Tbk = 0; + log->dw.ZZwP = 0; + } + + //------------------------------------------------------------------------------------------------------- + + + if(log->nast_.SPZ_ON && !log->dw.bl_WSPZ && log->dw.dBl_SPZ) // obsluga wylaczenia z automatyka SPZ + { + ///sprawdzenie wy³¹czenia w cyklu SPZ + temp_b = 0; + i=0; + while(!temp_b && i < 6) + { + temp_b = log->dw.Pzf[i] && log->nast_.Stf_W_SPZ[i] + && log->nast_.COM1->dbl_Com[i] && log->nast_.COM2->dbl_Com[i]; // czy jest pobudzenie w cyklu SPZ + i++; + } + + i--; + + + if (temp_b) //odliczenie czasu dodatkowego opóŸnienia wy³¹czenia w przypadku SPZ + { + log->dw.licz_WSPZ--; + } else { + log->dw.licz_WSPZ = log->nast_.tWSPZ; + } + + + + if (log->dw.licz_WSPZ<0) //po doliczeniu czasu jest wy³¹czenie w cyklu SPZ + { + log->dw.licz_WSPZ++; + + + log->dw.Z[i] = 1; + log->dw.Ws[i] = 1; + + if (log->dw.bl_SPZ1) + { + wylacz(log,start_trojfazowy); + } else { + wylacz(log,log->nast_.SPZst1); + } + + + } + } else { + for(i=1;i<6;i++) + { + log->dw.Ws[i] = log->dw.Z[i] && log->nast_.Stf_W[i]; + } + wylacz(log,wyl_definitywny); + } + + //jeœli nie ma SPZ to wy³¹czenie trójfazowe definitywne + + + + + //------------------------------------------------------------------------------------------------------- + + //wyprowadzenie wyjœæ + check_and_set_struct(log->dw.P,&log->P); + check_and_set_struct(log->dw.Pzf[0],&log->P1W); + check_and_set_struct(log->dw.Pzf[1],&log->P1); + check_and_set_struct(log->dw.Pzf[2],&log->P2); + check_and_set_struct(log->dw.Pzf[3],&log->P3); + check_and_set_struct(log->dw.Pzf[4],&log->P4); + check_and_set_struct(log->dw.Pzf[5],&log->P5); + + check_and_set_struct(log->dw.Z[0],&log->Z1W); + check_and_set_struct(log->dw.Z[1],&log->Z1); + check_and_set_struct(log->dw.Z[2],&log->Z2); + check_and_set_struct(log->dw.Z[3],&log->Z3); + check_and_set_struct(log->dw.Z[4],&log->Z4); + check_and_set_struct(log->dw.Z[5],&log->Z5); + + check_and_set_struct(log->dw.R || log->dw.Rbk,&log->R); + check_and_set_struct(log->dw.S || log->dw.Sbk,&log->S); + check_and_set_struct(log->dw.T || log->dw.Tbk,&log->T); + check_and_set_struct(log->dw.E,&log->E); + + check_and_set_struct(log->dw.W,&log->W); + check_and_set_struct(log->dw.W1,&log->W1); + check_and_set_struct(log->dw.W2,&log->W2); + check_and_set_struct(log->dw.W3,&log->W3); + + check_and_set_struct(log->dw.blIo,&log->SPZ_BlIo); + + if(log->dw.ZZwP) + { + set_struct(&log->ZZZw); + set_struct(&log->PZZw); + } + } else { + clear_struct(&log->P); + clear_struct(&log->P1W); + clear_struct(&log->P1); + clear_struct(&log->P2); + clear_struct(&log->P3); + clear_struct(&log->P4); + clear_struct(&log->P5); + clear_struct(&log->Z1W); + clear_struct(&log->Z1); + clear_struct(&log->Z2); + clear_struct(&log->Z3); + clear_struct(&log->Z4); + clear_struct(&log->Z5); + clear_struct(&log->R); + clear_struct(&log->S); + clear_struct(&log->T); + clear_struct(&log->E); + clear_struct(&log->W); + clear_struct(&log->W1); + clear_struct(&log->W2); + clear_struct(&log->W3); + clear_struct(&log->SPZ_BlIo); + } +} + +void ZDistL_100hz(void *arguments, void *logic) +{ + struct ZDistL_logic *log = (struct ZDistL_logic *)logic; + unsigned short i; + u8 tempbool = 0; + u8 Pob_zew; + unsigned short temp = 0; + + if (*log->nast_.ZdistA->on) + { + + //wczytanie danych o stanie wy³¹cznika + log->dw.W1_ON = check_struct(&log->W_ON_L1) && !check_struct(&log->W_OFF_L1); + log->dw.W2_ON = check_struct(&log->W_ON_L2) && !check_struct(&log->W_OFF_L2); + log->dw.W3_ON = check_struct(&log->W_ON_L3) && !check_struct(&log->W_OFF_L3); + log->dw.W_ON = log->dw.W1_ON && log->dw.W2_ON && log->dw.W3_ON; + log->dw.W1_OFF = !check_struct(&log->W_ON_L1) && check_struct(&log->W_OFF_L1); + log->dw.W2_OFF = !check_struct(&log->W_ON_L2) && check_struct(&log->W_OFF_L2); + log->dw.W3_OFF = !check_struct(&log->W_ON_L3) && check_struct(&log->W_OFF_L3); + log->dw.W_OFF = log->dw.W1_OFF && log->dw.W2_OFF && log->dw.W3_OFF; + log->dw.dBl_SPZ = !check_struct(&log->BL_SPZ); + log->dw.bl_SPZ_SCH = (log->nast_.Bl_SCH && check_struct(&log->Bl_SCH)) || !check_struct(&log->WM_OK); + + + + for (i=1;i<6;i++) + { + + //realizacja przekaznikow czasowych + if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona + { + + if (log->dw.Pf[i][0]) + { + if(!log->nast_.Zbz[i][0]) // jesli nie bezzwloczny + { + if (!log->dw.liczts[i][0]) + { + log->dw.Zf[i][0] = 1; + log->dw.liczts[i][0]++; + } + log->dw.liczts[i][0]--; + + } + + } + else//wylaczenie zadzialania (odpad) + { + log->dw.liczts[i][0] = log->nast_.t_stf[i][0]; + log->dw.Zf[i][0] = 0; + } + if (log->dw.Pf[i][1]) + { + if(!log->nast_.Zbz[i][1]) // jesli nie bezzwloczny + { + if (!log->dw.liczts[i][1]) + { + log->dw.Zf[i][1] = 1; + log->dw.liczts[i][1]++; + } + log->dw.liczts[i][1]--; + } + } + else//wylaczenie zadzialania (odpad) + { + log->dw.liczts[i][1] = log->nast_.t_stf[i][1]; + log->dw.Zf[i][1] = 0; + } + + } + //----------------------------------------------------------- + } + + Pob_zew = log->nast_.COM1->PSPZ || log->nast_.COM2->PSPZ || (log->nast_.SPZ_zew && check_struct(&log->Start_SPZ)); + log->dw.P_SPZ = log->dw.dBl_SPZ && (log->dw.P_SPZD || Pob_zew); //pobudzenie SPZ + log->dw.W_1faz = !log->dw.W && log->dw.P_SPZD; + log->dw.P_SPZD = 0; + + //obs³uga sygnalizacji wy³¹czenia definitywnego + if (log->nast_.SPZ_ON) + { + if (log->dw.stan_SPZ==0) + { + if(log->dw.WD && log->dw.W_OFF) + set_struct(&log->WD); + log->dw.WD = 0; + } + } else { + if(log->dw.W) + set_struct(&log->WD); + } + // + + switch(log->dw.stan_SPZ) //w zaleznosci od polozenia w cyklu SPZ + { + case 0: //oczekiwanie + log->dw.bl_WSPZ = 0; + if (log->dw.P_SPZ & log->dw.W_ON & log->nast_.SPZ_ON) //jezeli pobudzenie spz to start cyklu + { + + if(log->dw.W_1faz) log->dw.stan_SPZ = 100; // jesli wy³¹czenie jednofazowe + else log->dw.stan_SPZ = 1;//jesli wy³aczenie trójfazowe + set_struct(&log->P_SPZ); // pobudzenie spz + // skasowanie sygnalizacji z poprzedniego dzialania + log->dw.WZ = 0; + log->dw.WZW = 0; + log->dw.WD = 1; + clear_struct(&log->SPZ_P1); + clear_struct(&log->SPZ_P2); + clear_struct(&log->SPZ_P3); + clear_struct(&log->SPZ_P4); + clear_struct(&log->SPZ_P5); + clear_struct(&log->err_SPZ); + clear_struct(&log->WD); + // + log->dw.licznik_SPZ = log->nast_.tSPZow; + + } else { + if (check_struct(&log->Kas_Syg)) // skasowanie sygnalizacji z ostatniego cyklu + { + log->dw.WZ = 0; + log->dw.WZW = 0; + clear_struct(&log->SPZ_P1); + clear_struct(&log->SPZ_P2); + clear_struct(&log->SPZ_P3); + clear_struct(&log->SPZ_P4); + clear_struct(&log->SPZ_P5); + clear_struct(&log->err_SPZ); + clear_struct(&log->WD); + } + clear_struct(&log->P_SPZ); + log->dw.R1 = log->dw.S1 = log->dw.T1 = 0; + } + + log->dw.bl_SPZ1 = 0; + + break; + + case 17: //wylacz_5: + temp++; + case 13: //wylacz_4: + temp++; + case 9: //wylacz_3: + temp++; + case 5: //wylacz_2: + temp++; + case 1: //wy³¹cz trójfazowy stopieñ 1 + + if (log->dw.W_OFF && + !check_struct(&log->P)) + { + log->dw.licznik_SPZ = *((short *)&log->nast_.tSPZ_3f + temp); //ustawienie czasu przerwy beznapieciowej + log->dw.stan_SPZ += 1; + + log->dw.licznik_bl_SCH = log->nast_.tSPZSCH;//ustawienie czasu oczekiwania na synchronizm + set_struct(&log->tps_SPZ); + } else { + log->dw.licznik_SPZ--; + if (!log->dw.licznik_SPZ) // jesli brak otwarcia wylacznika + { + log->dw.stan_SPZ = 22; //SPZ_error + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + set_struct(&log->err_SPZ); + } + } + break; + + case 100://wy³¹cz jednofazowy stopieñ 1 + + if (log->dw.W) // jesli jednak wy³aczenie trojfazowe + { + if(log->nast_.SPZst1==start_jednofazowy) + { + log->dw.stan_SPZ = 22; + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + set_struct(&log->tps_SPZ); + set_struct(&log->WD); + } + else log->dw.stan_SPZ = 1; + + } else { // jesli wy³¹czenie jednofazowe + + if ((log->dw.jedfaz && ((log->dw.R && log->dw.W1_OFF) || (log->dw.S && log->dw.W2_OFF) || (log->dw.T && log->dw.W3_OFF)))) + { + log->dw.licznik_SPZ = log->nast_.tSPZ_1f; //ustawienie czasu przerwy beznapieciowej + log->dw.licznik_bl_SCH = log->nast_.tSPZSCH;//ustawienie czasu oczekiwania na synchronizm + log->dw.stan_SPZ = 102; + set_struct(&log->tps_SPZ); + } else { + log->dw.licznik_SPZ--; + if (!log->dw.licznik_SPZ) // jesli brak otwarcia wylacznika + { + log->dw.stan_SPZ = 22; //SPZ_error + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + set_struct(&log->err_SPZ); + } + } + } + + break; + + case 18: //czas_przerwy_5: + temp++; + case 14: //czas_przerwy_4: + temp++; + case 10: //czas_przerwy_3: + temp++; + case 6: //czas_przerwy_2: + temp++; + case 2: //czas_przerwy_1: // odliczanie czasu przerwy beznapieciowej + + log->dw.licznik_SPZ--; + + if (!log->dw.licznik_SPZ) // jesli odliczono czas przerwy beznapieciowej + { + log->dw.licznik_SPZ++; + log->dw.licznik1 = log->nast_.tSPZow; + set_struct(&log->tps_SPZ); + clear_struct(&log->t_bzn); + if (check_struct(&log->BL_SPZ)) + { + log->dw.stan_SPZ = 22; // jesli jest wlaczona blokada SPZ to koniec cyklu + set_struct(&log->err_SPZ); + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + } else { + if(log->dw.bl_SPZ_SCH) //jesli jest blokada od kontroli synchronizmu lub brak gotowosci napêdu + { + log->dw.licznik_bl_SCH--; + if(!log->dw.licznik_bl_SCH) // jeœli nie doczekano sie zezwolenia na za³¹cz od synchronizmu + { + log->dw.stan_SPZ = 22; // error i koniec cyklu + set_struct(&log->err_SPZ); + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + } + }else{ + log->dw.stan_SPZ += 1; + // wystawienie zalaczenia + set_struct(&log->zal); + log->dw.licznik2 = log->nast_.tSPZ_zal; + switch(temp) + { + case 0: + set_struct(&log->SPZ_P1); + break; + case 1: + set_struct(&log->SPZ_P2); + break; + case 2: + set_struct(&log->SPZ_P3); + break; + case 3: + set_struct(&log->SPZ_P4); + break; + case 4: + set_struct(&log->SPZ_P5); + break; + default: + set_struct(&log->SPZ_P1); + break; + } + + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + } + } + } + break; + + case 102: + + log->dw.licznik_SPZ--; + if(log->dw.W) // przerwanie cyklu jednofazowego SPZ + { + log->dw.licznik_SPZ = log->nast_.tSPZow; + log->dw.stan_SPZ = 103; + }else{ + if (!log->dw.licznik_SPZ) // jesli odliczono czas przerwy beznapieciowej + { + log->dw.licznik_SPZ++; + log->dw.licznik1 = log->nast_.tSPZow; + + set_struct(&log->tps_SPZ); + clear_struct(&log->t_bzn); + + if (check_struct(&log->BL_SPZ)) + { + log->dw.stan_SPZ = 22; // jesli jest wlaczona blokada SPZ to koniec cyklu + set_struct(&log->err_SPZ); + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + } else { + if(log->dw.bl_SPZ_SCH) //jesli jest blokada od kontroli synchronizmu lub brak gotowosci napedu + { + log->dw.licznik_bl_SCH--; + if(!log->dw.licznik_bl_SCH) // jeœli nie doczekano sie zezwolenia na za³¹cz od synchronizmu + { + log->dw.stan_SPZ = 22; // error i koniec cyklu + set_struct(&log->err_SPZ); + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + } + }else{ + log->dw.stan_SPZ = 3; + set_struct(&log->zal); + // wystawienie zalaczenia + log->dw.licznik2 = log->nast_.tSPZ_zal; + set_struct(&log->SPZ_P1); + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + } + } + } + } + break; + + case 103: //przerwanie cyklu SPZ - zwarcie rozwojowe + if (log->dw.W_OFF && + !check_struct(&log->P)) + { + log->dw.licznik_SPZ = log->nast_.tSPZ_zr; //ustawienie czasu przerwy beznapieciowej dla zwarcia rozwojowego + if(log->nast_.SPZst1==start_jednofazowy) //jesli start jednofazowy to blokada SPZ + { + log->dw.stan_SPZ = 22; + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + set_struct(&log->tps_SPZ); + set_struct(&log->WD); + } else { + log->dw.stan_SPZ = 2; + } + log->dw.licznik_bl_SCH = log->nast_.tSPZSCH;//ustawienie czasu oczekiwania na synchronizm + } else { + log->dw.licznik_SPZ--; + if (!log->dw.licznik_SPZ) // jesli brak otwarcia wylacznika + { + log->dw.stan_SPZ = 22; //SPZ_error + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + set_struct(&log->err_SPZ); + } + } + + break; + + case 19: //zalacz_5: + case 15: //zalacz_4: + case 11: //zalacz_3: + case 7: //zalacz_2: + case 3: //zalacz_1: // oczekiwanie na odpowiedz wylacznika + log->dw.bl_SPZ1 = 1; + log->dw.licznik_SPZ--; + log->dw.licznik1--; + log->dw.licznik2--; + if (!log->dw.licznik2) clear_struct(&log->zal); //wy³¹czenie impulsu za³¹czaj¹cego po odliczeniu nastawionego czasu + if (log->dw.W_ON) // jesli wylacznik zamkn¹l sie + { + log->dw.stan_SPZ += 1; + clear_struct(&log->zal); + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + } + if ((!log->dw.licznik_SPZ) || check_struct(&log->P) && (!log->dw.licznik1)) // jezeli przed informacja o zamknieciu wylacznika przyszedl sygnal wylacz, lub wylacznik sie nie zamknal + { + log->dw.licznik_SPZ = log->nast_.tSPZ_bl; + log->dw.stan_SPZ = 22; // jesli jest wlaczona blokada SPZ to koniec cyklu + set_struct(&log->err_SPZ); + clear_struct(&log->zal); + } + break; + + case 20: //odwzbudzenie_5: + case 16: //odwzbudzenie_4: + case 12: //odwzbudzenie_3: + case 8: //odwzbudzenie_2: + case 4: //odwzbudzenie_1: // odliczanie czasu odwzbudzenia po wyslaniu sygnalu zalacz + log->dw.bl_WSPZ = log->nast_.SPZilst < (log->dw.stan_SPZ / 4); + log->dw.licznik_SPZ--; + if (log->dw.P_SPZ) // jesli ponownie pobudzenie przed wybiegnieciem czasu blokady spz + { + log->dw.WZ = 0; + log->dw.WZW = 1; + + if ((log->nast_.SPZilst > (log->dw.stan_SPZ / 4)) & (log->nast_.SPZst1!=start_jednofazowy)) // przejscie do nastepnej proby w cyklu + { + log->dw.stan_SPZ += 1; + log->dw.licznik_SPZ = log->nast_.tSPZow; + } else { // wylaczenie definitywne + log->dw.stan_SPZ = 21; //koniec_cyklu + } + } else { // je¿eeli nie to koniec cyklu + if(!log->dw.licznik_SPZ) + { + if(log->dw.W_ON) + { + log->dw.WZ = 1; + log->dw.WZW = 0; + } else { + log->dw.WZ = 0; + log->dw.WZW = 1; + } + log->dw.stan_SPZ = 0; //oczekiwanie; + clear_struct(&log->tps_SPZ); + } + } + break; + + case 21: //koniec_cyklu: + log->dw.licznik_SPZ--; + if (!log->dw.licznik_SPZ) + { + log->dw.stan_SPZ = 0; //oczekiwanie; + clear_struct(&log->tps_SPZ); + } + break; + + case 22: //SPZ_error: + log->dw.licznik_SPZ--; + if(log->dw.licznik_SPZ) + { + set_struct(&log->tps_SPZ); + } else { + log->dw.stan_SPZ = 0; //oczekiwanie; + clear_struct(&log->tps_SPZ); + + } + + break; + default: + log->dw.stan_SPZ = 0; + break; + } + + + if (!check_struct(&log->tps_SPZ)) + { + check_and_set_struct(log->dw.WZ,&log->WZ); + check_and_set_struct(log->dw.WZW,&log->WZW); + } + + + //odpad wy³aczeñ + + + + for(i=0;i<6;i++) + { + tempbool |= log->dw.Ws[i]; + } + if (!tempbool) log->dw.W = log->dw.W1 = log->dw.W2 = log->dw.W3 = log->dw.R = log->dw.S = log->dw.T = log->dw.E = 0; + + + //zdjêcie blokady zabezpieczenia zeimnozwarciowego od jednofazowego SPZ + + if (log->dw.W1_ON && log->dw.W2_ON && log->dw.W3_ON) log->dw.blIo = 0; + + + //automatyka za³aczenia na zwarcie - identyfikacja stanu za³¹czenia na zwarcie + + //w³aczenie od otwarcia wy³¹cznika + + tempbool = (log->nast_.ZdistA->Zdist_dw->I1nast_.ZZw_Ir) && + (log->nast_.ZdistA->Zdist_dw->I2nast_.ZZw_Ir) && + (log->nast_.ZdistA->Zdist_dw->I3nast_.ZZw_Ir) && + (log->nast_.ZdistA->Zdist_dw->U1nast_.ZZw_Ur) && + (log->nast_.ZdistA->Zdist_dw->U2nast_.ZZw_Ur) && + (log->nast_.ZdistA->Zdist_dw->U3nast_.ZZw_Ur); + + sprawdz_P(&log->dw.ZZw_PIU,tempbool,!tempbool,&log->dw.licznik_PIU,log->nast_.ZZw_to,2); + + log->dw.ZZw_start = log->dw.ZZw_PIU || //w³¹czenie ZZw jeœli brak napiêcia i pr¹du + check_struct(&log->Z) || //w³¹czenie ZZw jeœli aktywny sygna³ zamkniêcia wy³¹cznika + (log->dw.W_OFF & !check_struct(&log->P_SPZ)); //w³¹czenie ZZw jeœli otwarte styki i nie ma cyklu SPZ + + log->dw.ZZw |= log->dw.ZZw_start; + if(log->dw.ZZw_start) + { + log->dw.licznik_ZZwa = 0; + } else { + log->dw.licznik_ZZwa++; + if(log->dw.licznik_ZZwa>log->nast_.ZZw_ta) + { + if (!log->dw.ZZwP) log->dw.ZZw = 0; + log->dw.licznik_ZZwa--; + } + } + + + /// + + // realizacja rejestratora parametrow ostatniego zaklocenia + switch(status_rej) + { + case oczekiwanie: + if (log->dw.P) + { + log->dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + } + break; + case sledzenie: + if (log->dw.W1 || log->dw.W2 || log->dw.W3) + { + status_rej = sledzenie_zad; + if (log->dw.Ws[0] || log->dw.Ws[1]) + { + log->dw.RL1Em = log->nast_.ZdistA->Zdist_dw->Z.R[0]; + log->dw.RL2Em = log->nast_.ZdistA->Zdist_dw->Z.R[1]; + log->dw.RL3Em = log->nast_.ZdistA->Zdist_dw->Z.R[2]; + log->dw.XL1Em = log->nast_.ZdistA->Zdist_dw->Z.X[0]; + log->dw.XL2Em = log->nast_.ZdistA->Zdist_dw->Z.X[1]; + log->dw.XL3Em = log->nast_.ZdistA->Zdist_dw->Z.X[2]; + } else { + log->dw.RL1Em = log->nast_.ZdistA->Zdist_dw->Z.R[3]; + log->dw.RL2Em = log->nast_.ZdistA->Zdist_dw->Z.R[4]; + log->dw.RL3Em = log->nast_.ZdistA->Zdist_dw->Z.R[5]; + log->dw.XL1Em = log->nast_.ZdistA->Zdist_dw->Z.X[3]; + log->dw.XL2Em = log->nast_.ZdistA->Zdist_dw->Z.X[4]; + log->dw.XL3Em = log->nast_.ZdistA->Zdist_dw->Z.X[5]; + } + log->dw.RL1L2m = log->nast_.ZdistA->Zdist_dw->Z.R[6]; + log->dw.RL2L3m = log->nast_.ZdistA->Zdist_dw->Z.R[7]; + log->dw.RL3L1m = log->nast_.ZdistA->Zdist_dw->Z.R[8]; + log->dw.XL1L2m = log->nast_.ZdistA->Zdist_dw->Z.X[6]; + log->dw.XL2L3m = log->nast_.ZdistA->Zdist_dw->Z.X[7]; + log->dw.XL3L1m = log->nast_.ZdistA->Zdist_dw->Z.X[8]; + + // log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (log->dw.P) + { + log->dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!log->dw.P) + { + log->dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + log->dw.dl_++; + } + break; + case zapis: + if (!log->dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + } + + } else { + clear_struct(&log->SPZ_P1); + clear_struct(&log->SPZ_P2); + clear_struct(&log->SPZ_P3); + clear_struct(&log->SPZ_P4); + clear_struct(&log->SPZ_P5); + clear_struct(&log->err_SPZ); + clear_struct(&log->WZ); + clear_struct(&log->WZW); + clear_struct(&log->WD); + clear_struct(&log->zal); + clear_struct(&log->P_SPZ); + clear_struct(&log->tps_SPZ); + clear_struct(&log->t_bzn); + } +} diff --git a/src/ZDistL_marian.h_ b/src/ZDistL_marian.h_ new file mode 100644 index 0000000..17debf2 --- /dev/null +++ b/src/ZDistL_marian.h_ @@ -0,0 +1,336 @@ +/* + * ZDistL.h + * + * + * Created on: 29-11-2016 + * Author: KJ + */ + +#ifndef ZDISTL_H_ +#define ZDISTL_H_ + +#include "../tdefs.h" +#include "helper.h" + +struct Wyjscie1_zdistl +{ + u8 dbl_Com[7]; ///deblokowanie dzia³ania stref w zale¿noœci od stanu ³¹cza + u8 PSPZ;//pobudzenie SPZ +}; + +enum Wylaczenie +{ + start_jednofazowy, + start_trojfazowy, + start_kombinowany, + wyl_definitywny + +}; + + +struct dane_wewnetrzne_ZDistL +{ + float RL1Em; ///< Rezystancja pêtli zwarciowej fazy L1 z ziemia (rejestracja) + float XL1Em; ///< Rezaktancja pêtli zwarciowej fazy L1 z ziemia (rejestracja) + float RL2Em; ///< Rezystancja pêtli zwarciowej fazy L2 z ziemia (rejestracja) + float XL2Em; ///< Rezaktancja pêtli zwarciowej fazy L2 z ziemia (rejestracja) + float RL3Em; ///< Rezystancja pêtli zwarciowej fazy L3 z ziemia (rejestracja) + float XL3Em; ///< Rezaktancja pêtli zwarciowej fazy L3 z ziemia (rejestracja) + float RL1L2m; ///< Rezystancja pêtli zwarciowej fazy L1 z L2 (rejestracja) + float XL1L2m; ///< Rezaktancja pêtli zwarciowej fazy L1 z L2 (rejestracja) + float RL2L3m; ///< Rezystancja pêtli zwarciowej fazy L2 z L3 (rejestracja) + float XL2L3m; ///< Rezaktancja pêtli zwarciowej fazy L2 z L3 (rejestracja) + float RL3L1m; ///< Rezystancja pêtli zwarciowej fazy L3 z L1 (rejestracja) + float XL3L1m; ///< Rezaktancja pêtli zwarciowej fazy L3 z L1 (rejestracja) + short liczts[6][2]; /// + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "ZDistS.h" +#include "ZDistA.h" + +struct ZDistS_nast +{ + long Stf1; ///< Sposób dzia³ania strefy 1 + long Stf2; ///< Sposób dzia³ania strefy 2 + long Stf3; ///< Sposób dzia³ania strefy 3 + long Stf4; ///< Sposób dzia³ania strefy 4 + long Stf5; ///< Sposób dzia³ania strefy 5 +}; + +int ZDistS_initlog(void *arguments, void *logic) +{ + struct ZDistS_args *args = (struct ZDistS_args *)arguments; + struct ZDistS_logic *log = (struct ZDistS_logic *)logic; + struct ZDistS_nast snast; + + if(set_bit_ptr_struct(args->io.stan_bl_in,&log->stan_bl)) + return -1; + if(set_pointer_in_ptr(args->io.we_zdist_ptr_in,(u32 *)&log->WE_Zdist)) + return -1; + if(set_bit_ptr_struct(args->io.bl_ps_in,&log->Bl_PS)) + return -1; + if(set_bit_ptr_struct(args->io.bl_ufail_in,&log->Bl_Ufail)) + return -1; + if(set_bit_ptr_struct(args->io.z_in,&log->Z)) + return -1; + if(set_bit_ptr_struct(args->io.w_on_in,&log->W_ON)) + return -1; + if(set_bit_ptr_struct(args->io.w_off_in,&log->W_OFF)) + return -1; + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.p_out,&log->P)) + return -1; + if(set_bit_ptr_struct(args->io.p1_out,&log->P1)) + return -1; + if(set_bit_ptr_struct(args->io.p2_out,&log->P2)) + return -1; + if(set_bit_ptr_struct(args->io.p3_out,&log->P3)) + return -1; + if(set_bit_ptr_struct(args->io.p4_out,&log->P4)) + return -1; + if(set_bit_ptr_struct(args->io.p5_out,&log->P5)) + return -1; + if(set_bit_ptr_struct(args->io.r_out,&log->R)) + return -1; + if(set_bit_ptr_struct(args->io.s_out,&log->S)) + return -1; + if(set_bit_ptr_struct(args->io.t_out,&log->T)) + return -1; + if(set_bit_ptr_struct(args->io.e_out,&log->E)) + return -1; + if(set_bit_ptr_struct(args->io.z1_out,&log->Z1)) + return -1; + if(set_bit_ptr_struct(args->io.z2_out,&log->Z2)) + return -1; + if(set_bit_ptr_struct(args->io.z3_out,&log->Z3)) + return -1; + if(set_bit_ptr_struct(args->io.z4_out,&log->Z4)) + return -1; + if(set_bit_ptr_struct(args->io.z5_out,&log->Z5)) + return -1; + if(set_bit_ptr_struct(args->io.w_out,&log->W)) + return -1; + + log->nast_.Bl_PS[0] = (args->params.bity & 0x0001)?1:0; + log->nast_.Bl_PS[1] = (args->params.bity & 0x0002)?1:0; + log->nast_.Bl_PS[2] = (args->params.bity & 0x0004)?1:0; + log->nast_.Bl_PS[3] = (args->params.bity & 0x0008)?1:0; + log->nast_.Bl_PS[4] = (args->params.bity & 0x0010)?1:0; + log->nast_.ZZw_ON = (args->params.bity & 0x0020)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0040)?1:0; + + log->nast_.ZdistA = log->WE_Zdist; + + uint16_t i; + int16_t temp_ti; + float temp_tf; + + for (i=0;i<10;i++) + { + switch(i) + { + case 0: + temp_tf = args->params.ts1LE; + break; + case 1: + temp_tf = args->params.ts1LL; + break; + case 2: + temp_tf = args->params.ts2LE; + break; + case 3: + temp_tf = args->params.ts2LL; + break; + case 4: + temp_tf = args->params.ts3LE; + break; + case 5: + temp_tf = args->params.ts3LL; + break; + case 6: + temp_tf = args->params.ts4LE; + break; + case 7: + temp_tf = args->params.ts4LL; + break; + case 8: + temp_tf = args->params.ts5LE; + break; + case 9: + temp_tf = args->params.ts6LL; + break; + default: + temp_tf = args->params.ts1LE; + break; + + } + + //przeliczenie czasow stref + temp_ti = (100 * (temp_tf - 0.005f)); + + if (temp_ti) + { + log->nast_.Zbz[i/2][i%2] = 0; + log->nast_.t_stf[i/2][i%2] = temp_ti; + } else { + log->nast_.Zbz[i/2][i%2] = 1; + } + //-------------------------- + } + //------------------------- + + log->nast_.ZZw_st = args->params.ZZw_st + 1; + + switch(log->nast_.ZZw_st) + { + case 0: + log->PZZw=log->P1; + log->ZZZw=log->Z1; + break; + case 1: + log->PZZw=log->P2; + log->ZZZw=log->Z2; + break; + case 2: + log->PZZw=log->P3; + log->ZZZw=log->Z3; + break; + case 3: + log->PZZw=log->P4; + log->ZZZw=log->Z4; + break; + case 4: + log->PZZw=log->P5; + log->ZZZw=log->Z5; + break; + default: + log->PZZw=log->P1; + log->ZZZw=log->Z1; + break; + } + + + + snast.Stf1=args->params.Stf1; + snast.Stf2=args->params.Stf2; + snast.Stf3=args->params.Stf3; + snast.Stf4=args->params.Stf4; + snast.Stf5=args->params.Stf5; + + long *temp_e = &snast.Stf1; + + for (i=0;i<5;i++) + { + + //ustawienie bitow sposobu dzialania stref + switch (*(temp_e)) + { + case 0: + log->nast_.Stf_ON[i] = 0; + log->nast_.Stf_W[i] = 0; + break; + case 1: + log->nast_.Stf_ON[i] = 1; + log->nast_.Stf_W[i] = 0; + break; + case 2: + log->nast_.Stf_ON[i] = 1; + log->nast_.Stf_W[i] = 1; + break; + } + temp_e++; + //-------------------------- + } + //------------------------- + + log->nast_.ZZw_ta = (int16_t)(100 * (args->params.ZZw_ta + 0.005f)); + log->nast_.ZZw_to = (int16_t)(100 * (args->params.ZZw_to + 0.005f)); + + log->nast_.ZZw_Ir = args->params.ZZw_Ir * args->params.ZZw_Ir; + log->nast_.ZZw_Ur = args->params.ZZw_Ur * args->params.ZZw_Ur; + + return 0; +} + +/** +Funkcja okraœlania petli zwarciwej przy wy³aczeniu +\param nr_strefy - numer strefy wy³aczaj¹cej +*/ +void wybiornik_f_ZDistS(struct ZDistS_logic *log,unsigned short nr_strefy) +{ + log->dw.R = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][0] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][5]; + + log->dw.S = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][1] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][4]; + + log->dw.T = log->nast_.ZdistA->Zdist_dw->P[nr_strefy][2] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][4] + || log->nast_.ZdistA->Zdist_dw->P[nr_strefy][5]; + + log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr; + +} + +/** +Funkcja okraœlania petli zwarciwej przy wy³aczeniu - dzia³anie bezkierunkowe +\param nr_strefy - numer strefy wy³aczaj¹cej +*/ +void wybiornik_fbk_ZDistS(struct ZDistS_logic *log, unsigned short nr_strefy) +{ + log->dw.Rbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][0] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5]; + + log->dw.Sbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][1] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][3] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4]; + + log->dw.Tbk = log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][2] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][4] + || log->nast_.ZdistA->Zdist_dw->Pbk[nr_strefy][5]; + + log->dw.E = log->nast_.ZdistA->Zdist_dw->Iogr; +} + + + +void ZDistS(void *arguments, void *logic) +{ + struct ZDistS_logic *log = (struct ZDistS_logic *)logic; + + uint16_t i; + log->dw.P = 0; + log->dw.Zz = 0; + + u8 temp_Bl,temp_Zezwolenie; + + if (*log->nast_.ZdistA->on) + { + temp_Bl = check_struct(&log->Bl_Ufail) + || (check_struct(&log->stan_bl) && log->nast_.bl_);// | czy_start(); + + + for (i=0;i<5;i++) + { + if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona + { + temp_Zezwolenie = !(temp_Bl || (log->nast_.Bl_PS[i] && check_struct(&log->Bl_PS))); + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.Pf[i][0] = log->dw.Pf[i][1] = check_struct(&log->test) && temp_Zezwolenie; + } + else + { + + //ustawienie pobudzen + log->dw.Pf[i][0] = (log->nast_.ZdistA->Zdist_dw->P[i+1][0] || + log->nast_.ZdistA->Zdist_dw->P[i+1][1] || + log->nast_.ZdistA->Zdist_dw->P[i+1][2]) + && temp_Zezwolenie; + log->dw.Pf[i][1] = (log->nast_.ZdistA->Zdist_dw->P[i+1][3] || + log->nast_.ZdistA->Zdist_dw->P[i+1][4] || + log->nast_.ZdistA->Zdist_dw->P[i+1][5]) + && temp_Zezwolenie; + } + log->dw.Pzf[i] = log->dw.Pf[i][0] || log->dw.Pf[i][1]; + + //ustawienie zadzialan jesli bezzwloczny + if (log->nast_.Zbz[i][0]) + { + log->dw.Zf[i][0] = log->dw.Pf[i][0]; + } + if (log->nast_.Zbz[i][1]) + { + log->dw.Zf[i][1] = log->dw.Pf[i][1]; + } + //ustawienie zadzialania strefy + log->dw.Z[i] = log->dw.Zf[i][0] || log->dw.Zf[i][1]; + log->dw.Ws[i] = log->dw.Z[i] && log->nast_.Stf_W[i]; + + + } else { + log->dw.Pzf[i] = 0; + log->dw.Z[i] = 0; + } + + log->dw.P |= log->dw.Pzf[i]; + log->dw.Zz |= log->dw.Z[i]; //zadzialanie zabezpieczenia + if(!log->dw.W) + { + log->dw.W |= log->dw.Ws[i]; + if(log->dw.W) + { + wybiornik_f_ZDistS(log,i+1); + } + } + } + + //------------------------------------------------------------------------------------------------------- + //automatyka za³aczenia na zwarcie + + + + + if (check_struct(&log->Z)) { + log->dw.ZZw |= 1; + } + + if (log->dw.ZZw && log->nast_.ZZw_ON) //jeseli jest w³aczony i jest spe³nione kryterium za³¹czenia na zwarcie + { + wybiornik_fbk_ZDistS(log,log->nast_.ZZw_st); + + if (log->dw.Rbk || log->dw.Sbk || log->dw.Tbk) + { + log->dw.W |= 1; + log->dw.ZZwP = 1; + } else log->dw.ZZwP = 0; + + + } else { + log->dw.Rbk = log->dw.Sbk = log->dw.Tbk = 0; + log->dw.ZZwP = 0; + } + + + //------------------------------------------------------------------------------------------------------- + + + + + //------------------------------------------------------------------------------------------------------- + + //wyprowadzenie wyjœæ + check_and_set_struct(log->dw.P,&log->P); + check_and_set_struct(log->dw.Pzf[0],&log->P1); + check_and_set_struct(log->dw.Pzf[1],&log->P2); + check_and_set_struct(log->dw.Pzf[2],&log->P3); + check_and_set_struct(log->dw.Pzf[3],&log->P4); + check_and_set_struct(log->dw.Pzf[4],&log->P5); + + check_and_set_struct(log->dw.Z[0],&log->Z1); + check_and_set_struct(log->dw.Z[1],&log->Z2); + check_and_set_struct(log->dw.Z[2],&log->Z3); + check_and_set_struct(log->dw.Z[3],&log->Z4); + check_and_set_struct(log->dw.Z[4],&log->Z5); + + check_and_set_struct(log->dw.R || log->dw.Rbk,&log->R); + check_and_set_struct(log->dw.S || log->dw.Sbk,&log->S); + check_and_set_struct(log->dw.T || log->dw.Tbk,&log->T); + check_and_set_struct(log->dw.E,&log->E); + + check_and_set_struct(log->dw.W,&log->W); + + if(log->dw.ZZwP) + { + set_struct(&log->PZZw); + set_struct(&log->ZZZw); + } + } else { + clear_struct(&log->P); + clear_struct(&log->P1); + clear_struct(&log->P2); + clear_struct(&log->P3); + clear_struct(&log->P4); + clear_struct(&log->P5); + + clear_struct(&log->Z1); + clear_struct(&log->Z2); + clear_struct(&log->Z3); + clear_struct(&log->Z4); + clear_struct(&log->Z5); + + clear_struct(&log->R); + clear_struct(&log->S); + clear_struct(&log->T); + clear_struct(&log->E); + clear_struct(&log->W); + } +} + +void ZDistS_100hz(void *arguments, void *logic) +{ + struct ZDistS_logic *log = (struct ZDistS_logic *)logic; + unsigned short i; + u8 tempbool = 0; + + + if (*log->nast_.ZdistA->on) + { + + //wczytanie danych o stanie wy³¹cznika + log->dw.W_ON = check_struct(&log->W_ON) && !check_struct(&log->W_OFF); + log->dw.W_OFF = !check_struct(&log->W_ON) && check_struct(&log->W_OFF); + + + for (i=0;i<5;i++) + { + //realizacja przekaznikow czasowych + if (log->nast_.Stf_ON[i]) //jesli strefa wlaczona + { + + if (log->dw.Pf[i][0]) + { + if(!log->nast_.Zbz[i][0]) // jesli nie bezzwloczny + { + if (!log->dw.liczts[i][0]) + { + log->dw.Zf[i][0] = 1; + log->dw.liczts[i][0]++; + } + log->dw.liczts[i][0]--; + + } + + } + else//wylaczenie zadzialania (odpad) + { + log->dw.liczts[i][0] = log->nast_.t_stf[i][0]; + log->dw.Zf[i][0] = 0; + } + if (log->dw.Pf[i][1]) + { + if(!log->nast_.Zbz[i][1]) // jesli nie bezzwloczny + { + if (!log->dw.liczts[i][1]) + { + log->dw.Zf[i][1] = 1; + log->dw.liczts[i][1]++; + } + log->dw.liczts[i][1]--; + } + } + else//wylaczenie zadzialania (odpad) + { + log->dw.liczts[i][1] = log->nast_.t_stf[i][1]; + log->dw.Zf[i][1] = 0; + } + + } + //----------------------------------------------------------- + } + + //odpad wy³aczeñ + + + + for(i=0;i<5;i++) + { + tempbool |= log->dw.Ws[i]; + } + if (!tempbool) log->dw.W = log->dw.R = log->dw.S = log->dw.T = log->dw.E = 0; + + + + + //automatyka za³aczenia na zwarcie - identyfikacja stanu za³¹czenia na zwarcie + + //w³aczenie od otwarcia wy³¹cznika + + tempbool = (log->nast_.ZdistA->Zdist_dw->I1nast_.ZZw_Ir) && + (log->nast_.ZdistA->Zdist_dw->I2nast_.ZZw_Ir) && + (log->nast_.ZdistA->Zdist_dw->I3nast_.ZZw_Ir) && + (log->nast_.ZdistA->Zdist_dw->U1nast_.ZZw_Ur) && + (log->nast_.ZdistA->Zdist_dw->U2nast_.ZZw_Ur) && + (log->nast_.ZdistA->Zdist_dw->U3nast_.ZZw_Ur); + + sprawdz_P(&log->dw.ZZw_PIU,tempbool,!tempbool,&log->dw.licznik_PIU,log->nast_.ZZw_to,2); + + log->dw.ZZw_start = log->dw.ZZw_PIU || //w³¹czenie ZZw jeœli brak napiêcia i pr¹du + check_struct(&log->Z) || //w³¹czenie ZZw jeœli aktywny sygna³ zamkniêcia wy³¹cznika + (log->dw.W_OFF); //w³¹czenie ZZw jeœli otwarte styki i nie ma cyklu SPZ + + log->dw.ZZw |= log->dw.ZZw_start; + if(log->dw.ZZw_start) + { + log->dw.licznik_ZZwa = 0; + } else { + log->dw.licznik_ZZwa++; + if(log->dw.licznik_ZZwa>log->nast_.ZZw_ta) + { + if (!log->dw.ZZwP) log->dw.ZZw = 0; + log->dw.licznik_ZZwa--; + } + } + + + /// + + // realizacja rejestratora parametrow ostatniego zaklocenia + switch(status_rej) + { + case oczekiwanie: + if (log->dw.P) + { + log->dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + } + break; + case sledzenie: + if (log->dw.W) + { + status_rej = sledzenie_zad; + if (log->dw.Ws[0] | log->dw.Ws[1]) + { + log->dw.RL1Em = log->nast_.ZdistA->Zdist_dw->Z.R[0]; + log->dw.RL2Em = log->nast_.ZdistA->Zdist_dw->Z.R[1]; + log->dw.RL3Em = log->nast_.ZdistA->Zdist_dw->Z.R[2]; + log->dw.XL1Em = log->nast_.ZdistA->Zdist_dw->Z.X[0]; + log->dw.XL2Em = log->nast_.ZdistA->Zdist_dw->Z.X[1]; + log->dw.XL3Em = log->nast_.ZdistA->Zdist_dw->Z.X[2]; + } else { + log->dw.RL1Em = log->nast_.ZdistA->Zdist_dw->Z.R[3]; + log->dw.RL2Em = log->nast_.ZdistA->Zdist_dw->Z.R[4]; + log->dw.RL3Em = log->nast_.ZdistA->Zdist_dw->Z.R[5]; + log->dw.XL1Em = log->nast_.ZdistA->Zdist_dw->Z.X[3]; + log->dw.XL2Em = log->nast_.ZdistA->Zdist_dw->Z.X[4]; + log->dw.XL3Em = log->nast_.ZdistA->Zdist_dw->Z.X[5]; + } + log->dw.RL1L2m = log->nast_.ZdistA->Zdist_dw->Z.R[6]; + log->dw.RL2L3m = log->nast_.ZdistA->Zdist_dw->Z.R[7]; + log->dw.RL3L1m = log->nast_.ZdistA->Zdist_dw->Z.R[8]; + log->dw.XL1L2m = log->nast_.ZdistA->Zdist_dw->Z.X[6]; + log->dw.XL2L3m = log->nast_.ZdistA->Zdist_dw->Z.X[7]; + log->dw.XL3L1m = log->nast_.ZdistA->Zdist_dw->Z.X[8]; + + //log->dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (log->dw.P) + { + log->dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!log->dw.P) + { + log->dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + log->dw.dl_++; + } + break; + case zapis: + if (!log->dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + } + + } else { + + } + +} diff --git a/src/ZDistS.h b/src/ZDistS.h new file mode 100644 index 0000000..fc178d5 --- /dev/null +++ b/src/ZDistS.h @@ -0,0 +1,185 @@ +/* + * ZDistL.h + * + * + * Created on: 16-03-2017 + * Author: KJ + */ + +#ifndef ZDISTS_H_ +#define ZDISTS_H_ + +#include "tdefs.h" +#include "helper.h" + + struct Nastawy_przeliczone_ZDistS + { + struct Wyjscia_ZDistA *ZdistA; + + unsigned short ZZw_st; ///< strefa wspó³pracy z automatyk¹ za³¹czenia na zwarcie + unsigned short adr_PZZw;///< adres zmiennej pobudzenia strefy wspó³pracujacej z automatyk¹ ZZw + unsigned short adr_ZZZw;///< adres zmiennej zadzia³ania strefy wspó³pracujacej z automatyk¹ ZZw + + short t_stf[5][2]; ///< czas dzialania strefy + + + float ZZw_ta;///< Czas dzia³ania funkcji po zamkniêciu wy³¹cznika + float ZZw_Ir;///< Pr¹d identyfikacji otwarcia wy³¹cznika + float ZZw_Ur;///< Napiêcie identyfikacji otwarcia wy³acznika + short ZZw_to;///< Czas identyfikacji otwarcia wy³¹cznika + + u8 Bl_PS[5]; + u8 Zbz[5][2]; ///< zadzia³anie bezzwloczne strefy + u8 Stf_ON[5]; ///< wlaczenie/odstawienie strefy + u8 Stf_W[5]; ///< dzia³anie na wylaczenie/sygnalizacje strefy + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 ZZw_ON; ///< w³¹czenie automatyki za³¹czenia na zwarcie + }; + + + struct dane_wewnetrzne_ZDistS + { + float RL1Em; ///< Rezystancja pêtli zwarciowej fazy L1 z ziemia (rejestracja) + float XL1Em; ///< Rezaktancja pêtli zwarciowej fazy L1 z ziemia (rejestracja) + float RL2Em; ///< Rezystancja pêtli zwarciowej fazy L2 z ziemia (rejestracja) + float XL2Em; ///< Rezaktancja pêtli zwarciowej fazy L2 z ziemia (rejestracja) + float RL3Em; ///< Rezystancja pêtli zwarciowej fazy L3 z ziemia (rejestracja) + float XL3Em; ///< Rezaktancja pêtli zwarciowej fazy L3 z ziemia (rejestracja) + float RL1L2m; ///< Rezystancja pêtli zwarciowej fazy L1 z L2 (rejestracja) + float XL1L2m; ///< Rezaktancja pêtli zwarciowej fazy L1 z L2 (rejestracja) + float RL2L3m; ///< Rezystancja pêtli zwarciowej fazy L2 z L3 (rejestracja) + float XL2L3m; ///< Rezaktancja pêtli zwarciowej fazy L2 z L3 (rejestracja) + float RL3L1m; ///< Rezystancja pêtli zwarciowej fazy L3 z L1 (rejestracja) + float XL3L1m; ///< Rezaktancja pêtli zwarciowej fazy L3 z L1 (rejestracja) + short liczts[5][2]; /// + +#include "an_gen.h" +#include "analog_in.h" +#include "dev_ctrl.h" +#include "elements.h" +#include "config.h" +#include + +//u16 bus_an_samples_buf_force[MAX_AN_CARDS][4][4]; + +#define SAMPLE_STEP (50.0f/(SAMPLES_PER_MS*1000.0f)) + +struct an_gen_force_params genpar; +u8 an_gen_buf_ready = 0; + +int an_gen_initlog(void *arguments, void *logic) +{ + return 0; +} + +void an_gen(void *arguments, void *logic) +{ + int i,j; + u8 an_gen_sample_num; + + if(!(dev_ctrl_state & DEV_CTRL_STATE_TEST_AN) || !genpar.an_force_mask) + { + an_gen_buf_ready=0; + return; + } + + UInt key = Hwi_disable(); + + an_gen_sample_num=bus_an_cur_sample_num+(SAMPLES_PER_MS*LOOP_CYCLE_MS); + an_gen_sample_num%=(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + + for(j=0;j<(SAMPLES_PER_MS*LOOP_CYCLE_MS);j++) + { + for(i=0;i<=(MAX_AN_CARDS/2 * 4);i++) + if(genpar.an_force_mask & (1<>2][i%4][an_gen_sample_num] = (u16)(genpar.an_force_amp[i/4][i%4] * sinf(2 * PI * SAMPLE_STEP * (bus_an_cur_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS)+j) + genpar.an_force_ph[i/4][i%4]) + 32768); + + an_gen_sample_num++; + an_gen_sample_num%=(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + } + + an_gen_buf_ready=1; + Hwi_restore(key); +} diff --git a/src/an_gen.h b/src/an_gen.h new file mode 100644 index 0000000..4f1a2a2 --- /dev/null +++ b/src/an_gen.h @@ -0,0 +1,50 @@ +/* + * an_gen.h + * + * + * Created on: 19-07-2017 + * Author: KJ + */ + +#ifndef AN_GEN_H_ +#define AN_GEN_H_ + +#include "tdefs.h" +#include "misc.h" +#include "an_gen.h" + +struct an_gen_logic +{ +// u16 an_count; +// u16 element_num[]; +}; + +struct an_gen_io +{ +}__attribute__((__packed__)); + +struct an_gen_params +{ +}__attribute__((__packed__)); + +struct an_gen_args +{ + struct an_gen_io io; + struct an_gen_params params; +// u16 crc; +}__attribute__((__packed__)); + +struct an_gen_force_params +{ + u32 an_force_mask; + u16 an_force_amp[MAX_AN_CARDS/2][4]; + float an_force_ph[MAX_AN_CARDS/2][4]; +} __attribute__((__packed__)); + +extern struct an_gen_force_params genpar; +extern u8 an_gen_buf_ready; + +extern int an_gen_initlog(void *args, void *logic); +extern void an_gen(void *args, void *logic); + +#endif /* AN_GEN_H_ */ diff --git a/src/analog_in.c b/src/analog_in.c new file mode 100644 index 0000000..67b913c --- /dev/null +++ b/src/analog_in.c @@ -0,0 +1,90 @@ +/* + * analog_in.c + * + * Created on: 06-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "analog_in.h" + +int analog_in_initlog(void *arguments, void *logic) +{ + struct analog_in_args *args = (struct analog_in_args *)arguments; + struct analog_in_logic *log = (struct analog_in_logic *)logic; + u32 *params_ptr; + u32 *samples_buf_ptr; + + log->kob_ptr = log_manager.nets_data + (args->io.kob_out >> 3); + log->kob_bit_no = args->io.kob_out & 0x07; + + if(log->kob_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + log->cur_sample_ptr = (float *)(log_manager.nets_data + (args->io.sample_float_out >> 3)); + if((u8*)log->cur_sample_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(float))) + return -1; + + log->raw_sample_ptr = (short *)(log_manager.nets_data + (args->io.sample_raw_short_out >> 3)); + if((u8*)log->raw_sample_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(short))) + return -1; + + params_ptr = (u32 *)(log_manager.nets_data+(args->io.params_ptr_out>>3)); + + if((u8 *)params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(void *)) ) + return -1; + + *params_ptr = (u32)&log->params_ext;//args->params; + + if(args->params.card_addr>(MAX_AN_CARDS-1)) + return -1; + + if(args->params.phase_num>3) + return -1; + + samples_buf_ptr = (u32 *)(log_manager.nets_data+(args->io.samples_buf_ptr_out>>3)); + + if((u8 *)samples_buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(void *)) ) + return -1; + + *samples_buf_ptr = (u32)&bus_an_samples_buf[args->params.card_addr][args->params.phase_num][0]; + + if(args->params.bits & ANALOG_NEGATION) + bus_an_samples_neg[args->params.card_addr][args->params.phase_num]=1; + else + bus_an_samples_neg[args->params.card_addr][args->params.phase_num]=0; + + log->params_ext.bits=args->params.bits; + log->params_ext.card_addr=args->params.card_addr; + log->params_ext.jednostka=args->params.jednostka; + log->params_ext.multiplier=args->params.multiplier; + log->params_ext.offset=args->params.offset; + log->params_ext.phase_num=args->params.phase_num; + log->params_ext.znam_wtor=args->params.znam_wtor; + log->params_ext.znam_pierw=args->params.znam_pierw; + log->params_ext.nast.offset=args->params.offset; + log->params_ext.nast.mnoznik=args->params.multiplier; + //nast.offset = n.korekcja_offsetu * 32767 / n.zasieg / 1.4142136f; + //nast.mnoznik = 1.4142136f * n.korekcja_ampl * n.zasieg / 32767; + + return 0; +} + +void analog_in(void *arguments, void *logic) +{ + struct analog_in_args *args = (struct analog_in_args *)arguments; + struct analog_in_logic *log = (struct analog_in_logic *)logic; + //u16 sample; + short sample; + + if(ic->kob_an & (1<params.card_addr)) + *log->kob_ptr|=(1<kob_bit_no); + else + *log->kob_ptr&=~(1<kob_bit_no); + + sample = bus_an_samples_buf[args->params.card_addr][args->params.phase_num][bus_an_cur_sample_num] - 0x7FFF; + *log->cur_sample_ptr=((float)sample - args->params.offset ) * args->params.multiplier; + *log->raw_sample_ptr=sample; +} diff --git a/src/analog_in.h b/src/analog_in.h new file mode 100644 index 0000000..275e84f --- /dev/null +++ b/src/analog_in.h @@ -0,0 +1,77 @@ +/* + * analog_in.h + * + * Created on: 06-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef ANALOG_IN_H_ +#define ANALOG_IN_H_ + +#include "tdefs.h" + + struct Nastawa_przeliczona_analog_in + { + float mnoznik;///< stala przeliczenia int na float z korekcja wzmocnienia + float offset; ///< wartosc korekcji przesuniecia skladowej stalej + }; + + struct analog_in_params_extended + { + u32 card_addr; + u32 phase_num; + double offset; + double multiplier; + double znam_wtor; ///< watosc znamionowa wtorna + double znam_pierw; ///< wartosc znamionowa pierwotna + long jednostka; + u32 bits; + struct Nastawa_przeliczona_analog_in nast; + }__attribute__((__packed__)); + +struct analog_in_logic +{ + u8 *kob_ptr; + u8 kob_bit_no; + float *cur_sample_ptr; + short *raw_sample_ptr; + struct analog_in_params_extended params_ext; +}; + +struct analog_in_io +{ + u32 sample_float_out; + u32 samples_buf_ptr_out; + u32 params_ptr_out; + u32 sample_raw_short_out; + u32 kob_out; +}__attribute__((__packed__)); + + +struct analog_in_params +{ + u32 card_addr; + u32 phase_num; + double offset; + double multiplier; + double znam_wtor; ///< watosc znamionowa wtorna + double znam_pierw; ///< wartosc znamionowa pierwotna + long jednostka; + u32 bits; +}__attribute__((__packed__)); + + +//bits +#define ANALOG_NEGATION 0x01 + +struct analog_in_args +{ + struct analog_in_io io; + struct analog_in_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void analog_in(void *args, void *logic); +extern int analog_in_initlog(void *arguments, void *logic); + +#endif /* ANALOG_IN_H_ */ diff --git a/src/and.c b/src/and.c new file mode 100644 index 0000000..7569a94 --- /dev/null +++ b/src/and.c @@ -0,0 +1,49 @@ +/* + * nand.c + * + * Created on: 29-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "and.h" + +int and_initlog(void *arguments, void *logic) +{ + struct and_args *args = (struct and_args *)arguments; + struct and_logic *log = (struct and_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_mask = 1<<(args->io.out & 0x07); + + log->in1_ptr = log_manager.nets_data + (args->io.in1 >> 3); + log->in1_bit_mask = 1<<(args->io.in1 & 0x07); + + log->in2_ptr = log_manager.nets_data + (args->io.in2 >> 3); + log->in2_bit_mask = 1<<(args->io.in2 & 0x07); + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in1_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in2_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void and(void *arguments, void *logic) +{ +// struct and_args *args = (struct and_args *)arguments; + struct and_logic *log = (struct and_logic *)logic; + + if((*log->in1_ptr & log->in1_bit_mask) && (*log->in2_ptr & log->in2_bit_mask)) + *log->out_ptr|=log->out_bit_mask; + else + *log->out_ptr&=~log->out_bit_mask; +} diff --git a/src/and.h b/src/and.h new file mode 100644 index 0000000..70782cd --- /dev/null +++ b/src/and.h @@ -0,0 +1,46 @@ +/* + * nand.h + * + * Created on: 29-04-2014 + * Author: KJ + */ + +#ifndef AND_H_ +#define AND_H_ + +#include "tdefs.h" + +struct and_logic +{ + u8 *out_ptr; + u8 out_bit_mask; + + u8 *in1_ptr; + u8 in1_bit_mask; + + u8 *in2_ptr; + u8 in2_bit_mask; +}; + +struct and_io +{ + u32 in1; + u32 in2; + u32 out; +}__attribute__((__packed__)); + +struct and_params +{ +}__attribute__((__packed__)); + +struct and_args +{ + struct and_io io; + struct and_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void and(void *args, void *logic); +extern int and_initlog(void *args, void *logic); + +#endif /* AND_H_ */ diff --git a/src/and4.c b/src/and4.c new file mode 100644 index 0000000..46fac22 --- /dev/null +++ b/src/and4.c @@ -0,0 +1,46 @@ +/* + * and4.c + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "and4.h" + +int and4_initlog(void *arguments, void *logic) +{ + struct and4_args *args = (struct and4_args *)arguments; + struct and4_logic *log = (struct and4_logic *)logic; + + if(set_bit_ptr_mask(args->io.in1_in,&log->in[0],&log->in_bit_mask[0])) + return -1; + if(set_bit_ptr_mask(args->io.in2_in,&log->in[1],&log->in_bit_mask[1])) + return -1; + if(set_bit_ptr_mask(args->io.in3_in,&log->in[2],&log->in_bit_mask[2])) + return -1; + if(set_bit_ptr_mask(args->io.in4_in,&log->in[3],&log->in_bit_mask[3])) + return -1; + if(set_bit_ptr_mask(args->io.out_out,&log->out,&log->out_bit_mask)) + return -1; + + + return 0; +} + +void and4(void *arguments, void *logic) +{ + struct and4_logic *log = (struct and4_logic *)logic; + u8 i; + + for(i=0;i<4;i++) + if(!(*log->in[i] & log->in_bit_mask[i])) + break; + + check_and_set_mask(i>=4,log->out,log->out_bit_mask); +} diff --git a/src/and4.h b/src/and4.h new file mode 100644 index 0000000..7a21535 --- /dev/null +++ b/src/and4.h @@ -0,0 +1,45 @@ +/* + * and4.h + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef AND4_H_ +#define AND4_H_ + +#include "tdefs.h" + +struct and4_logic +{ + u8 *in[4]; + u8 in_bit_mask[4]; + + u8 *out; + u8 out_bit_mask; +}; + +struct and4_io +{ + u32 in1_in; + u32 in2_in; + u32 in3_in; + u32 in4_in; + u32 out_out; +}__attribute__((__packed__)); + +struct and4_params +{ +}__attribute__((__packed__)); + +struct and4_args +{ + struct and4_io io; + struct and4_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void and4(void *args, void *logic); +extern int and4_initlog(void *arguments, void *logic); + +#endif /* AND4_H_ */ diff --git a/src/and8.c b/src/and8.c new file mode 100644 index 0000000..f1387e6 --- /dev/null +++ b/src/and8.c @@ -0,0 +1,54 @@ +/* + * and8.c + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "and8.h" + +int and8_initlog(void *arguments, void *logic) +{ + struct and8_args *args = (struct and8_args *)arguments; + struct and8_logic *log = (struct and8_logic *)logic; + + if(set_bit_ptr_mask(args->io.in1_in,&log->in[0],&log->in_bit_mask[0])) + return -1; + if(set_bit_ptr_mask(args->io.in2_in,&log->in[1],&log->in_bit_mask[1])) + return -1; + if(set_bit_ptr_mask(args->io.in3_in,&log->in[2],&log->in_bit_mask[2])) + return -1; + if(set_bit_ptr_mask(args->io.in4_in,&log->in[3],&log->in_bit_mask[3])) + return -1; + if(set_bit_ptr_mask(args->io.in5_in,&log->in[4],&log->in_bit_mask[4])) + return -1; + if(set_bit_ptr_mask(args->io.in6_in,&log->in[5],&log->in_bit_mask[5])) + return -1; + if(set_bit_ptr_mask(args->io.in7_in,&log->in[6],&log->in_bit_mask[6])) + return -1; + if(set_bit_ptr_mask(args->io.in8_in,&log->in[7],&log->in_bit_mask[7])) + return -1; + if(set_bit_ptr_mask(args->io.out_out,&log->out,&log->out_bit_mask)) + return -1; + + + return 0; +} + +void and8(void *arguments, void *logic) +{ + struct and8_logic *log = (struct and8_logic *)logic; + u8 i; + + for(i=0;i<8;i++) + if(!(*log->in[i] & log->in_bit_mask[i])) + break; + + check_and_set_mask(i>=8,log->out,log->out_bit_mask); +} diff --git a/src/and8.h b/src/and8.h new file mode 100644 index 0000000..1854eab --- /dev/null +++ b/src/and8.h @@ -0,0 +1,49 @@ +/* + * and8.h + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef AND8_H_ +#define AND8_H_ + +#include "tdefs.h" + +struct and8_logic +{ + u8 *in[8]; + u8 in_bit_mask[8]; + + u8 *out; + u8 out_bit_mask; +}; + +struct and8_io +{ + u32 in1_in; + u32 in2_in; + u32 in3_in; + u32 in4_in; + u32 in5_in; + u32 in6_in; + u32 in7_in; + u32 in8_in; + u32 out_out; +}__attribute__((__packed__)); + +struct and8_params +{ +}__attribute__((__packed__)); + +struct and8_args +{ + struct and8_io io; + struct and8_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void and8(void *args, void *logic); +extern int and8_initlog(void *arguments, void *logic); + +#endif /* AND8_H_ */ diff --git a/src/bank_sel.c b/src/bank_sel.c new file mode 100644 index 0000000..a2bdade --- /dev/null +++ b/src/bank_sel.c @@ -0,0 +1,98 @@ +/* + * bank_sel.c + * + * Created on: 13-12-2016 + * Author: KJ + */ + + + +#include "tdefs.h" +#include "misc.h" +#include "helper.h" +#include "bank_sel.h" +#include "comm.h" +#include "config.h" + +#include + + +int bank_sel_initlog(void *arguments, void *logic) +{ + struct bank_sel_args *args = (struct bank_sel_args *)arguments; + struct bank_sel_logic *log = (struct bank_sel_logic *)logic; + + if(set_bit_ptr_mask(args->io.bank0_in,&log->bank0_ptr,&log->bank0_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.bank1_in,&log->bank1_ptr,&log->bank1_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.bank2_in,&log->bank2_ptr,&log->bank2_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.bank3_in,&log->bank3_ptr,&log->bank3_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.bank4_in,&log->bank4_ptr,&log->bank4_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.bank5_in,&log->bank5_ptr,&log->bank5_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.blok_in,&log->blok_in,&log->blok_in_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.bank0_out,&log->bank0_out,&log->bank0_out_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.bank1_out,&log->bank1_out,&log->bank1_out_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.bank2_out,&log->bank2_out,&log->bank2_out_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.bank3_out,&log->bank3_out,&log->bank3_out_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.bank4_out,&log->bank4_out,&log->bank4_out_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.bank5_out,&log->bank5_out,&log->bank5_out_bit_mask)) + return -1; + + return 0; +} + +void bank_sel(void *arguments, void *logic) +{ + struct bank_sel_logic *log = (struct bank_sel_logic *)logic; + + if(!(*log->blok_in & log->blok_in_bit_mask)) + { + if(*log->bank0_ptr & log->bank0_bit_mask) + log->sel_bank=0; + else if(*log->bank1_ptr & log->bank1_bit_mask) + log->sel_bank=1; + else if(*log->bank2_ptr & log->bank2_bit_mask) + log->sel_bank=2; + else if(*log->bank3_ptr & log->bank3_bit_mask) + log->sel_bank=3; + else if(*log->bank4_ptr & log->bank4_bit_mask) + log->sel_bank=4; + else if(*log->bank5_ptr & log->bank5_bit_mask) + log->sel_bank=5; + else + log->sel_bank=0; + } + + check_and_set_mask(log->sel_bank==0,log->bank0_out,log->bank0_out_bit_mask); + check_and_set_mask(log->sel_bank==1,log->bank1_out,log->bank1_out_bit_mask); + check_and_set_mask(log->sel_bank==2,log->bank2_out,log->bank2_out_bit_mask); + check_and_set_mask(log->sel_bank==3,log->bank3_out,log->bank3_out_bit_mask); + check_and_set_mask(log->sel_bank==4,log->bank4_out,log->bank4_out_bit_mask); + check_and_set_mask(log->sel_bank==5,log->bank5_out,log->bank5_out_bit_mask); + + if(log->t_cnt++ > (1200/LOOP_CYCLE_MS)) // every 1.2 secs + { + + logman_notify|=(LOGMAN_NOTIFY_BANK0<sel_bank) | LOGMAN_NOTIFY_NEW_EVENTS; +// Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_SEL_BANK0+log->sel_bank, TRUE); + log->t_cnt=0; + } +} diff --git a/src/bank_sel.h b/src/bank_sel.h new file mode 100644 index 0000000..c013102 --- /dev/null +++ b/src/bank_sel.h @@ -0,0 +1,79 @@ +/* + * bank_sel.h + * + * + * Created on: 13-12-2016 + * Author: KJ + */ + +#ifndef BANK_SEL_H_ +#define BANK_SEL_H_ + +#include "tdefs.h" + + +struct bank_sel_logic +{ + u8 *bank0_ptr; + u8 bank0_bit_mask; + u8 *bank1_ptr; + u8 bank1_bit_mask; + u8 *bank2_ptr; + u8 bank2_bit_mask; + u8 *bank3_ptr; + u8 bank3_bit_mask; + u8 *bank4_ptr; + u8 bank4_bit_mask; + u8 *bank5_ptr; + u8 bank5_bit_mask; + + u8 *blok_in; + u8 blok_in_bit_mask; + u8 *bank0_out; + u8 bank0_out_bit_mask; + u8 *bank1_out; + u8 bank1_out_bit_mask; + u8 *bank2_out; + u8 bank2_out_bit_mask; + u8 *bank3_out; + u8 bank3_out_bit_mask; + u8 *bank4_out; + u8 bank4_out_bit_mask; + u8 *bank5_out; + u8 bank5_out_bit_mask; + u32 t_cnt; + u32 sel_bank; +}; + +struct bank_sel_io +{ + u32 bank0_in; + u32 bank1_in; + u32 bank2_in; + u32 bank3_in; + u32 bank4_in; + u32 bank5_in; + u32 blok_in; + u32 bank0_out; + u32 bank1_out; + u32 bank2_out; + u32 bank3_out; + u32 bank4_out; + u32 bank5_out; +}__attribute__((__packed__)); + +struct bank_sel_params +{ +}__attribute__((__packed__)); + +struct bank_sel_args +{ + struct bank_sel_io io; + struct bank_sel_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void bank_sel(void *args, void *logic); +extern int bank_sel_initlog(void *args, void *logic); + +#endif /* BANK_SEL_H_ */ diff --git a/src/bin_in.c b/src/bin_in.c new file mode 100644 index 0000000..510c995 --- /dev/null +++ b/src/bin_in.c @@ -0,0 +1,97 @@ +/* + * bin_in.c + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "bin_in.h" + +int bin_in_initlog(void *arguments, void *logic) +{ + struct bin_in_args *args = (struct bin_in_args *)arguments; + struct bin_in_logic *log = (struct bin_in_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_mask = 1<<(args->io.out & 0x07); + log->kob_ptr = log_manager.nets_data + (args->io.kob_out >> 3); + log->kob_bit_mask = 1<<(args->io.kob_out & 0x07); + + if(args->params.card_addr>=MAX_BIN_CARDS) + { + if(args->params.ac_filter) + ic->bin_ac_mask_ench[args->params.card_addr-16]|=(1<params.in_num); + else + ic->bin_ac_mask_ench[args->params.card_addr-16]&=~(1<params.in_num); + } + else + { + if(args->params.ac_filter) + ic->bin_ac_mask[args->params.card_addr]|=(1<params.in_num); + else + ic->bin_ac_mask[args->params.card_addr]&=~(1<params.in_num); + } + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->kob_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(args->params.card_addr>=(MAX_BIN_CARDS * 2)) // *2 bo mwd32 + return -1; + + log->in_num_mask=(1<params.in_num); + log->card_addr_mask=(1<params.card_addr); + + if(args->params.card_addr>=16) + { + mwd32_mask|=1<<(args->params.card_addr&0x07); + ic->mwd32_present=1; + } + + if(args->params.in_num>=8) + return -1; + + return 0; +} + +void bin_in(void *arguments, void *logic) +{ + struct bin_in_args *args = (struct bin_in_args *)arguments; + struct bin_in_logic *log = (struct bin_in_logic *)logic; + + + if(args->params.card_addr>=MAX_BIN_CARDS) // mwd32 + { + if(bus_bin_data_ench[args->params.card_addr-16] & log->in_num_mask) + *log->out_ptr|=log->out_bit_mask; + else + *log->out_ptr&=~log->out_bit_mask; + } + else + { + if(bus_bin_data[args->params.card_addr] & log->in_num_mask) + *log->out_ptr|=log->out_bit_mask; + else + *log->out_ptr&=~log->out_bit_mask; + } + + if(mwd32_mask & (1<<(args->params.card_addr&0x07))) + { + if(ic->kob_bin & (1<<(args->params.card_addr&0x07))) + *log->kob_ptr|=log->kob_bit_mask; + else + *log->kob_ptr&=~log->kob_bit_mask; + } + else + { + if(ic->kob_bin & log->card_addr_mask) + *log->kob_ptr|=log->kob_bit_mask; + else + *log->kob_ptr&=~log->kob_bit_mask; + } +} diff --git a/src/bin_in.h b/src/bin_in.h new file mode 100644 index 0000000..807d33b --- /dev/null +++ b/src/bin_in.h @@ -0,0 +1,46 @@ +/* + * bin_in.h + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef BIN_IN_H_ +#define BIN_IN_H_ + +#include "tdefs.h" + +struct bin_in_logic +{ + u8 *out_ptr; + u8 out_bit_mask; + u8 *kob_ptr; + u8 kob_bit_mask; + u8 in_num_mask; + u16 card_addr_mask; +}; + +struct bin_in_io +{ + u32 out; // one bit output for one binary input + u32 kob_out; +}__attribute__((__packed__)); + +struct bin_in_params +{ + u32 card_addr; + u32 in_num; + u32 ac_filter; +}__attribute__((__packed__)); + +struct bin_in_args +{ + struct bin_in_io io; + struct bin_in_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void bin_in(void *args, void *logic); +extern int bin_in_initlog(void *args, void *logic); + +#endif /* BIN_IN_H_ */ diff --git a/src/buzzer.c b/src/buzzer.c new file mode 100644 index 0000000..c5287f1 --- /dev/null +++ b/src/buzzer.c @@ -0,0 +1,38 @@ +/* + * buzzer.c + * + * Created on: 09-10-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "led.h" + +int buzzer_initlog(void *arguments, void *logic) +{ + struct led_args *args = (struct led_args *)arguments; + struct led_logic *log = (struct led_logic *)logic; + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_no = args->io.in & 0x07; + log->blink_in_ptr = log_manager.nets_data + (args->io.blink_in >> 3); + log->blink_in_bit_no = args->io.blink_in & 0x07; + log->clr_ptr = log_manager.nets_data + (args->io.clr_in >> 3); + log->clr_bit_no = args->io.clr_in & 0x07; + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->clr_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->blink_in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + args->params.led_num=BUZZER_LED_NUM; + + return 0; +} + diff --git a/src/c37_94.c b/src/c37_94.c new file mode 100644 index 0000000..6bbc2a9 --- /dev/null +++ b/src/c37_94.c @@ -0,0 +1,586 @@ +/* + * c37_94.c + * + * Created on: 16-07-2019 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "c37_94.h" +#include "helper.h" +#include "mkstream.h" + +#include + +#include "comm.h" +#include + +volatile struct stream_frame c37_str[C37_MAX_STREAMS][C37_STREAM_BUF_SIZE] __attribute__((aligned(4))); +volatile u8 c37_str_bits[C37_MAX_STREAMS][C37_STREAM_BUF_SIZE] __attribute__((aligned(4))); +volatile u32 c37_str_pos[C37_MAX_STREAMS] __attribute__((aligned(4))); + +int c37_94_initlog(void *arguments, void *logic) +{ + struct c37_94_args *args = (struct c37_94_args *)arguments; + struct c37_94_logic *log = (struct c37_94_logic *)logic; + + if(set_pointer_in_ptr(args->io.stream_ptr_in,(u32 *)&log->stream_in)) + return -1; + + if(set_bit_ptr_struct(args->io.stream_en_in,&log->stream_en)) + return -1; + + if(set_bit_ptr_struct(args->io.no_sync_in,&log->no_sync)) + return -1; + + if(set_bit_ptr_struct(args->io.clr_in,&log->clr)) + return -1; + + if(set_bit_ptr_struct(args->io.ping_in,&log->ping)) + return -1; + + if(set_pointer_out_ptr(args->io.stream0_ptr_out,(u32 *)&log->stream_out[0])) + return -1; + if(set_bit_ptr_struct(args->io.stream0_act_out,&log->stream_act[0])) + return -1; + if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32 *)&log->stream_out[1])) + return -1; + if(set_bit_ptr_struct(args->io.stream1_act_out,&log->stream_act[1])) + return -1; + if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32 *)&log->stream_out[2])) + return -1; + if(set_bit_ptr_struct(args->io.stream2_act_out,&log->stream_act[2])) + return -1; + + if(set_bit_ptr_struct(args->io.err_out,&log->err)) + return -1; + + if(set_bit_ptr_struct(args->io.y_bit_out,&log->y_bit)) + return -1; + + if(set_bit_ptr_struct(args->io.err2_out,&log->err2)) + return -1; + + if(set_bit_ptr_struct(args->io.y2_bit_out,&log->y_bit2)) + return -1; + + if(set_float_ptr(args->io.stream0_errs_float_out,&log->stream_errs[0])) + return -1; + if(set_float_ptr(args->io.stream1_errs_float_out,&log->stream_errs[1])) + return -1; + if(set_float_ptr(args->io.stream2_errs_float_out,&log->stream_errs[2])) + return -1; + + if(set_bit_ptr_struct(args->io.link0_out,&log->link[0])) + return -1; + if(set_bit_ptr_struct(args->io.link1_out,&log->link[1])) + return -1; + if(set_bit_ptr_struct(args->io.link2_out,&log->link[2])) + return -1; + + if(set_float_ptr(args->io.cur_shift_float_out,&log->cur_shift)) + return -1; + + if(set_float_ptr(args->io.cur_shift2_float_out,&log->cur_shift2)) + return -1; + + if(set_float_ptr(args->io.stream0_synt_errs_float_out,&log->stream_synt_errs[0])) + return -1; + if(set_float_ptr(args->io.stream1_synt_errs_float_out,&log->stream_synt_errs[1])) + return -1; + if(set_float_ptr(args->io.stream2_synt_errs_float_out,&log->stream_synt_errs[2])) + return -1; + + if(set_float_ptr(args->io.ping_float_out,&log->ping_time)) + return -1; + + log->wr_bits=((args->params.bits & 0x00000001)?MUX_REG_EXT_CLK:0)|((args->params.mux_chans<=12?args->params.mux_chans:12)<mux_reg=log->wr_bits|MUX_REG_SKIP_FF; + log->prev_addr=ic->mux_state & MUX_STATE_RX_BYTES_MASK; + + // + log->s_delay=args->params.sample_delay/LOOP_CYCLE_MS; + if(args->params.sample_delay%LOOP_CYCLE_MS) + log->s_delay++; + + log->s_delay*=(SAMPLES_PER_MS*LOOP_CYCLE_MS); + + if(log->s_delay>=((C37_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS))) + log->s_delay=(C37_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS); + // + + log->wr_addr=1; + + ic->mux_shift=0; + + // mux2 + log->wr_bits2=((args->params.bits & MUX2_EXT_CLK)?MUX_REG_EXT_CLK:0)|((args->params.mux_chans<=12?args->params.mux_chans:12)<mux2_reg=log->wr_bits2|MUX_REG_SKIP_FF; + log->prev_addr2=ic->mux2_state & MUX_STATE_RX_BYTES_MASK; + log->wr_addr2=1; + ic->mux2_shift=0; + + log->my_mask = (args->params.serial_num<<2)&C37_SERIAL_MASK; + return 0; +} + + +void c37_94(void *arguments, void *logic) +{ + struct c37_94_logic *log = (struct c37_94_logic *)logic; + struct c37_94_args *args = (struct c37_94_args *)arguments; + int i,rx_bytes; + u8 j,k; + u8 cksum; + u16 find_stamp; + u16 cur_addr; + u16 mux_offset; + u8 already_incremented[C37_MAX_STREAMS]={0,0,0}; + u8 need_pong=0; + + static struct stream_frame tmp_frame; + + + if(check_struct(&log->clr)) + { + for(i=0;istream_errs[i]=0; + *log->stream_synt_errs[i]=0; + } + } + + for(i=0;iprev_pos[i]!=c37_str_pos[i]) + { + if(log->link_up_cnt[i]>LINK_UP_TIME) + { + if(!check_struct(&log->link[i])) + *log->stream_errs[i]=0; + set_struct(&log->link[i]); + } + else + log->link_up_cnt[i]+=LOOP_CYCLE_MS; + + while(log->prev_pos[i]!=c37_str_pos[i]) + { + u16 prev_tstamp=(!log->prev_pos[i]?c37_str[i][C37_STREAM_BUF_SIZE-1].sample_stamp:c37_str[i][log->prev_pos[i]-1].sample_stamp) & 0x7FFF; + u16 next_tstamp=(c37_str[i][log->prev_pos[i]].sample_stamp)&0x7FFF; + + if((prev_tstamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=next_tstamp) + { + if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp) + *log->stream_errs[i]+=1.0f; + already_incremented[i]=1; + } + log->prev_pos[i]++; + log->prev_pos[i]%=C37_STREAM_BUF_SIZE; + } + + log->link_timeout_cnt[i]=0; + log->prev_pos[i]=c37_str_pos[i]; + } + else + { + if(log->link_timeout_cnt[i]>LINK_TIMEOUT_TIME) + { + clear_struct(&log->link[i]); + log->link_up_cnt[i]=0; + } + else + { + if(log->link_timeout_cnt[i]>(1*LOOP_CYCLE_MS)) + { + if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp) + *log->stream_errs[i]+=1.0f; + already_incremented[i]=1; + } + log->link_timeout_cnt[i]+=LOOP_CYCLE_MS; + } + } + } + + + check_and_set_struct(ic->mux_state & MUX_STATE_ERR,&log->err); + check_and_set_struct(ic->mux2_state & MUX_STATE_ERR,&log->err2); + + cur_addr = ic->mux_state & MUX_STATE_RX_BYTES_MASK; + rx_bytes = cur_addr>=log->prev_addr ? cur_addr-log->prev_addr : C37_MUX_BUFSIZE + cur_addr - log->prev_addr; + mux_offset = log->prev_addr; +/* + if(check_struct(&log->shift_out)) + { + log->out_shift_cnt++; + log->out_shift_cnt&=0x07; + ic->mux_shift=(ic->mux_shift & 0x0F) | (log->out_shift_cnt<<4); + } +*/ + +// MUX1 + if(!(ic->mux_state & MUX_STATE_ERR)) + { + log->no_data_cnt+=LOOP_CYCLE_MS; + + if(log->no_data_cnt > (C37_NO_DATA_SHIFT_INTERVAL)) + { + log->no_data_cnt=0; + log->shift_cnt++; + log->shift_cnt&=0x07; + ic->mux_shift=(ic->mux_shift & 0xF0)|log->shift_cnt; + *log->cur_shift=log->shift_cnt; + } + } + else + log->no_data_cnt=0; + + while(rx_bytes >= C37_FRAME_SIZE) + { + if(ic->mux_indata[mux_offset]!='T' || ic->mux_indata[(mux_offset+1)%C37_MUX_BUFSIZE]!='Z') + { + rx_bytes--; + mux_offset++; + mux_offset%=C37_MUX_BUFSIZE; + continue; + } + + if(mux_offset + C37_FRAME_SIZE > C37_MUX_BUFSIZE) + { + memcpy((u8*)&tmp_frame,(u8*)&ic->mux_indata[mux_offset],C37_MUX_BUFSIZE - mux_offset); + memcpy((u8*)&tmp_frame+(C37_MUX_BUFSIZE-mux_offset),(u8*)&ic->mux_indata[0],C37_FRAME_SIZE-(C37_MUX_BUFSIZE-mux_offset)); + } + else + { + memcpy((u8*)&tmp_frame,(u8*)&ic->mux_indata[mux_offset],C37_FRAME_SIZE); + } + + cksum=CKSUM_IV; + for(j=0;jmy_mask)) + { + if(tmp_frame.addr & 0x80) + need_pong=1; + if(tmp_frame.addr & 0x40 && log->ping_sent) + { + *log->ping_time=log->ping_cnt; + log->ping_sent=0; + } + tmp_frame.addr&=0x03; + log->no_data_cnt=0; + + if(tmp_frame.addr cksum_err+=1.0; + //else odjac tylko 2 bajty TZ i szukac kolejnego? + + rx_bytes-=C37_FRAME_SIZE; + mux_offset+=C37_FRAME_SIZE; + mux_offset%=C37_MUX_BUFSIZE; + } + + log->prev_addr=mux_offset; + +// *log->hw_err=(float)((ic->mux_state & MUX_STATE_ERR_CNT_MASK)>>MUX_STATE_ERR_CNT_OFFSET); + check_and_set_struct(ic->mux_state & MUX_STATE_YELLOW_BIT_IN, &log->y_bit); + +//MUX2 + if(args->params.bits & MUX2_ACTIVE) + { + cur_addr = ic->mux2_state & MUX_STATE_RX_BYTES_MASK; + rx_bytes = cur_addr>=log->prev_addr2 ? cur_addr-log->prev_addr2 : C37_MUX_BUFSIZE + cur_addr - log->prev_addr2; + mux_offset = log->prev_addr2; + + if(!(ic->mux2_state & MUX_STATE_ERR)) + { + log->no_data_cnt2+=LOOP_CYCLE_MS; + + if(log->no_data_cnt2 > (C37_NO_DATA_SHIFT_INTERVAL)) + { + log->no_data_cnt2=0; + log->shift_cnt2++; + log->shift_cnt2&=0x07; + ic->mux2_shift=(ic->mux2_shift & 0xF0)|log->shift_cnt2; + *log->cur_shift2=log->shift_cnt2; + } + } + else + log->no_data_cnt2=0; + + while(rx_bytes >= C37_FRAME_SIZE) + { + if(ic->mux2_indata[mux_offset]!='T' || ic->mux2_indata[(mux_offset+1)%C37_MUX_BUFSIZE]!='Z') + { + rx_bytes--; + mux_offset++; + mux_offset%=C37_MUX_BUFSIZE; + continue; + } + + if(mux_offset + C37_FRAME_SIZE > C37_MUX_BUFSIZE) + { + memcpy((u8*)&tmp_frame,(u8*)&ic->mux2_indata[mux_offset],C37_MUX_BUFSIZE - mux_offset); + memcpy((u8*)&tmp_frame+(C37_MUX_BUFSIZE-mux_offset),(u8*)&ic->mux2_indata[0],C37_FRAME_SIZE-(C37_MUX_BUFSIZE-mux_offset)); + } + else + { + memcpy((u8*)&tmp_frame,(u8*)&ic->mux2_indata[mux_offset],C37_FRAME_SIZE); + } + + cksum=CKSUM_IV; + for(j=0;jmy_mask)) + { + if(tmp_frame.addr & 0x80) + need_pong=1; + if(tmp_frame.addr & 0x40 && log->ping_sent) + { + *log->ping_time=log->ping_cnt; + log->ping_sent=0; + } + tmp_frame.addr&=0x03; + log->no_data_cnt2=0; + + if(tmp_frame.addr cksum_err+=1.0; + //else odjac tylko 2 bajty TZ i szukac kolejnego? + + rx_bytes-=C37_FRAME_SIZE; + mux_offset+=C37_FRAME_SIZE; + mux_offset%=C37_MUX_BUFSIZE; + } + + log->prev_addr2=mux_offset; + + // *log->hw_err=(float)((ic->mux_state & MUX_STATE_ERR_CNT_MASK)>>MUX_STATE_ERR_CNT_OFFSET); + check_and_set_struct(ic->mux2_state & MUX_STATE_YELLOW_BIT_IN, &log->y_bit2); + } + // + + if(bus_an_cur_timestamp>=log->s_delay) + find_stamp=bus_an_cur_timestamp-log->s_delay; + else + find_stamp=(SAMPLES_PER_MS*1000)+bus_an_cur_timestamp-log->s_delay; + + + for(i=0;ino_sync)) + { + if(!check_struct(&log->link[i])) + { + clear_struct(&log->stream_act[i]); + continue; + } + + for(k=0;kstream_out[i] = c37_str[i][k]; +// *log->stream_snum[i] = c37_str[i][k].sample_stamp; + break; + } + } + + if(kstream_act[i]); + else + { + if(!already_incremented[i]) + *log->stream_synt_errs[i]+=1.0f; + //*log->stream_errs[i]+=1.0f; + clear_struct(&log->stream_act[i]); + } + } + else + { + if(args->params.bits & 0x00000002) // no buf + { + if(!(c37_str_bits[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE] & STATUS_FRAME_READ)) + { + c37_str_bits[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE]|=STATUS_FRAME_READ; + log->stream_out[i] = c37_str[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE]; +// *log->stream_snum[i] = c37_str[i][(c37_str_pos[i]+C37_STREAM_BUF_SIZE-1) % C37_STREAM_BUF_SIZE].sample_stamp; + break; + } + } + else + { + for(k=0;kstream_out[i] = c37_str[i][(c37_str_pos[i]+k) % C37_STREAM_BUF_SIZE]; +// *log->stream_snum[i] = c37_str[i][(c37_str_pos[i]+k) % C37_STREAM_BUF_SIZE].sample_stamp; + break; + } + } + + if(kstream_act[i]); + else + { + if(!already_incremented[i]) + *log->stream_synt_errs[i]+=1.0f; + //*log->stream_errs[i]+=1.0f; + clear_struct(&log->stream_act[i]); + } + } + + } + + } + + // + log->prev_timestamp=bus_an_cur_timestamp; + if(log->stream_in->addrstream_in->addr][c37_str_pos[log->stream_in->addr]],(u8*)log->stream_in,C37_FRAME_SIZE); + c37_str_bits[log->stream_in->addr][c37_str_pos[log->stream_in->addr]++]=0; + c37_str_pos[log->stream_in->addr]%=C37_STREAM_BUF_SIZE; + + if(check_struct(&log->stream_en)) + { + // + log->stream_in->addr|=log->my_mask; + log->stream_in->cksum+=log->my_mask; + + if(check_struct(&log->ping) && log->ping_cnt==0) + { + log->stream_in->addr|=0x80; // ping req + log->stream_in->cksum+=0x80; + log->ping_sent=1; + } + if(need_pong) + { + log->stream_in->addr|=0x40; + log->stream_in->cksum+=0x40; + } + + if(log->wr_addr + C37_FRAME_SIZE > C37_MUX_BUFSIZE) + { + memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr); + memcpy((u8*)ic->mux_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr)); + } + else + memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_FRAME_SIZE); + + log->wr_addr+=C37_FRAME_SIZE-1; + log->wr_addr%=C37_MUX_BUFSIZE; + ic->mux_reg=log->wr_bits|MUX_REG_WR|log->wr_addr; + log->wr_addr++; + log->wr_addr%=C37_MUX_BUFSIZE; + // + //mux2 + if(args->params.bits & MUX2_ACTIVE) + { + if(log->wr_addr2 + C37_FRAME_SIZE > C37_MUX_BUFSIZE) + { + memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr2); + memcpy((u8*)ic->mux2_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr2),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr2)); + } + else + memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_FRAME_SIZE); + + log->wr_addr2+=C37_FRAME_SIZE-1; + log->wr_addr2%=C37_MUX_BUFSIZE; + ic->mux2_reg=log->wr_bits2|MUX_REG_WR|log->wr_addr2; + log->wr_addr2++; + log->wr_addr2%=C37_MUX_BUFSIZE; + } + } + else + { + if(check_struct(&log->ping) && log->ping_cnt==0) + { + memcpy((u8 *)&c37_str[log->stream_in->addr][c37_str_pos[log->stream_in->addr]],(u8*)log->stream_in,C37_FRAME_SIZE); + c37_str_bits[log->stream_in->addr][c37_str_pos[log->stream_in->addr]++]=0; + c37_str_pos[log->stream_in->addr]%=C37_STREAM_BUF_SIZE; + + log->ping_sent=1; + log->stream_in->addr|=0x80; // ping req + log->stream_in->cksum+=0x80; + log->stream_in->addr|=log->my_mask; + log->stream_in->cksum+=log->my_mask; + + if(need_pong) + { + log->stream_in->addr|=0x40; + log->stream_in->cksum+=0x40; + } + + // + if(log->wr_addr + C37_FRAME_SIZE > C37_MUX_BUFSIZE) + { + memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr); + memcpy((u8*)ic->mux_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr)); + } + else + memcpy((u8*)&ic->mux_outdata[log->wr_addr],(u8 *)log->stream_in,C37_FRAME_SIZE); + + log->wr_addr+=C37_FRAME_SIZE-1; + log->wr_addr%=C37_MUX_BUFSIZE; + ic->mux_reg=log->wr_bits|MUX_REG_WR|log->wr_addr; + log->wr_addr++; + log->wr_addr%=C37_MUX_BUFSIZE; + // + // + if(args->params.bits & MUX2_ACTIVE) + { + if(log->wr_addr + C37_FRAME_SIZE > C37_MUX_BUFSIZE) + { + memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_MUX_BUFSIZE - log->wr_addr2); + memcpy((u8*)ic->mux2_outdata,(u8 *)log->stream_in+(C37_MUX_BUFSIZE - log->wr_addr2),C37_FRAME_SIZE - (C37_MUX_BUFSIZE - log->wr_addr2)); + } + else + memcpy((u8*)&ic->mux2_outdata[log->wr_addr2],(u8 *)log->stream_in,C37_FRAME_SIZE); + + log->wr_addr2+=C37_FRAME_SIZE-1; + log->wr_addr2%=C37_MUX_BUFSIZE; + ic->mux2_reg=log->wr_bits2|MUX_REG_WR|log->wr_addr2; + log->wr_addr2++; + log->wr_addr2%=C37_MUX_BUFSIZE; + // + } + } + } + } + + if(log->ping_cnt<5000) + log->ping_cnt+=LOOP_CYCLE_MS; + else + { + if(log->ping_sent) + { + *log->ping_time=999999.0f; + log->ping_sent=0; + } + + log->ping_cnt=0; + } +} diff --git a/src/c37_94.h b/src/c37_94.h new file mode 100644 index 0000000..fb31f47 --- /dev/null +++ b/src/c37_94.h @@ -0,0 +1,126 @@ +/* + * c37_94.h + * + * Created on: 17-07-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef C37_94_H_ +#define C37_94_H_ + +#include "tdefs.h" +#include "helper.h" +#include "mkstream.h" +#include "eth.h" + +#define C37_MAX_STREAMS 3 +#define C37_STREAM_BUF_SIZE 16//8 +#define C37_NUM_OF_CHANS 6 +#define C37_FRAME_SIZE sizeof(struct stream_frame) + +#define C37_NO_DATA_SHIFT_INTERVAL 100 + +#define C37_SERIAL_MASK 0x3c + +extern volatile u8 c37_str_bits[C37_MAX_STREAMS][C37_STREAM_BUF_SIZE]; +//args->params.bits definitions + +#define EXT_CLK 0x00000001 +#define NO_BUF 0x00000002 +#define MUX2_ACTIVE 0x00000004 +#define MUX2_EXT_CLK 0x00000008 + +struct c37_94_logic +{ + struct binary_io stream_en; + struct binary_io no_sync; + struct binary_io clr; + struct binary_io ping; + struct stream_frame *stream_in; + struct stream_frame stream_out[C37_MAX_STREAMS]; + struct binary_io stream_act[C37_MAX_STREAMS]; + struct binary_io link[C37_MAX_STREAMS]; + float *stream_errs[C37_MAX_STREAMS]; + float *stream_synt_errs[C37_MAX_STREAMS]; + struct binary_io err; + struct binary_io y_bit; + struct binary_io err2; + struct binary_io y_bit2; + float *cur_shift; + float *cur_shift2; + float *ping_time; + u16 s_delay; + u16 wr_bits; + u16 wr_bits2; + u16 prev_addr; + u16 prev_addr2; + u16 wr_addr; + u16 wr_addr2; + u8 shift_cnt; + u8 shift_cnt2; + u8 out_shift_cnt; + u16 no_data_cnt; + u16 no_data_cnt2; + u32 prev_pos[C37_MAX_STREAMS]; + u32 link_timeout_cnt[C37_MAX_STREAMS]; + u32 link_up_cnt[C37_MAX_STREAMS]; + u16 prev_timestamp; + u16 ping_cnt; + u16 ping_sent; + u8 my_mask; +}; + +struct c37_94_io +{ + u32 stream_en_in; + u32 stream_ptr_in; + u32 no_sync_in; + u32 clr_in; + u32 ping_in; + + u32 stream0_ptr_out; + u32 stream0_act_out; + u32 link0_out; + u32 stream0_errs_float_out; + + u32 stream1_ptr_out; + u32 stream1_act_out; + u32 link1_out; + u32 stream1_errs_float_out; + + u32 stream2_ptr_out; + u32 stream2_act_out; + u32 link2_out; + u32 stream2_errs_float_out; + u32 err_out; + u32 y_bit_out; + u32 cur_shift_float_out; + u32 err2_out; + u32 y2_bit_out; + u32 cur_shift2_float_out; + u32 stream0_synt_errs_float_out; + u32 stream1_synt_errs_float_out; + u32 stream2_synt_errs_float_out; + u32 ping_float_out; + +}__attribute__((__packed__)); + +struct c37_94_params +{ + u32 bits; + u32 mux_chans; + u32 sample_delay; + u32 serial_num; +}__attribute__((__packed__)); + +struct c37_94_args +{ + struct c37_94_io io; + struct c37_94_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void c37_94(void *args, void *logic); +extern int c37_94_initlog(void *arguments, void *logic); + +#endif diff --git a/src/cap_comp.c b/src/cap_comp.c new file mode 100644 index 0000000..50f5829 --- /dev/null +++ b/src/cap_comp.c @@ -0,0 +1,137 @@ +/* + * cap_comp.c + * + * Created on: 19-08-2019 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "helper.h" +#include "cap_comp.h" +#include "analog_in.h" + +int cap_comp_initlog(void *arguments, void *logic) +{ + struct cap_comp_args *args = (struct cap_comp_args *)arguments; + struct cap_comp_logic *log = (struct cap_comp_logic *)logic; +/* + if(set_float_ptr(args->io.IL1_float_in,&log->I[0])) + return -1; + if(set_float_ptr(args->io.IL2_float_in,&log->I[1])) + return -1; + if(set_float_ptr(args->io.IL3_float_in,&log->I[2])) + return -1; + + if(set_float_ptr(args->io.UL1_float_in,&log->U[0])) + return -1; + if(set_float_ptr(args->io.UL2_float_in,&log->U[1])) + return -1; + if(set_float_ptr(args->io.UL3_float_in,&log->U[2])) + return -1; +*/ + if(set_pointer_in_ptr(args->io.IL1_buf_ptr_in,(u32*)&log->IL_buf[0])) + return -1; + if(set_pointer_in_ptr(args->io.IL2_buf_ptr_in,(u32*)&log->IL_buf[1])) + return -1; + if(set_pointer_in_ptr(args->io.IL3_buf_ptr_in,(u32*)&log->IL_buf[2])) + return -1; + + if(set_pointer_in_ptr(args->io.UL1_buf_ptr_in,(u32*)&log->UL_buf[0])) + return -1; + if(set_pointer_in_ptr(args->io.UL2_buf_ptr_in,(u32*)&log->UL_buf[1])) + return -1; + if(set_pointer_in_ptr(args->io.UL3_buf_ptr_in,(u32*)&log->UL_buf[2])) + return -1; + + if(set_float_ptr(args->io.IL1_comp_float_out,&log->I_comp[0])) + return -1; + if(set_float_ptr(args->io.IL2_comp_float_out,&log->I_comp[1])) + return -1; + if(set_float_ptr(args->io.IL3_comp_float_out,&log->I_comp[2])) + return -1; + + if(set_float_ptr(args->io.IL1_comp_prev_float_out,&log->I_comp_prev[0])) + return -1; + if(set_float_ptr(args->io.IL2_comp_prev_float_out,&log->I_comp_prev[1])) + return -1; + if(set_float_ptr(args->io.IL3_comp_prev_float_out,&log->I_comp_prev[2])) + return -1; + + if(set_pointer_in_ptr(args->io.IL1_params_ptr_in,(u32 *)&log->I_params[0])) + return -1; + if(set_pointer_in_ptr(args->io.IL2_params_ptr_in,(u32 *)&log->I_params[1])) + return -1; + if(set_pointer_in_ptr(args->io.IL3_params_ptr_in,(u32 *)&log->I_params[2])) + return -1; + + if(set_pointer_in_ptr(args->io.UL1_params_ptr_in,(u32 *)&log->U_params[0])) + return -1; + if(set_pointer_in_ptr(args->io.UL2_params_ptr_in,(u32 *)&log->U_params[1])) + return -1; + if(set_pointer_in_ptr(args->io.UL3_params_ptr_in,(u32 *)&log->U_params[2])) + return -1; + + //po staremu log->mul=args->params.k*args->params.c/2.0f*(1 / log->I_params[0]->znam_pierw); + log->mul=args->params.k*args->params.c/2.0f*((log->U_params[0]->znam_pierw/log->U_params[0]->znam_wtor)*(log->I_params[0]->znam_wtor/log->I_params[0]->znam_pierw)); + return 0; +} + +void cap_comp(void *arguments, void *logic) +{ + struct cap_comp_logic *log = (struct cap_comp_logic *)logic; + struct cap_comp_args *args = (struct cap_comp_args *)arguments; + u8 i; + short snum_next = bus_an_cur_sample_num; + short snum_cur,snum_prev; + short snum_next2, snum_cur2,snum_prev2; + + snum_cur = snum_next - 1; + if(snum_cur < 0) + snum_cur = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1; + + snum_prev = snum_cur - 1; + if(snum_prev < 0) + snum_prev = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1; + + snum_next2 = snum_next-SAMPLES_PER_MS; + if(snum_next2 < 0) + snum_next2 = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-SAMPLES_PER_MS; + + snum_cur2 = snum_next2 - 1; + if(snum_cur2 < 0) + snum_cur2 = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1; + + snum_prev2 = snum_cur2 - 1; + if(snum_prev2 < 0) + snum_prev2 = (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-1; + + + if(args->params.bits & 0x01) + { + for(i=0;i<3;i++) + { + float u_next = (((float)(log->UL_buf[i][snum_next] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier; + float u_prev = (((float)(log->UL_buf[i][snum_prev] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier; + float i_cur= (((float)(log->IL_buf[i][snum_cur] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier; + *log->I_comp[i]=i_cur-((u_next-u_prev)*log->mul); + + u_next = (((float)(log->UL_buf[i][snum_next2] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier; + u_prev = (((float)(log->UL_buf[i][snum_prev2] - 0x7FFF)) - log->U_params[i]->offset ) * log->U_params[i]->multiplier; + i_cur= (((float)(log->IL_buf[i][snum_cur2] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier; + *log->I_comp_prev[i]=i_cur-((u_next-u_prev)*log->mul); + } + } + else + { + for(i=0;i<3;i++) + { + *log->I_comp[i]=(((float)(log->IL_buf[i][snum_cur] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier; + *log->I_comp_prev[i]=(((float)(log->IL_buf[i][snum_cur2] - 0x7FFF)) - log->I_params[i]->offset ) * log->I_params[i]->multiplier; + } + } +} diff --git a/src/cap_comp.h b/src/cap_comp.h new file mode 100644 index 0000000..e7045b4 --- /dev/null +++ b/src/cap_comp.h @@ -0,0 +1,64 @@ +/* + * cap_comp.h + * + * Created on: 19-08-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef CAP_COMP_H_ +#define CAP_COMP_H_ + +#include "tdefs.h" + +struct cap_comp_logic +{ + float *I_comp[3]; + float *I_comp_prev[3]; + u16 *IL_buf[3]; + u16 *UL_buf[3]; + struct analog_in_params *I_params[3]; + struct analog_in_params *U_params[3]; + float mul; +}; + +struct cap_comp_io +{ + u32 IL1_buf_ptr_in; + u32 IL1_params_ptr_in; + u32 IL2_buf_ptr_in; + u32 IL2_params_ptr_in; + u32 IL3_buf_ptr_in; + u32 IL3_params_ptr_in; + u32 UL1_buf_ptr_in; + u32 UL1_params_ptr_in; + u32 UL2_buf_ptr_in; + u32 UL2_params_ptr_in; + u32 UL3_buf_ptr_in; + u32 UL3_params_ptr_in; + u32 IL1_comp_float_out; + u32 IL2_comp_float_out; + u32 IL3_comp_float_out; + u32 IL1_comp_prev_float_out; + u32 IL2_comp_prev_float_out; + u32 IL3_comp_prev_float_out; +}__attribute__((__packed__)); + + +struct cap_comp_params +{ + u32 bits; + double k; + double c; +}__attribute__((__packed__)); + +struct cap_comp_args +{ + struct cap_comp_io io; + struct cap_comp_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void cap_comp(void *args, void *logic); +extern int cap_comp_initlog(void *arguments, void *logic); + +#endif diff --git a/src/cfg_state.c b/src/cfg_state.c new file mode 100644 index 0000000..1cc5f39 --- /dev/null +++ b/src/cfg_state.c @@ -0,0 +1,27 @@ +/* + * cfg_state.c + * + * Created on: 22-02-2017 + * Author: + */ + + +#include "cfg_state.h" + +#include "helper.h" + +int state_initlog(void *arguments, void *logic) +{ + struct state_args *args = (struct state_args *)arguments; + struct state_logic *log = (struct state_logic *)logic; + + if(set_bit_ptr_mask(args->io.out,&log->out_bit,&log->out_bit_mask)) + return -1; + + if (args->params.bity) + *log->out_bit |= log->out_bit_mask; + else + *log->out_bit &= ~log->out_bit_mask; + + return 0; +} diff --git a/src/cfg_state.h b/src/cfg_state.h new file mode 100644 index 0000000..2bb491d --- /dev/null +++ b/src/cfg_state.h @@ -0,0 +1,38 @@ +/* + * cfg_state.h + * + * Created on: 22-02-2017 + * Author: + */ + +#ifndef CFG_STATE_H_ +#define CFG_STATE_H_ + +#include "tdefs.h" + +struct state_logic +{ + u8* out_bit; + u8 out_bit_mask; +}; + + +struct state_params +{ + u32 bity; +}__attribute__((__packed__)); + +struct state_io +{ + u32 out; +}__attribute__((__packed__)); + +struct state_args +{ + struct state_io io; + struct state_params params; +}__attribute__((__packed__)); + +extern int state_initlog(void *arguments, void *logic); + +#endif /* CFG_STATE_H_ */ diff --git a/src/cfg_var.h b/src/cfg_var.h index eae07aa..6cdb4b9 100644 --- a/src/cfg_var.h +++ b/src/cfg_var.h @@ -8,6 +8,8 @@ #ifndef CFG_VAR_H_ #define CFG_VAR_H_ +#include "misc.h" + extern const struct config_lookup_table cfg_lut[]; extern const unsigned int cfg_lut_elements; diff --git a/src/chka.c b/src/chka.c new file mode 100644 index 0000000..a0b6107 --- /dev/null +++ b/src/chka.c @@ -0,0 +1,239 @@ +/* + * chka.c + * + * Created on: 07-03-2018 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "chka.h" +#include "helper.h" +#include + +int chka_initlog(void *arguments, void *logic) +{ + struct chka_args *args = (struct chka_args *)arguments; + struct chka_logic *log = (struct chka_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_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.k * args->params.k; + + log->t0=(u32)(args->params.t0*1000); + return 0; +} + +void chka(void *arguments, void *logic) +{ + struct chka_logic *log = (struct chka_logic *)logic; + struct chka_args *args = (struct chka_args *)arguments; + float tplus_value; + u8 w,i; + + if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked + { + 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]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.n); + tplus_value-=1; + + if(!tplus_value || tplus_value<0) + tplus_value=0.0000001; + tplus_value=args->params.tm/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.n); + tplus_value-=1; + + if(!tplus_value || tplus_value<0) + tplus_value=0.0000001; + tplus_value=args->params.tm/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; + tplus_value=powf(tplus_value,(float)args->params.n); + tplus_value=(1-tplus_value); + + if(!tplus_value || tplus_value<0) + tplus_value=0.0000001; + tplus_value=args->params.tm/tplus_value; + tplus_value*=(1000/LOOP_CYCLE_MS); + + if(tplus_value) + { + tplus_value=1000000000/tplus_value; + if(tplus_value<=log->dw.t[i]) + log->dw.t[i]-=tplus_value; + else + log->dw.t[i]=0; + } + 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(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 + { + 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); + } +} diff --git a/src/chka.h b/src/chka.h new file mode 100644 index 0000000..2136c9c --- /dev/null +++ b/src/chka.h @@ -0,0 +1,82 @@ +/* + * chka.h + * + * Created on: 07-03-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef CHKA_H_ +#define CHKA_H_ + +#include "tdefs.h" +#include "helper.h" + +#define CHKA_SIL_ZAL 0 +#define CHKA_STALA 1 +#define CHKA_IDMT 2 + +struct dane_wewnetrzne_chka +{ + short lp[3]; /// + +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]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); +} diff --git a/src/chka_ieee.h b/src/chka_ieee.h new file mode 100644 index 0000000..6a91696 --- /dev/null +++ b/src/chka_ieee.h @@ -0,0 +1,101 @@ +/* + * chka_ieee.h + * + * Created on: 08-08-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef CHKA_IEEE_H_ +#define CHKA_IEEE_H_ + +#include "tdefs.h" +#include "helper.h" + +//#include "pawel_usun_to.h" + +#define CHKA_SIL_ZAL 0 +#define CHKA_STALA 1 +#define CHKA_IDMT 2 +#define CHKA_INVERSE 3 +#define CHKA_VERY_INVERSE 4 +#define CHKA_EXTREMELY_INVERSE 5 +#define CHKA_IEEE_MODERATELY_INVERSE 6 +#define CHKA_IEEE_VERY_INVERSE 7 +#define CHKA_IEEE_EXTREMELY_INVERSE 8 + +struct dane_wewnetrzne_chka_ieee +{ + short lp[3]; /// + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "cnt.h" + +float cnt_mem[8] = {0,0,0,0,0,0,0,0}; + +int cnt_initlog(void *arguments, void *logic) +{ + struct cnt_args *args = (struct cnt_args *)arguments; + struct cnt_logic *log = (struct cnt_logic *)logic; + + if(set_bit_ptr_mask(args->io.Pwe_in,&log->Pwe,&log->Pwe_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.Kas_in,&log->Kas,&log->Kas_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.Pwy_out,&log->Pwy,&log->Pwy_bit_mask)) + return -1; + + if(set_float_ptr(args->io.Licznik_float_out,&log->licznik)) + return -1; + + log->on = args->params.bity & 0x0001; + log->Wr = args->params.Wr_; // przepisanie wartosci progowej PKW + log->id = args->params.id & 0x0007; + +// Semaphore_post(spi_semaphore); + + return 0; +} + +void cnt(void *arguments, void *logic) +{ + struct cnt_logic *log = (struct cnt_logic *)logic; + struct cnt_args *args = (struct cnt_args *)arguments; + + if(log->on) // jesli funkcja przekaznika aktywna + { + if (*log->Kas & log->Kas_bit_mask) + cnt_mem[log->id]=args->params.kas; + + if (*log->Pwe & log->Pwe_bit_mask) + cnt_mem[log->id] += 1.0; + + check_and_set_mask(cnt_mem[log->id]>=log->Wr,log->Pwy,log->Pwy_bit_mask); + + *log->licznik = cnt_mem[log->id]; + } + else + *log->licznik = 0; +} + diff --git a/src/cnt.h b/src/cnt.h new file mode 100644 index 0000000..b2ec920 --- /dev/null +++ b/src/cnt.h @@ -0,0 +1,57 @@ +/* + * cnt.h + * + * Created on: 11-06-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef CNT_H_ +#define CNT_H_ + +#include "tdefs.h" + +extern float cnt_mem[8]; + +struct cnt_logic +{ + u8 *Pwe; + u8 Pwe_bit_mask; + u8 *Kas; + u8 Kas_bit_mask; + u8 *Pwy; + u8 Pwy_bit_mask; + + float *licznik; + + u16 Wr; ///< wartosc progowa sygnalizacji + u8 on; ///< wartosc nastawy aktywnosci PKWa 1- aktywny + u8 id; +}; + +struct cnt_io +{ + u32 Pwe_in; + u32 Kas_in; + u32 Pwy_out; + u32 Licznik_float_out; +}__attribute__((__packed__)); + +struct cnt_params +{ + u32 bity; ///< nastawy bitowe; + double Wr_; ///< wartosc progowa sygnalizacji + double kas; // wartosc przepisywana przy kasowaniu + u32 id; // id pkw od 0..7 +}__attribute__((__packed__)); + +struct cnt_args +{ + struct cnt_io io; + struct cnt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void cnt(void *args, void *logic); +extern int cnt_initlog(void *arguments, void *logic); + +#endif /* CNT_H_ */ diff --git a/src/comm.h b/src/comm.h index f98e566..cde3a17 100644 --- a/src/comm.h +++ b/src/comm.h @@ -8,8 +8,10 @@ #ifndef COMM_H_ #define COMM_H_ +/* #include #include +*/ #include "tdefs.h" #define DSP_CMD_INIT_COMM 0x01 diff --git a/src/comp_s.c b/src/comp_s.c new file mode 100644 index 0000000..872e25f --- /dev/null +++ b/src/comp_s.c @@ -0,0 +1,84 @@ +/* + * comp_s.c + * + * Created on: 08-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "helper.h" +#include "analog_in.h" + +#include "comp_s.h" + +int comp_s_initlog(void *arguments, void *logic) +{ + struct comp_s_args *args = (struct comp_s_args *)arguments; + struct comp_s_logic *log = (struct comp_s_logic *)logic; + struct analog_in_params *an_params; + + u32 *an_params_ptr; + u32 *buf_ptr; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + + log->an_params=an_params; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->nast_.limit1_ = args->params.Wr_ * args->params.mnoz_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.l_powt_pob = args->params.l_powt_pob; + + return 0; +} + +void comp_s(void *arguments, void *logic) +{ +// struct comp_s_args *args = (struct comp_s_args *)arguments; + struct comp_s_logic *log = (struct comp_s_logic *)logic; + + short i,i1,i2; + float pr; + + short pobud = 0; + i2 = bus_an_cur_sample_num; + + for (i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + i1 = i2 - i; + if(i1<0) + i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + pr = (float)(log->buf_ptr[i1] - 0x7FFF); + pr*=log->an_params->multiplier; + + if (pr < 0) pr = 0 - pr; + + if (pr > log->nast_.limit1_) + pobud ++; + + } + if (pobud >= log->nast_.l_powt_pob) + log->pob = 1; + else + log->pob = 0; + + check_and_set(log->pob,log->P,log->P_bit_no); +} diff --git a/src/comp_s.h b/src/comp_s.h new file mode 100644 index 0000000..2b648f0 --- /dev/null +++ b/src/comp_s.h @@ -0,0 +1,59 @@ +/* + * comp_s.h + * + * Created on: 01-04-2022 + * Author: Pawel + */ + +#ifndef COMP_S_H_ +#define COMP_S_H_ + +#include "tdefs.h" +//#include "pawel_usun_to.h" + +struct nastawy_przeliczone_comp_s +{ + float limit1_; ///< wartosc rozruchowa + short l_powt_pob; +}; + +struct comp_s_logic +{ + u16 *buf_ptr; + struct analog_in_params *an_params; + + u8 *P; + u8 P_bit_no; + + struct nastawy_przeliczone_comp_s nast_; + u8 pob; +}; + +struct comp_s_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 P_out; + +}__attribute__((__packed__)); + +struct comp_s_params +{ + double Wr_; ///< wartosc nastawy rozruchowej przekaznika w krotnosciach wartosci znamionowej + //double kp_; ///< wartosc nastawy wspolczynnika powrotu + double mnoz_; ///< mnoznik wart_roz + u32 l_powt_pob; + //u32 l_powt_odp; +}__attribute__((__packed__)); + +struct comp_s_args +{ + struct comp_s_io io; + struct comp_s_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void comp_s(void *args, void *logic); +extern int comp_s_initlog(void *arguments, void *logic); + +#endif /* COMP_S_H_ */ diff --git a/src/const_flt.c b/src/const_flt.c new file mode 100644 index 0000000..07c3235 --- /dev/null +++ b/src/const_flt.c @@ -0,0 +1,28 @@ +/* + * counter.c + * + * Created on: 04-06-2018 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "const_flt.h" + +int const_flt_initlog(void *arguments, void *logic) +{ + struct const_flt_args *args = (struct const_flt_args *)arguments; + struct const_flt_logic *log = (struct const_flt_logic *)logic; + + if(set_float_ptr(args->io.out_float_out,&log->out)) + return -1; + + *log->out=(float)args->params.val; + + return 0; +} + diff --git a/src/const_flt.h b/src/const_flt.h new file mode 100644 index 0000000..406ead1 --- /dev/null +++ b/src/const_flt.h @@ -0,0 +1,37 @@ +/* + * const_flt.h + * + * Created on: 04-06-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef CONST_FLT_H_ +#define CONST_FLT_H_ + +#include "tdefs.h" + +struct const_flt_logic +{ + float *out; +}; + +struct const_flt_io +{ + u32 out_float_out; +}__attribute__((__packed__)); + +struct const_flt_params +{ + double val; +}__attribute__((__packed__)); + +struct const_flt_args +{ + struct const_flt_io io; + struct const_flt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int const_flt_initlog(void *arguments, void *logic); + +#endif /* CONST_FLT_H_ */ diff --git a/src/counter.c b/src/counter.c new file mode 100644 index 0000000..8fc0ba7 --- /dev/null +++ b/src/counter.c @@ -0,0 +1,46 @@ +/* + * counter.c + * + * Created on: 04-06-2018 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "counter.h" + +int counter_initlog(void *arguments, void *logic) +{ + struct counter_args *args = (struct counter_args *)arguments; + struct counter_logic *log = (struct counter_logic *)logic; + + if(set_bit_ptr_struct(args->io.in_in,&log->in)) + return -1; + + if(set_bit_ptr_struct(args->io.clr_in,&log->clr)) + return -1; + + if(set_float_ptr(args->io.out_float_out,&log->out)) + return -1; + + return 0; +} + +void counter(void *arguments, void *logic) +{ + struct counter_logic *log = (struct counter_logic *)logic; + + if(!check_struct(&log->clr)) + { + if(check_struct(&log->in)) + log->cnt++; + } + else + log->cnt=0; + + *log->out=(float)log->cnt; +} diff --git a/src/counter.h b/src/counter.h new file mode 100644 index 0000000..a50546a --- /dev/null +++ b/src/counter.h @@ -0,0 +1,42 @@ +/* + * counter.h + * + * Created on: 04-06-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef COUNTER_H_ +#define COUNTER_H_ + +#include "tdefs.h" + +struct counter_logic +{ + struct binary_io in; + struct binary_io clr; + u32 cnt; + float *out; +}; + +struct counter_io +{ + u32 in_in; + u32 clr_in; + u32 out_float_out; +}__attribute__((__packed__)); + +struct counter_params +{ +}__attribute__((__packed__)); + +struct counter_args +{ + struct counter_io io; + struct counter_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void counter(void *args, void *logic); +extern int counter_initlog(void *arguments, void *logic); + +#endif /* COUNTER_H_ */ diff --git a/src/ddr.c b/src/ddr.c new file mode 100644 index 0000000..1f6e10f --- /dev/null +++ b/src/ddr.c @@ -0,0 +1,219 @@ +/* + * ddr.c + * + * Created on: 26-05-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include +#include +#include "ddr.h" +#include "ddr_drv.h" +#include "rec_an.h" +#include "rec_bin.h" +#include "comm.h" +#include "misc.h" +#include "config.h" + +#pragma LOCATION(ddr_bank0,0xc4000000) // @ non-cached memory above 128MB +volatile u8 ddr_bank0[DDR_REG_BANK_SIZE]; +#pragma LOCATION(ddr_bank1,0xc4100000) // @ non-cached memory above 128MB +volatile u8 ddr_bank1[DDR_REG_BANK_SIZE]; +#pragma LOCATION(ddr_bank2,0xc4200000) // @ non-cached memory above 128MB +volatile u8 ddr_bank2[DDR_REG_BANK_SIZE]; +#pragma LOCATION(ddr_bank3,0xc4300000) // @ non-cached memory above 128MB +volatile u8 ddr_bank3[DDR_REG_BANK_SIZE]; + +volatile struct ddr_reg_inf ddr_regs = { .cur_reg_bank = 0, + .bank[0].data = (void*) ddr_bank0, + .bank[0].state = BANK_BUFFERING, + .bank[0].addr_offset = 0, + .bank[0].pre_trigger_cnt = 0, + .bank[0].post_trigger_cnt = 0, + .bank[0].total_cnt = 0, + .bank[1].data = (void*) ddr_bank1, + .bank[2].data = (void*) ddr_bank2, + .bank[3].data = (void*) ddr_bank3, + }; + +int ddr_initlog(void *arguments, void *logic) +{ + struct ddr_args *args = (struct ddr_args *)arguments; + struct ddr_logic *log = (struct ddr_logic *)logic; + + log->trigger_ptr = log_manager.nets_data + (args->io.trigger_in >> 3); + log->trigger_bit_no = args->io.trigger_in & 0x07; + if(log->trigger_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + switch((enum ddr_fs)args->params.f_sampling) + { + case Fs_100Hz: + log->decimation=10/LOOP_CYCLE_MS; + break; + case Fs_50Hz: + log->decimation=20/LOOP_CYCLE_MS; + break; + case Fs_10Hz: + log->decimation=100/LOOP_CYCLE_MS; + break; + case Fs_5Hz: + log->decimation=200/LOOP_CYCLE_MS; + break; + case Fs_1Hz: + log->decimation=1000/LOOP_CYCLE_MS; + break; + case Fs_0_5Hz: + log->decimation=2000/LOOP_CYCLE_MS; + break; + case Fs_0_1Hz: + log->decimation=10000/LOOP_CYCLE_MS; + break; + default: + log->decimation=10/LOOP_CYCLE_MS; + break; + } + + log->pre_trigger_samples = args->params.pre_trigger_time / log->decimation; + if(args->params.pre_trigger_time) + log->pre_trigger_samples++; + + log->post_trigger_samples = args->params.post_trigger_time / log->decimation; + if(args->params.post_trigger_time) + log->post_trigger_samples++; + + log->max_reg_samples = args->params.max_reg_time / log->decimation; + if(args->params.max_reg_time) + log->max_reg_samples++; + + return 0; +} + +void ddr(void *arguments, void *logic) +{ + struct ddr_logic *log = (struct ddr_logic *)logic; + //struct ddr_args *args = (struct ddr_args *)arguments; + struct rec_an_logic *an_log; + struct rec_bin_logic *bin_log; + u16 sample_buf[(MAX_SAMPLE_SIZE/sizeof(u16)) + 8]; + int i,bit_cnt=0,pos=0; + u8 new_sample=0; + + if(ddr_drv_log_ptr == NULL) // ddr driver not initialized? + return; + + if(!log->decimation_cnt--) + { + log->decimation_cnt=log->decimation; + new_sample=1; + } + + switch(ddr_regs.bank[ddr_regs.cur_reg_bank].state) + { + case BANK_BUFFERING: + if(*log->trigger_ptr & (1<trigger_bit_no)) + { + ddr_regs.bank[ddr_regs.cur_reg_bank].state=BANK_TRIGGERED; + ddr_regs.bank[ddr_regs.cur_reg_bank].tr_time=(timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec; + ddr_regs.bank[ddr_regs.cur_reg_bank].tr_time_ms=(timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec; + ddr_regs.bank[ddr_regs.cur_reg_bank].sample_size=ddr_drv_log_ptr->sample_size; + ddr_regs.bank[ddr_regs.cur_reg_bank].an_count=ddr_drv_log_ptr->an_count; + ddr_regs.bank[ddr_regs.cur_reg_bank].bin_count=ddr_drv_log_ptr->bin_count; + ddr_regs.bank[ddr_regs.cur_reg_bank].post_trigger_cnt=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].trigger_offset=ddr_regs.bank[ddr_regs.cur_reg_bank].addr_offset; + ddr_regs.bank[ddr_regs.cur_reg_bank].total_cnt=ddr_regs.bank[ddr_regs.cur_reg_bank].pre_trigger_cnt; + } + else + { + if(new_sample && ddr_regs.bank[ddr_regs.cur_reg_bank].pre_trigger_cntpre_trigger_samples) + ddr_regs.bank[ddr_regs.cur_reg_bank].pre_trigger_cnt++; + } + break; + + case BANK_TRIGGERED: + if(new_sample) + { + if(ddr_regs.bank[ddr_regs.cur_reg_bank].total_cnt++>=log->max_reg_samples || ddr_regs.bank[ddr_regs.cur_reg_bank].post_trigger_cnt>=log->post_trigger_samples) + { + // send notify to ARM + ddr_regs.bank[ddr_regs.cur_reg_bank].state=BANK_FILLED; + ddr_regs.bank[ddr_regs.cur_reg_bank].decimation=log->decimation; + ddr_regs.cur_reg_bank++; + ddr_regs.cur_reg_bank%=DDR_MAX_REG_BANKS; + ddr_regs.bank[ddr_regs.cur_reg_bank].state=BANK_BUFFERING; + ddr_regs.bank[ddr_regs.cur_reg_bank].addr_offset=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].post_trigger_cnt=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].pre_trigger_cnt=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].trigger_offset=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].total_cnt=0; + logman_notify|=LOGMAN_NOTIFY_NEW_DDR; +// Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_NEW_DDR_REG, TRUE); + } + else + { + if(*log->trigger_ptr & (1<trigger_bit_no)) + ddr_regs.bank[ddr_regs.cur_reg_bank].post_trigger_cnt=0; + else + ddr_regs.bank[ddr_regs.cur_reg_bank].post_trigger_cnt++; + } + } + break; + + case BANK_EMPTY: + case BANK_FILLED: + default: + ddr_regs.bank[ddr_regs.cur_reg_bank].state=BANK_BUFFERING; + ddr_regs.bank[ddr_regs.cur_reg_bank].addr_offset=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].post_trigger_cnt=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].pre_trigger_cnt=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].trigger_offset=0; + ddr_regs.bank[ddr_regs.cur_reg_bank].total_cnt=0; + break; + } + + + +// +// will be overwritten by ARM +// *((u32 *)&sample_buf[0]) = 0; // sample number 1..n +// *((u32 *)&sample_buf[2]) = 0; // time stamp 0..n*1ms @ 1 kHz +// + if(new_sample) + { + + pos = 4; + + for(i=0;ian_count;i++) + { + an_log = (struct rec_an_logic *)log_manager.log_element[ddr_drv_log_ptr->element_num[i] & 0x3FFF].fun_log_ptr; + sample_buf[pos]=*an_log->in_ptr; + pos++; + } + + sample_buf[pos]=0; + + for(;i<(ddr_drv_log_ptr->an_count+ddr_drv_log_ptr->bin_count);i++) + { + bin_log = (struct rec_bin_logic *)log_manager.log_element[ddr_drv_log_ptr->element_num[i] & 0x3FFF].fun_log_ptr; + + if(*bin_log->in_ptr & bin_log->in_bit_mask) + sample_buf[pos]|=(1<<(bit_cnt%16)); + + if(!(++bit_cnt%16) && bit_cnt) + { + pos++; + sample_buf[pos]=0; + } + } + + if(ddr_regs.bank[ddr_regs.cur_reg_bank].addr_offset + ddr_drv_log_ptr->sample_size > DDR_REG_BANK_SIZE) + ddr_regs.bank[ddr_regs.cur_reg_bank].addr_offset=0; + + memcpy((u8*)ddr_regs.bank[ddr_regs.cur_reg_bank].data+ddr_regs.bank[ddr_regs.cur_reg_bank].addr_offset,(void *)sample_buf,ddr_drv_log_ptr->sample_size); + ddr_regs.bank[ddr_regs.cur_reg_bank].addr_offset+=ddr_drv_log_ptr->sample_size; + } +} diff --git a/src/ddr.h b/src/ddr.h new file mode 100644 index 0000000..9cc5b8b --- /dev/null +++ b/src/ddr.h @@ -0,0 +1,102 @@ +/* + * ddr.h + * + * + * Created on: 26-05-2017 + * Author: KJ + */ + +#ifndef DDR_H_ +#define DDR_H_ + +#include "tdefs.h" + +#define MAX_SAMPLE_SIZE 512 + + +#define DDR_REG_BANK_SIZE 0x100000 + +// bank states +#define BANK_EMPTY 0x00 +#define BANK_BUFFERING 0x01 +#define BANK_TRIGGERED 0x02 +#define BANK_FILLED 0x03 + +#define DDR_MAX_REG_BANKS 4 + +struct ddr_reg_data +{ + u8 *data; + u8 state; + u32 addr_offset; + u32 pre_trigger_cnt; + u32 post_trigger_cnt; + u32 total_cnt; + u32 trigger_offset; + u32 tr_time; + u32 tr_time_ms; + u16 sample_size; + u8 an_count; + u16 bin_count; + u16 decimation; +}__attribute__((__packed__)); + +struct ddr_reg_inf +{ + u8 cur_reg_bank; + struct ddr_reg_data bank[DDR_MAX_REG_BANKS]; +}__attribute__((__packed__)); + +enum ddr_fs +{ + Fs_100Hz, + Fs_50Hz, + Fs_10Hz, + Fs_5Hz, + Fs_1Hz, + Fs_0_5Hz, + Fs_0_1Hz +}; + +extern volatile u8 ddr_bank0[DDR_REG_BANK_SIZE]; +extern volatile u8 ddr_bank1[DDR_REG_BANK_SIZE]; +extern volatile u8 ddr_bank2[DDR_REG_BANK_SIZE]; +extern volatile u8 ddr_bank3[DDR_REG_BANK_SIZE]; + +extern volatile struct ddr_reg_inf ddr_regs; + +struct ddr_logic +{ + u8 *trigger_ptr; + u8 trigger_bit_no; + u32 decimation; + u32 decimation_cnt; + u32 pre_trigger_samples; + u32 post_trigger_samples; + u32 max_reg_samples; +}; + +struct ddr_io +{ + u32 trigger_in; +}__attribute__((__packed__)); + +struct ddr_params +{ + u32 pre_trigger_time; + u32 post_trigger_time; + u32 max_reg_time; + u32 f_sampling; +}__attribute__((__packed__)); + +struct ddr_args +{ + struct ddr_io io; + struct ddr_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int ddr_initlog(void *args, void *logic); +extern void ddr(void *args, void *logic); + +#endif /* DDR_H_ */ diff --git a/src/ddr_drv.c b/src/ddr_drv.c new file mode 100644 index 0000000..fa9b6a7 --- /dev/null +++ b/src/ddr_drv.c @@ -0,0 +1,98 @@ +/* + * ddr_drv.c + * + * Created on: 26-05-2017 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include +#include "rec_bin.h" +#include "rec_float.h" +#include "ddr.h" +#include "ddr_drv.h" + +struct ddr_drv_logic *ddr_drv_log_ptr = NULL; + +int ddr_drv_initlog(void *arguments, void *logic) +{ + struct ddr_drv_logic *log = (struct ddr_drv_logic *)logic; + int i; + + log->an_count=0; + log->bin_count=0; + + for(i=0;iparams.bits & REC_CHAN_ENABLED_DDR)) + { + if((u8 *)&log->element_num[log->an_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->an_count]=i|((log_manager.log_element[i].flags & FUNCTION_IS_REC_FLOAT)?ELEMENT_IS_REC_FLOAT:0); + log->an_count++; + } + } + + log->sample_size=log->an_count * sizeof(u16); + + for(i=0;iparams.bits & REC_CHAN_ENABLED_DDR)) + { + if((u8 *)&log->element_num[log->an_count+log->bin_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->an_count+log->bin_count]=i; + log->bin_count++; + } + } + + log->sample_size+=((log->bin_count/16)*2); + if(log->bin_count%16) + log->sample_size+=2; + + log->sample_size+=8; // sample number && time stamp + + if(log->sample_size>MAX_SAMPLE_SIZE) + return -1; + + for(i=0;imax_reg_samples > ((DDR_REG_BANK_SIZE / log->sample_size) - 2)) + dlog->max_reg_samples = ((DDR_REG_BANK_SIZE / log->sample_size) - 2); + //return -1; + + if((dlog->post_trigger_samples + dlog->pre_trigger_samples) > dlog->max_reg_samples) + { + dlog->post_trigger_samples=dlog->max_reg_samples/2; + dlog->pre_trigger_samples=dlog->max_reg_samples/2; + // return -1; + } + } + } + + ddr_drv_log_ptr=(struct ddr_drv_logic *)logic; + + return 0; +} + diff --git a/src/ddr_drv.h b/src/ddr_drv.h new file mode 100644 index 0000000..1180b02 --- /dev/null +++ b/src/ddr_drv.h @@ -0,0 +1,44 @@ +/* + * ddr_drv.h + * + * + * Created on: 26-05-2017 + * Author: KJ + */ + +#ifndef DDR_DRV_H_ +#define DDR_DRV_H_ + +#include "tdefs.h" +#include "ddr.h" + +#define ELEMENT_IS_REC_FLOAT 0x8000 + +extern struct ddr_drv_logic *ddr_drv_log_ptr; + +struct ddr_drv_logic +{ + u16 an_count; + u16 bin_count; + u16 sample_size; + u16 element_num[]; +}; + +struct ddr_drv_io +{ +}__attribute__((__packed__)); + +struct ddr_drv_params +{ +}__attribute__((__packed__)); + +struct ddr_drv_args +{ + struct ddr_drv_io io; + struct ddr_drv_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int ddr_drv_initlog(void *args, void *logic); + +#endif /* DDR_DRV_H_ */ diff --git a/src/dec_month.c b/src/dec_month.c new file mode 100644 index 0000000..ffb17e4 --- /dev/null +++ b/src/dec_month.c @@ -0,0 +1,118 @@ +/* + * klapacz.c + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "dec_month.h" +#include +//volatile struct klapacz_args tkl; + +int dec_month_initlog(void *arguments, void *logic) +{ + struct dec_month_args *args = (struct dec_month_args *)arguments; + struct dec_month_logic *log = (struct dec_month_logic *)logic; + + if(set_bit_ptr_struct(args->io.sty,&log->mon[0])) + return -1; + if(set_bit_ptr_struct(args->io.lut,&log->mon[1])) + return -1; + if(set_bit_ptr_struct(args->io.marz,&log->mon[2])) + return -1; + if(set_bit_ptr_struct(args->io.kwie,&log->mon[3])) + return -1; + if(set_bit_ptr_struct(args->io.maj,&log->mon[4])) + return -1; + if(set_bit_ptr_struct(args->io.czer,&log->mon[5])) + return -1; + if(set_bit_ptr_struct(args->io.lip,&log->mon[6])) + return -1; + if(set_bit_ptr_struct(args->io.sier,&log->mon[7])) + return -1; + if(set_bit_ptr_struct(args->io.wrz,&log->mon[8])) + return -1; + if(set_bit_ptr_struct(args->io.paz,&log->mon[9])) + return -1; + if(set_bit_ptr_struct(args->io.lis,&log->mon[10])) + return -1; + if(set_bit_ptr_struct(args->io.gru,&log->mon[11])) + return -1; + if(set_bit_ptr_struct(args->io.zima,&log->zima)) + return -1; + if(set_bit_ptr_struct(args->io.lato,&log->lato)) + return -1; + + return 0; +} + +extern void my_gmtime(const uint32_t *timer, struct tm *timep); +extern uint8_t isindst(struct tm *tb, uint8_t cest_cet_n); + +void dec_month(void *arguments, void *logic) +{ +// struct dec_month_args *args = (struct dec_month_args *)arguments; + struct dec_month_logic *log = (struct dec_month_logic *)logic; + struct tm tim; + u8 i; + + my_gmtime((const uint32_t*)&cur_time.tv_sec,&tim); + + for(i=0;i<12;i++) + check_and_set_struct(tim.tm_mon==i,&log->mon[i]); + + if(isindst(&tim,0)) + { + set_struct(&log->lato); + clear_struct(&log->zima); + } + else + { + set_struct(&log->zima); + clear_struct(&log->lato); + } + /*u8 day_of_week; + + day_of_week = (cur_time.tv_sec / 86400 + 4)%7; + + check_and_set_struct((day_of_week==0),&log->niedz); + check_and_set_struct((day_of_week==1),&log->pon); + check_and_set_struct((day_of_week==2),&log->wt); + check_and_set_struct((day_of_week==3),&log->sr); + check_and_set_struct((day_of_week==4),&log->czw); + check_and_set_struct((day_of_week==5),&log->pt); + check_and_set_struct((day_of_week==6),&log->sob); + check_and_set_struct((day_of_week==7),&log->niedz); + */ + + /* + local function GetMonth(seconds) + local dayduration,year = 3600*24 + local days={31,0,31,30,31,30,31,31,30,31,30,31} + for i=1970,10000 do -- For some reason too lazy to use while + local yeardays = i%4 == 0 and i%100 ~= 0 and 366 or 365 + local yearduration = dayduration * yeardays + if yearduration < seconds then + seconds = seconds - yearduration + else + year = i break + end + end + days[2]=(year%4==0) and 29 or 28 + seconds = seconds%(365*24*3600) + for i=1,12 do + if seconds>days[i]*dayduration then + seconds=seconds-days[i]*dayduration + else + return --i + year*12 <-- If you want a unique ID + end + end + end + */ + + +} diff --git a/src/dec_month.h b/src/dec_month.h new file mode 100644 index 0000000..9d6ab41 --- /dev/null +++ b/src/dec_month.h @@ -0,0 +1,57 @@ +/* + * klapacz.h + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DEC_MONTH_H_ +#define DEC_MONTH_H_ + +#include "tdefs.h" + +struct dec_month_logic +{ + + struct binary_io mon[12]; // one bit + + struct binary_io zima; // one bit + struct binary_io lato; // one bit + +}; + +struct dec_month_io +{ + u32 sty; // one bit + u32 lut; // one bit + u32 marz; // one bit + u32 kwie; // one bit + u32 maj; // one bit + u32 czer; // one bit + u32 lip; // one bit + u32 sier; // one bit + u32 wrz; // one bit + u32 paz; // one bit + u32 lis; // one bit + u32 gru; // one bit + + u32 zima; // one bit + u32 lato; // one bit + +}__attribute__((__packed__)); + +struct dec_month_params +{ +}__attribute__((__packed__)); + +struct dec_month_args +{ + struct dec_month_io io; + struct dec_month_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void dec_month(void *args, void *logic); +extern int dec_month_initlog(void *args, void *logic); + +#endif /* DEC_MONTH_H_ */ diff --git a/src/demux8.c b/src/demux8.c new file mode 100644 index 0000000..c18b620 --- /dev/null +++ b/src/demux8.c @@ -0,0 +1,64 @@ +/* + * demux8.c + * + * Created on: 22-05-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "demux8.h" + +int demux8_initlog(void *arguments, void *logic) +{ + struct demux8_args *args = (struct demux8_args *)arguments; + struct demux8_logic *log = (struct demux8_logic *)logic; + + if(set_bit_ptr_struct(args->io.we_in,&log->we)) + return -1; + if(set_bit_ptr_struct(args->io.a0_in,&log->a0)) + return -1; + if(set_bit_ptr_struct(args->io.a1_in,&log->a1)) + return -1; + if(set_bit_ptr_struct(args->io.a2_in,&log->a2)) + return -1; + + if(set_bit_ptr_struct(args->io.wy1_out,&log->wy[0])) + return -1; + if(set_bit_ptr_struct(args->io.wy2_out,&log->wy[1])) + return -1; + if(set_bit_ptr_struct(args->io.wy3_out,&log->wy[2])) + return -1; + if(set_bit_ptr_struct(args->io.wy4_out,&log->wy[3])) + return -1; + if(set_bit_ptr_struct(args->io.wy5_out,&log->wy[4])) + return -1; + if(set_bit_ptr_struct(args->io.wy6_out,&log->wy[5])) + return -1; + if(set_bit_ptr_struct(args->io.wy7_out,&log->wy[6])) + return -1; + if(set_bit_ptr_struct(args->io.wy8_out,&log->wy[7])) + return -1; + + return 0; +} + +void demux8(void *arguments, void *logic) +{ + struct demux8_logic *log = (struct demux8_logic *)logic; + u8 choice,i; + + choice = check_struct(&log->a0)?1:0; + choice |= check_struct(&log->a1)?2:0; + choice |= check_struct(&log->a2)?4:0; + + for(i=0;i<8;i++) + clear_struct(&log->wy[i]); + + if (check_struct(&log->we)) + set_struct(&log->wy[choice]); +} diff --git a/src/demux8.h b/src/demux8.h new file mode 100644 index 0000000..e79582c --- /dev/null +++ b/src/demux8.h @@ -0,0 +1,52 @@ +/* + * demux8.h + * + * Created on: 22-05-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DEMUX8_H_ +#define DEMUX8_H_ + +#include "tdefs.h" + +struct demux8_logic +{ + struct binary_io we; + struct binary_io a0; + struct binary_io a1; + struct binary_io a2; + struct binary_io wy[8]; +}; + +struct demux8_io +{ + u32 we_in; + u32 a0_in; + u32 a1_in; + u32 a2_in; + u32 wy1_out; + u32 wy2_out; + u32 wy3_out; + u32 wy4_out; + u32 wy5_out; + u32 wy6_out; + u32 wy7_out; + u32 wy8_out; +}__attribute__((__packed__)); + +struct demux8_params +{ +}__attribute__((__packed__)); + +struct demux8_args +{ + struct demux8_io io; + struct demux8_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void demux8(void *args, void *logic); +extern int demux8_initlog(void *arguments, void *logic); + +#endif /* DEMUX8_H_ */ diff --git a/src/destream.c b/src/destream.c new file mode 100644 index 0000000..99dbcc6 --- /dev/null +++ b/src/destream.c @@ -0,0 +1,58 @@ +/* + * destream.c + * + * Created on: 23-04-2019 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "destream.h" +#include "analog_in.h" + +int destream_initlog(void *arguments, void *logic) +{ + struct destream_args *args = (struct destream_args *)arguments; + struct destream_logic *log = (struct destream_logic *)logic; + u8 i; + + if(set_pointer_in_ptr(args->io.stream_ptr_in,(u32*)&log->stream)) + return -1; + + if(set_float_ptr(args->io.A1L1_float_out,&log->A[0])) + return -1; + if(set_float_ptr(args->io.A1L2_float_out,&log->A[1])) + return -1; + if(set_float_ptr(args->io.A1L3_float_out,&log->A[2])) + return -1; + if(set_float_ptr(args->io.snum_float_out,&log->snum)) + return -1; + + for(i=0;i<16;i++) + if(set_bit_ptr_struct(args->io.b_out[i],&log->b[i])) + return -1; + + return 0; +} + +void destream(void *arguments, void *logic) +{ + struct destream_logic *log = (struct destream_logic *)logic; +// struct destream_args *args = (struct mkstream_args *)arguments; + + u32 i; + + for(i=0;i<3;i++) + { + *log->A[i]=log->stream->A[i]; + *log->snum=(float)log->stream->sample_stamp; + } + + for(i=0;i<16;i++) + check_and_set_struct(log->stream->bits & (1<b[i]); + +} diff --git a/src/destream.h b/src/destream.h new file mode 100644 index 0000000..9a68e2a --- /dev/null +++ b/src/destream.h @@ -0,0 +1,52 @@ +/* + * destream.h + * + * Created on: 23-04-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DESTREAM_H_ +#define DESTREAM_H_ + +#include "tdefs.h" +#include "analog_in.h" +#include "mkstream.h" +#include "helper.h" + +struct destream_logic +{ + float *A[3]; + float *snum; + + struct stream_frame *stream; + struct binary_io b[16]; +}; + +struct destream_io +{ + u32 stream_ptr_in; + + u32 A1L1_float_out; + u32 A1L2_float_out; + u32 A1L3_float_out; + u32 snum_float_out; + u32 b_out[16]; +}__attribute__((__packed__)); + + +struct destream_params +{ + +}__attribute__((__packed__)); + +struct destream_args +{ + struct destream_io io; + struct destream_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void destream(void *args, void *logic); +extern int destream_initlog(void *arguments, void *logic); + +#endif diff --git a/src/dev_ctrl.c b/src/dev_ctrl.c new file mode 100644 index 0000000..7e38fc2 --- /dev/null +++ b/src/dev_ctrl.c @@ -0,0 +1,258 @@ +/* + * dev_ctrl.c + * + * Created on: 07-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "dev_ctrl.h" +#include "helper.h" + +u32 dev_ctrl_state = 0; +u32 powered_off=220; +u32 service_mode = 0; + +int dev_ctrl_initlog(void *arguments, void *logic) +{ + struct dev_ctrl_args *args = (struct dev_ctrl_args *)arguments; + struct dev_ctrl_logic *log = (struct dev_ctrl_logic *)logic; + + if(set_bit_ptr_mask(args->io.test_we_in,&log->test_we,&log->test_we_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.test_wy_in,&log->test_wy,&log->test_wy_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.test_log_in,&log->test_log,&log->test_log_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.blok_wy_in,&log->blok_wy,&log->blok_wy_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.test_an_in,&log->test_an,&log->test_an_bit_mask)) + return -1; + if(set_bit_ptr_struct(args->io.pps2_in,&log->pps2)) + return -1; + if(set_bit_ptr_struct(args->io.service_mode_in,&log->service)) + return -1; + if(set_bit_ptr_mask(args->io.pwr1_ok_out,&log->pwr1_ok,&log->pwr1_ok_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.pwr2_ok_out,&log->pwr2_ok,&log->pwr2_ok_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.pwr2_ok_out,&log->pwr2_ok,&log->pwr2_ok_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.nowe_nast_out,&log->nowe_nast,&log->nowe_nast_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.blad_konf_out,&log->blad_konf,&log->blad_konf_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.blad_sd_out,&log->blad_sd,&log->blad_sd_bit_mask)) + return -1; + + if(set_bit_ptr_struct(args->io.mgb_ch1_act_out,&log->mgb_ch[0])) + return -1; + if(set_bit_ptr_struct(args->io.mgb_ch2_act_out,&log->mgb_ch[1])) + return -1; + if(set_bit_ptr_struct(args->io.mgb_ch3_act_out,&log->mgb_ch[2])) + return -1; + if(set_bit_ptr_struct(args->io.mgb_ch4_act_out,&log->mgb_ch[3])) + return -1; + if(set_bit_ptr_struct(args->io.mgb_ch5_act_out,&log->mgb_ch[4])) + return -1; + if(set_bit_ptr_struct(args->io.mgb_ch6_act_out,&log->mgb_ch[5])) + return -1; + if(set_bit_ptr_struct(args->io.mgb_ch7_act_out,&log->mgb_ch[6])) + return -1; + if(set_bit_ptr_struct(args->io.mgb_ch8_act_out,&log->mgb_ch[7])) + return -1; + + if(set_bit_ptr_struct(args->io.mki_zp6_act_out,&log->mki_zp6)) + return -1; + if(set_bit_ptr_struct(args->io.mki_iec1_act_out,&log->mki_iec[0])) + return -1; + if(set_bit_ptr_struct(args->io.mki_iec2_act_out,&log->mki_iec[1])) + return -1; + if(set_bit_ptr_struct(args->io.mki_iec3_act_out,&log->mki_iec[2])) + return -1; + if(set_bit_ptr_struct(args->io.mki_iec4_act_out,&log->mki_iec[3])) + return -1; + + if(set_bit_ptr_struct(args->io.pps_ok_out,&log->pps_ok)) + return -1; + if(set_bit_ptr_struct(args->io.ptp_ok_out,&log->ptp_ok)) + return -1; + if(set_bit_ptr_struct(args->io.ntp_ok_out,&log->ntp_ok)) + return -1; + if(set_bit_ptr_struct(args->io.irigb_fix_ok_out,&log->irigb_fix_ok)) + return -1; + if(set_bit_ptr_struct(args->io.iec_err_out,&log->iec_err)) + return -1; + if(set_bit_ptr_struct(args->io.pps_sw_out,&log->pps_sw)) + return -1; + if(set_float_ptr(args->io.sat_cnt_float_out,&log->sat_cnt)) + return -1; + + dev_ctrl_state&=~DEV_CTRL_STATE_SD_ERR; + dev_ctrl_state&=~DEV_CTRL_STATE_PPS_OK; + dev_ctrl_state&=~DEV_CTRL_STATE_PTP_OK; + dev_ctrl_state&=~DEV_CTRL_STATE_IRIGB_FIX_OK; + + return 0; +} + +void dev_ctrl(void *arguments, void *logic) +{ + struct dev_ctrl_logic *log = (struct dev_ctrl_logic *)logic; + + if(powered_off) + { + if(powered_off>=LOOP_CYCLE_MS) + powered_off-=LOOP_CYCLE_MS; + else + powered_off=0; + } + else + { + check_and_set_mask(!(IN_DATA01 & (1<<12)),log->pwr1_ok,log->pwr1_ok_bit_mask); + check_and_set_mask(!(IN_DATA01 & (1<<5)),log->pwr2_ok,log->pwr2_ok_bit_mask); + } + + if(check_struct(&log->service)) + service_mode=1; + else + service_mode=0; + + if(*log->test_we & log->test_we_bit_mask) + dev_ctrl_state|=DEV_CTRL_STATE_TEST_IN; + else + dev_ctrl_state&=~DEV_CTRL_STATE_TEST_IN; + + if(*log->test_wy & log->test_wy_bit_mask) + dev_ctrl_state|=DEV_CTRL_STATE_TEST_OUT; + else + dev_ctrl_state&=~DEV_CTRL_STATE_TEST_OUT; + + if(*log->test_log & log->test_log_bit_mask) + dev_ctrl_state|=DEV_CTRL_STATE_TEST_LOG; + else + dev_ctrl_state&=~DEV_CTRL_STATE_TEST_LOG; + + if(*log->blok_wy & log->blok_wy_bit_mask) + dev_ctrl_state|=DEV_CTRL_STATE_BLOCK_OUT; + else + dev_ctrl_state&=~DEV_CTRL_STATE_BLOCK_OUT; + + if(*log->test_an & log->test_an_bit_mask) + dev_ctrl_state|=DEV_CTRL_STATE_TEST_AN; + else + dev_ctrl_state&=~DEV_CTRL_STATE_TEST_AN; + + + check_and_set_mask(dev_ctrl_state & DEV_CTRL_STATE_CFG_CHANGE,log->nowe_nast,log->nowe_nast_bit_mask); + dev_ctrl_state&=~DEV_CTRL_STATE_CFG_CHANGE; + check_and_set_mask(dev_ctrl_state & DEV_CTRL_STATE_CFG_ERR,log->blad_konf,log->blad_konf_bit_mask); + check_and_set_mask(dev_ctrl_state & DEV_CTRL_STATE_SD_ERR,log->blad_sd,log->blad_sd_bit_mask); +} + +void dev_ctrl_20hz(void *arguments, void *logic) +{ + struct dev_ctrl_logic *log = (struct dev_ctrl_logic *)logic; + struct time_data irigb_time; + u8 i; + + if(ic->sync_reg & 0xC000) // IRIG-B or PPS + { + log->pps_timeout_cnt2=0; + + if(log->pps_timeout_cnt>100) + dev_ctrl_state|=DEV_CTRL_STATE_PPS_OK; + else + log->pps_timeout_cnt++; + } + else + { + log->pps_timeout_cnt=0; + if(log->pps_timeout_cnt2>100) + dev_ctrl_state&=~DEV_CTRL_STATE_PPS_OK; + else + log->pps_timeout_cnt2++; + } + + if(ic->sync_reg & 0x4000) + { + irigb_time=irigb_process_frame(); + if(timesync_method==SYNC_METHOD_IRIG_B || timesync_method==SYNC_METHOD_IRIG_B_ZPRAE || (timesync_method==SYNC_METHOD_CUSTOM && (timesync_bits & CFG_TSYNC_FROM_DSP))) + { + if(irigb_time.fix_sat & 0x10) + { + if(cur_time.tv_usec>50 && cur_time.tv_usec<950) + cur_time.tv_sec = irigb_time.secs; + + if(cur_time_sw.tv_usec>50 && cur_time_sw.tv_usec<950) + cur_time_sw.tv_sec = irigb_time.secs; + } + } + + if(irigb_time.fix_sat & 0x10) + { + log->fix_timeout_cnt2=0; + if(log->fix_timeout_cnt>100) + dev_ctrl_state|=DEV_CTRL_STATE_IRIGB_FIX_OK; + else + log->fix_timeout_cnt++; +// zrobione juz wyzej - bez rozdzialu na irigb i irigb_zprae +// if((timesync_method==SYNC_METHOD_IRIG_B_ZPRAE || (timesync_method==SYNC_METHOD_CUSTOM && (timesync_bits & CFG_TSYNC_FROM_DSP))) && cur_time.tv_usec>50 && cur_time.tv_usec<950) +// cur_time.tv_sec = irigb_time.secs; + + *log->sat_cnt=irigb_time.fix_sat & 0x0f; + } + else + { + *log->sat_cnt=0; + log->fix_timeout_cnt=0; + if(log->fix_timeout_cnt2>100) + dev_ctrl_state&=~DEV_CTRL_STATE_IRIGB_FIX_OK; + else + log->fix_timeout_cnt2++; + } + } + else + { + *log->sat_cnt=0; + log->fix_timeout_cnt=0; + if(log->fix_timeout_cnt2>100) + dev_ctrl_state&=~DEV_CTRL_STATE_IRIGB_FIX_OK; + else + log->fix_timeout_cnt2++; + } + + if(pps3_timeout_cnt>60000) + clear_struct(&log->pps_sw); + else + set_struct(&log->pps_sw); + + if(check_struct(&log->pps2)) + { + if(!(ic->sync_reg & MKI_PPS_IN)) + ic->sync_reg|=MKI_PPS_IN; + } + else + { + if(ic->sync_reg & MKI_PPS_IN) + ic->sync_reg&=~MKI_PPS_IN; + } + + check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_PPS_OK,&log->pps_ok); + check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_PTP_OK,&log->ptp_ok); + check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_NTP_OK,&log->ntp_ok); + check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_IRIGB_FIX_OK,&log->irigb_fix_ok); + for(i=0;i<8;i++) + check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MGB_CHAN1<mgb_ch[i]); + + check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_CHAN_ZP),&log->mki_zp6); + check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_ERR),&log->iec_err); + + check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN1),&log->mki_iec[0]); + check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN2),&log->mki_iec[1]); + check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN3),&log->mki_iec[2]); + check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN4),&log->mki_iec[3]); + +} diff --git a/src/dev_ctrl.h b/src/dev_ctrl.h new file mode 100644 index 0000000..36c2d44 --- /dev/null +++ b/src/dev_ctrl.h @@ -0,0 +1,142 @@ +/* + * dev_ctrl.h + * + * Created on: 07-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef DEV_CTRL_H_ +#define DEV_CTRL_H_ + +#include "tdefs.h" +#include "helper.h" + +struct dev_ctrl_logic +{ + u8 *pwr1_ok; + u8 pwr1_ok_bit_mask; + u8 *pwr2_ok; + u8 pwr2_ok_bit_mask; + u8 *nowe_nast; + u8 nowe_nast_bit_mask; + u8 *blad_konf; + u8 blad_konf_bit_mask; + u8 *blad_sd; + u8 blad_sd_bit_mask; + + u8 *test_we; + u8 test_we_bit_mask; + u8 *test_wy; + u8 test_wy_bit_mask; + u8 *test_log; + u8 test_log_bit_mask; + u8 *blok_wy; + u8 blok_wy_bit_mask; + u8 *test_an; + u8 test_an_bit_mask; + struct binary_io pps2; + float *sat_cnt; + + struct binary_io mgb_ch[8]; + struct binary_io mki_zp6; + struct binary_io mki_iec[4]; + struct binary_io pps_ok; + struct binary_io ptp_ok; + struct binary_io ntp_ok; + struct binary_io irigb_fix_ok; + struct binary_io iec_err; + struct binary_io pps_sw; + struct binary_io service; + u16 pps_timeout_cnt; + u16 pps_timeout_cnt2; + u16 fix_timeout_cnt; + u16 fix_timeout_cnt2; +}; + +struct dev_ctrl_io +{ + u32 test_we_in; + u32 test_wy_in; + u32 test_log_in; + u32 blok_wy_in; + u32 test_an_in; + u32 pps2_in; + u32 service_mode_in; + u32 pwr1_ok_out; + u32 pwr2_ok_out; + u32 nowe_nast_out; + u32 blad_konf_out; + u32 blad_sd_out; + u32 mgb_ch1_act_out; + u32 mgb_ch2_act_out; + u32 mgb_ch3_act_out; + u32 mgb_ch4_act_out; + u32 mgb_ch5_act_out; + u32 mgb_ch6_act_out; + u32 mgb_ch7_act_out; + u32 mgb_ch8_act_out; + u32 mki_zp6_act_out; + u32 mki_iec1_act_out; + u32 mki_iec2_act_out; + u32 mki_iec3_act_out; + u32 mki_iec4_act_out; + u32 pps_ok_out; + u32 ptp_ok_out; + u32 irigb_fix_ok_out; + u32 iec_err_out; + u32 sat_cnt_float_out; + u32 ntp_ok_out; + u32 pps_sw_out; +}__attribute__((__packed__)); + +struct dev_ctrl_params +{ +}__attribute__((__packed__)); + +struct dev_ctrl_args +{ + struct dev_ctrl_io io; + struct dev_ctrl_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern u32 dev_ctrl_state; +extern u32 service_mode; + +#define DEV_CTRL_STATE_TEST_IN 0x01 +#define DEV_CTRL_STATE_TEST_OUT 0x02 +#define DEV_CTRL_STATE_TEST_LOG 0x04 +#define DEV_CTRL_STATE_BLOCK_OUT 0x08 +#define DEV_CTRL_STATE_TEST_AN 0x10 +#define DEV_CTRL_STATE_CFG_CHANGE 0x0100 +#define DEV_CTRL_STATE_CFG_ERR 0x0200 +#define DEV_CTRL_STATE_SD_ERR 0x0400 +#define DEV_CTRL_STATE_PPS_OK 0x0800 +#define DEV_CTRL_STATE_PTP_OK 0x1000 +#define DEV_CTRL_STATE_IRIGB_FIX_OK 0x2000 +#define DEV_CTRL_STATE_NTP_OK 0x4000 +#define DEV_CTRL_STATE_NTP_HISZP_OK 0x8000 +#define DEV_CTRL_STATE_MGB_CHAN1 0x010000UL +#define DEV_CTRL_STATE_MGB_CHAN2 0x020000 +#define DEV_CTRL_STATE_MGB_CHAN3 0x040000 +#define DEV_CTRL_STATE_MGB_CHAN4 0x080000 +#define DEV_CTRL_STATE_MGB_CHAN5 0x100000 +#define DEV_CTRL_STATE_MGB_CHAN6 0x200000 +#define DEV_CTRL_STATE_MGB_CHAN7 0x400000 +#define DEV_CTRL_STATE_MGB_CHAN8 0x800000 +#define DEV_CTRL_STATE_MKI_CHAN_ZP 0x1000000 +#define DEV_CTRL_STATE_MKI_IEC_ERR 0x2000000 +#define DEV_CTRL_STATE_MKI_UNUSED1 0x4000000 +#define DEV_CTRL_STATE_MKI_UNUSED2 0x8000000 +// +// +#define DEV_CTRL_STATE_MKI_IEC_CHAN1 0x10000000 +#define DEV_CTRL_STATE_MKI_IEC_CHAN2 0x20000000 +#define DEV_CTRL_STATE_MKI_IEC_CHAN3 0x40000000 +#define DEV_CTRL_STATE_MKI_IEC_CHAN4 0x80000000 + +extern int dev_ctrl_initlog(void *arguments, void *logic); +extern void dev_ctrl(void *arguments, void *logic); +extern void dev_ctrl_20hz(void *arguments, void *logic); + +#endif /* DEV_CTRL_H_ */ diff --git a/src/dfr.c b/src/dfr.c new file mode 100644 index 0000000..63c26b0 --- /dev/null +++ b/src/dfr.c @@ -0,0 +1,219 @@ +/* + * dfr.c + * + * Created on: 24-06-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include +#include +#include "dfr.h" +#include "dfr_drv.h" +#include "rec_an.h" +#include "rec_buf.h" +#include "rec_bin.h" +#include "comm.h" +#include "misc.h" +#include "config.h" + +#pragma LOCATION(bank0,0xc4400000) // @ non-cached memory above 128MB +volatile u8 bank0[REG_BANK_SIZE]; +#pragma LOCATION(bank1,0xc4800000) // @ non-cached memory above 128MB +volatile u8 bank1[REG_BANK_SIZE]; +#pragma LOCATION(bank2,0xc4c00000) // @ non-cached memory above 128MB +volatile u8 bank2[REG_BANK_SIZE]; +#pragma LOCATION(bank3,0xc5000000) // @ non-cached memory above 128MB +volatile u8 bank3[REG_BANK_SIZE]; + +volatile struct reg_inf regs = { .cur_reg_bank = 0, + .bank[0].data = (void*) bank0, + .bank[0].state = BANK_BUFFERING, + .bank[0].addr_offset = 0, + .bank[0].pre_trigger_cnt = 0, + .bank[0].post_trigger_cnt = 0, + .bank[0].total_cnt = 0, + .bank[1].data = (void*) bank1, + .bank[2].data = (void*) bank2, + .bank[3].data = (void*) bank3, + }; + +int dfr_initlog(void *arguments, void *logic) +{ + struct dfr_args *args = (struct dfr_args *)arguments; + struct dfr_logic *log = (struct dfr_logic *)logic; + + log->trigger_ptr = log_manager.nets_data + (args->io.trigger_in >> 3); + log->trigger_bit_no = args->io.trigger_in & 0x07; + if(log->trigger_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void dfr(void *arguments, void *logic) +{ + struct dfr_logic *log = (struct dfr_logic *)logic; + struct dfr_args *args = (struct dfr_args *)arguments; + struct rec_an_logic *an_log; + struct rec_buf_logic *an_buf_log; + struct rec_bin_logic *bin_log; + u16 sample_buf[(MAX_SAMPLE_SIZE/sizeof(u16)) + 8]; + int i,bit_cnt=0,pos=0; + int sdiff; + u32 prev_offset; + short *sample_buf_prev; + + if(dfr_drv_log_ptr == NULL) // dfr driver not initialized? + return; + + sdiff=cur_sample_diff; + + while(sdiff) + { + switch(regs.bank[regs.cur_reg_bank].state) + { + case BANK_BUFFERING: + if(*log->trigger_ptr & (1<trigger_bit_no)) + { + regs.bank[regs.cur_reg_bank].state=BANK_TRIGGERED; + regs.bank[regs.cur_reg_bank].tr_time=(timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec; + regs.bank[regs.cur_reg_bank].tr_time_ms=(timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec; + regs.bank[regs.cur_reg_bank].sample_size=dfr_drv_log_ptr->sample_size; + regs.bank[regs.cur_reg_bank].an_count=dfr_drv_log_ptr->an_count; + regs.bank[regs.cur_reg_bank].bin_count=dfr_drv_log_ptr->bin_count; + regs.bank[regs.cur_reg_bank].post_trigger_cnt=0; + regs.bank[regs.cur_reg_bank].trigger_offset=regs.bank[regs.cur_reg_bank].addr_offset; + regs.bank[regs.cur_reg_bank].total_cnt=regs.bank[regs.cur_reg_bank].pre_trigger_cnt; + } + else + { + if(regs.bank[regs.cur_reg_bank].pre_trigger_cntparams.pre_trigger_time) + regs.bank[regs.cur_reg_bank].pre_trigger_cnt++; + } + break; + + case BANK_TRIGGERED: + if(regs.bank[regs.cur_reg_bank].total_cnt++>=args->params.max_reg_time || regs.bank[regs.cur_reg_bank].post_trigger_cnt>=args->params.post_trigger_time) + { + // send notify to ARM + regs.bank[regs.cur_reg_bank].state=BANK_FILLED; + regs.cur_reg_bank++; + regs.cur_reg_bank%=MAX_REG_BANKS; + regs.bank[regs.cur_reg_bank].state=BANK_BUFFERING; + regs.bank[regs.cur_reg_bank].addr_offset=0; + regs.bank[regs.cur_reg_bank].post_trigger_cnt=0; + regs.bank[regs.cur_reg_bank].pre_trigger_cnt=0; + regs.bank[regs.cur_reg_bank].trigger_offset=0; + regs.bank[regs.cur_reg_bank].total_cnt=0; + logman_notify|=LOGMAN_NOTIFY_NEW_DFR; + // Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_NEW_DFR_REG, TRUE); + } + else + { + if(*log->trigger_ptr & (1<trigger_bit_no)) + regs.bank[regs.cur_reg_bank].post_trigger_cnt=0; + else + regs.bank[regs.cur_reg_bank].post_trigger_cnt++; + } + break; + + case BANK_EMPTY: + case BANK_FILLED: + default: + regs.bank[regs.cur_reg_bank].state=BANK_BUFFERING; + regs.bank[regs.cur_reg_bank].addr_offset=0; + regs.bank[regs.cur_reg_bank].post_trigger_cnt=0; + regs.bank[regs.cur_reg_bank].pre_trigger_cnt=0; + regs.bank[regs.cur_reg_bank].trigger_offset=0; + regs.bank[regs.cur_reg_bank].total_cnt=0; + break; + } + + + + // + // will be overwritten by ARM + // *((u32 *)&sample_buf[0]) = 0; // sample number 1..n + // *((u32 *)&sample_buf[2]) = 0; // time stamp 0..n*1ms @ 1 kHz + // + + pos = 4; + + + for(i=0;ian_count;i++) + { + if(regs.bank[regs.cur_reg_bank].addr_offset >= (dfr_drv_log_ptr->sample_size)) + prev_offset = regs.bank[regs.cur_reg_bank].addr_offset - (dfr_drv_log_ptr->sample_size); + else + prev_offset = dfr_drv_log_ptr->max_offset - (dfr_drv_log_ptr->sample_size); + + sample_buf_prev = (short*)((u8*)regs.bank[regs.cur_reg_bank].data+prev_offset+(pos<<1)); + + if(dfr_drv_log_ptr->element_num[i] & ELEMENT_IS_REC_BUF) + { + an_buf_log = (struct rec_buf_logic *)log_manager.log_element[dfr_drv_log_ptr->element_num[i] & 0x3FFF].fun_log_ptr; + + sample_buf[pos]=(short)an_buf_log->buf_ptr[bus_an_cur_sample_num]-0x7FFF; + + if(sdiff>SAMPLES_PER_MS) + { + int snum; + snum = bus_an_cur_sample_num; + snum-= (sdiff-SAMPLES_PER_MS); + if(snum<0) + snum+=(SAMPLES_PER_MS*2*MAIN_FREQ_PERIOD_MS); + *sample_buf_prev=(short)an_buf_log->buf_ptr[snum]-0x7FFF; + } + } + else + { + an_log = (struct rec_an_logic *)log_manager.log_element[dfr_drv_log_ptr->element_num[i] & 0x3FFF].fun_log_ptr; + sample_buf[pos]=*an_log->in_ptr; + + if(sdiff>SAMPLES_PER_MS)//first sample interpolation + { +#if SAMPLES_INTERPOLATION==1 + *sample_buf_prev=((short)*an_log->in_ptr+*sample_buf_prev)>>1; +#elif SAMPLES_INTERPOLATION==2 + *sample_buf_prev=32768; +#else + *sample_buf_prev=sample_buf[pos]; // only phase shift +#endif + } + + } + pos++; + } + + sample_buf[pos]=0; + bit_cnt=0; + + for(;i<(dfr_drv_log_ptr->an_count+dfr_drv_log_ptr->bin_count);i++) + { + bin_log = (struct rec_bin_logic *)log_manager.log_element[dfr_drv_log_ptr->element_num[i] & 0x3FFF].fun_log_ptr; + + if(*bin_log->in_ptr & bin_log->in_bit_mask) + sample_buf[pos]|=(1<<(bit_cnt%16)); + + if(!(++bit_cnt%16) && bit_cnt) + { + pos++; + sample_buf[pos]=0; + } + } + + if(regs.bank[regs.cur_reg_bank].addr_offset + dfr_drv_log_ptr->sample_size > REG_BANK_SIZE) + regs.bank[regs.cur_reg_bank].addr_offset=0; + + memcpy((u8*)regs.bank[regs.cur_reg_bank].data+regs.bank[regs.cur_reg_bank].addr_offset,(void *)sample_buf,dfr_drv_log_ptr->sample_size); + regs.bank[regs.cur_reg_bank].addr_offset+=dfr_drv_log_ptr->sample_size; + + sdiff-=SAMPLES_PER_MS; + if(sdiff<0) + sdiff=0; + } +} diff --git a/src/dfr.h b/src/dfr.h new file mode 100644 index 0000000..2ba2acf --- /dev/null +++ b/src/dfr.h @@ -0,0 +1,84 @@ +/* + * dfr.h + * + * + * Created on: 24-06-2014 + * Author: KJ + */ + +#ifndef DFR_H_ +#define DFR_H_ + +#include "tdefs.h" + +#define MAX_SAMPLE_SIZE 512 + +#define REG_BANK_SIZE 0x400000 +#define AN_IS_FLOAT 0x80000000 + +// bank states +#define BANK_EMPTY 0x00 +#define BANK_BUFFERING 0x01 +#define BANK_TRIGGERED 0x02 +#define BANK_FILLED 0x03 + +#define MAX_REG_BANKS 4 + +struct reg_data +{ + u8 *data; + u8 state; + u32 addr_offset; + u32 pre_trigger_cnt; + u32 post_trigger_cnt; + u32 total_cnt; + u32 trigger_offset; + u32 tr_time; + u32 tr_time_ms; + u16 sample_size; + u8 an_count; + u16 bin_count; +}__attribute__((__packed__)); + +struct reg_inf +{ + u8 cur_reg_bank; + struct reg_data bank[MAX_REG_BANKS]; +}__attribute__((__packed__)); + +extern volatile u8 bank0[REG_BANK_SIZE]; +extern volatile u8 bank1[REG_BANK_SIZE]; +extern volatile u8 bank2[REG_BANK_SIZE]; +extern volatile u8 bank3[REG_BANK_SIZE]; + +extern volatile struct reg_inf regs; + +struct dfr_logic +{ + u8 *trigger_ptr; + u8 trigger_bit_no; +}; + +struct dfr_io +{ + u32 trigger_in; +}__attribute__((__packed__)); + +struct dfr_params +{ + u32 pre_trigger_time; + u32 post_trigger_time; + u32 max_reg_time; +}__attribute__((__packed__)); + +struct dfr_args +{ + struct dfr_io io; + struct dfr_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int dfr_initlog(void *args, void *logic); +extern void dfr(void *args, void *logic); + +#endif /* DFR_H_ */ diff --git a/src/dfr_drv.c b/src/dfr_drv.c new file mode 100644 index 0000000..25de394 --- /dev/null +++ b/src/dfr_drv.c @@ -0,0 +1,122 @@ +/* + * dfr_drv.c + * + * Created on: 23-10-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include +#include "rec_bin.h" +#include "rec_an.h" +#include "rec_buf.h" +#include "rec_float.h" +#include "dfr.h" +#include "dfr_drv.h" + +struct dfr_drv_logic *dfr_drv_log_ptr = NULL; + +int dfr_drv_initlog(void *arguments, void *logic) +{ + struct dfr_drv_logic *log = (struct dfr_drv_logic *)logic; + int i; + + log->an_count=0; + log->bin_count=0; + + for(i=0;iparams.bits & REC_CHAN_ENABLED_DFR)) + continue; + } + else if(log_manager.log_element[i].flags & FUNCTION_IS_REC_BUF) + { + struct rec_buf_args *args; + + args = (struct rec_buf_args *)log_manager.log_element[i].fun_args_ptr; + if(!(args->params.bits & REC_CHAN_ENABLED_DFR)) + continue; + } + else + { + struct rec_float_args *args; + + args = (struct rec_float_args *)log_manager.log_element[i].fun_args_ptr; + if(!(args->params.bits & REC_CHAN_ENABLED_DFR)) + continue; + } + + + if((u8 *)&log->element_num[log->an_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->an_count]=i|((log_manager.log_element[i].flags & FUNCTION_IS_REC_FLOAT)?ELEMENT_IS_REC_FLOAT:(log_manager.log_element[i].flags & FUNCTION_IS_REC_BUF)?ELEMENT_IS_REC_BUF:0); + log->an_count++; + } + } + + log->sample_size=log->an_count * sizeof(u16); + + for(i=0;iparams.bits & REC_CHAN_ENABLED_DFR)) + { + if((u8 *)&log->element_num[log->an_count+log->bin_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->an_count+log->bin_count]=i; + log->bin_count++; + } + } + + log->sample_size+=((log->bin_count/16)*2); + if(log->bin_count%16) + log->sample_size+=2; + + log->sample_size+=8; // sample number && time stamp + + if(log->sample_size>MAX_SAMPLE_SIZE) + return -1; + + for(i=0;iparams.max_reg_time > ((REG_BANK_SIZE / log->sample_size) - 2)) + args->params.max_reg_time=((REG_BANK_SIZE / log->sample_size) - 2); + //return -1; + + if((args->params.post_trigger_time + args->params.pre_trigger_time) > args->params.max_reg_time) + { + args->params.post_trigger_time=args->params.max_reg_time/2; + args->params.pre_trigger_time=args->params.max_reg_time/2; +// return -1; + } + } + } + + log->max_offset=REG_BANK_SIZE/dfr_drv_log_ptr->sample_size*dfr_drv_log_ptr->sample_size; + + dfr_drv_log_ptr=(struct dfr_drv_logic *)logic; + + return 0; +} + diff --git a/src/dfr_drv.h b/src/dfr_drv.h new file mode 100644 index 0000000..3db0294 --- /dev/null +++ b/src/dfr_drv.h @@ -0,0 +1,45 @@ +/* + * dfr_drv.h + * + * + * Created on: 23-10-2014 + * Author: KJ + */ + +#ifndef DFR_DRV_H_ +#define DFR_DRV_H_ + +#include "tdefs.h" + +#define ELEMENT_IS_REC_FLOAT 0x8000 +#define ELEMENT_IS_REC_BUF 0x4000 + +extern struct dfr_drv_logic *dfr_drv_log_ptr; + +struct dfr_drv_logic +{ + u16 an_count; + u16 bin_count; + u16 sample_size; + u32 max_offset; + u16 element_num[]; +}; + +struct dfr_drv_io +{ +}__attribute__((__packed__)); + +struct dfr_drv_params +{ +}__attribute__((__packed__)); + +struct dfr_drv_args +{ + struct dfr_drv_io io; + struct dfr_drv_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int dfr_drv_initlog(void *args, void *logic); + +#endif /* DFR_DRV_H_ */ diff --git a/src/e_addvec.c b/src/e_addvec.c new file mode 100644 index 0000000..42eae6b --- /dev/null +++ b/src/e_addvec.c @@ -0,0 +1,69 @@ +/* + * e_addvec.c + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_addvec.h" +#include "analog_in.h" +#include + +int e_addvec_initlog(void *arguments, void *logic) +{ + struct e_addvec_args *args = (struct e_addvec_args *)arguments; + struct e_addvec_logic *log = (struct e_addvec_logic *)logic; + + log->orta1_ptr = (float *)(log_manager.nets_data + (args->io.orta1_float_in >> 3)); + if((u8 *)log->orta1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb1_ptr = (float *)(log_manager.nets_data + (args->io.ortb1_float_in >> 3)); + if((u8 *)log->ortb1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta2_ptr = (float *)(log_manager.nets_data + (args->io.orta2_float_in >> 3)); + if((u8 *)log->orta2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb2_ptr = (float *)(log_manager.nets_data + (args->io.ortb2_float_in >> 3)); + if((u8 *)log->ortb2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_addvec(void *arguments, void *logic) +{ + struct e_addvec_args *args = (struct e_addvec_args *)arguments; + struct e_addvec_logic *log = (struct e_addvec_logic *)logic; + + float orta, ortb; + + orta = *log->orta1_ptr + *log->orta2_ptr; + ortb = *log->ortb1_ptr + *log->ortb2_ptr; + + if(args->io.esk_float_out) + *log->esk_ptr=(orta*orta + ortb*ortb)/2; + + if(args->io.orta_float_out) + *log->orta_ptr = orta; + + if(args->io.ortb_float_out) + *log->ortb_ptr = ortb; +} diff --git a/src/e_addvec.h b/src/e_addvec.h new file mode 100644 index 0000000..2244a32 --- /dev/null +++ b/src/e_addvec.h @@ -0,0 +1,49 @@ +/* + * e_addvec.h + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_ADDVEC_H_ +#define E_ADDVEC_H_ + +#include "tdefs.h" + +struct e_addvec_logic +{ + float *orta1_ptr; + float *ortb1_ptr; + float *orta2_ptr; + float *ortb2_ptr; + float *orta_ptr; + float *ortb_ptr; + float *esk_ptr; +}; + +struct e_addvec_io +{ + u32 orta1_float_in; + u32 ortb1_float_in; + u32 orta2_float_in; + u32 ortb2_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_addvec_params +{ +}__attribute__((__packed__)); + +struct e_addvec_args +{ + struct e_addvec_io io; + struct e_addvec_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_addvec(void *args, void *logic); +extern int e_addvec_initlog(void *arguments, void *logic); + +#endif /* E_ADDVEC_H_ */ diff --git a/src/e_avg.c b/src/e_avg.c new file mode 100644 index 0000000..4149a04 --- /dev/null +++ b/src/e_avg.c @@ -0,0 +1,84 @@ +/* + * e_avg.c + * + * Created on: 12-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "e_avg.h" +#include "analog_in.h" +#include + +int e_avg_initlog(void *arguments, void *logic) +{ + struct e_avg_args *args = (struct e_avg_args *)arguments; + struct e_avg_logic *log = (struct e_avg_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->avg_ptr = (float *)(log_manager.nets_data + (args->io.avg_float_out >> 3)); + if((u8 *)log->avg_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->savg_ptr = (float *)(log_manager.nets_data + (args->io.savg_float_out >> 3)); + if((u8 *)log->savg_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->multiplier = an_params->multiplier; /* (float)1.1107 */; + log->multiplier /= (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS); + log->offset = 32767 + an_params->offset; + + return 0; +} + +void e_avg(void *arguments, void *logic) +{ + struct e_avg_args *args = (struct e_avg_args *)arguments; + struct e_avg_logic *log = (struct e_avg_logic *)logic; + short i,i1,i2; + float skut; + float pr; + + i2 = bus_an_cur_sample_num; + skut = 0; + + for (i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++/*=2*/) + { + i1 = i2 - i; + if(i1<0) + i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + pr = (float)log->buf_ptr[i1] - log->offset; + skut+=fabs(pr); + } + + skut*=log->multiplier; + + if(args->io.avg_float_out) + *log->avg_ptr=skut; + + if(args->io.savg_float_out) + *log->savg_ptr=skut*skut; +} diff --git a/src/e_avg.h b/src/e_avg.h new file mode 100644 index 0000000..b33624e --- /dev/null +++ b/src/e_avg.h @@ -0,0 +1,44 @@ +/* + * e_avg.h + * + * Created on: 12-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_AVG_H_ +#define E_AVG_H_ + +#include "tdefs.h" + +struct e_avg_logic +{ + float multiplier; + float offset; + u16 *buf_ptr; + float *avg_ptr; + float *savg_ptr; +}; + +struct e_avg_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 avg_float_out; + u32 savg_float_out; +}__attribute__((__packed__)); + +struct e_avg_params +{ +}__attribute__((__packed__)); + +struct e_avg_args +{ + struct e_avg_io io; + struct e_avg_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_avg(void *args, void *logic); +extern int e_avg_initlog(void *arguments, void *logic); + +#endif /* E_AVG_H_ */ diff --git a/src/e_avg_1t.c b/src/e_avg_1t.c new file mode 100644 index 0000000..32b34c4 --- /dev/null +++ b/src/e_avg_1t.c @@ -0,0 +1,109 @@ +/* + * e_avg_1t.c + * + * Created on: 17-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_avg_1t.h" +#include "analog_in.h" +#include +#include + +int e_avg_1t_initlog(void *arguments, void *logic) +{ + struct e_avg_1t_args *args = (struct e_avg_1t_args *)arguments; + struct e_avg_1t_logic *log = (struct e_avg_1t_logic *)logic; + +// an_params=(struct analog_in_params *)*an_params_ptr; + + log->probka_ptr = (float *)(log_manager.nets_data + (args->io.probka_float_in >> 3)); + if((u8 *)log->probka_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->avg_t_ptr = (float *)(log_manager.nets_data + (args->io.avg_t_float_out >> 3)); + if((u8 *)log->avg_t_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + memset(&log->plus, 0x00, sizeof(log->plus)); + memset(&log->minus, 0x00, sizeof(log->minus)); + log->licz_zero = 0; + memset(&log->stan_, 0x00, sizeof(log->stan_)); // zerowanie wyjsc + + return 0; +} + +wylicz_f(struct e_avg_1t_logic *log, struct e_avg_1t_dane_wew *d) +{ + float dopelnienie; + float okres; + + + //wyliczenie okresu pojedynczej iteracji + dopelnienie = log->probka_n_minus1 / (log->probka_n_minus1 - log->probka_n_); + okres = dopelnienie + d->reszta_ + d->il_p_; + log->stan_.n++; // zmiana znaczika przejscia przez zero + // + + log->stan_.Usr = d->war_sr_ / okres; //wartosc srednia wyprostowana + log->stan_.Uperf = d->war_sr_;// wyliczenie ilorazu wartosci sredniej wyprostowanej z czestotliwoscia + log->stan_.T = okres;// okres przebiegu w ilosciach probek + + d->reszta_ = 1 - dopelnienie; + d->war_sr_ = 0; //zerowanie licznika wartosci sredniej wyprostowanej przy przejsciu przez zero + d->il_p_ = 0;//zerowanie licznika pelnych probek(odliczanie nowego okresu) +} + +void e_avg_1t(void *arguments, void *logic) +{ + struct e_avg_1t_args *args = (struct e_avg_1t_args *)arguments; + struct e_avg_1t_logic *log = (struct e_avg_1t_logic *)logic; + + u8 przejscie_z_plusa = 0; + u8 przejscie_z_minusa = 0; + + log->probka_n_ = *log->probka_ptr;/* * (float)1.1107;*/ + + //szukanie przejsc przez zero + if ((log->probka_n_minus1 <0 ) && (log->probka_n_ >= 0)) przejscie_z_minusa = 1; + if ((log->probka_n_minus1 >0 ) && (log->probka_n_ <= 0)) przejscie_z_plusa = 1; + + // + + if(fabs(log->probka_n_)< 0.01) log->licz_zero++; else log->licz_zero = 0; + + if (log->plus.il_p_> ((MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*10) || log->minus.il_p_ > ((MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*10) || log->licz_zero > (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)) + { + log->licz_zero = 0; + log->stan_.n++; // zmiana znaczika przejscia przez zero + log->stan_.Usr = 0; //wartosc srednia wyprostowana + log->stan_.Uperf = 0;// wyliczenie ilorazu wartosci sredniej wyprostowanej z czestotliwoscia + log->stan_.T = (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*10+1;// okres przebiegu w ilosciach probek + log->plus.il_p_ = 0; + log->minus.il_p_ = 0; + } + + log->plus.war_sr_ +=fabs(log->probka_n_); //wyliczanie wartosci sredniej wyprostowanej za okres dla okresu od przejscia z plusa + log->minus.war_sr_ +=fabs(log->probka_n_); //wyliczanie wartosci sredniej wyprostowanej za okres dla okresu od przejscia z minusa + + if(przejscie_z_plusa) + { + wylicz_f(log,&log->plus); // wyliczenie nowej czestotliwosci + } else log->plus.il_p_++; + + if(przejscie_z_minusa) + { + wylicz_f(log,&log->minus); //wyliczenie nowej czestotliwosci + } else log->minus.il_p_++; + + log->probka_n_minus1 = log->probka_n_;//zapamietanie poprzedniej probki + + //wskazniki[nast.adr.bufor] = (void *)&stan_; + + if(args->io.avg_t_float_out) + *log->avg_t_ptr=log->stan_.Usr; + +} diff --git a/src/e_avg_1t.h b/src/e_avg_1t.h new file mode 100644 index 0000000..9aef79c --- /dev/null +++ b/src/e_avg_1t.h @@ -0,0 +1,61 @@ +/* + * e_avg_1t.h + * + * Created on: 17-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_AVG_1T_H_ +#define E_AVG_1T_H_ + +#include "tdefs.h" + +struct e_avg_1t_dane_wew +{ + float war_sr_; ///< wartosc srednia wyprostowana + u16 il_p_; ///< ilosc pelnych probek w okresie pomiarowym + float reszta_; ///< odcinek sinusa z poczatku (niepelna probka) +}; + +struct e_avg_1t_Wyjscie +{ + float Usr; ///< wartosc srednia wyprostowana + float Uperf; ///< iloraz wartosci sredniej wyprostowanej i czestotliwosci + float T; ///< Okres przebiegu podany jako krotnosc okresu probkowania + u16 n; ///< licznik kolejnych przejsc przez 0 +}; + +struct e_avg_1t_logic +{ + + float probka_n_minus1; // pamiec poprzedniej probki + float probka_n_; // probka biezaca + short licz_zero; + struct e_avg_1t_dane_wew plus; //zmienne pomocnicze dla obliczen okresu od plusa + struct e_avg_1t_dane_wew minus; //zmienne pomocnicze dla obliczen okresu od minusa + struct e_avg_1t_Wyjscie stan_; + float *probka_ptr; + float *avg_t_ptr; +}; + +struct e_avg_1t_io +{ + u32 probka_float_in; + u32 avg_t_float_out; +}__attribute__((__packed__)); + +struct e_avg_1t_params +{ +}__attribute__((__packed__)); + +struct e_avg_1t_args +{ + struct e_avg_1t_io io; + struct e_avg_1t_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_avg_1t(void *args, void *logic); +extern int e_avg_1t_initlog(void *arguments, void *logic); + +#endif /* E_AVG_1T_H_ */ diff --git a/src/e_avg_dt.c b/src/e_avg_dt.c new file mode 100644 index 0000000..52ddb5d --- /dev/null +++ b/src/e_avg_dt.c @@ -0,0 +1,139 @@ +/* + * e_avg_dt.c + * + * Created on: 31-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "e_avg_dt.h" +#include "analog_in.h" +#include +#include + +int e_avg_dt_initlog(void *arguments, void *logic) +{ + struct e_avg_dt_args *args = (struct e_avg_dt_args *)arguments; + struct e_avg_dt_logic *log = (struct e_avg_dt_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->avg_dt_ptr = (float *)(log_manager.nets_data + (args->io.avg_dt_float_out >> 3)); + if((u8 *)log->avg_dt_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + memset(&log->bufor, 0x00, sizeof(log->bufor)); // zerowanie bufora pamieci srednich wartosci za polokresu + log->n = (short)(args->params.dt * (SAMPLES_PER_MS*1000)); // przeliczenie milisekund na ilosc probek + log->mnoznik = an_params->multiplier/log->n; + log->n10 = (short)(args->params.dt * (1000/LOOP_CYCLE_MS) / (MAIN_FREQ_PERIOD_MS/2)); // ilosc polokresow usredniania + log->mnoznik10 = an_params->multiplier / log->n10 / (SAMPLES_PER_MS*10); + log->licznik_polokresow = 0; + log->licznik_probek = 0; + log->zakres = 1; + if (log->n > (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)) + log->zakres = 0; + + *log->avg_dt_ptr = 0; + log->offset = 32767 + an_params->offset; + log->offset10 = log->offset * log->n10 * (SAMPLES_PER_MS*10); + + return 0; +} + +void e_avg_dt(void *arguments, void *logic) +{ + struct e_avg_dt_args *args = (struct e_avg_dt_args *)arguments; + struct e_avg_dt_logic *log = (struct e_avg_dt_logic *)logic; + + float wynik; + float pr; + int i; + + if(log->zakres) //jesli nastawa mniejsza od wielkosci bufora probek - srednia co i probka + { + wynik = 0; + log->licznik_probek = bus_an_cur_sample_num; + for (i=0;in;i++) + { + pr = (float)log->buf_ptr[log->licznik_probek] - log->offset; + wynik += fabs(pr);//(float)log->buf_ptr[log->licznik_probek]; + log->licznik_probek--; + if(log->licznik_probek<0) + log->licznik_probek +=(SAMPLES_PER_MS*2*MAIN_FREQ_PERIOD_MS); + } + + //wynik -= log->offset; + wynik *= log->mnoznik; + + if(args->io.avg_dt_float_out) + *log->avg_dt_ptr=wynik; + } +} + +void e_avg_dt_100hz(void *arguments, void *logic) +{ + struct e_avg_dt_args *args = (struct e_avg_dt_args *)arguments; + struct e_avg_dt_logic *log = (struct e_avg_dt_logic *)logic; + + float wynik; + float pr; + int i; + + if (!log->zakres) + { // jezeli nastawa wieksza od bufora probek - srednia co polokresu + log->licznik_probek = bus_an_cur_sample_num; + wynik = 0; + for (i=0;i<(SAMPLES_PER_MS*10);i++) // wyliczenie sredniej za ostatnie polokresu + { + pr = (float)log->buf_ptr[log->licznik_probek] - log->offset; + wynik += fabs(pr); + log->licznik_probek--; + if(log->licznik_probek<0) + log->licznik_probek +=(SAMPLES_PER_MS*2*MAIN_FREQ_PERIOD_MS); + } + + if(log->licznik_polokresow<20) + log->bufor[log->licznik_polokresow] = wynik; + + wynik = 0; + // wyliczenie sredniej za czas nastawiony + for (i=0;in10;i++ ) + { + wynik += log->bufor[i]; + } + //wynik -= log->offset10; + wynik *= log->mnoznik10; + // + + log->licznik_polokresow++; + if(log->licznik_polokresow>log->n10) // aktualizacja wskaznika bufora + { + log->licznik_polokresow = 0; + } + + if(args->io.avg_dt_float_out) + *log->avg_dt_ptr=wynik; + } +} diff --git a/src/e_avg_dt.h b/src/e_avg_dt.h new file mode 100644 index 0000000..e7d908a --- /dev/null +++ b/src/e_avg_dt.h @@ -0,0 +1,52 @@ +/* + * e_avg_1t.h + * + * Created on: 01-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_AVG_DT_H_ +#define E_AVG_DT_H_ + +#include "tdefs.h" + +struct e_avg_dt_logic +{ + u16 *buf_ptr; + float *avg_dt_ptr; + float mnoznik; + float mnoznik10; + float offset; + float offset10; + float bufor[20]; + short n; + short n10; + short licznik_probek; //licznik petli - zmienna pomocnicza + short licznik_polokresow; //licznik polokresow - zmienna pomocnicza + u8 zakres; +}; + +struct e_avg_dt_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 avg_dt_float_out; +}__attribute__((__packed__)); + +struct e_avg_dt_params +{ + double dt; +}__attribute__((__packed__)); + +struct e_avg_dt_args +{ + struct e_avg_dt_io io; + struct e_avg_dt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_avg_dt(void *args, void *logic); +extern void e_avg_dt_100hz(void *args, void *logic); +extern int e_avg_dt_initlog(void *arguments, void *logic); + +#endif /* E_AVG_DT_H_ */ diff --git a/src/e_cu.c b/src/e_cu.c new file mode 100644 index 0000000..803960e --- /dev/null +++ b/src/e_cu.c @@ -0,0 +1,221 @@ +/* + * e_cu.c + * + * Created on: 24-06-2016 + * Author: Krzysztof Jakubczyk + * + * Wylicza calke oznaczona napiecia w nastawionym oknie czasowym + * + * + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_cu.h" +#include "e_mem1.h" +#include + +int e_cu_initlog(void *arguments, void *logic) +{ + struct e_cu_args *args = (struct e_cu_args *)arguments; + struct e_cu_logic *log = (struct e_cu_logic *)logic; + + u32 *buf_ptr; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + log->we = (struct e_mem1_wyjscie *)*buf_ptr; + + log->ok_ptr = log_manager.nets_data + (args->io.ok_out >> 3); + log->ok_bit_no = args->io.ok_out & 0x07; + + if(log->ok_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if ((int16_t)(args->params.czas_ - 16) % 2) + { + log->zakres_ = od16do30_nieparzyste; + } else + { + log->zakres_ = od16do30_parzyste; + } + + // wyliczanie polozenia wlasciwych wartosci w pamieci + log->ofset_ = (int16_t)((args->params.czas_ -16) / 0.4); + + if(args->params.czas_ < 16) + { + log->zakres_ = od8do16; + log->ofset_ = (int16_t)((args->params.czas_ - 8) / 0.2); + } + + if(args->params.czas_ < 8) + { + log->zakres_ = od4do8; + log->ofset_ = (int16_t)((args->params.czas_ - 4) / 0.1); + } + + if(args->params.czas_ < 4) + { + log->zakres_ = mniej4; + log->ofset_ = (int16_t)(args->params.czas_ / 0.05); + } + + log->czas = (int16_t)(100 * args->params.czas_); + log->Ubl_ = args->params.Ubl_; + + if(args->params.bity & 0x0001) + log->Uwe = 1; + else + log->Uwe = 0.57735; //jeden przez pierwiastek z 3 + // + + return 0; +} + +void e_cu_20hz(void *arguments, void *logic) +{ + struct e_cu_logic *log = (struct e_cu_logic *)logic; + struct e_mem1_wyjscie *w = log->we; + + short temp; + float temp2,temp3; + int i; + u8 OK; + + temp2 = 0; + temp3 = 0; + + if (w->czas_>=log->czas) OK = 1; else OK = 0; + + switch(log->zakres_)// wyliczanie w zaleznosci od zakresu nastawy czasu + { + case mniej4: + temp = w->idx_50ms_ - log->ofset_; // wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 80; // uwzglednienie "przekrecania" bufora" + //wyliczenie calki za czas nastawiony + while(temp!=w->idx_50ms_) + { + temp++; + if(temp>79) temp = 0; + temp2 += w->srednia_50ms_[temp]; + if (w->srednia_50ms_[temp] < log->Ubl_) OK = 0; + } + temp2 /= log->ofset_; + // + break; + case od4do8: + temp = w->idx_100ms_ - log->ofset_;// wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 40;// uwzglednienie "przekrecania" bufora" + for (i=0;i<80;i++) + { + temp3 += w->srednia_50ms_[i];//wyliczanie calki za czas od 0-4s; + if (w->srednia_50ms_[temp] < log->Ubl_) OK = 0; + } + temp2 = temp3 / 2; + //doliczenie calki za czesc czasu z zakresu powyzej 4s + temp3 = 0; + while(temp!=w->idx_100ms_) + { + temp++; + if(temp>39) temp = 0; + temp3 += w->srednia_100ms_[temp]; + if (w->srednia_100ms_[temp] < log->Ubl_) OK = 0; + } + temp2 += temp3; + temp2 /= 40 + log->ofset_; // calka za caly czas okna pomiarowego + // + break; + case od8do16: + temp = w->idx_200ms_ - log->ofset_;// wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 40;// uwzglednienie "przekrecania" bufora" + for (i=0;i<80;i++) + { + temp3 += w->srednia_50ms_[i];//wyliczanie calki za czas od 0-4s; + if (w->srednia_50ms_[temp] < log->Ubl_) OK = 0; + } + temp2 = temp3 / 4; + temp3 = 0; + for (i=0;i<40;i++) + { + temp3 += w->srednia_100ms_[i];//wyliczanie calki za czas od 4-8s; + if (w->srednia_100ms_[temp] < log->Ubl_) OK = 0; + } + temp2 += temp3 / 2; + //doliczenie calki za czesc czasu z zakresu powyzej 8s + temp3 = 0; + while(temp!=w->idx_200ms_) + { + temp++; + if(temp>39) temp = 0; + temp3 += w->srednia_200ms_[temp]; + if (w->srednia_200ms_[temp] < log->Ubl_) OK = 0; + } + temp2 += temp3; + temp2 /= 40 + log->ofset_; // calka za caly czas okna pomiarowego + // + break; + case od16do30_parzyste: + case od16do30_nieparzyste: + temp = w->idx_400ms_ - log->ofset_;// wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 36;// uwzglednienie "przekrecania" bufora" + for (i=0;i<80;i++) + { + temp3 += w->srednia_50ms_[i];//wyliczanie calki za czas od 0-4s; + if (w->srednia_50ms_[temp] < log->Ubl_) OK = 0; + } + temp2 = temp3 / 8; + temp3 = 0; + for (i=0;i<40;i++) + { + temp3 += w->srednia_100ms_[i];//wyliczanie calki za czas od 4-8s; + if (w->srednia_100ms_[temp] < log->Ubl_) OK = 0; + } + temp2 += temp3 / 4; + temp3 = 0; + for (i=0;i<40;i++) + { + temp3 += w->srednia_200ms_[i];//wyliczanie calki za czas od 8-16s; + if (w->srednia_200ms_[temp] < log->Ubl_) OK = 0; + } + temp2 += temp3 / 2; + //doliczenie calki za czesc czasu z zakresu powyzej 16s + temp3 = 0; + while(temp!=w->idx_400ms_) + { + temp++; + if(temp>35) temp = 0; + temp3 += w->srednia_400ms_[temp]; + if (w->srednia_400ms_[temp] < log->Ubl_) OK = 0; + } + if (log->zakres_ == od16do30_nieparzyste) + { + temp3 += w->srednia_400ms_[temp] / 2; + temp2 += temp3; + temp2 /= (float)(0.5 + 40 + log->ofset_); // calka za caly czas okna pomiarowego + } else { + temp2 += temp3; + temp2 /= 40 + log->ofset_; // calka za caly czas okna pomiarowego + } + // + break; + } + + if (OK) + { + *log->esk_ptr = temp2 - log->Uwe;//przepisanie wyliczonej wartosci na wyjscie + *log->ok_ptr|=(1<ok_bit_no); + } + else + { + *log->esk_ptr = 0; + *log->ok_ptr&=~(1<ok_bit_no); + } +} diff --git a/src/e_cu.h b/src/e_cu.h new file mode 100644 index 0000000..6b0de3f --- /dev/null +++ b/src/e_cu.h @@ -0,0 +1,62 @@ +/* + * e_cu.h + * + * Created on: 24-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_CU_IN_H_ +#define E_CU_IN_H_ + +#include "tdefs.h" +#include "e_mem1.h" + +enum e_cu_zakres +{ + mniej4 = 0, + od4do8, + od8do16, + od16do30_parzyste, + od16do30_nieparzyste +}; + +struct e_cu_logic +{ + float *esk_ptr; + struct e_mem1_wyjscie *we; + u8 *ok_ptr; + u8 ok_bit_no; + + enum e_cu_zakres zakres_; ///< zakres bufora pamieci + short czas; ///< ilosc petli 10ms w oknie calkowania + short ofset_; ///< przesuniecie danego bufora pamieci + float Ubl_; ///< wartosc napiecia blokady w krotnosciach napiecia znamionowego + float Uwe; ///< stala odpowiadajaca napieciu (fazowe/miedzyfazowe) +}; + +struct e_cu_io +{ + u32 buf_ptr_in; + u32 esk_float_out; + u32 ok_out; +}__attribute__((__packed__)); + +struct e_cu_params +{ + u32 bity; + double czas_; + double Ubl_; +}__attribute__((__packed__)); + +struct e_cu_args +{ + struct e_cu_io io; + struct e_cu_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_cu(void *args, void *logic); +extern void e_cu_20hz(void *args, void *logic); +extern int e_cu_initlog(void *arguments, void *logic); + +#endif /* E_CU_H_ */ diff --git a/src/e_df_3f.c b/src/e_df_3f.c new file mode 100644 index 0000000..32394ce --- /dev/null +++ b/src/e_df_3f.c @@ -0,0 +1,306 @@ +/* + * e_df_3f.c + * + * Created on: 30-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_df_3f.h" +#include "analog_in.h" +#include + +int e_df_3f_initlog(void *arguments, void *logic) +{ + struct e_df_3f_args *args = (struct e_df_3f_args *)arguments; + struct e_df_3f_logic *log = (struct e_df_3f_logic *)logic; + + u32 *f_ptr; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f1_ptr_in >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + log->f1=(struct e_f_wyjscie *)*f_ptr; + + if(!*f_ptr) + return -1; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f2_ptr_in >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + log->f2=(struct e_f_wyjscie *)*f_ptr; + + if(!*f_ptr) + return -1; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f3_ptr_in >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + log->f3=(struct e_f_wyjscie *)*f_ptr; + + if(!*f_ptr) + return -1; + + log->df_ptr = (float *)(log_manager.nets_data + (args->io.df_float_out >> 3)); + if((u8 *)log->df_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ok_ptr = log_manager.nets_data + (args->io.ok_out >> 3); + log->ok_bit_no = args->io.ok_out & 0x07; + + if(log->ok_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +struct e_df_3f_wyjscie wylicz_df(struct e_f_wyjscie *f) +{ + int16_t idxp; + float df; + float dt; + struct e_df_3f_wyjscie temp; + int16_t idx = f->idx_; + uint16_t temp_OK; + temp.OK_ = 0; + + if (f->OK_[f->idx_]) // jesli ostatni pomiar czestotliwosci poprawny + { + //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 1 okres + idxp = f->idx_ - 2; + if(idxp<0) idxp +=300; + // + if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego + { + //wyliczenie roznicy czestotliwosci dla okna 1 okres + df = f->f_[f->idx_] - f->f_[idxp]; + //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej + // jezeli za maly to wyliczenie z wiekszego okna + if(fabs(df)>0.08) + { + if (f->OK_[f->idx_ - 1]) + { + temp.df_ = df * f->f_[f->idx_]; + } else { + if (f->OK_[f->idx_] > f->OK_[idxp]) + { + dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f; + } else { + dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f; + } + temp.df_ = df / dt; + } + temp.OK_ = 1; + } + } + + + if (!temp.OK_) + { + //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 2 okresy + idxp = f->idx_ - 4; + if(idxp<0) idxp +=300; + // + if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego + { + + //wyliczenie roznicy czestotliwosci dla okna 2 okresy + df = f->f_[f->idx_] - f->f_[idxp]; + //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej + // jezeli za maly to wyliczenie z wiekszego okna + if(fabs(df)>0.08) + { + idx = f->idx_; + temp_OK = 1; + while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci + { + idx--; + if(idx<0) idx +=300; + if (!f->OK_[idx]) temp_OK = 0; + } + + if (temp_OK) + { + temp.df_ = (float)(0.25 * df * (f->f_[f->idx_] + f->f_[idxp])); + } else { + if (f->OK_[f->idx_] > f->OK_[idxp]) + { + dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f; + } else { + dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f; + } + temp.df_ = df / dt; + } + temp.OK_ = 1; + } + } + } + + + if (!temp.OK_) + { + //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 4 okresy + idxp = f->idx_ - 8; + if(idxp<0) idxp +=300; + // + if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego + { + + //wyliczenie roznicy czestotliwosci dla okna 4 okresy + df = f->f_[f->idx_] - f->f_[idxp]; + //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej + // jezeli za maly to wyliczenie z wiekszego okna + if(fabs(df)>0.08) + { + idx = f->idx_; + temp_OK = 1; + while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci + { + idx--; + if(idx<0) idx +=300; + if (!f->OK_[idx]) temp_OK = 0; + } + + if (temp_OK) + { + temp.df_ = (float)(0.125 * df * (f->f_[f->idx_] + f->f_[idxp])); + } else { + if (f->OK_[f->idx_] > f->OK_[idxp]) + { + dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f; + } else { + dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f; + } + temp.df_ = df / dt; + } + temp.OK_ = 1; + } + } + } + + + + + if (!temp.OK_) + { + //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 7 okresow + idxp = f->idx_ - 14; + if(idxp<0) idxp +=300; + // + if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego + { + + //wyliczenie roznicy czestotliwosci dla okna 7 okresow + df = f->f_[f->idx_] - f->f_[idxp]; + //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej + // jezeli za maly to wyliczenie z wiekszego okna + if(fabs(df)>0.08) + { + idx = f->idx_; + temp_OK = 1; + while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci + { + idx--; + if(idx<0) idx +=300; + if (!f->OK_[idx]) temp_OK = 0; + } + + if (temp_OK) + { + temp.df_ = (float)(0.07143 * df * (f->f_[f->idx_] + f->f_[idxp])); + } else { + if (f->OK_[f->idx_] > f->OK_[idxp]) + { + dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f; + } else { + dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f; + } + temp.df_ = df / dt; + } + temp.OK_ = 1; + } + } + } + + + if (!temp.OK_) + { + //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 10 okresy + idxp = f->idx_ - 20; + if(idxp<0) idxp +=300; + // + if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego + { + + //wyliczenie roznicy czestotliwosci dla okna 10 okresow + df = f->f_[f->idx_] - f->f_[idxp]; + + // wyliczenie pochodnej dla okna 10 okresow + idx = f->idx_; + temp_OK = 1; + while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci + { + idx--; + if(idx<0) idx +=300; + if (!f->OK_[idx]) temp_OK = 0; + } + + if (temp_OK) + { + temp.df_ = (float)(0.05 * df * (f->f_[f->idx_] + f->f_[idxp])); + } else { + if (f->OK_[f->idx_] > f->OK_[idxp]) + { + dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f; + } else { + dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f; + } + temp.df_ = df / dt; + } + temp.OK_ = 1; + } + } + } + + return(temp); +} + +void e_df_3f_100hz(void *arguments, void *logic) +{ + struct e_df_3f_logic *log = (struct e_df_3f_logic *)logic; + + float temp = 0; + int16_t temp1 = 0; + struct e_df_3f_wyjscie temp2; + + temp2 = wylicz_df(log->f1); + if(temp2.OK_) //jesli pomiar w kanale 1 sprawny + { + temp += temp2.df_; + temp1 +=1; + } + temp2 = wylicz_df(log->f2); + if(temp2.OK_) //jesli pomiar w kanale 2 sprawny + { + temp += temp2.df_; + temp1 +=1; + } + temp2 = wylicz_df(log->f3); + if(temp2.OK_) //jesli pomiar w kanale 3 sprawny + { + temp += temp2.df_; + temp1 +=1; + } + if(temp1)//jesli co najmniej jeden pomiar sprawny + { + *log->df_ptr=temp/temp1; + *log->ok_ptr|=(1<ok_bit_no); + } else { + *log->df_ptr=temp/temp1; + *log->ok_ptr&=~(1<ok_bit_no); + //pomiar niesprawny + } +} + diff --git a/src/e_df_3f.h b/src/e_df_3f.h new file mode 100644 index 0000000..6a2853c --- /dev/null +++ b/src/e_df_3f.h @@ -0,0 +1,55 @@ +/* + * e_df_3f.h + * + * Created on: 29-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_DF_3F_H_ +#define E_DF_3F_H_ + +#include "tdefs.h" +#include "e_f.h" + +struct e_df_3f_logic +{ + u8 *ok_ptr; + u8 ok_bit_no; + float *df_ptr; + + struct e_f_wyjscie *f1; + struct e_f_wyjscie *f2; + struct e_f_wyjscie *f3; +}; + +struct e_df_3f_wyjscie +{ + float df_; + u8 OK_; +}; + +struct e_df_3f_io +{ + u32 f1_ptr_in; + u32 f2_ptr_in; + u32 f3_ptr_in; + + u32 df_float_out; + u32 ok_out; +}__attribute__((__packed__)); + +struct e_df_3f_params +{ +}__attribute__((__packed__)); + +struct e_df_3f_args +{ + struct e_df_3f_io io; + struct e_df_3f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_df_3f_100hz(void *args, void *logic); +extern int e_df_3f_initlog(void *arguments, void *logic); + +#endif /* E_DF_3F_H_ */ diff --git a/src/e_df_dt.c b/src/e_df_dt.c new file mode 100644 index 0000000..d51ccdf --- /dev/null +++ b/src/e_df_dt.c @@ -0,0 +1,100 @@ +/* + * e_df_dt.c + * + * Created on: 05-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_df_dt.h" +#include "analog_in.h" +#include + +int e_df_dt_initlog(void *arguments, void *logic) +{ + struct e_df_dt_args *args = (struct e_df_dt_args *)arguments; + struct e_df_dt_logic *log = (struct e_df_dt_logic *)logic; + + u32 *f_ptr; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f_ptr_in >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + log->f=(struct e_f_wyjscie *)*f_ptr; + + if(!*f_ptr) + return -1; + + log->df_ptr = (float *)(log_manager.nets_data + (args->io.df_float_out >> 3)); + if((u8 *)log->df_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ok_ptr = log_manager.nets_data + (args->io.ok_out >> 3); + log->ok_bit_no = args->io.ok_out & 0x07; + + if(log->ok_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + log->DT_ = 1000 * args->params.DT_; + + return 0; +} + +void e_df_dt_100hz(void *arguments, void *logic) +{ + struct e_df_dt_logic *log = (struct e_df_dt_logic *)logic; + + struct e_f_wyjscie *w = log->f; + uint16_t i; + uint16_t nr_pp; + int16_t idxp = w->idx_; // index polozenia drugiej wartosci roznicy (cofniecie o czas T) + uint16_t OK_ = w->OK_[idxp]; + float f = w->f_[idxp]; + float DT; + + if(OK_) // jezeli pomiar czestotliwosci OK to wyliczenie Df/DT + { + + //szukanie "drugiego konca" okresu danych + if (OK_ > log->DT_) + { + nr_pp = OK_ - log->DT_; + for (i=0;i<300;i++) + { + idxp--; + if (idxp<0) idxp +=300; + if ((w->OK_[idxp]<=nr_pp) && w->OK_[idxp]) + { + DT = (float) (nr_pp - w->OK_[idxp] + log->DT_) * 0.001f; + *log->df_ptr = (f - w->f_[idxp]) / DT; + *log->ok_ptr|=(1<ok_bit_no); + break; + } + *log->ok_ptr&=~(1<ok_bit_no); + } + } else { + nr_pp = (0xFFFF - OK_) + log->DT_ + 1; + for (i=0;i<300;i++) + { + idxp--; + if (idxp<0) idxp +=300; + if ((w->OK_[idxp]<=nr_pp) && (w->OK_[idxp] > 0x7FFF)) + { + DT = (float)((float)nr_pp - (float)w->OK_[idxp] + *log->df_ptr) * 0.001f; + *log->df_ptr = (f - w->f_[idxp]) / DT; + *log->ok_ptr|=(1<ok_bit_no); + break; + } + *log->ok_ptr&=~(1<ok_bit_no); + } + } + // + + } else { + *log->ok_ptr&=~(1<ok_bit_no); + *log->df_ptr = 0; + } +} + diff --git a/src/e_df_dt.h b/src/e_df_dt.h new file mode 100644 index 0000000..2614c2e --- /dev/null +++ b/src/e_df_dt.h @@ -0,0 +1,46 @@ +/* + * e_df_dt.h + * + * Created on: 05-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_DF_DT_H_ +#define E_DF_DT_H_ + +#include "tdefs.h" +#include "e_f.h" + +struct e_df_dt_logic +{ + u8 *ok_ptr; + u8 ok_bit_no; + float *df_ptr; + float DT_; + struct e_f_wyjscie *f; +}; + +struct e_df_dt_io +{ + u32 f_ptr_in; + + u32 df_float_out; + u32 ok_out; +}__attribute__((__packed__)); + +struct e_df_dt_params +{ + double DT_; +}__attribute__((__packed__)); + +struct e_df_dt_args +{ + struct e_df_dt_io io; + struct e_df_dt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_df_dt_100hz(void *args, void *logic); +extern int e_df_dt_initlog(void *arguments, void *logic); + +#endif /* E_DF_DT_H_ */ diff --git a/src/e_df_dt_3f.c b/src/e_df_dt_3f.c new file mode 100644 index 0000000..7fc6a89 --- /dev/null +++ b/src/e_df_dt_3f.c @@ -0,0 +1,152 @@ +/* + * e_df_dt_3f.c + * + * Created on: 05-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_df_dt_3f.h" +#include "analog_in.h" +#include + +int e_df_dt_3f_initlog(void *arguments, void *logic) +{ + struct e_df_dt_3f_args *args = (struct e_df_dt_3f_args *)arguments; + struct e_df_dt_3f_logic *log = (struct e_df_dt_3f_logic *)logic; + + u32 *f_ptr; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f1_ptr_in >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + log->f1=(struct e_f_wyjscie *)*f_ptr; + + if(!*f_ptr) + return -1; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f2_ptr_in >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + log->f2=(struct e_f_wyjscie *)*f_ptr; + + if(!*f_ptr) + return -1; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f3_ptr_in >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + log->f3=(struct e_f_wyjscie *)*f_ptr; + + if(!*f_ptr) + return -1; + + log->df_ptr = (float *)(log_manager.nets_data + (args->io.df_float_out >> 3)); + if((u8 *)log->df_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ok_ptr = log_manager.nets_data + (args->io.ok_out >> 3); + log->ok_bit_no = args->io.ok_out & 0x07; + + if(log->ok_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + log->DT_ = 1000 * args->params.DT_; + + return 0; +} + +void przelicz1f(struct e_df_dt_3f_logic *log) +{ + int i; + + log->idxp = log->w->idx_; // index polozenia drugiej wartosci roznicy (cofniecie o czas T) + log->OK_ = log->w->OK_[log->idxp]; + log->f = log->w->f_[log->idxp]; + + if(log->OK_) // jezeli pomiar czestotliwosci OK to wyliczenie Df/DT + { + //szukanie "drugiego konca" okresu danych + if (log->OK_ > log->DT_) + { + log->nr_pp = log->OK_ - log->DT_; + for (i=0;i<300;i++) + { + log->idxp--; + if (log->idxp<0) log->idxp +=300; + if ((log->w->OK_[log->idxp]<=log->nr_pp) && log->w->OK_[log->idxp]) + { + log->DT = (float)((float)log->nr_pp - (float)log->w->OK_[log->idxp] + (float)log->DT_) * 0.001f; + log->wyDf = (log->f - log->w->f_[log->idxp]) / log->DT; + log->wyOK = 1; + break; + } + log->wyOK = 0; + } + } else { + log->nr_pp = (0xFFFF - log->OK_) + log->DT_ + 1; + for (i=0;i<300;i++) + { + log->idxp--; + if (log->idxp<0) log->idxp +=300; + if ((log->w->OK_[log->idxp]<=log->nr_pp) && (log->w->OK_[log->idxp] > 0x7FFF)) + { + log->DT = (float)((float)log->nr_pp - (float)log->w->OK_[log->idxp] + (float)log->wyDf) * 0.001f; + log->wyDf = (log->f - log->w->f_[log->idxp]) / log->DT; + log->wyOK = 1; + break; + } + log->wyOK = 0; + } + } + // + } else { + log->wyOK = 0; + log->wyDf = 0; + } +} + +void e_df_dt_3f_100hz(void *arguments, void *logic) +{ + struct e_df_dt_3f_logic *log = (struct e_df_dt_3f_logic *)logic; + float wynik; + uint16_t n; + + n = 0; + wynik = 0; + + log->w = log->f1; + przelicz1f(log); + if(log->wyOK) // jesli pomiar wejscia 1 poprawny + { + wynik = log->wyDf; + n++; + } + + log->w = log->f2; + przelicz1f(log); + if(log->wyOK) // jesli pomiar wejscia 2 poprawny + { + wynik += log->wyDf; + n++; + } + log->w = log->f3; + przelicz1f(log); + if(log->wyOK) // jesli pomiar wejscia 3 poprawny + { + wynik += log->wyDf; + n++; + } + + + if(n) //jesli rozna od zera to co najmniej jeden pomiar poprawny + { + *log->df_ptr=wynik/n; // wyliczenie sredniej z poprawwnie zmierzonych wejsc + *log->ok_ptr|=(1<ok_bit_no); // ustawienie stanu sprawnosci pomiaru + } + else + *log->ok_ptr&=~(1<ok_bit_no); +} + diff --git a/src/e_df_dt_3f.h b/src/e_df_dt_3f.h new file mode 100644 index 0000000..2c54b18 --- /dev/null +++ b/src/e_df_dt_3f.h @@ -0,0 +1,60 @@ +/* + * e_df_dt_3f.h + * + * Created on: 05-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_DF_DT_3F_H_ +#define E_DF_DT_3F_H_ + +#include "tdefs.h" +#include "e_f.h" + +struct e_df_dt_3f_logic +{ + u8 *ok_ptr; + u8 ok_bit_no; + float *df_ptr; + float DT_; + struct e_f_wyjscie *f1; + struct e_f_wyjscie *f2; + struct e_f_wyjscie *f3; + + float wyDf; + u8 wyOK; + struct e_f_wyjscie *w; + unsigned short i; + unsigned short nr_pp; + short idxp; + unsigned short OK_; + float DT; + float f; +}; + +struct e_df_dt_3f_io +{ + u32 f1_ptr_in; + u32 f2_ptr_in; + u32 f3_ptr_in; + + u32 df_float_out; + u32 ok_out; +}__attribute__((__packed__)); + +struct e_df_dt_3f_params +{ + double DT_; +}__attribute__((__packed__)); + +struct e_df_dt_3f_args +{ + struct e_df_dt_3f_io io; + struct e_df_dt_3f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_df_dt_3f_100hz(void *args, void *logic); +extern int e_df_dt_3f_initlog(void *arguments, void *logic); + +#endif /* E_DF_DT_3F_H_ */ diff --git a/src/e_du.c b/src/e_du.c new file mode 100644 index 0000000..854c032 --- /dev/null +++ b/src/e_du.c @@ -0,0 +1,121 @@ +/* + * e_du.c + * + * Created on: 29-06-2016 + * Author: Krzysztof Jakubczyk + * + * Wylicza zmiany napiecia w nastawionym czasie + * + * + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_du.h" +#include "e_mem1.h" +#include + +int e_du_initlog(void *arguments, void *logic) +{ + struct e_du_args *args = (struct e_du_args *)arguments; + struct e_du_logic *log = (struct e_du_logic *)logic; + + u32 *buf_ptr; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + log->we = (struct e_mem1_wyjscie *)*buf_ptr; + + if(!*buf_ptr) + return -1; + + log->ok_ptr = log_manager.nets_data + (args->io.ok_out >> 3); + log->ok_bit_no = args->io.ok_out & 0x07; + + if(log->ok_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + // wyliczanie polozenia wlasciwych wartosci w pamieci + log->ofset_ = (int16_t)((args->params.czas_ -16) / 0.4); + if(args->params.czas_ < 16) + { + log->zakres_ = e_du_od8do16; + log->ofset_ = (int16_t)((args->params.czas_ - 8) / 0.2); + } + if(args->params.czas_ < 8) + { + log->zakres_ = e_du_od4do8; + log->ofset_ = (int16_t)((args->params.czas_ - 4) / 0.1); + } + if(args->params.czas_ < 4) + { + log->zakres_ = e_du_mniej4; + log->ofset_ = (int16_t)(args->params.czas_ / 0.05); + } + log->czas = (int16_t)(100*args->params.czas_); + log->Ubl_ = args->params.Ubl_; + // + log->fczas = args->params.czas_; + + return 0; +} + +void e_du_20hz(void *arguments, void *logic) +{ + struct e_du_logic *log = (struct e_du_logic *)logic; + struct e_mem1_wyjscie *w = log->we; + + u8 OK; + float kwampl; + short temp; + + if (w->czas_>=log->czas) OK = 1; else OK =0; + if (w->srednia_50ms_[w->idx_50ms_] < log->Ubl_) OK = 0; + + switch(log->zakres_)// wyliczanie w zaleznosci od zakresu nastawy czasu + { + case e_du_mniej4: + temp = w->idx_50ms_ - log->ofset_; // wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 80; // uwzglednienie "przekrecania" bufora" + kwampl = w->srednia_50ms_[w->idx_50ms_] - w->srednia_50ms_[temp];//wyliczenie wartosci wyjsciowej roznicy napiecia + if (w->srednia_50ms_[temp] < log->Ubl_) OK = 0; + break; + case e_du_od4do8: + temp = w->idx_100ms_ - log->ofset_;// wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 40;// uwzglednienie "przekrecania" bufora" + kwampl = w->srednia_50ms_[w->idx_50ms_] - w->srednia_100ms_[temp];//wyliczenie wartosci wyjsciowej roznicy napiecia + if (w->srednia_100ms_[temp] < log->Ubl_) OK = 0; + break; + case e_du_od8do16: + temp = w->idx_200ms_ - log->ofset_;// wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 40;// uwzglednienie "przekrecania" bufora" + kwampl = w->srednia_50ms_[w->idx_50ms_] - w->srednia_200ms_[temp];//wyliczenie wartosci wyjsciowej roznicy napiecia + if (w->srednia_200ms_[temp] < log->Ubl_) OK = 0; + break; + case e_du_od16do30: + temp = w->idx_400ms_ - log->ofset_;// wyliczenie indeksu tablicy dla danej chwili czasu + if(temp<0) temp += 36;// uwzglednienie "przekrecania" bufora" + kwampl = w->srednia_50ms_[w->idx_50ms_] - w->srednia_400ms_[temp]; //wyliczenie wartosci wyjsciowej roznicy napiecia + if (w->srednia_400ms_[temp] < log->Ubl_) OK = 0; + break; + } + + if (!OK) + { + *log->ok_ptr&=~(1<ok_bit_no); + kwampl = 0; + } + else + { + *log->ok_ptr|=(1<ok_bit_no); + } + + *log->esk_ptr = kwampl; +} diff --git a/src/e_du.h b/src/e_du.h new file mode 100644 index 0000000..ec0e0a8 --- /dev/null +++ b/src/e_du.h @@ -0,0 +1,59 @@ +/* + * e_du.h + * + * Created on: 29-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_DU_H_ +#define E_DU_H_ + +#include "tdefs.h" +#include "e_mem1.h" + + enum e_du_zakres + { + e_du_mniej4 = 0, + e_du_od4do8, + e_du_od8do16, + e_du_od16do30 + }; + +struct e_du_logic +{ + float *esk_ptr; + struct e_mem1_wyjscie *we; + u8 *ok_ptr; + u8 ok_bit_no; + + enum e_du_zakres zakres_; ///< zakres bufora pamieci + short czas; ///< ilosc petli 10ms w oknie calkowania + short ofset_; ///< przesuniecie danego bufora pamieci + float Ubl_; ///< wartosc napiecia blokady w krotnosciach napiecia znamionowego + float fczas; +}; + +struct e_du_io +{ + u32 buf_ptr_in; + u32 esk_float_out; + u32 ok_out; +}__attribute__((__packed__)); + +struct e_du_params +{ + double czas_; + double Ubl_; +}__attribute__((__packed__)); + +struct e_du_args +{ + struct e_du_io io; + struct e_du_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_du_20hz(void *args, void *logic); +extern int e_du_initlog(void *arguments, void *logic); + +#endif /* E_DU_H_ */ diff --git a/src/e_du_dt.c b/src/e_du_dt.c new file mode 100644 index 0000000..3dc57ec --- /dev/null +++ b/src/e_du_dt.c @@ -0,0 +1,99 @@ +/* + * e_du_dt.c + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + * + * Wylicza wartosc pochodnej wartosci skutecznej napiecia + * + * + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_du_dt.h" +#include + +int e_du_dt_initlog(void *arguments, void *logic) +{ + struct e_du_dt_args *args = (struct e_du_dt_args *)arguments; + struct e_du_dt_logic *log = (struct e_du_dt_logic *)logic; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_in >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->du_ptr = (float *)(log_manager.nets_data + (args->io.du_float_out >> 3)); + if((u8 *)log->du_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->start=1; + + return 0; +} + +void e_du_dt_100hz(void *arguments, void *logic) +{ + struct e_du_dt_logic *log = (struct e_du_dt_logic *)logic; + + int16_t idx_n,idx_n1; + float temp; + float dU_; + + log->index++; + if (log->index>99) + { + log->index = 0; + log->start = 0; + } + + log->srednia_z_10ms[log->index] = sqrt(*log->esk_ptr); + + idx_n = log->index; // polozenie biezacej danej w buforze + + if (log->srednia_z_10ms[idx_n] > (float)0.05) + { + idx_n1 = idx_n - 99; + if (idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze + if (log->srednia_z_10ms[idx_n1] > (float) 0.05) + { + dU_ = (log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]) * 1.010101f;/// (float)0.99; //pochodna z oknem 990 ms + } else { + dU_ = 0; + } + + + idx_n1 = idx_n - 20; + if(idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze + temp = log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]; //pochodna z oknem 200 ms + if((fabs(temp) > 0.02) && (log->srednia_z_10ms[idx_n1] > (float)0.05)) + { + dU_ = temp * 5.0f;// / (float)0.2; // jesli dostatecznie duza roznica (1%) to wynik pochodnej z 200ms + } + + idx_n1 = idx_n - 4; + if(idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze + temp = log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]; //pochodna z oknem 200 ms + if((fabs(temp) > 0.02) && (log->srednia_z_10ms[idx_n1] > (float)0.05)) + { + dU_ = temp * 25.0f;// / (float)0.04; // jesli dostatecznie duza roznica (1%) to wynik pochodnej z 40ms + } + + idx_n1 = idx_n - 1; + if(idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze + temp = log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]; //pochodna z oknem 10 ms + if((fabs(temp) > 0.02) && (log->srednia_z_10ms[idx_n1] > (float)0.05)) + { + dU_ = temp * 100.0f;// / (float)0.01; // jesli dostatecznie duza roznica (1%) to wynik pochodnej z 10ms + } + + + } else { + dU_ = 0; + } + + if(log->start) dU_ = 0;// zerowanie wyjscia po bootowaniu do czasu napelnienia buforu + + *log->du_ptr=dU_; +} diff --git a/src/e_du_dt.h b/src/e_du_dt.h new file mode 100644 index 0000000..a949c6a --- /dev/null +++ b/src/e_du_dt.h @@ -0,0 +1,43 @@ +/* + * e_du_dt.h + * + * Created on: 07-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_DU_DT_H_ +#define E_DU_DT_H_ + +#include "tdefs.h" + +struct e_du_dt_logic +{ + float *esk_ptr; + float *du_ptr; + + u8 start; + float srednia_z_10ms[100];// zapamietywanie wartosci chwilowych z polokresu + unsigned short index; // polozenie aktualnej probki w tablicy +}; + +struct e_du_dt_io +{ + u32 esk_float_in; + u32 du_float_out; +}__attribute__((__packed__)); + +struct e_du_dt_params +{ +}__attribute__((__packed__)); + +struct e_du_dt_args +{ + struct e_du_dt_io io; + struct e_du_dt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_du_dt_100hz(void *args, void *logic); +extern int e_du_dt_initlog(void *arguments, void *logic); + +#endif /* E_DU_DT_H_ */ diff --git a/src/e_f.c b/src/e_f.c new file mode 100644 index 0000000..5f35640 --- /dev/null +++ b/src/e_f.c @@ -0,0 +1,317 @@ +/* + * e_f.c + * + * Created on: 28-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_f.h" +#include "analog_in.h" +#include + +int e_f_initlog(void *arguments, void *logic) +{ + struct e_f_args *args = (struct e_f_args *)arguments; + struct e_f_logic *log = (struct e_f_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + u32 *f_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + f_ptr = (u32 *)(log_manager.nets_data + (args->io.f_ptr_out >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + *f_ptr = (u32)&log->wy; + + log->nast_.Ublr_ = args->params.Ubl_;/// nast_.w->nast->mnoznik; + log->nast_.Ublr_ *= log->nast_.Ublr_; + log->nrprobki = 1; + log->nast_.ofset = 32767 + an_params->offset; + log->nast_.mnoznik1 = an_params->multiplier; + log->nast_.mnoznik = an_params->multiplier; + log->nast_.mnoznik *= log->nast_.mnoznik; + + log->mn[0]=1.0f; + log->mn[1]=0.5f; + log->mn[2]=0.33333333f; + + return 0; +} + +/** +Wylicza czestotliwosc przebiegu z danych pozyskanych od funkcji przelicz_1000Hz +\param: dane_wew& d - dane do obliczen wedlug struktury + WA::Wyjscie* w - wskaznik do danych probek +*/ +void e_f_wylicz_f(struct e_f_dane_do_wyliczenia *d, struct e_f_logic *log) +{ + float dopelnienie; + float temp, tempmax, tempmin,p1temp; + uint16_t temp1; + int i1; + float okres; + float probka_minus1,probka; + float il_p; + u8 cnt; + + log->idx_f++; + if(log->idx_f>4) + log->idx_f = 0; + log->f_cz.f_OK_[log->idx_f] = 1; + + +//szukanie probek przejscia przez zero + cnt=0; + + do + { + cnt++; + i1 = d->nrp - cnt; + + if (i1<0) + i1 += SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2; + + if(cnt==1) + probka = (float)log->buf_ptr[d->nrp] - log->nast_.ofset; + else + probka = probka_minus1; + + probka_minus1 = (float)log->buf_ptr[i1] - log->nast_.ofset; + } + while((probka * probka_minus1) > 0 && (d->skok-cnt>=0)); + + if ((probka * probka_minus1) > 0) + log->f_cz.f_OK_[log->idx_f] = 0; + else + il_p = d->skok - cnt; + + //wyliczenie pojedynczej iteracji + d->il_p_ += il_p; + + dopelnienie = probka_minus1 / (probka_minus1 - probka); + okres = dopelnienie + d->reszta_ + (float)d->il_p_; + p1temp = 1 / okres; + temp = (float)(SAMPLES_PER_MS*1000) * p1temp; + // + + log->f_cz.f_[log->idx_f] = temp; // zapisanie czestotliwosci pojedynczej iteracji do bufora podrecznego + + if ((temp < 38) || (temp > 72)) + log->f_cz.f_OK_[log->idx_f] = 0; // sprawdzenie warunku zakresu zmiennosci + + int16_t n = okres; + + d->war_sk_= 0; + d->war_sr_= 0; + + i1 = d->nrp - 1; + + int i; + + for(i=0;ibuf_ptr[i1] - log->nast_.ofset; + d->war_sk_ += probka * probka; + d->war_sr_ += fabs(probka); + i1--; + } + d->war_sk_ *= log->nast_.mnoznik; + d->war_sr_ *= log->nast_.mnoznik1; + + d->war_sk_ *= p1temp;// wyliczenie wartosci skutecznej + d->war_sr_ *= p1temp;// wyliczenie wartosci sredniej wyprostowanej + + //sprawdzenie poprawnosci wyniku czastkowego + if(d->war_sk_ < log->nast_.Ublr_) + log->f_cz.f_OK_[log->idx_f] = 0; //sprawdzenie warunku napiecia blokady + + temp = fabs(d->war_sk_ - d->war_sk_pop_); + if(temp > 0.1 * d->war_sk_) log->f_cz.f_OK_[log->idx_f] = 0; //sprawdzenie warunku zmiennosci - blokada pomiaru jesli zmiana amplitudy + + temp = (d->war_sr_ * d->war_sr_); //wspolczynnik ksztaltu powinien byc 1.1107 * 1.1107 = 1.2337 + if ((d->war_sk_ < (float)1.20 * temp) || (d->war_sk_ > (float)1.27 * temp)) + { + log->f_cz.f_OK_[log->idx_f] = 0; // sprawdzenie wspolczynnika ksztaltu sinusoidy + } + + if(fabs(okres - log->okres_n_minus1) > (float)2.0) + log->f_cz.f_OK_[log->idx_f] = 0; //kiedy duza roznica to blad pomiaru + + // wyliczenie czestotliwosci z pieciu iteracji i zapisanie stanu na wyjscie + temp = 0; + temp1 = 0; + tempmin = 10000.0; + tempmax = 0; + + for (i=0;i<5;i++) //usrednienie pomiaru z odrzuceniem dwoch skrajnych najgorszych wynikow + { + if(log->f_cz.f_OK_[i]) + { + temp1++; + temp += log->f_cz.f_[i]; + if(tempmin > log->f_cz.f_[i]) tempmin = log->f_cz.f_[i]; + if(tempmax < log->f_cz.f_[i]) tempmax = log->f_cz.f_[i]; + } + } + + log->wy.idx_++; + if(log->wy.idx_>299) + log->wy.idx_ = 0; + + if(temp1>2) //jesli co najmniej 3 pomiary OK to funkcja wystawia ze czestotliwosc jest OK + { + log->wy.OK_[log->wy.idx_] = d->nrprobki; + log->wy.f_[log->wy.idx_] = (temp - tempmin - tempmax) * log->mn[temp1-3]; + } else + { + log->wy.OK_[log->wy.idx_] = 0; + log->wy.f_[log->wy.idx_] = 0; + } + + d->reszta_ = 1 - dopelnienie; + d->war_sk_pop_ = d->war_sk_; //zapamietanie poprzedniej wartosci skutecznej + d->war_sk_ = 0; //zerowanie licznika wartosci skutecznej przy przejsciu przez zero + d->war_sr_ = 0; //zerowanie licznika wartosci sredniej wyprostowanej przy przejsciu przez zero + d->il_p_ = d->skok - 1 - il_p;//zerowanie licznika pelnych probek(odliczanie nowego okresu) + log->okres_n_minus1 = okres; +} + +void e_f(void *arguments, void *logic) +{ + struct e_f_logic *log = (struct e_f_logic *)logic; + + u8 przejscie_z_plusa = 0; + u8 przejscie_z_minusa = 0; + + log->nrp = bus_an_cur_sample_num; + log->skok = log->nrp - log->nrp_minus1; + + if(log->skok<0) + log->skok +=SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2; + +// probka_n_ = w.probka_; + log->probka_n_ = (float)log->buf_ptr[log->nrp] - log->nast_.ofset; + + //szukanie przejsc przez zero + + if ((log->probka_n_minus1 <0 ) && (log->probka_n_ >= 0)) + przejscie_z_minusa = 1; + if ((log->probka_n_minus1 >0 ) && (log->probka_n_ <= 0)) + przejscie_z_plusa = 1; + + + // + + + if(przejscie_z_plusa && (log->plus.il_p_ > 35*SAMPLES_PER_MS/3) && !log->d_plus.wylicz) + { + log->d_plus.il_p_ += log->plus.il_p_; + log->plus.il_p_ = 0; + log->d_plus.wylicz = 1; + log->d_plus.skok = log->skok; + log->d_plus.nrp = log->nrp; + log->d_plus.nrprobki = log->nrprobki; + } + else + { + log->plus.il_p_ += log->skok; //wyliczanie ilosci pelnych probek w okresie dla okresu od plusa + } + + if(przejscie_z_minusa && (log->minus.il_p_ > 35*SAMPLES_PER_MS/3) && !log->d_minus.wylicz) + { + log->d_minus.il_p_ += log->minus.il_p_; + log->minus.il_p_ = 0; + log->d_minus.wylicz = 1; + log->d_minus.skok = log->skok; + log->d_minus.nrp = log->nrp; + log->d_minus.nrprobki = log->nrprobki; + } + else + { + log->minus.il_p_ += log->skok; //wyliczanie ilosci pelnych probek w okresie dla okresu od minusa + } + + log->probka_n_minus1 = log->probka_n_;//zapamietanie poprzedniej probki + log->nrp_minus1 = log->nrp; + log->nrprobki++; + if(!(log->nrprobki)) + log->nrprobki++; + + + { + if ((log->plus.il_p_ > 100) || (log->minus.il_p_ > 100)) // jezeli brak probkowania + { + log->plus.il_p_ = 0; + log->minus.il_p_ = 0; + log->wy.OK_[log->wy.idx_] = 0; + log->wy.idx_++; + if(log->wy.idx_>299) + log->wy.idx_ = 0; + log->wy.OK_[log->wy.idx_] = 0; + } + + if(log->d_plus.wylicz) + { + e_f_wylicz_f(&log->d_plus,log); + log->d_plus.wylicz = 0; + } + + if(log->d_minus.wylicz) + { + e_f_wylicz_f(&log->d_minus,log); + log->d_minus.wylicz = 0; + } + } +} +/* +void e_f_100hz(void *arguments, void *logic) +{ + struct e_f_logic *log = (struct e_f_logic *)logic; + + if ((log->plus.il_p_ > 100) || (log->minus.il_p_ > 100)) // jezeli brak probkowania + { + log->plus.il_p_ = 0; + log->minus.il_p_ = 0; + log->wy.OK_[log->wy.idx_] = 0; + log->wy.idx_++; + if(log->wy.idx_>299) + log->wy.idx_ = 0; + log->wy.OK_[log->wy.idx_] = 0; + } + + if(log->d_plus.wylicz) + { + e_f_wylicz_f(&log->d_plus,log); + log->d_plus.wylicz = 0; + } + + if(log->d_minus.wylicz) + { + e_f_wylicz_f(&log->d_minus,log); + log->d_minus.wylicz = 0; + } +} +*/ diff --git a/src/e_f.h b/src/e_f.h new file mode 100644 index 0000000..37913a6 --- /dev/null +++ b/src/e_f.h @@ -0,0 +1,106 @@ +/* + * e_f.h + * + * Created on: 28-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_F_H_ +#define E_F_H_ + +#include "tdefs.h" + +struct e_f_wyjscie +{ + float f_[300]; ///< wartosc chwilowa + unsigned short idx_; ///< identyfikator polozenia ostatniego pomiaru w tablicy + unsigned short OK_[300]; ///< numer petli wyniku, jesli0 to pomiar zabity +}; + +struct e_f_dane_wew +{ + float war_sk_; ///< wartosc skuteczna + float war_sr_; ///< wartosc srednia wyprostowana + float il_p_; ///< ilosc pelnych probek w okresie pomiarowym +}; + +struct e_f_dane_do_wyliczenia +{ + float war_sk_; ///< wartosc skuteczna + float war_sk_pop_; ///< poprzednia wartosc skuteczna - z ostatniego pomiaru + float war_sr_; ///< wartosc srednia wyprostowana + float il_p_; ///< ilosc pelnych probek w okresie pomiarowym + float reszta_; ///< odcinek sinusa z poczatku (niepelna probka) + float skok; + short nrp; + unsigned short nrprobki; + u8 wylicz;///< flaga nowego pomiaru +}; + +struct e_f_dane_wew_2 +{ + float f_[5]; // pomiar czastkowy czestotliwosci - z jednego okresu + u8 f_OK_[5]; // poprawny pomiar dla danego okresu +}; + +struct e_f_Nastawy_przeliczone +{ + float Ublr_; ///< wartosc blokady + float ofset; + float mnoznik1; + float mnoznik; +}; + +struct e_f_logic +{ + u16 *buf_ptr; + float *esk_ptr; + float *orta_ptr; + float *ortb_ptr; + + float Ublr_; ///< wartosc blokady + float ofset; + float mnoznik1; + float mnoznik; + + struct e_f_dane_do_wyliczenia d_plus; + struct e_f_dane_do_wyliczenia d_minus; + struct e_f_Nastawy_przeliczone nast_; //wartosci blokady pomiaru czestotliwosci + float probka_n_minus1; // pamiec poprzedniej probki + float okres_n_minus1; // pamiec poprzednio zmierzonego okresu + float probka_n_; // probka biezaca + struct e_f_dane_wew plus; //zmienne pomocnicze dla obliczen okresu od plusa + struct e_f_dane_wew minus; //zmienne pomocnicze dla obliczen okresu od minusa + struct e_f_dane_wew_2 f_cz; //dane czastkowe pomiaru czestotliwosci + short idx_f;//indeks bufora pomiarow czastkowych czestotliwosci + unsigned short nrprobki; + short nrp,nrp_minus1; + float skok; + float mn[3]; + struct e_f_wyjscie wy; +}; + +struct e_f_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 f_ptr_out; +}__attribute__((__packed__)); + +struct e_f_params +{ + double Ubl_; +}__attribute__((__packed__)); + +struct e_f_args +{ + struct e_f_io io; + struct e_f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_f(void *args, void *logic); +extern void e_f_100hz(void *args, void *logic); +extern int e_f_initlog(void *arguments, void *logic); + +#endif /* E_F_H_ */ diff --git a/src/e_f_3f.c b/src/e_f_3f.c new file mode 100644 index 0000000..8b60bc6 --- /dev/null +++ b/src/e_f_3f.c @@ -0,0 +1,80 @@ +/* + * e_f_3f.c + * + * Created on: 18-01-2017 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_f_3f.h" +#include "analog_in.h" +#include "helper.h" +#include + +const float mn[3] = {1.0f,0.5f,0.33333333f}; + +int e_f_3f_initlog(void *arguments, void *logic) +{ + struct e_f_3f_args *args = (struct e_f_3f_args *)arguments; + struct e_f_3f_logic *log = (struct e_f_3f_logic *)logic; + + if(set_pointer_in_ptr(args->io.f1_ptr_in,(u32 *)&log->f1)) + return -1; + + if(set_pointer_in_ptr(args->io.f2_ptr_in,(u32 *)&log->f2)) + return -1; + + if(set_pointer_in_ptr(args->io.f3_ptr_in,(u32 *)&log->f3)) + return -1; + + if(set_float_ptr(args->io.f_float_out,(u32 *)&log->f_ptr)) + return -1; + + if(set_bit_ptr(args->io.ok_out,&log->ok_ptr,&log->ok_bit_no)) + return -1; + + return 0; +} + +void e_f_3f_100hz(void *arguments, void *logic) +{ + struct e_f_3f_logic *log = (struct e_f_3f_logic *)logic; + + float temp = 0; + short temp1 = 0; + struct e_f_wyjscie *w; + + w = log->f1; + if(w->OK_[w->idx_]) //jesli pomiar w kanale 1 sprawny + { + temp += w->f_[w->idx_]; + temp1 +=1; + } + + w = log->f2; + if(w->OK_[w->idx_]) //jesli pomiar w kanale 2 sprawny + { + temp += w->f_[w->idx_]; + temp1 +=1; + } + + w = log->f3; + if(w->OK_[w->idx_]) //jesli pomiar w kanale 3 sprawny + { + temp += w->f_[w->idx_]; + temp1 +=1; + } + + + if(temp1)//jesli co najmniej jeden pomiar sprawny + { + *log->f_ptr = temp * mn[temp1-1]; // wyliczenie sredniej czestotliwosci + *log->ok_ptr|=(1<ok_bit_no); + } else { + *log->ok_ptr&=~(1<ok_bit_no); + *log->f_ptr = 0; + } + +} diff --git a/src/e_f_3f.h b/src/e_f_3f.h new file mode 100644 index 0000000..e631029 --- /dev/null +++ b/src/e_f_3f.h @@ -0,0 +1,49 @@ +/* + * e_f_3f.h + * + * Created on: 18-01-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_F_3F_H_ +#define E_F_3F_H_ + +#include "tdefs.h" +#include "e_f.h" + +struct e_f_3f_logic +{ + u8 *ok_ptr; + u8 ok_bit_no; + float *f_ptr; + + struct e_f_wyjscie *f1; + struct e_f_wyjscie *f2; + struct e_f_wyjscie *f3; +}; + +struct e_f_3f_io +{ + u32 f1_ptr_in; + u32 f2_ptr_in; + u32 f3_ptr_in; + + u32 f_float_out; + u32 ok_out; +}__attribute__((__packed__)); + +struct e_f_3f_params +{ +}__attribute__((__packed__)); + +struct e_f_3f_args +{ + struct e_f_3f_io io; + struct e_f_3f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_f_3f_100hz(void *args, void *logic); +extern int e_f_3f_initlog(void *arguments, void *logic); + +#endif /* E_F_3F_H_ */ diff --git a/src/e_filtr1h.c b/src/e_filtr1h.c new file mode 100644 index 0000000..034013d --- /dev/null +++ b/src/e_filtr1h.c @@ -0,0 +1,105 @@ +/* + * e_filtr1h.c + * + * Created on: 07-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "e_filtr1h.h" +#include "analog_in.h" + +#include + +float wsp_filtr_sin_1h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa +float wsp_filtr_cos_1h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa +int wsp_filtr_1h_initialised=0; + +int e_filtr1h_initlog(void *arguments, void *logic) +{ + struct e_filtr1h_args *args = (struct e_filtr1h_args *)arguments; + struct e_filtr1h_logic *log = (struct e_filtr1h_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + int i; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + + log->an_params=an_params; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + if(!wsp_filtr_1h_initialised) + { + for(i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + /*log->*/wsp_filtr_sin_1h[i] = /*an_params->multiplier **/ + sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i - ((float)6.2832 / 360)/* * + nast.pw->nast->n.korekcja_fazy*/) /(SAMPLES_PER_MS*(MAIN_FREQ_PERIOD_MS/2)); + /*log->*/wsp_filtr_cos_1h[i] = /*an_params->multiplier **/ + cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)) * i - ((float)6.2832 / 360)/* * + nast.pw->nast->n.korekcja_fazy*/) /(SAMPLES_PER_MS*(MAIN_FREQ_PERIOD_MS/2)); + } + + wsp_filtr_1h_initialised=1; + } + + return 0; +} + +void e_filtr1h(void *arguments, void *logic) +{ +// struct e_filtr1h_args *args = (struct e_filtr1h_args *)arguments; + struct e_filtr1h_logic *log = (struct e_filtr1h_logic *)logic; + short i,i1,i2; + float orta = 0; + float ortb = 0; + float pr; + + i2 = bus_an_cur_sample_num; + + for (i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + i1 = i2 - i; + if(i1<0) + i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + pr = (float)log->buf_ptr[i1]; + pr*=log->an_params->multiplier; + orta += pr * wsp_filtr_sin_1h[i]; + ortb += pr * wsp_filtr_cos_1h[i]; + } + + *log->orta_ptr=orta; + *log->ortb_ptr=ortb; + *log->esk_ptr = (orta * orta + ortb * ortb) * 0.5f; +} diff --git a/src/e_filtr1h.h b/src/e_filtr1h.h new file mode 100644 index 0000000..081c0f5 --- /dev/null +++ b/src/e_filtr1h.h @@ -0,0 +1,48 @@ +/* + * e_filtr1h.h + * + * Created on: 07-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_FILTR1H_IN_H_ +#define E_FILTR1H_IN_H_ + +#include "tdefs.h" +#include "config.h" + +struct e_filtr1h_logic +{ +// float wsp_filtr_sin_[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa +// float wsp_filtr_cos_[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa + u16 *buf_ptr; + float *esk_ptr; + float *orta_ptr; + float *ortb_ptr; + struct analog_in_params *an_params; +}; + +struct e_filtr1h_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_filtr1h_params +{ +}__attribute__((__packed__)); + +struct e_filtr1h_args +{ + struct e_filtr1h_io io; + struct e_filtr1h_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_filtr1h(void *args, void *logic); +extern int e_filtr1h_initlog(void *arguments, void *logic); + +#endif /* E_FILTR1H_H_ */ diff --git a/src/e_filtr1h_2T.c b/src/e_filtr1h_2T.c new file mode 100644 index 0000000..85286ad --- /dev/null +++ b/src/e_filtr1h_2T.c @@ -0,0 +1,91 @@ +/* + * e_filtr1h_2T.c + * + * Created on: 06-07-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "e_filtr1h_2T.h" +#include "analog_in.h" +#include + +int e_filtr1h_2t_initlog(void *arguments, void *logic) +{ + struct e_filtr1h_2t_args *args = (struct e_filtr1h_2t_args *)arguments; + struct e_filtr1h_2t_logic *log = (struct e_filtr1h_2t_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + int i; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + for(i=0;i<(2*MAIN_FREQ_PERIOD_MS);i++) + { + log->wsp_filtr_sin_[i] = an_params->multiplier * + sin(((float)6.2832 / MAIN_FREQ_PERIOD_MS) * i - ((float)6.2832 / 360)/* * + nast.pw->nast->n.korekcja_fazy*/) / MAIN_FREQ_PERIOD_MS; + log->wsp_filtr_cos_[i] = an_params->multiplier * + cos(((float)6.2832 / MAIN_FREQ_PERIOD_MS) * i - ((float)6.2832 / 360)/* * + nast.pw->nast->n.korekcja_fazy*/) / MAIN_FREQ_PERIOD_MS; + } + + return 0; +} + +void e_filtr1h_2t(void *arguments, void *logic) +{ + struct e_filtr1h_2t_logic *log = (struct e_filtr1h_2t_logic *)logic; + short i,i1,i2; + float orta = 0; + float ortb = 0; + float pr; + + i2 = bus_an_cur_sample_num; + + for (i=0;i<(2*MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS);i+=SAMPLES_PER_MS) + { + i1 = i2 - i; + if(i1<0) + i1 += (2*MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS); + pr = (float)log->buf_ptr[i1]; + orta += pr * log->wsp_filtr_sin_[i/SAMPLES_PER_MS]; + ortb += pr * log->wsp_filtr_cos_[i/SAMPLES_PER_MS]; + } + + *log->orta_ptr=orta; + *log->ortb_ptr=ortb; + *log->esk_ptr = (orta * orta + ortb * ortb) * 0.5f; +} diff --git a/src/e_filtr1h_2T.h b/src/e_filtr1h_2T.h new file mode 100644 index 0000000..a08a6d3 --- /dev/null +++ b/src/e_filtr1h_2T.h @@ -0,0 +1,47 @@ +/* + * e_filtr1h_2T.h + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_FILTR1H_2T_IN_H_ +#define E_FILTR1H_2T_IN_H_ + +#include "tdefs.h" +#include "config.h" + +struct e_filtr1h_2t_logic +{ + float wsp_filtr_sin_[2*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa + float wsp_filtr_cos_[2*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa + u16 *buf_ptr; + float *esk_ptr; + float *orta_ptr; + float *ortb_ptr; +}; + +struct e_filtr1h_2t_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_filtr1h_2t_params +{ +}__attribute__((__packed__)); + +struct e_filtr1h_2t_args +{ + struct e_filtr1h_2t_io io; + struct e_filtr1h_2t_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_filtr1h_2t(void *args, void *logic); +extern int e_filtr1h_2t_initlog(void *arguments, void *logic); + +#endif /* E_FILTR1H_2T_H_ */ diff --git a/src/e_filtr2h.c b/src/e_filtr2h.c new file mode 100644 index 0000000..fb15964 --- /dev/null +++ b/src/e_filtr2h.c @@ -0,0 +1,102 @@ +/* + * e_filtr2h.c + * + * Created on: 22-04-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "e_filtr2h.h" +#include "analog_in.h" +#include + +float wsp_filtr_sin_2h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; +float wsp_filtr_cos_2h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; +int wsp_filtr_2h_initialised=0; + +int e_filtr2h_initlog(void *arguments, void *logic) +{ + struct e_filtr2h_args *args = (struct e_filtr2h_args *)arguments; + struct e_filtr2h_logic *log = (struct e_filtr2h_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + int i; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + log->an_params=an_params; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + if(!wsp_filtr_2h_initialised) + { + for(i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + wsp_filtr_sin_2h[i] =/* an_params->multiplier **/ + sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + + wsp_filtr_cos_2h[i] =/* an_params->multiplier **/ + cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + } + + wsp_filtr_2h_initialised=1; + } + + return 0; +} + +void e_filtr2h(void *arguments, void *logic) +{ +// struct e_filtr2h_args *args = (struct e_filtr2h_args *)arguments; + struct e_filtr2h_logic *log = (struct e_filtr2h_logic *)logic; + short i,i1,i2; + float orta = 0; + float ortb = 0; + float pr; + + i2 = bus_an_cur_sample_num; + + for (i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + i1 = i2 - i; + if(i1<0) + i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + pr = (float)log->buf_ptr[i1]; + pr*=log->an_params->multiplier; + orta += pr * wsp_filtr_sin_2h[i]; + ortb += pr * wsp_filtr_cos_2h[i]; + } + + *log->orta_ptr=orta; + *log->ortb_ptr=ortb; + *log->esk_ptr = (orta * orta + ortb * ortb) * 0.5f; +} diff --git a/src/e_filtr2h.h b/src/e_filtr2h.h new file mode 100644 index 0000000..8141f3a --- /dev/null +++ b/src/e_filtr2h.h @@ -0,0 +1,47 @@ +/* + * e_filtr2h.h + * + * Created on: 22-04-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_FILTR2H_IN_H_ +#define E_FILTR2H_IN_H_ + +#include "tdefs.h" + +struct e_filtr2h_logic +{ +// float wsp_filtr_sin_[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa +// float wsp_filtr_cos_[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa + u16 *buf_ptr; + float *esk_ptr; + float *orta_ptr; + float *ortb_ptr; + struct analog_in_params *an_params; +}; + +struct e_filtr2h_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_filtr2h_params +{ +}__attribute__((__packed__)); + +struct e_filtr2h_args +{ + struct e_filtr2h_io io; + struct e_filtr2h_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_filtr2h(void *args, void *logic); +extern int e_filtr2h_initlog(void *arguments, void *logic); + +#endif /* E_FILTR2H_H_ */ diff --git a/src/e_filtr2h_1khz.c b/src/e_filtr2h_1khz.c new file mode 100644 index 0000000..b3374c5 --- /dev/null +++ b/src/e_filtr2h_1khz.c @@ -0,0 +1,73 @@ +/* + * e_filtr2h_1khz.c + * + * Created on: 01-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_filtr2h_1khz.h" +#include "helper.h" +#include + +int e_filtr2h_1khz_initlog(void *arguments, void *logic) +{ + struct e_filtr2h_1khz_args *args = (struct e_filtr2h_1khz_args *)arguments; + struct e_filtr2h_1khz_logic *log = (struct e_filtr2h_1khz_logic *)logic; + int i; + + if(set_float_ptr(args->io.sample_float_in,&log->sample_in)) + return -1; + + if(set_float_ptr(args->io.orta_float_out,&log->orta_ptr)) + return -1; + + if(set_float_ptr(args->io.ortb_float_out,&log->ortb_ptr)) + return -1; + + if(set_float_ptr(args->io.esk_float_out,&log->esk_ptr)) + return -1; + + for(i=0;i<(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);i++) + { + log->wsp_filtr_sin_[i] = sin(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2)) * i) / (10/LOOP_CYCLE_MS); + log->wsp_filtr_cos_[i] = cos(((float)6.2832 / (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS/2)) * i) / (10/LOOP_CYCLE_MS); + } + + return 0; +} + +void e_filtr2h_1khz(void *arguments, void *logic) +{ +// struct e_filtr2h_1khz_args *args = (struct e_filtr2h_1khz_args *)arguments; + struct e_filtr2h_1khz_logic *log = (struct e_filtr2h_1khz_logic *)logic; + short i,i1,i2; + float orta = 0; + float ortb = 0; + float pr; + + i2 = log->sample_num; + + log->buf[i2]=*log->sample_in; + + for (i=0;i<(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);i++) + { + i1 = i2 - i; + if(i1<0) + i1 += (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS); + pr = log->buf[i1]; + orta += pr * log->wsp_filtr_sin_[i]; + ortb += pr * log->wsp_filtr_cos_[i]; + } + + *log->orta_ptr=orta; + *log->ortb_ptr=ortb; + *log->esk_ptr = (orta * orta + ortb * ortb) * 0.5f; + + i2++; + i2%=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS); + + log->sample_num=i2; +} diff --git a/src/e_filtr2h_1khz.h b/src/e_filtr2h_1khz.h new file mode 100644 index 0000000..df36de8 --- /dev/null +++ b/src/e_filtr2h_1khz.h @@ -0,0 +1,47 @@ +/* + * e_filtr2h_1khz.h + * + * Created on: 01-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_FILTR2H_1KHZ_IN_H_ +#define E_FILTR2H_1KHZ_IN_H_ + +#include "tdefs.h" + +struct e_filtr2h_1khz_logic +{ + float wsp_filtr_sin_[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa + float wsp_filtr_cos_[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa + float buf[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS]; + float *sample_in; + float *esk_ptr; + float *orta_ptr; + float *ortb_ptr; + u8 sample_num; +}; + +struct e_filtr2h_1khz_io +{ + u32 sample_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_filtr2h_1khz_params +{ +}__attribute__((__packed__)); + +struct e_filtr2h_1khz_args +{ + struct e_filtr2h_1khz_io io; + struct e_filtr2h_1khz_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_filtr2h_1khz(void *args, void *logic); +extern int e_filtr2h_1khz_initlog(void *arguments, void *logic); + +#endif /* E_FILTR2H_1KHZ_H_ */ diff --git a/src/e_filtr3h.c b/src/e_filtr3h.c new file mode 100644 index 0000000..2e81604 --- /dev/null +++ b/src/e_filtr3h.c @@ -0,0 +1,101 @@ +/* + * e_filtr3h.c + * + * Created on: 11-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "e_filtr3h.h" +#include "analog_in.h" +#include + +float wsp_filtr_sin_3h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa +float wsp_filtr_cos_3h[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa +int wsp_filtr_3h_initialised=0; + +int e_filtr3h_initlog(void *arguments, void *logic) +{ + struct e_filtr3h_args *args = (struct e_filtr3h_args *)arguments; + struct e_filtr3h_logic *log = (struct e_filtr3h_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + int i; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + log->an_params=an_params; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + if(!wsp_filtr_3h_initialised) + { + for(i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + wsp_filtr_sin_3h[i] = /*an_params->multiplier **/ + sin(((float)6.2832 / ((SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)/3)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + + wsp_filtr_cos_3h[i] =/* an_params->multiplier **/ + cos(((float)6.2832 / ((SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)/3)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + } + wsp_filtr_3h_initialised=1; + } + + return 0; +} + +void e_filtr3h(void *arguments, void *logic) +{ +// struct e_filtr3h_args *args = (struct e_filtr3h_args *)arguments; + struct e_filtr3h_logic *log = (struct e_filtr3h_logic *)logic; + short i,i1,i2; + float orta = 0; + float ortb = 0; + float pr; + + i2 = bus_an_cur_sample_num; + + for (i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + i1 = i2 - i; + if(i1<0) + i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + pr = (float)log->buf_ptr[i1]; + pr*=log->an_params->multiplier; + orta += pr * wsp_filtr_sin_3h[i]; + ortb += pr * wsp_filtr_cos_3h[i]; + } + + *log->orta_ptr=orta; + *log->ortb_ptr=ortb; + *log->esk_ptr = (orta * orta + ortb * ortb) * 0.5f; +} diff --git a/src/e_filtr3h.h b/src/e_filtr3h.h new file mode 100644 index 0000000..6be564d --- /dev/null +++ b/src/e_filtr3h.h @@ -0,0 +1,48 @@ +/* + * e_filtr3h.h + * + * Created on: 11-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_FILTR3H_IN_H_ +#define E_FILTR3H_IN_H_ + +#include "tdefs.h" +#include "config.h" + +struct e_filtr3h_logic +{ +// float wsp_filtr_sin_[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa +// float wsp_filtr_cos_[SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa + u16 *buf_ptr; + float *esk_ptr; + float *orta_ptr; + float *ortb_ptr; + struct analog_in_params *an_params; +}; + +struct e_filtr3h_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_filtr3h_params +{ +}__attribute__((__packed__)); + +struct e_filtr3h_args +{ + struct e_filtr3h_io io; + struct e_filtr3h_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_filtr3h(void *args, void *logic); +extern int e_filtr3h_initlog(void *arguments, void *logic); + +#endif /* E_FILTR3H_H_ */ diff --git a/src/e_filtr5h.c b/src/e_filtr5h.c new file mode 100644 index 0000000..12d47be --- /dev/null +++ b/src/e_filtr5h.c @@ -0,0 +1,102 @@ +/* + * e_filtr5h.c + * + * Created on: 12-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "config.h" + +#include "e_filtr5h.h" +#include "analog_in.h" +#include + +float wsp_filtr_sin_5h[(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa +float wsp_filtr_cos_5h[(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa +int wsp_filtr_5h_initialised=0; + +int e_filtr5h_initlog(void *arguments, void *logic) +{ + struct e_filtr5h_args *args = (struct e_filtr5h_args *)arguments; + struct e_filtr5h_logic *log = (struct e_filtr5h_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + int i; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + log->an_params = an_params; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + if(!wsp_filtr_5h_initialised) + { + for(i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + wsp_filtr_sin_5h[i] = //an_params->multiplier * + sin(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + + wsp_filtr_cos_5h[i] = //an_params->multiplier * + cos(((float)6.2832 / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/5)) * i) / (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS/2); + } + + wsp_filtr_5h_initialised=1; + } + + return 0; +} + +void e_filtr5h(void *arguments, void *logic) +{ +// struct e_filtr5h_args *args = (struct e_filtr5h_args *)arguments; + struct e_filtr5h_logic *log = (struct e_filtr5h_logic *)logic; + short i,i1,i2; + float orta = 0; + float ortb = 0; + float pr; + + i2 = bus_an_cur_sample_num; + + for (i=0;i<(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS);i++) + { + i1 = i2 - i; + if(i1<0) + i1 += (SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); + pr = (float)log->buf_ptr[i1]; + pr*=log->an_params->multiplier; + orta += pr * wsp_filtr_sin_5h[i]; + ortb += pr * wsp_filtr_cos_5h[i]; + } + + *log->orta_ptr=orta; + *log->ortb_ptr=ortb; + *log->esk_ptr = (orta * orta + ortb * ortb) * 0.5f; +} diff --git a/src/e_filtr5h.h b/src/e_filtr5h.h new file mode 100644 index 0000000..2716155 --- /dev/null +++ b/src/e_filtr5h.h @@ -0,0 +1,48 @@ +/* + * e_filtr5h.h + * + * Created on: 12-05-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_FILTR5H_IN_H_ +#define E_FILTR5H_IN_H_ + +#include "tdefs.h" +#include "config.h" + +struct e_filtr5h_logic +{ +// float wsp_filtr_sin_[(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)]; ///< wspolczynniki filtracji dla sinus z korekcja fazowa +// float wsp_filtr_cos_[(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS)]; ///< wspolczynniki filtracji dla cosinus z korekcja fazowa + u16 *buf_ptr; + float *esk_ptr; + float *orta_ptr; + float *ortb_ptr; + struct analog_in_params *an_params; +}; + +struct e_filtr5h_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_filtr5h_params +{ +}__attribute__((__packed__)); + +struct e_filtr5h_args +{ + struct e_filtr5h_io io; + struct e_filtr5h_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_filtr5h(void *args, void *logic); +extern int e_filtr5h_initlog(void *arguments, void *logic); + +#endif /* E_FILTR5H_H_ */ diff --git a/src/e_mem.c b/src/e_mem.c new file mode 100644 index 0000000..468bdd6 --- /dev/null +++ b/src/e_mem.c @@ -0,0 +1,365 @@ +/* + * e_mem.c + * + * Created on: 22-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "e_mem.h" + +int e_mem_initlog(void *arguments, void *logic) +{ + struct e_mem_args *args = (struct e_mem_args *)arguments; + struct e_mem_logic *log = (struct e_mem_logic *)logic; + + if(set_float_ptr(args->io.orta_float_in,&log->orta)) + return -1; + if(set_float_ptr(args->io.ortb_float_in,&log->ortb)) + return -1; + if(set_float_ptr(args->io.esk_float_in,&log->esk)) + return -1; + + if(set_float_ptr(args->io.orta_float_out,&log->orta_out)) + return -1; + if(set_float_ptr(args->io.ortb_float_out,&log->ortb_out)) + return -1; + if(set_float_ptr(args->io.esk_float_out,&log->esk_out)) + return -1; + + if(set_bit_ptr_struct(args->io.memory_ok_out,&log->memory_OK)) + return -1; + if(set_bit_ptr_struct(args->io.memory_on_out,&log->memory_ON)) + return -1; + + log->nast_.Ur_ = args->params.napiecie_ * args->params.napiecie_; // wyliczenie wartosci granicznej + log->nast_.Up_ = log->nast_.Ur_ * (float)0.81; // wyliczenia wartosci powrotu dla wsp 0.9 + log->nast_.licz_t_ = (unsigned short)(args->params.czas_ * 100); // wyliczenie ilosci petli dla czasu pamieci + log->korekcja_orta = 1; + log->korekcja_ortb = 0; + log->U_OK = 0; + log->idx_buf = log->idx_buf1 = 0; + log->stan_pamieci = oczekiwanie_na_OK; + log->licz_pow = 0; + log->index_buf_T = 0; + + return 0; +} + + +void oblicz_okres(struct e_mem_logic *log,struct dane_probki* we) +{ + float T; + float local1; + u8 OK = 1; + float local3; + + + OK = log->U_OK; + if (we->flag) + { + + we->flag = 0; + T = we->reszta; // pozostalosc z poprzedniego wyliczania + T += (we->il_p) * 2500 * LOOP_CYCLE_MS;//dodanie calych okresow probkowania + local1 = (float)((float)(2500*LOOP_CYCLE_MS) * we->minus1 / (we->minus1 - we->bie));//wyliczenie czasu dopelnienia + T += local1; // wyliczenie okresu + + + if (T > 52700) OK = 0; // jesli wynik mniejszy niz 47,5 Hz + if (T < 47300) OK = 0; // jesli wynik wiekszy niz 52,5 Hz + + local3 = fabs((float)we->T_last - T); + if (local3>(30*LOOP_CYCLE_MS)) OK = 0; //jesli zmiana wieksza niz 30mHz to traktujemy wynik za niepewny + + we->T_last = T; + we->reszta = (2500*LOOP_CYCLE_MS) - local1; // wyliczenie reszty do nastepnego okresu + } + else + { + + OK = 0; + T = 0; + } + + if (we->flag) OK = 0; + log->buf_T[log->index_buf_T].T = T; + log->buf_T[log->index_buf_T].OK = OK; + // +// log->buf_T[log->index_buf_T].OK=1; +// log->buf_T[log->index_buf_T].T = 50000; + // + + log->index_buf_T++; + if (log->index_buf_T>19) + { + log->index_buf_T = 0; + } +} + +void e_mem(void *arguments, void *logic) +{ + struct e_mem_logic *log = (struct e_mem_logic *)logic; + + short idx_temp; + + log->w.kwampl_ = *log->esk; + log->w.orta_ = *log->orta; + log->w.ortb_ = *log->ortb; + +//wykrywanie przejscia przez zero + + if ((log->dane.minus1>0)&&(log->w.orta_<=0)) //przejscie z plusa do minusa + { + log->dane.plus.flag = 1; + log->dane.plus.il_p = log->dane.plus_il_p; + log->dane.plus_il_p = 0; + log->dane.plus.bie = log->w.orta_; + log->dane.plus.minus1 = log->dane.minus1; + + } else { + log->dane.plus_il_p++;//=LOOP_CYCLE_MS; + } + + if ((log->dane.minus1<0)&&(log->w.orta_>=0)) //przejscie z minusa do plusa + { + + log->dane.minus.flag = 1; + log->dane.minus.il_p = log->dane.minus_il_p; + log->dane.minus_il_p = 0; + log->dane.minus.bie = log->w.orta_; + log->dane.minus.minus1 = log->dane.minus1; + } else { + log->dane.minus_il_p++;//=LOOP_CYCLE_MS; + } + log->dane.minus1 = log->w.orta_; +//-------------------------------------------------------------------- + + + sprawdz_P( + &log->U_OK, + log->w.kwampl_>log->nast_.Ur_, + log->w.kwampl_nast_.Up_, + &log->licz_pow, + 72,2 + ); + + + + log->stan_.memory_ON_ = 0; + log->stan_.memory_OK_ = 0; + + switch(log->stan_pamieci) + { + case oczekiwanie_na_OK: + //aktualizacja buforow danych + log->buf_orta[log->idx_buf][0] = log->w.orta_; + log->buf_ortb[log->idx_buf][0] = log->w.ortb_; + if(log->f.OK) log->f_kor = log->f.T; + + if(log->f.OK && log->U_OK) + { + log->stan_pamieci = napiecie_OK_0; + log->stan_.memory_OK_ = 1; + } + log->stan_.orta_ = log->w.orta_; + log->stan_.ortb_ = log->w.ortb_; + log->stan_.kwampl_ = log->w.kwampl_; + break; + + case napiecie_OK_0: + //wystawienie wyjsc + idx_temp = log->idx_buf - (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS); + if(idx_temp<0) idx_temp += MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2; + log->stan_.orta_ = log->buf_orta[idx_temp][0]; + log->stan_.ortb_ = log->buf_ortb[idx_temp][0]; + log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ + + log->stan_.ortb_ * log->stan_.ortb_) * 0.5f; + // + //aktualizacja buforow danych + if (log->U_OK) + { + log->buf_orta[log->idx_buf][0] = log->w.orta_; + log->buf_ortb[log->idx_buf][0] = log->w.ortb_; + if (log->f.OK) + { + log->f_kor = log->f.T; + } + } else { + log->stan_pamieci = napiecie_z_pamieci_0; + log->idx_buf_start = log->idx_buf; + log->idx_buf1 = 0; + } + log->stan_.memory_OK_ = 1; + break; + + case napiecie_OK_1: + //wystawienie wyjsc + idx_temp = log->idx_buf - (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS); + if(idx_temp<0) idx_temp +=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2; + log->stan_.orta_ = log->buf_orta[idx_temp][1]; + log->stan_.ortb_ = log->buf_ortb[idx_temp][1]; + log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ + + log->stan_.ortb_ * log->stan_.ortb_) * 0.5f; + // + //aktualizacja buforow danych + if (log->U_OK) + { + log->buf_orta[log->idx_buf][1] = log->w.orta_; + log->buf_ortb[log->idx_buf][1] = log->w.ortb_; + if (log->f.OK) + { + log->f_kor = log->f.T; + } + } else { + log->stan_pamieci = napiecie_z_pamieci_1; + log->idx_buf_start = log->idx_buf; + log->idx_buf1 = 0; + } + log->stan_.memory_OK_ = 1; + break; + + case napiecie_z_pamieci_0: + //aktualizacja buforow danych drugiego banku danych + log->buf_orta[log->idx_buf][1] = log->w.orta_; + log->buf_ortb[log->idx_buf][1] = log->w.ortb_; + if(log->f.OK) log->f_kor_new = log->f.T; + if(log->f.OK && log->U_OK) + { + log->stan_pamieci = napiecie_OK_1; + log->f_kor = log->f_kor_new; + log->stan_.memory_OK_ = 1; + } + //wystawienie wyjsc + + log->idx_buf1++; // przesuniecie pozycji w buforze probek + if(log->idx_buf1>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)-1)log->idx_buf1 = 0; + idx_temp = log->idx_buf_start + log->idx_buf1; + if(idx_temp>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1) idx_temp -= (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2; + log->stan_.orta_ = log->buf_orta[idx_temp][0] *log->korekcja_orta + - log->buf_ortb[idx_temp][0] * log->korekcja_ortb + ; // wyliczenie skladowej ortogonalnej a wyniku + log->stan_.ortb_ = log->buf_ortb[idx_temp][0] * log->korekcja_orta + + log->buf_orta[idx_temp][0] * log->korekcja_ortb + ; // wyliczenie skladowej ortogonalnej b wyniku + log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ + + log->stan_.ortb_ * log->stan_.ortb_) * 0.5f; + + // + log->stan_.memory_ON_ = 1; + break; + + case napiecie_z_pamieci_1: + //aktualizacja buforow danych drugiego banku danych + log->buf_orta[log->idx_buf][0] = log->w.orta_; + log->buf_ortb[log->idx_buf][0] = log->w.ortb_; + if(log->f.OK) log->f_kor_new = log->f.T; + if(log->f.OK && log->U_OK) + { + log->stan_pamieci = napiecie_OK_0; + log->f_kor = log->f_kor_new; + log->stan_.memory_OK_ = 1; + } + //wystawienie wyjsc + + log->idx_buf1++; // przesuniecie pozycji w buforze probek + if(log->idx_buf1>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)-1)log->idx_buf1 = 0; + idx_temp = log->idx_buf_start + log->idx_buf1; + if(idx_temp>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1) idx_temp -= (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2; + log->stan_.orta_ = log->buf_orta[idx_temp][1] * log->korekcja_orta + - log->buf_ortb[idx_temp][1] * log->korekcja_ortb + ; // wyliczenie skladowej ortogonalnej a wyniku + log->stan_.ortb_ = log->buf_ortb[idx_temp][1] * log->korekcja_orta + + log->buf_orta[idx_temp][1] * log->korekcja_ortb + ; // wyliczenie skladowej ortogonalnej b wyniku + log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ + + log->stan_.ortb_ * log->stan_.ortb_) * 0.5f; + + // + log->stan_.memory_ON_ = 1; + break; + + + } + + + + log->idx_buf++; // przesuniecie pozycjiw buforze probek + if(log->idx_buf>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1)log->idx_buf = 0; + + check_and_set_struct(log->stan_.memory_OK_,&log->memory_OK); + check_and_set_struct(log->stan_.memory_ON_,&log->memory_ON); + *log->esk_out=log->stan_.kwampl_; + *log->orta_out=log->stan_.orta_; + *log->ortb_out=log->stan_.ortb_; +} + +void e_mem_100hz(void *arguments, void *logic) +{ + struct e_mem_logic *log = (struct e_mem_logic *)logic; + + float kat_przesuniecia; + float a = 1; + float b = 0; + unsigned short idx = 0; + float suma = 0; + unsigned short i; + + //jesli nie wykryto przejsc przez zero zbyt dlugo + if (log->dane.minus_il_p > 30/LOOP_CYCLE_MS) + { + log->dane.minus_il_p = 0; + } + if (log->dane.plus_il_p > 30/LOOP_CYCLE_MS) + { + log->dane.plus_il_p = 0; + } + // + + oblicz_okres(log,&log->dane.plus); + oblicz_okres(log,&log->dane.minus); + + //wyliczenie jako srenia waona + + for (i=0;i<20;i++) + { + if (log->buf_T[i].OK) + { + idx++; + suma += (float)log->buf_T[i].T; + } + } + if (idx>8) + { + log->f.T = (float)idx * (2500000) / suma; + log->f.OK = 1; + } + else + { + log->f.OK = 0; + log->f.T = 0; + } + + + if (log->stan_.memory_ON_) + { + log->licznik_czasu++; // odliczanie czasu aktywnosci pamieci + if(log->licznik_czasu>log->nast_.licz_t_) + { + log->stan_pamieci = oczekiwanie_na_OK;// wylaczenie pamieci po czasie + log->licz_pow = 0; + } + kat_przesuniecia = ((float)3.1416 *log->licznik_czasu + (float)0.62832)*(1-log->f_kor*0.02f); + a = cos(kat_przesuniecia); + b = sin(kat_przesuniecia); + } else { + log->licznik_czasu = 0; + } + log->korekcja_orta = a; + log->korekcja_ortb = b; +} diff --git a/src/e_mem.h b/src/e_mem.h new file mode 100644 index 0000000..26cf470 --- /dev/null +++ b/src/e_mem.h @@ -0,0 +1,132 @@ +/* + * e_mem.h + * + * Created on: 22-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_MEM_H_ +#define E_MEM_H_ + +#include "tdefs.h" + +typedef enum +{ + oczekiwanie_na_OK = 0, + napiecie_OK_0, + napiecie_OK_1, + napiecie_z_pamieci_0, + napiecie_z_pamieci_1 +} typ1; + +struct dane_probki +{ + float minus1; //probka poprzednia + float bie; // probka biezaca + float reszta; //reszta z poprzedniego przejscia przez zero + float T_last; //poprzednio wyliczona wartosc okresu + unsigned short il_p; //ilosc pelnych okresow probkowania pomiedzy przejsciami przez zero + u8 flag; //flaga wykrycia nowego przejscia przez zero +}; + +struct danef +{ + struct dane_probki plus; // + struct dane_probki minus; + float minus1; + unsigned short plus_il_p; + unsigned short minus_il_p; +}; + +struct wartosc_T +{ + float T; + u8 OK; +}; + +struct wejscie_e_mem +{ + float orta_; ///< skladowa ortogonalna a + float ortb_; ///< skladowa ortogonalna b + float kwampl_;///< kwadrat wartosci skutecznej skladowej podstawowej +}; + +struct wyjscie_e_mem +{ + u8 memory_OK_; ///< wyjscie aktywne - dane sprawne + u8 memory_ON_; ///< wyjscie z pamieci napieciowej + float orta_; ///< skladowa ortogonalna a wyniku + float ortb_; ///< skladowa ortogonalna b wyniku + float kwampl_;///< kwadrat amplitudy wyniku +}; + +struct Nastawy_przeliczone_e_mem +{ + float Ur_; ///< wartosc rozruchowa napiecia + float Up_; ///< wartosc powrotu napiecia + unsigned short licz_t_; ///< ilosc petli dla odliczenia czasu dzialania pamieci +}; + +struct e_mem_logic +{ + float *orta; + float *ortb; + float *esk; + struct binary_io memory_OK; + struct binary_io memory_ON; + float *orta_out; + float *ortb_out; + float *esk_out; + + struct Nastawy_przeliczone_e_mem nast_; //wartosci blokady pomiaru czestotliwosci + float buf_orta[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2][2]; // bufor ortogonalnej a + float buf_ortb[MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2][2]; // bufor ortogonalnej B + float korekcja_orta; // ortogonalna a wektora korekcji odstepstwa czestotliwosci + float korekcja_ortb; // ortogonalna b wektora korekcji odstepstwa czestotliwosci + short idx_buf; // indeks polozenia w buforze pamieci dla zapisywania danych + short idx_buf1; // indeks polozenia w buforze pamieci dla czytania danych + short idx_buf_start; // index polozenia w buforze w momencie startu oczytu z pamieci + typ1 stan_pamieci; // okreslenie stanu algorytmu pracy napiecia + unsigned short licznik_czasu; // naliczanie czasu aktywnosci pamieci napieciowej + short licz_pow; // licznik powtorzen dla napiecia + u8 U_OK; + float f_kor; + float f_kor_new; + unsigned short index_buf_T; + struct wartosc_T buf_T[20]; + struct wartosc_T f; + struct danef dane; + struct wejscie_e_mem w; + struct wyjscie_e_mem stan_; +}; + +struct e_mem_io +{ + u32 orta_float_in; + u32 ortb_float_in; + u32 esk_float_in; + u32 memory_ok_out; + u32 memory_on_out; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_mem_params +{ + double napiecie_; + double czas_; +}__attribute__((__packed__)); + +struct e_mem_args +{ + struct e_mem_io io; + struct e_mem_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_mem(void *args, void *logic); +extern void e_mem_100hz(void *args, void *logic); +extern int e_mem_initlog(void *arguments, void *logic); + +#endif /* E_MEM_H_ */ diff --git a/src/e_mem1.c b/src/e_mem1.c new file mode 100644 index 0000000..0fe0bd5 --- /dev/null +++ b/src/e_mem1.c @@ -0,0 +1,159 @@ +/* + * e_mem1.c + * + * Created on: 23-06-2016 + * Author: Krzysztof Jakubczyk + * + * Pamieta wartosci amplitudy z ostatnich 30 sekund oraz wylicza calke z amplitudy przebiegu w zadanym oknie pomiarowym + * + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_mem1.h" +#include "analog_in.h" +#include + +int e_mem1_initlog(void *arguments, void *logic) +{ + struct e_mem1_args *args = (struct e_mem1_args *)arguments; + struct e_mem1_logic *log = (struct e_mem1_logic *)logic; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_in >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_ptr_out >> 3)); + if((u8*)log->buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + *log->buf_ptr = (u32)&log->wy; + + log->srednia_temp = 0; + log->licznik_petli_1000Hz = 0; + log->licznik_petli_100Hz = -2; // po starcie dluzsze o dwie petle oczekiwanie na gotowosc pierwszego wyniku + + return 0; +} + +void e_mem1(void *arguments, void *logic) +{ + struct e_mem1_logic *log = (struct e_mem1_logic *)logic; + + uint16_t local = log->licznik_petli_1000Hz / 10; // wskaznik polozenia w buforze srednich za 10 ms + + log->srednia_temp += *log->esk_ptr; // wyliczanie sumy za pol okresu + log->licznik_petli_1000Hz++; + uint16_t local1 = log->licznik_petli_1000Hz % 10; + + if(!local1) //jesli jest wielokrotnosc 10 probek to naliczenie nowego polokresu + { + log->srednia_z_10ms[local] = log->srednia_temp * 0.1f; + log->srednia_temp = 0; + } + + if(log->licznik_petli_1000Hz == 50) // "przekrecenie" bufora + { + log->licznik_petli_1000Hz = 0; + } +} + +void e_mem1_100hz(void *arguments, void *logic) +{ + struct e_mem1_logic *log = (struct e_mem1_logic *)logic; + + + unsigned short i; + short index; + + log->licznik_petli_100Hz++; + + if(log->licznik_petli_100Hz > 4) // nowy wynik dla zakresu z okresem 50ms + { + log->licznik_petli_100Hz = 0; + // obsluga pamieci z zakresu 4 - 8s + if(log->wy.idx_50ms_ % 2) + { + // obsluga pamieci z zakresu 8 - 16s + if(log->wy.idx_100ms_ % 2) + { + // obsluga pamieci z zakresu 16 - 30s + if(log->wy.idx_200ms_ % 2) + { + log->wy.idx_400ms_++; + if(log->wy.idx_400ms_>35) + log->wy.idx_400ms_ = 0; + + index = log->wy.idx_200ms_ + 1; + if (index>39) + index = 0; + log->wy.srednia_400ms_[log->wy.idx_400ms_] = log->wy.srednia_200ms_[index]; + index++; + if (index>39) + index = 0; + log->wy.srednia_400ms_[log->wy.idx_400ms_] += log->wy.srednia_200ms_[index]; + log->wy.srednia_400ms_[log->wy.idx_400ms_] /= 2; + } + // + + log->wy.idx_200ms_++; + if(log->wy.idx_200ms_>39) + log->wy.idx_200ms_ = 0; + + index = log->wy.idx_100ms_ + 1; + + if (index>39) + index = 0; + + log->wy.srednia_200ms_[log->wy.idx_200ms_] = log->wy.srednia_100ms_[index]; + index++; + + if (index>39) + index = 0; + + log->wy.srednia_200ms_[log->wy.idx_200ms_] += log->wy.srednia_100ms_[index]; + log->wy.srednia_200ms_[log->wy.idx_200ms_] /= 2; + } + // + + log->wy.idx_100ms_++; + + if(log->wy.idx_100ms_>39) + log->wy.idx_100ms_ = 0; + + index = log->wy.idx_50ms_ + 1; + + if (index>79) + index = 0; + + log->wy.srednia_100ms_[log->wy.idx_100ms_] = log->wy.srednia_50ms_[index]; + index++; + + if (index>79) + index = 0; + + log->wy.srednia_100ms_[log->wy.idx_100ms_] += log->wy.srednia_50ms_[index]; + log->wy.srednia_100ms_[log->wy.idx_100ms_] /= 2; + } + // + // obsluga pamieci z zakresu 0 - 4s + log->wy.idx_50ms_++; + + if(log->wy.idx_50ms_>79) + log->wy.idx_50ms_ = 0; + + log->wy.srednia_50ms_[log->wy.idx_50ms_] = 0; + + for (i=0;i<5;i++) //wyliczenie sredniej za okres 50 ms; + { + log->wy.srednia_50ms_[log->wy.idx_50ms_]+= sqrt(log->srednia_z_10ms[i]); + } + + log->wy.srednia_50ms_[log->wy.idx_50ms_] /= 5; + // + } + log->wy.czas_++; + if(log->wy.czas_>3000) + log->wy.czas_ = 3000; +} diff --git a/src/e_mem1.h b/src/e_mem1.h new file mode 100644 index 0000000..d4e75cd --- /dev/null +++ b/src/e_mem1.h @@ -0,0 +1,58 @@ +/* + * e_mem1.h + * + * Created on: 23-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_MEM1_IN_H_ +#define E_MEM1_IN_H_ + +#include "tdefs.h" + +struct e_mem1_wyjscie +{ + float srednia_50ms_[80];///< zapamietanie dla czasu 0-4 s + float srednia_100ms_[40];///< zapamietywanie dla czasu 4-8 s + float srednia_200ms_[40];///< zapamietywanie dla czasu 8-16 s + float srednia_400ms_[36];///< zapamietywanie dla czasu 16-30 s + short idx_50ms_; ///< numer aktualnej probki w pamieci dla zakresu 0-4s + short idx_100ms_; ///< numer aktualnej probki w pamieci dla zakresu 4-8s + short idx_200ms_; ///< numer aktualnej probki w pamieci dla zakresu 8-16s + short idx_400ms_; ///< numer aktualnej probki w pamieci dla zakresu 16-30s + short czas_ ; ///< czas napelnienia bufora od startu w [10*ms] - ilosc petli 100Hz +}; + +struct e_mem1_logic +{ + float *esk_ptr; + u32 *buf_ptr; + struct e_mem1_wyjscie wy; + short licznik_petli_1000Hz; + short licznik_petli_100Hz; + float srednia_temp; // wartosc posrednia calkowania + float srednia_z_10ms[5];// zapamietywanie wartosci chwilowych z polokresu +}; + +struct e_mem1_io +{ + u32 esk_float_in; + u32 buf_ptr_out; +}__attribute__((__packed__)); + +struct e_mem1_params +{ +}__attribute__((__packed__)); + +struct e_mem1_args +{ + struct e_mem1_io io; + struct e_mem1_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_mem1(void *args, void *logic); +extern void e_mem1_100hz(void *args, void *logic); +extern int e_mem1_initlog(void *arguments, void *logic); + +#endif /* E_MEM1_H_ */ diff --git a/src/e_mulcon.c b/src/e_mulcon.c new file mode 100644 index 0000000..3ac3106 --- /dev/null +++ b/src/e_mulcon.c @@ -0,0 +1,63 @@ +/* + * e_mulcon.c // Mnozenie wektora przez stala + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_mulcon.h" +#include + +int e_mulcon_initlog(void *arguments, void *logic) +{ + struct e_mulcon_args *args = (struct e_mulcon_args *)arguments; + struct e_mulcon_logic *log = (struct e_mulcon_logic *)logic; + + log->orta_in_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_in >> 3)); + if((u8 *)log->orta_in_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_in_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_in >> 3)); + if((u8 *)log->ortb_in_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_out_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_out_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_out_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_out_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_ = args->params.value * cos(args->params.phase * (float)3.1416/180); + log->ortb_ = args->params.value * sin(args->params.phase * (float)3.1416/180); + + return 0; +} + +void e_mulcon(void *arguments, void *logic) +{ + struct e_mulcon_args *args = (struct e_mulcon_args *)arguments; + struct e_mulcon_logic *log = (struct e_mulcon_logic *)logic; + + float orta, ortb; + + orta = *log->orta_in_ptr * log->orta_ + *log->ortb_in_ptr * log->ortb_; + ortb = *log->ortb_in_ptr * log->orta_ - *log->orta_in_ptr * log->ortb_; + + if(args->io.esk_float_out) + *log->esk_ptr=(orta*orta + ortb*ortb)/2; + + if(args->io.orta_float_out) + *log->orta_out_ptr = orta; + + if(args->io.ortb_float_out) + *log->ortb_out_ptr = ortb; +} diff --git a/src/e_mulcon.h b/src/e_mulcon.h new file mode 100644 index 0000000..d27b71c --- /dev/null +++ b/src/e_mulcon.h @@ -0,0 +1,49 @@ +/* + * e_mulcon.h + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_MULCON_H_ +#define E_MULCON_H_ + +#include "tdefs.h" + +struct e_mulcon_logic +{ + float *orta_in_ptr; + float *ortb_in_ptr; + float *orta_out_ptr; + float *ortb_out_ptr; + float *esk_ptr; + float orta_; + float ortb_; +}; + +struct e_mulcon_io +{ + u32 orta_float_in; + u32 ortb_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_mulcon_params +{ + double value; + double phase; +}__attribute__((__packed__)); + +struct e_mulcon_args +{ + struct e_mulcon_io io; + struct e_mulcon_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_mulcon(void *args, void *logic); +extern int e_mulcon_initlog(void *arguments, void *logic); + +#endif /* E_MULCON_H_ */ diff --git a/src/e_mulvec.c b/src/e_mulvec.c new file mode 100644 index 0000000..1ea0f65 --- /dev/null +++ b/src/e_mulvec.c @@ -0,0 +1,67 @@ +/* + * e_mulvec.c + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_mulvec.h" + +int e_mulvec_initlog(void *arguments, void *logic) +{ + struct e_mulvec_args *args = (struct e_mulvec_args *)arguments; + struct e_mulvec_logic *log = (struct e_mulvec_logic *)logic; + + log->orta1_ptr = (float *)(log_manager.nets_data + (args->io.orta1_float_in >> 3)); + if((u8 *)log->orta1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb1_ptr = (float *)(log_manager.nets_data + (args->io.ortb1_float_in >> 3)); + if((u8 *)log->ortb1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta2_ptr = (float *)(log_manager.nets_data + (args->io.orta2_float_in >> 3)); + if((u8 *)log->orta2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb2_ptr = (float *)(log_manager.nets_data + (args->io.ortb2_float_in >> 3)); + if((u8 *)log->ortb2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_mulvec(void *arguments, void *logic) +{ + struct e_mulvec_args *args = (struct e_mulvec_args *)arguments; + struct e_mulvec_logic *log = (struct e_mulvec_logic *)logic; + + float orta, ortb; + + ortb = *log->orta1_ptr * *log->orta2_ptr - *log->ortb1_ptr * *log->ortb2_ptr; + orta = *log->ortb1_ptr * *log->orta2_ptr + *log->orta1_ptr * *log->ortb2_ptr; + + if(args->io.esk_float_out) + *log->esk_ptr=(orta*orta + ortb*ortb)/2; + + if(args->io.orta_float_out) + *log->orta_ptr = orta; + + if(args->io.ortb_float_out) + *log->ortb_ptr = ortb; +} diff --git a/src/e_mulvec.h b/src/e_mulvec.h new file mode 100644 index 0000000..48b0c38 --- /dev/null +++ b/src/e_mulvec.h @@ -0,0 +1,49 @@ +/* + * e_mulvec.h + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_MULVEC_H_ +#define E_MULVEC_H_ + +#include "tdefs.h" + +struct e_mulvec_logic +{ + float *orta1_ptr; + float *ortb1_ptr; + float *orta2_ptr; + float *ortb2_ptr; + float *orta_ptr; + float *ortb_ptr; + float *esk_ptr; +}; + +struct e_mulvec_io +{ + u32 orta1_float_in; + u32 ortb1_float_in; + u32 orta2_float_in; + u32 ortb2_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_mulvec_params +{ +}__attribute__((__packed__)); + +struct e_mulvec_args +{ + struct e_mulvec_io io; + struct e_mulvec_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_mulvec(void *args, void *logic); +extern int e_mulvec_initlog(void *arguments, void *logic); + +#endif /* E_MULVEC_H_ */ diff --git a/src/e_phi.c b/src/e_phi.c new file mode 100644 index 0000000..025f004 --- /dev/null +++ b/src/e_phi.c @@ -0,0 +1,49 @@ +/* + * e_phi.c + * + * Created on: 11-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_phi.h" +#include "helper.h" +#include + +int e_phi_initlog(void *arguments, void *logic) +{ + struct e_phi_args *args = (struct e_phi_args *)arguments; + struct e_phi_logic *log = (struct e_phi_logic *)logic; + + if(set_float_ptr(args->io.orta_a_float_in,&log->orta_a)) + return -1; + if(set_float_ptr(args->io.ortb_a_float_in,&log->ortb_a)) + return -1; + if(set_float_ptr(args->io.orta_b_float_in,&log->orta_b)) + return -1; + if(set_float_ptr(args->io.ortb_b_float_in,&log->ortb_b)) + return -1; + if(set_float_ptr(args->io.phi_float_out,&log->phi)) + return -1; + + return 0; +} + +#define PI 3.14159265 + +float get_phase(float cos, float sin); +float get_phase_diff(float phi1, float phi2); + +void e_phi_20hz(void *arguments, void *logic) +{ + struct e_phi_logic *log = (struct e_phi_logic *)logic; + + float phi1,phi2; + + phi1=get_phase(*log->ortb_a,*log->orta_a); + phi2=get_phase(*log->ortb_b,*log->orta_b); + + *log->phi=get_phase_diff(phi1,phi2); +} diff --git a/src/e_phi.h b/src/e_phi.h new file mode 100644 index 0000000..893aa6e --- /dev/null +++ b/src/e_phi.h @@ -0,0 +1,50 @@ +/* + * e_phi.h + * + * Created on: 11-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_PHI_H_ +#define E_PHI_H_ + +#include "tdefs.h" + +struct e_phi_logic +{ + float *orta_a; + float *ortb_a; + + float *orta_b; + float *ortb_b; + + float *phi; +}; + +struct e_phi_io +{ + u32 orta_a_float_in; + u32 ortb_a_float_in; + u32 orta_b_float_in; + u32 ortb_b_float_in; + u32 phi_float_out; +}__attribute__((__packed__)); + +struct e_phi_params +{ +}__attribute__((__packed__)); + +struct e_phi_args +{ + struct e_phi_io io; + struct e_phi_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int e_phi_initlog(void *arguments, void *logic); +extern void e_phi_20hz(void *arguments, void *logic); + +extern float get_phase(float cos, float sin); +extern float get_phase_diff(float phi1, float phi2); + +#endif /* E_PHI_H_ */ diff --git a/src/e_pq_1f.c b/src/e_pq_1f.c new file mode 100644 index 0000000..418c701 --- /dev/null +++ b/src/e_pq_1f.c @@ -0,0 +1,67 @@ +/* + * e_pq_1f.c + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_pq_1f.h" +#include "analog_in.h" +#include + +int e_pq_1f_initlog(void *arguments, void *logic) +{ + struct e_pq_1f_args *args = (struct e_pq_1f_args *)arguments; + struct e_pq_1f_logic *log = (struct e_pq_1f_logic *)logic; + + log->i_orta = (float *)(log_manager.nets_data + (args->io.i_orta_float_in >> 3)); + if((u8 *)log->i_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i_ortb = (float *)(log_manager.nets_data + (args->io.i_ortb_float_in >> 3)); + if((u8 *)log->i_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u_orta = (float *)(log_manager.nets_data + (args->io.u_orta_float_in >> 3)); + if((u8 *)log->u_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u_ortb = (float *)(log_manager.nets_data + (args->io.u_ortb_float_in >> 3)); + if((u8 *)log->u_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->p = (float *)(log_manager.nets_data + (args->io.p_float_out >> 3)); + if((u8 *)log->p > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->q = (float *)(log_manager.nets_data + (args->io.q_float_out >> 3)); + if((u8 *)log->q > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_pq_1f_100hz(void *arguments, void *logic) +{ + struct e_pq_1f_logic *log = (struct e_pq_1f_logic *)logic; + + float P,Q; + P = *log->i_orta * *log->u_orta + + *log->i_ortb * *log->u_ortb; + Q = + *log->i_ortb * *log->u_orta - + *log->i_orta * *log->u_ortb; + + + if ((fabs(P) > 0.001f) || (fabs(Q) > 0.001f)) + { + *log->p = P; + *log->q = Q; + } else { + *log->p = 0; + *log->q = 0; + } +} diff --git a/src/e_pq_1f.h b/src/e_pq_1f.h new file mode 100644 index 0000000..0a5657b --- /dev/null +++ b/src/e_pq_1f.h @@ -0,0 +1,47 @@ +/* + * e_pq_1f.h + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_PQ_1F_H_ +#define E_PQ_1F_H_ + +#include "tdefs.h" + +struct e_pq_1f_logic +{ + float *i_orta; + float *i_ortb; + float *u_orta; + float *u_ortb; + float *p; + float *q; +}; + +struct e_pq_1f_io +{ + u32 i_orta_float_in; + u32 i_ortb_float_in; + u32 u_orta_float_in; + u32 u_ortb_float_in; + u32 p_float_out; + u32 q_float_out; +}__attribute__((__packed__)); + +struct e_pq_1f_params +{ +}__attribute__((__packed__)); + +struct e_pq_1f_args +{ + struct e_pq_1f_io io; + struct e_pq_1f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_pq_1f_100hz(void *args, void *logic); +extern int e_pq_1f_initlog(void *arguments, void *logic); + +#endif /* E_PQ_1F_H_ */ diff --git a/src/e_pq_3f.c b/src/e_pq_3f.c new file mode 100644 index 0000000..fdd5cea --- /dev/null +++ b/src/e_pq_3f.c @@ -0,0 +1,123 @@ +/* + * e_pq_3f.c + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_pq_3f.h" +#include "analog_in.h" +#include "helper.h" +#include + +#define _1_PRZEZ_2_PIERW_Z_3 0.288675135 +#define PIERW_Z_3 1.732050808 + +int e_pq_3f_initlog(void *arguments, void *logic) +{ + struct e_pq_3f_args *args = (struct e_pq_3f_args *)arguments; + struct e_pq_3f_logic *log = (struct e_pq_3f_logic *)logic; + + log->i1_orta = (float *)(log_manager.nets_data + (args->io.i1_orta_float_in >> 3)); + if((u8 *)log->i1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i1_ortb = (float *)(log_manager.nets_data + (args->io.i1_ortb_float_in >> 3)); + if((u8 *)log->i1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i2_orta = (float *)(log_manager.nets_data + (args->io.i2_orta_float_in >> 3)); + if((u8 *)log->i2_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i2_ortb = (float *)(log_manager.nets_data + (args->io.i2_ortb_float_in >> 3)); + if((u8 *)log->i2_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i3_orta = (float *)(log_manager.nets_data + (args->io.i3_orta_float_in >> 3)); + if((u8 *)log->i3_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i3_ortb = (float *)(log_manager.nets_data + (args->io.i3_ortb_float_in >> 3)); + if((u8 *)log->i3_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u1_orta = (float *)(log_manager.nets_data + (args->io.u1_orta_float_in >> 3)); + if((u8 *)log->u1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u1_ortb = (float *)(log_manager.nets_data + (args->io.u1_ortb_float_in >> 3)); + if((u8 *)log->u1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u2_orta = (float *)(log_manager.nets_data + (args->io.u2_orta_float_in >> 3)); + if((u8 *)log->u2_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u2_ortb = (float *)(log_manager.nets_data + (args->io.u2_ortb_float_in >> 3)); + if((u8 *)log->u2_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u3_orta = (float *)(log_manager.nets_data + (args->io.u3_orta_float_in >> 3)); + if((u8 *)log->u3_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u3_ortb = (float *)(log_manager.nets_data + (args->io.u3_ortb_float_in >> 3)); + if((u8 *)log->u3_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->p = (float *)(log_manager.nets_data + (args->io.p_float_out >> 3)); + if((u8 *)log->p > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->q = (float *)(log_manager.nets_data + (args->io.q_float_out >> 3)); + if((u8 *)log->q > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + +// dodatkowe we 2019.06.03 + if(set_pointer_in_ptr(args->io.I_params_ptr_in,(u32 *)&log->I_params)) + return -1; + if(set_pointer_in_ptr(args->io.U_params_ptr_in,(u32 *)&log->U_params)) + return -1; + if(set_pointer_out_ptr(args->io.P_params_ptr_out,(u32*)&log->P_params)) + return -1; + + log->P_params.znam_pierw = PIERW_Z_3 * log->I_params->znam_pierw * log->U_params->znam_pierw; + log->P_params.znam_wtor = PIERW_Z_3 * log->I_params->znam_wtor * log->U_params->znam_wtor; + + return 0; +} + +void e_pq_3f_100hz(void *arguments, void *logic) +{ + struct e_pq_3f_logic *log = (struct e_pq_3f_logic *)logic; + + float P,Q; + + P = + *log->i1_orta * *log->u1_orta + + *log->i1_ortb * *log->u1_ortb + + *log->i2_orta * *log->u2_orta + + *log->i2_ortb * *log->u2_ortb + + *log->i3_orta * *log->u3_orta + + *log->i3_ortb * *log->u3_ortb; + Q = + *log->i1_ortb * *log->u1_orta - + *log->i1_orta * *log->u1_ortb + + *log->i2_ortb * *log->u2_orta - + *log->i2_orta * *log->u2_ortb + + *log->i3_ortb * *log->u3_orta - + *log->i3_orta * *log->u3_ortb; + + if ((fabs(P) > 0.001f) || (fabs(Q) > 0.001f)) + { + *log->p=P * _1_PRZEZ_2_PIERW_Z_3; + *log->q=Q * _1_PRZEZ_2_PIERW_Z_3; + } else { + *log->p=0; + *log->q=0; + } +} diff --git a/src/e_pq_3f.h b/src/e_pq_3f.h new file mode 100644 index 0000000..23d5778 --- /dev/null +++ b/src/e_pq_3f.h @@ -0,0 +1,71 @@ +/* + * e_pq_3f.h + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_PQ_3F_H_ +#define E_PQ_3F_H_ + +#include "tdefs.h" +#include "analog_in.h" + +struct e_pq_3f_logic +{ + float *i1_orta; + float *i1_ortb; + float *i2_orta; + float *i2_ortb; + float *i3_orta; + float *i3_ortb; + float *u1_orta; + float *u1_ortb; + float *u2_orta; + float *u2_ortb; + float *u3_orta; + float *u3_ortb; + float *p; + float *q; + + struct analog_in_params *I_params; + struct analog_in_params *U_params; + struct analog_in_params P_params; +}; + +struct e_pq_3f_io +{ + u32 i1_orta_float_in; + u32 i1_ortb_float_in; + u32 i2_orta_float_in; + u32 i2_ortb_float_in; + u32 i3_orta_float_in; + u32 i3_ortb_float_in; + u32 u1_orta_float_in; + u32 u1_ortb_float_in; + u32 u2_orta_float_in; + u32 u2_ortb_float_in; + u32 u3_orta_float_in; + u32 u3_ortb_float_in; + u32 I_params_ptr_in; + u32 U_params_ptr_in; + u32 p_float_out; + u32 q_float_out; + u32 P_params_ptr_out; +}__attribute__((__packed__)); + +struct e_pq_3f_params +{ +}__attribute__((__packed__)); + +struct e_pq_3f_args +{ + struct e_pq_3f_io io; + struct e_pq_3f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_pq_3f_100hz(void *args, void *logic); +extern int e_pq_3f_initlog(void *arguments, void *logic); + +#endif /* E_PQ_3F_H_ */ diff --git a/src/e_pq_A.c b/src/e_pq_A.c new file mode 100644 index 0000000..8f0cd12 --- /dev/null +++ b/src/e_pq_A.c @@ -0,0 +1,89 @@ +/* + * e_pq_A.c + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_pq_A.h" +#include "analog_in.h" +#include + +int e_pq_a_initlog(void *arguments, void *logic) +{ + struct e_pq_a_args *args = (struct e_pq_a_args *)arguments; + struct e_pq_a_logic *log = (struct e_pq_a_logic *)logic; + + log->i1_orta = (float *)(log_manager.nets_data + (args->io.i1_orta_float_in >> 3)); + if((u8 *)log->i1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i1_ortb = (float *)(log_manager.nets_data + (args->io.i1_ortb_float_in >> 3)); + if((u8 *)log->i1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i2_orta = (float *)(log_manager.nets_data + (args->io.i2_orta_float_in >> 3)); + if((u8 *)log->i2_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->i2_ortb = (float *)(log_manager.nets_data + (args->io.i2_ortb_float_in >> 3)); + if((u8 *)log->i2_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u1_orta = (float *)(log_manager.nets_data + (args->io.u1_orta_float_in >> 3)); + if((u8 *)log->u1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u1_ortb = (float *)(log_manager.nets_data + (args->io.u1_ortb_float_in >> 3)); + if((u8 *)log->u1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u2_orta = (float *)(log_manager.nets_data + (args->io.u2_orta_float_in >> 3)); + if((u8 *)log->u2_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->u2_ortb = (float *)(log_manager.nets_data + (args->io.u2_ortb_float_in >> 3)); + if((u8 *)log->u2_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->p = (float *)(log_manager.nets_data + (args->io.p_float_out >> 3)); + if((u8 *)log->p > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->q = (float *)(log_manager.nets_data + (args->io.q_float_out >> 3)); + if((u8 *)log->q > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_pq_a_100hz(void *arguments, void *logic) +{ + struct e_pq_a_logic *log = (struct e_pq_a_logic *)logic; + + float P,Q; + P = 1.73205f * + (*log->i1_orta * *log->u1_orta + + *log->i1_ortb * *log->u1_ortb + + *log->i2_orta * *log->u2_orta + + *log->i2_ortb * *log->u2_ortb) + ; + Q = 1.73205f * + (*log->i1_ortb * *log->u1_orta - + *log->i1_orta * *log->u1_ortb + + *log->i2_ortb * *log->u2_orta - + *log->i2_orta * *log->u2_ortb) + ; + + if ((fabs(P) > 0.001f) || (fabs(Q) > 0.001f)) + { + *log->p=P; + *log->q=Q; + } else { + *log->p=0; + *log->q=0; + } +} diff --git a/src/e_pq_A.h b/src/e_pq_A.h new file mode 100644 index 0000000..d4fdce7 --- /dev/null +++ b/src/e_pq_A.h @@ -0,0 +1,55 @@ +/* + * e_pq_A.h + * + * Created on: 06-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_PQ_A_H_ +#define E_PQ_A_H_ + +#include "tdefs.h" + +struct e_pq_a_logic +{ + float *i1_orta; + float *i1_ortb; + float *i2_orta; + float *i2_ortb; + float *u1_orta; + float *u1_ortb; + float *u2_orta; + float *u2_ortb; + float *p; + float *q; +}; + +struct e_pq_a_io +{ + u32 i1_orta_float_in; + u32 i1_ortb_float_in; + u32 i2_orta_float_in; + u32 i2_ortb_float_in; + u32 u1_orta_float_in; + u32 u1_ortb_float_in; + u32 u2_orta_float_in; + u32 u2_ortb_float_in; + u32 p_float_out; + u32 q_float_out; +}__attribute__((__packed__)); + +struct e_pq_a_params +{ +}__attribute__((__packed__)); + +struct e_pq_a_args +{ + struct e_pq_a_io io; + struct e_pq_a_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_pq_a_100hz(void *args, void *logic); +extern int e_pq_a_initlog(void *arguments, void *logic); + +#endif /* E_PQ_A_H_ */ diff --git a/src/e_rms.c b/src/e_rms.c new file mode 100644 index 0000000..34c7cb1 --- /dev/null +++ b/src/e_rms.c @@ -0,0 +1,82 @@ +/* + * e_rms.c + * + * Created on: 07-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_rms.h" +#include "analog_in.h" +#include + +int e_rms_initlog(void *arguments, void *logic) +{ + struct e_rms_args *args = (struct e_rms_args *)arguments; + struct e_rms_logic *log = (struct e_rms_logic *)logic; + struct analog_in_params *an_params; + u32 *an_params_ptr; + u32 *buf_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + an_params=(struct analog_in_params *)*an_params_ptr; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + log->rms_ptr = (float *)(log_manager.nets_data + (args->io.rms_float_out >> 3)); + if((u8 *)log->rms_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->rms_kw_ptr = (float *)(log_manager.nets_data + (args->io.rms_kw_float_out >> 3)); + if((u8 *)log->rms_kw_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->multiplier = an_params->multiplier; /* (float)1.1107 */; + log->multiplier *= log->multiplier; + log->multiplier /= (MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS);//60; + log->offset = 32767 + an_params->offset; + + return 0; +} + +void e_rms(void *arguments, void *logic) +{ + struct e_rms_args *args = (struct e_rms_args *)arguments; + struct e_rms_logic *log = (struct e_rms_logic *)logic; + int16_t i,i1; + float pr,skut; + + i1 = bus_an_cur_sample_num; + + skut = 0; + for (i=0;i<(MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS);i++) + { + if(i1<0) i1 += (MAIN_FREQ_PERIOD_MS*SAMPLES_PER_MS)*2; + + pr = (float)log->buf_ptr[i1] - log->offset; + pr *= pr; + skut += pr; + i1--; + } + + if(args->io.rms_kw_float_out) + *log->rms_kw_ptr=skut*log->multiplier; + + if(args->io.rms_float_out) + *log->rms_ptr=sqrtf(skut*log->multiplier); +} diff --git a/src/e_rms.h b/src/e_rms.h new file mode 100644 index 0000000..441e19e --- /dev/null +++ b/src/e_rms.h @@ -0,0 +1,44 @@ +/* + * e_rms.h + * + * Created on: 06-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_RMS_H_ +#define E_RMS_H_ + +#include "tdefs.h" + +struct e_rms_logic +{ + float multiplier; + float offset; + u16 *buf_ptr; + float *rms_ptr; + float *rms_kw_ptr; +}; + +struct e_rms_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; + u32 rms_float_out; + u32 rms_kw_float_out; +}__attribute__((__packed__)); + +struct e_rms_params +{ +}__attribute__((__packed__)); + +struct e_rms_args +{ + struct e_rms_io io; + struct e_rms_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_rms(void *args, void *logic); +extern int e_rms_initlog(void *arguments, void *logic); + +#endif /* E_RMS_H_ */ diff --git a/src/e_rms_1t.c b/src/e_rms_1t.c new file mode 100644 index 0000000..622e570 --- /dev/null +++ b/src/e_rms_1t.c @@ -0,0 +1,130 @@ +/* + * e_rms.c + * + * Created on: 07-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_rms_1t.h" +#include "analog_in.h" +#include + +int e_rms_1t_initlog(void *arguments, void *logic) +{ + struct e_rms_1t_args *args = (struct e_rms_1t_args *)arguments; + struct e_rms_1t_logic *log = (struct e_rms_1t_logic *)logic; + + log->probka_ptr = (float *)(log_manager.nets_data + (args->io.probka_float_in >> 3)); + if((u8*)log->probka_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->rms_ptr = (float *)(log_manager.nets_data + (args->io.rms_1t_float_out >> 3)); + if((u8 *)log->rms_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->rms_kw_ptr = (float *)(log_manager.nets_data + (args->io.rms_1t_kw_float_out >> 3)); + if((u8 *)log->rms_kw_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_rms_1t(void *arguments, void *logic) +{ + struct e_rms_1t_args *args = (struct e_rms_1t_args *)arguments; + struct e_rms_1t_logic *log = (struct e_rms_1t_logic *)logic; + + float probka_ = *log->probka_ptr; + float local = probka_ * probka_; + float kwskut; + + log->dw.sumam += local; + log->dw.sumap += local; + log->dw.wskm++; + log->dw.wskp++; + + if ( (probka_ * log->dw.last) < 0) + { + log->dw.liczz = 0; + if (probka_<0) + { + if (log->dw.wskp>10) + { + kwskut = log->dw.sumap / log->dw.wskp; + if(args->io.rms_1t_kw_float_out) + *log->rms_kw_ptr = kwskut; + log->dw.sumap = 0; + log->dw.wskp = 0; + + if(args->io.rms_1t_float_out) + *log->rms_ptr = sqrtf(kwskut); + } + } + else + { + if (log->dw.wskm>10) + { + kwskut = log->dw.sumam / log->dw.wskm; + if(args->io.rms_1t_kw_float_out) + *log->rms_kw_ptr = kwskut; + log->dw.sumam = 0; + log->dw.wskm = 0; + + + if(args->io.rms_1t_float_out) + *log->rms_ptr = sqrtf(kwskut); + } + } + } + + if (local < (float)0.000625) + { + log->dw.liczz++; + } + + if(log->dw.liczz > 60) + { + log->dw.sumap = 0; + log->dw.wskp = 0; + log->dw.sumam = 0; + log->dw.wskm = 0; + log->dw.liczz = 0; + kwskut=0; + if(args->io.rms_1t_kw_float_out) + *log->rms_kw_ptr = kwskut; + + if(args->io.rms_1t_float_out) + *log->rms_ptr = sqrtf(kwskut); + } + + if (log->dw.wskp>500) + { + kwskut = log->dw.sumap / log->dw.wskp; + if(args->io.rms_1t_kw_float_out) + *log->rms_kw_ptr = kwskut; + + if(args->io.rms_1t_float_out) + *log->rms_ptr = sqrtf(kwskut); + log->dw.sumap = 0; + log->dw.wskp = 0; + log->dw.liczz = 0; + } + + if (log->dw.wskm>500) + { + kwskut = log->dw.sumam / log->dw.wskm; + if(args->io.rms_1t_kw_float_out) + *log->rms_kw_ptr = kwskut; + + if(args->io.rms_1t_float_out) + *log->rms_ptr = sqrtf(kwskut); + log->dw.sumam = 0; + log->dw.wskm = 0; + log->dw.liczz = 0; + } + + log->dw.last = probka_; +} diff --git a/src/e_rms_1t.h b/src/e_rms_1t.h new file mode 100644 index 0000000..a83ee02 --- /dev/null +++ b/src/e_rms_1t.h @@ -0,0 +1,52 @@ +/* + * e_rms_1t.h + * + * Created on: 06-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_RMS_1T_H_ +#define E_RMS_1T_H_ + +#include "tdefs.h" + +struct dane_wewnetrzne_rms_1t +{ + float sumap; + float sumam; + float last; + short liczz; + unsigned short wskp; + unsigned short wskm; +}; + +struct e_rms_1t_logic +{ + float *probka_ptr; + float *rms_ptr; + float *rms_kw_ptr; + struct dane_wewnetrzne_rms_1t dw; +}; + +struct e_rms_1t_io +{ + u32 probka_float_in; + u32 rms_1t_float_out; + u32 rms_1t_kw_float_out; +}__attribute__((__packed__)); + +struct e_rms_1t_params +{ +}__attribute__((__packed__)); + +struct e_rms_1t_args +{ + struct e_rms_1t_io io; + struct e_rms_1t_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_rms_1t(void *args, void *logic); +extern int e_rms_1t_initlog(void *arguments, void *logic); + +#endif /* E_RMS_1T_H_ */ diff --git a/src/e_sc_n.c b/src/e_sc_n.c new file mode 100644 index 0000000..b58650d --- /dev/null +++ b/src/e_sc_n.c @@ -0,0 +1,81 @@ +/* + * e_sc_n.c + * + * Created on: 14-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_sc_n.h" +#include "analog_in.h" +#include + +int e_sc_n_initlog(void *arguments, void *logic) +{ + struct e_sc_n_args *args = (struct e_sc_n_args *)arguments; + struct e_sc_n_logic *log = (struct e_sc_n_logic *)logic; + + log->orta_L1_ptr = (float *)(log_manager.nets_data + (args->io.orta_l1_float_in >> 3)); + if((u8 *)log->orta_L1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L1_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l1_float_in >> 3)); + if((u8 *)log->ortb_L1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_L2_ptr = (float *)(log_manager.nets_data + (args->io.orta_l2_float_in >> 3)); + if((u8 *)log->orta_L2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L2_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l2_float_in >> 3)); + if((u8 *)log->ortb_L2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_L3_ptr = (float *)(log_manager.nets_data + (args->io.orta_l3_float_in >> 3)); + if((u8 *)log->orta_L3_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L3_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l3_float_in >> 3)); + if((u8 *)log->ortb_L3_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_sc_n(void *arguments, void *logic) +{ +// struct e_sc_n_args *args = (struct e_sc_n_args *)arguments; + struct e_sc_n_logic *log = (struct e_sc_n_logic *)logic; + + float orta, ortb, ortaL1, ortbL1, ortaL2, ortbL2, ortaL3, ortbL3; + + ortaL1 = *log->orta_L1_ptr; + ortbL1 = *log->ortb_L1_ptr; + ortaL2 = *log->orta_L2_ptr; + ortbL2 = *log->ortb_L2_ptr; + ortaL3 = *log->orta_L3_ptr; + ortbL3 = *log->ortb_L3_ptr; + + orta = ortaL1 - (float)0.5*ortaL2 - (float)0.866*ortbL2 - (float)0.5*ortaL3 + (float)0.866*ortbL3; + orta *= 0.3333f; // wyliczenie skladowej ortogonalnej a skladowej symetrycznej przeciwnej + ortb = ortbL1 + (float)0.866*ortaL2 - (float)0.5*ortbL2 - (float)0.866*ortaL3 - (float)0.5*ortbL3; + ortb *= 0.3333f; // wyliczenie skladowej ortogonalnej b skladowej symetrycznej przeciwnej + + *log->esk_ptr=(orta*orta + ortb*ortb)*0.5f; + *log->orta_ptr = orta; + *log->ortb_ptr = ortb; +} diff --git a/src/e_sc_n.h b/src/e_sc_n.h new file mode 100644 index 0000000..3fb8714 --- /dev/null +++ b/src/e_sc_n.h @@ -0,0 +1,53 @@ +/* + * e_sc_n.h + * + * Created on: 14-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_SC_N_H_ +#define E_SC_N_H_ + +#include "tdefs.h" + +struct e_sc_n_logic +{ + float *orta_L1_ptr; + float *ortb_L1_ptr; + float *orta_L2_ptr; + float *ortb_L2_ptr; + float *orta_L3_ptr; + float *ortb_L3_ptr; + float *orta_ptr; + float *ortb_ptr; + float *esk_ptr; +}; + +struct e_sc_n_io +{ + u32 orta_l1_float_in; + u32 ortb_l1_float_in; + u32 orta_l2_float_in; + u32 ortb_l2_float_in; + u32 orta_l3_float_in; + u32 ortb_l3_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_sc_n_params +{ +}__attribute__((__packed__)); + +struct e_sc_n_args +{ + struct e_sc_n_io io; + struct e_sc_n_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_sc_n(void *args, void *logic); +extern int e_sc_n_initlog(void *arguments, void *logic); + +#endif /* E_SC_N_H_ */ diff --git a/src/e_sc_p.c b/src/e_sc_p.c new file mode 100644 index 0000000..17b449a --- /dev/null +++ b/src/e_sc_p.c @@ -0,0 +1,81 @@ +/* + * e_sc_p.c + * + * Created on: 14-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_sc_p.h" +#include "analog_in.h" +#include + +int e_sc_p_initlog(void *arguments, void *logic) +{ + struct e_sc_p_args *args = (struct e_sc_p_args *)arguments; + struct e_sc_p_logic *log = (struct e_sc_p_logic *)logic; + + log->orta_L1_ptr = (float *)(log_manager.nets_data + (args->io.orta_l1_float_in >> 3)); + if((u8 *)log->orta_L1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L1_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l1_float_in >> 3)); + if((u8 *)log->ortb_L1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_L2_ptr = (float *)(log_manager.nets_data + (args->io.orta_l2_float_in >> 3)); + if((u8 *)log->orta_L2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L2_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l2_float_in >> 3)); + if((u8 *)log->ortb_L2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_L3_ptr = (float *)(log_manager.nets_data + (args->io.orta_l3_float_in >> 3)); + if((u8 *)log->orta_L3_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L3_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l3_float_in >> 3)); + if((u8 *)log->ortb_L3_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_sc_p(void *arguments, void *logic) +{ +// struct e_sc_p_args *args = (struct e_sc_p_args *)arguments; + struct e_sc_p_logic *log = (struct e_sc_p_logic *)logic; + + float orta, ortb, ortaL1, ortbL1, ortaL2, ortbL2, ortaL3, ortbL3; + + ortaL1 = *log->orta_L1_ptr; + ortbL1 = *log->ortb_L1_ptr; + ortaL2 = *log->orta_L2_ptr; + ortbL2 = *log->ortb_L2_ptr; + ortaL3 = *log->orta_L3_ptr; + ortbL3 = *log->ortb_L3_ptr; + + orta = ortaL1 - 0.5f * ortaL2 + 0.866f * ortbL2 - 0.5f * ortaL3 - 0.866f * ortbL3; + orta *= 0.3333f; // wyliczenie skladowej ortogonalnej a skladowej symetrycznej zgodnej + ortb = ortbL1 - 0.866f * ortaL2 - 0.5f * ortbL2 + 0.866f * ortaL3 - 0.5f * ortbL3; + ortb *= 0.3333f; // wyliczenie skladowej ortogonalnej b skladowej symetrycznej zgodnej + + *log->esk_ptr=(orta*orta + ortb*ortb)*0.5f; + *log->orta_ptr = orta; + *log->ortb_ptr = ortb; +} diff --git a/src/e_sc_p.h b/src/e_sc_p.h new file mode 100644 index 0000000..7a224a7 --- /dev/null +++ b/src/e_sc_p.h @@ -0,0 +1,53 @@ +/* + * e_sc_p.h + * + * Created on: 14-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_SC_P_H_ +#define E_SC_P_H_ + +#include "tdefs.h" + +struct e_sc_p_logic +{ + float *orta_L1_ptr; + float *ortb_L1_ptr; + float *orta_L2_ptr; + float *ortb_L2_ptr; + float *orta_L3_ptr; + float *ortb_L3_ptr; + float *orta_ptr; + float *ortb_ptr; + float *esk_ptr; +}; + +struct e_sc_p_io +{ + u32 orta_l1_float_in; + u32 ortb_l1_float_in; + u32 orta_l2_float_in; + u32 ortb_l2_float_in; + u32 orta_l3_float_in; + u32 ortb_l3_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_sc_p_params +{ +}__attribute__((__packed__)); + +struct e_sc_p_args +{ + struct e_sc_p_io io; + struct e_sc_p_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_sc_p(void *args, void *logic); +extern int e_sc_p_initlog(void *arguments, void *logic); + +#endif /* E_SC_P_H_ */ diff --git a/src/e_sc_z.c b/src/e_sc_z.c new file mode 100644 index 0000000..7e3865c --- /dev/null +++ b/src/e_sc_z.c @@ -0,0 +1,79 @@ +/* + * e_sc_z.c + * + * Created on: 14-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_sc_z.h" +#include "analog_in.h" +#include + +int e_sc_z_initlog(void *arguments, void *logic) +{ + struct e_sc_z_args *args = (struct e_sc_z_args *)arguments; + struct e_sc_z_logic *log = (struct e_sc_z_logic *)logic; + + log->orta_L1_ptr = (float *)(log_manager.nets_data + (args->io.orta_l1_float_in >> 3)); + if((u8 *)log->orta_L1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L1_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l1_float_in >> 3)); + if((u8 *)log->ortb_L1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_L2_ptr = (float *)(log_manager.nets_data + (args->io.orta_l2_float_in >> 3)); + if((u8 *)log->orta_L2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L2_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l2_float_in >> 3)); + if((u8 *)log->ortb_L2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_L3_ptr = (float *)(log_manager.nets_data + (args->io.orta_l3_float_in >> 3)); + if((u8 *)log->orta_L3_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_L3_ptr = (float *)(log_manager.nets_data + (args->io.ortb_l3_float_in >> 3)); + if((u8 *)log->ortb_L3_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_sc_z(void *arguments, void *logic) +{ +// struct e_sc_z_args *args = (struct e_sc_z_args *)arguments; + struct e_sc_z_logic *log = (struct e_sc_z_logic *)logic; + + float orta, ortb, ortaL1, ortbL1, ortaL2, ortbL2, ortaL3, ortbL3; + + ortaL1 = *log->orta_L1_ptr; + ortbL1 = *log->ortb_L1_ptr; + ortaL2 = *log->orta_L2_ptr; + ortbL2 = *log->ortb_L2_ptr; + ortaL3 = *log->orta_L3_ptr; + ortbL3 = *log->ortb_L3_ptr; + + orta = (ortaL1 + ortaL2 + ortaL3) * 0.3333f; ///< wyliczenie skladowej ortogonalnej a wyniku + ortb = (ortbL1 + ortbL2 + ortbL3) * 0.3333f;///< wyliczenie skladowej ortogonalnej b wyniku + + *log->esk_ptr=(orta*orta + ortb*ortb)* 0.5f; + *log->orta_ptr = orta; + *log->ortb_ptr = ortb; +} diff --git a/src/e_sc_z.h b/src/e_sc_z.h new file mode 100644 index 0000000..c5ea4fe --- /dev/null +++ b/src/e_sc_z.h @@ -0,0 +1,53 @@ +/* + * e_sc_z.h + * + * Created on: 14-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_SC_Z_H_ +#define E_SC_Z_H_ + +#include "tdefs.h" + +struct e_sc_z_logic +{ + float *orta_L1_ptr; + float *ortb_L1_ptr; + float *orta_L2_ptr; + float *ortb_L2_ptr; + float *orta_L3_ptr; + float *ortb_L3_ptr; + float *orta_ptr; + float *ortb_ptr; + float *esk_ptr; +}; + +struct e_sc_z_io +{ + u32 orta_l1_float_in; + u32 ortb_l1_float_in; + u32 orta_l2_float_in; + u32 ortb_l2_float_in; + u32 orta_l3_float_in; + u32 ortb_l3_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_sc_z_params +{ +}__attribute__((__packed__)); + +struct e_sc_z_args +{ + struct e_sc_z_io io; + struct e_sc_z_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_sc_z(void *args, void *logic); +extern int e_sc_z_initlog(void *arguments, void *logic); + +#endif /* E_SC_Z_H_ */ diff --git a/src/e_subvec.c b/src/e_subvec.c new file mode 100644 index 0000000..3612cb0 --- /dev/null +++ b/src/e_subvec.c @@ -0,0 +1,64 @@ +/* + * e_subvec.c + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "e_subvec.h" +#include "analog_in.h" +#include + +int e_subvec_initlog(void *arguments, void *logic) +{ + struct e_subvec_args *args = (struct e_subvec_args *)arguments; + struct e_subvec_logic *log = (struct e_subvec_logic *)logic; + + log->orta1_ptr = (float *)(log_manager.nets_data + (args->io.orta1_float_in >> 3)); + if((u8 *)log->orta1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb1_ptr = (float *)(log_manager.nets_data + (args->io.ortb1_float_in >> 3)); + if((u8 *)log->ortb1_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta2_ptr = (float *)(log_manager.nets_data + (args->io.orta2_float_in >> 3)); + if((u8 *)log->orta2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb2_ptr = (float *)(log_manager.nets_data + (args->io.ortb2_float_in >> 3)); + if((u8 *)log->ortb2_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->orta_ptr = (float *)(log_manager.nets_data + (args->io.orta_float_out >> 3)); + if((u8 *)log->orta_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->ortb_ptr = (float *)(log_manager.nets_data + (args->io.ortb_float_out >> 3)); + if((u8 *)log->ortb_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_out >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + return 0; +} + +void e_subvec(void *arguments, void *logic) +{ +// struct e_subvec_args *args = (struct e_subvec_args *)arguments; + struct e_subvec_logic *log = (struct e_subvec_logic *)logic; + + float orta, ortb; + + orta = *log->orta1_ptr - *log->orta2_ptr; + ortb = *log->ortb1_ptr - *log->ortb2_ptr; + + *log->esk_ptr=(orta*orta + ortb*ortb)*0.5f; + *log->orta_ptr = orta; + *log->ortb_ptr = ortb; +} diff --git a/src/e_subvec.h b/src/e_subvec.h new file mode 100644 index 0000000..596caaf --- /dev/null +++ b/src/e_subvec.h @@ -0,0 +1,49 @@ +/* + * e_subvec.h + * + * Created on: 10-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_SUBVEC_H_ +#define E_SUBVEC_H_ + +#include "tdefs.h" + +struct e_subvec_logic +{ + float *orta1_ptr; + float *ortb1_ptr; + float *orta2_ptr; + float *ortb2_ptr; + float *orta_ptr; + float *ortb_ptr; + float *esk_ptr; +}; + +struct e_subvec_io +{ + u32 orta1_float_in; + u32 ortb1_float_in; + u32 orta2_float_in; + u32 ortb2_float_in; + u32 orta_float_out; + u32 ortb_float_out; + u32 esk_float_out; +}__attribute__((__packed__)); + +struct e_subvec_params +{ +}__attribute__((__packed__)); + +struct e_subvec_args +{ + struct e_subvec_io io; + struct e_subvec_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void e_subvec(void *args, void *logic); +extern int e_subvec_initlog(void *arguments, void *logic); + +#endif /* E_SUBVEC_H_ */ diff --git a/src/edge_det.c b/src/edge_det.c new file mode 100644 index 0000000..4b34b0d --- /dev/null +++ b/src/edge_det.c @@ -0,0 +1,57 @@ +/* + * edge_det.c + * + * Created on: 28-06-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "edge_det.h" + +int edge_det_initlog(void *arguments, void *logic) +{ + struct edge_det_args *args = (struct edge_det_args *)arguments; + struct edge_det_logic *log = (struct edge_det_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_mask = 1<<(args->io.out & 0x07); + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_mask = 1<<(args->io.in & 0x07); + + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void edge_det(void *arguments, void *logic) +{ + struct edge_det_args *args = (struct edge_det_args *)arguments; + struct edge_det_logic *log = (struct edge_det_logic *)logic; + u8 st = *log->in_ptr & log->in_bit_mask; + + if(st) + { + if(!log->prev_state && args->params.rising) + *log->out_ptr|=log->out_bit_mask; + else + *log->out_ptr&=~log->out_bit_mask; + } + else + { + if(log->prev_state && args->params.falling) + *log->out_ptr|=log->out_bit_mask; + else + *log->out_ptr&=~log->out_bit_mask; + } + + log->prev_state=st; +} diff --git a/src/edge_det.h b/src/edge_det.h new file mode 100644 index 0000000..52985de --- /dev/null +++ b/src/edge_det.h @@ -0,0 +1,47 @@ +/* + * edge_det.h + * + * + * Created on: 28-06-2014 + * Author: KJ + */ + +#ifndef EDGE_DET_H_ +#define EDGE_DET_H_ + +#include "tdefs.h" + +struct edge_det_logic +{ + u8 *out_ptr; + u8 out_bit_mask; + + u8 *in_ptr; + u8 in_bit_mask; + + u8 prev_state; +}; + +struct edge_det_io +{ + u32 in; + u32 out; +}__attribute__((__packed__)); + +struct edge_det_params +{ + u32 rising; + u32 falling; +}__attribute__((__packed__)); + +struct edge_det_args +{ + struct edge_det_io io; + struct edge_det_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void edge_det(void *args, void *logic); +extern int edge_det_initlog(void *args, void *logic); + +#endif /* EDGE_H_ */ diff --git a/src/elements.c b/src/elements.c new file mode 100644 index 0000000..80d66e4 --- /dev/null +++ b/src/elements.c @@ -0,0 +1,175 @@ +/* + * elements.c + * + * Created on: 08-04-2014 + * Author: Krzysztof Jakubczyk + */ + + +#include "elements.h" + +/* const */struct element_info log_elements[] = + { + /* id, init_ptr, fun_ptr, fun_100hz_ptr, fun_20hz_ptr, args size, log size */ + { 0, &klapacz_initlog, &klapacz, NULL, NULL, sizeof(struct klapacz_args), sizeof(struct klapacz_logic) }, + { 1, &bin_in_initlog, &bin_in, NULL, NULL,sizeof(struct bin_in_args), sizeof(struct bin_in_logic) }, + { 2, &relay_out_initlog,/*&relay_out*/NULL, NULL, NULL,sizeof(struct relay_out_args), sizeof(struct relay_out_logic) }, + { 3, &nand_initlog,&nand, NULL, NULL,sizeof(struct nand_args), sizeof(struct nand_logic) }, + { 4, &and_initlog,&and, NULL, NULL, sizeof(struct and_args), sizeof(struct and_logic) }, + { 5, &nor_initlog,&nor, NULL, NULL, sizeof(struct nor_args), sizeof(struct nor_logic) }, + { 6, &or_initlog,&or, NULL, NULL,sizeof(struct or_args), sizeof(struct or_logic) }, + { 7, &xor_initlog,&xor, NULL, NULL, sizeof(struct xor_args), sizeof(struct xor_logic) }, + { 8, ¬_initlog,¬, NULL, NULL, sizeof(struct not_args), sizeof(struct not_logic) }, + { 9, &tz_initlog,&tz, NULL, NULL, sizeof(struct tz_args), sizeof(struct tz_logic) }, + { 10, &tp_initlog,&tp, NULL, NULL, sizeof(struct tp_args), sizeof(struct tp_logic) }, + { 11, &analog_in_initlog,&analog_in, NULL, NULL, sizeof(struct analog_in_args), sizeof(struct analog_in_logic) }, + { 12, &valf_gt_initlog,&valf_gt, NULL, NULL, sizeof(struct valf_gt_args), sizeof(struct valf_gt_logic) }, + { 13, &e_filtr1h_initlog,&e_filtr1h, NULL, NULL, sizeof(struct e_filtr1h_args), sizeof(struct e_filtr1h_logic) }, + { 14, &ov_1ph_initlog,&ov_1ph, &ov_1ph_100hz, NULL, sizeof(struct ov_1ph_args), sizeof(struct ov_1ph_logic) }, // I1ph> + { 15, &ov_1ph_initlog,&ov_1ph, &ov_1ph_100hz, NULL, sizeof(struct ov_1ph_args), sizeof(struct ov_1ph_logic) }, // U1ph> + { 16, &sr_initlog,&sr, NULL, NULL, sizeof(struct sr_args), sizeof(struct sr_logic) }, + { 17, &under_1ph_initlog,&under_1ph, under_1ph_100hz, NULL, sizeof(struct under_1ph_args), sizeof(struct under_1ph_logic) }, // I1ph< + { 18, &under_1ph_initlog,&under_1ph, under_1ph_100hz, NULL, sizeof(struct under_1ph_args), sizeof(struct under_1ph_logic) }, // U1ph< + { 19, &event_initlog,NULL, NULL, NULL, sizeof(struct event_args), sizeof(struct event_logic) }, + { 20, &virt_in_initlog,NULL, NULL, NULL, sizeof(struct virt_in_args),sizeof(struct virt_in_logic) }, + { 21, &rec_bin_initlog,NULL, NULL, NULL, sizeof(struct rec_bin_args), sizeof(struct rec_bin_logic) }, + { 22, &rec_an_initlog,NULL, NULL, NULL, sizeof(struct rec_an_args), sizeof(struct rec_an_logic) }, + { 23, &dfr_initlog,&dfr, NULL, NULL, sizeof(struct dfr_args), sizeof(struct dfr_logic) }, + { 24, &edge_det_initlog,&edge_det, NULL, NULL,sizeof(struct edge_det_args), sizeof(struct edge_det_logic) }, + { 25, &measurand_initlog,NULL, &measurand_100hz, NULL,sizeof(struct measurand_args), sizeof(struct measurand_logic) }, + { 26, &led_initlog,NULL, NULL, NULL, sizeof(struct led_args), sizeof(struct led_logic) }, + { 27, &buzzer_initlog,NULL, NULL, NULL, sizeof(struct led_args), sizeof(struct led_logic) }, + { 28, &e_filtr2h_initlog,&e_filtr2h, NULL, NULL, sizeof(struct e_filtr2h_args), sizeof(struct e_filtr2h_logic) }, + { 29, &e_filtr3h_initlog,&e_filtr3h, NULL, NULL, sizeof(struct e_filtr3h_args), sizeof(struct e_filtr3h_logic) }, + { 30, &e_filtr5h_initlog,&e_filtr5h, NULL, NULL, sizeof(struct e_filtr5h_args), sizeof(struct e_filtr5h_logic) }, + { 31, &e_avg_initlog,&e_avg, NULL, NULL, sizeof(struct e_avg_args), sizeof(struct e_avg_logic) }, + { 32, &e_avg_1t_initlog,&e_avg_1t, NULL, NULL, sizeof(struct e_avg_1t_args), sizeof(struct e_avg_1t_logic) }, + { 33, &e_avg_dt_initlog,&e_avg_dt,&e_avg_dt_100hz, NULL, sizeof(struct e_avg_dt_args), sizeof(struct e_avg_dt_logic) }, + { 34, &pow_initlog,&pow_1,NULL, NULL, sizeof(struct pow_args), sizeof(struct pow_logic) }, + { 35, &e_rms_initlog,&e_rms, NULL, NULL, sizeof(struct e_rms_args), sizeof(struct e_rms_logic) }, + { 36, &e_rms_1t_initlog,&e_rms_1t, NULL, NULL, sizeof(struct e_rms_1t_args), sizeof(struct e_rms_1t_logic) }, + { 37, &e_addvec_initlog,&e_addvec, NULL, NULL, sizeof(struct e_addvec_args), sizeof(struct e_addvec_logic) }, + { 38, &e_subvec_initlog,&e_subvec, NULL, NULL, sizeof(struct e_subvec_args), sizeof(struct e_subvec_logic) }, + { 39, &e_mulvec_initlog,&e_mulvec, NULL, NULL, sizeof(struct e_mulvec_args), sizeof(struct e_mulvec_logic) }, + { 40, &e_mulcon_initlog,&e_mulcon, NULL, NULL, sizeof(struct e_mulcon_args), sizeof(struct e_mulcon_logic) }, + { 41, &e_sc_p_initlog,&e_sc_p, NULL, NULL, sizeof(struct e_sc_p_args), sizeof(struct e_sc_p_logic) }, + { 42, &e_sc_n_initlog,&e_sc_n, NULL, NULL, sizeof(struct e_sc_n_args), sizeof(struct e_sc_n_logic) }, + { 43, &e_sc_z_initlog,&e_sc_z, NULL, NULL, sizeof(struct e_sc_z_args), sizeof(struct e_sc_z_logic) }, + { 44, &e_mem1_initlog,&e_mem1, &e_mem1_100hz, NULL, sizeof(struct e_mem1_args), sizeof(struct e_mem1_logic) }, + { 45, &e_cu_initlog,NULL, NULL, &e_cu_20hz, sizeof(struct e_cu_args), sizeof(struct e_cu_logic) }, + { 46, &e_f_initlog,&e_f, NULL/*&e_f_100hz*/, NULL, sizeof(struct e_f_args), sizeof(struct e_f_logic) }, + { 47, &e_du_initlog,NULL, NULL, &e_du_20hz, sizeof(struct e_du_args), sizeof(struct e_du_logic) }, + { 48, &e_df_3f_initlog,NULL, &e_df_3f_100hz, NULL, sizeof(struct e_df_3f_args), sizeof(struct e_df_3f_logic) }, + { 49, &e_df_dt_initlog,NULL, &e_df_dt_100hz, NULL, sizeof(struct e_df_dt_args), sizeof(struct e_df_dt_logic) }, + { 50, &e_df_dt_3f_initlog,NULL, &e_df_dt_3f_100hz, NULL, sizeof(struct e_df_dt_3f_args), sizeof(struct e_df_dt_3f_logic) }, + { 51, &e_du_dt_initlog,NULL, &e_du_dt_100hz, NULL, sizeof(struct e_du_dt_args), sizeof(struct e_du_dt_logic) }, + { 52, &e_filtr1h_2t_initlog,&e_filtr1h_2t, NULL, NULL, sizeof(struct e_filtr1h_2t_args), sizeof(struct e_filtr1h_2t_logic) }, + { 53, &e_pq_1f_initlog,NULL, &e_pq_1f_100hz, NULL, sizeof(struct e_pq_1f_args), sizeof(struct e_pq_1f_logic) }, + { 54, &e_pq_3f_initlog,NULL, &e_pq_3f_100hz, NULL, sizeof(struct e_pq_3f_args), sizeof(struct e_pq_3f_logic) }, + { 55, &e_pq_a_initlog,NULL, &e_pq_a_100hz, NULL, sizeof(struct e_pq_a_args), sizeof(struct e_pq_a_logic) }, + { 56, &r_67n_lwn_initlog, &r_67n_lwn, &r_67n_lwn_100hz, &r_67n_lwn_20hz, sizeof(struct r_67n_lwn_args), sizeof(struct r_67n_lwn_logic) }, + { 57, &u_fail_initlog, &u_fail, &u_fail_100hz, NULL, sizeof(struct u_fail_args), sizeof(struct u_fail_logic) }, + { 58, &i_fail_initlog, &i_fail, &i_fail_100hz, NULL, sizeof(struct i_fail_args), sizeof(struct i_fail_logic) }, + { 59, &ps_initlog, &ps, &ps_100hz, NULL, sizeof(struct ps_args), sizeof(struct ps_logic) }, + { 60, &l_comm_initlog, &l_comm, &l_comm_100hz, NULL, sizeof(struct l_comm_args), sizeof(struct l_comm_logic) }, + { 61, &ov_3ph_initlog, &ov_3ph, &ov_3ph_100hz, NULL, sizeof(struct ov_3ph_args), sizeof(struct ov_3ph_logic) }, // >I 3f + { 62, &ov_3ph_initlog, &ov_3ph, &ov_3ph_100hz, NULL, sizeof(struct ov_3ph_args), sizeof(struct ov_3ph_logic) }, // >U 3f + { 63, &under_3ph_initlog, &under_3ph, &under_3ph_100hz, NULL, sizeof(struct under_3ph_args), sizeof(struct under_3ph_logic) }, // 2h + { 93, &ov_3ph2h_initlog, &ov_3ph2h, &ov_3ph2h_100hz, NULL, sizeof(struct ov_3ph2h_args), sizeof(struct ov_3ph2h_logic) }, // >I 3f2h + { 94, &zzr_initlog, &zzr, NULL, NULL, sizeof(struct zzr_args), sizeof(struct zzr_logic) }, + { 95, &kier_pr_initlog, &kier_pr, NULL, NULL, sizeof(struct kier_pr_args), sizeof(struct kier_pr_logic) }, + { 96, &chka_initlog, &chka,NULL, NULL, sizeof(struct chka_args), sizeof(struct chka_logic) }, + { 97, &chka_initlog, &chka,NULL, NULL, sizeof(struct chka_args), sizeof(struct chka_logic) }, + { 98, &mux_bin_initlog, &mux_bin,NULL, NULL, sizeof(struct mux_bin_args), sizeof(struct mux_bin_logic) }, + { 99, &hr_3ph2h_initlog, &hr_3ph2h,NULL, NULL, sizeof(struct hr_3ph2h_args), sizeof(struct hr_3ph2h_logic) }, + { 100, &r21g_initlog, &r21g,NULL, NULL, sizeof(struct r21g_args), sizeof(struct r21g_logic) }, + { 101, &counter_initlog, &counter,NULL, NULL, sizeof(struct counter_args), sizeof(struct counter_logic) }, + { 102, &const_flt_initlog, NULL,NULL, NULL, sizeof(struct const_flt_args), sizeof(struct const_flt_logic) }, + { 103, &chka_ieee_initlog, &chka_ieee,NULL, NULL, sizeof(struct chka_ieee_args), sizeof(struct chka_ieee_logic) }, + { 104, &chka_ieee_initlog, &chka_ieee,NULL, NULL, sizeof(struct chka_ieee_args), sizeof(struct chka_ieee_logic) }, + { 105, &r_67n_lwn2_initlog, &r_67n_lwn2, &r_67n_lwn2_100hz, NULL /*&r_67n_lwn2_20hz*/, sizeof(struct r_67n_lwn2_args), sizeof(struct r_67n_lwn2_logic) }, + { 106, ðernet_initlog, ðernet, NULL, NULL, sizeof(struct eth_args), sizeof(struct eth_logic) }, + { 107, &mkstream_initlog, &mkstream, NULL, NULL, sizeof(struct mkstream_args), sizeof(struct mkstream_logic) }, + { 108, &destream_initlog, &destream, NULL, NULL, sizeof(struct destream_args), sizeof(struct destream_logic) }, + { 109, &R87L_initlog, &R87L, NULL, NULL, sizeof(struct r87l_args), sizeof(struct r87l_logic) }, + { 110, &and4_initlog, &and4, NULL, NULL, sizeof(struct and4_args), sizeof(struct and4_logic) }, + { 111, &or4_initlog, &or4, NULL, NULL, sizeof(struct or4_args), sizeof(struct or4_logic) }, + { 112, &cnt_initlog, &cnt, NULL, NULL, sizeof(struct cnt_args), sizeof(struct cnt_logic) }, + { 113, &c37_94_initlog, &c37_94, NULL, NULL, sizeof(struct c37_94_args), sizeof(struct c37_94_logic) }, + { 114, &sum_flt_3f_initlog, &sum_flt_3f, NULL, NULL, sizeof(struct sum_flt_3f_args), sizeof(struct sum_flt_3f_logic) }, + { 115, &sum_e_3f_initlog, &sum_e_3f, NULL, NULL, sizeof(struct sum_e_3f_args), sizeof(struct sum_e_3f_logic) }, + { 116, &cap_comp_initlog, &cap_comp, NULL, NULL, sizeof(struct cap_comp_args), sizeof(struct cap_comp_logic) }, + { 117, &s_demux_initlog, &s_demux, NULL, NULL, sizeof(struct s_demux_args), sizeof(struct s_demux_logic) }, + { 118, &ov_3ph2h_slow_initlog, &ov_3ph2h_slow, &ov_3ph2h_slow_100hz, NULL, sizeof(struct ov_3ph2h_slow_args), sizeof(struct ov_3ph2h_slow_logic) }, // >I 3f2h + { 119, &tz_8_initlog,&tz_8, NULL, NULL, sizeof(struct tz_8_args), sizeof(struct tz_8_logic) }, + { 120, &mki7_initlog, NULL, NULL, &mki7_20hz, sizeof(struct mki7_args), sizeof(struct mki7_logic) }, + { 121, &goose_initlog, NULL, NULL, NULL, sizeof(struct goose_args), sizeof(struct goose_logic) }, + { 122, &rec_buf_initlog,NULL, NULL, NULL, sizeof(struct rec_buf_args), sizeof(struct rec_buf_logic) }, + { 123, &sv_initlog, &sv, NULL, NULL, sizeof(struct sv_args), sizeof(struct sv_logic) }, + { 124, &rnt_initlog, &rnt, NULL, NULL, sizeof(struct rnt_args), sizeof(struct rnt_logic) }, + { 125, &goose_drv_initlog, &goose_drv, NULL, NULL, sizeof(struct goose_drv_args), sizeof(struct goose_drv_logic) }, + { 126, &goose_out_initlog, &goose_out, NULL, NULL, sizeof(struct goose_out_args), sizeof(struct goose_out_logic) }, + { 127, &nr_zacz_initlog, &nr_zacz, NULL, NULL, sizeof(struct nr_zacz_args), sizeof(struct nr_zacz_logic) }, + { 128, &prog_czas_initlog, &prog_czas, NULL, NULL, sizeof(struct prog_czas_args), sizeof(struct prog_czas_logic) }, + { 129, &r_freq_initlog, NULL, r_freq_100hz, NULL, sizeof(struct r_freq_args), sizeof(struct r_freq_logic) }, + { 130, &dec_month_initlog, &dec_month, NULL, NULL, sizeof(struct dec_month_args), sizeof(struct dec_month_logic) }, + { 131, &mux_t_initlog, &mux_t, NULL, NULL, sizeof(struct mux_t_args), sizeof(struct mux_t_logic) }, + { 132, &lacz_initlog, &lacz, NULL, NULL, sizeof(struct lacz_args), sizeof(struct lacz_logic) }, + { 133, &LRW_initlog, &LRW, NULL, NULL, sizeof(struct LRW_args), sizeof(struct LRW_logic) }, + { 134, &comp_s_initlog, &comp_s, NULL, NULL, sizeof(struct comp_s_args), sizeof(struct comp_s_logic) }, + { 135, &ps2_initlog, &ps2, &ps2_100hz, NULL, sizeof(struct ps2_args), sizeof(struct ps2_logic) }, + { 136, &ZDistA_komp_initlog, &ZDistA_komp, NULL, NULL, sizeof(struct ZDistA_komp_args), sizeof(struct ZDistA_komp_logic) }, + { 137, &ZDistL_initlog_komp, &ZDistL_komp, &ZDistL_100hz_komp, NULL, sizeof(struct ZDistL_args_komp), sizeof(struct ZDistL_logic_komp) }, + { 138, &spz_3f_2_initlog, &spz_3f_2, NULL, NULL, sizeof(struct spz_3f_2_args), sizeof(struct spz_3f_2_logic) }, + { 139, &s_demux2_initlog, &s_demux2, NULL, NULL, sizeof(struct s_demux2_args), sizeof(struct s_demux2_logic) }, + { 140, ðernet_ench_initlog, ðernet_ench, NULL, NULL, sizeof(struct eth_ench_args), sizeof(struct eth_ench_logic) }, + { 141, ð_demux_initlog, ð_demux, NULL, NULL, sizeof(struct eth_demux_args), sizeof(struct eth_demux_logic) }, + { 142, &R87L_8_initlog, &R87L_8, NULL, NULL, sizeof(struct r87l_8_args), sizeof(struct r87l_8_logic) }, + { 143, &mki7_2_initlog, NULL, NULL, &mki7_2_20hz, sizeof(struct mki7_2_args), sizeof(struct mki7_2_logic) }, + // iec sv + { 144, &mki7_2_initlog, NULL, NULL, &mki7_2_20hz, sizeof(struct mki7_2_args), sizeof(struct mki7_2_logic) }, + + //iec sv drv here + { 145, &diff_1_bis_initlog, &diff_1_bis, NULL, NULL, sizeof(struct diff_1_bis_args), sizeof(struct diff_1_bis_logic) }, + { 146, &od_bl_initlog, &od_bl, NULL, NULL, sizeof(struct od_bl_args), sizeof(struct od_bl_logic) }, + { 147, &log_wyl_initlog, &log_wyl, NULL, NULL, sizeof(struct log_wyl_args), sizeof(struct log_wyl_logic) }, + + { 65528, &out_drv_initlog,&out_drv, NULL, NULL, sizeof(struct out_drv_args), sizeof(struct out_drv_logic) }, + { 65529, &an_gen_initlog,&an_gen, NULL, NULL, sizeof(struct an_gen_args), sizeof(struct an_gen_logic) }, + { 65530, &ddr_drv_initlog,NULL, NULL, NULL, sizeof(struct ddr_drv_args), sizeof(struct ddr_drv_logic) }, + { 65531, &dfr_drv_initlog,NULL, NULL, NULL, sizeof(struct dfr_drv_args), sizeof(struct dfr_drv_logic) }, + { 65532, &virt_in_drv_initlog,NULL, &virt_in_drv_100hz, NULL, sizeof(struct virt_in_drv_args), sizeof(struct virt_in_drv_logic) }, + { 65533, &events_reg_initlog,&events_reg, NULL, NULL, sizeof(struct events_reg_args), sizeof(struct events_reg_logic) }, + { 65534, &leds_drv_initlog,&leds_drv, NULL, NULL, sizeof(struct leds_drv_args), sizeof(struct leds_drv_logic) }, + { EOF_ELEMENT, 0, 0, 0 } + }; + diff --git a/src/elements.h b/src/elements.h new file mode 100644 index 0000000..1ffee6c --- /dev/null +++ b/src/elements.h @@ -0,0 +1,201 @@ +/* + * elements.h + * + * Created on: 08-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef ELEMENTS_H_ +#define ELEMENTS_H_ + +#include "klapacz.h" +#include "relay_out.h" +#include "bin_in.h" +#include "nand.h" +#include "and.h" +#include "nor.h" +#include "or.h" +#include "xor.h" +#include "not.h" +#include "tz.h" +#include "tp.h" +#include "analog_in.h" +#include "valf_gt.h" +#include "e_filtr1h.h" +#include "ov_1ph.h" +#include "sr.h" +#include "under_1ph.h" +#include "event.h" +#include "events_reg.h" +#include "rec_bin.h" +#include "rec_an.h" +#include "dfr.h" +#include "edge_det.h" +#include "measurand.h" +#include "led.h" +#include "leds_drv.h" +#include "virt_in.h" +#include "virt_in_drv.h" +#include "dfr_drv.h" +#include "e_filtr2h.h" +#include "e_filtr3h.h" +#include "e_filtr5h.h" +#include "e_avg.h" +#include "e_avg_1t.h" +#include "e_avg_dt.h" +#include "pow.h" +#include "e_rms.h" +#include "e_rms_1t.h" +#include "e_addvec.h" +#include "e_subvec.h" +#include "e_mulvec.h" +#include "e_mulcon.h" +#include "e_sc_p.h" +#include "e_sc_n.h" +#include "e_sc_z.h" +#include "e_mem1.h" +#include "e_cu.h" +#include "e_f.h" +#include "e_du.h" +#include "e_df_3f.h" +#include "e_df_dt.h" +#include "e_df_dt_3f.h" +#include "e_du_dt.h" +#include "e_filtr1h_2T.h" +#include "e_pq_1f.h" +#include "e_pq_3f.h" +#include "e_pq_A.h" +#include "R_67N_LWN.h" +#include "u_fail.h" +#include "i_fail.h" +#include "ps.h" +#include "l_comm.h" +#include "ov_3ph.h" +#include "under_3ph.h" +#include "bank_sel.h" +#include "rec_float.h" +#include "sck.h" +#include "e_f_3f.h" +#include "pkw.h" +#include "log_state.h" +#include "spz_3f.h" +#include "dev_ctrl.h" +#include "e_phi.h" +#include "mul_float.h" +#include "and8.h" +#include "or8.h" +#include "CBR.h" +#include "sqrt.h" +#include "cfg_state.h" +#include "mux_float.h" +#include "e_filtr2h_1khz.h" +#include "sum_float.h" +#include "ZDistA.h" +#include "ZDistL.h" +#include "ZDistS.h" +#include "e_mem.h" +#include "Diff.h" +#include "Diff_1.h" +#include "Diff_2.h" +#include "demux8.h" +#include "LMZ.h" +#include "ddr.h" +#include "ddr_drv.h" +#include "ov_1ph2h.h" +#include "ov_3ph2h.h" +#include "zzr.h" +#include "an_gen.h" +#include "kier_pr.h" +#include "chka.h" +#include "mux_bin.h" +#include "hr_3ph2h.h" +#include "21g.h" +#include "counter.h" +#include "const_flt.h" +#include "chka_ieee.h" +#include "R_67N_LWN2.h" +#include "eth.h" +#include "mkstream.h" +#include "destream.h" +#include "R87L.h" +#include "and4.h" +#include "or4.h" +#include "cnt.h" +#include "c37_94.h" +#include "sum_flt_3f.h" +#include "sum_e_3f.h" +#include "cap_comp.h" +#include "s_demux.h" +#include "out_drv.h" +#include "ov_3ph2h_slow.h" +#include "tz_8.h" +#include "mki7.h" +#include "goose.h" +#include "rec_buf.h" +#include "sv.h" +#include "goose_drv.h" +#include "goose_out.h" +#include "RNT.h" +#include "nr_zacz.h" +#include "prog_czas.h" +#include "r_freq.h" +#include "dec_month.h" +#include "mux_t.h" +#include "lacz.h" +#include "LRW.h" +#include "comp_s.h" +#include "ps2.h" +#include "ZDistA_komp.h" +#include "ZDistL_komp.h" +#include "spz_3f_2.h" +#include "s_demux2.h" +#include "eth_ench.h" +#include "eth_demux.h" +#include "R87L_8.h" +#include "mki7_2.h" +#include "Diff_1_bis.h" +#include "od_bl.h" +#include "log_wyl.h" + +#define EVENT_FUN_ID 19 + +#define REC_BIN_FUN_ID 21 +#define REC_AN_FUN_ID 22 +#define REC_FLOAT_FUN_ID 66 +#define REC_BUF_FUN_ID 122 +#define DFR_FUN_ID 23 +#define DDR_FUN_ID 91 +#define LED_FUN_ID 26 +#define BUZZER_FUN_ID 27 +#define VIRT_IN_FUN_ID 20 +#define REL_OUT_FUN_ID 2 +#define GOOSE_FUN_ID 121 +#define GOOSE_OUT_FUN_ID 126 +#define SV_FUN_ID 123 +#define GOOSE_DRV_FUN_ID 125 + +#define LEDS_DRV_FUN_ID 65534 +#define EVENTS_REG_FUN_ID 65533 +#define VIRT_IN_DRV_FUN_ID 65532 +#define DFR_DRV_FUN_ID 65531 +#define DDR_DRV_FUN_ID 65530 +#define AN_GEN_FUN_ID 65529 +#define OUT_DRV_FUN_ID 65528 + +#define EOF_ELEMENT 0xFFFF +#define NULL 0 + +struct element_info +{ + u16 id; + int (*initlog_ptr)(void*,void*); + void (*fun_main_ptr)(void*,void*); + void (*fun_100hz_ptr)(void*,void*); + void (*fun_20hz_ptr)(void*,void*); + u32 args_size; + u32 log_size; +}; + +extern /*const*/ struct element_info log_elements[]; + +#endif /* ELEMENTS_H_ */ diff --git a/src/eth.c b/src/eth.c new file mode 100644 index 0000000..11b2750 --- /dev/null +++ b/src/eth.c @@ -0,0 +1,354 @@ +/* + * eth.c + * + * Created on: 22-03-2019 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "eth.h" +#include "mkstream.h" + +#include + +#include "comm.h" +#include "../ethernet/ports/am1808/include/lwiplib.h" +#include "../ethernet/emac.h" +#include "../ethernet/hw_types.h" +#include "ports/am1808/include/netif/sitaraif.h" +#include +//#include + +struct udp_pcb *listen_pcb_udp; +struct udp_pcb *bcast_pcb_udp; +struct udp_pcb *stream_pcb_udp; +struct pbuf *udp_p_bcast = NULL; +struct pbuf *udp_p_stream = NULL; +struct pbuf *udp_ping = NULL; +struct ip_addr unicast_ip; + +//#pragma LOCATION(str,0xcd000000) // @ non-cached memory above 128MB +/*volatile*/ struct stream_frame str[ETH_MAX_RECV_STREAMS][ETH_STREAM_BUF_SIZE] __attribute__((aligned(4))); +volatile u8 str_bits[ETH_MAX_RECV_STREAMS][ETH_STREAM_BUF_SIZE] __attribute__((aligned(4))); +volatile u32 str_pos[ETH_MAX_RECV_STREAMS] __attribute__((aligned(4))); + +volatile u8 ping_seq_sent=0; +volatile u32 ping_time=0; +volatile u8 ping_adr=0; + +#define ETH_NO_TZ_CHECKSUM // don't check the checksum inside a TZ frame + +struct str_eth_frame r87l_tx_packet __attribute__((aligned(4))); + +const u8 zprae_multicast_pool[] = {0x01,0x00,0x5e,'Z','P','6'}; + +void ethernet_recv(char *buf) +{ + struct stream_frame *s = (struct stream_frame *)buf; + struct ping_info *ping = (struct ping_info *)(buf+(sizeof(struct stream_frame))); + + if(s->addraddr][str_pos[s->addr]],(u8*)s,sizeof(struct stream_frame)); + str_bits[s->addr][str_pos[s->addr]++]=0; + str_pos[s->addr]%=ETH_STREAM_BUF_SIZE; + + if((ping->adr & 0x7F) < ETH_MAX_STREAMS) // Additional PING NFO + { + if(ping->adr & 0x80) // ping reply + { + if(ping_seq_sent==ping->seq) + { + u32 pwm = PWM1_GET(PWM1_TBCNT); + + if(pwm>=ping->pwmval) + ping_time = pwm-ping->pwmval; + else + ping_time = pwm+65536-ping->pwmval; + } + } + else // ping request + { + ping_nfo.adr=ping_adr | 0x80; + ping_nfo.seq=ping->seq; + ping_nfo.pwmval=ping->pwmval; + ping_nfo.tstamp=bus_an_cur_timestamp; + } + } + } +} + +int ethernet_initlog(void *arguments, void *logic) +{ + struct eth_args *args = (struct eth_args *)arguments; + struct eth_logic *log = (struct eth_logic *)logic; + u8 i; + + for(i=0;i<6;i++) + r87l_tx_packet.src_mac[i]=eth.hwaddr[i]; + + for(i=0;i<6;i++) + r87l_tx_packet.dst_mac[i]=zprae_multicast_pool[i]; + + r87l_tx_packet.dst_mac[5]=args->params.multicast_id; + + emac_hash_add(EMAC_0_BASE,r87l_tx_packet.dst_mac); + + r87l_tx_packet.lenh=0; + r87l_tx_packet.lenl=sizeof(struct stream_frame)+sizeof(struct ping_info)+4; //4 bytes cksum + r87l_tx_packet.padding1=0; + r87l_tx_packet.str.hdr[0]='T'; + r87l_tx_packet.str.hdr[1]='Z'; + + if(set_bit_ptr_struct(args->io.en_in,&log->en)) + return -1; + + if(set_pointer_in_ptr(args->io.stream_ptr_in,(u32 *)&log->stream_in)) + return -1; + + if(set_bit_ptr_struct(args->io.stream_en_in,&log->stream_en)) + return -1; + + if(set_bit_ptr_struct(args->io.clr_in,&log->clr)) + return -1; + + if(set_bit_ptr_struct(args->io.ping_in,&log->ping)) + return -1; + + if(set_bit_ptr_struct(args->io.alive_out,&log->alive)) + return -1; + + if(set_pointer_out_ptr(args->io.stream0_ptr_out,(u32 *)&log->stream_out[0])) + return -1; + if(set_bit_ptr_struct(args->io.stream0_act_out,&log->stream_act[0])) + return -1; + if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32 *)&log->stream_out[1])) + return -1; + if(set_bit_ptr_struct(args->io.stream1_act_out,&log->stream_act[1])) + return -1; + if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32 *)&log->stream_out[2])) + return -1; + if(set_bit_ptr_struct(args->io.stream2_act_out,&log->stream_act[2])) + return -1; + + if(set_bit_ptr_struct(args->io.link0_out,&log->link[0])) + return -1; + if(set_float_ptr(args->io.stream0_errs_float_out,&log->stream_errs[0])) + return -1; + + if(set_bit_ptr_struct(args->io.link1_out,&log->link[1])) + return -1; + if(set_float_ptr(args->io.stream1_errs_float_out,&log->stream_errs[1])) + return -1; + + if(set_bit_ptr_struct(args->io.link2_out,&log->link[2])) + return -1; + if(set_float_ptr(args->io.stream2_errs_float_out,&log->stream_errs[2])) + return -1; + + if(set_float_ptr(args->io.ping_float_out,&log->ping_time)) + return -1; + + if(set_float_ptr(args->io.stream0_synt_errs_float_out,&log->stream_synt_errs[0])) + return -1; + if(set_float_ptr(args->io.stream1_synt_errs_float_out,&log->stream_synt_errs[1])) + return -1; + if(set_float_ptr(args->io.stream2_synt_errs_float_out,&log->stream_synt_errs[2])) + return -1; + + log->s_delay=args->params.sample_delay/LOOP_CYCLE_MS; + if(args->params.sample_delay%LOOP_CYCLE_MS) + log->s_delay++; + + log->s_delay*=(SAMPLES_PER_MS*LOOP_CYCLE_MS); + + if(log->s_delay>=((ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS))) + log->s_delay=(ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS); + + ping_nfo.adr=0xFF; + + return 0; +} + +void ethernet(void *arguments, void *logic) +{ + struct eth_logic *log = (struct eth_logic *)logic; + struct eth_args *args = (struct eth_args *)arguments; + + u8 i,k; +#ifndef ETH_NO_TZ_CHECKSUM + u8 j,cksum; +#endif + u16 find_stamp; + u8 already_incremented[ETH_MAX_STREAMS]={0,0,0}; + //struct str_eth_frame *eth_str = (struct str_eth_frame *)r87l_tx_packet; + + if(check_struct(&log->clr)) + { + for(i=0;istream_errs[i]=0; + *log->stream_synt_errs[i]=0; + } + } + + for(i=0;iprev_pos[i]!=str_pos[i]) + { + if(log->link_up_cnt[i]>LINK_UP_TIME) + { + if(!check_struct(&log->link[i])) + *log->stream_errs[i]=0; + set_struct(&log->link[i]); + } + else + log->link_up_cnt[i]+=LOOP_CYCLE_MS; + + while(log->prev_pos[i]!=str_pos[i]) + { + u16 prev_tstamp=(!log->prev_pos[i]?str[i][ETH_STREAM_BUF_SIZE-1].sample_stamp:str[i][log->prev_pos[i]-1].sample_stamp) & 0x7FFF; + u16 next_tstamp=(str[i][log->prev_pos[i]].sample_stamp)&0x7FFF; + + if((prev_tstamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=next_tstamp) + { + if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp) + *log->stream_errs[i]+=1.0f; + already_incremented[i]=1; + } + log->prev_pos[i]++; + log->prev_pos[i]%=ETH_STREAM_BUF_SIZE; + } + + log->link_timeout_cnt[i]=0; + log->prev_pos[i]=str_pos[i]; + } + else + { + if(log->link_timeout_cnt[i]>LINK_TIMEOUT_TIME) + { + clear_struct(&log->link[i]); + log->link_up_cnt[i]=0; + } + else + { + if(log->link_timeout_cnt[i]>(1*LOOP_CYCLE_MS)) // at least 2ms between samples + { + if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp) + *log->stream_errs[i]+=1.0f; + already_incremented[i]=1; + } + log->link_timeout_cnt[i]+=LOOP_CYCLE_MS; + } + } + } + + if(bus_an_cur_timestamp>=log->s_delay/*ETH_BUF_DELAY*/) + find_stamp=bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY; + else + find_stamp=(SAMPLES_PER_MS*1000)+bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY; + + for(i=0;ilink[i])) + { + clear_struct(&log->stream_act[i]); + continue; + } + for(k=0;kparams.bits & ETH_NO_SYNC || str[i][k].sample_stamp == find_stamp) && !(str_bits[i][k] & STATUS_FRAME_READ)) + { +#ifndef ETH_NO_TZ_CHECKSUM + cksum=CKSUM_IV; + for(j=0;jstream_out[i],(u8*)&str[i][k],sizeof(struct stream_frame)); + break; + } + } + } + + if(kstream_act[i]); + else + { + clear_struct(&log->stream_act[i]); + if(!already_incremented[i]) // wyrzucono bledy od synT z licznikow + *log->stream_synt_errs[i]+=1.0f; + } + } + + if(eth.flags & ETH_INITIALIZED) // eth active + { + ping_adr=log->stream_in->addr; + + if(check_struct(&log->ping)) + { + if(log->tcnt>=5000) + { + ping_nfo.adr=ping_adr; + ping_nfo.seq++; + ping_seq_sent=ping_nfo.seq; + ping_time=0xFFFFFFFF; + ping_nfo.pwmval=PWM1_GET(PWM1_TBCNT); + ping_nfo.tstamp=bus_an_cur_timestamp; + } + } + + if(check_struct(&log->stream_en)) + { + memcpy((u8*)&r87l_tx_packet.str,(u8 *)log->stream_in,sizeof(struct stream_frame)); + + if(ping_nfo.adr != 0xFF) // need send ping request or reply + memcpy((u8*)&r87l_tx_packet.ping,(u8*)&ping_nfo,sizeof(struct ping_info)); + else + r87l_tx_packet.ping.adr=0xFF; + + ping_nfo.adr=0xFF; + omapl138EthSendPacket((u8*)&r87l_tx_packet,sizeof(struct str_eth_frame)); + } + else + { + if(ping_nfo.adr != 0xFF) // need to send ping request or reply + { + memcpy((u8*)&r87l_tx_packet.str,(u8 *)log->stream_in,sizeof(struct stream_frame)); + memcpy((u8*)&r87l_tx_packet.ping,(u8*)&ping_nfo,sizeof(struct ping_info)); + ping_nfo.adr=0xFF; + omapl138EthSendPacket((u8*)&r87l_tx_packet,sizeof(struct str_eth_frame)); + } + } + + //my stream + memcpy((u8 *)&str[log->stream_in->addr][str_pos[log->stream_in->addr]],(u8*)log->stream_in,sizeof(struct stream_frame)); + str_bits[log->stream_in->addr][str_pos[log->stream_in->addr]++]=0; + str_pos[log->stream_in->addr]%=ETH_STREAM_BUF_SIZE; + + if(ping_time!=log->ping_time_prev) + { + *log->ping_time = ping_time==0xFFFFFFFF?999999.0f:(float)ping_time*0.000213219616f; + log->ping_time_prev=ping_time; + } + + + if(log->tcnt >= 5000) + log->tcnt=0; + else + log->tcnt+=LOOP_CYCLE_MS; + } + + check_and_set_struct((eth.flags & ETH_INITIALIZED),&log->alive); + log->prev_timestamp=bus_an_cur_timestamp; +} diff --git a/src/eth.h b/src/eth.h new file mode 100644 index 0000000..29a4ed4 --- /dev/null +++ b/src/eth.h @@ -0,0 +1,117 @@ +/* + * eth.h + * + * Created on: 22-03-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef ETH_H_ +#define ETH_H_ + +#include "tdefs.h" +#include "mkstream.h" +#include "helper.h" +#include "comm.h" + +#define ETH_STREAM_PORT 31337 +#define ETH_WAIT_FOR_ARM_TIME 200 +#define ETH_MAX_STREAMS 3 +#define ETH_MAX_RECV_STREAMS 8 + +#define ETH_STREAM_BUF_SIZE 16//8 +#define LINK_TIMEOUT_TIME 1000 +#define LINK_UP_TIME 5 + +extern volatile u8 str_bits[ETH_MAX_RECV_STREAMS][ETH_STREAM_BUF_SIZE]; +extern struct str_eth_frame r87l_tx_packet; + +extern volatile u32 str_pos[ETH_MAX_RECV_STREAMS]; +extern struct stream_frame str[ETH_MAX_RECV_STREAMS][ETH_STREAM_BUF_SIZE] __attribute__((aligned(4))); + +struct str_eth_frame +{ + u8 dst_mac[6]; + u8 src_mac[6]; + u8 lenh; + u8 lenl; + u32 padding1; + struct stream_frame str; + struct ping_info ping; + u8 padding2[14]; +}__attribute__((__packed__)); + +struct eth_logic +{ + struct binary_io en; + struct binary_io stream_en; + struct stream_frame *stream_in; + struct binary_io alive; + struct binary_io clr; + struct binary_io ping; + struct stream_frame stream_out[ETH_MAX_STREAMS]; + struct binary_io stream_act[ETH_MAX_STREAMS]; + struct binary_io link[ETH_MAX_STREAMS]; + float *stream_errs[ETH_MAX_STREAMS]; + float *stream_synt_errs[ETH_MAX_STREAMS]; + float *ping_time; + u16 s_delay; + u32 state; + u32 tcnt; + u32 prev_pos[ETH_MAX_STREAMS]; + u32 link_timeout_cnt[ETH_MAX_STREAMS]; + u32 link_up_cnt[ETH_MAX_STREAMS]; + u32 ping_time_prev; + u16 prev_timestamp; +}; + +struct eth_io +{ + u32 en_in; + u32 stream_en_in; + u32 stream_ptr_in; + u32 clr_in; + u32 ping_in; + u32 alive_out; + u32 stream0_ptr_out; + u32 stream0_act_out; + u32 stream1_ptr_out; + u32 stream1_act_out; + u32 stream2_ptr_out; + u32 stream2_act_out; + u32 link0_out; + u32 stream0_errs_float_out; + u32 link1_out; + u32 stream1_errs_float_out; + u32 link2_out; + u32 stream2_errs_float_out; + u32 ping_float_out; + u32 stream0_synt_errs_float_out; + u32 stream1_synt_errs_float_out; + u32 stream2_synt_errs_float_out; +}__attribute__((__packed__)); + + +//bits +#define ETH_EN 0x01 +#define ETH_EN_BCASTS 0x02 +#define ETH_NO_SYNC 0x04 + +struct eth_params +{ + u32 bits; + u32 sample_delay; + u32 multicast_id; +}__attribute__((__packed__)); + +struct eth_args +{ + struct eth_io io; + struct eth_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void ethernet_recv(char *buf); +extern void ethernet(void *args, void *logic); +extern int ethernet_initlog(void *arguments, void *logic); + +#endif diff --git a/src/eth_demux.c b/src/eth_demux.c new file mode 100644 index 0000000..f685a51 --- /dev/null +++ b/src/eth_demux.c @@ -0,0 +1,205 @@ +/* + * eth_demux.c + * + * Created on: 21-05-2024 + * Author: Krzysztof Jakubczyk + */ + +#include +#include + +#include "tdefs.h" +#include "misc.h" +#include "eth.h" +#include "helper.h" +#include "eth_demux.h" +#include "analog_in.h" +#include "R87L.h" + +u16 eth_comm_bits_act[8] = {0,0,0,0,0,0,0,0}; + +int eth_demux_initlog(void *arguments, void *logic) +{ + struct eth_demux_args *args = (struct eth_demux_args *)arguments; + struct eth_demux_logic *log = (struct eth_demux_logic *)logic; +// u8 i; + + if(set_pointer_in_ptr(args->io.stream1_ptr_in,(u32*)&log->stream_in[0])) + return -1; + if(set_pointer_in_ptr(args->io.stream2_ptr_in,(u32*)&log->stream_in[1])) + return -1; + if(set_pointer_in_ptr(args->io.stream3_ptr_in,(u32*)&log->stream_in[2])) + return -1; + if(set_pointer_in_ptr(args->io.stream4_ptr_in,(u32*)&log->stream_in[3])) + return -1; + if(set_pointer_in_ptr(args->io.stream5_ptr_in,(u32*)&log->stream_in[4])) + return -1; + if(set_pointer_in_ptr(args->io.stream6_ptr_in,(u32*)&log->stream_in[5])) + return -1; + if(set_pointer_in_ptr(args->io.stream7_ptr_in,(u32*)&log->stream_in[6])) + return -1; + if(set_pointer_in_ptr(args->io.stream8_ptr_in,(u32*)&log->stream_in[7])) + return -1; + + if(set_bit_ptr_struct(args->io.stream1_act_in,&log->stream_in_act[0])) + return -1; + if(set_bit_ptr_struct(args->io.stream2_act_in,&log->stream_in_act[1])) + return -1; + if(set_bit_ptr_struct(args->io.stream3_act_in,&log->stream_in_act[2])) + return -1; + if(set_bit_ptr_struct(args->io.stream4_act_in,&log->stream_in_act[3])) + return -1; + if(set_bit_ptr_struct(args->io.stream5_act_in,&log->stream_in_act[4])) + return -1; + if(set_bit_ptr_struct(args->io.stream6_act_in,&log->stream_in_act[5])) + return -1; + if(set_bit_ptr_struct(args->io.stream7_act_in,&log->stream_in_act[6])) + return -1; + if(set_bit_ptr_struct(args->io.stream8_act_in,&log->stream_in_act[7])) + return -1; + + + if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32*)&log->stream_out[0])) + return -1; + if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32*)&log->stream_out[1])) + return -1; + if(set_pointer_out_ptr(args->io.stream3_ptr_out,(u32*)&log->stream_out[2])) + return -1; + if(set_pointer_out_ptr(args->io.stream4_ptr_out,(u32*)&log->stream_out[3])) + return -1; + if(set_pointer_out_ptr(args->io.stream5_ptr_out,(u32*)&log->stream_out[4])) + return -1; + if(set_pointer_out_ptr(args->io.stream6_ptr_out,(u32*)&log->stream_out[5])) + return -1; + if(set_pointer_out_ptr(args->io.stream7_ptr_out,(u32*)&log->stream_out[6])) + return -1; + if(set_pointer_out_ptr(args->io.stream8_ptr_out,(u32*)&log->stream_out[7])) + return -1; + + if(set_bit_ptr_struct(args->io.streams_ok_out,&log->streams_ok)) + return -1; + + if(set_bit_ptr_struct(args->io.przeglad1_out,&log->przeglad[0])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad2_out,&log->przeglad[1])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad3_out,&log->przeglad[2])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad4_out,&log->przeglad[3])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad5_out,&log->przeglad[4])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad6_out,&log->przeglad[5])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad7_out,&log->przeglad[6])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad8_out,&log->przeglad[7])) + return -1; + + log->jednostki = args->params.liczba_jednostek+2; + + if(log->jednostki>8) + log->jednostki=8; + + if(set_float_ptr(args->io.endpoints_float_out,&log->endpoints)) + return -1; + + *log->endpoints=log->jednostki; + + return 0; +} + +void eth_demux(void *arguments, void *logic) +{ + struct eth_demux_logic *log = (struct eth_demux_logic *)logic; + + u8 i,j,stream_ok,str_ok[8],str_ok_ndelay[8]; + stream_ok=1; + + for(i=0;ijednostki;i++) + { + str_ok[i]=1; + str_ok_ndelay[i]=1; + + if(check_struct(&log->stream_in_act[i])) + { + log->snum_cur[i]=log->stream_in[i]->sample_stamp; + + if((log->snum_prev[i]+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=log->snum_cur[i]) + { + log->sfault_delay_cnt[i]=20; + if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok[i]=0; + str_ok_ndelay[i]=0; + } + else if(log->sfault_delay_cnt[i]) + { + if(log->sfault_delay_cnt[i]>=LOOP_CYCLE_MS) + log->sfault_delay_cnt[i]-=LOOP_CYCLE_MS; + else + log->sfault_delay_cnt[i]=0; + + if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok[i]=0; + } + + log->ostream_ptr[i]=log->stream_in[i]; + log->snum_prev[i]=log->snum_cur[i]; + } + else + { + if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok_ndelay[i]=0; + str_ok[i]=0; + } + + if(i>0 && log->snum_cur[i]!=log->snum_cur[i-1] && !(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD) && !(eth_comm_bits_act[i-1] & R87L_COMM_PRZEGLAD)) + { + log->sfault_delay_cnt[i]=20; + stream_ok=0; + str_ok[i]=0; + str_ok_ndelay[i]=0; + } + } + + for(i=0;ijednostki;i++) + { + if(str_ok[i]) + eth_comm_bits_act[i]=log->ostream_ptr[i]->bits; + + check_and_set_struct(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD,&log->przeglad[i]); + } + + check_and_set_struct(stream_ok,&log->streams_ok); + + if(stream_ok) + { + for(i=0;ijednostki;i++) + { + if(!(eth_comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame)); + else + { + memset((u8*)&log->stream_out[i],0,sizeof(log->stream_out[0])); + log->stream_out[i].bits=(eth_comm_bits_act[i] & (R87L_COMM_PRZEGLAD|0xFF00)); // przeglad bit i GPbits przepisuj w przegladzie + } + } + } + else + { + for(i=0;ijednostki;i++) + if(str_ok_ndelay[i]) + memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame)); + else + { + for(j=0;j<3;j++) + log->stream_out[i].A[j]=32768.0f; + } +// memset((u8*)log->stream_out,0,sizeof(log->stream_out)); // zostawic ostatnie dane? +// for(i=0;i<3;i++) +// log->stream_out[i].bits=(comm_bits_act[i] & R87L_COMM_PRZEGLAD); + } +} diff --git a/src/eth_demux.h b/src/eth_demux.h new file mode 100644 index 0000000..d3f3f48 --- /dev/null +++ b/src/eth_demux.h @@ -0,0 +1,100 @@ +/* + * eth_demux.h + * + * Created on: 21-05-2024 + * Author: Krzysztof Jakubczyk + */ + +#ifndef ETH_DEMUX_H_ +#define ETH_DEMUX_H_ + +#include "tdefs.h" +#include "helper.h" +#include "mkstream.h" + +extern u16 eth_comm_bits_act[8]; +extern u16 eth_comm_bits_restored[8]; + +struct eth_demux_logic +{ + struct stream_frame *stream_in[8]; + struct binary_io stream_in_act[8]; + struct stream_frame stream_out[8]; + + struct binary_io streams_ok; + + struct stream_frame *ostream_ptr[8]; + struct binary_io przeglad[8]; + float *endpoints; + + u32 snum_cur[8]; + u32 snum_prev[8]; + u32 sfault_delay_cnt[8]; + u32 jednostki; +}; + +struct eth_demux_io +{ + u32 stream1_ptr_in; + u32 stream1_act_in; + + u32 stream2_ptr_in; + u32 stream2_act_in; + + u32 stream3_ptr_in; + u32 stream3_act_in; + + u32 stream4_ptr_in; + u32 stream4_act_in; + + u32 stream5_ptr_in; + u32 stream5_act_in; + + u32 stream6_ptr_in; + u32 stream6_act_in; + + u32 stream7_ptr_in; + u32 stream7_act_in; + + u32 stream8_ptr_in; + u32 stream8_act_in; + + u32 stream1_ptr_out; + u32 stream2_ptr_out; + u32 stream3_ptr_out; + u32 stream4_ptr_out; + u32 stream5_ptr_out; + u32 stream6_ptr_out; + u32 stream7_ptr_out; + u32 stream8_ptr_out; + u32 streams_ok_out; + + u32 przeglad1_out; + u32 przeglad2_out; + u32 przeglad3_out; + u32 przeglad4_out; + u32 przeglad5_out; + u32 przeglad6_out; + u32 przeglad7_out; + u32 przeglad8_out; + + u32 endpoints_float_out; +}__attribute__((__packed__)); + + +struct eth_demux_params +{ + u32 liczba_jednostek; +}__attribute__((__packed__)); + +struct eth_demux_args +{ + struct eth_demux_io io; + struct eth_demux_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void eth_demux(void *args, void *logic); +extern int eth_demux_initlog(void *arguments, void *logic); + +#endif diff --git a/src/eth_ench.c b/src/eth_ench.c new file mode 100644 index 0000000..fa1ee00 --- /dev/null +++ b/src/eth_ench.c @@ -0,0 +1,226 @@ +/* + * eth_ench.c + * + * Created on: 17-05-2024 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "eth.h" +#include "eth_ench.h" +#include "mkstream.h" + +#include + +#include "comm.h" +#include "../ethernet/ports/am1808/include/lwiplib.h" +#include "../ethernet/emac.h" +#include "../ethernet/hw_types.h" +#include "ports/am1808/include/netif/sitaraif.h" +#include +//#include + + +#define ETH_NO_TZ_CHECKSUM // don't check the checksum inside a TZ frame + + +int ethernet_ench_initlog(void *arguments, void *logic) +{ + struct eth_ench_args *args = (struct eth_ench_args *)arguments; + struct eth_ench_logic *log = (struct eth_ench_logic *)logic; + + if(set_bit_ptr_struct(args->io.clr_in,&log->clr)) + return -1; + + if(set_pointer_out_ptr(args->io.stream3_ptr_out,(u32 *)&log->stream_out[0])) + return -1; + if(set_bit_ptr_struct(args->io.stream3_act_out,&log->stream_act[0])) + return -1; + if(set_pointer_out_ptr(args->io.stream4_ptr_out,(u32 *)&log->stream_out[1])) + return -1; + if(set_bit_ptr_struct(args->io.stream4_act_out,&log->stream_act[1])) + return -1; + if(set_pointer_out_ptr(args->io.stream5_ptr_out,(u32 *)&log->stream_out[2])) + return -1; + if(set_bit_ptr_struct(args->io.stream5_act_out,&log->stream_act[2])) + return -1; + if(set_pointer_out_ptr(args->io.stream6_ptr_out,(u32 *)&log->stream_out[3])) + return -1; + if(set_bit_ptr_struct(args->io.stream6_act_out,&log->stream_act[3])) + return -1; + if(set_pointer_out_ptr(args->io.stream7_ptr_out,(u32 *)&log->stream_out[4])) + return -1; + if(set_bit_ptr_struct(args->io.stream7_act_out,&log->stream_act[4])) + return -1; + + if(set_bit_ptr_struct(args->io.link3_out,&log->link[0])) + return -1; + if(set_float_ptr(args->io.stream3_errs_float_out,&log->stream_errs[0])) + return -1; + + if(set_bit_ptr_struct(args->io.link4_out,&log->link[1])) + return -1; + if(set_float_ptr(args->io.stream4_errs_float_out,&log->stream_errs[1])) + return -1; + + if(set_bit_ptr_struct(args->io.link5_out,&log->link[2])) + return -1; + if(set_float_ptr(args->io.stream5_errs_float_out,&log->stream_errs[2])) + return -1; + + if(set_bit_ptr_struct(args->io.link6_out,&log->link[3])) + return -1; + if(set_float_ptr(args->io.stream6_errs_float_out,&log->stream_errs[3])) + return -1; + + if(set_bit_ptr_struct(args->io.link7_out,&log->link[4])) + return -1; + if(set_float_ptr(args->io.stream7_errs_float_out,&log->stream_errs[4])) + return -1; + + if(set_float_ptr(args->io.stream3_synt_errs_float_out,&log->stream_synt_errs[0])) + return -1; + if(set_float_ptr(args->io.stream4_synt_errs_float_out,&log->stream_synt_errs[1])) + return -1; + if(set_float_ptr(args->io.stream5_synt_errs_float_out,&log->stream_synt_errs[2])) + return -1; + if(set_float_ptr(args->io.stream6_synt_errs_float_out,&log->stream_synt_errs[3])) + return -1; + if(set_float_ptr(args->io.stream7_synt_errs_float_out,&log->stream_synt_errs[4])) + return -1; + + log->s_delay=args->params.sample_delay/LOOP_CYCLE_MS; + if(args->params.sample_delay%LOOP_CYCLE_MS) + log->s_delay++; + + log->s_delay*=(SAMPLES_PER_MS*LOOP_CYCLE_MS); + + if(log->s_delay>=((ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS))) + log->s_delay=(ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS); + + return 0; +} + +void ethernet_ench(void *arguments, void *logic) +{ + struct eth_ench_logic *log = (struct eth_ench_logic *)logic; + struct eth_ench_args *args = (struct eth_ench_args *)arguments; + + u8 i,k; +#ifndef ETH_NO_TZ_CHECKSUM + u8 j,cksum; +#endif + u16 find_stamp; + u8 already_incremented[ETH_ENCH_STREAMS]={0,0,0,0,0}; + //struct str_eth_frame *eth_str = (struct str_eth_frame *)r87l_tx_packet; + + if(check_struct(&log->clr)) + { + for(i=0;istream_errs[i]=0; + *log->stream_synt_errs[i]=0; + } + } + + for(i=0;iprev_pos[i]!=str_pos[i+ETH_MAX_STREAMS]) + { + if(log->link_up_cnt[i]>LINK_UP_TIME) + { + if(!check_struct(&log->link[i])) + *log->stream_errs[i]=0; + set_struct(&log->link[i]); + } + else + log->link_up_cnt[i]+=LOOP_CYCLE_MS; + + while(log->prev_pos[i]!=str_pos[i+ETH_MAX_STREAMS]) + { + u16 prev_tstamp=(!log->prev_pos[i]?str[i+ETH_MAX_STREAMS][ETH_STREAM_BUF_SIZE-1].sample_stamp:str[i+ETH_MAX_STREAMS][log->prev_pos[i]-1].sample_stamp) & 0x7FFF; + u16 next_tstamp=(str[i+ETH_MAX_STREAMS][log->prev_pos[i]].sample_stamp)&0x7FFF; + + if((prev_tstamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=next_tstamp) + { + if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp) + *log->stream_errs[i]+=1.0f; + already_incremented[i]=1; + } + log->prev_pos[i]++; + log->prev_pos[i]%=ETH_STREAM_BUF_SIZE; + } + + log->link_timeout_cnt[i]=0; + log->prev_pos[i]=str_pos[i+ETH_MAX_STREAMS]; + } + else + { + if(log->link_timeout_cnt[i]>LINK_TIMEOUT_TIME) + { + clear_struct(&log->link[i]); + log->link_up_cnt[i]=0; + } + else + { + if(log->link_timeout_cnt[i]>(1*LOOP_CYCLE_MS)) // at least 2ms between samples + { + if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp) + *log->stream_errs[i]+=1.0f; + already_incremented[i]=1; + } + log->link_timeout_cnt[i]+=LOOP_CYCLE_MS; + } + } + } + + if(bus_an_cur_timestamp>=log->s_delay/*ETH_BUF_DELAY*/) + find_stamp=bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY; + else + find_stamp=(SAMPLES_PER_MS*1000)+bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY; + + for(i=0;ilink[i])) + { + clear_struct(&log->stream_act[i]); + continue; + } + for(k=0;kparams.bits & ETH_NO_SYNC || str[i+ETH_MAX_STREAMS][k].sample_stamp == find_stamp) && !(str_bits[i+ETH_MAX_STREAMS][k] & STATUS_FRAME_READ)) + { +#ifndef ETH_NO_TZ_CHECKSUM + cksum=CKSUM_IV; + for(j=0;jstream_out[i],(u8*)&str[i+ETH_MAX_STREAMS][k],sizeof(struct stream_frame)); + break; + } + } + } + + if(kstream_act[i]); + else + { + clear_struct(&log->stream_act[i]); + if(!already_incremented[i]) // wyrzucono bledy od synT z licznikow + *log->stream_synt_errs[i]+=1.0f; + } + } + + log->prev_timestamp=bus_an_cur_timestamp; +} diff --git a/src/eth_ench.h b/src/eth_ench.h new file mode 100644 index 0000000..c44d325 --- /dev/null +++ b/src/eth_ench.h @@ -0,0 +1,90 @@ +/* + * eth_ench.h + * + * Created on: 17-05-2024 + * Author: Krzysztof Jakubczyk + */ + +#ifndef ETH_ENCH_H_ +#define ETH_ENCH_H_ + +#include "tdefs.h" +#include "mkstream.h" +#include "helper.h" +#include "comm.h" + +#define ETH_ENCH_STREAMS (ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS) + +struct eth_ench_logic +{ + struct binary_io clr; + struct binary_io ping; + struct stream_frame stream_out[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + struct binary_io stream_act[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + struct binary_io link[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + float *stream_errs[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + float *stream_synt_errs[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + float *ping_time; + u16 s_delay; + u32 state; + u32 tcnt; + u32 prev_pos[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + u32 link_timeout_cnt[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + u32 link_up_cnt[ETH_MAX_RECV_STREAMS-ETH_MAX_STREAMS]; + u16 prev_timestamp; +}; + +struct eth_ench_io +{ + u32 clr_in; + u32 stream3_ptr_out; + u32 stream3_act_out; + u32 stream4_ptr_out; + u32 stream4_act_out; + u32 stream5_ptr_out; + u32 stream5_act_out; + u32 stream6_ptr_out; + u32 stream6_act_out; + u32 stream7_ptr_out; + u32 stream7_act_out; + u32 link3_out; + u32 stream3_errs_float_out; + u32 link4_out; + u32 stream4_errs_float_out; + u32 link5_out; + u32 stream5_errs_float_out; + u32 link6_out; + u32 stream6_errs_float_out; + u32 link7_out; + u32 stream7_errs_float_out; + u32 stream3_synt_errs_float_out; + u32 stream4_synt_errs_float_out; + u32 stream5_synt_errs_float_out; + u32 stream6_synt_errs_float_out; + u32 stream7_synt_errs_float_out; +}__attribute__((__packed__)); + + +//bits +#define ETH_EN 0x01 +#define ETH_EN_BCASTS 0x02 +#define ETH_NO_SYNC 0x04 + +struct eth_ench_params +{ + u32 bits; + u32 sample_delay; + u32 multicast_id; +}__attribute__((__packed__)); + +struct eth_ench_args +{ + struct eth_ench_io io; + struct eth_ench_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void ethernet_ench(void *args, void *logic); +extern int ethernet_ench_initlog(void *arguments, void *logic); + +#endif diff --git a/src/event.c b/src/event.c new file mode 100644 index 0000000..4ca83a4 --- /dev/null +++ b/src/event.c @@ -0,0 +1,33 @@ +/* + * event.c + * + * Created on: 21-05-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "event.h" +#include "events_reg.h" + +int event_initlog(void *arguments, void *logic) +{ + struct event_args *args = (struct event_args *)arguments; + struct event_logic *log = (struct event_logic *)logic; + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_no = args->io.in & 0x07; + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(args->params.fun==0) + log->prev_state = (saved_events[(args->params.inf&0x7F)>>3] & (1<<(args->params.inf%8)))? 1 : 0; + else + log->prev_state = (*log->in_ptr & (1<in_bit_no)) ? 1 : 0; + + return 0; +} + diff --git a/src/event.h b/src/event.h new file mode 100644 index 0000000..caa09f0 --- /dev/null +++ b/src/event.h @@ -0,0 +1,42 @@ +/* + * event.h + * + * + * Created on: 21-05-2014 + * Author: KJ + */ + +#ifndef EVENT_H_ +#define EVENT_H_ + +#include "tdefs.h" + +struct event_logic +{ + u8 *in_ptr; + u8 in_bit_no; + u8 prev_state; + u8 transition_cnt; // lockbit|b6..b0 +}; + +struct event_io +{ + u32 in; +}__attribute__((__packed__)); + +struct event_params +{ + u32 fun; + u32 inf; +}__attribute__((__packed__)); + +struct event_args +{ + struct event_io io; + struct event_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int event_initlog(void *args, void *logic); + +#endif /* EVENT_H_ */ diff --git a/src/events_reg.c b/src/events_reg.c new file mode 100644 index 0000000..9fc32d0 --- /dev/null +++ b/src/events_reg.c @@ -0,0 +1,165 @@ +/* + * events_reg.c + * + * Created on: 02-06-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include +#include +#include "events_reg.h" +#include "event.h" +#include "comm.h" +#include + +struct events_db ev_db; +struct events_reg_logic *ev_reg_log = NULL; +volatile u8 events_reg_filter_act=0; +volatile u16 events_reg_filter_period=2000; +volatile u8 events_reg_filter_trans_limit=10; +volatile u8 saved_events[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + +void zprae_event_add(u8 fun, u8 inf) +{ + ev_db.ev[ev_db.pos].sec=(timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec; + ev_db.ev[ev_db.pos].msec=(timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec; + ev_db.ev[ev_db.pos].fun=fun; + ev_db.ev[ev_db.pos].inf=inf; + ev_db.pos++; + ev_db.pos%=MAX_EVENTS_COUNT; +} + +void zprae_event_add_with_time(u8 fun, u8 inf,struct timeval *cur_time) +{ + ev_db.ev[ev_db.pos].sec=cur_time->tv_sec; + ev_db.ev[ev_db.pos].msec=cur_time->tv_usec; + ev_db.ev[ev_db.pos].fun=fun; + ev_db.ev[ev_db.pos].inf=inf; + ev_db.pos++; + ev_db.pos%=MAX_EVENTS_COUNT; +} + +int events_reg_initlog(void *arguments, void *logic) +{ +// struct events_reg_args *args = (struct events_reg_args *)arguments; + struct events_reg_logic *log = (struct events_reg_logic *)logic; + int i; + + ev_reg_log=log; + + log->events_count=0; + + for(i=0;ielement_num[log->events_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->events_count]=i; + log->events_count++; + } + } + + memset(&ev_db,0,sizeof(ev_db)); + + return 0; +} + +void events_reg(void *arguments, void *logic) +{ + //struct events_reg_args *args = (struct events_reg_args *)arguments; + struct events_reg_logic *log = (struct events_reg_logic *)logic; + struct event_args *ev_args; + struct event_logic *ev_log; + int i,new_events; +// dbg.temp_dbg=(u32)saved_events[0] | ((u32)saved_events[1]<<8) | ((u32)saved_events[2]<<16)| ((u32)saved_events[3]<<24); + if(!log->start_cnt) // 1 cycle without events + { + log->start_cnt++; + return; + } + + new_events=0; + + if(!events_reg_filter_act) + log->time_cnt=0; + else + log->time_cnt+=LOOP_CYCLE_MS; + + for(i=0;ievents_count;i++) + { + ev_args = (struct event_args *)log_manager.log_element[log->element_num[i]].fun_args_ptr; + ev_log = (struct event_logic *)log_manager.log_element[log->element_num[i]].fun_log_ptr; + + if(events_reg_filter_act) + { + if(log->time_cnt==events_reg_filter_period) + { + if(ev_log->transition_cnt & EVENT_FILTERED) + if((ev_log->transition_cnt & 0x7F)params.fun | EVENT_FILTERED,ev_args->params.inf | EVENT_OFF); + ev_log->transition_cnt=0; + new_events=1; + } + else + ev_log->transition_cnt&=~0x7F; + else + ev_log->transition_cnt=0; + } + + if(ev_log->transition_cnt>=events_reg_filter_trans_limit && !(ev_log->transition_cnt & EVENT_FILTERED)) + { + zprae_event_add(ev_args->params.fun | EVENT_FILTERED,ev_args->params.inf | EVENT_ON); + ev_log->transition_cnt|=EVENT_FILTERED; + new_events=1; + } + } + else + ev_log->transition_cnt=0; + + if(*ev_log->in_ptr & (1<in_bit_no)) + { + if(!ev_log->prev_state) + { + if((log_manager.status & LOGMAN_STATUS_SOFT_STARTED) && (ev_log->transition_cnt & 0x7F) <= 0x7E) + ev_log->transition_cnt++; + + if(((log_manager.status & LOGMAN_STATUS_SOFT_STARTED)||!ev_args->params.fun) && !(ev_log->transition_cnt & EVENT_FILTERED)) + zprae_event_add(ev_args->params.fun,ev_args->params.inf | EVENT_ON); + ev_log->prev_state=1; + + if(!ev_args->params.fun) + saved_events[(ev_args->params.inf&0x7F)>>3]|=(1<<(ev_args->params.inf%8)); + + new_events=1; + } + } + else + { + if(ev_log->prev_state) + { + if((log_manager.status & LOGMAN_STATUS_SOFT_STARTED) && (ev_log->transition_cnt & 0x7F) <= 0x7E) + ev_log->transition_cnt++; + + if(((log_manager.status & LOGMAN_STATUS_SOFT_STARTED)||!ev_args->params.fun) && !(ev_log->transition_cnt & EVENT_FILTERED)) + zprae_event_add(ev_args->params.fun,ev_args->params.inf | EVENT_OFF); + ev_log->prev_state=0; + if(!ev_args->params.fun) + saved_events[(ev_args->params.inf & 0x7F)>>3]&=~(1<<(ev_args->params.inf%8)); + new_events=1; + } + } + } + + if(log->time_cnt >= events_reg_filter_period) + log->time_cnt=0; + + if(new_events) + logman_notify|=LOGMAN_NOTIFY_NEW_EVENTS; +} diff --git a/src/events_reg.h b/src/events_reg.h new file mode 100644 index 0000000..892661e --- /dev/null +++ b/src/events_reg.h @@ -0,0 +1,74 @@ +/* + * events_reg.h + * + * + * Created on: 02-06-2014 + * Author: KJ + */ + +#ifndef EVENTS_REG_H_ +#define EVENTS_REG_H_ + +#include "tdefs.h" +#include "helper.h" +#include "misc.h" + +#define MAX_EVENTS_COUNT 256 +#define EVENT_ON 0x80 +#define EVENT_OFF 0x00 + +extern struct events_reg_logic *ev_reg_log; + +extern volatile u8 events_reg_filter_act; +extern volatile u16 events_reg_filter_period; +extern volatile u8 events_reg_filter_trans_limit; +extern volatile u8 saved_events[16]; + +extern void zprae_event_add(u8 fun, u8 inf); +extern void zprae_event_add_with_time(u8 fun, u8 inf,struct timeval *cur_time); + +struct event_data +{ + u8 fun; + u8 inf; + u32 sec; + u16 msec; +}__attribute__((__packed__)); + +struct events_db +{ + struct event_data ev[MAX_EVENTS_COUNT]; + u16 pos; +}__attribute__((__packed__)); + +extern struct events_db ev_db; + +struct events_reg_logic +{ + u16 events_count; + u16 time_cnt; + u8 start_cnt; + u16 element_num[]; +}; + +struct events_reg_io +{ +}__attribute__((__packed__)); + +struct events_reg_params +{ +}__attribute__((__packed__)); + +struct events_reg_args +{ + struct events_reg_io io; + struct events_reg_params params; +// u16 crc; +}__attribute__((__packed__)); + +#define EVENT_FILTERED 0x80 + +extern int events_reg_initlog(void *args, void *logic); +extern void events_reg(void *args, void *logic); + +#endif /* EVENTS_REG_H_ */ diff --git a/src/goose.c b/src/goose.c new file mode 100644 index 0000000..0b3d1bb --- /dev/null +++ b/src/goose.c @@ -0,0 +1,380 @@ +/* + * goose.c + * + * Created on: 16-07-2020 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "eth.h" +#include "goose.h" + +#include + +#include "comm.h" +#include "../ethernet/ports/am1808/include/lwiplib.h" +#include "../ethernet/emac.h" +#include "../ethernet/hw_types.h" +#include "ports/am1808/include/netif/sitaraif.h" +#include +//#include +#include +#include + +//TODO: stnum i seqnum dokladniej analizowac? +// dorobic lub sprawdzac wyjscia QUALITY? + +//#pragma LOCATION(gqueue,0xcb800000) // @ non-cached memory above 128MB +struct goose_queue gqueue = { .cur = 0, .next = 0, .lost = 0 }; + +void iec61850_goose_recv(struct goose_frame *gfr,int len,char type) +{ + if(!(gqueue.status[gqueue.next] & (GOOSE_TO_PROCESS))) + { + if(htons(gfr->len)==len && len<=GOOSE_MAX_FR_SIZE) + { + memcpy(gqueue.buf[gqueue.next],(char*)gfr,len); + gqueue.status[gqueue.next]|=GOOSE_TO_PROCESS; + gqueue.next++; + gqueue.next%=GOOSE_QUEUE_DEPTH; + } + } + else + gqueue.lost++; +} + +int goose_initlog(void *arguments, void *logic) +{ + struct goose_args *args = (struct goose_args *)arguments; + struct goose_logic *log = (struct goose_logic *)logic; + u8 i; + int values[6]; + u8 dstaddr[6]; + + if(set_float_ptr(args->io.recv_cnt_float_out,&log->recv_cnt)) + return -1; + + for(i=0;i<8;i++) + if(set_bit_ptr_struct(args->io.state_out[i],&log->state[i])) + return -1; + + if(set_bit_ptr_struct(args->io.expired_out,&log->expired)) + return -1; + + if(set_bit_ptr_struct(args->io.test_out,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.ndscom_out,&log->ndscom)) + return -1; + + gqueue.lost=0; + + log->goose_id=(char*)&args->params.strings_start; + u8 len = strlen(log->goose_id) + 1; + if(len%4) + len+=4-(len%4); + + log->goose_dataset=log->goose_id+len; + len = strlen(log->goose_dataset) + 1; + if(len%4) + len+=4-(len%4); + + log->goose_cbref=log->goose_dataset+len; + len = strlen(log->goose_cbref) + 1; + if(len%4) + len+=4-(len%4); + + log->goose_dstaddr=log->goose_cbref+len; + len = strlen(log->goose_dstaddr) + 1; + if(len%4) + len+=4-(len%4); + + if( 6 == sscanf( log->goose_dstaddr, "%x-%x-%x-%x-%x-%x%*c", + &values[0], &values[1], &values[2], + &values[3], &values[4], &values[5] ) ) + { + for( i = 0; i < 6; ++i ) + dstaddr[i] = (u8) values[i]; + } + else + return -1; + + emac_hash_add(EMAC_0_BASE,dstaddr); + + + return 0; +} + + +void goose(void *arguments, void *logic) // not used, replaced by goose_drv.c +{ + struct goose_logic *log = (struct goose_logic *)logic; + struct goose_args *args = (struct goose_args *)arguments; + struct goose_frame *gfr; + u8 *pdu; + u8 *pdu_prev; + u8 matched_crit; + int len,pdu_len; + int bytes_left, bytes_left_prev; + int err; + u8 tag; + int alldata_len; + int bytes_left_alldata; + u8 *pdu_alldata; + u8 data_cnt; + u8 i; + u32 sqnum,stnum,ttl,cfg_rev; + + //*log->recv_cnt=log->processed; + //*log->lost=gqueue.lost; + + if(log->ttl) + log->ttl--; + + check_and_set_struct(!log->ttl,&log->expired); + + while(gqueue.status[gqueue.cur] & (GOOSE_TO_PROCESS)) + { + + err = 0; + gfr = (struct goose_frame *)gqueue.buf[gqueue.cur]; + + if(htons(gfr->appid)==args->params.appid) + { + matched_crit = 0; + + bytes_left=htons(gfr->len)-8; + pdu = (u8 *)&gfr->pdu_tag; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + + if(len<0) + { + err=1; + break; + } + + if(tag==GOOSE_TAG_PDU) + { + pdu_len = len; + pdu_prev = pdu; + bytes_left_prev = bytes_left; + bytes_left = pdu_len; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + if(len<0) + { + err=1; + break; + } + + switch(tag) + { + case GOOSE_TAG_GOCBREF: + // process data (compare LEN bytes with STR) + if(len!=strlen(log->goose_cbref) || memcmp(log->goose_cbref,pdu,len)) + err=1; + else + matched_crit++; + + pdu+=len; + bytes_left-=len; + break; + + case GOOSE_TAG_DATSET: + // process data (compare LEN bytes with STR) + if(len!=strlen(log->goose_dataset) || memcmp(log->goose_dataset,pdu,len)) + err=1; + else + matched_crit++; + + pdu+=len; + bytes_left-=len; + break; + + case GOOSE_TAG_CONFREV: //dodac bytes left-=len ?! + if(len>4) + err=1; + else + { + cfg_rev=0; + + for(i=0;iparams.cfg_rev==cfg_rev) + matched_crit++; + else + err=1; + } + break; + + case GOOSE_TAG_TTL: //dodac bytes left-=len ?! + if(len>4) + err=1; + else + { + ttl=0; + + for(i=0;i4) + err=1; + else + { + stnum=0; + + for(i=0;i4) + err=1; + else + { + sqnum=0; + + for(i=0;igoose_id) || memcmp(log->goose_id,pdu,len)) + err=1; + else + matched_crit++; + + pdu+=len; + bytes_left-=len; + break; + + case GOOSE_TAG_ALLDATA: + data_cnt = 0; + alldata_len = len; + bytes_left_alldata = bytes_left; + pdu_alldata = pdu; + bytes_left=alldata_len; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + if(len<0) + { + err=1; + break; + } + + switch(tag) + { + case GOOSE_DATA_STRUCT: + break; + default: + for(i=0;i<8;i++) + if(data_cnt==args->params.data_index[i] && tag==GOOSE_DATA_BOOLEAN) + { + log->values&=~(1<values|=*pdu?(1<=(log->sqnum+1) && stnum==log->stnum) + || stnum>=log->stnum+1 + || !log->ttl) + { + //log->processed++; + *log->recv_cnt=(*log->recv_cnt)+1; + log->stnum=stnum; + log->sqnum=sqnum; + log->ttl=ttl; + + for(i=0;i<8;i++) + check_and_set_struct(log->values & (1<state[i]); + } + break; + } + + pdu=pdu_prev+pdu_len; + bytes_left=bytes_left_prev-pdu_len; + } + else + { + pdu+=len; + bytes_left-=len; + } + } + } + + gqueue.status[gqueue.cur]=0; + gqueue.cur=((gqueue.cur+1)%GOOSE_QUEUE_DEPTH); + } +} diff --git a/src/goose.h b/src/goose.h new file mode 100644 index 0000000..66cbb7e --- /dev/null +++ b/src/goose.h @@ -0,0 +1,153 @@ +/* + * goose.h + * + * Created on: 16-07-2020 + * Author: Krzysztof Jakubczyk + */ + +#ifndef GOOSE_H_ +#define GOOSE_H_ + +#include "tdefs.h" +#include "helper.h" +#include "comm.h" +#include "config.h" + +#define GOOSE_NORMAL 0 +#define GOOSE_VLAN 1 + +#define GOOSE_TAG_GOCBREF 0x80 +#define GOOSE_TAG_TTL 0x81 +#define GOOSE_TAG_DATSET 0x82 +#define GOOSE_TAG_ID 0x83 +#define GOOSE_TAG_T 0x84 +#define GOOSE_TAG_STNUM 0x85 +#define GOOSE_TAG_SQNUM 0x86 +#define GOOSE_TAG_SIMULATION 0x87 +#define GOOSE_TAG_CONFREV 0x88 +#define GOOSE_TAG_NDSCOM 0x89 +#define GOOSE_TAG_NUMDATSETENTRIES 0x8a +#define GOOSE_TAG_ALLDATA 0xAB +#define GOOSE_TAG_PDU 0x61 + +#define GOOSE_DATA_ARRAY 0x81 +#define GOOSE_DATA_STRUCT 0xA2 +//#define GOOSE_DATA_STRUCT 0x82 +#define GOOSE_DATA_BOOLEAN 0x83 +#define GOOSE_DATA_BITSTRING 0x84 +#define GOOSE_DATA_INTEGER 0x85 +#define GOOSE_DATA_UNSIGNED 0x86 +#define GOOSE_DATA_FLOAT 0x87 +#define GOOSE_DATA_OCTETSTRING 0x89 +#define GOOSE_DATA_VISIBLESTRING 0x8A +#define GOOSE_DATA_TIMEOFDAY 0x8C +#define GOOSE_DATA_BCD 0x8D +#define GOOSE_DATA_BOOLEANARRAY 0x8E + +#define GOOSE_ETHERTYPE_LE 0xb888 +#define SV_ETHERTYPE_LE 0xba88 + +struct goose_queue +{ + u32 cur; + u32 next; + u32 lost; + char buf[GOOSE_QUEUE_DEPTH][GOOSE_MAX_FR_SIZE]; + char status[GOOSE_QUEUE_DEPTH]; +}; + +#define GOOSE_TO_PROCESS 0x01 +#define SV_TO_PROCESS 0x02 + +struct goose_frame +{ + u16 appid; + u16 len; + u32 reserved; + u8 pdu_tag; + u8 pdu_len; + u8 pdu_data[]; +}__attribute__((__packed__)); + +struct goose_frame2 +{ + u16 appid; + u16 len; + u32 reserved; + u8 pdu_tag; + u8 pdu_taglen; + u8 pdu_len; + u8 pdu_data[]; +}__attribute__((__packed__)); + +struct goose_frame_vlan +{ + u8 dst_mac[6]; + u8 src_mac[6]; + u16 tpid; // 802.1Q + u16 tci; // 802.1Q + u16 ethertype; + struct goose_frame goose; +}__attribute__((__packed__)); + +struct goose_frame_normal +{ + u8 dst_mac[6]; + u8 src_mac[6]; + u16 ethertype; + struct goose_frame goose; +}__attribute__((__packed__)); + +extern struct goose_queue gqueue; + +struct goose_logic +{ + float *recv_cnt; + struct binary_io state[8]; + struct binary_io expired; + struct binary_io ndscom; + struct binary_io test; + char *goose_id; + char *goose_dataset; + char *goose_cbref; + char *goose_dstaddr; + char *sv_id; + char *sv_dstaddr; + uint64_t ttl; + uint64_t stnum; + uint64_t sqnum; + u8 values; +}; + +struct goose_io +{ + u32 recv_cnt_float_out; + u32 state_out[8]; + u32 expired_out; + u32 ndscom_out; + u32 test_out; +}__attribute__((__packed__)); + + +struct goose_params +{ + u32 bits; + u32 appid; + u32 data_index[8]; + u32 cfg_rev; + u32 strings_start; +}__attribute__((__packed__)); + +struct goose_args +{ + struct goose_io io; + struct goose_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void goose(void *args, void *logic); +extern int goose_initlog(void *arguments, void *logic); + +extern void iec61850_goose_recv(struct goose_frame *gfr,int len,char type); + +#endif diff --git a/src/goose_drv.c b/src/goose_drv.c new file mode 100644 index 0000000..ec7634a --- /dev/null +++ b/src/goose_drv.c @@ -0,0 +1,394 @@ +/* + * goose.c + * + * Created on: 16-07-2020 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "eth.h" +#include "goose.h" +#include "goose_drv.h" + +#include + +#include "comm.h" +#include "../ethernet/ports/am1808/include/lwiplib.h" +#include "../ethernet/emac.h" +#include "../ethernet/hw_types.h" +#include "ports/am1808/include/netif/sitaraif.h" +#include +//#include +#include +#include + +//TODO: stnum i seqnum dokladniej analizowac? +// dorobic lub sprawdzac wyjscia QUALITY? + +int goose_drv_initlog(void *arguments, void *logic) +{ + struct goose_drv_args *args = (struct goose_drv_args *)arguments; + struct goose_drv_logic *log = (struct goose_drv_logic *)logic; + u32 i; + + if(set_float_ptr(args->io.lost_float_out,&log->lost)) + return -1; + if(set_float_ptr(args->io.recv_float_out,&log->recv)) + return -1; + + gqueue.lost=0; + + for(i=0;ielement_num[log->gooses_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->gooses_count]=i; + log->gooses_count++; + } + } + + return 0; +} + + +void goose_drv(void *arguments, void *logic) +{ + struct goose_drv_logic *log = (struct goose_drv_logic *)logic; +// struct goose_drv_args *args = (struct goose_drv_args *)arguments; + struct goose_args *g_args; + struct goose_logic *g_log; + + struct goose_frame *gfr; + u8 *pdu; +// u8 *pdu_prev; + u8 matched_crit; + int len,pdu_len; + int bytes_left;//, bytes_left_prev; + int err; + u8 tag; + int alldata_len; + int bytes_left_alldata; + u8 *pdu_alldata; + u8 data_cnt; + u8 i; + u8 j; + u32 /*sqnum,stnum,ttl*/cfg_rev,ndscom,test; + uint64_t sqnum,stnum,ttl; + u8 ndscom_valid,test_valid; + + *log->lost=gqueue.lost; + + for(j=0;jgooses_count;j++) + { + g_args = (struct goose_args *)log_manager.log_element[log->element_num[j]].fun_args_ptr; + g_log = (struct goose_logic *)log_manager.log_element[log->element_num[j]].fun_log_ptr; + if(g_log->ttl>=LOOP_CYCLE_MS) + g_log->ttl-=LOOP_CYCLE_MS; + else + g_log->ttl=0; + + check_and_set_struct(!g_log->ttl,&g_log->expired); + } + + while(gqueue.status[gqueue.cur] & (GOOSE_TO_PROCESS)) + { + gfr = (struct goose_frame *)gqueue.buf[gqueue.cur]; + *log->recv=(*log->recv)+1; + + for(j=0;jgooses_count;j++) + { + g_args = (struct goose_args *)log_manager.log_element[log->element_num[j]].fun_args_ptr; + g_log = (struct goose_logic *)log_manager.log_element[log->element_num[j]].fun_log_ptr; + + err = 0; + ndscom_valid=0; + test_valid=0; + + if(htons(gfr->appid)==g_args->params.appid) + { + matched_crit = 0; + + bytes_left=htons(gfr->len)-8; + pdu = (u8 *)&gfr->pdu_tag; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + + if(len<0) + { + err=1; + break; + } + + if(tag==GOOSE_TAG_PDU) + { + pdu_len = len; +// pdu_prev = pdu; +// bytes_left_prev = bytes_left; + bytes_left = pdu_len; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + if(len<0) + { + err=1; + break; + } + + switch(tag) + { + case GOOSE_TAG_GOCBREF: + // process data (compare LEN bytes with STR) + if(len!=strlen(g_log->goose_cbref) || memcmp(g_log->goose_cbref,pdu,len)) + err=1; + else + matched_crit++; + + pdu+=len; + bytes_left-=len; + break; + + case GOOSE_TAG_DATSET: + // process data (compare LEN bytes with STR) + if(len!=strlen(g_log->goose_dataset) || memcmp(g_log->goose_dataset,pdu,len)) + err=1; + else + matched_crit++; + + pdu+=len; + bytes_left-=len; + break; + + case GOOSE_TAG_CONFREV: //dodac bytes left-=len ?! + if(len>4) + err=1; + else + { + cfg_rev=0; + + for(i=0;iparams.cfg_rev==cfg_rev) + matched_crit++; + else + err=1; + } + break; + + case GOOSE_TAG_TTL: //dodac bytes left-=len ?! + if(len>5) + err=1; + else + { + ttl=0; + + for(i=0;i5) + err=1; + else + { + stnum=0; + + for(i=0;i5) + err=1; + else + { + sqnum=0; + + for(i=0;i4) + err=1; + else + { + test=0; + + for(i=0;i4) + err=1; + else + { + ndscom=0; + + for(i=0;igoose_id) || memcmp(g_log->goose_id,pdu,len)) + err=1; + else + matched_crit++; + + pdu+=len; + bytes_left-=len; + break; + + case GOOSE_TAG_ALLDATA: + data_cnt = 0; + alldata_len = len; + bytes_left_alldata = bytes_left; + pdu_alldata = pdu; + bytes_left=alldata_len; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + if(len<0) + { + err=1; + break; + } + + switch(tag) + { + case GOOSE_DATA_STRUCT: + break; + default: + for(i=0;i<8;i++) + if(data_cnt==g_args->params.data_index[i] && tag==GOOSE_DATA_BOOLEAN) + { + g_log->values&=~(1<values|=*pdu?(1<=(g_log->sqnum+1) && stnum==g_log->stnum) + || stnum>=g_log->stnum+1 + || !g_log->ttl) + { + *g_log->recv_cnt=(*g_log->recv_cnt)+1; + g_log->stnum=stnum; + g_log->sqnum=sqnum; + g_log->ttl=ttl; + clear_struct(&g_log->expired); + + for(i=0;i<8;i++) + check_and_set_struct(g_log->values & (1<state[i]); + + check_and_set_struct(ndscom,&g_log->ndscom); + check_and_set_struct(test,&g_log->test); + } + break; + } + + if(err) //do nastepnego goosa + { + break; + } + + } + + break; + } + else + { + pdu+=len; + bytes_left-=len; + } + } + } + } + gqueue.status[gqueue.cur]=0; + gqueue.cur=((gqueue.cur+1)%GOOSE_QUEUE_DEPTH); + } +} diff --git a/src/goose_drv.h b/src/goose_drv.h new file mode 100644 index 0000000..59d1404 --- /dev/null +++ b/src/goose_drv.h @@ -0,0 +1,45 @@ +/* + * goose.h + * + * Created on: 16-07-2020 + * Author: Krzysztof Jakubczyk + */ + +#ifndef GOOSE_DRV_H_ +#define GOOSE_DRV_H_ + +#include "tdefs.h" +#include "helper.h" +#include "comm.h" +#include "config.h" + +struct goose_drv_logic +{ + float *recv; + float *lost; + u8 gooses_count; + u16 element_num[]; +}; + +struct goose_drv_io +{ + u32 recv_float_out; + u32 lost_float_out; +}__attribute__((__packed__)); + + +struct goose_drv_params +{ +}__attribute__((__packed__)); + +struct goose_drv_args +{ + struct goose_drv_io io; + struct goose_drv_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void goose_drv(void *args, void *logic); +extern int goose_drv_initlog(void *arguments, void *logic); + +#endif diff --git a/src/goose_out.c b/src/goose_out.c new file mode 100644 index 0000000..b9768dc --- /dev/null +++ b/src/goose_out.c @@ -0,0 +1,281 @@ +/* + * goose_out.c + * + * Created on: 08-02-2020 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "eth.h" +#include "goose.h" +#include "goose_out.h" + +#include + +#include "comm.h" +#include "../ethernet/ports/am1808/include/lwiplib.h" +#include "../ethernet/emac.h" +#include "../ethernet/hw_types.h" +#include "ports/am1808/include/netif/sitaraif.h" +#include +#include +#include + +int goose_out_initlog(void *arguments, void *logic) +{ + struct goose_out_args *args = (struct goose_out_args *)arguments; + struct goose_out_logic *log = (struct goose_out_logic *)logic; + u8 i; + int values[6]; + u8 dstaddr[6]; + struct goose_frame_normal *gfr = (struct goose_frame_normal *)log->goose_out_frame; + struct goose_frame_vlan *gfr_vlan = (struct goose_frame_vlan *)log->goose_out_frame; + struct goose_frame2 *g; + + for(i=0;i<16;i++) + if(set_bit_ptr_struct(args->io.state_in[i],&log->state_in[i])) + return -1; + + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.ndscom_in,&log->ndscom_in)) + return -1; + + log->goose_id=(char*)&args->params.strings_start; + u8 len = strlen(log->goose_id) + 1; + if(len%4) + len+=4-(len%4); + + log->goose_dataset=log->goose_id+len; + len = strlen(log->goose_dataset) + 1; + if(len%4) + len+=4-(len%4); + + log->goose_cbref=log->goose_dataset+len; + len = strlen(log->goose_cbref) + 1; + if(len%4) + len+=4-(len%4); + + log->goose_dstaddr=log->goose_cbref+len; + len = strlen(log->goose_dstaddr) + 1; + if(len%4) + len+=4-(len%4); + + if( 6 == sscanf( log->goose_dstaddr, "%x-%x-%x-%x-%x-%x%*c", + &values[0], &values[1], &values[2], + &values[3], &values[4], &values[5] ) ) + { + for( i = 0; i < 6; ++i ) + dstaddr[i] = (u8) values[i]; + } + else + return -1; + + log->stnum_cur=1; + log->sqnum_cur=1; + + // prepare frame headers + memset(log->goose_out_frame,0,sizeof(log->goose_out_frame)); + memcpy(gfr->dst_mac,dstaddr,6); + memcpy(gfr->src_mac,eth.hwaddr,6); + + if(args->params.bits & GOOSE_OUT_VLAN_EN) + { + gfr_vlan->ethertype=GOOSE_ETHERTYPE_LE; + gfr_vlan->tpid=0x0081; // 0x8100 LE coded + gfr_vlan->tci=((args->params.priority & 0x07)<<5)|((args->params.vlan_no & 0x0F00)>>8)|((args->params.vlan_no & 0x00FF)<<8); + g=(struct goose_frame2 *)&gfr_vlan->goose; + } + else + { + gfr->ethertype=GOOSE_ETHERTYPE_LE; + g=(struct goose_frame2 *)&gfr->goose; + } + + g->appid=htons(args->params.appid); + g->pdu_tag=GOOSE_TAG_PDU; + g->pdu_taglen=0x81; + log->len=11; + + u32 dcnt = 0; + + g->pdu_data[dcnt++]=GOOSE_TAG_GOCBREF; + g->pdu_data[dcnt++]=strlen(log->goose_cbref); + memcpy(&g->pdu_data[dcnt],log->goose_cbref,strlen(log->goose_cbref)); + dcnt+=strlen(log->goose_cbref); + + g->pdu_data[dcnt++]=GOOSE_TAG_TTL; + g->pdu_data[dcnt++]=2; + log->ttl=&g->pdu_data[dcnt]; + *(u16 *)&g->pdu_data[dcnt] = htons(args->params.ttl); + dcnt+=2; + + g->pdu_data[dcnt++]=GOOSE_TAG_DATSET; + g->pdu_data[dcnt++]=strlen(log->goose_dataset); + memcpy(&g->pdu_data[dcnt],log->goose_dataset,strlen(log->goose_dataset)); + dcnt+=strlen(log->goose_dataset); + + g->pdu_data[dcnt++]=GOOSE_TAG_ID; + g->pdu_data[dcnt++]=strlen(log->goose_id); + memcpy(&g->pdu_data[dcnt],log->goose_id,strlen(log->goose_id)); + dcnt+=strlen(log->goose_id); + + g->pdu_data[dcnt++]=GOOSE_TAG_T; + g->pdu_data[dcnt++]=8; + log->tstamp_secs=&g->pdu_data[dcnt]; + + + for(i=0;i<4;i++) + g->pdu_data[dcnt++]=(((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec)>>((3-i)*8)); + + log->tstamp_frac=&g->pdu_data[dcnt]; + u32 stamp = ((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec)*GOOSE_TIMESTAMP_MS_MULTIPLIER; + for(i=0;i<4;i++) + g->pdu_data[dcnt++]=stamp>>((3-i)*8); + + g->pdu_data[dcnt++]=GOOSE_TAG_STNUM; + g->pdu_data[dcnt++]=4; + log->stnum=&g->pdu_data[dcnt]; + + for(i=0;i<4;i++) + g->pdu_data[dcnt++]=(log->stnum_cur>>((3-i)*8)); + + g->pdu_data[dcnt++]=GOOSE_TAG_SQNUM; + g->pdu_data[dcnt++]=4; + + log->sqnum=&g->pdu_data[dcnt]; + for(i=0;i<4;i++) + g->pdu_data[dcnt++]=(log->sqnum_cur>>((3-i)*8)); + + g->pdu_data[dcnt++]=GOOSE_TAG_SIMULATION; + g->pdu_data[dcnt++]=1; + log->simulation=&g->pdu_data[dcnt]; + g->pdu_data[dcnt++]=0; + + g->pdu_data[dcnt++]=GOOSE_TAG_CONFREV; + g->pdu_data[dcnt++]=4; + for(i=0;i<4;i++) + g->pdu_data[dcnt++]=(args->params.cfg_rev>>((3-i)*8)); + + g->pdu_data[dcnt++]=GOOSE_TAG_NDSCOM; + g->pdu_data[dcnt++]=1; + log->ndscom=&g->pdu_data[dcnt]; + g->pdu_data[dcnt++]=0; + + g->pdu_data[dcnt++]=GOOSE_TAG_NUMDATSETENTRIES; + g->pdu_data[dcnt++]=1; + g->pdu_data[dcnt++]=16; + + g->pdu_data[dcnt++]=GOOSE_TAG_ALLDATA; + g->pdu_data[dcnt++]=16*3; // 16 states * 3 bytes each + for(i=0;i<16;i++) + { + g->pdu_data[dcnt++]=GOOSE_DATA_BOOLEAN; + g->pdu_data[dcnt++]=1; + log->state[i]=&g->pdu_data[dcnt]; + g->pdu_data[dcnt++]=0; + } + + g->pdu_len+=dcnt; + log->len+=dcnt; + g->len=htons(log->len); + + + if(log->len>=(GOOSE_MAX_FR_OUT_SIZE-18)) + return -1; + + log->first_packet=1; + + return 0; +} + + +void goose_out(void *arguments, void *logic) +{ + struct goose_out_logic *log = (struct goose_out_logic *)logic; + struct goose_out_args *args = (struct goose_out_args *)arguments; + u8 i; + u32 stamp; + u8 st_changed; + + + st_changed=0; + + for(i=0;i<16;i++) + { + *log->state[i]=check_struct(&log->state_in[i])?1:0; + if(*log->state[i]!=log->state_prev[i]) + { + st_changed=1; + log->state_prev[i]=*log->state[i]; + } + } + + if(st_changed||log->first_packet) + { + log->retrans_cnt=0; + log->retrans_cycle=0; + + stamp=((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec); + for(i=0;i<4;i++) + *(log->tstamp_secs+i)=(stamp>>((3-i)*8)); + + stamp = ((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec)*GOOSE_TIMESTAMP_MS_MULTIPLIER; + for(i=0;i<4;i++) + *(log->tstamp_frac+i)=stamp>>((3-i)*8); + } + + if(!log->retrans_cnt) + { + *log->simulation=check_struct(&log->test)?1:0; + *log->ndscom=check_struct(&log->ndscom_in)?1:0; + + if(st_changed && !log->first_packet) + { + log->stnum_cur++; + log->sqnum_cur=0; + } + + for(i=0;i<4;i++) + *(log->stnum+i)=log->stnum_cur>>((3-i)*8); + + for(i=0;i<4;i++) + *(log->sqnum+i)=log->sqnum_cur>>((3-i)*8); + + log->sqnum_cur++; + + log->retrans_cnt = (2<retrans_cycle); + if(log->retrans_cnt>=(args->params.ttl>>1)) + { + log->retrans_cnt=(args->params.ttl>>1); + *log->ttl = (args->params.ttl)>>8; + *(log->ttl+1) = (args->params.ttl); + } + else + { + *log->ttl = (log->retrans_cnt<<1)>>8; + *(log->ttl+1) = (log->retrans_cnt<<1); + log->retrans_cycle++; + } + + if(args->params.bits & GOOSE_OUT_VLAN_EN) + omapl138EthSendPacket(log->goose_out_frame,(log->len+18)>64?(log->len+18):64); + else + omapl138EthSendPacket(log->goose_out_frame,(log->len+14)>64?(log->len+14):64); + + log->first_packet=0; + + } + + if(log->retrans_cnt>=LOOP_CYCLE_MS) + log->retrans_cnt-=LOOP_CYCLE_MS; + else + log->retrans_cnt=0; + +} diff --git a/src/goose_out.h b/src/goose_out.h new file mode 100644 index 0000000..9d103cf --- /dev/null +++ b/src/goose_out.h @@ -0,0 +1,82 @@ +/* + * goose_out.h + * + * Created on: 08-02-2020 + * Author: Krzysztof Jakubczyk + */ + +#ifndef GOOSE_OUT_H_ +#define GOOSE_OUT_H_ + +#include "tdefs.h" +#include "helper.h" +#include "comm.h" +#include "config.h" + +//goose_out bits +#define GOOSE_OUT_VLAN_EN 0x01 + +#define GOOSE_TIMESTAMP_MS_MULTIPLIER (u32)((float)0xFFFFFFFF/1000.0f) + +struct goose_out_logic +{ + struct binary_io state_in[16]; + struct binary_io ndscom_in; + struct binary_io test; + char *goose_id; + char *goose_dataset; + char *goose_cbref; + char *goose_dstaddr; + char *sv_id; + char *sv_dstaddr; + u8 *tstamp_secs; + u8 *tstamp_frac; + + u8 *ttl; + u8 *stnum; + u8 *sqnum; + u8 *simulation; + u8 *ndscom; + u8 *state[16]; + + u8 state_prev[16]; + u32 stnum_cur; + u32 sqnum_cur; + u8 values; + u32 len; + u32 retrans_cnt; + u32 retrans_cycle; + u8 first_packet; + u8 goose_out_frame[GOOSE_MAX_FR_OUT_SIZE] __attribute__((aligned(4))); +}; + +struct goose_out_io +{ + u32 state_in[16]; + u32 ndscom_in; + u32 test_in; +}__attribute__((__packed__)); + + +struct goose_out_params +{ + u32 bits; + u32 appid; + u32 cfg_rev; + u32 vlan_no; + u32 priority; + u32 ttl; + u32 strings_start; +}__attribute__((__packed__)); + +struct goose_out_args +{ + struct goose_out_io io; + struct goose_out_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void goose_out(void *args, void *logic); +extern int goose_out_initlog(void *arguments, void *logic); + +#endif diff --git a/src/helper.c b/src/helper.c new file mode 100644 index 0000000..35cd87e --- /dev/null +++ b/src/helper.c @@ -0,0 +1,149 @@ +/* + * helper.c + * + * Created on: 21-07-2016 + * Author: KJ + */ + +#include "tdefs.h" +#include "logman.h" +#include "dev_ctrl.h" +#include "helper.h" +#include "config.h" + +enum rej_ost status_rej; + +int set_bit_ptr(u32 io,void *bit_ptr, u8 *bit_no) +{ + u8 *bptr; + + bptr = (u8*)log_manager.nets_data + (io >> 3); + *bit_no = io & 0x07; + + if((u8*)bptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + *(u32 *)bit_ptr=(u32)bptr; + + return 0; +} + +int set_bit_ptr_mask(u32 io,void *bit_ptr, u8 *mask) +{ + u8 *bptr; + + bptr = (u8*)log_manager.nets_data + (io >> 3); + *mask = (1<<(io & 0x07)); + + if((u8*)bptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + *(u32 *)bit_ptr=(u32)bptr; + + return 0; +} + +int set_bit_ptr_struct(u32 io,struct binary_io *b) +{ + b->ptr = (u8*)log_manager.nets_data + (io >> 3); + b->bit_mask = (1<<(io & 0x07)); + + if((u8*)b->ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + return 0; +} + +int set_float_ptr(u32 io,void *float_ptr) +{ + float *fptr; + + fptr = (float *)(log_manager.nets_data + (io >> 3)); + if((u8 *)fptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + *(u32 *)float_ptr=(u32)fptr; + + return 0; +} + +int set_pointer_out_ptr(u32 io, u32 *pointer_ptr) +{ + u32 *f_ptr; + + f_ptr = (u32 *)(log_manager.nets_data + (io >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + *f_ptr = (u32)pointer_ptr; + return 0; +} + +int set_pointer_in_ptr(u32 io, u32 *pointer_ptr) +{ + u32 *f_ptr; + + f_ptr = (u32 *)(log_manager.nets_data + (io >> 3)); + if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if((u32)f_ptr == (u32)&log_manager.nets_data[0]) // if pointer connected to net '0' + *(u32 *)pointer_ptr=(u32)f_ptr; + else + *(u32 *)pointer_ptr=(u32)*f_ptr; + + if(!*f_ptr) + return -1; + + return 0; +} + +void sprawdz_P(u8 *Wy, u8 P, u8 O, short *lp, short iz, short io) +{ + if (*Wy) + { + if (O) *lp +=LOOP_CYCLE_MS; else *lp -= LOOP_CYCLE_MS; + if (*lp < 0) *lp =0; + if (*lp >= io) + { + *lp = 0; + *Wy = 0; + } + } else { + if (P) *lp +=LOOP_CYCLE_MS; else *lp -=LOOP_CYCLE_MS; + if (*lp < 0) *lp =0; + if (*lp >= iz) + { + *Wy = 1; + *lp = 0; + } + } + +} + +void sprawdz_P_100(u8 *Wy, u8 P, u8 O, short *lp, short iz, short io) +{ + if (*Wy) + { + if (O) *lp +=1; else *lp -= 1; + if (*lp < 0) *lp =0; + if (*lp >= io) + { + *lp = 0; + *Wy = 0; + } + } else { + if (P) *lp +=1; else *lp -=1; + if (*lp < 0) *lp =0; + if (*lp >= iz) + { + *Wy = 1; + *lp = 0; + } + } +} + +int czy_test_R() +{ + return (dev_ctrl_state & DEV_CTRL_STATE_TEST_LOG); +} diff --git a/src/helper.h b/src/helper.h new file mode 100644 index 0000000..8bee47f --- /dev/null +++ b/src/helper.h @@ -0,0 +1,85 @@ +/* + * helper.h + * + * Created on: 21-07-2016 + * Author: KJ + */ + +#ifndef HELPER_H_ +#define HELPER_H_ + +struct ortho +{ + float orta; + float ortb; +}; + +struct binary_io +{ + u8 *ptr; + u8 bit_mask; +}; + +enum rej_ost //typ dla stanu rejestratora ostatniego popbudzenia +{ + oczekiwanie, + sledzenie, + sledzenie_zad, + zapis, + blokada, + stan_nieustalony +}; + +extern enum rej_ost status_rej; + +extern void sprawdz_P(u8 *Wy, u8 P, u8 O, short *lp, short iz, short io); +extern void sprawdz_P_100(u8 *Wy, u8 P, u8 O, short *lp, short iz, short io); + +inline void check_and_set(u32 cond, u8 *byte, u8 bit) +{ + if(cond) + *byte|=(1<ptr|=b->bit_mask; + else + *b->ptr&=~b->bit_mask; +} + +inline int check_struct(struct binary_io *b) +{ + return (*b->ptr & b->bit_mask); +} + +inline void set_struct(struct binary_io *b) +{ + *b->ptr|=b->bit_mask; +} + +inline void clear_struct(struct binary_io *b) +{ + *b->ptr&=~b->bit_mask; +} + +extern int czy_test_R(); +extern int set_bit_ptr(u32 io,void *bit_ptr, u8 *bit_no); +extern int set_bit_ptr_mask(u32 io,void *bit_ptr, u8 *mask); +extern int set_bit_ptr_struct(u32 io,struct binary_io *b); +extern int set_float_ptr(u32 io,void *float_ptr); +extern int set_pointer_in_ptr(u32 io, u32 *pointer_ptr); +extern int set_pointer_out_ptr(u32 io, u32 *pointer_ptr); + +#endif /* HELPER_H_ */ diff --git a/src/hr_3ph2h.c b/src/hr_3ph2h.c new file mode 100644 index 0000000..d1cf93f --- /dev/null +++ b/src/hr_3ph2h.c @@ -0,0 +1,151 @@ +/* + * hr_3ph2h.c + * + * Created on: 20-04-2018 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "hr_3ph2h.h" + + +int hr_3ph2h_initlog(void *arguments, void *logic) +{ + struct hr_3ph2h_args *args = (struct hr_3ph2h_args *)arguments; + struct hr_3ph2h_logic *log = (struct hr_3ph2h_logic *)logic; + + if(set_bit_ptr(args->io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.BL_out,&log->BL,&log->BL_bit_no)) + return -1; + + if(set_bit_ptr(args->io.BL1_out,&log->BL1,&log->BL1_bit_no)) + return -1; + + if(set_bit_ptr(args->io.BL2_out,&log->BL2,&log->BL2_bit_no)) + return -1; + + if(set_bit_ptr(args->io.BL3_out,&log->BL3,&log->BL3_bit_no)) + return -1; + + if(set_float_ptr(args->io.I1_float_in,&log->I1)) + return -1; + + if(set_float_ptr(args->io.I2_float_in,&log->I2)) + return -1; + + if(set_float_ptr(args->io.I3_float_in,&log->I3)) + return -1; + + if(set_float_ptr(args->io.I12h_float_in,&log->I12h)) + return -1; + + if(set_float_ptr(args->io.I22h_float_in,&log->I22h)) + return -1; + + if(set_float_ptr(args->io.I32h_float_in,&log->I32h)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0002)?1:0; + log->nast_.or_ = (args->params.bity & 0x0004)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + + log->nast_.I2h = args->params.I2h * args->params.I2h; + log->nast_.I2h_powr = (args->params.kp_ * args->params.I2h) * (args->params.kp_ * args->params.I2h); + + return 0; +} + +void hr_3ph2h(void *arguments, void *logic) +{ + struct hr_3ph2h_logic *log = (struct hr_3ph2h_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob12h, + (*log->I12h>(log->nast_.I2h * *log->I1)), + (*log->I12h<(log->nast_.I2h_powr * *log->I1)), + &log->dw.lp12h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob11h, + *log->I1>log->nast_.limit1_, + *log->I1nast_.limit2_, + &log->dw.lp11h, + 3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob1, + log->dw.pob11h && log->dw.pob12h, + !log->dw.pob11h || !log->dw.pob12h, + &log->dw.lp1, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob22h, + (*log->I22h>(log->nast_.I2h * *log->I2)), + (*log->I22h<(log->nast_.I2h_powr * *log->I2)), + &log->dw.lp22h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob21h, + *log->I2>log->nast_.limit1_, + *log->I2nast_.limit2_, + &log->dw.lp21h, + 3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob2, + log->dw.pob21h && log->dw.pob22h, + !log->dw.pob21h || !log->dw.pob22h, + &log->dw.lp2, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob32h, + (*log->I32h>(log->nast_.I2h * *log->I3)), + (*log->I32h<(log->nast_.I2h_powr * *log->I3)), + &log->dw.lp32h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob31h, + *log->I3>log->nast_.limit1_, + *log->I3nast_.limit2_, + &log->dw.lp31h, + 3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob3, + log->dw.pob31h && log->dw.pob32h, + !log->dw.pob31h || !log->dw.pob32h, + &log->dw.lp3, + 1,1); //realizacja przekaznika + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*| czy_start()*/) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0; + } + // + + if(log->nast_.or_) // zaleznie od nastawienia logiki pobudzen "and" lub "or" + log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or" + else + log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and" + + check_and_set(log->dw.pob,log->BL,log->BL_bit_no); + check_and_set(log->dw.pob1,log->BL1,log->BL1_bit_no); + check_and_set(log->dw.pob2,log->BL2,log->BL2_bit_no); + check_and_set(log->dw.pob3,log->BL3,log->BL3_bit_no); + } + else + { + *log->BL&=~(1<BL_bit_no); + *log->BL1&=~(1<BL1_bit_no); + *log->BL2&=~(1<BL2_bit_no); + *log->BL3&=~(1<BL3_bit_no); + } +} diff --git a/src/hr_3ph2h.h b/src/hr_3ph2h.h new file mode 100644 index 0000000..2d6b71c --- /dev/null +++ b/src/hr_3ph2h.h @@ -0,0 +1,127 @@ +/* + * hr_3ph2h.h + * + * Created on: 20-04-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef HR_3PH2H_H_ +#define HR_3PH2H_H_ + +#include "tdefs.h" + + struct Nastawy_przeliczone_hr3ph2h + { + float limit1_; ///< wartosc rozruchowa + float limit2_; ///< wartosc powrotu + u32 t_; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 w_; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 or_; ///< logika pobudzen "0" oznacza dzialanie w trybie "and", a "1" w trybie "or" + float I2h; + float I2h_powr; + float Ibezwar; + float Ibezwar_powr; + }; + + struct dane_wewnetrzne_hr3ph2h + { +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + u32 dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + float max1_; // maksymalna watosc wejscia 1 (rejestr tymczasowy - przed zapisem) + float max2_; // maksymalna watosc wejscia 2 (rejestr tymczasowy - przed zapisem) + float max3_; // maksymalna watosc wejscia 3 (rejestr tymczasowy - przed zapisem) + short lp1; /// + +int i_fail_initlog(void *arguments, void *logic) +{ + struct i_fail_args *args = (struct i_fail_args *)arguments; + struct i_fail_logic *log = (struct i_fail_logic *)logic; + + log->test = (u8 *)(log_manager.nets_data + (args->io.test_in >> 3)); + if((u8 *)log->test > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->test_bit = (args->io.test_in & 0x07); + + log->I1S0a = (float *)(log_manager.nets_data + (args->io.I1S0a_float_in >> 3)); + if((u8 *)log->I1S0a > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1S0b = (float *)(log_manager.nets_data + (args->io.I1S0b_float_in >> 3)); + if((u8 *)log->I1S0b > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1S0 = (float *)(log_manager.nets_data + (args->io.I1S0_float_in >> 3)); + if((u8 *)log->I1S0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->Irefa = (float *)(log_manager.nets_data + (args->io.Irefa_float_in >> 3)); + if((u8 *)log->Irefa > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->Irefb = (float *)(log_manager.nets_data + (args->io.Irefb_float_in >> 3)); + if((u8 *)log->Irefb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->Iref = (float *)(log_manager.nets_data + (args->io.Iref_float_in >> 3)); + if((u8 *)log->Iref > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1L1 = (float *)(log_manager.nets_data + (args->io.I1L1_float_in >> 3)); + if((u8 *)log->I1L1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1L2 = (float *)(log_manager.nets_data + (args->io.I1L2_float_in >> 3)); + if((u8 *)log->I1L2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1L3 = (float *)(log_manager.nets_data + (args->io.I1L3_float_in >> 3)); + if((u8 *)log->I1L3 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U1S0 = (float *)(log_manager.nets_data + (args->io.U1S0_float_in >> 3)); + if((u8 *)log->U1S0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->stan_bl = (u8 *)(log_manager.nets_data + (args->io.stan_bl_in >> 3)); + if((u8 *)log->stan_bl > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->stan_bl_bit = (args->io.stan_bl_in & 0x07); + + log->Bl_Ifail = (u8 *)(log_manager.nets_data + (args->io.Bl_Ifail_out >> 3)); + if((u8 *)log->Bl_Ifail > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->Bl_Ifail_bit = (args->io.Bl_Ifail_out & 0x07); + + log->Ifailsyg = (u8 *)(log_manager.nets_data + (args->io.Ifailsyg_out >> 3)); + if((u8 *)log->Ifailsyg > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->Ifailsyg_bit = (args->io.Ifailsyg_out & 0x07); + + log->nast_.on = args->params.bity & 0x0001; + log->nast_.WyBl_ON = args->params.bity & 0x002; + log->nast_.Idbl_ON = args->params.bity & 0x004; + log->nast_.Udbl_ON = args->params.bity & 0x008; + log->nast_.bl = args->params.bity & 0x0010; + + log->nast_.Imin = args->params.Imin * args->params.Imin * 2; + log->nast_.Ikh = args->params.Ikh * args->params.Ikh * 2; + log->nast_.ksch = args->params.ksch; + log->nast_.ksch2 = args->params.ksch * args->params.ksch * 2; + + + log->nast_.Idbl = args->params.Idbl * args->params.Idbl; + log->nast_.Udbl = args->params.Udbl * args->params.Udbl; + + + log->nast_.tz = (uint16_t)(100 * (args->params.tdbl-0.005f)); + + return 0; +} + +void i_fail(void *arguments, void *logic) +{ + struct i_fail_logic *log = (struct i_fail_logic *)logic; + + u8 local, local2; + float tempa,tempb,max; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + tempa = *log->I1S0a - log->nast_.ksch * *log->Irefa; + tempb = *log->I1S0b - log->nast_.ksch * *log->Irefb; + + max = *log->I1L1; + if (max < *log->I1L2) max = *log->I1L2; + if (max < *log->I1L3) max = *log->I1L3; + + log->dw.Irr = tempa * tempa + tempb * tempb; + local = (log->dw.Irr > log->nast_.Imin) && (log->dw.Irr > log->nast_.Ikh * max); + local2 = (log->dw.Irr < (0.96 * log->nast_.Imin)) || (log->dw.Irr < (0.96 + * log->nast_.Ikh * max)); + + //sprawdzenie kryterium rznicowego + sprawdz_P( + &log->dw.PIrr, + local, + local2, + &log->dw.licz_Irr, + 3,3 + ); + + + + //sprawdzenie deblokady prdowej + if (log->nast_.Idbl_ON) + { + local = max > log->nast_.Idbl; + sprawdz_P( + &log->dw.PIdbl, + local, + !local, + &log->dw.licz_Idbl, + 3,10 + ); + } else log->dw.PIdbl = 0; + + //sprawdzenie deblokady napicia zerowego + if (log->nast_.Udbl_ON) + { + local = *log->U1S0 > log->nast_.Udbl; + sprawdz_P( + &log->dw.PUdbl, + local, + !local, + &log->dw.licz_Udbl, + 3,10 + ); + } else log->dw.PUdbl = 0; + + + //ustawienie wyjcia + + if (*log->Ifailsyg & (1<Ifailsyg_bit)) + { + log->dw.P = log->dw.PIrr; + } else { + log->dw.P = log->dw.PIrr & !log->dw.PIdbl & !log->dw.PUdbl; + } + + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.P = (*log->test & (1<test_bit))?1:0; + } + // + + + // realizacja blokady dzialania przekaznika + if(log->nast_.bl && (*log->stan_bl & (1<stan_bl_bit))) { + log->dw.P = 0; + } + // + + if(log->nast_.WyBl_ON) + { + if(log->dw.P) + *log->Bl_Ifail|=(1<Bl_Ifail_bit); + else + *log->Bl_Ifail&=~(1<Bl_Ifail_bit); + } else *log->Bl_Ifail&=~(1<Bl_Ifail_bit); + + + //---------------------------------- + + + } else { + *log->Bl_Ifail&=~(1<Bl_Ifail_bit); + } +} + +void i_fail_100hz(void *arguments, void *logic) +{ + struct i_fail_logic *log = (struct i_fail_logic *)logic; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + //realizacja przekaznika czasowego + if (*log->Bl_Ifail & (1<Bl_Ifail_bit)) + { + log->dw.liczt++; + if (log->dw.liczt>log->nast_.tz) + { + log->dw.Syg_Ifail = 1; + log->dw.liczt--; + } + } else { + log->dw.Syg_Ifail = 0; + log->dw.liczt = 0; + } + + + + + if(log->dw.Syg_Ifail) + *log->Ifailsyg|=(1<Ifailsyg_bit); + else + *log->Ifailsyg&=~(1<Ifailsyg_bit); + } else { + *log->Ifailsyg&=~(1<Ifailsyg_bit); + } +} + +void i_fail_20hz(void *arguments, void *logic) +{ + struct i_fail_logic *log = (struct i_fail_logic *)logic; + + if(!(*log->Bl_Ifail & (1<Bl_Ifail_bit)) && log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + { +// log->dw.R.Irr = sqrt(log->dw.RIrr / 2); +// log->dw.R.I1L1 = sqrt(log->dw.RI1L1); +// log->dw.R.I1L2 = sqrt(log->dw.RI1L2); +// log->dw.R.I1L3 = sqrt(log->dw.RI1L3); +// log->dw.R.Iref = sqrt(log->dw.RIref); +// log->dw.R.I1S0 = sqrt(log->dw.RI1S0); +// log->dw.R.US0 = sqrt(log->dw.RUS0); +// log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia +// log->dw.R.dlugosc = (float)log->dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia +// log->dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + } +} diff --git a/src/i_fail.h b/src/i_fail.h new file mode 100644 index 0000000..8e3cd65 --- /dev/null +++ b/src/i_fail.h @@ -0,0 +1,132 @@ +/* + * i_fail.h + * + * Created on: 03-08-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef I_FAIL_H_ +#define I_FAIL_H_ + +#include "tdefs.h" + +struct Nastawy_przeliczone_i_fail + { + float Imin;/// + +int kier_pr_initlog(void *arguments, void *logic) +{ + struct kier_pr_args *args = (struct kier_pr_args *)arguments; + struct kier_pr_logic *log = (struct kier_pr_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_float_ptr(args->io.UL1_orta_float_in,&log->UL_orta[0])) + return -1; + if(set_float_ptr(args->io.UL1_ortb_float_in,&log->UL_ortb[0])) + return -1; + if(set_float_ptr(args->io.UL1_esk_float_in,&log->UL_esk[0])) + return -1; + if(set_float_ptr(args->io.UL2_orta_float_in,&log->UL_orta[1])) + return -1; + if(set_float_ptr(args->io.UL2_ortb_float_in,&log->UL_ortb[1])) + return -1; + if(set_float_ptr(args->io.UL2_esk_float_in,&log->UL_esk[1])) + return -1; + if(set_float_ptr(args->io.UL3_orta_float_in,&log->UL_orta[2])) + return -1; + if(set_float_ptr(args->io.UL3_ortb_float_in,&log->UL_ortb[2])) + return -1; + if(set_float_ptr(args->io.UL3_esk_float_in,&log->UL_esk[2])) + return -1; + + if(set_float_ptr(args->io.IL1_orta_float_in,&log->IL_orta[0])) + return -1; + if(set_float_ptr(args->io.IL1_ortb_float_in,&log->IL_ortb[0])) + return -1; + if(set_float_ptr(args->io.IL1_esk_float_in,&log->IL_esk[0])) + return -1; + if(set_float_ptr(args->io.IL2_orta_float_in,&log->IL_orta[1])) + return -1; + if(set_float_ptr(args->io.IL2_ortb_float_in,&log->IL_ortb[1])) + return -1; + if(set_float_ptr(args->io.IL2_esk_float_in,&log->IL_esk[1])) + return -1; + if(set_float_ptr(args->io.IL3_orta_float_in,&log->IL_orta[2])) + return -1; + if(set_float_ptr(args->io.IL3_ortb_float_in,&log->IL_ortb[2])) + return -1; + if(set_float_ptr(args->io.IL3_esk_float_in,&log->IL_esk[2])) + return -1; + + if(set_bit_ptr_struct(args->io.p3f_out,&log->P3F)) + return -1; + if(set_bit_ptr_struct(args->io.pl1_out,&log->PL1)) + return -1; + if(set_bit_ptr_struct(args->io.pl2_out,&log->PL2)) + return -1; + if(set_bit_ptr_struct(args->io.pl3_out,&log->PL3)) + return -1; + + if(set_bit_ptr_struct(args->io.w3f_out,&log->W3F)) + return -1; + if(set_bit_ptr_struct(args->io.wl1_out,&log->WL1)) + return -1; + if(set_bit_ptr_struct(args->io.wl2_out,&log->WL2)) + return -1; + if(set_bit_ptr_struct(args->io.wl3_out,&log->WL3)) + return -1; + + if(set_bit_ptr_struct(args->io.z3f_out,&log->Z3F)) + return -1; + if(set_bit_ptr_struct(args->io.zl1_out,&log->ZL1)) + return -1; + if(set_bit_ptr_struct(args->io.zl2_out,&log->ZL2)) + return -1; + if(set_bit_ptr_struct(args->io.zl3_out,&log->ZL3)) + return -1; + + log->umin = args->params.U_min * args->params.U_min; + log->umin_hyst = log->umin * args->params.kp_ui * args->params.kp_ui; + + log->imin = args->params.I_min * args->params.I_min; + log->imin_hyst = log->imin * args->params.kp_ui * args->params.kp_ui; + + log->imax = args->params.I_max * args->params.I_max; + log->imax_hyst = log->imax * (1/args->params.kp_ui) * (1/args->params.kp_ui); + + log->phi_char_360 = (args->params.phi_char<0)?360+args->params.phi_char:args->params.phi_char; + log->phi_min = log->phi_char_360 - args->params.phi_dev; + log->phi_max = log->phi_char_360 + args->params.phi_dev; + log->phi_min=log->phi_min<0?(360+log->phi_min):log->phi_min; + if(log->phi_min>log->phi_max) + log->phi_ov=1; + + log->phi_min_hyst = log->phi_char_360 - args->params.phi_dev - args->params.kp_phi; + log->phi_max_hyst = log->phi_char_360 + args->params.phi_dev + args->params.kp_phi; + log->phi_min_hyst = log->phi_min_hyst<0?(360+log->phi_min_hyst):log->phi_min_hyst; + + if(log->phi_min_hyst>log->phi_max_hyst) + log->phi_hyst_ov=1; + + log->td=(u32)(args->params.td*1000); + if(!log->td) + log->td=1; + if(log->td>32767) + log->td=32767; + + return 0; +} + +void kier_pr(void *arguments, void *logic) +{ + struct kier_pr_logic *log = (struct kier_pr_logic *)logic; + struct kier_pr_args *args = (struct kier_pr_args *)arguments; + u8 i; + float phi,phi1,phi2; + + if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked + { + for(i=0;i<3;i++) + { + sprawdz_P(&log->dw.pob_i[i], + *log->IL_esk[i]>log->imin&&*log->IL_esk[i]imax, + *log->IL_esk[i]imin_hyst||*log->IL_esk[i]>log->imax_hyst, + &log->dw.lp_i[i], + 5,3); + sprawdz_P(&log->dw.pob_u[i], + *log->UL_esk[i]>log->umin, + *log->UL_esk[i]umin_hyst, + &log->dw.lp_u[i], + 5,3); + + phi1=get_phase(*log->IL_ortb[i],*log->IL_orta[i]); + phi2=get_phase(*log->UL_ortb[i],*log->UL_orta[i]); + + phi=get_phase_diff(phi1,phi2); + + phi=phi<0?(360+phi):phi; + + + if(log->dw.pob_u[i]&&log->dw.pob_i[i]) + { + if(log->phi_max_hyst>=360 && phi<=(log->phi_max_hyst-360)) + phi+=360; + + if (args->params.bity & 0x00000010) + log->dw.pob_phi[i] = 1; + else + { + + sprawdz_P(&log->dw.pob_phi[i], + log->phi_ov?(phi>log->phi_min||phiphi_max):(phi>log->phi_min && phiphi_max), + log->phi_hyst_ov?(phiphi_min_hyst&&phi>log->phi_max_hyst):(phiphi_min_hyst || phi>log->phi_max_hyst), + //(log->phi_min_ov ? (phiphi_min_hyst && phi>log->phi_max_hyst) : phiphi_min_hyst) || (log->phi_max_ov ? phiphi_max_hyst : log->phi_min_ov? (phi>log->phi_max_hyst && phiphi_min_hyst): phi>log->phi_max_hyst ), + &log->dw.lp_phi[i], + 20,10); + } + } + else + { + log->dw.lp_phi[i]=0; + log->dw.pob_phi[i]=0; + } + + if(log->dw.pob_phi[i]) + log->pob[i]=1; + else + log->pob[i]=0; + + sprawdz_P(&log->dw.zadz[i], + log->pob[i], + !log->pob[i], + &log->dw.lp[i],log->td,1); + } + + if(args->params.bity & 0x00000004) // OR logic + { + log->pob[3]=log->pob[0]||log->pob[1]||log->pob[2]; + log->dw.zadz[3]=log->dw.zadz[0]||log->dw.zadz[1]||log->dw.zadz[2]; + } + else + { + log->pob[3]=log->pob[0]&&log->pob[1]&&log->pob[2]; + log->dw.zadz[3]=log->dw.zadz[0]&&log->dw.zadz[1]&&log->dw.zadz[2]; + } + + if(czy_test_R() && check_struct(&log->test)) + { + log->pob[0]=1; + log->pob[1]=1; + log->pob[2]=1; + log->pob[3]=1; + log->dw.zadz[0]=1; + log->dw.zadz[1]=1; + log->dw.zadz[2]=1; + log->dw.zadz[3]=1; + } + + check_and_set_struct(log->pob[0],&log->PL1); + check_and_set_struct(log->pob[1],&log->PL2); + check_and_set_struct(log->pob[2],&log->PL3); + check_and_set_struct(log->pob[3],&log->P3F); + check_and_set_struct(log->dw.zadz[0],&log->ZL1); + check_and_set_struct(log->dw.zadz[1],&log->ZL2); + check_and_set_struct(log->dw.zadz[2],&log->ZL3); + check_and_set_struct(log->dw.zadz[3],&log->Z3F); + + if(args->params.bity & 0x00000008) // na wylacz + { + check_and_set_struct(log->dw.zadz[0],&log->WL1); + check_and_set_struct(log->dw.zadz[1],&log->WL2); + check_and_set_struct(log->dw.zadz[2],&log->WL3); + check_and_set_struct(log->dw.zadz[3],&log->W3F); + } + + } + else + { + clear_struct(&log->P3F); + clear_struct(&log->PL1); + clear_struct(&log->PL2); + clear_struct(&log->PL3); + clear_struct(&log->Z3F); + clear_struct(&log->ZL1); + clear_struct(&log->ZL2); + clear_struct(&log->ZL3); + clear_struct(&log->W3F); + clear_struct(&log->WL1); + clear_struct(&log->WL2); + clear_struct(&log->WL3); + } +} diff --git a/src/kier_pr.c_ b/src/kier_pr.c_ new file mode 100644 index 0000000..cf315ed --- /dev/null +++ b/src/kier_pr.c_ @@ -0,0 +1,226 @@ +/* + * kier_pr.c + * + * Created on: 27-02-2018 + * Author: Krzysztof Jakubczyk + */ + +#include "../tdefs.h" +#include "../misc.h" + +#include "kier_pr.h" +#include "e_phi.h" +#include "helper.h" +#include + +int kier_pr_initlog(void *arguments, void *logic) +{ + struct kier_pr_args *args = (struct kier_pr_args *)arguments; + struct kier_pr_logic *log = (struct kier_pr_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_float_ptr(args->io.UL1_orta_float_in,&log->UL_orta[0])) + return -1; + if(set_float_ptr(args->io.UL1_ortb_float_in,&log->UL_ortb[0])) + return -1; + if(set_float_ptr(args->io.UL1_esk_float_in,&log->UL_esk[0])) + return -1; + if(set_float_ptr(args->io.UL2_orta_float_in,&log->UL_orta[1])) + return -1; + if(set_float_ptr(args->io.UL2_ortb_float_in,&log->UL_ortb[1])) + return -1; + if(set_float_ptr(args->io.UL2_esk_float_in,&log->UL_esk[1])) + return -1; + if(set_float_ptr(args->io.UL3_orta_float_in,&log->UL_orta[2])) + return -1; + if(set_float_ptr(args->io.UL3_ortb_float_in,&log->UL_ortb[2])) + return -1; + if(set_float_ptr(args->io.UL3_esk_float_in,&log->UL_esk[2])) + return -1; + + if(set_float_ptr(args->io.IL1_orta_float_in,&log->IL_orta[0])) + return -1; + if(set_float_ptr(args->io.IL1_ortb_float_in,&log->IL_ortb[0])) + return -1; + if(set_float_ptr(args->io.IL1_esk_float_in,&log->IL_esk[0])) + return -1; + if(set_float_ptr(args->io.IL2_orta_float_in,&log->IL_orta[1])) + return -1; + if(set_float_ptr(args->io.IL2_ortb_float_in,&log->IL_ortb[1])) + return -1; + if(set_float_ptr(args->io.IL2_esk_float_in,&log->IL_esk[1])) + return -1; + if(set_float_ptr(args->io.IL3_orta_float_in,&log->IL_orta[2])) + return -1; + if(set_float_ptr(args->io.IL3_ortb_float_in,&log->IL_ortb[2])) + return -1; + if(set_float_ptr(args->io.IL3_esk_float_in,&log->IL_esk[2])) + return -1; + + if(set_bit_ptr_struct(args->io.p3f_out,&log->P3F)) + return -1; + if(set_bit_ptr_struct(args->io.pl1_out,&log->PL1)) + return -1; + if(set_bit_ptr_struct(args->io.pl2_out,&log->PL2)) + return -1; + if(set_bit_ptr_struct(args->io.pl3_out,&log->PL3)) + return -1; + + if(set_bit_ptr_struct(args->io.w3f_out,&log->W3F)) + return -1; + if(set_bit_ptr_struct(args->io.wl1_out,&log->WL1)) + return -1; + if(set_bit_ptr_struct(args->io.wl2_out,&log->WL2)) + return -1; + if(set_bit_ptr_struct(args->io.wl3_out,&log->WL3)) + return -1; + + if(set_bit_ptr_struct(args->io.z3f_out,&log->Z3F)) + return -1; + if(set_bit_ptr_struct(args->io.zl1_out,&log->ZL1)) + return -1; + if(set_bit_ptr_struct(args->io.zl2_out,&log->ZL2)) + return -1; + if(set_bit_ptr_struct(args->io.zl3_out,&log->ZL3)) + return -1; + + log->umin = args->params.U_min * args->params.U_min; + log->umin_hyst = log->umin * args->params.kp_ui * args->params.kp_ui; + + log->imin = args->params.I_min * args->params.I_min; + log->imin_hyst = log->imin * args->params.kp_ui * args->params.kp_ui; + + log->imax = args->params.I_max * args->params.I_max; + log->imax_hyst = log->imax * (1/args->params.kp_ui) * (1/args->params.kp_ui); + + log->phi_min_hyst = args->params.phi_min - args->params.kp_phi; + + if(log->phi_min_hyst<-180) + { + log->phi_min_hyst=180+(180+log->phi_min_hyst); + log->phi_min_ov=1; + } + + log->phi_max_hyst = args->params.phi_max + args->params.kp_phi; + if(log->phi_max_hyst>180) + { + log->phi_max_hyst=-180+(log->phi_max_hyst-180); + log->phi_max_ov=1; + } + + log->td=(u32)(args->params.td*1000); + if(!log->td) + log->td=1; + if(log->td>32767) + log->td=32767; + + return 0; +} + +void kier_pr(void *arguments, void *logic) +{ + struct kier_pr_logic *log = (struct kier_pr_logic *)logic; + struct kier_pr_args *args = (struct kier_pr_args *)arguments; + u8 i; + float phi,phi1,phi2; + + if((args->params.bity & 0x00000001) && !((args->params.bity & 0x00000002) && check_struct(&log->stan_bl))) // ON && unblocked + { + for(i=0;i<3;i++) + { + sprawdz_P(&log->dw.pob_i[i], + *log->IL_esk[i]>log->imin&&*log->IL_esk[i]imax, + *log->IL_esk[i]imin_hyst||*log->IL_esk[i]>log->imax_hyst, + &log->dw.lp_i[i], + 12,3); + sprawdz_P(&log->dw.pob_u[i], + *log->UL_esk[i]>log->umin, + *log->UL_esk[i]umin_hyst, + &log->dw.lp_u[i], + 12,3); + + phi1=get_phase(*log->IL_ortb[i],*log->IL_orta[i]); + phi2=get_phase(*log->UL_ortb[i],*log->UL_orta[i]); + + phi=get_phase_diff(phi1,phi2); + + sprawdz_P(&log->dw.pob_phi[i], + phi>args->params.phi_min && phiparams.phi_max, + //phiphi_min_hyst || phi>log->phi_max_hyst, + (log->phi_min_ov ? (phiphi_min_hyst && phi>log->phi_max_hyst):phiphi_min_hyst) + || (log->phi_max_ov ? phiphi_max_hyst:log->phi_min_ov?(phi>log->phi_max_hyst && phiphi_min_hyst):phi>log->phi_max_hyst), + &log->dw.lp_phi[i], + 20,10); + + if(log->dw.pob_phi[i] && log->dw.pob_i[i] && log->dw.pob_u[i]) + log->pob[i]=1; + else + log->pob[i]=0; + + sprawdz_P(&log->dw.zadz[i], + log->pob[i], + !log->pob[i], + &log->dw.lp[i],log->td,1); + } + + if(args->params.bity & 0x00000004) // OR logic + { + log->pob[3]=log->pob[0]||log->pob[1]||log->pob[2]; + log->dw.zadz[3]=log->dw.zadz[0]||log->dw.zadz[1]||log->dw.zadz[2]; + } + else + { + log->pob[3]=log->pob[0]&&log->pob[1]&&log->pob[2]; + log->dw.zadz[3]=log->dw.zadz[0]&&log->dw.zadz[1]&&log->dw.zadz[2]; + } + + if(check_struct(&log->test)) + { + log->pob[0]=1; + log->pob[1]=1; + log->pob[2]=1; + log->pob[3]=1; + log->dw.zadz[0]=1; + log->dw.zadz[1]=1; + log->dw.zadz[2]=1; + log->dw.zadz[3]=1; + } + + check_and_set_struct(log->pob[0],&log->PL1); + check_and_set_struct(log->pob[1],&log->PL2); + check_and_set_struct(log->pob[2],&log->PL3); + check_and_set_struct(log->pob[3],&log->P3F); + check_and_set_struct(log->dw.zadz[0],&log->ZL1); + check_and_set_struct(log->dw.zadz[1],&log->ZL2); + check_and_set_struct(log->dw.zadz[2],&log->ZL3); + check_and_set_struct(log->dw.zadz[3],&log->Z3F); + + if(args->params.bity & 0x00000008) // na wylacz + { + check_and_set_struct(log->dw.zadz[0],&log->WL1); + check_and_set_struct(log->dw.zadz[1],&log->WL2); + check_and_set_struct(log->dw.zadz[2],&log->WL3); + check_and_set_struct(log->dw.zadz[3],&log->W3F); + } + + } + else + { + clear_struct(&log->P3F); + clear_struct(&log->PL1); + clear_struct(&log->PL2); + clear_struct(&log->PL3); + clear_struct(&log->Z3F); + clear_struct(&log->ZL1); + clear_struct(&log->ZL2); + clear_struct(&log->ZL3); + clear_struct(&log->W3F); + clear_struct(&log->WL1); + clear_struct(&log->WL2); + clear_struct(&log->WL3); + } +} diff --git a/src/kier_pr.h b/src/kier_pr.h new file mode 100644 index 0000000..1ae79b6 --- /dev/null +++ b/src/kier_pr.h @@ -0,0 +1,134 @@ +/* + * kier_pr.h + * + * Created on: 27-02-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef E_KIER_PR_H_ +#define E_KIER_PR_H_ + +#include "tdefs.h" +#include "helper.h" + +struct dane_wewnetrzne_kier_pr +{ + short lp[3]; ///ptr = log_manager.nets_data + (args->io.out >> 3); + log->bit_no = args->io.out & 0x07; + + if(log->ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void klapacz(void *arguments, void *logic) +{ + struct klapacz_args *args = (struct klapacz_args *)arguments; + struct klapacz_logic *log = (struct klapacz_logic *)logic; + + if(args->io.out) + { + if(*log->ptr & (1<bit_no) && log->t_cnt>=args->params.interval1) + { + log->t_cnt=0; + *log->ptr &= ~(1<bit_no); + } + else if(!(*log->ptr & (1<bit_no)) && log->t_cnt>=args->params.interval2) + { + log->t_cnt=0; + *log->ptr |= (1<bit_no); + } + + log->t_cnt+=LOOP_CYCLE_MS; + } +} diff --git a/src/klapacz.h b/src/klapacz.h new file mode 100644 index 0000000..1bd62d0 --- /dev/null +++ b/src/klapacz.h @@ -0,0 +1,41 @@ +/* + * klapacz.h + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef KLAPACZ_H_ +#define KLAPACZ_H_ + +#include "tdefs.h" + +struct klapacz_logic +{ + u8 *ptr; + u8 bit_no; + u32 t_cnt; +}; + +struct klapacz_io +{ + u32 out; // one bit +}__attribute__((__packed__)); + +struct klapacz_params +{ + u32 interval1; + u32 interval2; +}__attribute__((__packed__)); + +struct klapacz_args +{ + struct klapacz_io io; + struct klapacz_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void klapacz(void *args, void *logic); +extern int klapacz_initlog(void *args, void *logic); + +#endif /* KLAPACZ_H_ */ diff --git a/src/l_comm.c b/src/l_comm.c new file mode 100644 index 0000000..8b43631 --- /dev/null +++ b/src/l_comm.c @@ -0,0 +1,317 @@ +/* + * l_comm.c + * + * Created on: 28-11-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "l_comm.h" +#include "helper.h" +#include "ZDistL.h" +#include + + +int l_comm_initlog(void *arguments, void *logic) +{ + struct l_comm_args *args = (struct l_comm_args *)arguments; + struct l_comm_logic *log = (struct l_comm_logic *)logic; + u32 *ptr; + + log->stan_bl = (u8 *)(log_manager.nets_data + (args->io.stan_bl_in >> 3)); + if((u8 *)log->stan_bl > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + log->stan_bl_bit_no = (args->io.stan_bl_in & 0x07); + + log->ok = (u8 *)(log_manager.nets_data + (args->io.ok_in >> 3)); + if((u8 *)log->ok > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + log->ok_bit_no = (args->io.ok_in & 0x07); + + log->odb = (u8 *)(log_manager.nets_data + (args->io.odb_in >> 3)); + if((u8 *)log->odb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + log->odb_bit_no = (args->io.odb_in & 0x07); + + log->U1 = (float *)(log_manager.nets_data + (args->io.U1_float_in >> 3)); + if((u8 *)log->U1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U2 = (float *)(log_manager.nets_data + (args->io.U2_float_in >> 3)); + if((u8 *)log->U2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U3 = (float *)(log_manager.nets_data + (args->io.U3_float_in >> 3)); + if((u8 *)log->U3 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->P_Io1 = (u8 *)(log_manager.nets_data + (args->io.P_Io1_in >> 3)); + if((u8 *)log->P_Io1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + log->P_Io1_bit_no = (args->io.P_Io1_in & 0x07); + + log->test = (u8 *)(log_manager.nets_data + (args->io.test_in >> 3)); + if((u8 *)log->test > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + log->test_bit_no = (args->io.test_in & 0x07); + + log->W = (u8 *)(log_manager.nets_data + (args->io.W_out >> 3)); + if((u8 *)log->W > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + log->W_bit_no = (args->io.W_out & 0x07); + + log->NRL = (u8 *)(log_manager.nets_data + (args->io.NRL_out >> 3)); + if((u8 *)log->NRL > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + log->NRL_bit_no = (args->io.NRL_out & 0x07); + + ptr = (u32 *)(log_manager.nets_data + (args->io.We_ZdistL_ptr_in >> 3)); + if((u8*)ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + log->We_ZdistL_ptr=(void *)*ptr; + + ptr = (u32 *)(log_manager.nets_data + (args->io.Stan_L_ptr_out >> 3)); + if((u8*)ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + *ptr = (u32)&log->dw.stan; + + log->nast_.On = (args->params.bity & 0x0001)?1:0; + log->nast_.bl = (args->params.bity & 0x0002)?1:0; + log->nast_.CRL_ON = (args->params.bity & 0x0004)?1:0; + log->nast_.ECHO_ON = (args->params.bity & 0x0008)?1:0; + log->nast_.WD_SPZ_ON = (args->params.bity & 0x0010)?1:0; + log->nast_.Echo_SPZ_ON = (args->params.bity & 0x0020)?1:0;; + + + log->nast_.Typ = args->params.Typ; + + + log->nast_.t_Nad = (int)((args->params.t_Nad + 0.005f) * 100); + log->nast_.t_CRL = (int)(args->params.t_CRL * 1000); + log->nast_.t_W = (int)((args->params.t_W - 0.005f) * 100); + if(log->nast_.t_W) + log->nast_.Wbz = 0; + else + log->nast_.Wbz = 1; + log->nast_.Ur = args->params.Ur * args->params.Ur; + log->nast_.Ur = (1.03 * args->params.Ur) * (1.03 * args->params.Ur); + +// log->nast_.WeZ0.Zdist_dw = &nast_.dw; +// if (nast_.adr.WE_Zdist) nast_.WeZ = (R_ZdistA::Wyjscia *)wskazniki[nast_.adr.WE_Zdist]; +// else nast_.WeZ = &nast_.WeZ0; + + log->nast_.ECHO_ON &= (args->params.Typ != 1); // funkcja echa nie dziaa kiedy jest typ blokujcy + + + if (args->params.Typ==2) args->params.Odb = 10; + log->nast_.Com_ON[0] = (args->params.Odb != 0); + log->nast_.Com_ON[1] = (args->params.Odb != 1); + log->nast_.Com_ON[2] = (args->params.Odb != 2); + log->nast_.Com_ON[3] = (args->params.Odb != 3); + log->nast_.Com_ON[4] = (args->params.Odb != 4); + log->nast_.Com_ON[5] = (args->params.Odb != 5); + log->nast_.Com_ON[6] = (args->params.Odb != 6); + + //log->nast_.nad = &dane_binarne[0]; + + switch(args->params.Nad) + { + case 0: +// if (nast_.adr.WE_Zdist) nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[0]); + break; + case 1: +// if (nast_.adr.WE_Zdist) nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[1]); + break; + case 2: +// if (nast_.adr.WE_Zdist) nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[2]); + break; + case 3: +// if (nast_.adr.WE_Zdist) nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[3]); + break; + case 4: +// if (nast_.adr.WE_Zdist) nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[4]); + break; + case 5: +// if (nast_.adr.WE_Zdist) nast_.nad = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[5]); + break; + case 6: + /*if (log->nast_.adr.P_Io1)*/ log->nast_.nad = log->P_Io1;//&dane_binarne[nast_.adr.P_Io1]; + break; + } + + + //nast_.Str_rev = &dane_binarne[0]; + + switch(args->params.Str_rev) + { + case 0: + //if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[0]); + break; + case 1: + //if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[1]); + break; + case 2: + //if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[2]); + break; + case 3: + //if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[3]); + break; + case 4: + //if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[4]); + break; + case 5: + //if (nast_.adr.WE_Zdist) nast_.Str_rev = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->Pzf[5]); + break; + } + + log->zero=0; + log->nast_.Pz = &log->zero; + + //if (nast_.adr.WE_Zdist) nast_.Pz = &(((R_ZDistL::dane_wewnetrzne *)wskazniki[nast_.adr.WE_Zdist])->P); + // else nast_.Pz = &dane_binarne[0]; + + + return 0; +} + +void l_comm(void *arguments, void *logic) +{ + struct l_comm_logic *log = (struct l_comm_logic *)logic; + float local; + u8 localb,localc; + int i; + + if (log->nast_.On) + { + + switch(log->nast_.Typ) + { + case 0: + log->dw.dbl = (*log->odb & (1<odb_bit_no))?1:0; + break; + case 1: + log->dw.dbl = !(*log->odb & (1<odb_bit_no))?1:0; + break; + case 2://typ bezwarunkowy + log->dw.W_I = (((*log->odb & (1<odb_bit_no)) && (*log->ok & (1<ok_bit_no)))) ? 1:0; + break; + } + + if(log->nast_.CRL_ON) //jesli wczona automatyka prdu wstecznego + { + if (*log->nast_.Str_rev) + { + log->dw.licztIrev = log->nast_.t_CRL; + log->dw.dbl = 0; + } else { + if (log->dw.licztIrev) + { + log->dw.licztIrev--; + log->dw.dbl = 0; + } + } + + } + + if (log->nast_.ECHO_ON)//jeli wczona automatyka echa + { + local = *log->U1; + if (*log->U2 < local) local = *log->U2; + if (*log->U3 < local) local = *log->U3; + localb = local < log->nast_.Ur; + localc = local > log->nast_.Ur_powr; + sprawdz_P(&log->dw.Um,localb,localc,&log->dw.liczU,3,3); + log->dw.W_E = (log->dw.Um && !(*log->nast_.Pz) && (*log->odb & (1<odb_bit_no)) && (*log->ok & (1<ok_bit_no)))?1:0; + } + + localb = log->dw.W_E || (*log->nast_.nad & (1<P_Io1_bit_no)); + log->dw.Nad |= localb && !log->dw.Nadp; + log->dw.Nadp = localb; + + + log->dw.W |= (log->dw.W_E | log->dw.W_I) && log->nast_.Wbz; + + + log->dw.dbl |= !(*log->ok & (1<ok_bit_no))?1:0; // jeli cze niesprawne to zdjta blokada + + + //obsluga wejscia testu przekaznika +// if (czy_test_R()) + // { + // dw.W = dw.Nad = dw.dbl = dane_binarne[nast_.adr.test]; + //} + // + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl && (*log->stan_bl & (1<stan_bl_bit_no))) /*| czy_start()*/) + { + log->dw.W = log->dw.Nad = 0; + log->dw.dbl = 1; + } + // + + + for(i=0;i<7;i++) //ustawienie blokad stref od cza + { + log->dw.stan.dbl_Com[i] = log->nast_.Com_ON[i] || log->dw.dbl; + } + + + if(log->dw.W) + *log->W|=(1<W_bit_no); + else + *log->W&=~(1<W_bit_no); + + if(log->dw.Nad) + *log->NRL|=(1<NRL_bit_no); + else + *log->NRL&=~(1<NRL_bit_no); + + } else { + + for(i=0;i<7;i++) //ustawienie blokad stref od cza + { + log->dw.stan.dbl_Com[i] = 1; + } + + *log->W&=~(1<W_bit_no); + *log->NRL&=~(1<NRL_bit_no); + } +} + + +void l_comm_100hz(void *arguments, void *logic) +{ + struct l_comm_logic *log = (struct l_comm_logic *)logic; + u8 lokalW; + + if (log->nast_.On) + { + //generowanie impulsu nadawania i wycz + lokalW = log->dw.W_E || log->dw.W_I; + if (lokalW) log->dw.licztW++; else log->dw.licztW = 0; + + if(log->dw.licztW>log->nast_.t_W) + { + log->dw.W |=1; + log->dw.licztW--; + } + + log->dw.W &= lokalW?1:0; + + log->dw.stan.PSPZ = log->dw.W && ((log->dw.W_E && log->nast_.Echo_SPZ_ON) || (log->dw.W_I && log->nast_.WD_SPZ_ON)); + + + if (log->dw.Nad) + { + log->dw.licztNad--; + if(!log->dw.licztNad) log->dw.Nad = 0; + } else { + log->dw.licztNad = log->nast_.t_Nad; + } + } +} diff --git a/src/l_comm.h b/src/l_comm.h new file mode 100644 index 0000000..3f5a65a --- /dev/null +++ b/src/l_comm.h @@ -0,0 +1,141 @@ +/* + * l_comm.h + * + * Created on: 28-11-2016 + * Author: Krzysztof Jakubczyk + */ + +/** +realizacja ³¹cza teletechnicznego dla zabezpieczenia odleg³oœciowego - wy³¹czenie trójfazowe +*/ + +#ifndef L_COMM_H_ +#define L_COMM_H_ + +#include "tdefs.h" +#include "ZDistL.h" + +struct dane_wew_comm +{ + int licztW; + int licztNad; + int licztIrev; + short liczU; + u8 dbl; + u8 W_I; // wys³anie impulsu wy³aczaj¹cego od bezwarunkowego + u8 W_E; //wys³anie impulsu wy³¹czaj¹cego od echa + u8 W; + u8 Um; + u8 Nad; + u8 Nadp; + + struct Wyjscie1_zdistl stan; +}; + +struct Nastawy_przeliczone_comm +{ + //Adresy adr; + //R_ZdistA::dane_wewnetrzne dw; + //R_ZdistA::Wyjscia WeZ0; + //R_ZdistA::Wyjscia *WeZ; + + u16 Typ;///< Program pracy automatyki + + int t_Nad;///< D³ugoœc impulsu nadawania + int t_CRL;///< Czas blokady w logice odwróconego pr¹du + int t_W;///< Czas wy³¹czenie od ³¹cza + float Ur;///< Wartoœæ napiêcia dla warunku funkcji ECHA + float Ur_powr;///< Wartoœæ napiêcia dla warunku funkcji ECHA + + u8 On;///< nastawa aktywnosci funkcji + u8 bl; ///< nastawa aktywnosci bitu blokady + u8 CRL_ON; ///< Aktywnoœæ logiki odwróconego pr¹du + u8 ECHO_ON; ///< Aktywnoœæ logiki echa + u8 WD_SPZ_ON; ///< Pobudzenie SPZ przy pracy bezwarunkowej + u8 Echo_SPZ_ON; ///< Pobudzenie SPZ przy logice ECHA + u8 Com_ON[7]; ///< wspólpraca z ³¹czem strefy + u8 Wbz;///< wy³¹czenie bezzw³oczne + u8 *nad; ///< adres sygna³u nadawania + u8 *Str_rev;///< adres sygna³u pobudzenia strfy wstecznej + u8 *Pz;///< adres do pobudzenia zbiorczego odleg³oœciówki +}; + +struct l_comm_logic +{ + u8 *stan_bl; + u8 stan_bl_bit_no; + + u8 *ok; + u8 ok_bit_no; + + u8 *odb; + u8 odb_bit_no; + + float *U1; + float *U2; + float *U3; + + u8 *P_Io1; + u8 P_Io1_bit_no; + + void *We_ZdistL_ptr; + + u8 *test; + u8 test_bit_no; + + u8 *W; + u8 W_bit_no; + + u8 *NRL; + u8 NRL_bit_no; + + struct dane_wew_comm dw; + struct Nastawy_przeliczone_comm nast_; + u8 zero; +}; + +struct l_comm_io +{ + u32 stan_bl_in; + u32 ok_in; + u32 odb_in; + + u32 U1_float_in; + u32 U2_float_in; + u32 U3_float_in; + + u32 P_Io1_in; + u32 We_ZdistL_ptr_in; + u32 test_in; + + u32 W_out; + u32 NRL_out; + u32 Stan_L_ptr_out; +}__attribute__((__packed__)); + +struct l_comm_params +{ + u32 bity; ///< nastawy bitowe; + u32 Typ;///< Program pracy automatyki + u32 Nad;///< Nadawanie sygna³u od (1,1W,2,3,4,5,Io) + u32 Odb;///< Sygna³ odbierany do wspó³pracy z (1,1W,2,3,4,5,Io) + u32 Str_rev;///io.we_ON,&log->we_ON,&log->we_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_OFF,&log->we_OFF,&log->we_OFF_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_zamk1,&log->we_zamk1,&log->we_zamk1_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_zamk2,&log->we_zamk2,&log->we_zamk2_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_otw1,&log->we_otw1,&log->we_otw1_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_otw2,&log->we_otw2,&log->we_otw2_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_blk_otw,&log->we_blk_otw,&log->we_blk_otw_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.we_blk_zamk,&log->we_blk_zamk,&log->we_blk_zamk_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_ON,&log->wy_ON,&log->wy_ON_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_NZG,&log->wy_NZG,&log->wy_NZG_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_imp_zamk,&log->wy_imp_zamk,&log->wy_imp_zamk_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_imp_otw,&log->wy_imp_otw,&log->wy_imp_otw_bit_mask)) + return -1; + + log->licz_nast_on = (u16)((args->params.t_on) * 1000/ LOOP_CYCLE_MS); + log->licz_nast_off = (u16)((args->params.t_off) * 1000/ LOOP_CYCLE_MS); + log->licz_nast_nzg = (u16)((args->params.t_nzg) * 1000/ LOOP_CYCLE_MS); + + log->licz_t_on = 0; + log->licz_t_off = 0; + log->licz_t_nzg = 0; + + log->we_zamk_pop = 0; + log->we_otw_pop = 0; + return 0; +} + +void lacz(void *arguments, void *logic) +{ + + struct lacz_logic *log = (struct lacz_logic *)logic; + + u8 we_on = ((*log->we_ON & log->we_ON_bit_mask) != 0) ? 1 : 0; + u8 we_off = ((*log->we_OFF & log->we_OFF_bit_mask) != 0) ? 1 : 0; + + u8 we_zamk = (((*log->we_zamk1 & log->we_zamk1_bit_mask) != 0) ? 1 : 0) + || (((*log->we_zamk2 & log->we_zamk2_bit_mask) != 0) ? 1 : 0); + + u8 we_otw = (((*log->we_otw1 & log->we_otw1_bit_mask) != 0) ? 1 : 0) + || (((*log->we_otw2 & log->we_otw2_bit_mask) != 0) ? 1 : 0); + + u8 blk_zamk = ((*log->we_blk_zamk & log->we_blk_zamk_bit_mask) != 0) ? 1 : 0; + u8 blk_otw = ((*log->we_blk_otw & log->we_blk_otw_bit_mask) != 0) ? 1 : 0; + + if (we_on && !we_off) + { + log->licz_t_nzg = 0; + *log->wy_ON |= log->wy_ON_bit_mask; + *log->wy_NZG &= ~log->wy_NZG_bit_mask; + } + else if (!we_on && we_off) + { + log->licz_t_nzg = 0; + *log->wy_ON &= ~log->wy_ON_bit_mask; + *log->wy_NZG &= ~log->wy_NZG_bit_mask; + + } + else //niezgodnosc + { + //nie zmieniac stanu + log->licz_t_nzg++; + + } + + if (log->licz_t_nzg > log->licz_nast_nzg) + { + log->licz_t_nzg = log->licz_nast_nzg + 5; + *log->wy_NZG |= log->wy_NZG_bit_mask; + } + + + if (!blk_zamk) + { + if (log->licz_t_on != 0) + { + if (++log->licz_t_on > log->licz_nast_on) + { + log->licz_t_on = 0; + *log->wy_imp_zamk &= ~log->wy_imp_zamk_bit_mask; + } + } + else if (we_zamk && (!log->we_zamk_pop)) + { + *log->wy_imp_zamk |= log->wy_imp_zamk_bit_mask; + log->licz_t_on = 1; + } + + log->we_zamk_pop = we_zamk; + } + else + { + log->licz_t_on = 0; + *log->wy_imp_zamk &= ~log->wy_imp_zamk_bit_mask; + log->we_zamk_pop = 0; + } + + + + if (!blk_otw) + { + if (log->licz_t_off != 0) + { + if (++log->licz_t_off > log->licz_nast_off) + { + log->licz_t_off = 0; + *log->wy_imp_otw &= ~log->wy_imp_otw_bit_mask; + } + } + else if (we_otw && (!log->we_otw_pop)) + { + *log->wy_imp_otw |= log->wy_imp_otw_bit_mask; + log->licz_t_off = 1; + } + + log->we_otw_pop = we_otw; + } + else + { + log->licz_t_off = 0; + *log->wy_imp_otw &= ~log->wy_imp_otw_bit_mask; + log->we_otw_pop = 0; + } + +} + + + + diff --git a/src/lacz.h b/src/lacz.h new file mode 100644 index 0000000..f720de5 --- /dev/null +++ b/src/lacz.h @@ -0,0 +1,85 @@ +#ifndef LACZ_H_ +#define LACZ_H_ + +#include + +//#include "pawel_usun_to.h" + +#include "tdefs.h" + +struct lacz_logic +{ + u8* we_ON; + u8 we_ON_bit_mask; + u8* we_OFF; + u8 we_OFF_bit_mask; + u8* we_zamk1; + u8 we_zamk1_bit_mask; + u8* we_zamk2; + u8 we_zamk2_bit_mask; + u8* we_otw1; + u8 we_otw1_bit_mask; + u8* we_otw2; + u8 we_otw2_bit_mask; + u8* we_blk_zamk; + u8 we_blk_zamk_bit_mask; + u8* we_blk_otw; + u8 we_blk_otw_bit_mask; + + u8* wy_ON; + u8 wy_ON_bit_mask; + u8* wy_NZG; + u8 wy_NZG_bit_mask; + u8* wy_imp_zamk; + u8 wy_imp_zamk_bit_mask; + u8* wy_imp_otw; + u8 wy_imp_otw_bit_mask; + + u16 licz_t_on; + u16 licz_t_off; + u16 licz_t_nzg; + u16 licz_nast_on; + u16 licz_nast_off; + u16 licz_nast_nzg; + u8 we_zamk_pop; + u8 we_otw_pop; + +}; + +struct lacz_params +{ + + double t_on; + double t_off; + double t_nzg; + +}__attribute__((__packed__)); + +struct lacz_io +{ + u32 we_ON; + u32 we_OFF; + u32 we_zamk1; + u32 we_zamk2; + u32 we_otw1; + u32 we_otw2; + u32 we_blk_zamk; + u32 we_blk_otw; + + u32 wy_ON; + u32 wy_NZG; + u32 wy_imp_zamk; + u32 wy_imp_otw; + +}__attribute__((__packed__)); + +struct lacz_args +{ + struct lacz_io io; + struct lacz_params params; +}__attribute__((__packed__)); + +extern void lacz(void *args, void *logic); +extern int lacz_initlog(void *arguments, void *logic); + +#endif /*LACZ_H_*/ diff --git a/src/led.c b/src/led.c new file mode 100644 index 0000000..added94 --- /dev/null +++ b/src/led.c @@ -0,0 +1,39 @@ +/* + * led.c + * + * Created on: 09-10-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "led.h" + +int led_initlog(void *arguments, void *logic) +{ + struct led_args *args = (struct led_args *)arguments; + struct led_logic *log = (struct led_logic *)logic; + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_no = args->io.in & 0x07; + log->blink_in_ptr = log_manager.nets_data + (args->io.blink_in >> 3); + log->blink_in_bit_no = args->io.blink_in & 0x07; + log->clr_ptr = log_manager.nets_data + (args->io.clr_in >> 3); + log->clr_bit_no = args->io.clr_in & 0x07; + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->clr_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->blink_in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(args->params.led_num>=MAX_LEDS_COUNT) + return -1; + + return 0; +} + diff --git a/src/led.h b/src/led.h new file mode 100644 index 0000000..210df88 --- /dev/null +++ b/src/led.h @@ -0,0 +1,49 @@ +/* + * led.h + * + * Created on: 09-10-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef LED_H_ +#define LED_H_ + +#include "tdefs.h" + +#define MAX_LEDS_COUNT 21 // 20 LEDS + 1 BUZZER +#define BUZZER_LED_NUM 20 + +struct led_logic +{ + u8 *in_ptr; + u8 in_bit_no; + u8 *blink_in_ptr; + u8 blink_in_bit_no; + u8 *clr_ptr; + u8 clr_bit_no; +}; + +struct led_io +{ + u32 in; + u32 blink_in; + u32 clr_in; +}__attribute__((__packed__)); + +struct led_params +{ + u32 led_num; + u32 until_clr; +}__attribute__((__packed__)); + +struct led_args +{ + struct led_io io; + struct led_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int led_initlog(void *arguments, void *logic); +extern int buzzer_initlog(void *arguments, void *logic); + +#endif /* LED_H_ */ diff --git a/src/leds_drv.c b/src/leds_drv.c new file mode 100644 index 0000000..585f359 --- /dev/null +++ b/src/leds_drv.c @@ -0,0 +1,80 @@ +/* + * leds_drv.c + * + * Created on: 10-10-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "leds_drv.h" +#include "led.h" + +u32 led_states = 0; +u32 led_blink_states = 0; + +int leds_drv_initlog(void *arguments, void *logic) +{ +// struct events_reg_args *args = (struct events_reg_args *)arguments; + struct leds_drv_logic *log = (struct leds_drv_logic *)logic; + int i; + + log->leds_count=0; + + for(i=0;ielement_num[log->leds_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->leds_count]=i; + log->leds_count++; + } + } + +// led_states = 0; +// led_blink_states = 0; + + return 0; +} + +void leds_drv(void *arguments, void *logic) +{ + struct leds_drv_logic *log = (struct leds_drv_logic *)logic; + struct led_args *l_args; + struct led_logic *l_log; + int i; + + for(i=0;ileds_count;i++) + { + l_args = (struct led_args *)log_manager.log_element[log->element_num[i]].fun_args_ptr; + l_log = (struct led_logic *)log_manager.log_element[log->element_num[i]].fun_log_ptr; + + if(*l_log->in_ptr & (1<in_bit_no)) + { + led_states|=(1<params.led_num); + } + else + { + if(!l_args->params.until_clr) + led_states&=~(1<params.led_num); + else if(*l_log->clr_ptr & (1<clr_bit_no)) + led_states&=~(1<params.led_num); + } + + if(*l_log->blink_in_ptr & (1<blink_in_bit_no)) + { + led_blink_states|=(1<params.led_num); + } + else + { + if(!l_args->params.until_clr) + led_blink_states&=~(1<params.led_num); + else if(*l_log->clr_ptr & (1<clr_bit_no)) + led_blink_states&=~(1<params.led_num); + } + } +} diff --git a/src/leds_drv.h b/src/leds_drv.h new file mode 100644 index 0000000..e76acb7 --- /dev/null +++ b/src/leds_drv.h @@ -0,0 +1,41 @@ +/* + * leds_drv.h + * + * + * Created on: 10-10-2014 + * Author: KJ + */ + +#ifndef LEDS_DRV_H_ +#define LEDS_DRV_H_ + +#include "tdefs.h" + +extern u32 led_states; +extern u32 led_blink_states; + +struct leds_drv_logic +{ + u8 leds_count; + u16 element_num[]; +}; + +struct leds_drv_io +{ +}__attribute__((__packed__)); + +struct leds_drv_params +{ +}__attribute__((__packed__)); + +struct leds_drv_args +{ + struct leds_drv_io io; + struct leds_drv_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int leds_drv_initlog(void *args, void *logic); +extern void leds_drv(void *args, void *logic); + +#endif /* LEDS_DRV_H_ */ diff --git a/src/log_state.c b/src/log_state.c new file mode 100644 index 0000000..c377706 --- /dev/null +++ b/src/log_state.c @@ -0,0 +1,33 @@ +/* + * log_state.c + * + * Created on: 25-01-2017 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "log_state.h" +#include "helper.h" + +int log_state_initlog(void *arguments, void *logic) +{ + struct log_state_args *args = (struct log_state_args *)arguments; + struct log_state_logic *log = (struct log_state_logic *)logic; + + if(set_bit_ptr_mask(args->io.out,&log->out,&log->out_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->params.net_number,&log->in,&log->in_bit_mask)) + return -1; + + return 0; +} + +void log_state(void *arguments, void *logic) +{ + struct log_state_logic *log = (struct log_state_logic *)logic; + + check_and_set_mask(*log->in & log->in_bit_mask, log->out, log->out_bit_mask); +} diff --git a/src/log_state.h b/src/log_state.h new file mode 100644 index 0000000..3190e55 --- /dev/null +++ b/src/log_state.h @@ -0,0 +1,41 @@ +/* + * log_state.h + * + * Created on: 25-01-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef LOG_STATE_H_ +#define LOG_STATE_H_ + +#include "tdefs.h" + +struct log_state_logic +{ + u8 *out; + u8 out_bit_mask; + u8 *in; + u8 in_bit_mask; +}; + +struct log_state_io +{ + u32 out; +}__attribute__((__packed__)); + +struct log_state_params +{ + u32 net_number; +}__attribute__((__packed__)); + +struct log_state_args +{ + struct log_state_io io; + struct log_state_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int log_state_initlog(void *arguments, void *logic); +extern void log_state(void *arguments, void *logic); + +#endif /* LOG_STATE_H_ */ diff --git a/src/log_wyl.c b/src/log_wyl.c new file mode 100644 index 0000000..f13bc9c --- /dev/null +++ b/src/log_wyl.c @@ -0,0 +1,153 @@ +/* + * LRW.c + * + * Created on: 09-04-2025 + * Author: + * Pawe Skoczykoda + */ + + +#include "tdefs.h" +#include "misc.h" +#include "helper.h" + + +#include "log_wyl.h" + +int log_wyl_initlog(void *arguments, void *logic) +{ + struct log_wyl_args *args = (struct log_wyl_args *)arguments; + struct log_wyl_logic *log = (struct log_wyl_logic *)logic; + + if(set_bit_ptr_struct(args->io.wyl_L1_in,&log->w_L1_in)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_L2_in,&log->w_L2_in)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_L3_in,&log->w_L3_in)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_3f_in,&log->w_3f)) + return -1; + if(set_bit_ptr_struct(args->io.blok_w_in,&log->blok_w)) + return -1; + + if(set_bit_ptr_struct(args->io.wyl_L1_out,&log->w_L1_out)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_L2_out,&log->w_L2_out)) + return -1; + if(set_bit_ptr_struct(args->io.wyl_L3_out,&log->w_L3_out)) + return -1; + + log->dobijacz = (args->params.bity & 0x0001)?1:0; + log->t_limit = (u32)((args->params.t1) * 1000 / LOOP_CYCLE_MS); + + log->we_L1_pop = 0; + log->we_L2_pop = 0; + log->we_L3_pop = 0; + + log->timer_L1 = 0; + log->timer_L2 = 0; + log->timer_L3 = 0; + + return 0; +} + +void log_wyl(void *arguments, void *logic) +{ + struct log_wyl_logic *log = (struct log_wyl_logic *)logic; + + u8 we_L1 = check_struct(&log->w_L1_in)?1:0; + u8 we_L2 = check_struct(&log->w_L2_in)?1:0; + u8 we_L3 = check_struct(&log->w_L3_in)?1:0; + u8 we_bl = check_struct(&log->blok_w)?1:0; + u8 we_3f = check_struct(&log->w_3f)?1:0; + + if (we_bl) + { + log->we_L1_imp = 0; + log->we_L2_imp = 0; + log->we_L3_imp = 0; + we_L1 = 0; + we_L2 = 0; + we_L3 = 0; + } + else + { + + if (log->dobijacz) + { + if (((log->we_L1_imp || we_L1) && (log->we_L2_imp || we_L2)) || + ((log->we_L1_imp || we_L1) && (log->we_L3_imp || we_L3)) || + ((log->we_L3_imp || we_L3) && (log->we_L2_imp || we_L2))) + { + we_3f = 1; //dobijacz + } + } + + if (we_3f) + { + we_L1 = 1; + we_L2 = 1; + we_L3 = 1; + } + + if (!log->we_L1_imp) + { + if (we_L1 && !log->we_L1_pop) + { + log->we_L1_imp = 1; + } + } + else + { + if (++log->timer_L1 > log->t_limit) + { + log->timer_L1 = 0; + log->we_L1_imp = 0; + } + } + + if (!log->we_L2_imp) + { + if (we_L2 && !log->we_L2_pop) + { + log->we_L2_imp = 1; + } + } + else + { + if (++log->timer_L2 > log->t_limit) + { + log->timer_L2 = 0; + log->we_L2_imp = 0; + } + } + + if (!log->we_L3_imp) + { + if (we_L3 && !log->we_L3_pop) + { + log->we_L3_imp = 1; + } + } + else + { + if (++log->timer_L3 > log->t_limit) + { + log->timer_L3 = 0; + log->we_L3_imp = 0; + } + } + } + + check_and_set_struct((log->we_L1_imp || we_L1),&log->w_L1_out); + check_and_set_struct((log->we_L2_imp || we_L2),&log->w_L2_out); + check_and_set_struct((log->we_L3_imp || we_L3),&log->w_L3_out); + + log->we_L1_pop = we_L1; + log->we_L2_pop = we_L2; + log->we_L3_pop = we_L3; + +} + + + diff --git a/src/log_wyl.h b/src/log_wyl.h new file mode 100644 index 0000000..2834715 --- /dev/null +++ b/src/log_wyl.h @@ -0,0 +1,76 @@ +/* + * LRW.h + * + * Created on: 09-04-2025 + * Author: Pawel Skoczykloda + */ + +#ifndef LOG_WYL_ +#define LOG_WYL_ + +//#include "pawel_usun_to.h" + +#include "tdefs.h" + +struct log_wyl_logic +{ + + struct binary_io w_L1_in; + struct binary_io w_L2_in; + struct binary_io w_L3_in; + struct binary_io w_3f; + struct binary_io blok_w; + + struct binary_io w_L1_out; + struct binary_io w_L2_out; + struct binary_io w_L3_out; + + u8 dobijacz; + u32 t_limit; + + u8 we_L1_pop; + u8 we_L2_pop; + u8 we_L3_pop; + + u8 we_L1_imp; + u8 we_L2_imp; + u8 we_L3_imp; + + u32 timer_L1; + u32 timer_L2; + u32 timer_L3; + +}; + +struct log_wyl_io +{ + u32 wyl_L1_in; + u32 wyl_L2_in; + u32 wyl_L3_in; + u32 wyl_3f_in; + u32 blok_w_in; + + u32 wyl_L1_out; + u32 wyl_L2_out; + u32 wyl_L3_out; + +}__attribute__((__packed__)); + +struct log_wyl_params +{ + u32 bity; ///< nastawy bitowe; + double t1; ///< wartosc nastawy czasu + +}__attribute__((__packed__)); + +struct log_wyl_args +{ + struct log_wyl_io io; + struct log_wyl_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void log_wyl(void *args, void *logic); +extern int log_wyl_initlog(void *arguments, void *logic); + +#endif /* LOG_WYL_ */ diff --git a/src/logman.c b/src/logman.c index db91907..297305f 100644 --- a/src/logman.c +++ b/src/logman.c @@ -7,17 +7,20 @@ #include #include +/* #include #include #include +*/ +#include "mocks.h" #include "tdefs.h" #include "logman.h" #include "misc.h" -#include "comm.h" +// #include "comm.h" -#include "logic_elements/elements.h" -#include "logic_elements/eth.h" +#include "elements.h" +#include "eth.h" #include "ports/am1808/include/netif/sitaraif.h" struct logic_manager log_manager; diff --git a/src/main.c b/src/main.c deleted file mode 100644 index 68a6446..0000000 --- a/src/main.c +++ /dev/null @@ -1,533 +0,0 @@ -/* - * main.c - * - * Created on: 01-06-2013 - * Author: Krzysztof Jakubczyk - */ - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "misc.h" -#include "comm.h" -#include "tdefs.h" -#include "logman.h" -#include "spi.h" -#include "logic_elements/dev_ctrl.h" -#include "logic_elements/an_gen.h" -#include "logic_elements/events_reg.h" -//pwm -#include "psc.h" -#include "soc_OMAPL138.h" -#include "hw_syscfg0_OMAPL138.h" -#include "hw_types.h" -// -/// ethernet -#include "ethernet/ports/am1808/include/lwiplib.h" -/// -#include "config.h" - -volatile unsigned short last_sample_no; -volatile unsigned short samples_dropped = 0; -volatile unsigned short last_sample_sync_no; -volatile unsigned short samples_sync_dropped = 0; -volatile int is_first_sample = 1; -volatile int irqs = 0; -u16 bus_an_cur_timestamp = 0; - -#pragma LOCATION(shared_buf,0xc5500000) // @ non-cached memory above 128MB -volatile int shared_buf[1024]; - -#pragma DATA_ALIGN(analog_buf, 128) // place @ cache line boundary -volatile short analog_buf[127]; - -//volatile int analog_buf_ready = 0; -//volatile int analog_buf_cnt = 0; -unsigned int analog_buf_card = 0; -unsigned int analog_buf_channel = 0; - -struct device_config dev_cfg; - -u8 mwd_states_cur[MAX_BIN_CARDS]; -u8 mwd_states_prev[MAX_BIN_CARDS]; - -Swi_Handle swi_notify; -Swi_Params swi_notify_params; - -Swi_Handle swi_ms; -Swi_Params swi_ms_params; - -Swi_Handle swi_10ms; -Swi_Params swi_10ms_params; - -Swi_Handle swi_50ms; -Swi_Params swi_50ms_params; - -extern Timer_Handle timer0; - -u8 loops_synced=0; - -u8 khz_trigger=LOOP_CYCLE_MS * SAMPLES_PER_MS; - -volatile u32 cycle_pwm=0; -volatile struct timeval cur_time = { 1388563200, 0 }; // default 2014-01-01 -volatile struct timeval cur_time_sw = { 1577836800, 0 }; // default 2020-01-01 -u8 bus_an_cur_sample_num_prev=0; -u8 sample_watch_init=0; -u8 cur_sample_diff=0; - -void pwm_init() -{ - PSCModuleControl(SOC_PSC_1_REGS, HW_PSC_EHRPWM, PSC_POWERDOMAIN_ALWAYS_ON, PSC_MDCTL_NEXT_ENABLE); - PWM1_SET(PWM1_TBCTL, 0x8D03); //FREE RUN, CLKDIV /8, HSPCLKDIV /4, STOP - PWM1_SET(PWM1_TBPRD, 0xFFFF); - PWM1_SET(PWM1_TBCNT, 0x0000); - HWREG(SOC_SYSCFG_0_REGS + SYSCFG0_CFGCHIP1) |= SYSCFG_CFGCHIP1_TBCLKSYNC; -} - -void pwm_start_cnt() -{ - PWM1_SET(PWM1_TBCNT, 0x0000); //COUNTER = 0 - PWM1_SET(PWM1_TBCTL, 0x8D00); //START -} - -u32 pwm_stop_cnt() -{ - PWM1_SET(PWM1_TBCTL, 0x8D03);//STOP - return PWM1_GET(PWM1_TBCNT); -} - -Void notify_swi(UArg a0, UArg a1) -{ - u32 requests = logman_notify; - - if(requests & LOGMAN_NOTIFY_NEW_EVENTS) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_NEW_EVENTS | ((u32)ev_db.pos<<8), TRUE); - logman_notify&=~LOGMAN_NOTIFY_NEW_EVENTS; - } - - if(requests & LOGMAN_NOTIFY_NEW_DFR) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_NEW_DFR_REG, TRUE); - logman_notify&=~LOGMAN_NOTIFY_NEW_DFR; - } - - if(requests & LOGMAN_NOTIFY_NEW_DDR) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_NEW_DDR_REG, TRUE); - logman_notify&=~LOGMAN_NOTIFY_NEW_DDR; - } - - if(requests & LOGMAN_NOTIFY_BANK0) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_SEL_BANK0|(service_mode<<8), TRUE); - logman_notify&=~LOGMAN_NOTIFY_BANK0; - } - - if(requests & LOGMAN_NOTIFY_BANK1) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_SEL_BANK1|(service_mode<<8), TRUE); - logman_notify&=~LOGMAN_NOTIFY_BANK1; - } - - if(requests & LOGMAN_NOTIFY_BANK2) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_SEL_BANK2|(service_mode<<8), TRUE); - logman_notify&=~LOGMAN_NOTIFY_BANK2; - } - - if(requests & LOGMAN_NOTIFY_BANK3) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_SEL_BANK3|(service_mode<<8), TRUE); - logman_notify&=~LOGMAN_NOTIFY_BANK3; - } - - if(requests & LOGMAN_NOTIFY_BANK4) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_SEL_BANK4|(service_mode<<8), TRUE); - logman_notify&=~LOGMAN_NOTIFY_BANK4; - } - - if(requests & LOGMAN_NOTIFY_BANK5) - { - Notify_sendEvent(notify.remoteProcId,notify.lineId,notify.eventId, NOTIFY_SEL_BANK5|(service_mode<<8), TRUE); - logman_notify&=~LOGMAN_NOTIFY_BANK5; - } - -} - -volatile u16 pps_low_cnt=0; -volatile u16 pps3_timeout_cnt=0xFFFF; - -Void ms_hook(UArg a0, UArg a1) -{ - u32 t_pre,t_post,sync_reg; - u8 pps=0; - - t_pre=PWM1_GET(PWM1_TBCNT); - - bus_an_cur_sample_num=bus_an_cur_sample_num_xkhz;//bus_an_cur_sample_num_3khz; - - // gdy IEC SV to cofamy czas/pozycje bufora o obiegi pêtli w celu synchronizacji próbek - if(log_manager.status & LOGMAN_USING_IEC_SV) - { - if(bus_an_cur_sample_num<(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES)) - bus_an_cur_sample_num=SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2-((SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES)-bus_an_cur_sample_num); - else - bus_an_cur_sample_num-=(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES); - } - - sync_reg = ic->sync_reg; - if(sync_reg & (PPS_SYNCED|IRIGB_SYNCED)) - ext_sync=1; - - - if(timesync_method==SYNC_METHOD_IEC103||timesync_method==SYNC_METHOD_ZP6||timesync_method==SYNC_METHOD_MLB || (timesync_method==SYNC_METHOD_CUSTOM & !(timesync_bits & CFG_TSYNC_USE_PPS))) - ext_sync=0; - - bus_an_cur_timestamp=tstamp_khz; - if(log_manager.status & LOGMAN_USING_IEC_SV) - { - if(bus_an_cur_timestamp<(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES)) - bus_an_cur_timestamp=SAMPLES_PER_MS*1000-((SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES)-bus_an_cur_timestamp); - else - bus_an_cur_timestamp-=(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES); - } - - if(!ext_sync) - { - cur_time.tv_usec+=LOOP_CYCLE_MS; - - if(cur_time.tv_usec>=1000) - { - cur_time.tv_usec=0; - cur_time.tv_sec++; - } - } - else - { - u32 usec_prev; - usec_prev=cur_time.tv_usec; - cur_time.tv_usec=bus_an_cur_timestamp/SAMPLES_PER_MS; - if(!cur_time.tv_usec && usec_prev) - cur_time.tv_sec++; - - } - - if(pps3_timeout_cnt!=0xFFFF) - pps3_timeout_cnt++; - - if(!(IN_DATA01 & (1<<4))) - { - if(pps_low_cnt<(1000/LOOP_CYCLE_MS)) - pps_low_cnt++; - } - else - { - if(pps_low_cnt>(600/LOOP_CYCLE_MS) && pps_low_cnt<(900/LOOP_CYCLE_MS)) - { - pps=1; - pps3_timeout_cnt=0; - } - - pps_low_cnt=0; - } - - if(((sync_reg & 0x80) && (timesync_method==SYNC_METHOD_IRIG_B || timesync_method==SYNC_METHOD_IRIG_B_ZPRAE || (timesync_method==SYNC_METHOD_CUSTOM && (timesync_bits & CFG_TSYNC_FROM_DSP)))) - || (pps && (timesync_method==SYNC_METHOD_CUSTOM && (timesync_bits & CFG_TSYNC_USE_SWPPS))) - ) - { - if(cur_time_sw.tv_usec>900) - cur_time_sw.tv_sec++; - - //if(pps) - // zprae_event_add(1,97); - cur_time_sw.tv_usec=0; - } - else - { - cur_time_sw.tv_usec+=LOOP_CYCLE_MS; - - if(cur_time_sw.tv_usec>=1000) - { - cur_time_sw.tv_usec=0; - cur_time_sw.tv_sec++; - } - } - - - if(bus_an_cur_sample_num_prev<=bus_an_cur_sample_num) - cur_sample_diff=bus_an_cur_sample_num-bus_an_cur_sample_num_prev; - else - cur_sample_diff=(u16)bus_an_cur_sample_num+(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)-bus_an_cur_sample_num_prev; - - if(cur_sample_diff!=(SAMPLES_PER_MS*LOOP_CYCLE_MS) && sample_watch_init) // debug lost x kHz loops - { - // if(cur_sample_diff>dbg.delta_period) - // dbg.delta_period=cur_sample_diff; - dbg.temp_dbg++; - } - - bus_an_cur_sample_num_prev = bus_an_cur_sample_num; - sample_watch_init=1; - - logman_xkhz(); - - t_post=PWM1_GET(PWM1_TBCNT); - - if(t_post>=t_pre) - dbg.logman_cycle_time = t_post-t_pre; - else - dbg.logman_cycle_time = t_post+65536-t_pre; - - if(dbg.logman_cycle_time>dbg.logman_cycle_time_max) - dbg.logman_cycle_time_max=dbg.logman_cycle_time; - - - -} - -Void ms10_hook(UArg a0, UArg a1) -{ - logman_100hz(); -} - -Void ms50_hook(UArg a0, UArg a1) -{ - logman_20hz(); -} - -Void spi_irq(UArg arg) -{ - Hwi_clearInterrupt(14); -} - -UInt IntGlobalDisable() -{ - return Hwi_disable(); -} - -UInt IntGlobalRestore(UInt hwi_key) -{ - Hwi_restore(hwi_key); - return 0; -} - -u8 spi_frame[32]; -u8 spi_processed=1; - -Void comm_irq(UArg arg) -{ - int i; - u32 t_pre,t_post; - - t_pre=PWM1_GET(PWM1_TBCNT); - - bus_an_cur_sample_num_3khz++; - bus_an_cur_sample_num_3khz%=(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2); - -/* Nie potrzebne przy PPS - if(!ext_sync && timesync_method!=SYNC_METHOD_IRIG_B && timesync_method!=SYNC_METHOD_IRIG_B_ZPRAE && !(timesync_bits & CFG_TSYNC_FROM_DSP) && ic->spi_reg & 0x01) - { - if(ic->spi_frame[7]==0x01 && ic->spi_frame[0]==0x68 && ic->spi_frame[31]==0x16 && ic->spi_frame[1]==0x1a && spi_processed) - { - memcpy(spi_frame,(char *)&ic->spi_frame[0],32); - if(mod256_cksum(spi_frame+4,26)==spi_frame[30] && spi_frame[1]==0x1a && spi_frame[7]==0x01 && spi_frame[9]!=0xD5) // time sync req - { - u16 ms; - ms = (u16)spi_frame[9] | ((u16)spi_frame[10]<<8); - ms%=1000; - - if(!ext_sync) - cur_time.tv_usec = ms; - - cur_time_sw.tv_usec = ms; - - //dbg.temp_dbg++; - spi_processed=0; - } - } - } - else - spi_processed=1; -*/ - - for(i=0;ian_in[i].l1 + 32767)):((u16)ic->an_in[i].l1 + 32767); - - if(!(genpar.an_force_mask & (1<< ((i<<2)+(1))))) - bus_an_samples_buf[i][1][bus_an_cur_sample_num_3khz]=bus_an_samples_neg[i][1]?(65534-((u16)ic->an_in[i].l2 + 32767)):((u16)ic->an_in[i].l2 + 32767); - - if(!(genpar.an_force_mask & (1<< ((i<<2)+(2))))) - bus_an_samples_buf[i][2][bus_an_cur_sample_num_3khz]=bus_an_samples_neg[i][2]?(65534-((u16)ic->an_in[i].l3 + 32767)):((u16)ic->an_in[i].l3 + 32767); - - if(!(genpar.an_force_mask & (1<< ((i<<2)+(3))))) - bus_an_samples_buf[i][3][bus_an_cur_sample_num_3khz]=bus_an_samples_neg[i][3]?(65534-((u16)ic->an_in[i].l4 + 32767)):((u16)ic->an_in[i].l4 + 32767); - - } - else - { - bus_an_samples_buf[i][0][bus_an_cur_sample_num_3khz]=bus_an_samples_neg[i][0]?(65534-((u16)ic->an_in[i].l1 + 32767)):((u16)ic->an_in[i].l1 + 32767); - bus_an_samples_buf[i][1][bus_an_cur_sample_num_3khz]=bus_an_samples_neg[i][1]?(65534-((u16)ic->an_in[i].l2 + 32767)):((u16)ic->an_in[i].l2 + 32767); - bus_an_samples_buf[i][2][bus_an_cur_sample_num_3khz]=bus_an_samples_neg[i][2]?(65534-((u16)ic->an_in[i].l3 + 32767)):((u16)ic->an_in[i].l3 + 32767); - bus_an_samples_buf[i][3][bus_an_cur_sample_num_3khz]=bus_an_samples_neg[i][3]?(65534-((u16)ic->an_in[i].l4 + 32767)):((u16)ic->an_in[i].l4 + 32767); - } - } - - - if(loops_synced) - { - if(!--khz_trigger) - { - bus_an_cur_sample_num_xkhz=bus_an_cur_sample_num_3khz; - tstamp_khz=ic->sample_sync_no; - Swi_post(swi_ms); - khz_trigger=SAMPLES_PER_MS*LOOP_CYCLE_MS; - } - //Swi_dec(swi_1ms); - Swi_dec(swi_10ms); - Swi_dec(swi_50ms); - } - - if(!loops_synced && (ic->sample_sync_no==0)) - { - loops_synced=1; - bus_an_cur_sample_num_3khz=0; //dodano przy okazji SV - } - - //dodano na wszelki wypadek, bo czasem pêtle siê nie synchronizowa³y, dlaczego? FPGA? - //trzeba to sprawdzic bo moze byc skutek uboczny || zakomentowano jednak po poprawie sync na 2ms w fpga - //if(ic->sample_sync_no==0) - // khz_trigger=LOOP_CYCLE_MS * SAMPLES_PER_MS; - // - - i=ic->sample_no; - - if(is_first_sample) - is_first_sample=0; - else - samples_dropped+=(i - last_sample_no)-1; - - last_sample_no=i; - - t_post=PWM1_GET(PWM1_TBCNT); - - if(t_post>=t_pre) - dbg.irq_time = t_post-t_pre; - else - dbg.irq_time = t_post+65536-t_pre; -} - -void configure_pwr_ok_gpio() -{ - KICK0R=KICK0R_VAL; - KICK1R=KICK1R_VAL; - - PINMUX0=(PINMUX0 & ~(PINMUX3_15_12)) | (0x08 << 12); // pwr1_ok GP0[12] as GPIO - PINMUX1=(PINMUX1 & ~(PINMUX3_11_8)) | (0x08 << 8); // pwr2_ok GP0[5] as GPIO - PINMUX1=(PINMUX1 & ~(PINMUX3_15_12)) | (0x08 << 12); // pps3 GP0[4] as GPIO - PINMUX2=(PINMUX2 & ~(PINMUX3_19_16)) | (0x04 << 16); // gp1[11] (PPS from GPS) as GPIO - - DIR01|=((1<<4)|(1<<5)|(1<<12)|(1<<27)); // gp0.4, gp0.5, gp0.12 and gp1 .11 as input - PUPD_ENA|=((1<<0)|(1<<2)); // enable pull up/down control 1<<5 == GP1[11] - PUPD_ENA&=~(1<<5); // disable pull up/down for gp1[11] - PUPD_SEL|=((1<<0)|(1<<2)); // set pull up -} - -/* -** Interrupt Handler for Core 0 Receive interrupt -*/ -void EMACCore0RxIsr(void) -{ - lwIPRxIntHandler(0); -} - -/* -** Interrupt Handler for Core 0 Transmit interrupt -*/ -void EMACCore0TxIsr(void) -{ - lwIPTxIntHandler(0); -} - -Void main() -{ - Task_Handle task,task2; - - memset((u8*)&dbg,0,sizeof(dbg)); - ic->sync_reg|=NEG_PPS_IN; - - task = Task_create(commFxn, NULL, NULL); - Task_setPri(task,3); - - if (task == NULL) { - BIOS_exit(0); - } - - task2 = Task_create(spiFxn, NULL, NULL); - Task_setPri(task2,2); - - if (task2 == NULL) { - BIOS_exit(0); - } - - analog_inputs_init(); - - logman_init(); - - Swi_Params_init(&swi_ms_params); - swi_ms_params.priority = 15; - - swi_ms_params.trigger = SAMPLES_PER_MS * LOOP_CYCLE_MS; // every x samples - swi_ms = Swi_create(&ms_hook,&swi_ms_params,NULL); - - Swi_Params_init(&swi_10ms_params); - swi_10ms_params.priority = 10; - swi_10ms_params.trigger = SAMPLES_PER_MS * 10; // (10 ms) - swi_10ms = Swi_create(&ms10_hook,&swi_10ms_params,NULL); - - Swi_Params_init(&swi_50ms_params); - swi_50ms_params.priority = 8; - swi_50ms_params.trigger = SAMPLES_PER_MS * 50; // (50 ms) - swi_50ms = Swi_create(&ms50_hook,&swi_50ms_params,NULL); - - Swi_Params_init(&swi_notify_params); - swi_notify_params.priority = 1; - swi_notify_params.trigger = 1; - swi_notify = Swi_create(¬ify_swi,&swi_notify_params,NULL); - - SET_RIS_TRIG8 = (1<<15); // rising edge trig from GP8.15 - - configure_pwr_ok_gpio(); - pwm_init(); - pwm_start_cnt(); - -// eth.hwaddr[0]=0xC4; - // eth.hwaddr[1]=0xff; -// eth.hwaddr[2]=0xBC; -// eth.hwaddr[3]=0x70; -// eth.hwaddr[4]=0x14; -// eth.hwaddr[5]=0x3f; - //lwIPInit(0, eth.hwaddr, eth.ip, eth.netmask, eth.gateway, IPADDR_USE_STATIC); - //Hwi_enableInterrupt(10); - //Hwi_enableInterrupt(11); - - BIOS_start(); /* enable interrupts and start SYS/BIOS */ -} diff --git a/src/measurand.c b/src/measurand.c new file mode 100644 index 0000000..03a1ca8 --- /dev/null +++ b/src/measurand.c @@ -0,0 +1,80 @@ +/* + * measurand.c + * + * Created on: 07-10-2014 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "measurand.h" +#include "analog_in.h" + +volatile struct measurands_table measurands; + +int measurand_initlog(void *arguments, void *logic) +{ + struct measurand_args *args = (struct measurand_args *)arguments; + struct measurand_logic *log = (struct measurand_logic *)logic; + u32 *an_params_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_in >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->an_params=(struct analog_in_params *)*an_params_ptr; + + measurands.value[measurands.count]=&log->calculated; + measurands.value_pri[measurands.count]=&log->calculated_pri; + measurands.value_sec[measurands.count]=&log->calculated_sec; + measurands.id[measurands.count]=args->params.id; + measurands.count++; + + return 0; +} + +void measurand_100hz(void *arguments, void *logic) +{ + struct measurand_args *args = (struct measurand_args *)arguments; + struct measurand_logic *log = (struct measurand_logic *)logic; + float m; + + if(*log->esk_ptr == 32768.0f) + m=0; + else if(args->params.bits & MEASURAND_SQRT) + m=sqrtf(*log->esk_ptr); + else + m=*log->esk_ptr; + + log->calculated = m; + +// if(args->params.bits & MEASURAND_LIMITED_OUTPUT) + { + if(log->calculatedparams.minval) + { + log->calculated = 0; + m = 0; + } + } + + if(args->params.bits & MEASURAND_NO_PRI_SEC) + { + log->calculated_pri=log->calculated; + log->calculated_sec=log->calculated; + } + else + { + log->calculated_pri=m * log->an_params->znam_pierw; + log->calculated_sec=m * log->an_params->znam_wtor; + } +} diff --git a/src/measurand.h b/src/measurand.h new file mode 100644 index 0000000..d4ba550 --- /dev/null +++ b/src/measurand.h @@ -0,0 +1,62 @@ +/* + * measurand.h + * + * Created on: 07-10-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef MEASURAND_H_ +#define MEASURAND_H_ + +#include "tdefs.h" + +#define MAX_MEASURANDS_COUNT 64 + +struct measurands_table +{ + u16 count; + float *value[MAX_MEASURANDS_COUNT]; + float *value_pri[MAX_MEASURANDS_COUNT]; + float *value_sec[MAX_MEASURANDS_COUNT]; + u8 id[MAX_MEASURANDS_COUNT]; +}__attribute__((__packed__)); + +struct measurand_logic +{ + float *esk_ptr; + float calculated; + float calculated_pri; + float calculated_sec; + struct analog_in_params *an_params; +}; + +struct measurand_io +{ + u32 esk_float_in; + u32 param_an_ptr_in; +}__attribute__((__packed__)); + +#define MEASURAND_SQRT 0x01 +#define MEASURAND_NO_PRI_SEC 0x02 +#define MEASURAND_LIMITED_OUTPUT 0x04 + +struct measurand_params +{ + u32 id; + u32 bits; + double minval; +}__attribute__((__packed__)); + +struct measurand_args +{ + struct measurand_io io; + struct measurand_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern volatile struct measurands_table measurands; + +extern void measurand_100hz(void *args, void *logic); +extern int measurand_initlog(void *args, void *logic); + +#endif /* MEASURAND_H_ */ diff --git a/src/misc.h b/src/misc.h index e4ceb15..c28b226 100644 --- a/src/misc.h +++ b/src/misc.h @@ -12,9 +12,12 @@ #include "logman.h" #include "config.h" +/* #include #include #include +*/ +#include "mocks.h" #define MAX_AN_CARDS 16 #define MAX_BIN_CARDS 16 diff --git a/src/mki7.c b/src/mki7.c new file mode 100644 index 0000000..a60df28 --- /dev/null +++ b/src/mki7.c @@ -0,0 +1,56 @@ +/* + * mki7.c + * + * Created on: 20-03-2020 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "mki7.h" +#include "helper.h" + +volatile u8 mki7_sfplinks = 0xFF; + + +int mki7_initlog(void *arguments, void *logic) +{ + struct mki7_args *args = (struct mki7_args *)arguments; + struct mki7_logic *log = (struct mki7_logic *)logic; + + if(set_bit_ptr_struct(args->io.z141_out,&log->Z141)) + return -1; + if(set_bit_ptr_struct(args->io.z142_out,&log->Z142)) + return -1; + if(set_bit_ptr_struct(args->io.z143_out,&log->Z143)) + return -1; + if(set_bit_ptr_struct(args->io.z144_out,&log->Z144)) + return -1; + if(set_bit_ptr_struct(args->io.err_out,&log->err)) + return -1; + + return 0; +} + +void mki7_20hz(void *arguments, void *logic) +{ + struct mki7_logic *log = (struct mki7_logic *)logic; + + if(mki7_sfplinks == 0xFF) + { + set_struct(&log->err); + clear_struct(&log->Z141); + clear_struct(&log->Z142); + clear_struct(&log->Z143); + clear_struct(&log->Z144); + } + else + { + clear_struct(&log->err); + check_and_set_struct(mki7_sfplinks & 0x01,&log->Z141); + check_and_set_struct(mki7_sfplinks & 0x02,&log->Z142); + check_and_set_struct(mki7_sfplinks & 0x04,&log->Z143); + check_and_set_struct(mki7_sfplinks & 0x08,&log->Z144); + } +} diff --git a/src/mki7.h b/src/mki7.h new file mode 100644 index 0000000..46272e9 --- /dev/null +++ b/src/mki7.h @@ -0,0 +1,48 @@ +/* + * mki7.h + * + * Created on: 20-03-2020 + * Author: Krzysztof Jakubczyk + */ + +#ifndef MKI7_H_ +#define MKI7_H_ + +#include "tdefs.h" +#include "helper.h" + +struct mki7_logic +{ + struct binary_io Z141; + struct binary_io Z142; + struct binary_io Z143; + struct binary_io Z144; + struct binary_io err; +}; + +struct mki7_io +{ + u32 z141_out; + u32 z142_out; + u32 z143_out; + u32 z144_out; + u32 err_out; +}__attribute__((__packed__)); + +struct mki7_params +{ +}__attribute__((__packed__)); + +struct mki7_args +{ + struct mki7_io io; + struct mki7_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern volatile u8 mki7_sfplinks; + +extern int mki7_initlog(void *arguments, void *logic); +extern void mki7_20hz(void *arguments, void *logic); + +#endif /* MKI7 */ diff --git a/src/mki7_2.c b/src/mki7_2.c new file mode 100644 index 0000000..e2a794a --- /dev/null +++ b/src/mki7_2.c @@ -0,0 +1,55 @@ +/* + * mki7_2.c + * + * Created on: 02-08-2024 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "mki7_2.h" +#include "helper.h" + +volatile u8 mki7_2_sfplinks = 0xFF; + +int mki7_2_initlog(void *arguments, void *logic) +{ + struct mki7_2_args *args = (struct mki7_2_args *)arguments; + struct mki7_2_logic *log = (struct mki7_2_logic *)logic; + + if(set_bit_ptr_struct(args->io.z145_out,&log->Z145)) + return -1; + if(set_bit_ptr_struct(args->io.z146_out,&log->Z146)) + return -1; + if(set_bit_ptr_struct(args->io.z147_out,&log->Z147)) + return -1; + if(set_bit_ptr_struct(args->io.z148_out,&log->Z148)) + return -1; + if(set_bit_ptr_struct(args->io.err_out,&log->err)) + return -1; + + return 0; +} + +void mki7_2_20hz(void *arguments, void *logic) +{ + struct mki7_2_logic *log = (struct mki7_2_logic *)logic; + + if(mki7_2_sfplinks == 0xFF) + { + set_struct(&log->err); + clear_struct(&log->Z145); + clear_struct(&log->Z146); + clear_struct(&log->Z147); + clear_struct(&log->Z148); + } + else + { + clear_struct(&log->err); + check_and_set_struct(mki7_2_sfplinks & 0x01,&log->Z148); + check_and_set_struct(mki7_2_sfplinks & 0x02,&log->Z146); + check_and_set_struct(mki7_2_sfplinks & 0x04,&log->Z147); + check_and_set_struct(mki7_2_sfplinks & 0x08,&log->Z148); + } +} diff --git a/src/mki7_2.h b/src/mki7_2.h new file mode 100644 index 0000000..fed8676 --- /dev/null +++ b/src/mki7_2.h @@ -0,0 +1,48 @@ +/* + * mki7_2.h + * + * Created on: 02-08-2024 + * Author: Krzysztof Jakubczyk + */ + +#ifndef MKI7_2_H_ +#define MKI7_2_H_ + +#include "tdefs.h" +#include "helper.h" + +struct mki7_2_logic +{ + struct binary_io Z145; + struct binary_io Z146; + struct binary_io Z147; + struct binary_io Z148; + struct binary_io err; +}; + +struct mki7_2_io +{ + u32 z145_out; + u32 z146_out; + u32 z147_out; + u32 z148_out; + u32 err_out; +}__attribute__((__packed__)); + +struct mki7_2_params +{ +}__attribute__((__packed__)); + +struct mki7_2_args +{ + struct mki7_2_io io; + struct mki7_2_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern volatile u8 mki7_2_sfplinks; + +extern int mki7_2_initlog(void *arguments, void *logic); +extern void mki7_2_20hz(void *arguments, void *logic); + +#endif /* MKI7 */ diff --git a/src/mkstream.c b/src/mkstream.c new file mode 100644 index 0000000..99881fd --- /dev/null +++ b/src/mkstream.c @@ -0,0 +1,95 @@ +/* + * mkstream.c + * + * Created on: 22-03-2019 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" +#include "eth.h" +#include "helper.h" +#include "mkstream.h" +#include "analog_in.h" + +int mkstream_initlog(void *arguments, void *logic) +{ + struct mkstream_args *args = (struct mkstream_args *)arguments; + struct mkstream_logic *log = (struct mkstream_logic *)logic; + u8 i; + + if(set_float_ptr(args->io.A1L1_float_in,&log->A[0])) + return -1; + if(set_float_ptr(args->io.A1L2_float_in,&log->A[1])) + return -1; + if(set_float_ptr(args->io.A1L3_float_in,&log->A[2])) + return -1; + if(set_pointer_in_ptr(args->io.A1_params_ptr_in,(u32 *)&log->A_params)) + return -1; + + if(set_pointer_out_ptr(args->io.stream_ptr_out,(u32*)&log->stream)) + return -1; + + if(set_float_ptr(args->io.bits_float_in,(float *)&log->bits)) + return -1; + + for(i=0;i<8;i++) + if(set_bit_ptr_struct(args->io.gpbit_in[i],&log->gpbit[i])) + return -1; + + if(set_bit_ptr_struct(args->io.valid_sync_in,&log->valid_sync)) + return -1; + + if(set_float_ptr(args->io.A1L1_prev_float_in,&log->A_prev[0])) + return -1; + if(set_float_ptr(args->io.A1L2_prev_float_in,&log->A_prev[1])) + return -1; + if(set_float_ptr(args->io.A1L3_prev_float_in,&log->A_prev[2])) + return -1; + + log->stream.hdr[0] = 'T'; + log->stream.hdr[1] = 'Z'; + log->stream.addr = args->params.addr; + + if(args->params.addr>=ETH_MAX_RECV_STREAMS) + { + log->stream.addr = 0; + return -1; + } + + for(i=0;i<3;i++) + log->mul[i]=log->A_params->znam_pierw;/* / log->A_params->znam_wtor*/ + + return 0; +} + +void mkstream(void *arguments, void *logic) +{ + struct mkstream_logic *log = (struct mkstream_logic *)logic; + struct mkstream_args *args = (struct mkstream_args *)arguments; + + u32 i; + + for(i=0;i<3;i++) + { + log->stream.A[i]=*log->A[i] * log->mul[i]; + log->stream.A_prev[i]=*log->A_prev[i] * log->mul[i]; + } + + log->stream.bits = *log->bits & 0xFF; + + for(i=0;i<8;i++) + if(check_struct(&log->gpbit[i])) + log->stream.bits|=(256<stream.addr = args->params.addr; + log->stream.cksum=CKSUM_IV; + log->stream.sample_stamp=bus_an_cur_timestamp; + if(!check_struct(&log->valid_sync)) + log->stream.sample_stamp|=STREAM_NO_SYNC; + + for(i=0;istream.cksum+=*((u8*)&log->stream+i); +} diff --git a/src/mkstream.h b/src/mkstream.h new file mode 100644 index 0000000..58e9320 --- /dev/null +++ b/src/mkstream.h @@ -0,0 +1,71 @@ +/* + * mkstream.h + * + * Created on: 16-04-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef MKSTREAM_H_ +#define MKSTREAM_H_ + +#include "tdefs.h" +#include "helper.h" + +#define STATUS_FRAME_READ 0x80 +#define STREAM_NO_SYNC 0x8000 + +struct stream_frame +{ + u8 hdr[2]; + u16 sample_stamp; + float A[3]; + float A_prev[3]; + u16 bits; + u8 addr; + u8 cksum; +};//__attribute__((__packed__)); + +struct mkstream_logic +{ + float *A[3]; + float *A_prev[3]; + struct analog_in_params *A_params; + struct stream_frame stream; + struct binary_io gpbit[8]; + struct binary_io valid_sync; + u32 *bits; + float mul[3]; +}; + +struct mkstream_io +{ + u32 A1L1_float_in; + u32 A1L2_float_in; + u32 A1L3_float_in; + u32 A1_params_ptr_in; + u32 bits_float_in; + u32 gpbit_in[8]; + u32 valid_sync_in; + u32 A1L1_prev_float_in; + u32 A1L2_prev_float_in; + u32 A1L3_prev_float_in; + u32 stream_ptr_out; +}__attribute__((__packed__)); + + +struct mkstream_params +{ + u32 addr; +}__attribute__((__packed__)); + +struct mkstream_args +{ + struct mkstream_io io; + struct mkstream_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void mkstream(void *args, void *logic); +extern int mkstream_initlog(void *arguments, void *logic); + +#endif diff --git a/src/mocks.c b/src/mocks.c new file mode 100644 index 0000000..828d9f4 --- /dev/null +++ b/src/mocks.c @@ -0,0 +1,98 @@ +#include "mocks.h" +// DoÅ‚Ä…czamy oryginalny kod (lub kompilujemy go razem) +// Uwaga: w oryginalnym pliku C musisz zakomentować oryginalne #include i dodać #include "mocks.h" +#include "ZDistA_komp.c" + +// Struktury parametrów (muszÄ… być zgodne z tymi w kodzie, tu uproszczone deklaracje jeÅ›li brak plików .h) +// W rzeczywistoÅ›ci powinieneÅ› doÅ‚Ä…czyć oryginalne pliki nagłówkowe z definicjami struktur logic i args. + +int main() { + // 1. Alokacja struktur + struct ZDistA_komp_args args; + struct ZDistA_komp_logic logic; + memset(&args, 0, sizeof(args)); + memset(&logic, 0, sizeof(logic)); + + // 2. Symulacja sygnałów wejÅ›ciowych (Hardware Registers) + float sim_I1_orta, sim_I1_ortb, sim_I1; + float sim_U1_orta, sim_U1_ortb, sim_U1; + // ... (zadeklaruj resztÄ™: I2, I3, U2, U3, U12, I0, itd.) + + struct binary_io sim_bl_in = {0}; + struct binary_io sim_bl_k_in = {0}; + + // Parametry nastawcze (Dummy params) + struct param_I_struct pI = { .znam_wtor = 5.0, .znam_pierw = 400.0 }; + struct param_U_struct pU = { .znam_wtor = 100.0 }; + + // 3. Mapowanie wejść w args (to co robiÅ‚by system operacyjny) + args.io.i1_orta_float_in = &sim_I1_orta; + args.io.i1_ortb_float_in = &sim_I1_ortb; + args.io.i1_float_in = &sim_I1; // ModuÅ‚ prÄ…du + + args.io.u1_orta_float_in = &sim_U1_orta; + args.io.u1_ortb_float_in = &sim_U1_ortb; + // ... (przypisz resztÄ™ wskaźników dla wszystkich faz i skÅ‚adowych) + + args.io.i_param_an_ptr_in = (u32*)&pI; + args.io.u_param_an_ptr_in = (u32*)&pU; + + // Ustawienie nastaw zabezpieczeniowych (przykÅ‚adowe) + args.params.X1_Zr1_LE = 10.0; // 10 Ohm + args.params.R1_Zf1_LE = 5.0; + args.params.kp = 1.0; + args.params.fi1 = 75.0; // KÄ…t linii + // ... ustaw resztÄ™ nastaw (bity, strefy itd.) + + // 4. Inicjalizacja algorytmu + if (ZDistA_komp_initlog(&args, &logic) != 0) { + printf("BÅ‚Ä…d inicjalizacji!\n"); + return -1; + } + + // 5. Otwarcie plików z danymi + FILE *f_in = fopen("pomiary.csv", "r"); + FILE *f_out = fopen("wyniki.csv", "w"); + if (!f_in) { printf("Brak pliku pomiary.csv\n"); return -1; } + + fprintf(f_out, "Z1_R,Z1_X,Trip_L1E,Trip_L2E,Trip_L3E\n"); // Nagłówek + + // Bufor na liniÄ™ CSV + char line[1024]; + // PomiÅ„ nagłówek wejÅ›ciowy + fgets(line, sizeof(line), f_in); + + // 6. Główna pÄ™tla symulacyjna + int step = 0; + while (fgets(line, sizeof(line), f_in)) { + // Parsowanie CSV: ZakÅ‚adamy format: I1_Re, I1_Im, U1_Re, U1_Im... + // Musisz dostosować sscanf do swojego formatu pliku CSV + sscanf(line, "%f,%f,%f,%f", + &sim_I1_orta, &sim_I1_ortb, + &sim_U1_orta, &sim_U1_ortb); + + // Oblicz moduÅ‚y (algorytm wymaga też modułów w 'i1_float_in') + sim_I1 = sqrtf(sim_I1_orta*sim_I1_orta + sim_I1_ortb*sim_I1_ortb); + // ... oblicz resztÄ™ modułów + + // WYKONANIE ALGORYTMU + ZDistA_komp(&args, &logic); + + // Zapis wyników + // Odczytujemy ze struktury 'logic', tam gdzie algorytm zapisaÅ‚ wyniki + fprintf(f_out, "%f,%f,%d,%d,%d\n", + logic.dw.Z.R[0], // Impedancja L1-E Re + logic.dw.Z.X[0], // Impedancja L1-E Im + logic.P1W_L1E.val, // Pobudzenie strefy 1 L1-E + logic.P1W_L2E.val, + logic.P1W_L3E.val + ); + + step++; + } + + fclose(f_in); + fclose(f_out); + printf("Test zakoÅ„czony. Przetworzono %d próbek.\n", step); + return 0; +} diff --git a/src/mocks.h b/src/mocks.h new file mode 100644 index 0000000..199316e --- /dev/null +++ b/src/mocks.h @@ -0,0 +1,188 @@ +#ifndef MOCKS_H +#define MOCKS_H + +#include +#include +#include +#include +#include + +// 1. Definicje typów z tdefs.h +typedef uint8_t u8; +typedef uint16_t u16; +typedef uint32_t u32; + +// 2. Struktury pomocnicze (symulacja wejść/wyjść sprzÄ™towych) + +// Struktury parametrów analogowych (np. przekÅ‚adniki) +struct param_I_struct { + float znam_wtor; // np. 1.0 lub 5.0 A + float znam_pierw; // np. 400.0 A +}; +struct param_U_struct { + float znam_wtor; // np. 100.0 V + float znam_pierw; // np. 110000.0 V +}; +// 3. Mockowanie funkcji systemowych z helper.h +// Funkcje te w oryginale mapujÄ… pamięć. Na PC po prostu przypisujemy adresy. +#include + +// static inline int set_float_ptr(float *in_ptr, float **logic_ptr) { +// *logic_ptr = in_ptr; // Przypisz adres zmiennej symulowanej do wskaźnika w logice +// return 0; +// } + +// static inline int set_bit_ptr_struct(struct binary_io *in_ptr, struct binary_io **logic_ptr) { +// *logic_ptr = in_ptr; +// return 0; +// } + +// ObsÅ‚uga wskaźników (void*) +// static inline int set_pointer_in_ptr(u32 *in_ptr, u32 **logic_ptr) { +// *logic_ptr = (u32*)in_ptr; +// return 0; +// } + +// static inline int set_pointer_out_ptr(u32 *out_ptr, u32 **logic_ptr) { +// *logic_ptr = (u32*)out_ptr; +// return 0; +// } + +// // Funkcja pomocnicza check_struct +// static inline int check_struct(struct binary_io *ptr) { +// return ptr->val; +// } + +// // Funkcja ustawiajÄ…ca wyjÅ›cia +// static inline void check_and_set_struct(u8 val, struct binary_io **out_ptr) { +// if (*out_ptr) (*out_ptr)->val = val; +// } +// static inline void clear_struct(struct binary_io **out_ptr) { +// if (*out_ptr) (*out_ptr)->val = 0; +// } + +// 4. Funkcje matematyczne specyficzne dla platformy +// _rcpsp to zazwyczaj "reciprocal square root" (1/sqrt) +static inline float _rcpsp(float x) { + if (x == 0.0f) return 0.0f; // Zabezpieczenie + return 1.0f / sqrtf(x); +} + +// Funkcje fazy (uproszczone dla testu) +static inline float get_phase(float im, float re) { + return atan2f(im, re) * 180.0f / 3.14159265f; +} + +static inline float get_phase_diff(float p1, float p2) { + float diff = p1 - p2; + if (diff > 180.0f) diff -= 360.0f; + if (diff < -180.0f) diff += 360.0f; + return diff; +} + +// Funkcja sprawdzajÄ…ca opóźnienia (timer) +// W teÅ›cie na PC musisz symulować upÅ‚yw czasu. +// Tu prosta implementacja inkrementujÄ…ca licznik. +// static inline void sprawdz_P(u8 *out, u8 cond_start, u8 cond_reset, short *counter, int time_set, int time_reset) { +// if (cond_reset) { +// *counter = 0; +// *out = 0; +// } else if (cond_start) { +// if (*counter < time_set) (*counter)++; +// if (*counter >= time_set) *out = 1; +// } else { +// // Histereza/podtrzymanie (uproszczone) +// if (*counter > 0) (*counter)--; +// if (*counter == 0) *out = 0; +// } +// } + +// Atrapa funkcji czy_test_R +static inline int czy_test_R() { return 0; } + + + +// Struktura dla pojedynczej strefy wielokÄ…tnej (poligonalnej) +struct n_pol_struct { + float Rr, Xr; // Granice Rezystancji i Reaktancji (prawo/góra) + float Rp, Xp; // Granice (lewo/dół lub po przesuniÄ™ciu kp) + float Z, Zp; // Impedancja graniczna (moduÅ‚) + float Rrtanfi1, Rptanfi1; // Granice nachylone +}; + +// Struktura wyjÅ›cia +struct wyjscie_struct { + float przekladnia; + void *Zdist_dw; // Wskaźnik void na log->dw + int *on; // Wskaźnik na log->nast_.on_ + u8 SOTF_zwrotnie; +}; + +// Struktura obliczonych nastaw (log->nast_) +struct nast_struct { + int on_; + int bl_; + int Bl_L[6]; // Blokady Load + int z6_kolo; + + float ReK1, ImK1; + float ReKr, ImKr; + float ReKrown, ImKrown; + float Krown_ignac; + + float kp, kpp, kpk; + float kp_obc, kpp_obc; + + int typ[6]; // Typy stref + float tanfi1, tanfi2; + + struct n_pol_struct n_pol[6][2]; // [strefa][typ pÄ™tli: 0=fazowa, 1=miedzyfazowa] + + float Xr1f, Xr1fp; + float Xr1Wf, Xr1Wfp; + float Igr, Igrp; + float Zgr; + int kierunek[6]; + + float XKR, XKX, KL; + float RLf, RLr; + float Iogr, khio, Uomin; + + struct wyjscie_struct wyjscie; +}; + +// Struktura zmiennych wewnÄ™trznych (pomiary, flagi, liczniki) -> log->dw +struct Z_measurement { + float R[9], X[9], Z[9], E[9]; // 9 pÄ™tli pomiarowych + float absR[9], absX[9]; + float XRtanfi1_noabs[9], XRtanfi2[9]; +}; + +struct dw_struct { + float I1, I2, I3, U1, U2, U3; + struct Z_measurement Z; + u8 Igr[6]; // Warunki prÄ…dowe + + void *log_ptr; // Wskaźnik zwrotny na logic + + u8 XKp[6][9], XKm[6][9]; // Kierunkowość + u8 BL_Load[9]; // Blokada od obciążenia + u8 Zm[9]; // Zmienne pomocnicze + + // Liczniki czasu (timery) + short liczpIo; + short liczp11[6][9], liczp12[6][9], liczp10[9]; + short liczps[6][9]; + short liczKdod; + + u8 Iogr; // Przekroczenie I0 granicznego + u8 Kp, Km; // Kierunek (+/-) + + u8 P_s[6][9]; // Pobudzenia surowe + u8 P_sss[6][9]; // Pobudzenia super surowe? + u8 Pbk[6][9]; // Pobudzenia bezkierunkowe + u8 P[6][9]; // Pobudzenia finalne (kierunkowe) +}; + + +#endif diff --git a/src/mul_float.c b/src/mul_float.c new file mode 100644 index 0000000..58801ef --- /dev/null +++ b/src/mul_float.c @@ -0,0 +1,36 @@ +/* + * mul_float.c + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "mul_float.h" + +int mul_initlog(void *arguments, void *logic) +{ + struct mul_args *args = (struct mul_args *)arguments; + struct mul_logic *log = (struct mul_logic *)logic; + + if(set_float_ptr(args->io.in_float_in,&log->fin)) + return -1; + + if(set_float_ptr(args->io.out_float_out,&log->fout)) + return -1; + + return 0; +} + +void mul(void *arguments, void *logic) +{ + struct mul_logic *log = (struct mul_logic *)logic; + struct mul_args *args = (struct mul_args *)arguments; + + *log->fout=*log->fin*args->params.multiplier; +} diff --git a/src/mul_float.h b/src/mul_float.h new file mode 100644 index 0000000..723deb7 --- /dev/null +++ b/src/mul_float.h @@ -0,0 +1,40 @@ +/* + * mul_float.h + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef MUL_FLOAT_H_ +#define MUL_FLOAT_H_ + +#include "tdefs.h" + +struct mul_logic +{ + float *fin; + float *fout; +}; + +struct mul_io +{ + u32 in_float_in; + u32 out_float_out; +}__attribute__((__packed__)); + +struct mul_params +{ + double multiplier; +}__attribute__((__packed__)); + +struct mul_args +{ + struct mul_io io; + struct mul_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void mul(void *args, void *logic); +extern int mul_initlog(void *arguments, void *logic); + +#endif /* MUL_FLOAT_H_ */ diff --git a/src/mux_bin.c b/src/mux_bin.c new file mode 100644 index 0000000..417b030 --- /dev/null +++ b/src/mux_bin.c @@ -0,0 +1,41 @@ +/* + * mux_bin.c + * + * Created on: 29-03-2018 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "mux_bin.h" + +int mux_bin_initlog(void *arguments, void *logic) +{ + struct mux_bin_args *args = (struct mux_bin_args *)arguments; + struct mux_bin_logic *log = (struct mux_bin_logic *)logic; + + if(set_bit_ptr_struct(args->io.we1_in,&log->we1)) + return -1; + if(set_bit_ptr_struct(args->io.we2_in,&log->we2)) + return -1; + if(set_bit_ptr_struct(args->io.a_in,&log->a)) + return -1; + if(set_bit_ptr_struct(args->io.wy_out,&log->wy)) + return -1; + + return 0; +} + +void mux_bin(void *arguments, void *logic) +{ + struct mux_bin_logic *log = (struct mux_bin_logic *)logic; + + if(!check_struct(&log->a)) + check_and_set_struct(check_struct(&log->we1),&log->wy); + else + check_and_set_struct(check_struct(&log->we2),&log->wy); +} diff --git a/src/mux_bin.h b/src/mux_bin.h new file mode 100644 index 0000000..1e8e31a --- /dev/null +++ b/src/mux_bin.h @@ -0,0 +1,43 @@ +/* + * mux_bin.h + * + * Created on: 29-03-2018 + * Author: Krzysztof Jakubczyk + */ + +#ifndef MUX_BIN_H_ +#define MUX_BIN_H_ + +#include "tdefs.h" + +struct mux_bin_logic +{ + struct binary_io we1; + struct binary_io we2; + struct binary_io a; + struct binary_io wy; +}; + +struct mux_bin_io +{ + u32 we1_in; + u32 we2_in; + u32 a_in; + u32 wy_out; +}__attribute__((__packed__)); + +struct mux_bin_params +{ +}__attribute__((__packed__)); + +struct mux_bin_args +{ + struct mux_bin_io io; + struct mux_bin_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void mux_bin(void *args, void *logic); +extern int mux_bin_initlog(void *arguments, void *logic); + +#endif /* MUX_BIN_H_ */ diff --git a/src/mux_float.c b/src/mux_float.c new file mode 100644 index 0000000..a572065 --- /dev/null +++ b/src/mux_float.c @@ -0,0 +1,42 @@ +/* + * mux_float.c + * + * Created on: 28-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "mux_float.h" + +int mux_float_initlog(void *arguments, void *logic) +{ + struct mux_float_args *args = (struct mux_float_args *)arguments; + struct mux_float_logic *log = (struct mux_float_logic *)logic; + + if(set_float_ptr(args->io.in1_float_in,&log->in1)) + return -1; + + if(set_float_ptr(args->io.in2_float_in,&log->in2)) + return -1; + + if(set_bit_ptr_struct(args->io.addr_in,&log->addr)) + return -1; + + if(set_float_ptr(args->io.out_float_out,&log->out)) + return -1; + + return 0; +} + +void mux_float(void *arguments, void *logic) +{ + struct mux_float_logic *log = (struct mux_float_logic *)logic; +// struct mux_float_args *args = (struct mux_float_args *)arguments; + + *log->out=check_struct(&log->addr)?*log->in2:*log->in1; +} diff --git a/src/mux_float.h b/src/mux_float.h new file mode 100644 index 0000000..5f054f3 --- /dev/null +++ b/src/mux_float.h @@ -0,0 +1,43 @@ +/* + * mux_float.h + * + * Created on: 28-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef MUX_FLOAT_H_ +#define MUX_FLOAT_H_ + +#include "tdefs.h" + +struct mux_float_logic +{ + float *in1; + float *in2; + struct binary_io addr; + float *out; +}; + +struct mux_float_io +{ + u32 in1_float_in; + u32 in2_float_in; + u32 addr_in; + u32 out_float_out; +}__attribute__((__packed__)); + +struct mux_float_params +{ +}__attribute__((__packed__)); + +struct mux_float_args +{ + struct mux_float_io io; + struct mux_float_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void mux_float(void *args, void *logic); +extern int mux_float_initlog(void *arguments, void *logic); + +#endif /* MUX_FLOAT_H_ */ diff --git a/src/mux_t.c b/src/mux_t.c new file mode 100644 index 0000000..2340fba --- /dev/null +++ b/src/mux_t.c @@ -0,0 +1,143 @@ +/* + * mux_t.c + * + * Created on: 02-02-2017 + * Author: PS + */ + +#include + +//#include "pawel_usun_to.h" + +#include "mux_t.h" + +#include "helper.h" +#include "config.h" + +int mux_t_initlog(void *arguments, void *logic) +{ + struct mux_t_args *args = (struct mux_t_args *)arguments; + struct mux_t_logic *log = (struct mux_t_logic *)logic; + + int i = 0; + //wejscia + for (i = 0; i < 21; i++) + { + if(set_float_ptr(args->io.we_in[i],&log->we[i])) + return -1; + } + if (set_bit_ptr_mask(args->io.wb1_in, &log->wb1, &log->wb1_bit_mask)) + return -1; + if (set_bit_ptr_mask(args->io.wb2_in, &log->wb2, &log->wb2_bit_mask)) + return -1; + if (set_bit_ptr_mask(args->io.wb3_in, &log->wb3, &log->wb3_bit_mask)) + return -1; + if (set_bit_ptr_mask(args->io.wyl_in, &log->wyl, &log->wyl_bit_mask)) + return -1; + + //wyjscia + for (i = 0; i < 7; i++) + { + if(set_float_ptr(args->io.wy_out[i],&log->wy[i])) + return -1; + } + if (set_bit_ptr_mask(args->io.wb_mux_out, &log->wb_mux, &log->wb_mux_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wyl_s1_out, &log->wyl_s1 ,&log->wyl_s1_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wyl_s2_out, &log->wyl_s2 ,&log->wyl_s2_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wyl_s3_out, &log->wyl_s3 ,&log->wyl_s3_bit_mask)) + return -1; + + log->wyl_calosc = (args->params.bity & 0x0001) ? 1: 0; + log->strona = (u8)(args->params.strona); + + return 0; +} +void mux_t(void *arguments, void *logic) +{ + struct mux_t_logic *log = (struct mux_t_logic *)logic; + + // multiplekser wejsc analogowych (3 zestawy wejsc po 7) + int i,start = 0; + + if (log->strona == 1) + start = 7; + else if (log->strona == 2) + start = 14; + else + start = 0; + + for (i = 0; i < 7; i++) + { + *log->wy[i] = *log->we[i+start]; + } + + // multiplekser binarny + switch (log->strona) + { + case 0: + if ((*log->wb1 & log->wb1_bit_mask) != 0) + *log->wb_mux |= log->wb_mux_bit_mask; + else + *log->wb_mux &= ~log->wb_mux_bit_mask; + break; + case 1: + if ((*log->wb2 & log->wb2_bit_mask) != 0) + *log->wb_mux |= log->wb_mux_bit_mask; + else + *log->wb_mux &= ~log->wb_mux_bit_mask; + break; + case 2: + if ((*log->wb3 & log->wb3_bit_mask) != 0) + *log->wb_mux |= log->wb_mux_bit_mask; + else + *log->wb_mux &= ~log->wb_mux_bit_mask; + break; + default: + break; + } + + // demultiplekser binarny + u8 we_wyl = ((*log->wyl & log->wyl_bit_mask) != 0) ? 1 : 0; + if (we_wyl) + { + if (log->wyl_calosc) + { + *log->wyl_s1 |= log->wyl_s1_bit_mask; + *log->wyl_s2 |= log->wyl_s2_bit_mask; + *log->wyl_s3 |= log->wyl_s3_bit_mask; + } + else + { + switch (log->strona) + { + case 0: + *log->wyl_s1 |= log->wyl_s1_bit_mask; + *log->wyl_s2 &= ~log->wyl_s2_bit_mask; + *log->wyl_s3 &= ~log->wyl_s3_bit_mask; + break; + case 1: + *log->wyl_s1 &= ~log->wyl_s1_bit_mask; + *log->wyl_s2 |= log->wyl_s2_bit_mask; + *log->wyl_s3 &= ~log->wyl_s3_bit_mask; + break; + case 2: + *log->wyl_s1 &= ~log->wyl_s1_bit_mask; + *log->wyl_s2 &= ~log->wyl_s2_bit_mask; + *log->wyl_s3 |= log->wyl_s3_bit_mask; + break; + default: + break; + } + } + } + else + { + *log->wyl_s1 &= ~log->wyl_s1_bit_mask; + *log->wyl_s2 &= ~log->wyl_s2_bit_mask; + *log->wyl_s3 &= ~log->wyl_s3_bit_mask; + } +} + diff --git a/src/mux_t.h b/src/mux_t.h new file mode 100644 index 0000000..b3ef240 --- /dev/null +++ b/src/mux_t.h @@ -0,0 +1,79 @@ +/* + * mux_t.h + * + * Created on: 02-02-2017 + * Author: PS + */ + +#ifndef MUX_T_ +#define MUX_T_ + +#include "tdefs.h" + + +struct mux_t_logic +{ + //wejscia + float *we[21]; + + u8 *wb1; + u8 wb1_bit_mask; + u8 *wb2; + u8 wb2_bit_mask; + u8 *wb3; + u8 wb3_bit_mask; + + u8 *wyl; + u8 wyl_bit_mask; + + //wyjscia + float *wy[7]; + + u8 *wb_mux; + u8 wb_mux_bit_mask; + u8 *wyl_s1; + u8 wyl_s1_bit_mask; + u8 *wyl_s2; + u8 wyl_s2_bit_mask; + u8 *wyl_s3; + u8 wyl_s3_bit_mask; + + //nastawy + u8 strona; + u8 wyl_calosc; +}; + +struct mux_t_io +{ + //wejscia + u32 we_in[21]; + u32 wb1_in; + u32 wb2_in; + u32 wb3_in; + u32 wyl_in; + //wyjscia + u32 wy_out[7]; + u32 wb_mux_out; + u32 wyl_s1_out; + u32 wyl_s2_out; + u32 wyl_s3_out; + +}__attribute__((__packed__)); + +struct mux_t_params +{ + u32 bity; // nastawy bitowe + u32 strona; +}__attribute__((__packed__)); + +struct mux_t_args +{ + struct mux_t_io io; + struct mux_t_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void mux_t(void *args, void *logic); +extern int mux_t_initlog(void *arguments, void *logic); + +#endif /* MUX_T_ */ diff --git a/src/nand.c b/src/nand.c new file mode 100644 index 0000000..e8c7f58 --- /dev/null +++ b/src/nand.c @@ -0,0 +1,52 @@ +/* + * nand.c + * + * Created on: 29-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "nand.h" + +int nand_initlog(void *arguments, void *logic) +{ + struct nand_args *args = (struct nand_args *)arguments; + struct nand_logic *log = (struct nand_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + log->in1_ptr = log_manager.nets_data + (args->io.in1 >> 3); + log->in1_bit_no = args->io.in1 & 0x07; + + log->in2_ptr = log_manager.nets_data + (args->io.in2 >> 3); + log->in2_bit_no = args->io.in2 & 0x07; + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in1_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in2_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void nand(void *arguments, void *logic) +{ + struct nand_args *args = (struct nand_args *)arguments; + struct nand_logic *log = (struct nand_logic *)logic; + + if(args->io.out) + { + if(!( (*log->in1_ptr & (1<in1_bit_no)) && (*log->in2_ptr & (1<in2_bit_no)) )) + *log->out_ptr|=(1<out_bit_no); + else + *log->out_ptr&=~(1<out_bit_no); + } +} diff --git a/src/nand.h b/src/nand.h new file mode 100644 index 0000000..6a0444a --- /dev/null +++ b/src/nand.h @@ -0,0 +1,46 @@ +/* + * nand.h + * + * Created on: 29-04-2014 + * Author: KJ + */ + +#ifndef NAND_H_ +#define NAND_H_ + +#include "tdefs.h" + +struct nand_logic +{ + u8 *out_ptr; + u8 out_bit_no; + + u8 *in1_ptr; + u8 in1_bit_no; + + u8 *in2_ptr; + u8 in2_bit_no; +}; + +struct nand_io +{ + u32 in1; + u32 in2; + u32 out; +}__attribute__((__packed__)); + +struct nand_params +{ +}__attribute__((__packed__)); + +struct nand_args +{ + struct nand_io io; + struct nand_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void nand(void *args, void *logic); +extern int nand_initlog(void *args, void *logic); + +#endif /* NAND_H_ */ diff --git a/src/nor.c b/src/nor.c new file mode 100644 index 0000000..f8965c4 --- /dev/null +++ b/src/nor.c @@ -0,0 +1,52 @@ +/* + * nor.c + * + * Created on: 29-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "nor.h" + +int nor_initlog(void *arguments, void *logic) +{ + struct nor_args *args = (struct nor_args *)arguments; + struct nor_logic *log = (struct nor_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + log->in1_ptr = log_manager.nets_data + (args->io.in1 >> 3); + log->in1_bit_no = args->io.in1 & 0x07; + + log->in2_ptr = log_manager.nets_data + (args->io.in2 >> 3); + log->in2_bit_no = args->io.in2 & 0x07; + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in1_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in2_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void nor(void *arguments, void *logic) +{ + struct nor_args *args = (struct nor_args *)arguments; + struct nor_logic *log = (struct nor_logic *)logic; + + if(args->io.out) + { + if(!( (*log->in1_ptr & (1<in1_bit_no)) || (*log->in2_ptr & (1<in2_bit_no)) )) + *log->out_ptr|=(1<out_bit_no); + else + *log->out_ptr&=~(1<out_bit_no); + } +} diff --git a/src/nor.h b/src/nor.h new file mode 100644 index 0000000..dfcf252 --- /dev/null +++ b/src/nor.h @@ -0,0 +1,47 @@ +/* + * nor.h + * + * + * Created on: 29-04-2014 + * Author: KJ + */ + +#ifndef NOR_H_ +#define NOR_H_ + +#include "tdefs.h" + +struct nor_logic +{ + u8 *out_ptr; + u8 out_bit_no; + + u8 *in1_ptr; + u8 in1_bit_no; + + u8 *in2_ptr; + u8 in2_bit_no; +}; + +struct nor_io +{ + u32 in1; + u32 in2; + u32 out; +}__attribute__((__packed__)); + +struct nor_params +{ +}__attribute__((__packed__)); + +struct nor_args +{ + struct nor_io io; + struct nor_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void nor(void *args, void *logic); +extern int nor_initlog(void *args, void *logic); + +#endif /* NOR_H_ */ diff --git a/src/not.c b/src/not.c new file mode 100644 index 0000000..cbac379 --- /dev/null +++ b/src/not.c @@ -0,0 +1,44 @@ +/* + * not.c + * + * Created on: 29-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "not.h" + +int not_initlog(void *arguments, void *logic) +{ + struct not_args *args = (struct not_args *)arguments; + struct not_logic *log = (struct not_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_mask = 1<<(args->io.out & 0x07); + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_mask = 1<<(args->io.in & 0x07); + + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void not(void *arguments, void *logic) +{ +// struct not_args *args = (struct not_args *)arguments; + struct not_logic *log = (struct not_logic *)logic; + + if(*log->in_ptr & log->in_bit_mask) + *log->out_ptr&=~log->out_bit_mask; + else + *log->out_ptr|=log->out_bit_mask; +} diff --git a/src/not.h b/src/not.h new file mode 100644 index 0000000..2cd2b1e --- /dev/null +++ b/src/not.h @@ -0,0 +1,43 @@ +/* + * not.h + * + * + * Created on: 29-04-2014 + * Author: KJ + */ + +#ifndef NOT_H_ +#define NOT_H_ + +#include "tdefs.h" + +struct not_logic +{ + u8 *out_ptr; + u8 out_bit_mask; + + u8 *in_ptr; + u8 in_bit_mask; +}; + +struct not_io +{ + u32 in; + u32 out; +}__attribute__((__packed__)); + +struct not_params +{ +}__attribute__((__packed__)); + +struct not_args +{ + struct not_io io; + struct not_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void not(void *args, void *logic); +extern int not_initlog(void *args, void *logic); + +#endif /* NOT_H_ */ diff --git a/src/nr_zacz.c b/src/nr_zacz.c new file mode 100644 index 0000000..ec075ee --- /dev/null +++ b/src/nr_zacz.c @@ -0,0 +1,151 @@ +/* + * demux8.c + * + * Created on: 22-05-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "nr_zacz.h" + +int nr_zacz_initlog(void *arguments, void *logic) +{ + struct nr_zacz_args *args = (struct nr_zacz_args *)arguments; + struct nr_zacz_logic *log = (struct nr_zacz_logic *)logic; + + if(set_bit_ptr_struct(args->io.b0_in,&log->b0)) + return -1; + if(set_bit_ptr_struct(args->io.b1_in,&log->b1)) + return -1; + if(set_bit_ptr_struct(args->io.b2_in,&log->b2)) + return -1; + if(set_bit_ptr_struct(args->io.b3_in,&log->b3)) + return -1; + if(set_bit_ptr_struct(args->io.b4_in,&log->b4)) + return -1; + if(set_bit_ptr_struct(args->io.b5_in,&log->b5)) + return -1; + + if(set_float_ptr(args->io.nr_zacz_out,&log->nr_zacz_out)) + return -1; + + if(set_bit_ptr_struct(args->io.err_out,&log->err)) + return -1; + if(set_bit_ptr_struct(args->io.min_out,&log->min)) + return -1; + if(set_bit_ptr_struct(args->io.max_out,&log->max)) + return -1; + if(set_bit_ptr_struct(args->io.zo_zacz_out,&log->zo_zacz)) + return -1; + + log->t_odczytu = 0; + + return 0; +} + +void nr_zacz(void *arguments, void *logic) +{ + struct nr_zacz_args *args = (struct nr_zacz_args *)arguments; + struct nr_zacz_logic *log = (struct nr_zacz_logic *)logic; + u8 choice,bcd1; + + switch(args->params.typ_numer) ///< nastawa trybu + { + case BCD: + //BCD + choice = check_struct(&log->b0)?1:0; + choice |= check_struct(&log->b1)?2:0; + choice |= check_struct(&log->b2)?4:0; + choice |= check_struct(&log->b3)?8:0; + bcd1 = choice;//tylko do ustalenia czy BCD jednosci > 9 + choice += check_struct(&log->b4)?10:0; + choice += check_struct(&log->b5)?20:0; + break; + case BIN: + //BIN + choice = check_struct(&log->b0)?1:0; + choice |= check_struct(&log->b1)?2:0; + choice |= check_struct(&log->b2)?4:0; + choice |= check_struct(&log->b3)?8:0; + bcd1 = 0;//tylko do ustalenia czy BCD jednosci > 9 + choice |= check_struct(&log->b4)?16:0; + choice |= check_struct(&log->b5)?32:0; + break; + case GRAY: + //GRAY + bcd1 = 0;//tylko do ustalenia czy BCD jednosci > 9 + choice = check_struct(&log->b5)?0x20:0; + choice |= check_struct(&log->b4)?((~choice & (1<<5))>>1):((choice & (1<<5))>>1); + choice |= check_struct(&log->b3)?((~choice & (1<<4))>>1):((choice & (1<<4))>>1); + choice |= check_struct(&log->b2)?((~choice & (1<<3))>>1):((choice & (1<<3))>>1); + choice |= check_struct(&log->b1)?((~choice & (1<<2))>>1):((choice & (1<<2))>>1); + choice |= check_struct(&log->b0)?((~choice & (1<<1))>>1):((choice & (1<<1))>>1); + break; + default: + choice = 0; + break; + } + // + + + //if (!check_struct(&log->err)) + { + //if *log->nr_zacz_out-(float)choice //jak różnica o 1 to przepisz na log->nr_zacz_out + //if(fabs(*log->nr_zacz_out-choice)<1.5) + //if (log->last_choice == choice) +/* + if ((u8)*log->nr_zacz_out == choice) + { + *log->nr_zacz_out = (float)choice; + log->t_odczytu = 0; + clear_struct(&log->zo_zacz); + check_and_set_struct((choice <= args->params.nr_min_zaczep),&log->min); + check_and_set_struct((choice >= args->params.nr_max_zaczep),&log->max); + check_and_set_struct(((choice < args->params.nr_min_zaczep)||(choice > args->params.nr_max_zaczep)||(bcd1>9)) ,&log->err); + } + else + { + set_struct(&log->zo_zacz); + if (log->last_choice == choice) + log->t_odczytu+=LOOP_CYCLE_MS; + else + log->t_odczytu = 0; + log->last_choice = choice; + if (log->t_odczytu > ((args->params.t_op_zacz)*1000)) + *log->nr_zacz_out = (float)choice; + } +*/ + if (log->last_choice == choice) + { + log->t_odczytu+=LOOP_CYCLE_MS; + if (log->t_odczytu > ((args->params.t_op_zacz)*1000)) + { + check_and_set_struct(((choice < args->params.nr_min_zaczep)||(choice > args->params.nr_max_zaczep)||(bcd1>9)) ,&log->err); + if (!check_struct(&log->err)) + { + *log->nr_zacz_out = (float)choice; + check_and_set_struct((choice <= args->params.nr_min_zaczep),&log->min); + check_and_set_struct((choice >= args->params.nr_max_zaczep),&log->max); + } + log->t_odczytu=LOOP_CYCLE_MS; + clear_struct(&log->zo_zacz); + } + } + else + { + set_struct(&log->zo_zacz); + log->t_odczytu = 0; + log->last_choice = choice; + } + + + + } + + +} diff --git a/src/nr_zacz.h b/src/nr_zacz.h new file mode 100644 index 0000000..4a7e0ba --- /dev/null +++ b/src/nr_zacz.h @@ -0,0 +1,72 @@ +/* + * nr_zacz.h + * + * Created on: 10-02-2021 + * Author: Sergiusz Trzcionka + */ + +#ifndef NR_ZACZ_H_ +#define NR_ZACZ_H_ + +#include "tdefs.h" + +enum _typ_nrumer +{ + BCD = 0, + BIN = 1, + GRAY = 2 +}; + +struct nr_zacz_logic +{ + struct binary_io b0; + struct binary_io b1; + struct binary_io b2; + struct binary_io b3; + struct binary_io b4; + struct binary_io b5; + float *nr_zacz_out; + struct binary_io err; + struct binary_io min; + struct binary_io max; + struct binary_io zo_zacz; + + int t_odczytu; /// czas odczytu + u8 last_choice; /// czas odczytu + + +}; + +struct nr_zacz_io +{ + u32 b0_in; + u32 b1_in; + u32 b2_in; + u32 b3_in; + u32 b4_in; + u32 b5_in; + u32 nr_zacz_out; + u32 err_out; + u32 min_out; + u32 max_out; + u32 zo_zacz_out; +}__attribute__((__packed__)); + +struct nr_zacz_params +{ + u32 nr_min_zaczep; ///< nastawa minimalnego nr zaczepu + u32 nr_max_zaczep; ///< nastawa maxymalnego nr zaczepu + double t_op_zacz; + u32 typ_numer; /// tryb numeracji BCD/BIN/GRAY +}__attribute__((__packed__)); + +struct nr_zacz_args +{ + struct nr_zacz_io io; + struct nr_zacz_params params; +}__attribute__((__packed__)); + +extern void nr_zacz(void *args, void *logic); +extern int nr_zacz_initlog(void *arguments, void *logic); + +#endif /* NR_ZACZ_H_ */ diff --git a/src/od_bl.c b/src/od_bl.c new file mode 100644 index 0000000..5750e09 --- /dev/null +++ b/src/od_bl.c @@ -0,0 +1,67 @@ +/* + * or4.c + * + * Created on: 07-04-2025 + * Author: Pawel Skoczykloda + */ + + +#include +#include "tdefs.h" +#include "misc.h" +#include "helper.h" + + +#include "od_bl.h" + +int od_bl_initlog(void *arguments, void *logic) +{ + struct od_bl_args *args = (struct od_bl_args *)arguments; + struct od_bl_logic *log = (struct od_bl_logic *)logic; + + if(set_bit_ptr_mask(args->io.in1_odst_in,&log->in_odst[0],&log->in_odst_bit_mask[0])) + return -1; + if(set_bit_ptr_mask(args->io.in2_odst_in,&log->in_odst[1],&log->in_odst_bit_mask[1])) + return -1; + if(set_bit_ptr_mask(args->io.in3_odst_in,&log->in_odst[2],&log->in_odst_bit_mask[2])) + return -1; + + if(set_bit_ptr_mask(args->io.in1_bl_in,&log->in_bl[0],&log->in_bl_bit_mask[0])) + return -1; + if(set_bit_ptr_mask(args->io.in2_bl_in,&log->in_bl[1],&log->in_bl_bit_mask[1])) + return -1; + if(set_bit_ptr_mask(args->io.in3_bl_in,&log->in_bl[2],&log->in_bl_bit_mask[2])) + return -1; + + if(set_bit_ptr_mask(args->io.bl_out,&log->out_bl,&log->out_bl_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.odst_out,&log->out_odst,&log->out_odst_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.od_bl_out,&log->out_od_bl,&log->out_od_bl_bit_mask)) + return -1; + + log->odstawione = (args->params.bity != 0) ? 1 : 0; + + return 0; +} + +void od_bl(void *arguments, void *logic) +{ + struct od_bl_logic *log = (struct od_bl_logic *)logic; + + u8 temp_odst, temp_blok; + + temp_odst = log->odstawione || (*log->in_odst[0] & log->in_odst_bit_mask[0]) || + (*log->in_odst[1] & log->in_odst_bit_mask[1]) || (*log->in_odst[2] & log->in_odst_bit_mask[2]); + + if (temp_odst) + temp_blok = 0; + else + temp_blok = (*log->in_bl[0] & log->in_bl_bit_mask[0]) || (*log->in_bl[1] & log->in_bl_bit_mask[1]) || + (*log->in_bl[2] & log->in_bl_bit_mask[2]); + + check_and_set_mask(temp_odst,log->out_odst,log->out_odst_bit_mask); + check_and_set_mask(temp_blok,log->out_bl,log->out_bl_bit_mask); + check_and_set_mask((temp_blok || temp_odst),log->out_od_bl,log->out_od_bl_bit_mask); + +} diff --git a/src/od_bl.h b/src/od_bl.h new file mode 100644 index 0000000..a030e47 --- /dev/null +++ b/src/od_bl.h @@ -0,0 +1,60 @@ +/* + * or4.h + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OD_BL_H_ +#define OD_BL_H_ + + +#include "tdefs.h" + +//#include "pawel_usun_to.h" + +struct od_bl_logic +{ + u8 *in_odst[3]; + u8 in_odst_bit_mask[3]; + u8 *in_bl[3]; + u8 in_bl_bit_mask[3]; + u8 *out_odst; + u8 out_odst_bit_mask; + u8 *out_bl; + u8 out_bl_bit_mask; + u8 *out_od_bl; + u8 out_od_bl_bit_mask; + + u8 odstawione; +}; + +struct od_bl_io +{ + u32 in1_odst_in; + u32 in2_odst_in; + u32 in3_odst_in; + u32 in1_bl_in; + u32 in2_bl_in; + u32 in3_bl_in; + u32 odst_out; + u32 bl_out; + u32 od_bl_out; +}__attribute__((__packed__)); + +struct od_bl_params +{ + u32 bity; +}__attribute__((__packed__)); + +struct od_bl_args +{ + struct od_bl_io io; + struct od_bl_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void od_bl(void *args, void *logic); +extern int od_bl_initlog(void *arguments, void *logic); + +#endif /* OD_BL_H_ */ diff --git a/src/or.c b/src/or.c new file mode 100644 index 0000000..88572a0 --- /dev/null +++ b/src/or.c @@ -0,0 +1,49 @@ +/* + * or.c + * + * Created on: 29-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "or.h" + +int or_initlog(void *arguments, void *logic) +{ + struct or_args *args = (struct or_args *)arguments; + struct or_logic *log = (struct or_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_mask = 1<<(args->io.out & 0x07); + + log->in1_ptr = log_manager.nets_data + (args->io.in1 >> 3); + log->in1_bit_mask = 1<<(args->io.in1 & 0x07); + + log->in2_ptr = log_manager.nets_data + (args->io.in2 >> 3); + log->in2_bit_mask = 1<<(args->io.in2 & 0x07); + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in1_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in2_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void or(void *arguments, void *logic) +{ +// struct or_args *args = (struct or_args *)arguments; + struct or_logic *log = (struct or_logic *)logic; + + if( (*log->in1_ptr & log->in1_bit_mask) || (*log->in2_ptr & log->in2_bit_mask) ) + *log->out_ptr|=log->out_bit_mask; + else + *log->out_ptr&=~log->out_bit_mask; +} diff --git a/src/or.h b/src/or.h new file mode 100644 index 0000000..967fcd4 --- /dev/null +++ b/src/or.h @@ -0,0 +1,47 @@ +/* + * or.h + * + * + * Created on: 29-04-2014 + * Author: KJ + */ + +#ifndef OR_H_ +#define OR_H_ + +#include "tdefs.h" + +struct or_logic +{ + u8 *out_ptr; + u8 out_bit_mask; + + u8 *in1_ptr; + u8 in1_bit_mask; + + u8 *in2_ptr; + u8 in2_bit_mask; +}; + +struct or_io +{ + u32 in1; + u32 in2; + u32 out; +}__attribute__((__packed__)); + +struct or_params +{ +}__attribute__((__packed__)); + +struct or_args +{ + struct or_io io; + struct or_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void or(void *args, void *logic); +extern int or_initlog(void *args, void *logic); + +#endif /* OR_H_ */ diff --git a/src/or4.c b/src/or4.c new file mode 100644 index 0000000..dde6cf7 --- /dev/null +++ b/src/or4.c @@ -0,0 +1,46 @@ +/* + * or4.c + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "or4.h" + +int or4_initlog(void *arguments, void *logic) +{ + struct or4_args *args = (struct or4_args *)arguments; + struct or4_logic *log = (struct or4_logic *)logic; + + if(set_bit_ptr_mask(args->io.in1_in,&log->in[0],&log->in_bit_mask[0])) + return -1; + if(set_bit_ptr_mask(args->io.in2_in,&log->in[1],&log->in_bit_mask[1])) + return -1; + if(set_bit_ptr_mask(args->io.in3_in,&log->in[2],&log->in_bit_mask[2])) + return -1; + if(set_bit_ptr_mask(args->io.in4_in,&log->in[3],&log->in_bit_mask[3])) + return -1; + if(set_bit_ptr_mask(args->io.out_out,&log->out,&log->out_bit_mask)) + return -1; + + + return 0; +} + +void or4(void *arguments, void *logic) +{ + struct or4_logic *log = (struct or4_logic *)logic; + u8 i; + + for(i=0;i<4;i++) + if(*log->in[i] & log->in_bit_mask[i]) + break; + + check_and_set_mask(i<4,log->out,log->out_bit_mask); +} diff --git a/src/or4.h b/src/or4.h new file mode 100644 index 0000000..11a5210 --- /dev/null +++ b/src/or4.h @@ -0,0 +1,45 @@ +/* + * or4.h + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OR4_H_ +#define OR4_H_ + +#include "tdefs.h" + +struct or4_logic +{ + u8 *in[4]; + u8 in_bit_mask[4]; + + u8 *out; + u8 out_bit_mask; +}; + +struct or4_io +{ + u32 in1_in; + u32 in2_in; + u32 in3_in; + u32 in4_in; + u32 out_out; +}__attribute__((__packed__)); + +struct or4_params +{ +}__attribute__((__packed__)); + +struct or4_args +{ + struct or4_io io; + struct or4_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void or4(void *args, void *logic); +extern int or4_initlog(void *arguments, void *logic); + +#endif /* OR4_H_ */ diff --git a/src/or8.c b/src/or8.c new file mode 100644 index 0000000..caa447a --- /dev/null +++ b/src/or8.c @@ -0,0 +1,54 @@ +/* + * or8.c + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "or8.h" + +int or8_initlog(void *arguments, void *logic) +{ + struct or8_args *args = (struct or8_args *)arguments; + struct or8_logic *log = (struct or8_logic *)logic; + + if(set_bit_ptr_mask(args->io.in1_in,&log->in[0],&log->in_bit_mask[0])) + return -1; + if(set_bit_ptr_mask(args->io.in2_in,&log->in[1],&log->in_bit_mask[1])) + return -1; + if(set_bit_ptr_mask(args->io.in3_in,&log->in[2],&log->in_bit_mask[2])) + return -1; + if(set_bit_ptr_mask(args->io.in4_in,&log->in[3],&log->in_bit_mask[3])) + return -1; + if(set_bit_ptr_mask(args->io.in5_in,&log->in[4],&log->in_bit_mask[4])) + return -1; + if(set_bit_ptr_mask(args->io.in6_in,&log->in[5],&log->in_bit_mask[5])) + return -1; + if(set_bit_ptr_mask(args->io.in7_in,&log->in[6],&log->in_bit_mask[6])) + return -1; + if(set_bit_ptr_mask(args->io.in8_in,&log->in[7],&log->in_bit_mask[7])) + return -1; + if(set_bit_ptr_mask(args->io.out_out,&log->out,&log->out_bit_mask)) + return -1; + + + return 0; +} + +void or8(void *arguments, void *logic) +{ + struct or8_logic *log = (struct or8_logic *)logic; + u8 i; + + for(i=0;i<8;i++) + if(*log->in[i] & log->in_bit_mask[i]) + break; + + check_and_set_mask(i<8,log->out,log->out_bit_mask); +} diff --git a/src/or8.h b/src/or8.h new file mode 100644 index 0000000..21a6287 --- /dev/null +++ b/src/or8.h @@ -0,0 +1,49 @@ +/* + * and8.h + * + * Created on: 15-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OR8_H_ +#define OR8_H_ + +#include "tdefs.h" + +struct or8_logic +{ + u8 *in[8]; + u8 in_bit_mask[8]; + + u8 *out; + u8 out_bit_mask; +}; + +struct or8_io +{ + u32 in1_in; + u32 in2_in; + u32 in3_in; + u32 in4_in; + u32 in5_in; + u32 in6_in; + u32 in7_in; + u32 in8_in; + u32 out_out; +}__attribute__((__packed__)); + +struct or8_params +{ +}__attribute__((__packed__)); + +struct or8_args +{ + struct or8_io io; + struct or8_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void or8(void *args, void *logic); +extern int or8_initlog(void *arguments, void *logic); + +#endif /* OR8_H_ */ diff --git a/src/out_drv.c b/src/out_drv.c new file mode 100644 index 0000000..4bd6c56 --- /dev/null +++ b/src/out_drv.c @@ -0,0 +1,80 @@ +/* + * out_drv.c + * + * Created on: 02-12-2019 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include +#include +#include "out_drv.h" +#include "relay_out.h" +#include "comm.h" +#include + +int out_drv_initlog(void *arguments, void *logic) +{ +// struct events_reg_args *args = (struct events_reg_args *)arguments; + struct out_drv_logic *log = (struct out_drv_logic *)logic; + int i; + + log->out_count=0; + + for(i=0;ielement_num[log->out_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + log->element_num[log->out_count]=i; + log->out_count++; + } + } + + return 0; +} + +void out_drv(void *arguments, void *logic) +{ + //struct events_reg_args *args = (struct events_reg_args *)arguments; + struct out_drv_logic *log = (struct out_drv_logic *)logic; + + struct relay_out_args *rel_args; + struct relay_out_logic *rel_log; + + int i; + + for(i=0;iout_count;i++) + { + rel_args = (struct relay_out_args *)log_manager.log_element[log->element_num[i]].fun_args_ptr; + rel_log = (struct relay_out_logic *)log_manager.log_element[log->element_num[i]].fun_log_ptr; + + if(*rel_log->in_ptr & rel_log->in_bit_mask) + bus_out_data[rel_args->params.card_addr]|=(1<params.relay_num); + else if(!(rel_args->params.bits & RELAY_OUT_UNTIL_CLR)) + bus_out_data[rel_args->params.card_addr]&=~(1<params.relay_num); + + if(!(*rel_log->in_ptr & rel_log->in_bit_mask) && (*rel_log->clr_ptr & rel_log->clr_bit_mask)) + bus_out_data[rel_args->params.card_addr]&=~(1<params.relay_num); + + if(rel_args->params.bits & RELAY_OUT_VIA_MSW) + { + if((ic->kob_out & (1<params.card_addr)) && (ic->out_set[rel_args->params.card_addr] & rel_log->kob_msw_bit_mask)) + *rel_log->kob_ptr|=rel_log->kob_bit_mask; + else + *rel_log->kob_ptr&=~rel_log->kob_bit_mask; + } + else + { + if(ic->kob_out & (1<params.card_addr)) + *rel_log->kob_ptr|=rel_log->kob_bit_mask; + else + *rel_log->kob_ptr&=~rel_log->kob_bit_mask; + } + } +} diff --git a/src/out_drv.h b/src/out_drv.h new file mode 100644 index 0000000..0e3d0c3 --- /dev/null +++ b/src/out_drv.h @@ -0,0 +1,39 @@ +/* + * out_drv.h + * + * + * Created on: 02-12-2019 + * Author: KJ + */ + +#ifndef OUT_DRV_H_ +#define OUT_DRV_H_ + +#include "tdefs.h" +#include "helper.h" + +struct out_drv_logic +{ + u16 out_count; + u16 element_num[]; +}; + +struct out_drv_io +{ +}__attribute__((__packed__)); + +struct out_drv_params +{ +}__attribute__((__packed__)); + +struct out_drv_args +{ + struct out_drv_io io; + struct out_drv_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int out_drv_initlog(void *args, void *logic); +extern void out_drv(void *args, void *logic); + +#endif /* OUT_DRV_H_ */ diff --git a/src/ov_1ph.c b/src/ov_1ph.c new file mode 100644 index 0000000..ee8b06a --- /dev/null +++ b/src/ov_1ph.c @@ -0,0 +1,106 @@ +/* + * ov_1ph.c + * + * Created on: 08-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include "helper.h" + +#include "ov_1ph.h" + +int ov_1ph_initlog(void *arguments, void *logic) +{ + struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct ov_1ph_logic *log = (struct ov_1ph_logic *)logic; + + if(set_bit_ptr(args->io.stan_bl_in,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.test_in,&log->test,&log->test_bit_no)) + return -1; + + if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no)) + return -1; + + if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no)) + return -1; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + if(set_float_ptr(args->io.I_float_in,&log->I)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.full_period = args->params.filter_type; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = ((args->params.t_ - 0.005f) * 100); //czas dzialania + + return 0; +} + +void ov_1ph(void *arguments, void *logic) +{ +// struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct ov_1ph_logic *log = (struct ov_1ph_logic *)logic; + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob, + *log->I>log->nast_.limit1_, + *log->Inast_.limit2_, + &log->dw.lp, + log->nast_.full_period?20:3,3); //realizacja przekaznika + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob = (*log->test & (1<test_bit_no))?1:0; + } + // + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*|| czy_start()*/) + log->dw.pob = 0; + // + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad && log->dw.pob) + { + log->dw.zad = 1; +// dw.czas_ = time; + } + + check_and_set(log->dw.pob,log->P,log->P_bit_no); + check_and_set(log->dw.zad,log->Z,log->Z_bit_no); + check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no); + } else { + *log->W&=~(1<W_bit_no); + *log->Z&=~(1<Z_bit_no); + *log->P&=~(1<P_bit_no); + } +} + +void ov_1ph_100hz(void *arguments, void *logic) +{ +// struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct ov_1ph_logic *log = (struct ov_1ph_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + } +} diff --git a/src/ov_1ph.h b/src/ov_1ph.h new file mode 100644 index 0000000..d8adf1c --- /dev/null +++ b/src/ov_1ph.h @@ -0,0 +1,86 @@ +/* + * ov_1ph.h + * + * Created on: 08-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OV_1PH_H_ +#define OV_1PH_H_ + +#include "tdefs.h" + +struct dane_wewnetrzne_ov1ph +{ +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) +// uint32_t dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) +// float max_; // maksymalna watosc zaklocenia (rejestr tymczasowy - przed zapisem) + short lp; ///io.stan_bl_in,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.test_in,&log->test,&log->test_bit_no)) + return -1; + + if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no)) + return -1; + + if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no)) + return -1; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + if(set_float_ptr(args->io.I_float_in,&log->I)) + return -1; + + if(set_float_ptr(args->io.I2h_float_in,&log->I2h)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_out,&log->bl_2h)) + return -1; + + if(set_bit_ptr_struct(args->io.bezwar_out,&log->bezwar)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = ((args->params.t_ - 0.005f) * 100); //czas dzialania + + log->nast_.I2h = args->params.I2h * args->params.I2h; + //log->nast_.I2h_powr = (args->params.kp_ * args->params.I2h) * (args->params.kp_ * args->params.I2h); + log->nast_.I2h_powr = (0.99 * args->params.I2h) * (0.99 * args->params.I2h); + + log->nast_.I2h_min = log->nast_.I2h * log->nast_.limit1_; + + log->nast_.Ibezwar = args->params.Ibezwar * args->params.Ibezwar; + log->nast_.Ibezwar_powr = (args->params.kp_ * args->params.Ibezwar) * (args->params.kp_ * args->params.Ibezwar); + log->nast_.full_period = args->params.filter_type; + return 0; +} + +void ov_1ph2h(void *arguments, void *logic) +{ +// struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct ov_1ph2h_logic *log = (struct ov_1ph2h_logic *)logic; + u8 jest_blokada = 0; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + + sprawdz_P(&log->dw.pob2h, + (*log->I2h>(log->nast_.I2h * *log->I) && *log->I2h > log->nast_.I2h_min), + (*log->I2h<(log->nast_.I2h_powr * *log->I) || *log->I2h < log->nast_.I2h_min), + &log->dw.lp2h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob1h, + *log->I>log->nast_.limit1_, + *log->Inast_.limit2_, + &log->dw.lp1h, + log->nast_.full_period?20:3,3); //realizacja przekaznika + + sprawdz_P(&log->dw.pobbezwar,*log->I>log->nast_.Ibezwar,*log->Inast_.Ibezwar_powr, + &log->dw.lpbezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob, + log->dw.pob1h && (!log->dw.pob2h || log->dw.pobbezwar), + !log->dw.pob1h || (log->dw.pob2h && !log->dw.pobbezwar), + &log->dw.lp, + 1,1); //realizacja przekaznika + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob = (*log->test & (1<test_bit_no))?1:0; + } + // + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*|| czy_start()*/) + log->dw.pob = 0; + // + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad && log->dw.pob) + { + log->dw.zad = 1; +// dw.czas_ = time; + } + + jest_blokada = (log->dw.pob1h && log->dw.pob2h); + + check_and_set_struct(jest_blokada && !(*log->stan_bl & (1<stan_bl_bit_no)),&log->bl_2h); + check_and_set_struct(log->dw.pobbezwar && !(log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) && log->dw.zad,&log->bezwar); + + check_and_set(log->dw.pob,log->P,log->P_bit_no); + check_and_set(log->dw.zad,log->Z,log->Z_bit_no); + check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no); + + } else { + clear_struct(&log->bl_2h); + clear_struct(&log->bezwar); + *log->W&=~(1<W_bit_no); + *log->Z&=~(1<Z_bit_no); + *log->P&=~(1<P_bit_no); + } +} + +void ov_1ph2h_100hz(void *arguments, void *logic) +{ +// struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct ov_1ph2h_logic *log = (struct ov_1ph2h_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + } +} diff --git a/src/ov_1ph2h.h b/src/ov_1ph2h.h new file mode 100644 index 0000000..3800557 --- /dev/null +++ b/src/ov_1ph2h.h @@ -0,0 +1,101 @@ +/* + * ov_1ph2h.h + * + * Created on: 08-06-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OV_1PH2H_H_ +#define OV_1PH2H_H_ + +#include "tdefs.h" + +struct dane_wewnetrzne_ov1ph2h +{ +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) +// uint32_t dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) +// float max_; // maksymalna watosc zaklocenia (rejestr tymczasowy - przed zapisem) + short lp,lp1h,lp2h,lpbezwar; ///action_ptr = log_manager.nets_data + (args->io.action_out >> 3); + log->action_bit_no = args->io.action_out & 0x07; + + if(log->action_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->trig_ptr = log_manager.nets_data + (args->io.trig_out >> 3); + log->trig_bit_no = args->io.trig_out & 0x07; + + if(log->trig_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_in >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->limit_val1 = args->params.trig_val * args->params.trig_val; + log->limit_val2 = log->limit_val1 * args->params.kp * args->params.kp; + + log->t_delay = args->params.t_delay * 1000; + + return 0; +} + +void ov_1ph(void *arguments, void *logic) +{ +// struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct ov_1ph_logic *log = (struct ov_1ph_logic *)logic; + + if(*log->trig_ptr & (1<trig_bit_no)) + { + if(!(*log->action_ptr & (1<action_bit_no))) + { + log->time_cnt++; + if(log->time_cnt>log->t_delay) + *log->action_ptr|=(1<action_bit_no); + } + + if(*log->esk_ptr < log->limit_val2) + { + *log->action_ptr&=~(1<action_bit_no); + *log->trig_ptr&=~(1<trig_bit_no); + } + } + else + { + if(*log->esk_ptr > log->limit_val1) + { + *log->trig_ptr|=(1<trig_bit_no); + log->time_cnt=0; + } + } +} diff --git a/src/ov_1ph_old.h_ b/src/ov_1ph_old.h_ new file mode 100644 index 0000000..4d3d3d4 --- /dev/null +++ b/src/ov_1ph_old.h_ @@ -0,0 +1,50 @@ +/* + * ov_1ph.h + * + * Created on: 08-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OV_1PH_H_ +#define OV_1PH_H_ + +#include "../tdefs.h" + +struct ov_1ph_logic +{ + u32 time_cnt; + u8 *trig_ptr; + u8 trig_bit_no; + u8 *action_ptr; + u8 action_bit_no; + float *esk_ptr; + float limit_val1; + float limit_val2; + u32 t_delay; +}; + +struct ov_1ph_io +{ + u32 esk_float_in; + u32 trig_out; + u32 action_out; +}__attribute__((__packed__)); + +struct ov_1ph_params +{ + double trig_val; + double kp; + double t_delay; +}__attribute__((__packed__)); + +struct ov_1ph_args +{ + struct ov_1ph_io io; + struct ov_1ph_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void ov_1ph(void *args, void *logic); +extern int ov_1ph_initlog(void *arguments, void *logic); + +#endif /* OV_1PH_H_ */ diff --git a/src/ov_3ph.c b/src/ov_3ph.c new file mode 100644 index 0000000..357bfc7 --- /dev/null +++ b/src/ov_3ph.c @@ -0,0 +1,226 @@ +/* + * ov_3ph.c + * + * Created on: 29-11-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "ov_3ph.h" + + +int ov_3ph_initlog(void *arguments, void *logic) +{ + struct ov_3ph_args *args = (struct ov_3ph_args *)arguments; + struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic; + + if(set_bit_ptr(args->io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.test_in,&log->test,&log->test_bit_no)) + return -1; + + if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no)) + return -1; + + if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no)) + return -1; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL1_out,&log->PL1,&log->PL1_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL2_out,&log->PL2,&log->PL2_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL3_out,&log->PL3,&log->PL3_bit_no)) + return -1; + + if(set_float_ptr(args->io.I1_float_in,&log->I1)) + return -1; + + if(set_float_ptr(args->io.I2_float_in,&log->I2)) + return -1; + + if(set_float_ptr(args->io.I3_float_in,&log->I3)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.or_ = (args->params.bity & 0x0008)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = (args->params.t_ - 0.005f) * 100; //czas dzialania + log->nast_.full_period = args->params.filter_type; + //*((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = nast_.on_; + + return 0; +} + +void ov_3ph(void *arguments, void *logic) +{ + struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic; + + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob1, + *log->I1>log->nast_.limit1_, + *log->I1nast_.limit2_, + &log->dw.lp1, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob2, + *log->I2>log->nast_.limit1_, + *log->I2nast_.limit2_, + &log->dw.lp2, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 2 + + sprawdz_P(&log->dw.pob3, + *log->I3>log->nast_.limit1_, + *log->I3nast_.limit2_, + &log->dw.lp3, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 3 + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = (*log->test & (1<test_bit_no))?1:0; + } + + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*| czy_start()*/) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0; + } + // + + if(log->nast_.or_) // zaleznie od nastawienia logiki pobudzen "and" lub "or" + { + log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or" + } else { + log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and" + } + + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad & log->dw.pob) + { + log->dw.zad = 1; + //log->dw.czas_ = time; + } + + check_and_set(log->dw.pob,log->P,log->P_bit_no); + check_and_set(log->dw.pob1,log->PL1,log->PL1_bit_no); + check_and_set(log->dw.pob2,log->PL2,log->PL2_bit_no); + check_and_set(log->dw.pob3,log->PL3,log->PL3_bit_no); + check_and_set(log->dw.zad,log->Z,log->Z_bit_no); + check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no); + } else { + *log->W&=~(1<W_bit_no); + *log->Z&=~(1<Z_bit_no); + *log->P&=~(1<P_bit_no); + *log->PL1&=~(1<PL1_bit_no); + *log->PL2&=~(1<PL2_bit_no); + *log->PL3&=~(1<PL3_bit_no); + } +} + +void ov_3ph_100hz(void *arguments, void *logic) +{ + struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + // realizacja rejestratora parametrow ostatniego zaklocenia + /* float wej1 = dane_analogowe[nast_.adr.kwampl1]; + float wej2 = dane_analogowe[nast_.adr.kwampl2]; + float wej3 = dane_analogowe[nast_.adr.kwampl3]; + + switch(status_rej) + { + case oczekiwanie: + if (dw.pob) + { + dw.max1_ = wej1; + dw.max2_ = wej2; + dw.max3_ = wej3; + dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + if (dw.zad) + { + status_rej = sledzenie_zad; + } + } + break; + case sledzenie: + if (dw.zad) + { + status_rej = sledzenie_zad; + dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (dw.pob) + { + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!dw.pob) + { + dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + dw.dl_++; + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + } + break; + case zapis: + if (!dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + }*/ + } +} + +void ov_3ph_20hz(void *arguments, void *logic) +{ + //struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic; + + //if(dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + //{ + // dw.R.max_L1_ = sqrt(dw.max1_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L2_ = sqrt(dw.max2_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L3_ = sqrt(dw.max3_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.moment_wystapienia = dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // dw.R.dlugosc = (float)dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia + // dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + // dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + //} + +} diff --git a/src/ov_3ph.h b/src/ov_3ph.h new file mode 100644 index 0000000..ec8db96 --- /dev/null +++ b/src/ov_3ph.h @@ -0,0 +1,106 @@ +/* + * ov_3ph.h + * + * Created on: 29-11-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OV_3PH_H_ +#define OV_3PH_H_ + +#include "tdefs.h" + + struct Nastawy_przeliczone_ov3ph + { + float limit1_; ///< wartosc rozruchowa + float limit2_; ///< wartosc powrotu + u32 t_; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 w_; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 or_; ///< logika pobudzen "0" oznacza dzialanie w trybie "and", a "1" w trybie "or" + u8 full_period; + }; + + struct dane_wewnetrzne_ov3ph + { +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + u32 dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + float max1_; // maksymalna watosc wejscia 1 (rejestr tymczasowy - przed zapisem) + float max2_; // maksymalna watosc wejscia 2 (rejestr tymczasowy - przed zapisem) + float max3_; // maksymalna watosc wejscia 3 (rejestr tymczasowy - przed zapisem) + short lp1; ///io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.test_in,&log->test,&log->test_bit_no)) + return -1; + + if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no)) + return -1; + + if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no)) + return -1; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL1_out,&log->PL1,&log->PL1_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL2_out,&log->PL2,&log->PL2_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL3_out,&log->PL3,&log->PL3_bit_no)) + return -1; + + if(set_float_ptr(args->io.I1_float_in,&log->I1)) + return -1; + + if(set_float_ptr(args->io.I2_float_in,&log->I2)) + return -1; + + if(set_float_ptr(args->io.I3_float_in,&log->I3)) + return -1; + + if(set_float_ptr(args->io.I12h_float_in,&log->I12h)) + return -1; + + if(set_float_ptr(args->io.I22h_float_in,&log->I22h)) + return -1; + + if(set_float_ptr(args->io.I32h_float_in,&log->I32h)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.or_ = (args->params.bity & 0x0008)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = (args->params.t_ - 0.005f) * 100; //czas dzialania + + log->nast_.I2h = args->params.I2h * args->params.I2h; + log->nast_.I2h_powr = (args->params.kp_ * args->params.I2h) * (args->params.kp_ * args->params.I2h); + + log->nast_.Ibezwar = args->params.Ibezwar * args->params.Ibezwar; + log->nast_.Ibezwar_powr = (args->params.kp_ * args->params.Ibezwar) * (args->params.kp_ * args->params.Ibezwar); + + //*((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = nast_.on_; + + return 0; +} + +void ov_3ph2h(void *arguments, void *logic) +{ + struct ov_3ph2h_logic *log = (struct ov_3ph2h_logic *)logic; + u8 jest_jedna_blokada = 0; + u8 jest_jeden_bezwar = 0; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob12h, + (*log->I12h>(log->nast_.I2h * *log->I1)), + (*log->I12h<(log->nast_.I2h_powr * *log->I1)), + &log->dw.lp12h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob11h, + *log->I1>log->nast_.limit1_, + *log->I1nast_.limit2_, + &log->dw.lp11h, + 3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob1bezwar,*log->I1>log->nast_.Ibezwar,*log->I1nast_.Ibezwar_powr, + &log->dw.lp1bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob1, + log->dw.pob11h && (!log->dw.pob12h || log->dw.pob1bezwar), + !log->dw.pob11h || (log->dw.pob12h && !log->dw.pob1bezwar), + &log->dw.lp1, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob22h, + (*log->I22h>(log->nast_.I2h * *log->I2)), + (*log->I22h<(log->nast_.I2h_powr * *log->I2)), + &log->dw.lp22h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob21h, + *log->I2>log->nast_.limit1_, + *log->I2nast_.limit2_, + &log->dw.lp21h, + 3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob2bezwar,*log->I2>log->nast_.Ibezwar,*log->I2nast_.Ibezwar_powr, + &log->dw.lp2bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob2, + log->dw.pob21h && (!log->dw.pob22h || log->dw.pob2bezwar), + !log->dw.pob21h || (log->dw.pob22h && !log->dw.pob2bezwar), + &log->dw.lp2, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob32h, + (*log->I32h>(log->nast_.I2h * *log->I3)), + (*log->I32h<(log->nast_.I2h_powr * *log->I3)), + &log->dw.lp32h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob31h, + *log->I3>log->nast_.limit1_, + *log->I3nast_.limit2_, + &log->dw.lp31h, + 3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob3bezwar,*log->I3>log->nast_.Ibezwar,*log->I3nast_.Ibezwar_powr, + &log->dw.lp3bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob3, + log->dw.pob31h && (!log->dw.pob32h || log->dw.pob3bezwar), + !log->dw.pob31h || (log->dw.pob32h && !log->dw.pob3bezwar), + &log->dw.lp3, + 1,1); //realizacja przekaznika + +// + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = (*log->test & (1<test_bit_no))?1:0; + } + + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*| czy_start()*/) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0; + } + // + + if(log->nast_.or_) // zaleznie od nastawienia logiki pobudzen "and" lub "or" + { + jest_jedna_blokada = (log->dw.pob11h && log->dw.pob12h) || (log->dw.pob21h && log->dw.pob22h) || (log->dw.pob31h && log->dw.pob32h); + jest_jeden_bezwar = log->dw.pob1bezwar || log->dw.pob2bezwar || log->dw.pob3bezwar; + + if (jest_jedna_blokada && !jest_jeden_bezwar) + log->dw.pob = 0; //blokowanie gdy zablokowana jest jedna faza + else + log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or" + } else { + log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and" + } + + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad & log->dw.pob) + { + log->dw.zad = 1; + //log->dw.czas_ = time; + } + + check_and_set(log->dw.pob,log->P,log->P_bit_no); + check_and_set(log->dw.pob1,log->PL1,log->PL1_bit_no); + check_and_set(log->dw.pob2,log->PL2,log->PL2_bit_no); + check_and_set(log->dw.pob3,log->PL3,log->PL3_bit_no); + check_and_set(log->dw.zad,log->Z,log->Z_bit_no); + check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no); + } else { + *log->W&=~(1<W_bit_no); + *log->Z&=~(1<Z_bit_no); + *log->P&=~(1<P_bit_no); + *log->PL1&=~(1<PL1_bit_no); + *log->PL2&=~(1<PL2_bit_no); + *log->PL3&=~(1<PL3_bit_no); + } +} + +void ov_3ph2h_100hz(void *arguments, void *logic) +{ + struct ov_3ph2h_logic *log = (struct ov_3ph2h_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + // realizacja rejestratora parametrow ostatniego zaklocenia + /* float wej1 = dane_analogowe[nast_.adr.kwampl1]; + float wej2 = dane_analogowe[nast_.adr.kwampl2]; + float wej3 = dane_analogowe[nast_.adr.kwampl3]; + + switch(status_rej) + { + case oczekiwanie: + if (dw.pob) + { + dw.max1_ = wej1; + dw.max2_ = wej2; + dw.max3_ = wej3; + dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + if (dw.zad) + { + status_rej = sledzenie_zad; + } + } + break; + case sledzenie: + if (dw.zad) + { + status_rej = sledzenie_zad; + dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (dw.pob) + { + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!dw.pob) + { + dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + dw.dl_++; + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + } + break; + case zapis: + if (!dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + }*/ + } +} + +void ov_3ph2h_20hz(void *arguments, void *logic) +{ + //struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic; + + //if(dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + //{ + // dw.R.max_L1_ = sqrt(dw.max1_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L2_ = sqrt(dw.max2_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L3_ = sqrt(dw.max3_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.moment_wystapienia = dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // dw.R.dlugosc = (float)dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia + // dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + // dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + //} + +} diff --git a/src/ov_3ph2h.c b/src/ov_3ph2h.c new file mode 100644 index 0000000..da1f953 --- /dev/null +++ b/src/ov_3ph2h.c @@ -0,0 +1,320 @@ +/* + * ov_3ph2h.c + * + * Created on: 08-06-2017 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "ov_3ph2h.h" + + +int ov_3ph2h_initlog(void *arguments, void *logic) +{ + struct ov_3ph2h_args *args = (struct ov_3ph2h_args *)arguments; + struct ov_3ph2h_logic *log = (struct ov_3ph2h_logic *)logic; + + if(set_bit_ptr(args->io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.test_in,&log->test,&log->test_bit_no)) + return -1; + + if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no)) + return -1; + + if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no)) + return -1; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL1_out,&log->PL1,&log->PL1_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL2_out,&log->PL2,&log->PL2_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL3_out,&log->PL3,&log->PL3_bit_no)) + return -1; + + if(set_float_ptr(args->io.I1_float_in,&log->I1)) + return -1; + + if(set_float_ptr(args->io.I2_float_in,&log->I2)) + return -1; + + if(set_float_ptr(args->io.I3_float_in,&log->I3)) + return -1; + + if(set_float_ptr(args->io.I12h_float_in,&log->I12h)) + return -1; + + if(set_float_ptr(args->io.I22h_float_in,&log->I22h)) + return -1; + + if(set_float_ptr(args->io.I32h_float_in,&log->I32h)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_out,&log->bl_2h)) + return -1; + + if(set_bit_ptr_struct(args->io.bezwar_out,&log->bezwar)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.or_ = (args->params.bity & 0x0008)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = (args->params.t_ - 0.005f) * 100; //czas dzialania + + log->nast_.I2h = args->params.I2h * args->params.I2h; +// log->nast_.I2h_powr = (args->params.kp_ * args->params.I2h) * (args->params.kp_ * args->params.I2h); + log->nast_.I2h_powr = (0.99 * args->params.I2h) * (0.99 * args->params.I2h); + + log->nast_.I2h_min = log->nast_.I2h * log->nast_.limit1_; + + log->nast_.Ibezwar = args->params.Ibezwar * args->params.Ibezwar; + log->nast_.Ibezwar_powr = (args->params.kp_ * args->params.Ibezwar) * (args->params.kp_ * args->params.Ibezwar); + log->nast_.full_period = args->params.filter_type; + //*((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = nast_.on_; + + return 0; +} + +void ov_3ph2h(void *arguments, void *logic) +{ + struct ov_3ph2h_logic *log = (struct ov_3ph2h_logic *)logic; + u8 jest_jedna_blokada = 0; + u8 jest_jeden_bezwar = 0; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob12h, + *log->I12h>(log->nast_.I2h * *log->I1) && *log->I12h>log->nast_.I2h_min, + *log->I12h<(log->nast_.I2h_powr * *log->I1) || *log->I12hnast_.I2h_min, + &log->dw.lp12h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob11h, + *log->I1>log->nast_.limit1_, + *log->I1nast_.limit2_, + &log->dw.lp11h, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob1bezwar,*log->I1>log->nast_.Ibezwar,*log->I1nast_.Ibezwar_powr, + &log->dw.lp1bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob1, + log->dw.pob11h && (!log->dw.pob12h || log->dw.pob1bezwar), + !log->dw.pob11h || (log->dw.pob12h && !log->dw.pob1bezwar), + &log->dw.lp1, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob22h, + *log->I22h>(log->nast_.I2h * *log->I2)&&*log->I22h>log->nast_.I2h_min, + *log->I22h<(log->nast_.I2h_powr * *log->I2)||*log->I22hnast_.I2h_min, + &log->dw.lp22h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob21h, + *log->I2>log->nast_.limit1_, + *log->I2nast_.limit2_, + &log->dw.lp21h, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob2bezwar,*log->I2>log->nast_.Ibezwar,*log->I2nast_.Ibezwar_powr, + &log->dw.lp2bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob2, + log->dw.pob21h && (!log->dw.pob22h || log->dw.pob2bezwar), + !log->dw.pob21h || (log->dw.pob22h && !log->dw.pob2bezwar), + &log->dw.lp2, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob32h, + *log->I32h>(log->nast_.I2h * *log->I3)&&*log->I32h>log->nast_.I2h_min, + *log->I32h<(log->nast_.I2h_powr * *log->I3)||*log->I32hnast_.I2h_min, + &log->dw.lp32h, + 2,10); //realizacja przekaznika + + sprawdz_P(&log->dw.pob31h, + *log->I3>log->nast_.limit1_, + *log->I3nast_.limit2_, + &log->dw.lp31h, + log->nast_.full_period?20:3,3); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob3bezwar,*log->I3>log->nast_.Ibezwar,*log->I3nast_.Ibezwar_powr, + &log->dw.lp3bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob3, + log->dw.pob31h && (!log->dw.pob32h || log->dw.pob3bezwar), + !log->dw.pob31h || (log->dw.pob32h && !log->dw.pob3bezwar), + &log->dw.lp3, + 1,1); //realizacja przekaznika + +// + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = (*log->test & (1<test_bit_no))?1:0; + } + + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*| czy_start()*/) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0; + } + // + + jest_jedna_blokada = (log->dw.pob11h && log->dw.pob12h) || (log->dw.pob21h && log->dw.pob22h) || (log->dw.pob31h && log->dw.pob32h); + u8 bezwar=0; + if(log->nast_.or_) // zaleznie od nastawienia logiki pobudzen "and" lub "or" + { + jest_jeden_bezwar = log->dw.pob1bezwar || log->dw.pob2bezwar || log->dw.pob3bezwar; + + if(jest_jeden_bezwar) + bezwar=1; + + if (jest_jedna_blokada && !jest_jeden_bezwar) + { + log->dw.pob = 0; //blokowanie gdy zablokowana jest jedna faza + } + else + { + log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or" + } + } else { + bezwar = log->dw.pob1bezwar && log->dw.pob2bezwar && log->dw.pob3bezwar; + log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and" + } + + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad & log->dw.pob) + { + log->dw.zad = 1; + //log->dw.czas_ = time; + } + + check_and_set(log->dw.pob,log->P,log->P_bit_no); + check_and_set(log->dw.pob1,log->PL1,log->PL1_bit_no); + check_and_set(log->dw.pob2,log->PL2,log->PL2_bit_no); + check_and_set(log->dw.pob3,log->PL3,log->PL3_bit_no); + check_and_set(log->dw.zad,log->Z,log->Z_bit_no); + check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no); + check_and_set_struct(jest_jedna_blokada && !(*log->stan_bl & (1<stan_bl_bit_no)),&log->bl_2h); + check_and_set_struct(bezwar && !(log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) ,&log->bezwar); + } else { + clear_struct(&log->bl_2h); + *log->W&=~(1<W_bit_no); + *log->Z&=~(1<Z_bit_no); + *log->P&=~(1<P_bit_no); + *log->PL1&=~(1<PL1_bit_no); + *log->PL2&=~(1<PL2_bit_no); + *log->PL3&=~(1<PL3_bit_no); + } +} + +void ov_3ph2h_100hz(void *arguments, void *logic) +{ + struct ov_3ph2h_logic *log = (struct ov_3ph2h_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + // realizacja rejestratora parametrow ostatniego zaklocenia + /* float wej1 = dane_analogowe[nast_.adr.kwampl1]; + float wej2 = dane_analogowe[nast_.adr.kwampl2]; + float wej3 = dane_analogowe[nast_.adr.kwampl3]; + + switch(status_rej) + { + case oczekiwanie: + if (dw.pob) + { + dw.max1_ = wej1; + dw.max2_ = wej2; + dw.max3_ = wej3; + dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + if (dw.zad) + { + status_rej = sledzenie_zad; + } + } + break; + case sledzenie: + if (dw.zad) + { + status_rej = sledzenie_zad; + dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (dw.pob) + { + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!dw.pob) + { + dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + dw.dl_++; + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + } + break; + case zapis: + if (!dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + }*/ + } +} + +void ov_3ph2h_20hz(void *arguments, void *logic) +{ + //struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic; + + //if(dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + //{ + // dw.R.max_L1_ = sqrt(dw.max1_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L2_ = sqrt(dw.max2_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L3_ = sqrt(dw.max3_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.moment_wystapienia = dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // dw.R.dlugosc = (float)dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia + // dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + // dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + //} + +} diff --git a/src/ov_3ph2h.h b/src/ov_3ph2h.h new file mode 100644 index 0000000..8992a21 --- /dev/null +++ b/src/ov_3ph2h.h @@ -0,0 +1,145 @@ +/* + * ov_3ph.h + * + * Created on: 08-06-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OV_3PH2H_H_ +#define OV_3PH2H_H_ + +#include "tdefs.h" + + struct Nastawy_przeliczone_ov3ph2h + { + float limit1_; ///< wartosc rozruchowa + float limit2_; ///< wartosc powrotu + u32 t_; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 w_; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 or_; ///< logika pobudzen "0" oznacza dzialanie w trybie "and", a "1" w trybie "or" + float I2h; + float I2h_powr; + float I2h_min; + float Ibezwar; + float Ibezwar_powr; + u8 full_period; + }; + + struct dane_wewnetrzne_ov3ph2h + { +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + u32 dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + float max1_; // maksymalna watosc wejscia 1 (rejestr tymczasowy - przed zapisem) + float max2_; // maksymalna watosc wejscia 2 (rejestr tymczasowy - przed zapisem) + float max3_; // maksymalna watosc wejscia 3 (rejestr tymczasowy - przed zapisem) + short lp1; ///io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.test_in,&log->test,&log->test_bit_no)) + return -1; + + if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no)) + return -1; + + if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no)) + return -1; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL1_out,&log->PL1,&log->PL1_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL2_out,&log->PL2,&log->PL2_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL3_out,&log->PL3,&log->PL3_bit_no)) + return -1; + + if(set_float_ptr(args->io.I1_float_in,&log->I1)) + return -1; + + if(set_float_ptr(args->io.I2_float_in,&log->I2)) + return -1; + + if(set_float_ptr(args->io.I3_float_in,&log->I3)) + return -1; + + if(set_float_ptr(args->io.I12h_float_in,&log->I12h)) + return -1; + + if(set_float_ptr(args->io.I22h_float_in,&log->I22h)) + return -1; + + if(set_float_ptr(args->io.I32h_float_in,&log->I32h)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_2h_out,&log->bl_2h)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.or_ = (args->params.bity & 0x0008)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = (args->params.t_ - 0.005f) * 100; //czas dzialania + + log->nast_.I2h = args->params.I2h * args->params.I2h; +// log->nast_.I2h_powr = (args->params.kp_ * args->params.I2h) * (args->params.kp_ * args->params.I2h); + log->nast_.I2h_powr = (0.99 * args->params.I2h) * (0.99 * args->params.I2h); + + log->nast_.I2h_min = log->nast_.I2h * log->nast_.limit1_; + + log->nast_.Ibezwar = args->params.Ibezwar * args->params.Ibezwar; + log->nast_.Ibezwar_powr = (args->params.kp_ * args->params.Ibezwar) * (args->params.kp_ * args->params.Ibezwar); + //*((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = nast_.on_; + + return 0; +} + +void ov_3ph2h_slow(void *arguments, void *logic) +{ + struct ov_3ph2h_slow_logic *log = (struct ov_3ph2h_slow_logic *)logic; + u8 jest_jedna_blokada = 0; + u8 jest_jeden_bezwar = 0; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob12h, + *log->I12h>(log->nast_.I2h * *log->I1) && *log->I12h>log->nast_.I2h_min, + *log->I12h<(log->nast_.I2h_powr * *log->I1) || *log->I12hnast_.I2h_min, + &log->dw.lp12h, + 20,20); //realizacja przekaznika + + sprawdz_P(&log->dw.pob11h, + *log->I1>log->nast_.limit1_, + *log->I1nast_.limit2_, + &log->dw.lp11h, + 30,20); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob1bezwar,*log->I1>log->nast_.Ibezwar,*log->I1nast_.Ibezwar_powr, + &log->dw.lp1bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob1, + log->dw.pob11h && (!log->dw.pob12h || log->dw.pob1bezwar), + !log->dw.pob11h || (log->dw.pob12h && !log->dw.pob1bezwar), + &log->dw.lp1, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob22h, + *log->I22h>(log->nast_.I2h * *log->I2)&&*log->I22h>log->nast_.I2h_min, + *log->I22h<(log->nast_.I2h_powr * *log->I2)||*log->I22hnast_.I2h_min, + &log->dw.lp22h, + 20,20); //realizacja przekaznika + + sprawdz_P(&log->dw.pob21h, + *log->I2>log->nast_.limit1_, + *log->I2nast_.limit2_, + &log->dw.lp21h, + 30,20); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob2bezwar,*log->I2>log->nast_.Ibezwar,*log->I2nast_.Ibezwar_powr, + &log->dw.lp2bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob2, + log->dw.pob21h && (!log->dw.pob22h || log->dw.pob2bezwar), + !log->dw.pob21h || (log->dw.pob22h && !log->dw.pob2bezwar), + &log->dw.lp2, + 1,1); //realizacja przekaznika + +// + sprawdz_P(&log->dw.pob32h, + *log->I32h>(log->nast_.I2h * *log->I3)&&*log->I32h>log->nast_.I2h_min, + *log->I32h<(log->nast_.I2h_powr * *log->I3)||*log->I32hnast_.I2h_min, + &log->dw.lp32h, + 20,20); //realizacja przekaznika + + sprawdz_P(&log->dw.pob31h, + *log->I3>log->nast_.limit1_, + *log->I3nast_.limit2_, + &log->dw.lp31h, + 30,20); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob3bezwar,*log->I3>log->nast_.Ibezwar,*log->I3nast_.Ibezwar_powr, + &log->dw.lp3bezwar, + 1,1); //realizacja przekaznika + + sprawdz_P(&log->dw.pob3, + log->dw.pob31h && (!log->dw.pob32h || log->dw.pob3bezwar), + !log->dw.pob31h || (log->dw.pob32h && !log->dw.pob3bezwar), + &log->dw.lp3, + 1,1); //realizacja przekaznika + +// + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = (*log->test & (1<test_bit_no))?1:0; + } + + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*| czy_start()*/) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0; + } + // + + u8 bl2h=0; + + if(log->nast_.or_) // zaleznie od nastawienia logiki pobudzen "and" lub "or" + { + jest_jedna_blokada = (log->dw.pob11h && log->dw.pob12h) || (log->dw.pob21h && log->dw.pob22h) || (log->dw.pob31h && log->dw.pob32h); + jest_jeden_bezwar = log->dw.pob1bezwar || log->dw.pob2bezwar || log->dw.pob3bezwar; + + if (jest_jedna_blokada && !jest_jeden_bezwar) + { + bl2h=1; + log->dw.pob = 0; //blokowanie gdy zablokowana jest jedna faza + } + else + { + bl2h=0; + log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or" + } + } else { + log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and" + bl2h=(log->dw.pob11h && log->dw.pob12h) || (log->dw.pob21h && log->dw.pob22h) || (log->dw.pob31h && log->dw.pob32h); + } + + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad & log->dw.pob) + { + log->dw.zad = 1; + //log->dw.czas_ = time; + } + + check_and_set(log->dw.pob,log->P,log->P_bit_no); + check_and_set(log->dw.pob1,log->PL1,log->PL1_bit_no); + check_and_set(log->dw.pob2,log->PL2,log->PL2_bit_no); + check_and_set(log->dw.pob3,log->PL3,log->PL3_bit_no); + check_and_set(log->dw.zad,log->Z,log->Z_bit_no); + check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no); + check_and_set_struct(bl2h,&log->bl_2h); + } else { + clear_struct(&log->bl_2h); + *log->W&=~(1<W_bit_no); + *log->Z&=~(1<Z_bit_no); + *log->P&=~(1<P_bit_no); + *log->PL1&=~(1<PL1_bit_no); + *log->PL2&=~(1<PL2_bit_no); + *log->PL3&=~(1<PL3_bit_no); + } +} + +void ov_3ph2h_slow_100hz(void *arguments, void *logic) +{ + struct ov_3ph2h_slow_logic *log = (struct ov_3ph2h_slow_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + // realizacja rejestratora parametrow ostatniego zaklocenia + /* float wej1 = dane_analogowe[nast_.adr.kwampl1]; + float wej2 = dane_analogowe[nast_.adr.kwampl2]; + float wej3 = dane_analogowe[nast_.adr.kwampl3]; + + switch(status_rej) + { + case oczekiwanie: + if (dw.pob) + { + dw.max1_ = wej1; + dw.max2_ = wej2; + dw.max3_ = wej3; + dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + if (dw.zad) + { + status_rej = sledzenie_zad; + } + } + break; + case sledzenie: + if (dw.zad) + { + status_rej = sledzenie_zad; + dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (dw.pob) + { + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!dw.pob) + { + dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + dw.dl_++; + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + } + break; + case zapis: + if (!dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + }*/ + } +} + +void ov_3ph2h_slow_20hz(void *arguments, void *logic) +{ + //struct ov_3ph_logic *log = (struct ov_3ph_logic *)logic; + + //if(dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + //{ + // dw.R.max_L1_ = sqrt(dw.max1_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L2_ = sqrt(dw.max2_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.max_L3_ = sqrt(dw.max3_); // wyliczenie maksymalnej wartosci faza 1 + // dw.R.moment_wystapienia = dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // dw.R.dlugosc = (float)dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia + // dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + // dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + //} + +} diff --git a/src/ov_3ph2h_slow.h b/src/ov_3ph2h_slow.h new file mode 100644 index 0000000..1af9ca9 --- /dev/null +++ b/src/ov_3ph2h_slow.h @@ -0,0 +1,141 @@ +/* + * ov_3ph.h + * + * Created on: 08-06-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef OV_3PH2H_SLOW_H_ +#define OV_3PH2H_SLOW_H_ + +#include "tdefs.h" + + struct Nastawy_przeliczone_ov3ph2h_slow + { + float limit1_; ///< wartosc rozruchowa + float limit2_; ///< wartosc powrotu + u32 t_; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 w_; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 or_; ///< logika pobudzen "0" oznacza dzialanie w trybie "and", a "1" w trybie "or" + float I2h; + float I2h_powr; + float I2h_min; + float Ibezwar; + float Ibezwar_powr; + }; + + struct dane_wewnetrzne_ov3ph2h_slow + { +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + u32 dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + float max1_; // maksymalna watosc wejscia 1 (rejestr tymczasowy - przed zapisem) + float max2_; // maksymalna watosc wejscia 2 (rejestr tymczasowy - przed zapisem) + float max3_; // maksymalna watosc wejscia 3 (rejestr tymczasowy - przed zapisem) + short lp1; /// + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "pkw.h" + +float pkw_mem[8] = {0,0,0,0,0,0,0,0}; + +int pkw_initlog(void *arguments, void *logic) +{ + struct pkw_args *args = (struct pkw_args *)arguments; + struct pkw_logic *log = (struct pkw_logic *)logic; + + if(set_bit_ptr_mask(args->io.Pwe_in,&log->Pwe,&log->Pwe_bit_mask)) + return -1; + + if(set_float_ptr(args->io.I_float_in,&log->kwampl)) + return -1; + + if(set_bit_ptr_mask(args->io.Kas_in,&log->Kas,&log->Kas_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.Pwy_out,&log->Pwy,&log->Pwy_bit_mask)) + return -1; + + if(set_float_ptr(args->io.Licznik_float_out,&log->licznik)) + return -1; + + log->on = args->params.bity & 0x0001; + log->Wr = args->params.Wr_; // przepisanie wartosci progowej PKW + log->id = args->params.id & 0x0007; + +// Semaphore_post(spi_semaphore); + + return 0; +} + +void pkw(void *arguments, void *logic) +{ + struct pkw_logic *log = (struct pkw_logic *)logic; + struct pkw_args *args = (struct pkw_args *)arguments; + + if(log->on) // jesli funkcja przekaznika aktywna + { + if (*log->Kas & log->Kas_bit_mask) + pkw_mem[log->id]=args->params.kas; + + if (*log->Pwe & log->Pwe_bit_mask) + { + if (log->later) + { + log->prad = *log->kwampl; + log->zapis = 1; + } + log->later = 0; + } + else + { + log->later = 1; + } + } +} + +void pkw_20hz(void *arguments, void *logic) +{ + struct pkw_logic *log = (struct pkw_logic *)logic; + + *log->licznik = pkw_mem[log->id]; + + if(log->zapis) + { + pkw_mem[log->id] += sqrt(log->prad); + log->zapis = 0; + } + + log->prad = 0; + + check_and_set_mask(pkw_mem[log->id]>=log->Wr,log->Pwy,log->Pwy_bit_mask); +} diff --git a/src/pkw.h b/src/pkw.h new file mode 100644 index 0000000..9e607ea --- /dev/null +++ b/src/pkw.h @@ -0,0 +1,65 @@ +/* + * pkw.h + * + * Created on: 19-01-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef PKW_H_ +#define PKW_H_ + +#include "tdefs.h" + +extern float pkw_mem[8]; + +struct pkw_logic +{ + u8 *Pwe; + u8 Pwe_bit_mask; + u8 *Kas; + u8 Kas_bit_mask; + + float *kwampl; + + u8 *Pwy; + u8 Pwy_bit_mask; + + float *licznik; + + u16 Wr; ///< wartosc progowa sygnalizacji + u8 on; ///< wartosc nastawy aktywnosci PKWa 1- aktywny + u8 zapis; // wartosc stanu wejscia z poprzedniej iteracji + u8 later; // wartosc stanu wejscia z poprzedniej iteracji + float prad; + u8 id; +}; + +struct pkw_io +{ + u32 Pwe_in; + u32 I_float_in; + u32 Kas_in; + u32 Pwy_out; + u32 Licznik_float_out; +}__attribute__((__packed__)); + +struct pkw_params +{ + u32 bity; ///< nastawy bitowe; + double Wr_; ///< wartosc progowa sygnalizacji + double kas; // wartosc przepisywana przy kasowaniu + u32 id; // id pkw od 0..7 +}__attribute__((__packed__)); + +struct pkw_args +{ + struct pkw_io io; + struct pkw_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void pkw(void *args, void *logic); +extern void pkw_20hz(void *args, void *logic); +extern int pkw_initlog(void *arguments, void *logic); + +#endif /* PKW_H_ */ diff --git a/src/ports/am1808/include/netif/sitaraif.h b/src/ports/am1808/include/netif/sitaraif.h new file mode 100644 index 0000000..592b754 --- /dev/null +++ b/src/ports/am1808/include/netif/sitaraif.h @@ -0,0 +1,6 @@ +#ifndef SITARAIF_H_ +#define SITARAIF_H_ + +// Mock for sitaraif.h + +#endif /* SITARAIF_H_ */ diff --git a/src/pow.c b/src/pow.c new file mode 100644 index 0000000..b36b7b3 --- /dev/null +++ b/src/pow.c @@ -0,0 +1,39 @@ +/* + * pow.c + * + * Created on: 02-06-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "pow.h" + +int pow_initlog(void *arguments, void *logic) +{ + struct pow_args *args = (struct pow_args *)arguments; + struct pow_logic *log = (struct pow_logic *)logic; + + log->out_ptr = (float *)(log_manager.nets_data + (args->io.out_float_out >> 3)); + + if((u8 *)log->out_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(float))) + return -1; + + log->in_ptr = (float *)(log_manager.nets_data + (args->io.in_float_in >> 3)); + if((u8*)log->in_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(float))) + return -1; + + return 0; +} + +void pow_1(void *arguments, void *logic) +{ + struct pow_args *args = (struct pow_args *)arguments; + struct pow_logic *log = (struct pow_logic *)logic; + + if(args->io.out_float_out) + { + *log->out_ptr=*log->in_ptr * *log->in_ptr; + } +} diff --git a/src/pow.h b/src/pow.h new file mode 100644 index 0000000..181b1d9 --- /dev/null +++ b/src/pow.h @@ -0,0 +1,39 @@ +/* + * pow.h + * + * Created on: 02-06-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef POW_H_ +#define POW_H_ + +#include "tdefs.h" + +struct pow_logic +{ + float *in_ptr; + float *out_ptr; +}; + +struct pow_io +{ + u32 in_float_in; + u32 out_float_out; +}__attribute__((__packed__)); + +struct pow_params +{ +}__attribute__((__packed__)); + +struct pow_args +{ + struct pow_io io; + struct pow_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void pow_1(void *args, void *logic); +extern int pow_initlog(void *arguments, void *logic); + +#endif /* POW_H_ */ diff --git a/src/prog_czas.c b/src/prog_czas.c new file mode 100644 index 0000000..452fea7 --- /dev/null +++ b/src/prog_czas.c @@ -0,0 +1,84 @@ +/* + * klapacz.c + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" +#include + +#include "helper.h" +#include "prog_czas.h" + +//volatile struct klapacz_args tkl; + +int prog_czas_initlog(void *arguments, void *logic) +{ + struct prog_czas_args *args = (struct prog_czas_args *)arguments; + struct prog_czas_logic *log = (struct prog_czas_logic *)logic; + + + if(set_bit_ptr_struct(args->io.pon,&log->pon)) + return -1; + if(set_bit_ptr_struct(args->io.wt,&log->wt)) + return -1; + if(set_bit_ptr_struct(args->io.sr,&log->sr)) + return -1; + if(set_bit_ptr_struct(args->io.czw,&log->czw)) + return -1; + if(set_bit_ptr_struct(args->io.pt,&log->pt)) + return -1; + if(set_bit_ptr_struct(args->io.sob,&log->sob)) + return -1; + if(set_bit_ptr_struct(args->io.niedz,&log->niedz)) + return -1; + + return 0; +} + +void prog_czas(void *arguments, void *logic) +{ +// struct prog_czas_args *args = (struct prog_czas_args *)arguments; + struct prog_czas_logic *log = (struct prog_czas_logic *)logic; + + u8 day_of_week; + + day_of_week = (cur_time.tv_sec / 86400 + 4)%7; + check_and_set_struct((day_of_week==0),&log->niedz); + check_and_set_struct((day_of_week==1),&log->pon); + check_and_set_struct((day_of_week==2),&log->wt); + check_and_set_struct((day_of_week==3),&log->sr); + check_and_set_struct((day_of_week==4),&log->czw); + check_and_set_struct((day_of_week==5),&log->pt); + check_and_set_struct((day_of_week==6),&log->sob); + //check_and_set_struct((day_of_week==7),&log->niedz); + + /* + local function GetMonth(seconds) + local dayduration,year = 3600*24 + local days={31,0,31,30,31,30,31,31,30,31,30,31} + for i=1970,10000 do -- For some reason too lazy to use while + local yeardays = i%4 == 0 and i%100 ~= 0 and 366 or 365 + local yearduration = dayduration * yeardays + if yearduration < seconds then + seconds = seconds - yearduration + else + year = i break + end + end + days[2]=(year%4==0) and 29 or 28 + seconds = seconds%(365*24*3600) + for i=1,12 do + if seconds>days[i]*dayduration then + seconds=seconds-days[i]*dayduration + else + return --i + year*12 <-- If you want a unique ID + end + end + end + */ + + +} diff --git a/src/prog_czas.h b/src/prog_czas.h new file mode 100644 index 0000000..8466214 --- /dev/null +++ b/src/prog_czas.h @@ -0,0 +1,51 @@ +/* + * klapacz.h + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef PROG_CZAS_H_ +#define PROG_CZAS_H_ + +#include "tdefs.h" + +struct prog_czas_logic +{ + struct binary_io pon; // one bit + struct binary_io wt; // one bit + struct binary_io sr; // one bit + struct binary_io czw; // one bit + struct binary_io pt; // one bit + struct binary_io sob; // one bit + struct binary_io niedz; // one bit +}; + +struct prog_czas_io +{ + + u32 pon; // one bit + u32 wt; // one bit + u32 sr; // one bit + u32 czw; // one bit + u32 pt; // one bit + u32 sob; // one bit + u32 niedz; // one bit + +}__attribute__((__packed__)); + +struct prog_czas_params +{ +}__attribute__((__packed__)); + +struct prog_czas_args +{ + struct prog_czas_io io; + struct prog_czas_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void prog_czas(void *args, void *logic); +extern int prog_czas_initlog(void *args, void *logic); + +#endif /* PROG_CZAS_H_ */ diff --git a/src/ps.c b/src/ps.c new file mode 100644 index 0000000..d6830e0 --- /dev/null +++ b/src/ps.c @@ -0,0 +1,472 @@ +/* + * ps.c + * + * Created on: 09-08-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "ps.h" +#include "helper.h" +#include "analog_in.h" +#include + +int ps_initlog(void *arguments, void *logic) +{ + struct ps_args *args = (struct ps_args *)arguments; + struct ps_logic *log = (struct ps_logic *)logic; + u32 *an_params_ptr; + + log->stan_bl = (u8 *)(log_manager.nets_data + (args->io.stan_bl_in >> 3)); + if((u8 *)log->stan_bl > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->stan_bl_bit = (args->io.stan_bl_in & 0x07); + + log->U1_orta = (float *)(log_manager.nets_data + (args->io.U1_orta_float_in >> 3)); + if((u8 *)log->U1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U1_ortb = (float *)(log_manager.nets_data + (args->io.U1_ortb_float_in >> 3)); + if((u8 *)log->U1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U1 = (float *)(log_manager.nets_data + (args->io.U1_float_in >> 3)); + if((u8 *)log->U1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1_orta = (float *)(log_manager.nets_data + (args->io.I1_orta_float_in >> 3)); + if((u8 *)log->I1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1_ortb = (float *)(log_manager.nets_data + (args->io.I1_ortb_float_in >> 3)); + if((u8 *)log->I1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1 = (float *)(log_manager.nets_data + (args->io.I1_float_in >> 3)); + if((u8 *)log->I1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I2 = (float *)(log_manager.nets_data + (args->io.I2_float_in >> 3)); + if((u8 *)log->I2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I0 = (float *)(log_manager.nets_data + (args->io.I0_float_in >> 3)); + if((u8 *)log->I0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_I_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->param_I = (struct analog_in_params *)*an_params_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_U_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->param_U = (struct analog_in_params *)*an_params_ptr; + + log->test = (u8 *)(log_manager.nets_data + (args->io.test_in >> 3)); + if((u8 *)log->test > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->test_bit = (args->io.test_in & 0x07); + + log->Bl_PS = (u8 *)(log_manager.nets_data + (args->io.Bl_PS_out >> 3)); + if((u8 *)log->Bl_PS > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->Bl_PS_bit = (args->io.Bl_PS_out & 0x07); + +/* log->PS_syn = (u8 *)(log_manager.nets_data + (args->io.PS_syn_out >> 3)); + if((u8 *)log->PS_syn > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->PS_syn_bit = (args->io.PS_syn_out & 0x07); + + log->PS_asyn = (u8 *)(log_manager.nets_data + (args->io.PS_asyn_out >> 3)); + if((u8 *)log->PS_asyn > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->PS_asyn_bit = (args->io.PS_asyn_out & 0x07); +*/ + log->Z = (u8 *)(log_manager.nets_data + (args->io.Z_out >> 3)); + if((u8 *)log->Z > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->Z_bit = (args->io.Z_out & 0x07); + + log->W = (u8 *)(log_manager.nets_data + (args->io.W_out >> 3)); + if((u8 *)log->W > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->W_bit = (args->io.W_out & 0x07); + + + log->nast_.on = args->params.bity & 0x0001; + log->nast_.W = args->params.bity & 0x0002; + log->nast_.bl = args->params.bity & 0x0004; + log->nast_.D3Io = args->params.bity & 0x0008; + log->nast_.D3I2 = args->params.bity & 0x0010; + + + log->nast_.tpass = (uint32_t)(1000 * args->params.tpass); + log->nast_.tPS = (uint32_t)(100 * (args->params.tPS + 0.005f)); + log->nast_.tdbl = (uint32_t)(100 * (args->params.tdbl + 0.005f)); + log->nast_.lPS = (uint16_t)args->params.lPS; + + //przaeliczenie nastaw + + log->nast_.Iomin = args->params.Iomin * args->params.Iomin; + log->nast_.I2min = args->params.I2min * args->params.I2min; + log->nast_.Iokh = args->params.Iokh * args->params.Iokh; + log->nast_.I2kh = args->params.I2kh * args->params.I2kh; + log->nast_.Imin = args->params.Imin * args->params.Imin; + + log->nast_.R1f = args->params.Rf; + log->nast_.R1r = - args->params.Rr; + log->nast_.X1r = - args->params.Xr; + log->nast_.X1f = args->params.Xf; + + log->nast_.R2f = args->params.Rf - args->params.dRf; + log->nast_.R2r = args->params.dRr - args->params.Rr; + log->nast_.X2r = args->params.dXr - args->params.Xr; + log->nast_.X2f = args->params.Xf - args->params.dXf; + + log->nast_.dRlf = args->params.dRf; + log->nast_.dRlr = args->params.dRr; + + log->nast_.dRl1f = args->params.RLf; + log->nast_.dRl1r = - args->params.RLr; + log->nast_.dRl2f = args->params.RLf - args->params.dRf; + log->nast_.dRl2r = args->params.dRr - args->params.RLr; + + log->nast_.tanfi1 = tan((3.141593 / 180) * args->params.fi1); + log->nast_.tanfi2 = tan((3.141593 / 180) * args->params.fi2); + log->nast_.przekladnia = log->param_U->znam_wtor / log->param_I->znam_wtor; + + log->dw.stan_kolysanie = kolysanie_OK; + + + return 0; +} + +void ps(void *arguments, void *logic) +{ + struct ps_logic *log = (struct ps_logic *)logic; + + float local; + u8 wk1,wk2,wk3; + + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + log->w.I0 = *log->I0; + log->w.I1 = *log->I1; + log->w.I2 = *log->I2; + log->w.I1_orta = *log->I1_orta; + log->w.I1_ortb = *log->I1_ortb; + log->w.U1 = *log->U1; + log->w.U1_orta = *log->U1_orta; + log->w.U1_ortb = *log->U1_ortb; + + + // sprawdzenie stanu przekanikw skadowej zerowej i przeciwnej. + if(log->nast_.D3Io) + { + wk1 = (log->w.I0 > log->nast_.Iomin) && (log->w.I1 * log->nast_.Iokh < log->w.I0); + sprawdz_P( + &log->dw.P_Io, + wk1, + !wk1, + &log->dw.liczIo, + 3,3 + ); + } else log->dw.P_Io = 0; + if(log->nast_.D3I2) + { + wk1 = (log->w.I2 > log->nast_.I2min) && (log->w.I1 * log->nast_.I2kh < log->w.I2); + sprawdz_P( + &log->dw.P_I2, + wk1, + !wk1, + &log->dw.liczI2, + 3,3 + ); + } else log->dw.P_I2 = 0; + + wk3 = log->dw.P_Io || log->dw.P_I2; ///< jest zwarcie (na pewno nie koysania mocy) +//-------------------------------------------------------------------------------------------------------------- + + //wyliczenie impedancji + log->dw.R = log->w.I1_orta * log->w.U1_orta + log->w.I1_ortb * log->w.U1_ortb; + log->dw.X = log->w.I1_ortb * log->w.U1_orta - log->w.I1_orta * log->w.U1_ortb; + local = log->w.I1_orta * log->w.I1_orta + log->w.I1_ortb * log->w.I1_ortb; + local = log->nast_.przekladnia / local; + log->dw.R *= local; + log->dw.X *= local; + log->dw.absR = fabs(log->dw.R); + log->dw.absX = fabs(log->dw.X); + + + + // okreslenie polozenia wektora impedancji wzgledem plaszczyzny nastaw + wk1 = (log->w.I1 > log->nast_.Imin) && + (log->dw.X < log->nast_.X1f) && + (log->dw.X > log->nast_.X1r) && + ((log->dw.X > (log->dw.R - log->nast_.R1f) * log->nast_.tanfi1) || (log->dw.R < log->nast_.R1f)) && + ((log->dw.X < (log->dw.R - log->nast_.R1r) * log->nast_.tanfi1) || (log->dw.R > log->nast_.R1r)) && + ((log->dw.absX > (log->dw.absR * log->nast_.tanfi2)) || ((log->dw.R < log->nast_.dRl1f) && (log->dw.R > log->nast_.dRl1r))) ; + +float Rwk2; + if(log->dw.R>0) + { + Rwk2 = log->dw.absR + log->nast_.dRlf; + } else { + Rwk2 = log->dw.absR + log->nast_.dRlr; + } + + wk2 = (log->w.I1 > log->nast_.Imin) && + (log->dw.X < log->nast_.X2f) && + (log->dw.X > log->nast_.X2r) && + ((log->dw.X > (log->dw.R - log->nast_.R2f) * log->nast_.tanfi1) || (log->dw.R < log->nast_.R2f)) && + ((log->dw.X < (log->dw.R - log->nast_.R2r) * log->nast_.tanfi1) || (log->dw.R > log->nast_.R2r)) && + ((log->dw.absX > (Rwk2 * log->nast_.tanfi2)) || + ((log->dw.R < log->nast_.dRl2f) && (log->dw.R > log->nast_.dRl2r))); + + sprawdz_P( + &log->dw.Ps1, + wk1, + !wk1, + &log->dw.liczs1, + 5,5 + ); + + sprawdz_P( + &log->dw.Ps2, + wk2, + !wk2, + &log->dw.liczs2, + 5,5 + ); + + switch(log->dw.stan_kolysanie) + { + case kolysanie_OK: + + //check_and_set(0, log->Bl_PS, log->Bl_PS_bit); + + if(log->dw.Ps1 && !wk3) + { + log->dw.liczk = log->nast_.tpass; + log->dw.stan_kolysanie = wejscie_w_strefe1; + log->dw.tpass = 0; + log->dw.dl_ = 0; + log->dw.t1 = 0; + } + break; + case wejscie_w_strefe1: + + //check_and_set(1, log->Bl_PS, log->Bl_PS_bit); + + if(log->dw.liczk>LOOP_CYCLE_MS) + log->dw.liczk-=LOOP_CYCLE_MS; + else + log->dw.liczk=0; + log->dw.tpass+=LOOP_CYCLE_MS; + log->dw.dl_+=LOOP_CYCLE_MS; + + if(log->dw.Ps2) + { + log->dw.stan_kolysanie = blokada_kolysania; + } else { + if(!log->dw.liczk) + { + log->dw.stan_kolysanie = oczekiwanie_na_strefe2; + } + } + if (!log->dw.Ps1 || wk3) + { + log->dw.stan_kolysanie = kolysanie_OK; + } + break; + case oczekiwanie_na_strefe2: + log->dw.tpass+=LOOP_CYCLE_MS; + log->dw.dl_+=LOOP_CYCLE_MS; + if (wk3) + { + log->dw.stan_kolysanie = blokada_kolysania; + } else { + if(log->dw.Ps2) + { + log->dw.b.Bl_PS = 1; ///< wczenie blokady + log->dw.liczdbl = log->nast_.tdbl; + log->dw.stan_kolysanie = kolysanie_mocy; + log->dw.Rz = log->dw.R; + //log->dw.czas_ = Time(); + log->dw.t1++; + } + } + break; + case kolysanie_mocy: + log->dw.dl_+=LOOP_CYCLE_MS; + if (log->dw.Ps2) log->dw.t1+=LOOP_CYCLE_MS; + if(wk3) + { + log->dw.stan_kolysanie = blokada_kolysania; + log->dw.wsk_rej_ = 1; + } + break; + case blokada_kolysania: + log->dw.b.Bl_PS = 0; ///< wyczenie blokady + break; + } + + //obsuga wejcia testu przekanika + if (czy_test_R()) + { + if(*log->test & (1<test_bit)) + { + log->dw.b.Bl_PS = + log->dw.b.PS_asyn = + log->dw.b.PS_syn = + log->dw.b.W = + log->dw.b.Z = 1; + } + else + { + log->dw.b.Bl_PS = + log->dw.b.PS_asyn = + log->dw.b.PS_syn = + log->dw.b.W = + log->dw.b.Z = 0; + } + } + // + + + // realizacja blokady dzialania przekaznika + if(log->nast_.bl && (*log->stan_bl & (1<stan_bl_bit))) { + log->dw.b.Bl_PS = + log->dw.b.PS_asyn = + log->dw.b.PS_syn = + log->dw.b.W = + log->dw.b.Z = + log->dw.liczlPS = 0; + } + // + + //---------------------------------- + + + check_and_set(log->dw.b.Bl_PS, log->Bl_PS, log->Bl_PS_bit); + check_and_set(log->dw.b.W, log->W, log->W_bit); + check_and_set(log->dw.b.Z, log->Z, log->Z_bit); +// check_and_set(log->dw.b.PS_asyn, log->PS_asyn, log->PS_asyn_bit); +// check_and_set(log->dw.b.PS_syn, log->PS_syn, log->PS_syn_bit); + + + } else { + *log->Z&=~(1<Z_bit); + *log->W&=~(1<W_bit); + *log->Bl_PS&=~(1<Bl_PS_bit); +// *log->PS_syn&=~(1<PS_syn_bit); +// *log->PS_asyn&=~(1<PS_asyn_bit); + } +} + +void ps_100hz(void *arguments, void *logic) +{ + struct ps_logic *log = (struct ps_logic *)logic; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + + + switch(log->dw.stan_kolysanie) + { + case kolysanie_OK: + log->dw.licztPS--; + if(log->dw.licztPS<0) + { + log->dw.b.PS_asyn = 0; + log->dw.b.PS_syn = 0; + log->dw.b.W = 0; + log->dw.b.Z = 0; + log->dw.liczlPS = 0; + } + break; + case wejscie_w_strefe1: + break; + case oczekiwanie_na_strefe2: + if (!log->dw.Ps1) + { + log->dw.stan_kolysanie = kolysanie_OK; + } + + break; + case kolysanie_mocy: + if(!log->dw.Ps1) + { + log->dw.b.Bl_PS = 0; //wylaczenie blokady + log->dw.licztPS = log->nast_.tPS; + if((log->dw.Rz * log->dw.R) > 0) //kolysania synchroniczne + { + log->dw.b.PS_asyn = 1; + + } else { //kolysania asynchroniczne + log->dw.b.PS_syn = 1; + log->dw.liczlPS++; + if (log->dw.liczlPS==log->nast_.lPS) + { + log->dw.b.Z = 1; + } + } + log->dw.stan_kolysanie = kolysanie_OK; + log->dw.wsk_rej_ = 1; + } else { + log->dw.liczdbl--; + if(!log->dw.liczdbl) + { + log->dw.stan_kolysanie = blokada_kolysania; + log->dw.b.Bl_PS = 0; //wylaczenie blokady od kolysan mocy + log->dw.wsk_rej_ = 1; + } + } + + break; + case blokada_kolysania: + if (!log->dw.Ps1) + { + log->dw.stan_kolysanie = kolysanie_OK; + } + + break; + } + + log->dw.b.W = log->dw.b.Z && log->nast_.W; + } +} + +void ps_20hz(void *arguments, void *logic) +{ + struct ps_logic *log = (struct ps_logic *)logic; + if(log->dw.wsk_rej_) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + { +// log->dw.Rej.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia +// log->dw.Rej.dlugosc = (float)log->dw.dl_ * 0.001f;// / 1000; // wpisanie czasu trwania zwarcia +// log->dw.Rej.tpass = (float)log->dw.tpass * 0.001f;// / 1000;//log->dw.tpass / 1000; +// log->dw.Rej.t1 = (float)log->dw.t1 * 0.001f;// / 1000; +// log->dw.Rej.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + log->dw.wsk_rej_ = 0; // zerowanie wskaznika powstania nowego zaklocenia + } +} diff --git a/src/ps.h b/src/ps.h new file mode 100644 index 0000000..330a35c --- /dev/null +++ b/src/ps.h @@ -0,0 +1,216 @@ +/* + * ps.h + * + * Created on: 09-08-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef PS_H_ +#define PS_H_ + +#include "tdefs.h" + +struct Nastawy_przeliczone_ps + { + float Imin;///< minimalna wartosc pr¹adu dzialania + float Iomin;///< Minimalna wartoœæ rozruchowa pr¹du zerowego przekaŸnika identyfikacji zwaræ z udzia³em ziemi + float Iokh;/// + +int ps2_initlog(void *arguments, void *logic) +{ + struct ps2_args *args = (struct ps2_args *)arguments; + struct ps2_logic *log = (struct ps2_logic *)logic; + u32 *an_params_ptr; + + log->stan_bl = (u8 *)(log_manager.nets_data + (args->io.stan_bl_in >> 3)); + if((u8 *)log->stan_bl > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->stan_bl_bit = (args->io.stan_bl_in & 0x07); + + log->U1_orta = (float *)(log_manager.nets_data + (args->io.U1_orta_float_in >> 3)); + if((u8 *)log->U1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U1_ortb = (float *)(log_manager.nets_data + (args->io.U1_ortb_float_in >> 3)); + if((u8 *)log->U1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U1 = (float *)(log_manager.nets_data + (args->io.U1_float_in >> 3)); + if((u8 *)log->U1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1_orta = (float *)(log_manager.nets_data + (args->io.I1_orta_float_in >> 3)); + if((u8 *)log->I1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1_ortb = (float *)(log_manager.nets_data + (args->io.I1_ortb_float_in >> 3)); + if((u8 *)log->I1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1 = (float *)(log_manager.nets_data + (args->io.I1_float_in >> 3)); + if((u8 *)log->I1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I2 = (float *)(log_manager.nets_data + (args->io.I2_float_in >> 3)); + if((u8 *)log->I2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I0 = (float *)(log_manager.nets_data + (args->io.I0_float_in >> 3)); + if((u8 *)log->I0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_I_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->param_I = (struct analog_in_params *)*an_params_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_U_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->param_U = (struct analog_in_params *)*an_params_ptr; + + log->test = (u8 *)(log_manager.nets_data + (args->io.test_in >> 3)); + if((u8 *)log->test > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->test_bit = (args->io.test_in & 0x07); + + log->Bl_PS = (u8 *)(log_manager.nets_data + (args->io.Bl_PS_out >> 3)); + if((u8 *)log->Bl_PS > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->Bl_PS_bit = (args->io.Bl_PS_out & 0x07); + + log->PS_syn = (u8 *)(log_manager.nets_data + (args->io.PS_syn_out >> 3)); + if((u8 *)log->PS_syn > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->PS_syn_bit = (args->io.PS_syn_out & 0x07); + + log->PS_asyn = (u8 *)(log_manager.nets_data + (args->io.PS_asyn_out >> 3)); + if((u8 *)log->PS_asyn > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->PS_asyn_bit = (args->io.PS_asyn_out & 0x07); + + log->nast_.on = args->params.bity & 0x0001; + log->nast_.W = args->params.bity & 0x0002; + log->nast_.bl = args->params.bity & 0x0004; + log->nast_.D3Io = args->params.bity & 0x0008; + log->nast_.D3I2 = args->params.bity & 0x0010; + + + log->nast_.tpass = (uint32_t)(1000 * args->params.tpass); + log->nast_.tdbl = (uint32_t)(100 * (args->params.tdbl + 0.005f)); + + //przaeliczenie nastaw + + log->nast_.Iomin = args->params.Iomin * args->params.Iomin; + log->nast_.I2min = args->params.I2min * args->params.I2min; + log->nast_.Iokh = args->params.Iokh * args->params.Iokh; + log->nast_.I2kh = args->params.I2kh * args->params.I2kh; + log->nast_.Imin = args->params.Imin * args->params.Imin; + + log->nast_.R1f = args->params.Rf; + log->nast_.R1r = - args->params.Rr; + log->nast_.X1r = - args->params.Xr; + log->nast_.X1f = args->params.Xf; + + log->nast_.R2f = args->params.Rf - args->params.dRf; + log->nast_.R2r = args->params.dRr - args->params.Rr; + log->nast_.X2r = args->params.dXr - args->params.Xr; + log->nast_.X2f = args->params.Xf - args->params.dXf; + + log->nast_.dRlf = args->params.dRf; + log->nast_.dRlr = args->params.dRr; + + log->nast_.dRl1f = args->params.RLf; + log->nast_.dRl1r = - args->params.RLr; + log->nast_.dRl2f = args->params.RLf - args->params.dRf; + log->nast_.dRl2r = args->params.dRr - args->params.RLr; + + log->nast_.tanfi1 = tan((3.141593 / 180) * args->params.fi1); + log->nast_.tanfi2 = tan((3.141593 / 180) * args->params.fi2); + log->nast_.przekladnia = log->param_U->znam_wtor / log->param_I->znam_wtor; + + log->dw.stan_kolysanie = kolysanie_OK; + + + return 0; +} + +void ps2(void *arguments, void *logic) +{ + struct ps2_logic *log = (struct ps2_logic *)logic; + + float local; + u8 wk1,wk2,wk3; + + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + log->w.I0 = *log->I0; + log->w.I1 = *log->I1; + log->w.I2 = *log->I2; + log->w.I1_orta = *log->I1_orta; + log->w.I1_ortb = *log->I1_ortb; + log->w.U1 = *log->U1; + log->w.U1_orta = *log->U1_orta; + log->w.U1_ortb = *log->U1_ortb; + + + // sprawdzenie stanu przekanikw skadowej zerowej i przeciwnej. + if(log->nast_.D3Io) + { + wk1 = (log->w.I0 > log->nast_.Iomin) && (log->w.I1 * log->nast_.Iokh < log->w.I0); + sprawdz_P( + &log->dw.P_Io, + wk1, + !wk1, + &log->dw.liczIo, + 3,3 + ); + } else log->dw.P_Io = 0; + if(log->nast_.D3I2) + { + wk1 = (log->w.I2 > log->nast_.I2min) && (log->w.I1 * log->nast_.I2kh < log->w.I2); + sprawdz_P( + &log->dw.P_I2, + wk1, + !wk1, + &log->dw.liczI2, + 3,3 + ); + } else log->dw.P_I2 = 0; + + wk3 = log->dw.P_Io || log->dw.P_I2; ///< jest zwarcie (na pewno nie koysania mocy) +//-------------------------------------------------------------------------------------------------------------- + + //wyliczenie impedancji + log->dw.R = log->w.I1_orta * log->w.U1_orta + log->w.I1_ortb * log->w.U1_ortb; + log->dw.X = log->w.I1_ortb * log->w.U1_orta - log->w.I1_orta * log->w.U1_ortb; + local = log->w.I1_orta * log->w.I1_orta + log->w.I1_ortb * log->w.I1_ortb; + local = log->nast_.przekladnia / local; + log->dw.R *= local; + log->dw.X *= local; + log->dw.absR = fabs(log->dw.R); + log->dw.absX = fabs(log->dw.X); + + + + // okreslenie polozenia wektora impedancji wzgledem plaszczyzny nastaw + wk1 = (log->w.I1 > log->nast_.Imin) && + (log->dw.X < log->nast_.X1f) && + (log->dw.X > log->nast_.X1r) && + ((log->dw.X > (log->dw.R - log->nast_.R1f) * log->nast_.tanfi1) || (log->dw.R < log->nast_.R1f)) && + ((log->dw.X < (log->dw.R - log->nast_.R1r) * log->nast_.tanfi1) || (log->dw.R > log->nast_.R1r)) && + ((log->dw.absX > (log->dw.absR * log->nast_.tanfi2)) || ((log->dw.R < log->nast_.dRl1f) && (log->dw.R > log->nast_.dRl1r))) ; + +float Rwk2; + if(log->dw.R>0) + { + Rwk2 = log->dw.absR + log->nast_.dRlf; + } else { + Rwk2 = log->dw.absR + log->nast_.dRlr; + } + + wk2 = (log->w.I1 > log->nast_.Imin) && + (log->dw.X < log->nast_.X2f) && + (log->dw.X > log->nast_.X2r) && + ((log->dw.X > (log->dw.R - log->nast_.R2f) * log->nast_.tanfi1) || (log->dw.R < log->nast_.R2f)) && + ((log->dw.X < (log->dw.R - log->nast_.R2r) * log->nast_.tanfi1) || (log->dw.R > log->nast_.R2r)) && + ((log->dw.absX > (Rwk2 * log->nast_.tanfi2)) || + ((log->dw.R < log->nast_.dRl2f) && (log->dw.R > log->nast_.dRl2r))); + + sprawdz_P( + &log->dw.Ps1, + wk1, + !wk1, + &log->dw.liczs1, + 5,5 + ); + + sprawdz_P( + &log->dw.Ps2, + wk2, + !wk2, + &log->dw.liczs2, + 5,5 + ); + + switch(log->dw.stan_kolysanie) + { + case kolysanie_OK: + + //check_and_set(0, log->Bl_PS, log->Bl_PS_bit); + + if(log->dw.Ps1 && !wk3) + { + log->dw.liczk = log->nast_.tpass; + log->dw.stan_kolysanie = wejscie_w_strefe1; + log->dw.tpass = 0; + log->dw.t1 = 0; + } + break; + case wejscie_w_strefe1: + + //check_and_set(1, log->Bl_PS, log->Bl_PS_bit); + + if(log->dw.liczk>LOOP_CYCLE_MS) + log->dw.liczk-=LOOP_CYCLE_MS; + else + log->dw.liczk=0; + log->dw.tpass+=LOOP_CYCLE_MS; + + if(log->dw.Ps2) + { + log->dw.stan_kolysanie = blokada_kolysania; + } else { + if(!log->dw.liczk) + { + log->dw.stan_kolysanie = oczekiwanie_na_strefe2; + } + } + if (!log->dw.Ps1 || wk3) + { + log->dw.stan_kolysanie = kolysanie_OK; + } + break; + case oczekiwanie_na_strefe2: + log->dw.tpass+=LOOP_CYCLE_MS; + if (wk3) + { + log->dw.stan_kolysanie = blokada_kolysania; + } else { + if(log->dw.Ps2) + { + log->dw.b.Bl_PS = 1; ///< wczenie blokady + log->dw.liczdbl = log->nast_.tdbl; + log->dw.stan_kolysanie = kolysanie_mocy; + log->dw.Rz = log->dw.R; + log->dw.t1++; + } + } + break; + case kolysanie_mocy: + if (log->dw.Ps2) log->dw.t1+=LOOP_CYCLE_MS; + if(wk3) + { + log->dw.stan_kolysanie = blokada_kolysania; + } + break; + case blokada_kolysania: + log->dw.b.Bl_PS = 0; ///< wyczenie blokady + break; + } + + //obsuga wejcia testu przekanika + if (czy_test_R()) + { + if(*log->test & (1<test_bit)) + { + log->dw.b.Bl_PS = + log->dw.b.PS_asyn = + log->dw.b.PS_syn = 1; + } + else + { + log->dw.b.Bl_PS = + log->dw.b.PS_asyn = + log->dw.b.PS_syn = 0; + } + } + // + + + // realizacja blokady dzialania przekaznika + if(log->nast_.bl && (*log->stan_bl & (1<stan_bl_bit))) { + log->dw.b.Bl_PS = + log->dw.b.PS_asyn = + log->dw.b.PS_syn = 0; + } + // + + //---------------------------------- + + + check_and_set(log->dw.b.Bl_PS, log->Bl_PS, log->Bl_PS_bit); + check_and_set(log->dw.b.PS_asyn, log->PS_asyn, log->PS_asyn_bit); + check_and_set(log->dw.b.PS_syn, log->PS_syn, log->PS_syn_bit); + + + } else { + + *log->Bl_PS&=~(1<Bl_PS_bit); + *log->PS_syn&=~(1<PS_syn_bit); + *log->PS_asyn&=~(1<PS_asyn_bit); + } +} + +void ps2_100hz(void *arguments, void *logic) +{ + struct ps2_logic *log = (struct ps2_logic *)logic; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + log->dw.b.PS_syn = 0; + log->dw.b.PS_asyn = 0; + + switch(log->dw.stan_kolysanie) + { + case kolysanie_OK: + break; + case wejscie_w_strefe1: + break; + case oczekiwanie_na_strefe2: + if (!log->dw.Ps1) + { + log->dw.stan_kolysanie = kolysanie_OK; + } + + break; + case kolysanie_mocy: + if(!log->dw.Ps1) + { + log->dw.b.Bl_PS = 0; //wylaczenie blokady + if((log->dw.Rz * log->dw.R) > 0) //kolysania synchroniczne + { + log->dw.b.PS_syn = 1; + + } else + { //kolysania asynchroniczne + log->dw.b.PS_asyn = 1; + + } + log->dw.stan_kolysanie = kolysanie_OK; + } else { + log->dw.liczdbl--; + if(!log->dw.liczdbl) + { + log->dw.stan_kolysanie = blokada_kolysania; + log->dw.b.Bl_PS = 0; //wylaczenie blokady od kolysan mocy + } + } + + break; + case blokada_kolysania: + if (!log->dw.Ps1) + { + log->dw.stan_kolysanie = kolysanie_OK; + } + break; + default: + break; + } + } +} diff --git a/src/ps2.h b/src/ps2.h new file mode 100644 index 0000000..61070d7 --- /dev/null +++ b/src/ps2.h @@ -0,0 +1,192 @@ +/* + * ps2.h + * + * Created on: 08-08-2022 + * Author: PS + */ + +#ifndef PS2_H_ +#define PS2_H_ + +#include "tdefs.h" + +#include "ps.h" + +struct Nastawy_przeliczone_ps2 + { + float Imin;///< minimalna wartosc pr¹adu dzialania + float Iomin;///< Minimalna wartoœæ rozruchowa pr¹du zerowego przekaŸnika identyfikacji zwaræ z udzia³em ziemi + float Iokh;///io.stan_bl_in,&log->stan_bl,&log->stan_bl_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.f_ok_in, &log->f_ok, &log->f_ok_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.test_in, &log->test, &log->test_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.W_out, &log->W, &log->W_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.Z_out, &log->Z, &log->Z_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_out, &log->P, &log->P_bit_mask)) + return -1; + + if(set_float_ptr(args->io.I_float_in,&log->I)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.t_ = ((args->params.t_ - 0.005f) * 100); //czas dzialania + log->nast_.nad_pod = args->params.tryb; + + + log->nast_.rozruch = args->params.Wr_; + if (log->nast_.nad_pod) + log->nast_.odpad = log->nast_.rozruch - args->params.delta_; + else + log->nast_.odpad = log->nast_.rozruch + args->params.delta_; + + log->dw.lp_max = (short)(args->params.czas_wlasny_ / 10); + if (log->dw.lp_max < 3) + log->dw.lp_max = 3; + log->dw.lp = 0; + + return 0; +} + + +void r_freq_100hz(void *arguments, void *logic) +{ + struct r_freq_logic *log = (struct r_freq_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + if (log->nast_.nad_pod) //tryb nadczestotliwosciowy + { + u8 P = (*log->I >= log->nast_.rozruch) && (*log->f_ok & log->f_ok_bit_mask); + u8 O = (*log->I <= log->nast_.odpad) || ((*log->f_ok & log->f_ok_bit_mask)== 0); + + sprawdz_P_100(&log->dw.pob, P, O, &log->dw.lp, log->dw.lp_max, CZAS_ODPADU); + + } + else //tryb podczestotliwosciowy + { + u8 P = (*log->I <= log->nast_.rozruch) && (*log->f_ok & log->f_ok_bit_mask); + u8 O = (*log->I >= log->nast_.odpad) || ((*log->f_ok & log->f_ok_bit_mask)== 0); + + sprawdz_P_100(&log->dw.pob, P, O, &log->dw.lp, log->dw.lp_max, CZAS_ODPADU); + } + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob = (*log->test & log->test_bit_mask)?1:0; + } + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & log->stan_bl_bit_mask)) /*|| czy_start()*/) + log->dw.pob = 0; + + // realizacja przekaznika czasowego + if(log->dw.pob) + log->dw.licz_t_++; + else + log->dw.licz_t_ = 0; // odliczanie czasu dzialania + + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } + else + log->dw.zad = 0; + + check_and_set_mask(log->dw.pob,log->P,log->P_bit_mask); + check_and_set_mask(log->dw.zad,log->Z,log->Z_bit_mask); + check_and_set_mask(log->dw.zad && log->nast_.w_,log->W,log->W_bit_mask); + + + } + else + { + *log->W&=~log->W_bit_mask; + *log->Z&=~log->Z_bit_mask; + *log->P&=~log->P_bit_mask; + } +} diff --git a/src/r_freq.h b/src/r_freq.h new file mode 100644 index 0000000..0131eff --- /dev/null +++ b/src/r_freq.h @@ -0,0 +1,87 @@ +/* + * r_freq.h + * + * Created on: 08-05-2014 + * Author: Pawel Skoczykloda + */ + +#ifndef R_FREQ_H_ +#define R_FREQ_H_ +/* +#include "pawel_usun_to.h" +*/ +#include "tdefs.h" + + +struct dane_wewnetrzne_r_freq +{ + short lp; ///in_ptr = (u16 *)(log_manager.nets_data + (args->io.sample_short_in >> 3)); + if((u8*)log->in_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(u16))) + return -1; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->an_params=(struct analog_in_params *)*an_params_ptr; + + return 0; +} + diff --git a/src/rec_an.h b/src/rec_an.h new file mode 100644 index 0000000..115c22f --- /dev/null +++ b/src/rec_an.h @@ -0,0 +1,42 @@ +/* + * rec_an.h + * + * + * Created on: 24-06-2014 + * Author: KJ + */ + +#ifndef REC_AN_H_ +#define REC_AN_H_ + +#define REC_CHAN_ENABLED_DFR 0x00000001 + +#include "tdefs.h" + +struct rec_an_logic +{ + u16 *in_ptr; + struct analog_in_params *an_params; +}; + +struct rec_an_io +{ + u32 sample_short_in; + u32 param_an_ptr_in; +}__attribute__((__packed__)); + +struct rec_an_params +{ + u32 bits; +}__attribute__((__packed__)); + +struct rec_an_args +{ + struct rec_an_io io; + struct rec_an_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int rec_an_initlog(void *args, void *logic); + +#endif /* REC_AN_H_ */ diff --git a/src/rec_bin.c b/src/rec_bin.c new file mode 100644 index 0000000..66e25f7 --- /dev/null +++ b/src/rec_bin.c @@ -0,0 +1,27 @@ +/* + * rec_bin.c + * + * Created on: 24-06-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "rec_bin.h" + +int rec_bin_initlog(void *arguments, void *logic) +{ + struct rec_bin_args *args = (struct rec_bin_args *)arguments; + struct rec_bin_logic *log = (struct rec_bin_logic *)logic; + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_mask = 1<<(args->io.in & 0x07); + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + diff --git a/src/rec_bin.h b/src/rec_bin.h new file mode 100644 index 0000000..d06d386 --- /dev/null +++ b/src/rec_bin.h @@ -0,0 +1,42 @@ +/* + * rec_bin.h + * + * + * Created on: 24-06-2014 + * Author: KJ + */ + +#ifndef REC_BIN_H_ +#define REC_BIN_H_ + +#include "tdefs.h" + +#define REC_CHAN_ENABLED_DFR 0x00000001 +#define REC_CHAN_ENABLED_DDR 0x00000002 + +struct rec_bin_logic +{ + u8 *in_ptr; + u8 in_bit_mask; +}; + +struct rec_bin_io +{ + u32 in; +}__attribute__((__packed__)); + +struct rec_bin_params +{ + u32 bits; +}__attribute__((__packed__)); + +struct rec_bin_args +{ + struct rec_bin_io io; + struct rec_bin_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int rec_bin_initlog(void *args, void *logic); + +#endif /* REC_BIN_H_ */ diff --git a/src/rec_buf.c b/src/rec_buf.c new file mode 100644 index 0000000..43a3708 --- /dev/null +++ b/src/rec_buf.c @@ -0,0 +1,42 @@ +/* + * rec_an.c + * + * Created on: 18-01-2021 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "rec_buf.h" + +int rec_buf_initlog(void *arguments, void *logic) +{ + struct rec_buf_args *args = (struct rec_buf_args *)arguments; + struct rec_buf_logic *log = (struct rec_buf_logic *)logic; + + u32 *an_params_ptr; + u32 *buf_ptr; + + buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3)); + if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->buf_ptr=(u16*)*buf_ptr; + + an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3)); + if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *))) + return -1; + + if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before) + return -1; + + log->an_params=(struct analog_in_params *)*an_params_ptr; + + return 0; +} + diff --git a/src/rec_buf.h b/src/rec_buf.h new file mode 100644 index 0000000..e18b2e4 --- /dev/null +++ b/src/rec_buf.h @@ -0,0 +1,42 @@ +/* + * rec_buf.h + * + * + * Created on: 18-01-2021 + * Author: KJ + */ + +#ifndef REC_BUF_H_ +#define REC_BUF_H_ + +#define REC_CHAN_ENABLED_DFR 0x00000001 + +#include "tdefs.h" + +struct rec_buf_logic +{ + u16 *buf_ptr; + struct analog_in_params *an_params; +}; + +struct rec_buf_io +{ + u32 buf_u16_ptr_in; + u32 param_an_ptr_in; +}__attribute__((__packed__)); + +struct rec_buf_params +{ + u32 bits; +}__attribute__((__packed__)); + +struct rec_buf_args +{ + struct rec_buf_io io; + struct rec_buf_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int rec_buf_initlog(void *args, void *logic); + +#endif /* REC_BUF_H_ */ diff --git a/src/rec_float.c b/src/rec_float.c new file mode 100644 index 0000000..2a5b72b --- /dev/null +++ b/src/rec_float.c @@ -0,0 +1,43 @@ +/* + * rec_float.c + * + * Created on: 04-01-2017 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "rec_float.h" +#include "helper.h" + +int rec_float_initlog(void *arguments, void *logic) +{ + struct rec_float_args *args = (struct rec_float_args *)arguments; + struct rec_float_logic *log = (struct rec_float_logic *)logic; + + if(set_float_ptr(args->io.sample_float_in,&log->sample)) + return -1; + + log->sample_new=0; + log->in_ptr = &log->sample_new; + + log->mul=((double)32768.0/args->params.wart_absmax); + + return 0; +} + +void rec_float(void *arguments, void *logic) +{ + struct rec_float_logic *log = (struct rec_float_logic *)logic; + float tmp; + + if(*log->sample==32768.0f) + log->sample_new=32768; + else + { + tmp = (*log->sample*log->mul); + log->sample_new=tmp>32767.0f?32767:(tmp<-32767.0f?-32767:(u16)tmp); + } +} diff --git a/src/rec_float.h b/src/rec_float.h new file mode 100644 index 0000000..0860e15 --- /dev/null +++ b/src/rec_float.h @@ -0,0 +1,49 @@ +/* + * rec_float.h + * + * + * Created on: 04-01-2017 + * Author: KJ + */ + +#ifndef REC_FLOAT_H_ +#define REC_FLOAT_H_ + +#include "tdefs.h" + +#define REC_CHAN_ENABLED_DFR 0x00000001 +#define REC_CHAN_ENABLED_DDR 0x00000002 + +struct rec_float_logic +{ + u16 *in_ptr; + u16 sample_new; + float *sample; + double mul; +}; + +struct rec_float_io +{ + u32 sample_float_in; +}__attribute__((__packed__)); + +struct rec_float_params +{ + u32 bits; + u32 jednostka; + double pierw; + double wtor; + double wart_absmax; +}__attribute__((__packed__)); + +struct rec_float_args +{ + struct rec_float_io io; + struct rec_float_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int rec_float_initlog(void *args, void *logic); +extern void rec_float(void *args, void *logic); + +#endif /* REC_FLOAT_H_ */ diff --git a/src/relay_out.c b/src/relay_out.c new file mode 100644 index 0000000..c5fa3a8 --- /dev/null +++ b/src/relay_out.c @@ -0,0 +1,80 @@ +/* + * relay_out.c + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "relay_out.h" + +int relay_out_initlog(void *arguments, void *logic) +{ + struct relay_out_args *args = (struct relay_out_args *)arguments; + struct relay_out_logic *log = (struct relay_out_logic *)logic; + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_mask = args->io.in & 0x07; + log->in_bit_mask = (1<in_bit_mask); + + log->clr_ptr = log_manager.nets_data + (args->io.clr_in >> 3); + log->clr_bit_mask = args->io.clr_in & 0x07; + log->clr_bit_mask = (1<clr_bit_mask); + + log->kob_ptr = log_manager.nets_data + (args->io.kob_out >> 3); + log->kob_bit_mask = args->io.kob_out & 0x07; + log->kob_bit_mask = (1<kob_bit_mask); + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->clr_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->kob_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + log->kob_msw_bit_mask=(1<<(args->params.relay_num/3))<<12; + + if(args->params.card_addr>(MAX_OUT_CARDS-1)) + return -1; + + if(args->params.relay_num>15) + return -1; + + return 0; +} + +void relay_out(void *arguments, void *logic) +{ + struct relay_out_args *args = (struct relay_out_args *)arguments; + struct relay_out_logic *log = (struct relay_out_logic *)logic; + + if(*log->in_ptr & log->in_bit_mask) + bus_out_data[args->params.card_addr]|=(1<params.relay_num); + else if(!(args->params.bits & RELAY_OUT_UNTIL_CLR)) + bus_out_data[args->params.card_addr]&=~(1<params.relay_num); + + if(!(*log->in_ptr & log->in_bit_mask) && (*log->clr_ptr & log->clr_bit_mask)) + bus_out_data[args->params.card_addr]&=~(1<params.relay_num); + + if(args->io.kob_out) + { + if(args->params.bits & RELAY_OUT_VIA_MSW) + { + if((ic->kob_out & (1<params.card_addr)) && (ic->out_set[args->params.card_addr] & log->kob_msw_bit_mask)) + *log->kob_ptr|=log->kob_bit_mask; + else + *log->kob_ptr&=~log->kob_bit_mask; + } + else + { + if(ic->kob_out & (1<params.card_addr)) + *log->kob_ptr|=log->kob_bit_mask; + else + *log->kob_ptr&=~log->kob_bit_mask; + } + } +} diff --git a/src/relay_out.h b/src/relay_out.h new file mode 100644 index 0000000..cceae56 --- /dev/null +++ b/src/relay_out.h @@ -0,0 +1,52 @@ +/* + * relay_out.h + * + * Created on: 07-04-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef RELAY_OUT_H_ +#define RELAY_OUT_H_ + +#include "tdefs.h" + +#define RELAY_OUT_UNTIL_CLR 0x01 +#define RELAY_OUT_VIA_MSW 0x02 + +struct relay_out_logic +{ + u8 *in_ptr; + u8 in_bit_mask; + u8 *clr_ptr; + u8 clr_bit_mask; + u8 *kob_ptr; + u8 kob_bit_mask; + u16 kob_msw_bit_mask; +}; + +struct relay_out_io +{ + u32 in; // one bit input for relay + u32 clr_in; // clr bit input + u32 kob_out; // kob bit output +}__attribute__((__packed__)); + +struct relay_out_params +{ + u32 card_addr; + u32 relay_num; + u32 bits; +}__attribute__((__packed__)); + + +struct relay_out_args +{ + struct relay_out_io io; + struct relay_out_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void relay_out(void *args, void *logic); +extern int relay_out_initlog(void *arguments, void *logic); + +#endif /* RELAY_OUT_H_ */ diff --git a/src/s_demux.c b/src/s_demux.c new file mode 100644 index 0000000..3671683 --- /dev/null +++ b/src/s_demux.c @@ -0,0 +1,218 @@ +/* + * s_demux.c + * + * Created on: 29-10-2019 + * Author: Krzysztof Jakubczyk + */ + +#include +#include + +#include "tdefs.h" +#include "misc.h" +#include "eth.h" +#include "helper.h" +#include "s_demux.h" +#include "analog_in.h" +#include "R87L.h" + +u16 comm_bits_act[3] = {0,0,0}; + +int s_demux_initlog(void *arguments, void *logic) +{ + struct s_demux_args *args = (struct s_demux_args *)arguments; + struct s_demux_logic *log = (struct s_demux_logic *)logic; +// u8 i; + + if(set_pointer_in_ptr(args->io.stream1_ptr_in,(u32*)&log->stream_in[0])) + return -1; + if(set_pointer_in_ptr(args->io.stream2_ptr_in,(u32*)&log->stream_in[1])) + return -1; + if(set_pointer_in_ptr(args->io.stream3_ptr_in,(u32*)&log->stream_in[2])) + return -1; + + if(set_pointer_in_ptr(args->io.astream1_ptr_in,(u32*)&log->astream_in[0])) + return -1; + if(set_pointer_in_ptr(args->io.astream2_ptr_in,(u32*)&log->astream_in[1])) + return -1; + if(set_pointer_in_ptr(args->io.astream3_ptr_in,(u32*)&log->astream_in[2])) + return -1; + + if(set_bit_ptr_struct(args->io.stream1_act_in,&log->stream_in_act[0])) + return -1; + if(set_bit_ptr_struct(args->io.stream2_act_in,&log->stream_in_act[1])) + return -1; + if(set_bit_ptr_struct(args->io.stream3_act_in,&log->stream_in_act[2])) + return -1; + + if(set_bit_ptr_struct(args->io.astream1_act_in,&log->astream_in_act[0])) + return -1; + if(set_bit_ptr_struct(args->io.astream2_act_in,&log->astream_in_act[1])) + return -1; + if(set_bit_ptr_struct(args->io.astream3_act_in,&log->astream_in_act[2])) + return -1; + + if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32*)&log->stream_out[0])) + return -1; + if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32*)&log->stream_out[1])) + return -1; + if(set_pointer_out_ptr(args->io.stream3_ptr_out,(u32*)&log->stream_out[2])) + return -1; + if(set_bit_ptr_struct(args->io.streams_ok_out,&log->streams_ok)) + return -1; + + if(set_bit_ptr_struct(args->io.przeglad1_out,&log->przeglad[0])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad2_out,&log->przeglad[1])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad3_out,&log->przeglad[2])) + return -1; + + log->jednostki = args->params.liczba_jednostek+2; + + if(log->jednostki!=2 && log->jednostki !=3) + log->jednostki=2; + + if(set_bit_ptr_struct(args->io.three_endpoints_out,&log->three_endpoints)) + return -1; + + check_and_set_struct(log->jednostki==3,&log->three_endpoints); + + return 0; +} + +void s_demux(void *arguments, void *logic) +{ + struct s_demux_logic *log = (struct s_demux_logic *)logic; + + u8 i,j,stream_ok,str_ok[3],str_ok_ndelay[3]; + stream_ok=1; + u8 alt_stream; + + alt_stream=0; + + if(log->jednostki==2) + { + if((!check_struct(&log->stream_in_act[0]) && !(comm_bits_act[0] & R87L_COMM_PRZEGLAD)) || (!check_struct(&log->stream_in_act[1]) && !(comm_bits_act[1] & R87L_COMM_PRZEGLAD))) + alt_stream=1; + } + else if(log->jednostki==3) + { + if((!check_struct(&log->stream_in_act[0]) && !(comm_bits_act[0] & R87L_COMM_PRZEGLAD)) || (!check_struct(&log->stream_in_act[1]) && !(comm_bits_act[1] & R87L_COMM_PRZEGLAD))|| (!check_struct(&log->stream_in_act[2]) && !(comm_bits_act[2] & R87L_COMM_PRZEGLAD))) + alt_stream=1; + } + + for(i=0;ijednostki;i++) + { + str_ok[i]=1; + str_ok_ndelay[i]=1; + + if(check_struct(&log->stream_in_act[i]) && !alt_stream) + { + log->snum_cur[i]=log->stream_in[i]->sample_stamp; + + if((log->snum_prev[i]+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=log->snum_cur[i]) + { + log->sfault_delay_cnt[i]=20; + if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok[i]=0; + str_ok_ndelay[i]=0; + } + else if(log->sfault_delay_cnt[i]) + { + if(log->sfault_delay_cnt[i]>=LOOP_CYCLE_MS) + log->sfault_delay_cnt[i]-=LOOP_CYCLE_MS; + else + log->sfault_delay_cnt[i]=0; + + if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok[i]=0; + } + + log->ostream_ptr[i]=log->stream_in[i]; + log->snum_prev[i]=log->snum_cur[i]; + } + else if(check_struct(&log->astream_in_act[i])) + { + log->snum_cur[i]=log->astream_in[i]->sample_stamp; + + if((log->snum_prev[i]+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=log->snum_cur[i]) + { + log->sfault_delay_cnt[i]=20; + if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + + str_ok[i]=0; + str_ok_ndelay[i]=0; + } + else if(log->sfault_delay_cnt[i]) + { + if(log->sfault_delay_cnt[i]>=LOOP_CYCLE_MS) + log->sfault_delay_cnt[i]-=LOOP_CYCLE_MS; + else + log->sfault_delay_cnt[i]=0; + if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok[i]=0; + } + + log->ostream_ptr[i]=log->astream_in[i]; + log->snum_prev[i]=log->snum_cur[i]; + } + else + { + if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok_ndelay[i]=0; + str_ok[i]=0; + } + + if(i>0 && log->snum_cur[i]!=log->snum_cur[i-1] && !(comm_bits_act[i] & R87L_COMM_PRZEGLAD) && !(comm_bits_act[i-1] & R87L_COMM_PRZEGLAD)) + { + log->sfault_delay_cnt[i]=20; + stream_ok=0; + str_ok[i]=0; + str_ok_ndelay[i]=0; + } + } + + for(i=0;ijednostki;i++) + { + if(str_ok[i]) + comm_bits_act[i]=log->ostream_ptr[i]->bits; + + check_and_set_struct(comm_bits_act[i] & R87L_COMM_PRZEGLAD,&log->przeglad[i]); + } + + check_and_set_struct(stream_ok,&log->streams_ok); + + if(stream_ok) + { + for(i=0;ijednostki;i++) + { + if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame)); + else + { + memset((u8*)&log->stream_out[i],0,sizeof(log->stream_out[0])); + log->stream_out[i].bits=(comm_bits_act[i] & (R87L_COMM_PRZEGLAD|0xFF00)); // przeglad bit i GPbits przepisuj w przegladzie + } + } + } + else + { + for(i=0;ijednostki;i++) + if(str_ok_ndelay[i]) + memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame)); + else + { + for(j=0;j<3;j++) + log->stream_out[i].A[j]=32768.0f; + } +// memset((u8*)log->stream_out,0,sizeof(log->stream_out)); // zostawic ostatnie dane? +// for(i=0;i<3;i++) +// log->stream_out[i].bits=(comm_bits_act[i] & R87L_COMM_PRZEGLAD); + } +} diff --git a/src/s_demux.h b/src/s_demux.h new file mode 100644 index 0000000..0acb015 --- /dev/null +++ b/src/s_demux.h @@ -0,0 +1,82 @@ +/* + * s_demux.h + * + * Created on: 29-10-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef S_DEMUX_H_ +#define S_DEMUX_H_ + +#include "tdefs.h" +#include "helper.h" +#include "mkstream.h" + +extern u16 comm_bits_act[3]; +extern u16 comm_bits_restored[3]; + +struct s_demux_logic +{ + struct stream_frame *stream_in[3]; + struct binary_io stream_in_act[3]; + struct stream_frame *astream_in[3]; + struct binary_io astream_in_act[3]; + struct stream_frame stream_out[3]; + struct binary_io streams_ok; + + struct stream_frame *ostream_ptr[3]; + struct binary_io przeglad[3]; + struct binary_io three_endpoints; + + u32 snum_cur[3]; + u32 snum_prev[3]; + u32 sfault_delay_cnt[3]; + u32 jednostki; +}; + +struct s_demux_io +{ + u32 stream1_ptr_in; + u32 stream1_act_in; + u32 astream1_ptr_in; + u32 astream1_act_in; + + u32 stream2_ptr_in; + u32 stream2_act_in; + u32 astream2_ptr_in; + u32 astream2_act_in; + + u32 stream3_ptr_in; + u32 stream3_act_in; + u32 astream3_ptr_in; + u32 astream3_act_in; + + u32 stream1_ptr_out; + u32 stream2_ptr_out; + u32 stream3_ptr_out; + u32 streams_ok_out; + + u32 przeglad1_out; + u32 przeglad2_out; + u32 przeglad3_out; + + u32 three_endpoints_out; +}__attribute__((__packed__)); + + +struct s_demux_params +{ + u32 liczba_jednostek; +}__attribute__((__packed__)); + +struct s_demux_args +{ + struct s_demux_io io; + struct s_demux_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void s_demux(void *args, void *logic); +extern int s_demux_initlog(void *arguments, void *logic); + +#endif diff --git a/src/s_demux2.c b/src/s_demux2.c new file mode 100644 index 0000000..7dda02c --- /dev/null +++ b/src/s_demux2.c @@ -0,0 +1,209 @@ +/* + * s_demux2.c + * + * Created on: 26-04-2024 + * Author: Krzysztof Jakubczyk + */ + +#include +#include + +#include "tdefs.h" +#include "misc.h" +#include "eth.h" +#include "helper.h" +#include "s_demux2.h" +#include "analog_in.h" +#include "R87L.h" + +int s_demux2_initlog(void *arguments, void *logic) +{ + struct s_demux2_args *args = (struct s_demux2_args *)arguments; + struct s_demux2_logic *log = (struct s_demux2_logic *)logic; +// u8 i; + + if(set_pointer_in_ptr(args->io.stream1_ptr_in,(u32*)&log->stream_in[0])) + return -1; + if(set_pointer_in_ptr(args->io.stream2_ptr_in,(u32*)&log->stream_in[1])) + return -1; + if(set_pointer_in_ptr(args->io.stream3_ptr_in,(u32*)&log->stream_in[2])) + return -1; + + if(set_pointer_in_ptr(args->io.astream1_ptr_in,(u32*)&log->astream_in[0])) + return -1; + if(set_pointer_in_ptr(args->io.astream2_ptr_in,(u32*)&log->astream_in[1])) + return -1; + if(set_pointer_in_ptr(args->io.astream3_ptr_in,(u32*)&log->astream_in[2])) + return -1; + + if(set_bit_ptr_struct(args->io.stream1_act_in,&log->stream_in_act[0])) + return -1; + if(set_bit_ptr_struct(args->io.stream2_act_in,&log->stream_in_act[1])) + return -1; + if(set_bit_ptr_struct(args->io.stream3_act_in,&log->stream_in_act[2])) + return -1; + + if(set_bit_ptr_struct(args->io.astream1_act_in,&log->astream_in_act[0])) + return -1; + if(set_bit_ptr_struct(args->io.astream2_act_in,&log->astream_in_act[1])) + return -1; + if(set_bit_ptr_struct(args->io.astream3_act_in,&log->astream_in_act[2])) + return -1; + + if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32*)&log->stream_out[0])) + return -1; + if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32*)&log->stream_out[1])) + return -1; + if(set_pointer_out_ptr(args->io.stream3_ptr_out,(u32*)&log->stream_out[2])) + return -1; + if(set_bit_ptr_struct(args->io.streams_ok_out,&log->streams_ok)) + return -1; + + if(set_bit_ptr_struct(args->io.przeglad1_out,&log->przeglad[0])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad2_out,&log->przeglad[1])) + return -1; + if(set_bit_ptr_struct(args->io.przeglad3_out,&log->przeglad[2])) + return -1; + + log->jednostki = args->params.liczba_jednostek+2; + + if(log->jednostki!=2 && log->jednostki !=3) + log->jednostki=2; + + if(set_bit_ptr_struct(args->io.three_endpoints_out,&log->three_endpoints)) + return -1; + + check_and_set_struct(log->jednostki==3,&log->three_endpoints); + + return 0; +} + +void s_demux2(void *arguments, void *logic) +{ + struct s_demux2_logic *log = (struct s_demux2_logic *)logic; + + u8 i,j,stream_ok,str_ok[3],str_ok_ndelay[3]; + stream_ok=1; + u8 alt_stream; + + alt_stream=0; + + if(log->jednostki==2) + { + if((!check_struct(&log->stream_in_act[0])) || (!check_struct(&log->stream_in_act[1]) )) + alt_stream=1; + } + else if(log->jednostki==3) + { + if((!check_struct(&log->stream_in_act[0]) ) || (!check_struct(&log->stream_in_act[1]) )|| (!check_struct(&log->stream_in_act[2]) )) + alt_stream=1; + } + + for(i=0;ijednostki;i++) + { + str_ok[i]=1; + str_ok_ndelay[i]=1; + + if(check_struct(&log->stream_in_act[i]) && !alt_stream) + { + log->snum_cur[i]=log->stream_in[i]->sample_stamp; + + if((log->snum_prev[i]+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=log->snum_cur[i]) + { + log->sfault_delay_cnt[i]=20; +// if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) +// stream_ok=0; + str_ok[i]=0; + str_ok_ndelay[i]=0; + } + else if(log->sfault_delay_cnt[i]) + { + if(log->sfault_delay_cnt[i]>=LOOP_CYCLE_MS) + log->sfault_delay_cnt[i]-=LOOP_CYCLE_MS; + else + log->sfault_delay_cnt[i]=0; + +// if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) +// stream_ok=0; + str_ok[i]=0; + } + + log->ostream_ptr[i]=log->stream_in[i]; + log->snum_prev[i]=log->snum_cur[i]; + } + else if(check_struct(&log->astream_in_act[i])) + { + log->snum_cur[i]=log->astream_in[i]->sample_stamp; + + if((log->snum_prev[i]+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=log->snum_cur[i]) + { + log->sfault_delay_cnt[i]=20; +// if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) +// stream_ok=0; + + str_ok[i]=0; + str_ok_ndelay[i]=0; + } + else if(log->sfault_delay_cnt[i]) + { + if(log->sfault_delay_cnt[i]>=LOOP_CYCLE_MS) + log->sfault_delay_cnt[i]-=LOOP_CYCLE_MS; + else + log->sfault_delay_cnt[i]=0; +// if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) +// stream_ok=0; + str_ok[i]=0; + } + + log->ostream_ptr[i]=log->astream_in[i]; + log->snum_prev[i]=log->snum_cur[i]; + } + else + { +// if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + stream_ok=0; + str_ok_ndelay[i]=0; + str_ok[i]=0; + } + +//Wyrownywanie probek strumieni +// if(i>0 && log->snum_cur[i]!=log->snum_cur[i-1] /*&& !(comm_bits_act[i] & R87L_COMM_PRZEGLAD) && !(comm_bits_act[i-1] & R87L_COMM_PRZEGLAD)*/) +// { +// log->sfault_delay_cnt[i]=20; +// stream_ok=0; +// str_ok[i]=0; +// str_ok_ndelay[i]=0; +// } + } + + check_and_set_struct(stream_ok,&log->streams_ok); + + if(stream_ok) + { + for(i=0;ijednostki;i++) + { +// if(!(comm_bits_act[i] & R87L_COMM_PRZEGLAD)) + memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame)); +/* else + { + memset((u8*)&log->stream_out[i],0,sizeof(log->stream_out[0])); + log->stream_out[i].bits=(comm_bits_act[i] & (R87L_COMM_PRZEGLAD|0xFF00)); // przeglad bit i GPbits przepisuj w przegladzie + }*/ + } + } + else + { + for(i=0;ijednostki;i++) + if(str_ok_ndelay[i]) + memcpy((u8 *)&log->stream_out[i],(u8 *)log->ostream_ptr[i],sizeof(struct stream_frame)); + else + { + for(j=0;j<3;j++) + log->stream_out[i].A[j]=32768.0f; + } +// memset((u8*)log->stream_out,0,sizeof(log->stream_out)); // zostawic ostatnie dane? +// for(i=0;i<3;i++) +// log->stream_out[i].bits=(comm_bits_act[i] & R87L_COMM_PRZEGLAD); + } +} diff --git a/src/s_demux2.h b/src/s_demux2.h new file mode 100644 index 0000000..3985fd6 --- /dev/null +++ b/src/s_demux2.h @@ -0,0 +1,81 @@ +/* + * s_demux2.h + * + * Created on: 26-04-2024 + * Author: Krzysztof Jakubczyk + */ + +#ifndef S_DEMUX2_H_ +#define S_DEMUX2_H_ + +#include "tdefs.h" +#include "helper.h" +#include "mkstream.h" + + + +struct s_demux2_logic +{ + struct stream_frame *stream_in[3]; + struct binary_io stream_in_act[3]; + struct stream_frame *astream_in[3]; + struct binary_io astream_in_act[3]; + struct stream_frame stream_out[3]; + struct binary_io streams_ok; + + struct stream_frame *ostream_ptr[3]; + struct binary_io przeglad[3]; + struct binary_io three_endpoints; + + u32 snum_cur[3]; + u32 snum_prev[3]; + u32 sfault_delay_cnt[3]; + u32 jednostki; +}; + +struct s_demux2_io +{ + u32 stream1_ptr_in; + u32 stream1_act_in; + u32 astream1_ptr_in; + u32 astream1_act_in; + + u32 stream2_ptr_in; + u32 stream2_act_in; + u32 astream2_ptr_in; + u32 astream2_act_in; + + u32 stream3_ptr_in; + u32 stream3_act_in; + u32 astream3_ptr_in; + u32 astream3_act_in; + + u32 stream1_ptr_out; + u32 stream2_ptr_out; + u32 stream3_ptr_out; + u32 streams_ok_out; + + u32 przeglad1_out; + u32 przeglad2_out; + u32 przeglad3_out; + + u32 three_endpoints_out; +}__attribute__((__packed__)); + + +struct s_demux2_params +{ + u32 liczba_jednostek; +}__attribute__((__packed__)); + +struct s_demux2_args +{ + struct s_demux2_io io; + struct s_demux2_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void s_demux2(void *args, void *logic); +extern int s_demux2_initlog(void *arguments, void *logic); + +#endif diff --git a/src/sck.c b/src/sck.c new file mode 100644 index 0000000..e7deb6e --- /dev/null +++ b/src/sck.c @@ -0,0 +1,336 @@ +/* + * sck.c + * + * Created on: 11-01-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "sck.h" + +const u32 NaN = 0xFFFFFFFF; + +int sck_initlog(void *arguments, void *logic) +{ + struct sck_args *args = (struct sck_args *)arguments; + struct sck_logic *log = (struct sck_logic *)logic; + + if(set_bit_ptr_mask(args->io.bl_in,&log->bl,&log->bl_bit_mask)) + return -1; + + if(set_float_ptr(args->io.U1_orta_float_in,&log->U1_orta)) + return -1; + if(set_float_ptr(args->io.U1_ortb_float_in,&log->U1_ortb)) + return -1; + + if(set_float_ptr(args->io.U2_orta_float_in,&log->U2_orta)) + return -1; + if(set_float_ptr(args->io.U2_ortb_float_in,&log->U2_ortb)) + return -1; + + if(set_float_ptr(args->io.U3_orta_float_in,&log->U3_orta)) + return -1; + if(set_float_ptr(args->io.U3_ortb_float_in,&log->U3_ortb)) + return -1; + + if(set_float_ptr(args->io.Ub_orta_float_in,&log->Ub_orta)) + return -1; + + if(set_float_ptr(args->io.Ub_ortb_float_in,&log->Ub_ortb)) + return -1; + + if(set_float_ptr(args->io.Ub_float_in,&log->Ub)) + return -1; + + if(set_float_ptr(args->io.fa_float_in,&log->fa)) + return -1; + + if(set_bit_ptr_mask(args->io.fa_OK_in,&log->fa_OK,&log->fa_OK_bit_mask)) + return -1; + + if(set_float_ptr(args->io.fb_float_in,&log->fb)) + return -1; + + if(set_bit_ptr_mask(args->io.fb_OK_in,&log->fb_OK,&log->fb_OK_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.test_in,&log->test,&log->test_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.Z_out,&log->Z,&log->Z_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_Uam_out,&log->P_Uam,&log->P_Uam_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_Uaw_out,&log->P_Uaw,&log->P_Uaw_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_Ubm_out,&log->P_Ubm,&log->P_Ubm_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_Ubw_out,&log->P_Ubw,&log->P_Ubw_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_DU_out,&log->P_DU,&log->P_DU_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_Df_out,&log->P_Df,&log->P_Df_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_Dfi_out,&log->P_Dfi,&log->P_Dfi_bit_mask)) + return -1; + + if(set_float_ptr(args->io.Df_float_out,&log->Df)) + return -1; + if(set_float_ptr(args->io.Dfi_float_out,&log->Dfi)) + return -1; + if(set_float_ptr(args->io.DU_float_out,&log->DU)) + return -1; + + if(set_bit_ptr_mask(args->io.f_OK_out,&log->f_OK,&log->f_OK_bit_mask)) + return -1; + + log->nast_.On = (args->params.bity & 0x0001)?1:0; + log->nast_.bl = (args->params.bity & 0x0002)?1:0; + log->nast_.N00 = (args->params.bity & 0x0004)?1:0; + log->nast_.N01 = (args->params.bity & 0x0008)?1:0; + log->nast_.N10 = (args->params.bity & 0x0010)?1:0; + log->nast_.N11 = (args->params.bity & 0x0020)?1:0; + log->nast_.delU = (args->params.bity & 0x0040)?1:0; + + log->nast_.Ub = args->params.Ub; + + //wartosci progowe dla kryterium fazowego + log->nast_.Dfir = cos(args->params.Dfir * 3.14159f / 180); + log->nast_.Dfir = log->nast_.Dfir * log->nast_.Dfir * 4; + log->nast_.Dfip = cos((args->params.Dfir + 0.2) * 3.14159f / 180); + log->nast_.Dfip = log->nast_.Dfip * log->nast_.Dfip * 4; + // + //wartosci progowe dla kryterium czestotliwosci + log->nast_.Dfr = args->params.Dfr; + log->nast_.Dfp = args->params.Dfr + 0.002; + // + //wartosci progowe dla kryterium roznicy napiecia + log->nast_.DUr = args->params.DUr * args->params.DUr; + log->nast_.DUp = log->nast_.DUr * 1.01; + // + //wartosci rozruchowe dla dedekcji obecnosci napiec + log->nast_.Uarm = args->params.Uarm * args->params.Uarm; + log->nast_.Uapm = log->nast_.Uarm * 1.04; + log->nast_.Ubrm = args->params.Ubrm * args->params.Ubrm; + log->nast_.Ubpm = log->nast_.Ubrm * 1.04; + log->nast_.Uarw = args->params.Uarw * args->params.Uarw; + log->nast_.Uapw = log->nast_.Uarw * 0.96; + log->nast_.Ubrw = args->params.Ubrw * args->params.Ubrw; + log->nast_.Ubpw = log->nast_.Ubrw * 0.96; + // + + return 0; +} + +void sck(void *arguments, void *logic) +{ + struct sck_logic *log = (struct sck_logic *)logic; + + float orta_a; + float ortb_a; + float cosfi2; + + if(log->nast_.On) + { + //wyliczenie napiecia wejsciowego w zalenosci od nastaw + switch(log->nast_.Ub) + { + case 0: //faza L1 + orta_a = *log->U1_orta; + ortb_a = *log->U1_ortb; + break; + case 1: //faza L2 + orta_a = *log->U2_orta; + ortb_a = *log->U2_ortb; + break; + case 2: //faza L3 + orta_a = *log->U3_orta; + ortb_a = *log->U3_ortb; + break; + case 3: //faza L12 + orta_a = *log->U1_orta - *log->U2_orta; + ortb_a = *log->U1_ortb - *log->U2_ortb; + break; + case 4: //faza L23 + orta_a = *log->U2_orta - *log->U3_orta; + ortb_a = *log->U2_ortb - *log->U3_ortb; + break; + case 5: //faza L31 + orta_a = *log->U3_orta - *log->U1_orta; + ortb_a = *log->U3_ortb - *log->U1_ortb; + break; + } + + log->A = (orta_a * orta_a + ortb_a * ortb_a) / 2; + + if (log->nast_.delU) + { + //Wyliczenie roznicy wartosci skutecznych + log->dw.DU = sqrtf(log->A) - sqrtf(*log->Ub); + log->dw.DU*=log->dw.DU; + } + else + { + //wyliczenie ronicy wektorowej napiec + log->dw.DUa = orta_a - *log->Ub_orta; + log->dw.DUb = ortb_a - *log->Ub_ortb; + log->dw.DU = (log->dw.DUa * log->dw.DUa + log->dw.DUb * log->dw.DUb) / 2; + } + + //sprawdzenie kryteriow napieciowcyh fazowych + sprawdz_P( + &log->stan_.P_Uam, + log->A < log->nast_.Uarm, + log->A > log->nast_.Uapm, + &log->dw.lUarm, + 5,5); + + sprawdz_P( + &log->stan_.P_Ubm, + *log->Ub < log->nast_.Ubrm, + *log->Ub > log->nast_.Ubpm, + &log->dw.lUbrm, + 5,5); + + sprawdz_P( + &log->stan_.P_Uaw, + log->A > log->nast_.Uarw, + log->A < log->nast_.Uapw, + &log->dw.lUarw, + 5,5); + + sprawdz_P( + &log->stan_.P_Ubw, + *log->Ub > log->nast_.Ubrw, + *log->Ub < log->nast_.Ubpw, + &log->dw.lUbrw, + 5,5); + // + + + + //Sprawdzenie kryterium ronicy napiec + sprawdz_P( + &log->stan_.P_DU, + log->dw.DU < log->nast_.DUr, + log->dw.DU > log->nast_.DUp, + &log->dw.lDU, + 5,5); + + // + // sprawdzenie kryterium kata fazowego + log->dw.cosfi = orta_a * *log->Ub_orta + ortb_a * *log->Ub_ortb; + cosfi2 = log->dw.cosfi * log->dw.cosfi;// + log->A = (log->A * *log->Ub) ; + sprawdz_P( + &log->stan_.P_Dfi, + (cosfi2 > (log->nast_.Dfir * log->A)) && (log->dw.cosfi > 0), + (cosfi2 < (log->nast_.Dfip * log->A)) || (log->dw.cosfi < 0), + &log->dw.lDfi, + 5,5); + + // + + + //wyprowadzenie wyjscia zezwolenia na zalaczenie + if (czy_test_R()) + { + log->stan_.Z = !(log->nast_.bl && (*log->bl & log->bl_bit_mask)) && (*log->test & log->test_bit_mask); + } + else + { + log->stan_.Z = !(log->nast_.bl && (*log->bl & log->bl_bit_mask)) && // jesli nastawiona blokada i jest blokada + ( + (log->nast_.N00 && (log->stan_.P_Uam && log->stan_.P_Ubm)) || + (log->nast_.N01 && (log->stan_.P_Uam && log->stan_.P_Ubw)) || + (log->nast_.N10 && (log->stan_.P_Uaw && log->stan_.P_Ubm)) || + (log->nast_.N11 && (log->stan_.P_Df && log->stan_.P_Dfi && log->stan_.P_DU && log->stan_.P_Uaw && log->stan_.P_Ubw)) + ); + } + // + + check_and_set_mask(log->stan_.Z,log->Z,log->Z_bit_mask); + check_and_set_mask(log->stan_.P_Uam,log->P_Uam,log->P_Uam_bit_mask); + check_and_set_mask(log->stan_.P_Uaw,log->P_Uaw,log->P_Uaw_bit_mask); + check_and_set_mask(log->stan_.P_Ubm,log->P_Ubm,log->P_Ubm_bit_mask); + check_and_set_mask(log->stan_.P_Ubw,log->P_Ubw,log->P_Ubw_bit_mask); + check_and_set_mask(log->stan_.P_Dfi,log->P_Dfi,log->P_Dfi_bit_mask); + check_and_set_mask(log->stan_.P_DU,log->P_DU,log->P_DU_bit_mask); + } + else + { + *log->Z&=~log->Z_bit_mask; + *log->P_Uam&=~log->P_Uam_bit_mask; + *log->P_Uaw&=~log->P_Uaw_bit_mask; + *log->P_Ubm&=~log->P_Ubm_bit_mask; + *log->P_Ubw&=~log->P_Ubw_bit_mask; + *log->P_Dfi&=~log->P_Dfi_bit_mask; + *log->P_DU&=~log->P_DU_bit_mask; + } +} + +void sck_100hz(void *arguments, void *logic) +{ + struct sck_logic *log = (struct sck_logic *)logic; + + float Df; + u8 OK; + + if(log->nast_.On) + { + Df = fabs(*log->fa - *log->fb); + OK = (*log->fa_OK & log->fa_OK_bit_mask) && (*log->fb_OK & log->fb_OK_bit_mask); + + sprawdz_P( + &log->stan_.P_Df, + Df < log->nast_.Dfr && OK, + Df > log->nast_.Dfp || !OK, + &log->dw.lDf, + 4,4); + //wyprowadzenie aktualnych pomiarow + + // + + *log->Df = Df; + check_and_set_mask(OK,log->f_OK,log->f_OK_bit_mask); + check_and_set_mask(log->stan_.P_Df,log->P_Df,log->P_Df_bit_mask); + } + else + { + *log->f_OK&=~log->f_OK_bit_mask; + *log->P_Df&=~log->P_Df_bit_mask; + } +} + +void sck_20hz(void *arguments, void *logic) +{ + struct sck_logic *log = (struct sck_logic *)logic; + + float loc; + if(log->dw.licz_pom>9) + { + loc = sqrt(4 * log->A); + log->dw.licz_pom=0; + *log->DU=log->dw.DU; + + if (log->stan_.P_Uam || log->stan_.P_Ubm) + { + *log->Dfi = *(float *)&NaN; + } else { + *log->Dfi = acos(log->dw.cosfi / loc) * 57.2958f;// 180 / 3.14159f ; + } + } + log->dw.licz_pom++; +} diff --git a/src/sck.h b/src/sck.h new file mode 100644 index 0000000..138636c --- /dev/null +++ b/src/sck.h @@ -0,0 +1,186 @@ +/* + * sck.h + * + * Created on: 12-01-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef SCK_H_ +#define SCK_H_ + +#include "tdefs.h" + + struct Nastawy_przeliczone_sck + { + float Uarm;/// + +//#include "pawel_usun_to.h" + +#include "spz_3f.h" + +#include "helper.h" +#include "config.h" + +int spz_3f_initlog(void *arguments, void *logic) +{ + struct spz_3f_args *args = (struct spz_3f_args *)arguments; + struct spz_3f_logic *log = (struct spz_3f_logic *)logic; + + if(set_bit_ptr_mask(args->io.Pob_in,&log->Pob,&log->Pob_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.Blokada_in,&log->Blokada,&log->Blokada_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.W1_ON_in,&log->W1_ON,&log->W1_ON_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.W2_ON_in,&log->W2_ON,&log->W2_ON_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.W3_ON_in,&log->W3_ON,&log->W3_ON_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.WYL_1_in,&log->WYL_1,&log->WYL_1_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.WYL_2_in,&log->WYL_2,&log->WYL_2_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.WYL_3_in,&log->WYL_3,&log->WYL_3_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.FAZ_1_in,&log->FAZ_1,&log->FAZ_1_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.FAZ_3_in,&log->FAZ_3,&log->FAZ_3_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.kasowanie_in,&log->kasowanie,&log->kasowanie_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.gotowosc_in,&log->gotowosc,&log->gotowosc_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wydluzenie_in,&log->wydluzenie_we,&log->wydluzenie_we_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.sck_in,&log->sck_we,&log->sck_we_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.master_slave_in,&log->master_slave,&log->master_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wyl_2_in,&log->wyl_2,&log->wyl_2_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.we_zal_slave_in,&log->we_zal_slave,&log->we_zal_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.zal_in,&log->we_zal,&log->we_zal_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_pob_out,&log->wy_pob,&log->wy_pob_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.Z_out,&log->Z,&log->Z_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.ZGODA_1F_out,&log->ZGODA_1F,&log->ZGODA_1F_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_akcji_out,&log->t_akcji,&log->t_akcji_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_o_out,&log->t_o,&log->t_o_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_prz_bez_out,&log->t_prz_bez,&log->t_prz_bez_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_blokady_out,&log->t_blokady,&log->t_blokady_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba1_out,&log->proba1,&log->proba1_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba2_out,&log->proba2,&log->proba2_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba3_out,&log->proba3,&log->proba3_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba4_out,&log->proba4,&log->proba4_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba5_out,&log->proba5,&log->proba5_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba6_out,&log->proba6,&log->proba6_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba7_out,&log->proba7,&log->proba7_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba8_out,&log->proba8,&log->proba8_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.udany_out,&log->udany,&log->udany_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.nieudany_out,&log->nieudany,&log->nieudany_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.rozwojowe_out,&log->rozwojowe,&log->rozwojowe_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_blokady_out,&log->wy_blokady,&log->wy_blokady_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.error_out,&log->error,&log->error_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.brak_SCK_out,&log->brakSCK,&log->brak_SCK_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.niegotowy_out,&log->niegotowy,&log->niegotowy_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.zal_zw_out,&log->zal_zw,&log->zal_zw_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_zal_slave_out,&log->wy_zal_slave,&log->wy_zal_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_jest_slave_out,&log->wy_jest_slave,&log->wy_jest_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_jest_master_out,&log->wy_jest_master,&log->wy_jest_master_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_czek_na_master,&log->wy_czek_na_master,&log->wy_czek_na_master_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_czek_na_sck,&log->wy_czek_na_SCK,&log->wy_czek_na_SCK_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.t_prz_bez_1f_out,&log->t_prz_bez_1f,&log->t_prz_bez_1f_bit_mask)) + return -1; + + + log->nast_.SPZ_on = (args->params.bity & 0x0001) ? 1: 0; + log->nast_.ZW_rozw = ((args->params.bity & 0x0002) != 0) ? 1 : 0; + log->nast_.Blok_nieudany = ((args->params.bity & 0x0004) != 0) ? 1 : 0; + log->nast_.kontr_pocz = ((args->params.bity & 0x0008) != 0) ? 1 : 0; + log->nast_.kontr_kon = ((args->params.bity & 0x0010) != 0) ? 1 : 0; + log->nast_.zal_zw_bit = ((args->params.bity & 0x0020) != 0) ? 1 : 0; + log->nast_.master_slave_on = ((args->params.bity & 0x0040) != 0) ? 1 : 0; + + log->nast_.licz_p = (u8)args->params.licz_p; + log->nast_.t[0] = (u16)((args->params.t1) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 1 + log->nast_.t[1] = (u16)((args->params.t2) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 2 + log->nast_.t[2] = (u16)((args->params.t3) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 3 + log->nast_.t[3] = (u16)((args->params.t4) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 4 + log->nast_.t[4] = (u16)((args->params.t5) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.t[5] = (u16)((args->params.t6) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.t[6] = (u16)((args->params.t7) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.t[7] = (u16)((args->params.t8) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.to = (u16)((args->params.to) * 1000); // wyliczenie ilosci petli dla odliczenia czasu oczekiwania na odpowiedz wylacznika + log->nast_.tb = (u16)((args->params.tb) * 1000); // wyliczenie ilosci petli dla odliczenia czasu odwzbudzania SPZ + log->nast_.tz = (u16)((args->params.tz) * 1000); // wyliczenie ilosci petli dla odliczenia czasu impulsu zalaczajacego + log->nast_.tp1 = (u16)((args->params.tp1) * 1000); // czas przerwy jednofazowej + log->nast_.twy = (u16)((args->params.twy) * 1000); // czas przerwy jednofazowej + log->nast_.trozw = (u16)((args->params.troz) * 1000); // czas przerwy w zawrciu rzowojowym + log->nast_.tsck = (u16)((args->params.tsck) * 1000); // czas oczewkiwania na SCK + log->nast_.t_akcji = (u16)((args->params.takcji) * 1000); // czas akcji + log->nast_.t_pod_blokady = (u16)((args->params.t_pod_blokady) *1000); + log->nast_.t_zal_zw = (u16)((args->params.t_zal_zw) *1000); + log->nast_.t_slave = (u16)((args->params.t_slave) *1000); + log->nast_.t_czek_na_master = (u16)((args->params.t_czek_na_master) * 1000); + + + log->licznik_SPZ = 1; + log->stan_wew = CZEKANIE; + log->jednofazowy = 1; + log->wydluzenie = 0; + log->licznik_SCK = 0; + log->licznik_zal_zw = 0; + log->licznik_czek_na_master = 0; + log->licznik_master_slave = 0; + + log->W_ON1_old = 0; + log->W_ON2_old = 0; + log->W_ON3_old = 0; + log->we_zal_old = 0; + log->byl_zalacz = 0; + log->tryb_master_slave = NORMAL; + log->jest_blok_trw = 0; + + log->start_log = 0; + + return 0; +} +void ustaw_numer_SPZ(struct spz_3f_logic *log) +{ + *log->proba1&=~log->proba1_bit_mask; + *log->proba2&=~log->proba2_bit_mask; + *log->proba3&=~log->proba3_bit_mask; + *log->proba4&=~log->proba4_bit_mask; + *log->proba5&=~log->proba5_bit_mask; + *log->proba6&=~log->proba6_bit_mask; + *log->proba7&=~log->proba7_bit_mask; + *log->proba8&=~log->proba8_bit_mask; + + switch (log->licznik_SPZ) + { + case 1: + //*log->proba1|=log->proba1_bit_mask; + break; + case 2: + *log->proba2|=log->proba2_bit_mask; + break; + case 3: + *log->proba3|=log->proba3_bit_mask; + break; + case 4: + *log->proba4|=log->proba4_bit_mask; + break; + case 5: + *log->proba5|=log->proba5_bit_mask; + break; + case 6: + *log->proba6|=log->proba6_bit_mask; + break; + case 7: + *log->proba7|=log->proba7_bit_mask; + break; + case 8: + *log->proba8|=log->proba8_bit_mask; + break; + + default: + break; + } +} + +void przejdz_do_error(struct spz_3f_logic *log) +{ + *log->wy_pob&= ~log->wy_pob_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + *log->error |= log->error_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->brakSCK &= ~log->brak_SCK_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + log->stan_wew = ERROR; + log->tryb_master_slave = NORMAL; +} + +void przejdz_do_nieudany(struct spz_3f_logic *log) +{ + *log->wy_pob&= ~log->wy_pob_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->nieudany |= log->nieudany_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + *log->error &= ~log->error_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + log->licznik_odwzb = 0; + log->stan_wew = NIEUDANY; + log->tryb_master_slave = NORMAL; +} + +void wy_blokada(struct spz_3f_logic *log) +{ + *log->wy_pob&= ~log->wy_pob_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->t_blokady &= ~log->t_blokady_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + *log->error &= ~log->error_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->brakSCK &= ~log->brak_SCK_bit_mask; + *log->zal_zw &= ~log->zal_zw_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + +} + +void domyslne_wy(struct spz_3f_logic *log) +{ + wy_blokada(log); + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; +} + +void spz_3f(void *arguments, void *logic) +{ + struct spz_3f_logic *log = (struct spz_3f_logic *)logic; + + u8 we_pob = ((*log->Pob & log->Pob_bit_mask) != 0) ? 1 : 0; + u8 we_bl = ((*log->Blokada & log->Blokada_bit_mask) != 0) ? 1 : 0; + u8 we_W1 = ((*log->W1_ON & log->W1_ON_bit_mask) != 0) ? 1 : 0; + u8 we_W2 = ((*log->W2_ON & log->W2_ON_bit_mask) != 0) ? 1 : 0; + u8 we_W3 = ((*log->W3_ON & log->W3_ON_bit_mask) != 0) ? 1 : 0; + u8 we_wyl1 = ((*log->WYL_1 & log->WYL_1_bit_mask) != 0) ? 1 : 0; + u8 we_wyl2 = ((*log->WYL_2 & log->WYL_2_bit_mask) != 0) ? 1 : 0; + u8 we_wyl3 = ((*log->WYL_3 & log->WYL_3_bit_mask) != 0) ? 1 : 0; + u8 we_1faz = ((*log->FAZ_1 & log->FAZ_1_bit_mask) != 0) ? 1 : 0; + u8 we_3faz = ((*log->FAZ_3 & log->FAZ_3_bit_mask) != 0) ? 1 : 0; + u8 we_kasuj = ((*log->kasowanie & log->kasowanie_bit_mask) != 0) ? 1 : 0; + u8 we_gotowosc = ((*log->gotowosc & log->gotowosc_bit_mask) != 0) ? 1 : 0; + u8 we_wydluz = ((*log->wydluzenie_we & log->wydluzenie_we_bit_mask) != 0) ? 1 : 0; + u8 we_sck = ((*log->sck_we & log->sck_we_bit_mask) != 0) ? 1 : 0; + + u8 we_master_slave = ((*log->master_slave & log->master_slave_bit_mask) != 0) ? 1 : 0; + u8 we_wyl2u = ((*log->wyl_2 & log->wyl_2_bit_mask) != 0) ? 1 : 0; + u8 we_zal_slave = ((*log->we_zal_slave & log->we_zal_slave_bit_mask) != 0) ? 1 : 0; + u8 we_zal = ((*log->we_zal & log->we_zal_bit_mask) != 0) ? 1 : 0; + + if (log->start_log < 20) + { + log->start_log++; + domyslne_wy(log); + return; + } + + +if (log->nast_.SPZ_on) + { + + //gdy jest pobudzone wejscie wydluzenie do zmiennej wydluzenie + //wpisywana jest nastawa + if (we_wydluz) + { + log->wydluzenie = log->nast_.twy; + } + else + { + log->wydluzenie = 0; + } + + //ustawiwenie zgody na wylacz jednofazowy + if (log->jednofazowy && !we_1faz && log->stan_wew != ERROR && log->stan_wew != NIEUDANY + && log->stan_wew != BLOKADA && log->stan_wew != CZEKAJ_NA_ODPAD && we_bl==0) + { + *log->ZGODA_1F |= log->ZGODA_1F_bit_mask; + log->wyj_zgoda_jednofazowy = 1; + } + else + { + *log->ZGODA_1F &= ~log->ZGODA_1F_bit_mask; + log->wyj_zgoda_jednofazowy = 0; + } + + //ustawienie zgody na wylacz trojfazowy + if (!we_3faz) + log->wyj_zgoda_trojfazowy = 1; + else + log->wyj_zgoda_trojfazowy = 0; + + //pobudzenie blokady powoduje przejscie do stanu blokady + //gdy bylo to w trakcie przerwy beznapieciowej przy pob 1 faz. + //ustawia flage + + if (we_bl) + { + if (log->stan_wew == PRZERWA_BEZNAP_1FAZ) + { + //log->blokada_w_1faz = 1; + } + log->stan_wew = BLOKADA; + log->licznik_pod_blokady = 0; + } + + if ((log->stan_wew == BLOKADA)|| (log->stan_wew == CZEKAJ_NA_ODPAD) || (we_1faz && we_3faz) || (log->jest_blok_trw)) + *log->wy_blokady |= log->wy_blokady_bit_mask; + else + *log->wy_blokady &= ~log->wy_blokady_bit_mask; + + if ((log->stan_wew == BLOKADA)|| (log->stan_wew == CZEKAJ_NA_ODPAD) || (we_1faz && we_3faz) || + (log->stan_wew ==ERROR) || (log->stan_wew ==NIEUDANY) || + ((log->stan_wew == ZALACZ_BLOKADA) && (log->licznik_SPZ == log->nast_.licz_p)) || + ((log->stan_wew == ZALACZ_BLOKADA) && (we_3faz))) + { + *log->niegotowy |= log->niegotowy_bit_mask; + } + else + { + *log->niegotowy &= ~log->niegotowy_bit_mask; + } + + //kasowanie - powrot do stanu normalnego + if (we_kasuj) + { + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ(log); + log->jednofazowy = 1; + domyslne_wy(log); + log->jest_blok_trw = 0; + } + + //autoamtyka zal na zwarocie + + *log->zal_zw &= ~log->zal_zw_bit_mask; //zawsze zdejmuje sygnal + + + if (log->nast_.zal_zw_bit == 1) + { + if (log->W_ON1_old == 0 && we_W1 == 1) + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + if (log->W_ON2_old == 0 && we_W2 == 1) + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + if (log->W_ON3_old == 0 && we_W3 == 1) + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + if (log->we_zal_old == 0 && we_zal == 1) + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + + log->W_ON1_old = we_W1; + log->W_ON2_old = we_W2; + log->W_ON3_old = we_W3; + log->we_zal_old = we_zal; + + if (log->byl_zalacz == 1) + { + log->licznik_zal_zw += LOOP_CYCLE_MS; + if (log->licznik_zal_zw >= log->nast_.t_zal_zw) + { + log->byl_zalacz = 0; + } + } + + if (log->byl_zalacz && (we_wyl1 || we_wyl2 || we_wyl3)) + { + //wykryto zal na zwarcie + przejdz_do_nieudany(log); + + *log->zal_zw |= log->zal_zw_bit_mask; + + } + + + } + //wyjcia jest master jest slave + if (log->tryb_master_slave == MASTER) + { + *log->wy_jest_master |= log->wy_jest_master_bit_mask; + *log->wy_jest_slave &= ~log->wy_jest_slave_bit_mask; + } + else if (log->tryb_master_slave == SLAVE) + { + *log->wy_jest_master &= ~log->wy_jest_master_bit_mask; + *log->wy_jest_slave |= log->wy_jest_slave_bit_mask; + } + else + { + *log->wy_jest_master &= ~log->wy_jest_master_bit_mask; + *log->wy_jest_slave &= ~log->wy_jest_slave_bit_mask; + } + + + switch(log->stan_wew) + { + //********************************************************************* + // czekanie na pobudzenie SPZ + case CZEKANIE: + + if (log->licznik_udany > 0) + log->licznik_udany--; + + if (log->licznik_udany == 1) + { + *log->udany &= ~log->udany_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + } + + if (we_pob) + { + if (log->nast_.kontr_pocz) + { + if (!we_W1 || !we_W2 || !we_W3) + { + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + } + + + if (we_wyl2u && log->nast_.master_slave_on) + { + if (we_master_slave) + { + log->tryb_master_slave = MASTER; + } + else + { + log->tryb_master_slave = SLAVE; + } + } + else + { + log->tryb_master_slave = NORMAL; + } + + + log->stan_wew = AKCJA; + + log->byl_wyl_f1 = 0; + log->byl_wyl_f2 = 0; + log->byl_wyl_f3 = 0; + + log->byl_odpad_f1 = 0; + log->byl_odpad_f2 = 0; + log->byl_odpad_f3 = 0; + + log->licznik_akcji = 0; + + *log->proba1|=log->proba1_bit_mask; //proba 1 dopiero po pobudzeniu + *log->wy_pob |= log->wy_pob_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + *log->t_akcji |= log->t_akcji_bit_mask; + + } + break; + + //********************************************************************* + + case AKCJA: + log->licznik_akcji += LOOP_CYCLE_MS; + if (log->licznik_akcji >= log->nast_.t_akcji) + { + przejdz_do_nieudany(log); + break; + } + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->stan_wew = POBUDZENIE; + log->licznik_pob = 0; + + *log->t_o |= log->t_o_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + + } + + break; + + + + //********************************************************************* + //SPZ pobudzony - sprawdzenie czy wylacz jedno czy trojfazowy + + case POBUDZENIE: + + + log->licznik_pob += LOOP_CYCLE_MS; + if (log->licznik_pob >= log->nast_.to) + { + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if ((we_wyl1 == 0) && (log->byl_wyl_f1 == 1)) + log->byl_odpad_f1 = 1; + + if ((we_wyl2 == 0) && (log->byl_wyl_f2 == 1)) + log->byl_odpad_f2 = 1; + + if ((we_wyl3 == 0) && (log->byl_wyl_f3 == 1)) + log->byl_odpad_f3 = 1; + + + // dla wylaczenia jednofazowego sprawdz jedno wylaczenie + if (log->wyj_zgoda_jednofazowy) + { + + if ((log->byl_odpad_f1&& !log->byl_wyl_f2 && !log->byl_wyl_f3) || + (log->byl_odpad_f2 && !log->byl_wyl_f1 && !log->byl_wyl_f3) || + (log->byl_odpad_f3 && !log->byl_wyl_f1 && !log->byl_wyl_f2)) + { + log->stan_wew = PRZERWA_BEZNAP_1FAZ; + log->licznik_beznap = 0; + log->limit_beznap_1faz = log->nast_.tp1; + + // zapisz ktora faza wylaczyla + log->wylaczyla_f1 = 0; + log->wylaczyla_f2 = 0; + log->wylaczyla_f3 = 0; + + if (log->byl_wyl_f1) log->wylaczyla_f1 = 1; + if (log->byl_wyl_f2) log->wylaczyla_f2 = 1; + if (log->byl_wyl_f3) log->wylaczyla_f3 = 1; + + //byl wylacz 1 fazowy nie pozwalaj na nastepny + log->jednofazowy = 0; + + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + *log->t_prz_bez_1f |= log->t_prz_bez_1f_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + break; + } + } + else //nie ma zgody na jednofazowy a jest + { + if ((log->byl_odpad_f1&& !log->byl_wyl_f2 && !log->byl_wyl_f3) || + (log->byl_odpad_f2 && !log->byl_wyl_f1 && !log->byl_wyl_f3) || + (log->byl_odpad_f3 && !log->byl_wyl_f1 && !log->byl_wyl_f2)) + { + przejdz_do_nieudany(log); + break; + } + } + + // dla wylacza trojfazowego musza odpasc wylacza + if (log->wyj_zgoda_trojfazowy) + { + + if (log->byl_odpad_f1 && log->byl_odpad_f2 && log->byl_odpad_f3) + { + log->stan_wew = PRZERWA_BEZNAP; + + log->licznik_beznap = 0; + log->limit_beznap = log->nast_.t[log->licznik_SPZ - 1]; + //byl wylacz 3 fazowy nie pozwalaj na 1 fazowy + log->jednofazowy = 0; + + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + } + } + else + { + //nie ma zgody na trojfazowy a jest + if (log->byl_odpad_f1 && log->byl_odpad_f2 && log->byl_odpad_f3) + { + przejdz_do_nieudany(log); + break; + } + } + + if (!log->wyj_zgoda_trojfazowy && !log->wyj_zgoda_jednofazowy) + { + //przyszlo pobudzenie i jest blokada jedno i 3 fazowego + przejdz_do_nieudany(log); + } + + + break; + //********************************************************************* + // przerwa beznapieciowa - po uplywie idzie do czekanie na sck + // lub, jesli jest tryb m/s idzie do czekanie na imp od master + + case PRZERWA_BEZNAP: + + log->ile_fazowy = 3; + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + + if(we_3faz) //w czasie przerwy beznap przyszla blokada 3 fazowego + { + przejdz_do_nieudany(log); + } + + log->licznik_beznap += LOOP_CYCLE_MS; + if (log->licznik_beznap >= (log->limit_beznap + log->wydluzenie)) + { + if ((log->nast_.master_slave_on) && (log->tryb_master_slave == SLAVE)) + { + log->stan_wew = CZEKANIE_NA_MASTER; + *log->wy_czek_na_master |= log->wy_czek_na_master_bit_mask; + log->licznik_czek_na_master = 0; + } + else + { + log->stan_wew = CZEKANIE_NA_SCK; + *log->wy_czek_na_SCK |= log->wy_czek_na_SCK_bit_mask; + log->licznik_SCK = 0; + } + + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + } + break; + + //********************************************************************* + // czekanie na master - w trybie m/s gdy jest slave + // czeka na impuls od master, gdy go dostanie i jest to SPZ 1-fazowy + // nastepuje zalacz a gdy trojfazowy idzie do czekania na SCK + case CZEKANIE_NA_MASTER: + { + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + + if (we_zal_slave) + { + if (log->ile_fazowy == 1) + { + if (we_gotowosc && !we_wyl1 && !we_wyl2 && !we_wyl3) + { + log->stan_wew = ZALACZ_BLOKADA; + log->licznik_zalacz = 0; + log->licznik_odwzb = 0; + log->licznik_pob = 0; + log->licznik_master_slave = 0; + log->blokuj_slave = 0; + + *log->Z |= log->Z_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; //sygnal przeznaczony do wylaczenia + break; + } + else + { + //nie bylo gotowosci do zalaczenia + *log->brakSCK |= log->brak_SCK_bit_mask; + przejdz_do_nieudany(log); + break; + } + } + else + { + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK |= log->wy_czek_na_SCK_bit_mask; + log->licznik_SCK = 0; + log->stan_wew = CZEKANIE_NA_SCK; + break; + } + } + + log->licznik_czek_na_master += LOOP_CYCLE_MS; + if(log->licznik_czek_na_master >= log->nast_.t_czek_na_master) + { + przejdz_do_nieudany(log); + } + break; + } + + //********************************************************************* + // czeaknie na sck - w SPZ 3 fazowym czeka na sck + case CZEKANIE_NA_SCK: + { + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + + //przed zalaczeniem sprawdz gotowosc do zaleczenia i nie ma wylacza + if (we_sck && we_gotowosc && !we_wyl1 && !we_wyl2 && !we_wyl3) + { + log->stan_wew = ZALACZ_BLOKADA; + log->licznik_zalacz = 0; + log->licznik_odwzb = 0; + log->licznik_pob = 0; + log->licznik_master_slave = 0; + log->blokuj_slave = 0; + + *log->Z |= log->Z_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + break; + } + + log->licznik_SCK += LOOP_CYCLE_MS; + if (log->licznik_SCK >= log->nast_.tsck) + { + //nie bylo gotowosci do zalaczenia + *log->brakSCK |= log->brak_SCK_bit_mask; + przejdz_do_nieudany(log); + break; + } + break; + } + //********************************************************************* + //przerwa beznapieciowa jednofazowa + // jesli w czsie oczekiwania przyjdzie wylacz na innej fazie + // przejdz do trybu ZWARCIE_ROZWOJOWE + // po czasie blokady sprawdza gotowosc do zalaczenia + + case PRZERWA_BEZNAP_1FAZ: + + log->ile_fazowy = 1; + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if(we_1faz) //w czasie przerwy beznap przyszla blokada 1 fazowego + { + przejdz_do_nieudany(log); + } + + if ((log->wylaczyla_f1 && (log->byl_wyl_f2 || log->byl_wyl_f3)) + || (log->wylaczyla_f2 && (log->byl_wyl_f1 || log->byl_wyl_f3)) + || (log->wylaczyla_f3 && (log->byl_wyl_f1 || log->byl_wyl_f2))) + { + if (log->nast_.ZW_rozw) + { + //blokuj przy zwarciu nieudanym + przejdz_do_nieudany(log); + break; + } + else + { + if (log->wyj_zgoda_trojfazowy) + { + log->licznik_pob = 0; // zwarcie rozwijajace sie, licznik oczewkiania od nowa + log->licznik_beznap = 0; //restart licznika przerwy baznap. + log->stan_wew = ZWARCIE_ROZWOJOWE; + + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + *log->rozwojowe |= log->rozwojowe_bit_mask; + *log->t_o |= log->t_o_bit_mask; + } + else + { + przejdz_do_nieudany(log); + } + } + break; + } + + log->licznik_beznap += LOOP_CYCLE_MS; + if (log->licznik_beznap >= (log->limit_beznap_1faz + log->wydluzenie)) + { + if ((log->nast_.master_slave_on) && (log->tryb_master_slave == SLAVE)) + { + log->stan_wew = CZEKANIE_NA_MASTER; + *log->wy_czek_na_master |= log->wy_czek_na_master_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + log->licznik_czek_na_master = 0; + } + else + { + if (we_gotowosc && !we_wyl1 && !we_wyl2 && !we_wyl3) + { + log->stan_wew = ZALACZ_BLOKADA; + log->licznik_zalacz = 0; + log->licznik_odwzb = 0; + log->licznik_pob = 0; + log->licznik_master_slave = 0; + log->blokuj_slave = 0; + + *log->Z |= log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + } + else + { + //nie bylo gotowosci do zalaczenia + *log->brakSCK |= log->brak_SCK_bit_mask; + przejdz_do_nieudany(log); + } + } + } + + + + break; + //********************************************************************* + //w czasie oczekiwania nastapilo juz zwarie na innej fazie + //czekaj az przyjdzie zawrcie na 3 fazach + //jesli przyszlo przejdz to trybu przerwy beznapieciowej trojfazowej + //jesli nie to zwracie dwufazowe - error + + case ZWARCIE_ROZWOJOWE: + + //sprawdz czy w pozostalym czasie oczewkiania wylaczyly 3 fazy + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if ((we_wyl1 == 0) && (log->byl_wyl_f1 == 1)) + log->byl_odpad_f1 = 1; + + if ((we_wyl2 == 0) && (log->byl_wyl_f2 == 1)) + log->byl_odpad_f2 = 1; + + if ((we_wyl3 == 0) && (log->byl_wyl_f3 == 1)) + log->byl_odpad_f3 = 1; + + log->licznik_pob += LOOP_CYCLE_MS; + if (log->licznik_pob >= log->nast_.to) + { + //w czasie oczekiwania nie wylaczyly wszystkie fazy + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + + + if (((log->byl_odpad_f1) && (log->byl_odpad_f2) && (log->byl_odpad_f3))/* || W_OFF*/) + { + //w czasie oczekiwanie po wylaczeniu jednofazowym wylaczyly pozostale fazy + + log->stan_wew = PRZERWA_BEZNAP; + log->licznik_beznap = 0; + log->limit_beznap = log->nast_.trozw; + + *log->t_o &= ~log->t_o_bit_mask; + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + + log->licznik_SCK = 0; + } + break; + + //********************************************************************* + //stan generuje sygnal zalacz i czas blokady + //jesli przyjdzie wylacz - przerywa zlacz i idzie do KOLEJNE_POB + //jesli przydzie pobudzenie - idzie do KOLEJNE_POB + //po czasie oczekiwania sprawdza czy jest wszystko zalaczone + //po czasie blokady sprawdza czy udany czy nie udany + + case ZALACZ_BLOKADA: + + //gdy minal czas impulsu wylaczajacego wylacz impuls + log->licznik_zalacz += LOOP_CYCLE_MS; + if (log->licznik_zalacz >= log->nast_.tz) + { + *log->Z &= ~log->Z_bit_mask; + + } + + if (log->tryb_master_slave == MASTER) + { + if (!log->blokuj_slave) + { + log->licznik_master_slave += LOOP_CYCLE_MS; + + if (log->licznik_master_slave >= (log->nast_.t_slave + 1000)) + { + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + } + else if (log->licznik_master_slave >= log->nast_.t_slave) + { + *log->wy_zal_slave |= log->wy_zal_slave_bit_mask; + } + + + } + } + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + *log->Z &= ~log->Z_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + log->blokuj_slave = 1; + } + + if (we_pob) + { + if ( log->licznik_SPZ == log->nast_.licz_p) + { + przejdz_do_nieudany(log); + break; + } + + log->byl_wyl_f1 = 0; + log->byl_wyl_f2 = 0; + log->byl_wyl_f3 = 0; + + log->byl_odpad_f1 = 0; + log->byl_odpad_f2 = 0; + log->byl_odpad_f3 = 0; + + log->licznik_pob = 0; + log->licznik_akcji = 0; + log->stan_wew = KOLEJNA_AKCJA; + *log->t_akcji |= log->t_akcji_bit_mask; + break; + } + + // minal czas blokady rozpoznaj czy udany czy nieudany + log->licznik_odwzb += LOOP_CYCLE_MS; + if (log->licznik_odwzb >= log->nast_.tb) + { + *log->Z &= ~log->Z_bit_mask; + *log->t_blokady &= ~log->t_blokady_bit_mask; + *log->wy_pob &= ~log->wy_pob_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + log->tryb_master_slave = NORMAL; + //zdarzenie udany + if (log->nast_.kontr_kon) + { + if (we_W1 && we_W2 && we_W3) + { + *log->udany |= log->udany_bit_mask; + log->licznik_udany = 3000; // 3 sekundy podtrzumanie stanu (dla zdarzen) + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ(log); + log->jednofazowy = 1; + } + else + { + *log->nieudany |= log->nieudany_bit_mask; + log->licznik_udany = 3000; // 3 sekundy podtrzumanie stanu (dla zdarzen) + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ(log); + log->jednofazowy = 1; + } + } + else + { + *log->udany |= log->udany_bit_mask; + log->licznik_udany = 3000; // 3 sekundy podtrzumanie stanu (dla zdarzen) + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ(log); + log->jednofazowy = 1; + } + } + break; + +//********************************************************************* + // pobudzenie lub wylaczenie w czasie blokady + //sprawdza warunki po kolejnej prby zalaczenia + + case KOLEJNE_POB: + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if ((we_wyl1 == 0) && (log->byl_wyl_f1 == 1)) + log->byl_odpad_f1 = 1; + + if ((we_wyl2 == 0) && (log->byl_wyl_f2 == 1)) + log->byl_odpad_f2 = 1; + + if ((we_wyl3 == 0) && (log->byl_wyl_f3 == 1)) + log->byl_odpad_f3 = 1; + + log->licznik_pob += LOOP_CYCLE_MS; + if (log->licznik_pob >= log->nast_.to) + { + //w czasie oczekiwania nie wylaczyly wszystkie fazy lub nie przyszlo pobudzenie + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + + + if (log->byl_odpad_f1 && log->byl_odpad_f2 && log->byl_odpad_f3) + { + //w czasie oczekiwanie po wylaczeniu jednofazowym wylaczyly pozostale fazy + *log->t_akcji &= ~log->t_akcji_bit_mask; + + if (we_3faz) + { + //byl wylacz 3 fazowy i blokada 3 faz + przejdz_do_nieudany(log); + break; + } + + log->licznik_SPZ++; + + ustaw_numer_SPZ(log); + log->stan_wew = PRZERWA_BEZNAP; + + log->licznik_beznap = 0; + log->limit_beznap = log->nast_.t[log->licznik_SPZ - 1]; + //byl wylacz 3 fazowy nie pozwalaj na 1 fazowy + log->jednofazowy = 0; + + *log->t_o &= ~log->t_o_bit_mask; + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + log->licznik_SCK = 0; + + break; + + //} + } + break; +//********************************************************************* + // stan zablokowanie SPZ + + case BLOKADA: + // w blokadzie pusc zalacz + *log->Z &= ~log->Z_bit_mask; + wy_blokada(log); + + if (we_bl == 0) + { + log->licznik_pod_blokady += LOOP_CYCLE_MS; + if (log->licznik_pod_blokady >= log->nast_.t_pod_blokady) + { + + if (log->licznik_pod_blokady >= 2) + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + + log->stan_wew = CZEKAJ_NA_ODPAD; + log->tryb_master_slave = NORMAL; + + //gdy byl pobudzony i przyszla blokada + // zdarzenie nieudany + if ((*log->wy_pob & log->wy_pob_bit_mask) != 0) + { + *log->nieudany |= log->nieudany_bit_mask; + } + + } + } + break; + +//********************************************************************* + case KOLEJNA_AKCJA: + + log->licznik_akcji += LOOP_CYCLE_MS; + if (log->licznik_akcji >= log->nast_.t_akcji) + { + log->licznik_odwzb = 0; + przejdz_do_error(log); + break; + } + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if (++log->licznik_zalacz > log->nast_.tz) + { + *log->Z &= ~log->Z_bit_mask; + } + + if (log->tryb_master_slave == MASTER) + { + if (!log->blokuj_slave) + { + log->licznik_master_slave += LOOP_CYCLE_MS; + + if (log->licznik_master_slave >= (log->nast_.t_slave + 1000)) + { + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + } + else if (log->licznik_master_slave >= log->nast_.t_slave) + { + *log->wy_zal_slave |= log->wy_zal_slave_bit_mask; + } + + + } + } + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->stan_wew = KOLEJNE_POB; + *log->Z &= ~log->Z_bit_mask; + *log->t_o |= log->t_o_bit_mask; + *log->t_blokady &= ~log->t_blokady_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + log->blokuj_slave = 1; + log->licznik_pob = 0; + } + break; + + + +//********************************************************************* + // stan po opadnieciu blokady + // czeakj az minie pobudzenie zeby wystartowac od poczatku + + case CZEKAJ_NA_ODPAD: + + //wyjdz z tego stanu dopiero gdy odpadnie pobudzenie + if (we_pob == 0) + { + log->stan_wew = CZEKANIE; + log->jednofazowy = 1; + domyslne_wy(log); + log->licznik_SPZ = 1; + ustaw_numer_SPZ(log); + + } + break; + +//********************************************************************* + + // stan error + case ERROR: + + // stan nieudany + case NIEUDANY: + + //tutaj sciaga przerwe bez nap 1 f zeby byl jeden obieg na sume z blokadami + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + + *log->brakSCK &= ~log->brak_SCK_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + + if (log->nast_.Blok_nieudany) + { + log->jest_blok_trw = 1; + } + else + { + log->licznik_odwzb += LOOP_CYCLE_MS; + if (log->licznik_odwzb >= log->nast_.tb) + { + log->stan_wew = CZEKANIE; + domyslne_wy(log); + log->licznik_SPZ = 1; + ustaw_numer_SPZ(log); + log->jednofazowy = 1; + } + } + break; + +//********************************************************************* + + default: + break; + + } + } + else + { + domyslne_wy(log); + *log->wy_blokady |= log->wy_blokady_bit_mask; + *log->ZGODA_1F &= ~log->ZGODA_1F_bit_mask; + } +} + + diff --git a/src/spz_3f.h b/src/spz_3f.h new file mode 100644 index 0000000..202126a --- /dev/null +++ b/src/spz_3f.h @@ -0,0 +1,309 @@ +/* + * spz_3f.h + * + * Created on: 02-02-2017 + * Author: PS + */ + +#ifndef SPZ_3F_H_ +#define SPZ_3F_H_ + +enum _stan_wew_spz_3f +{ + CZEKANIE = 0, + AKCJA = 1, + POBUDZENIE = 2, + PRZERWA_BEZNAP = 3, + ZALACZ_BLOKADA = 4, + NIEUDANY = 5, + PRZERWA_BEZNAP_1FAZ = 6, + ZWARCIE_ROZWOJOWE = 7, + BLOKADA = 8, + CZEKAJ_NA_ODPAD = 9, + KOLEJNE_POB = 10, + KOLEJNA_AKCJA = 11, + ERROR = 12, + CZEKANIE_NA_SCK = 13, + CZEKANIE_NA_MASTER = 14 +}; + +enum _master_slave +{ + NORMAL = 0, + MASTER = 1, + SLAVE = 2 +}; + + +#include "tdefs.h" + + struct Nastawy_przeliczone_spz_3f + { + u8 SPZ_on; //automatyka SPZ aktywna + u8 ZW_rozw; ///nie pozwol na zwarcie rzowojowe + u8 Blok_nieudany; ///blokuj trwale po nieudanym + u8 kontr_pocz; //czy sprawdzac wylacznik na poczatku + u8 kontr_kon; //czy sprawdzac wyacznik na koncu + u8 zal_zw_bit; + u8 licz_p; // liczba prob (od 1 do 8) + u8 master_slave_on; //wlaczony tryb master slave + u16 t[8]; // czas przerwy beznapieciowej (przeliczony na ilosc petli) + u16 to; // czas oczekiwania na odpowiedz wylacznika (przeliczony na ilosc petli) + u16 tb; // czas blokowania (przeliczony na ilosc petli) + u16 tz; // czas impulsu zalaczajacego + u16 tp1; // czas przerwy jednofazowej + u16 twy; // czas wydluzenia przerwy beznap + u16 trozw; // czas przerwy beznap w zwarciu rozwojowym + u16 tsck; // czekanie na zgode z SCK + u16 t_akcji; // czas akcji + u16 t_pod_blokady; //czas podtrzymania blokady + u16 t_zal_zw; //czas autoamt zal na zw + u16 t_slave; + u16 t_czek_na_master; + + + }; + +struct spz_3f_logic +{ + u8 *Pob; + u8 Pob_bit_mask; + u8 *Blokada; + u8 Blokada_bit_mask; + u8 *W1_ON; + u8 W1_ON_bit_mask; + u8 *W2_ON; + u8 W2_ON_bit_mask; + u8 *W3_ON; + u8 W3_ON_bit_mask; + u8 *WYL_1; + u8 WYL_1_bit_mask; + u8 *WYL_2; + u8 WYL_2_bit_mask; + u8 *WYL_3; + u8 WYL_3_bit_mask; + u8 *FAZ_1; + u8 FAZ_1_bit_mask; + u8 *FAZ_3; + u8 FAZ_3_bit_mask; + u8 *kasowanie; + u8 kasowanie_bit_mask; + u8 *gotowosc; + u8 gotowosc_bit_mask; + u8 *wydluzenie_we; + u8 wydluzenie_we_bit_mask; + u8 *sck_we; + u8 sck_we_bit_mask; + + u8 *master_slave; + u8 master_slave_bit_mask; + u8 *wyl_2; + u8 wyl_2_bit_mask; + u8 *we_zal_slave; + u8 we_zal_slave_bit_mask; + u8 *we_zal; + u8 we_zal_bit_mask; + + u8 *wy_pob; + u8 wy_pob_bit_mask; + u8 *Z; + u8 Z_bit_mask; + u8 *ZGODA_1F; + u8 ZGODA_1F_bit_mask; + u8 *t_akcji; + u8 t_akcji_bit_mask; + u8 *t_o; + u8 t_o_bit_mask; + u8 *t_prz_bez; + u8 t_prz_bez_bit_mask; + u8 *t_blokady; + u8 t_blokady_bit_mask; + u8 *proba1; + u8 proba1_bit_mask; + u8 *proba2; + u8 proba2_bit_mask; + u8 *proba3; + u8 proba3_bit_mask; + u8 *proba4; + u8 proba4_bit_mask; + u8 *proba5; + u8 proba5_bit_mask; + u8 *proba6; + u8 proba6_bit_mask; + u8 *proba7; + u8 proba7_bit_mask; + u8 *proba8; + u8 proba8_bit_mask; + u8 *udany; + u8 udany_bit_mask; + u8 *nieudany; + u8 nieudany_bit_mask; + u8 *rozwojowe; + u8 rozwojowe_bit_mask; + u8 *wy_blokady; + u8 wy_blokady_bit_mask; + u8 *error; + u8 error_bit_mask; + u8 *brakSCK; + u8 brak_SCK_bit_mask; + u8 *niegotowy; + u8 niegotowy_bit_mask; + u8 *zal_zw; + u8 zal_zw_bit_mask; + + u8 *wy_zal_slave; + u8 wy_zal_slave_bit_mask; + u8 *wy_jest_master; + u8 wy_jest_master_bit_mask; + u8 *wy_jest_slave; + u8 wy_jest_slave_bit_mask; + + u8 *wy_czek_na_master; + u8 wy_czek_na_master_bit_mask; + + u8 *wy_czek_na_SCK; + u8 wy_czek_na_SCK_bit_mask; + + u8 *t_prz_bez_1f; + u8 t_prz_bez_1f_bit_mask; + + struct Nastawy_przeliczone_spz_3f nast_;// struktura z parametramio + + u8 byl_wyl_f1; + u8 byl_wyl_f2; + u8 byl_wyl_f3; + + u8 byl_odpad_f1; + u8 byl_odpad_f2; + u8 byl_odpad_f3; + + u8 wylaczyla_f1; + u8 wylaczyla_f2; + u8 wylaczyla_f3; + + u8 W_ON1_old; + u8 W_ON2_old; + u8 W_ON3_old; + u8 we_zal_old; + u8 byl_zalacz; + u8 blokuj_slave; + u8 ile_fazowy; + u8 jest_blok_trw; + + u8 start_log; + + enum _stan_wew_spz_3f stan_wew; + enum _master_slave tryb_master_slave; + u16 licznik_pob; + u16 licznik_beznap; + u16 limit_beznap; + u16 limit_beznap_1faz; + u16 licznik_zalacz; + u16 licznik_odwzb; + u16 licznik_udany; + u16 wydluzenie; + u16 licznik_SCK; + u16 licznik_akcji; + u16 licznik_pod_blokady; + u16 licznik_zal_zw; + u8 licznik_SPZ; + u16 licznik_czek_na_master; + + u16 licznik_master_slave; + + u8 jednofazowy; + u8 wyj_zgoda_jednofazowy; + u8 wyj_zgoda_trojfazowy; + +}; + +struct spz_3f_io +{ + u32 Pob_in; + u32 Blokada_in; + u32 W1_ON_in; + u32 W2_ON_in; + u32 W3_ON_in; + u32 WYL_1_in; + u32 WYL_2_in; + u32 WYL_3_in; + u32 FAZ_1_in; + u32 FAZ_3_in; + u32 kasowanie_in; + u32 gotowosc_in; + u32 wydluzenie_in; + u32 sck_in; + u32 master_slave_in; //nowe + u32 wyl_2_in; //nowe + u32 we_zal_slave_in; //nowe + u32 zal_in; + + u32 wy_pob_out; + u32 Z_out; + u32 ZGODA_1F_out; + u32 t_akcji_out; + u32 t_o_out; + u32 t_prz_bez_out; + u32 t_blokady_out; + u32 proba1_out; + u32 proba2_out; + u32 proba3_out; + u32 proba4_out; + u32 proba5_out; + u32 proba6_out; + u32 proba7_out; + u32 proba8_out; + u32 udany_out; + u32 nieudany_out; + u32 rozwojowe_out; + u32 wy_blokady_out; + u32 error_out; + u32 brak_SCK_out; + u32 niegotowy_out; + u32 zal_zw_out; + u32 wy_zal_slave_out; //nowe + u32 wy_jest_slave_out; //nowe + u32 wy_jest_master_out; //nowe + u32 wy_czek_na_master; + u32 wy_czek_na_sck; + u32 t_prz_bez_1f_out; + +}__attribute__((__packed__)); + +struct spz_3f_params +{ + u32 bity; // nastawy bitowe + double licz_p; // liczba prob (od 1 do 5) + double t1; // czas przerwy beznapieciowej proby 1 + double t2; // czas przerwy beznapieciowej proby 2 + double t3; // czas przerwy beznapieciowej proby 3 + double t4; // czas przerwy beznapieciowej proby 4 + double t5; // czas przerwy beznapieciowej proby 5 + double t6; // czas przerwy beznapieciowej proby 6 + double t7; // czas przerwy beznapieciowej proby 7 + double t8; // czas przerwy beznapieciowej proby 8 + double to; // czas oczekiwania na odpowiedz wylacznika + double tb; // czas blokowania + double tz; // czas impulsu zalaczajacego + double tp1; // czas przerwy jednofazowej + double twy; // czas wydluzenia przerwy beznap + double troz; // czas przerwy beznapieciowej w zwarciu rozwojowym + double tsck; // czas oczekiwania na SCK + double takcji; //czas akcji + double t_pod_blokady; + double t_zal_zw; + double t_slave; + double t_czek_na_master; +}__attribute__((__packed__)); + +struct spz_3f_args +{ + struct spz_3f_io io; + struct spz_3f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void spz_3f(void *args, void *logic); +extern int spz_3f_initlog(void *arguments, void *logic); + +#endif /* SPZ_3F_H_ */ diff --git a/src/spz_3f_2.c b/src/spz_3f_2.c new file mode 100644 index 0000000..d25f39a --- /dev/null +++ b/src/spz_3f_2.c @@ -0,0 +1,1305 @@ +/* + * spz_3f_2.c + * + * Created on: 05-01-2024 + * Author: PS + */ + +#include + +//#include "pawel_usun_to.h" +#include "spz_3f_2.h" + +#include "helper.h" +#include "config.h" + +int spz_3f_2_initlog(void *arguments, void *logic) +{ + struct spz_3f_2_args *args = (struct spz_3f_2_args *)arguments; + struct spz_3f_2_logic *log = (struct spz_3f_2_logic *)logic; + + if(set_bit_ptr_mask(args->io.Pob_in,&log->Pob,&log->Pob_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.Blokada_in,&log->Blokada,&log->Blokada_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.W1_ON_in,&log->W1_ON,&log->W1_ON_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.W2_ON_in,&log->W2_ON,&log->W2_ON_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.W3_ON_in,&log->W3_ON,&log->W3_ON_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.WYL_1_in,&log->WYL_1,&log->WYL_1_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.WYL_2_in,&log->WYL_2,&log->WYL_2_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.WYL_3_in,&log->WYL_3,&log->WYL_3_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.FAZ_1_in,&log->FAZ_1,&log->FAZ_1_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.FAZ_3_in,&log->FAZ_3,&log->FAZ_3_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.kasowanie_in,&log->kasowanie,&log->kasowanie_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.gotowosc_in,&log->gotowosc,&log->gotowosc_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wydluzenie_in,&log->wydluzenie_we,&log->wydluzenie_we_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.sck_in,&log->sck_we,&log->sck_we_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.master_slave_in,&log->master_slave,&log->master_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wyl_2_in,&log->wyl_2,&log->wyl_2_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.we_zal_slave_in,&log->we_zal_slave,&log->we_zal_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.zal_in,&log->we_zal,&log->we_zal_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.przysp_in,&log->we_przysp,&log->we_przysp_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.wy_pob_out,&log->wy_pob,&log->wy_pob_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.Z_out,&log->Z,&log->Z_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.ZGODA_1F_out,&log->ZGODA_1F,&log->ZGODA_1F_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_akcji_out,&log->t_akcji,&log->t_akcji_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_o_out,&log->t_o,&log->t_o_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_prz_bez_out,&log->t_prz_bez,&log->t_prz_bez_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_blokady_out,&log->t_blokady,&log->t_blokady_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba1_out,&log->proba1,&log->proba1_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba2_out,&log->proba2,&log->proba2_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba3_out,&log->proba3,&log->proba3_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba4_out,&log->proba4,&log->proba4_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba5_out,&log->proba5,&log->proba5_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba6_out,&log->proba6,&log->proba6_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba7_out,&log->proba7,&log->proba7_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.proba8_out,&log->proba8,&log->proba8_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.udany_out,&log->udany,&log->udany_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.nieudany_out,&log->nieudany,&log->nieudany_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.rozwojowe_out,&log->rozwojowe,&log->rozwojowe_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_blokady_out,&log->wy_blokady,&log->wy_blokady_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.error_out,&log->error,&log->error_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.brak_SCK_out,&log->brakSCK,&log->brak_SCK_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.niegotowy_out,&log->niegotowy,&log->niegotowy_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.zal_zw_out,&log->zal_zw,&log->zal_zw_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_zal_slave_out,&log->wy_zal_slave,&log->wy_zal_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_jest_slave_out,&log->wy_jest_slave,&log->wy_jest_slave_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_jest_master_out,&log->wy_jest_master,&log->wy_jest_master_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_czek_na_master,&log->wy_czek_na_master,&log->wy_czek_na_master_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.wy_czek_na_sck,&log->wy_czek_na_SCK,&log->wy_czek_na_SCK_bit_mask)) + return -1; + if(set_bit_ptr_mask(args->io.t_prz_bez_1f_out,&log->t_prz_bez_1f,&log->t_prz_bez_1f_bit_mask)) + return -1; + + + log->nast_.SPZ_on = (args->params.bity & 0x0001) ? 1: 0; + log->nast_.ZW_rozw = ((args->params.bity & 0x0002) != 0) ? 1 : 0; + log->nast_.Blok_nieudany = ((args->params.bity & 0x0004) != 0) ? 1 : 0; + log->nast_.kontr_pocz = ((args->params.bity & 0x0008) != 0) ? 1 : 0; + log->nast_.kontr_kon = ((args->params.bity & 0x0010) != 0) ? 1 : 0; + log->nast_.zal_zw_bit = ((args->params.bity & 0x0020) != 0) ? 1 : 0; + log->nast_.master_slave_on = ((args->params.bity & 0x0040) != 0) ? 1 : 0; + log->nast_.zal_zw_kryt_W = ((args->params.bity & 0x0080) != 0) ? 1 : 0; + + log->nast_.licz_p = (u8)args->params.licz_p; + log->nast_.t[0] = (u16)((args->params.t1) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 1 + log->nast_.t[1] = (u16)((args->params.t2) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 2 + log->nast_.t[2] = (u16)((args->params.t3) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 3 + log->nast_.t[3] = (u16)((args->params.t4) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 4 + log->nast_.t[4] = (u16)((args->params.t5) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.t[5] = (u16)((args->params.t6) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.t[6] = (u16)((args->params.t7) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.t[7] = (u16)((args->params.t8) * 1000); // wyliczenie ilosci petli dla odliczenia czasu prerwy beznapieciowej 5 + log->nast_.to = (u16)((args->params.to) * 1000); // wyliczenie ilosci petli dla odliczenia czasu oczekiwania na odpowiedz wylacznika + log->nast_.tb = (u16)((args->params.tb) * 1000); // wyliczenie ilosci petli dla odliczenia czasu odwzbudzania SPZ + log->nast_.tz = (u16)((args->params.tz) * 1000); // wyliczenie ilosci petli dla odliczenia czasu impulsu zalaczajacego + log->nast_.tp1 = (u16)((args->params.tp1) * 1000); // czas przerwy jednofazowej + log->nast_.twy = (u16)((args->params.twy) * 1000); // czas przerwy jednofazowej + log->nast_.trozw = (u16)((args->params.troz) * 1000); // czas przerwy w zawrciu rzowojowym + log->nast_.tsck = (u16)((args->params.tsck) * 1000); // czas oczewkiwania na SCK + log->nast_.t_akcji = (u16)((args->params.takcji) * 1000); // czas akcji + log->nast_.t_zal_zw = (u16)((args->params.t_zal_zw) *1000); + log->nast_.t_slave = (u16)((args->params.t_slave) *1000); + log->nast_.t_czek_na_master = (u16)((args->params.t_czek_na_master) * 1000); + log->nast_.t_zbierania = (u16)((args->params.t_zbierania) * 1000); + + log->licznik_SPZ = 1; + log->stan_wew = CZEKANIE; + log->jednofazowy = 1; + log->wydluzenie = 0; + log->licznik_SCK = 0; + log->licznik_zal_zw = 0; + log->licznik_czek_na_master = 0; + log->licznik_master_slave = 0; + log->licznik_zbierania = 0; + + log->W_ON1_old = 0; + log->W_ON2_old = 0; + log->W_ON3_old = 0; + log->we_zal_old = 0; + log->byl_zalacz = 0; + log->tryb_master_slave = NORMAL; + log->jest_blok_trw = 0; + + log->start_log = 0; + + return 0; +} +void ustaw_numer_SPZ_2(struct spz_3f_2_logic *log) +{ + *log->proba1&=~log->proba1_bit_mask; + *log->proba2&=~log->proba2_bit_mask; + *log->proba3&=~log->proba3_bit_mask; + *log->proba4&=~log->proba4_bit_mask; + *log->proba5&=~log->proba5_bit_mask; + *log->proba6&=~log->proba6_bit_mask; + *log->proba7&=~log->proba7_bit_mask; + *log->proba8&=~log->proba8_bit_mask; + + switch (log->licznik_SPZ) + { + case 1: + //*log->proba1|=log->proba1_bit_mask; + break; + case 2: + *log->proba2|=log->proba2_bit_mask; + break; + case 3: + *log->proba3|=log->proba3_bit_mask; + break; + case 4: + *log->proba4|=log->proba4_bit_mask; + break; + case 5: + *log->proba5|=log->proba5_bit_mask; + break; + case 6: + *log->proba6|=log->proba6_bit_mask; + break; + case 7: + *log->proba7|=log->proba7_bit_mask; + break; + case 8: + *log->proba8|=log->proba8_bit_mask; + break; + + default: + break; + } +} + +void przejdz_do_error_2(struct spz_3f_2_logic *log) +{ + *log->wy_pob&= ~log->wy_pob_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + *log->error |= log->error_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->brakSCK &= ~log->brak_SCK_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + log->stan_wew = ERROR; + log->tryb_master_slave = NORMAL; +} + +void przejdz_do_nieudany_2(struct spz_3f_2_logic *log) +{ + *log->wy_pob&= ~log->wy_pob_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->nieudany |= log->nieudany_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + *log->error &= ~log->error_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + log->licznik_odwzb = 0; + log->stan_wew = NIEUDANY; + log->tryb_master_slave = NORMAL; +} + +void wy_blokada_2(struct spz_3f_2_logic *log) +{ + *log->wy_pob&= ~log->wy_pob_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->t_blokady &= ~log->t_blokady_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + *log->error &= ~log->error_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->brakSCK &= ~log->brak_SCK_bit_mask; + *log->zal_zw &= ~log->zal_zw_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + +} + +void domyslne_wy_2(struct spz_3f_2_logic *log) +{ + wy_blokada_2(log); + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; +} + +void spz_3f_2(void *arguments, void *logic) +{ + struct spz_3f_2_logic *log = (struct spz_3f_2_logic *)logic; + + u8 we_pob = ((*log->Pob & log->Pob_bit_mask) != 0) ? 1 : 0; + u8 we_bl = ((*log->Blokada & log->Blokada_bit_mask) != 0) ? 1 : 0; + u8 we_W1 = ((*log->W1_ON & log->W1_ON_bit_mask) != 0) ? 1 : 0; + u8 we_W2 = ((*log->W2_ON & log->W2_ON_bit_mask) != 0) ? 1 : 0; + u8 we_W3 = ((*log->W3_ON & log->W3_ON_bit_mask) != 0) ? 1 : 0; + u8 we_wyl1 = ((*log->WYL_1 & log->WYL_1_bit_mask) != 0) ? 1 : 0; + u8 we_wyl2 = ((*log->WYL_2 & log->WYL_2_bit_mask) != 0) ? 1 : 0; + u8 we_wyl3 = ((*log->WYL_3 & log->WYL_3_bit_mask) != 0) ? 1 : 0; + u8 we_1faz = ((*log->FAZ_1 & log->FAZ_1_bit_mask) != 0) ? 1 : 0; + u8 we_3faz = ((*log->FAZ_3 & log->FAZ_3_bit_mask) != 0) ? 1 : 0; + u8 we_kasuj = ((*log->kasowanie & log->kasowanie_bit_mask) != 0) ? 1 : 0; + u8 we_gotowosc = ((*log->gotowosc & log->gotowosc_bit_mask) != 0) ? 1 : 0; + u8 we_wydluz = ((*log->wydluzenie_we & log->wydluzenie_we_bit_mask) != 0) ? 1 : 0; + u8 we_sck = ((*log->sck_we & log->sck_we_bit_mask) != 0) ? 1 : 0; + + u8 we_master_slave = ((*log->master_slave & log->master_slave_bit_mask) != 0) ? 1 : 0; + u8 we_wyl2u = ((*log->wyl_2 & log->wyl_2_bit_mask) != 0) ? 1 : 0; + u8 we_zal_slave = ((*log->we_zal_slave & log->we_zal_slave_bit_mask) != 0) ? 1 : 0; + u8 we_zal = ((*log->we_zal & log->we_zal_bit_mask) != 0) ? 1 : 0; + u8 we_przysp = ((*log->we_przysp & log->we_przysp_bit_mask) != 0) ? 1 : 0; + + if (log->start_log < 20) + { + log->start_log++; + domyslne_wy_2(log); + return; + } + + +if (log->nast_.SPZ_on) + { + + //gdy jest pobudzone wejscie wydluzenie do zmiennej wydluzenie + //wpisywana jest nastawa + if (we_wydluz) + { + log->wydluzenie = log->nast_.twy; + } + else + { + log->wydluzenie = 0; + } + + //ustawiwenie zgody na wylacz jednofazowy + if (log->jednofazowy && !we_1faz && log->stan_wew != ERROR && log->stan_wew != NIEUDANY + && log->stan_wew != BLOKADA && we_bl==0) + { + *log->ZGODA_1F |= log->ZGODA_1F_bit_mask; + log->wyj_zgoda_jednofazowy = 1; + } + else + { + *log->ZGODA_1F &= ~log->ZGODA_1F_bit_mask; + log->wyj_zgoda_jednofazowy = 0; + } + + //ustawienie zgody na wylacz trojfazowy + if (!we_3faz) + log->wyj_zgoda_trojfazowy = 1; + else + log->wyj_zgoda_trojfazowy = 0; + + //pobudzenie blokady powoduje przejscie do stanu blokady + //gdy bylo to w trakcie przerwy beznapieciowej przy pob 1 faz. + //ustawia flage + + if (we_bl) + { + if (log->stan_wew == PRZERWA_BEZNAP_1FAZ) + { + //log->blokada_w_1faz = 1; + } + log->stan_wew = BLOKADA; + } + + if ((log->stan_wew == BLOKADA) || (we_1faz && we_3faz) || (log->jest_blok_trw)) + *log->wy_blokady |= log->wy_blokady_bit_mask; + else + *log->wy_blokady &= ~log->wy_blokady_bit_mask; + + if ((log->stan_wew == BLOKADA) || (we_1faz && we_3faz) || + (log->stan_wew ==ERROR) || (log->stan_wew ==NIEUDANY) || + ((log->stan_wew == ZALACZ_BLOKADA) && (log->licznik_SPZ == log->nast_.licz_p)) || + ((log->stan_wew == ZALACZ_BLOKADA) && (we_3faz))) + { + *log->niegotowy |= log->niegotowy_bit_mask; + } + else + { + *log->niegotowy &= ~log->niegotowy_bit_mask; + } + + //kasowanie - powrot do stanu normalnego + if (we_kasuj) + { + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ_2(log); + log->jednofazowy = 1; + domyslne_wy_2(log); + log->jest_blok_trw = 0; + } + + //autoamtyka zal na zwarocie + + *log->zal_zw &= ~log->zal_zw_bit_mask; //zawsze zdejmuje sygnal + + + if (log->nast_.zal_zw_bit == 1) + { + if (log->nast_.zal_zw_kryt_W == 1) //kryterium od stanu wycznika + { + if (log->W_ON1_old == 0 && we_W1 == 1) + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + if (log->W_ON2_old == 0 && we_W2 == 1) + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + if (log->W_ON3_old == 0 && we_W3 == 1) + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + } + + if (log->we_zal_old == 0 && we_zal == 1) //kryterium od sygnalu zal + { + log->byl_zalacz = 1; + log->licznik_zal_zw = 0; + } + + log->W_ON1_old = we_W1; + log->W_ON2_old = we_W2; + log->W_ON3_old = we_W3; + log->we_zal_old = we_zal; + + if (log->byl_zalacz == 1) + { + log->licznik_zal_zw += LOOP_CYCLE_MS; + if (log->licznik_zal_zw >= log->nast_.t_zal_zw) + { + log->byl_zalacz = 0; + } + } + + if (log->byl_zalacz && (we_wyl1 || we_wyl2 || we_wyl3)) + { + //wykryto zal na zwarcie + przejdz_do_nieudany_2(log); + + *log->zal_zw |= log->zal_zw_bit_mask; + + } + + + } + //wyjcia jest master jest slave + if (log->tryb_master_slave == MASTER) + { + *log->wy_jest_master |= log->wy_jest_master_bit_mask; + *log->wy_jest_slave &= ~log->wy_jest_slave_bit_mask; + } + else if (log->tryb_master_slave == SLAVE) + { + *log->wy_jest_master &= ~log->wy_jest_master_bit_mask; + *log->wy_jest_slave |= log->wy_jest_slave_bit_mask; + } + else + { + *log->wy_jest_master &= ~log->wy_jest_master_bit_mask; + *log->wy_jest_slave &= ~log->wy_jest_slave_bit_mask; + } + + + switch(log->stan_wew) + { + //********************************************************************* + // czekanie na pobudzenie SPZ + case CZEKANIE: + + if (log->licznik_udany > 0) + log->licznik_udany--; + + if (log->licznik_udany == 1) + { + *log->udany &= ~log->udany_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + } + + if (we_pob) + { + if (log->nast_.kontr_pocz) + { + if (!we_W1 || !we_W2 || !we_W3) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + } + + + if (we_wyl2u && log->nast_.master_slave_on) + { + if (we_master_slave) + { + log->tryb_master_slave = MASTER; + } + else + { + log->tryb_master_slave = SLAVE; + } + } + else + { + log->tryb_master_slave = NORMAL; + } + + + log->stan_wew = AKCJA; + + log->byl_wyl_f1 = 0; + log->byl_wyl_f2 = 0; + log->byl_wyl_f3 = 0; + + log->byl_odpad_f1 = 0; + log->byl_odpad_f2 = 0; + log->byl_odpad_f3 = 0; + + log->licznik_akcji = 0; + + *log->proba1|=log->proba1_bit_mask; //proba 1 dopiero po pobudzeniu + *log->wy_pob |= log->wy_pob_bit_mask; + *log->udany &= ~log->udany_bit_mask; + *log->nieudany &= ~log->nieudany_bit_mask; + *log->t_akcji |= log->t_akcji_bit_mask; + + } + break; + + //********************************************************************* + + case AKCJA: + log->licznik_akcji += LOOP_CYCLE_MS; + if (log->licznik_akcji >= log->nast_.t_akcji) + { + przejdz_do_nieudany_2(log); + break; + } + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->stan_wew = POBUDZENIE; + log->licznik_pob = 0; + + log->licznik_zbierania = 0; + log->spr_wylaczenia = 0; + log->jednofazowy_L1 = 0; + log->jednofazowy_L2 = 0; + log->jednofazowy_L3 = 0; + + *log->t_o |= log->t_o_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + + } + + break; + + + + //********************************************************************* + //SPZ pobudzony - sprawdzenie czy wylacz jedno czy trojfazowy + + case POBUDZENIE: + + + log->licznik_pob += LOOP_CYCLE_MS; + if (log->licznik_pob >= log->nast_.to) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + +// kontrola wylaczen nowa \/\/\/ + + if (log->spr_wylaczenia == 0) + { + log->licznik_zbierania += LOOP_CYCLE_MS; + if (log->licznik_zbierania >= log->nast_.t_zbierania) + { + log->spr_wylaczenia = 1; + if ((log->byl_wyl_f1 == 1) && (log->byl_wyl_f2 == 0) && (log->byl_wyl_f3 == 0)) + log->jednofazowy_L1 = 1; + if ((log->byl_wyl_f1 == 0) && (log->byl_wyl_f2 == 1) && (log->byl_wyl_f3 == 0)) + log->jednofazowy_L2 = 1; + if ((log->byl_wyl_f1 == 0) && (log->byl_wyl_f2 == 0) && (log->byl_wyl_f3 == 1)) + log->jednofazowy_L3 = 1; + + // w momencie sprawdzenia by wycz 2 fazowy + if (((log->byl_wyl_f1 == 1) && (log->byl_wyl_f2 == 1) && (log->byl_wyl_f3 == 0)) || + ((log->byl_wyl_f1 == 0) && (log->byl_wyl_f2 == 1) && (log->byl_wyl_f3 == 1)) || + ((log->byl_wyl_f1 == 1) && (log->byl_wyl_f2 == 0) && (log->byl_wyl_f3 == 1))) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + } + } else + { + if ((log->jednofazowy_L1 == 1) && ((log->byl_wyl_f2 == 1) || (log->byl_wyl_f3 == 1)) || + (log->jednofazowy_L2 == 1) && ((log->byl_wyl_f1 == 1) || (log->byl_wyl_f3 == 1)) || + (log->jednofazowy_L3 == 1) && ((log->byl_wyl_f1 == 1) || (log->byl_wyl_f2 == 1))) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + } + +// kontrola wylaczen nowa /\/\/\ + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if ((we_wyl1 == 0) && (log->byl_wyl_f1 == 1)) + log->byl_odpad_f1 = 1; + + if ((we_wyl2 == 0) && (log->byl_wyl_f2 == 1)) + log->byl_odpad_f2 = 1; + + if ((we_wyl3 == 0) && (log->byl_wyl_f3 == 1)) + log->byl_odpad_f3 = 1; + + + // dla wylaczenia jednofazowego sprawdz jedno wylaczenie + if (log->wyj_zgoda_jednofazowy) + { + + if ((log->byl_odpad_f1&& !log->byl_wyl_f2 && !log->byl_wyl_f3) || + (log->byl_odpad_f2 && !log->byl_wyl_f1 && !log->byl_wyl_f3) || + (log->byl_odpad_f3 && !log->byl_wyl_f1 && !log->byl_wyl_f2)) + { + log->stan_wew = PRZERWA_BEZNAP_1FAZ; + log->licznik_beznap = 0; + log->limit_beznap_1faz = log->nast_.tp1; + + // zapisz ktora faza wylaczyla + log->wylaczyla_f1 = 0; + log->wylaczyla_f2 = 0; + log->wylaczyla_f3 = 0; + + if (log->byl_wyl_f1) log->wylaczyla_f1 = 1; + if (log->byl_wyl_f2) log->wylaczyla_f2 = 1; + if (log->byl_wyl_f3) log->wylaczyla_f3 = 1; + + //byl wylacz 1 fazowy nie pozwalaj na nastepny + log->jednofazowy = 0; + + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + *log->t_prz_bez_1f |= log->t_prz_bez_1f_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + break; + } + } + else //nie ma zgody na jednofazowy a jest + { + if ((log->byl_odpad_f1&& !log->byl_wyl_f2 && !log->byl_wyl_f3) || + (log->byl_odpad_f2 && !log->byl_wyl_f1 && !log->byl_wyl_f3) || + (log->byl_odpad_f3 && !log->byl_wyl_f1 && !log->byl_wyl_f2)) + { + przejdz_do_nieudany_2(log); + break; + } + } + + // dla wylacza trojfazowego musza odpasc wylacza + if (log->wyj_zgoda_trojfazowy) + { + + if (log->byl_odpad_f1 && log->byl_odpad_f2 && log->byl_odpad_f3) + { + log->stan_wew = PRZERWA_BEZNAP; + + log->licznik_beznap = 0; + log->limit_beznap = log->nast_.t[log->licznik_SPZ - 1]; + //byl wylacz 3 fazowy nie pozwalaj na 1 fazowy + log->jednofazowy = 0; + + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + *log->t_o &= ~log->t_o_bit_mask; + } + } + else + { + //nie ma zgody na trojfazowy a jest + if (log->byl_odpad_f1 && log->byl_odpad_f2 && log->byl_odpad_f3) + { + przejdz_do_nieudany_2(log); + break; + } + } + + if (!log->wyj_zgoda_trojfazowy && !log->wyj_zgoda_jednofazowy) + { + //przyszlo pobudzenie i jest blokada jedno i 3 fazowego + przejdz_do_nieudany_2(log); + } + + + break; + //********************************************************************* + // przerwa beznapieciowa - po uplywie idzie do czekanie na sck + // lub, jesli jest tryb m/s idzie do czekanie na imp od master + + case PRZERWA_BEZNAP: + + log->ile_fazowy = 3; + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + + if(we_3faz) //w czasie przerwy beznap przyszla blokada 3 fazowego + { + przejdz_do_nieudany_2(log); + } + + log->licznik_beznap += LOOP_CYCLE_MS; + if ((log->licznik_beznap >= (log->limit_beznap + log->wydluzenie)) || we_przysp) + { + if ((log->nast_.master_slave_on) && (log->tryb_master_slave == SLAVE)) + { + log->stan_wew = CZEKANIE_NA_MASTER; + *log->wy_czek_na_master |= log->wy_czek_na_master_bit_mask; + log->licznik_czek_na_master = 0; + } + else + { + log->stan_wew = CZEKANIE_NA_SCK; + *log->wy_czek_na_SCK |= log->wy_czek_na_SCK_bit_mask; + log->licznik_SCK = 0; + } + + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + } + break; + + //********************************************************************* + // czekanie na master - w trybie m/s gdy jest slave + // czeka na impuls od master, gdy go dostanie i jest to SPZ 1-fazowy + // nastepuje zalacz a gdy trojfazowy idzie do czekania na SCK + case CZEKANIE_NA_MASTER: + { + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + + if (we_zal_slave) + { + if (log->ile_fazowy == 1) + { + if (we_gotowosc && !we_wyl1 && !we_wyl2 && !we_wyl3) + { + log->stan_wew = ZALACZ_BLOKADA; + log->licznik_zalacz = 0; + log->licznik_odwzb = 0; + log->licznik_pob = 0; + log->licznik_master_slave = 0; + log->blokuj_slave = 0; + + *log->Z |= log->Z_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; //sygnal przeznaczony do wylaczenia + break; + } + else + { + //nie bylo gotowosci do zalaczenia + *log->brakSCK |= log->brak_SCK_bit_mask; + przejdz_do_nieudany_2(log); + break; + } + } + else + { + *log->wy_czek_na_master &= ~log->wy_czek_na_master_bit_mask; + *log->wy_czek_na_SCK |= log->wy_czek_na_SCK_bit_mask; + log->licznik_SCK = 0; + log->stan_wew = CZEKANIE_NA_SCK; + break; + } + } + + log->licznik_czek_na_master += LOOP_CYCLE_MS; + if(log->licznik_czek_na_master >= log->nast_.t_czek_na_master) + { + przejdz_do_nieudany_2(log); + } + break; + } + + //********************************************************************* + // czeaknie na sck - w SPZ 3 fazowym czeka na sck + case CZEKANIE_NA_SCK: + { + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + + //przed zalaczeniem sprawdz gotowosc do zaleczenia i nie ma wylacza + if (we_sck && we_gotowosc && !we_wyl1 && !we_wyl2 && !we_wyl3) + { + log->stan_wew = ZALACZ_BLOKADA; + log->licznik_zalacz = 0; + log->licznik_odwzb = 0; + log->licznik_pob = 0; + log->licznik_master_slave = 0; + log->blokuj_slave = 0; + + *log->Z |= log->Z_bit_mask; + *log->wy_czek_na_SCK &= ~log->wy_czek_na_SCK_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + break; + } + + log->licznik_SCK += LOOP_CYCLE_MS; + if (log->licznik_SCK >= log->nast_.tsck) + { + //nie bylo gotowosci do zalaczenia + *log->brakSCK |= log->brak_SCK_bit_mask; + przejdz_do_nieudany_2(log); + break; + } + break; + } + //********************************************************************* + //przerwa beznapieciowa jednofazowa + // jesli w czsie oczekiwania przyjdzie wylacz na innej fazie + // przejdz do trybu ZWARCIE_ROZWOJOWE + // po czasie blokady sprawdza gotowosc do zalaczenia + + case PRZERWA_BEZNAP_1FAZ: + + log->ile_fazowy = 1; + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if(we_1faz) //w czasie przerwy beznap przyszla blokada 1 fazowego + { + przejdz_do_nieudany_2(log); + } + + if ((log->wylaczyla_f1 && (log->byl_wyl_f2 || log->byl_wyl_f3)) + || (log->wylaczyla_f2 && (log->byl_wyl_f1 || log->byl_wyl_f3)) + || (log->wylaczyla_f3 && (log->byl_wyl_f1 || log->byl_wyl_f2))) + { + if (log->nast_.ZW_rozw) + { + //blokuj przy zwarciu nieudanym + przejdz_do_nieudany_2(log); + break; + } + else + { + if (log->wyj_zgoda_trojfazowy) + { + log->licznik_pob = 0; // zwarcie rozwijajace sie, licznik oczewkiania od nowa + log->licznik_beznap = 0; //restart licznika przerwy baznap. + log->stan_wew = ZWARCIE_ROZWOJOWE; + + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + *log->rozwojowe |= log->rozwojowe_bit_mask; + *log->t_o |= log->t_o_bit_mask; + } + else + { + przejdz_do_nieudany_2(log); + } + } + break; + } + + log->licznik_beznap += LOOP_CYCLE_MS; + if ((log->licznik_beznap >= (log->limit_beznap_1faz + log->wydluzenie)) || (we_przysp)) + { + if ((log->nast_.master_slave_on) && (log->tryb_master_slave == SLAVE)) + { + log->stan_wew = CZEKANIE_NA_MASTER; + *log->wy_czek_na_master |= log->wy_czek_na_master_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + log->licznik_czek_na_master = 0; + } + else + { + if (we_gotowosc && !we_wyl1 && !we_wyl2 && !we_wyl3) + { + log->stan_wew = ZALACZ_BLOKADA; + log->licznik_zalacz = 0; + log->licznik_odwzb = 0; + log->licznik_pob = 0; + log->licznik_master_slave = 0; + log->blokuj_slave = 0; + + *log->Z |= log->Z_bit_mask; + *log->t_prz_bez &= ~log->t_prz_bez_bit_mask; + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + } + else + { + //nie bylo gotowosci do zalaczenia + *log->brakSCK |= log->brak_SCK_bit_mask; + przejdz_do_nieudany_2(log); + } + } + } + + + + break; + //********************************************************************* + //w czasie oczekiwania nastapilo juz zwarie na innej fazie + //czekaj az przyjdzie zawrcie na 3 fazach + //jesli przyszlo przejdz to trybu przerwy beznapieciowej trojfazowej + //jesli nie to zwracie dwufazowe - error + + case ZWARCIE_ROZWOJOWE: + + //sprawdz czy w pozostalym czasie oczewkiania wylaczyly 3 fazy + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if ((we_wyl1 == 0) && (log->byl_wyl_f1 == 1)) + log->byl_odpad_f1 = 1; + + if ((we_wyl2 == 0) && (log->byl_wyl_f2 == 1)) + log->byl_odpad_f2 = 1; + + if ((we_wyl3 == 0) && (log->byl_wyl_f3 == 1)) + log->byl_odpad_f3 = 1; + + log->licznik_pob += LOOP_CYCLE_MS; + if (log->licznik_pob >= log->nast_.to) + { + //w czasie oczekiwania nie wylaczyly wszystkie fazy + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + + + if (((log->byl_odpad_f1) && (log->byl_odpad_f2) && (log->byl_odpad_f3))/* || W_OFF*/) + { + //w czasie oczekiwanie po wylaczeniu jednofazowym wylaczyly pozostale fazy + + log->stan_wew = PRZERWA_BEZNAP; + log->licznik_beznap = 0; + log->limit_beznap = log->nast_.trozw; + + *log->t_o &= ~log->t_o_bit_mask; + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + + log->licznik_SCK = 0; + } + break; + + //********************************************************************* + //stan generuje sygnal zalacz i czas blokady + //jesli przyjdzie wylacz - przerywa zlacz i idzie do KOLEJNE_POB + //jesli przydzie pobudzenie - idzie do KOLEJNE_POB + //po czasie oczekiwania sprawdza czy jest wszystko zalaczone + //po czasie blokady sprawdza czy udany czy nie udany + + case ZALACZ_BLOKADA: + + //gdy minal czas impulsu wylaczajacego wylacz impuls + log->licznik_zalacz += LOOP_CYCLE_MS; + if (log->licznik_zalacz >= log->nast_.tz) + { + *log->Z &= ~log->Z_bit_mask; + + } + + if (log->tryb_master_slave == MASTER) + { + if (!log->blokuj_slave) + { + log->licznik_master_slave += LOOP_CYCLE_MS; + + if (log->licznik_master_slave >= (log->nast_.t_slave + 1000)) + { + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + } + else if (log->licznik_master_slave >= log->nast_.t_slave) + { + *log->wy_zal_slave |= log->wy_zal_slave_bit_mask; + } + + + } + } + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + *log->Z &= ~log->Z_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + log->blokuj_slave = 1; + } + + if (we_pob) + { + if ( log->licznik_SPZ == log->nast_.licz_p) + { + przejdz_do_nieudany_2(log); + break; + } + + log->byl_wyl_f1 = 0; + log->byl_wyl_f2 = 0; + log->byl_wyl_f3 = 0; + + log->byl_odpad_f1 = 0; + log->byl_odpad_f2 = 0; + log->byl_odpad_f3 = 0; + + log->licznik_pob = 0; + log->licznik_akcji = 0; + log->stan_wew = KOLEJNA_AKCJA; + *log->t_akcji |= log->t_akcji_bit_mask; + break; + } + + // minal czas blokady rozpoznaj czy udany czy nieudany + log->licznik_odwzb += LOOP_CYCLE_MS; + if (log->licznik_odwzb >= log->nast_.tb) + { + *log->Z &= ~log->Z_bit_mask; + *log->t_blokady &= ~log->t_blokady_bit_mask; + *log->wy_pob &= ~log->wy_pob_bit_mask; + *log->rozwojowe &= ~log->rozwojowe_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + log->tryb_master_slave = NORMAL; + //zdarzenie udany + if (log->nast_.kontr_kon) + { + if (we_W1 && we_W2 && we_W3) + { + *log->udany |= log->udany_bit_mask; + log->licznik_udany = 3000; // 3 sekundy podtrzumanie stanu (dla zdarzen) + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ_2(log); + log->jednofazowy = 1; + } + else + { + *log->nieudany |= log->nieudany_bit_mask; + log->licznik_udany = 3000; // 3 sekundy podtrzumanie stanu (dla zdarzen) + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ_2(log); + log->jednofazowy = 1; + } + } + else + { + *log->udany |= log->udany_bit_mask; + log->licznik_udany = 3000; // 3 sekundy podtrzumanie stanu (dla zdarzen) + log->stan_wew = CZEKANIE; + log->licznik_SPZ = 1; + ustaw_numer_SPZ_2(log); + log->jednofazowy = 1; + } + } + break; + +//********************************************************************* + // pobudzenie lub wylaczenie w czasie blokady + //sprawdza warunki po kolejnej prby zalaczenia + + case KOLEJNE_POB: + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if ((we_wyl1 == 0) && (log->byl_wyl_f1 == 1)) + log->byl_odpad_f1 = 1; + + if ((we_wyl2 == 0) && (log->byl_wyl_f2 == 1)) + log->byl_odpad_f2 = 1; + + if ((we_wyl3 == 0) && (log->byl_wyl_f3 == 1)) + log->byl_odpad_f3 = 1; + + log->licznik_pob += LOOP_CYCLE_MS; + if (log->licznik_pob >= log->nast_.to) + { + //w czasie oczekiwania nie wylaczyly wszystkie fazy lub nie przyszlo pobudzenie + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + + + if (log->byl_odpad_f1 && log->byl_odpad_f2 && log->byl_odpad_f3) + { + //w czasie oczekiwanie po wylaczeniu jednofazowym wylaczyly pozostale fazy + *log->t_akcji &= ~log->t_akcji_bit_mask; + + if (we_3faz) + { + //byl wylacz 3 fazowy i blokada 3 faz + przejdz_do_nieudany_2(log); + break; + } + + log->licznik_SPZ++; + + ustaw_numer_SPZ_2(log); + log->stan_wew = PRZERWA_BEZNAP; + + log->licznik_beznap = 0; + log->limit_beznap = log->nast_.t[log->licznik_SPZ - 1]; + //byl wylacz 3 fazowy nie pozwalaj na 1 fazowy + log->jednofazowy = 0; + + *log->t_o &= ~log->t_o_bit_mask; + *log->t_prz_bez |= log->t_prz_bez_bit_mask; + log->licznik_SCK = 0; + + break; + + //} + } + break; +//********************************************************************* + // stan zablokowanie SPZ + + case BLOKADA: + // w blokadzie pusc zalacz + *log->Z &= ~log->Z_bit_mask; + wy_blokada_2(log); + + if (we_bl == 0) + { + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + + //gdy byl pobudzony i przyszla blokada + // zdarzenie nieudany + if ((*log->wy_pob & log->wy_pob_bit_mask) != 0) + { + *log->nieudany |= log->nieudany_bit_mask; + } + + log->stan_wew = CZEKANIE; + log->tryb_master_slave = NORMAL; + domyslne_wy_2(log); + log->licznik_SPZ = 1; + ustaw_numer_SPZ_2(log); + } + break; + +//********************************************************************* + case KOLEJNA_AKCJA: + + log->licznik_akcji += LOOP_CYCLE_MS; + if (log->licznik_akcji >= log->nast_.t_akcji) + { + log->licznik_odwzb = 0; + przejdz_do_error_2(log); + break; + } + + if (we_wyl1) + log->byl_wyl_f1 = 1; + + if (we_wyl2) + log->byl_wyl_f2 = 1; + + if (we_wyl3) + log->byl_wyl_f3 = 1; + + if (++log->licznik_zalacz > log->nast_.tz) + { + *log->Z &= ~log->Z_bit_mask; + } + + if (log->tryb_master_slave == MASTER) + { + if (!log->blokuj_slave) + { + log->licznik_master_slave += LOOP_CYCLE_MS; + + if (log->licznik_master_slave >= (log->nast_.t_slave + 1000)) + { + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + } + else if (log->licznik_master_slave >= log->nast_.t_slave) + { + *log->wy_zal_slave |= log->wy_zal_slave_bit_mask; + } + + + } + } + + if (we_wyl1 || we_wyl2 || we_wyl3) + { + log->stan_wew = KOLEJNE_POB; + *log->Z &= ~log->Z_bit_mask; + *log->t_o |= log->t_o_bit_mask; + *log->t_blokady &= ~log->t_blokady_bit_mask; + *log->t_akcji &= ~log->t_akcji_bit_mask; + *log->wy_zal_slave &= ~log->wy_zal_slave_bit_mask; + log->blokuj_slave = 1; + log->licznik_pob = 0; + } + break; + +//********************************************************************* + + // stan error + case ERROR: + + // stan nieudany + case NIEUDANY: + + //tutaj sciaga przerwe bez nap 1 f zeby byl jeden obieg na sume z blokadami + *log->t_prz_bez_1f &= ~log->t_prz_bez_1f_bit_mask; + + *log->brakSCK &= ~log->brak_SCK_bit_mask; + *log->Z &= ~log->Z_bit_mask; + *log->t_blokady |= log->t_blokady_bit_mask; + + if (log->nast_.Blok_nieudany) + { + log->jest_blok_trw = 1; + } + else + { + log->licznik_odwzb += LOOP_CYCLE_MS; + if (log->licznik_odwzb >= log->nast_.tb) + { + log->stan_wew = CZEKANIE; + domyslne_wy_2(log); + log->licznik_SPZ = 1; + ustaw_numer_SPZ_2(log); + log->jednofazowy = 1; + } + } + break; + +//********************************************************************* + + default: + break; + + } + } + else + { + domyslne_wy_2(log); + *log->wy_blokady |= log->wy_blokady_bit_mask; + *log->ZGODA_1F &= ~log->ZGODA_1F_bit_mask; + } +} + + diff --git a/src/spz_3f_2.h b/src/spz_3f_2.h new file mode 100644 index 0000000..c42de67 --- /dev/null +++ b/src/spz_3f_2.h @@ -0,0 +1,290 @@ +/* + * spz_3f.h + * + * Created on: 02-02-2017 + * Author: PS + */ + +#ifndef SPZ_3F_2_H_ +#define SPZ_3F_2_H_ +#include "spz_3f.h" + +#include "tdefs.h" + + struct Nastawy_przeliczone_spz_3f_2 + { + u8 SPZ_on; //automatyka SPZ aktywna + u8 ZW_rozw; ///nie pozwol na zwarcie rzowojowe + u8 Blok_nieudany; ///blokuj trwale po nieudanym + u8 kontr_pocz; //czy sprawdzac wylacznik na poczatku + u8 kontr_kon; //czy sprawdzac wyacznik na koncu + u8 zal_zw_bit; + u8 zal_zw_kryt_W; //zaslaczenie na zwarcie - kryterium wy³¹cznikowe + u8 licz_p; // liczba prob (od 1 do 8) + u8 master_slave_on; //wlaczony tryb master slave + u16 t[8]; // czas przerwy beznapieciowej (przeliczony na ilosc petli) + u16 to; // czas oczekiwania na odpowiedz wylacznika (przeliczony na ilosc petli) + u16 tb; // czas blokowania (przeliczony na ilosc petli) + u16 tz; // czas impulsu zalaczajacego + u16 tp1; // czas przerwy jednofazowej + u16 twy; // czas wydluzenia przerwy beznap + u16 trozw; // czas przerwy beznap w zwarciu rozwojowym + u16 tsck; // czekanie na zgode z SCK + u16 t_akcji; // czas akcji + u16 t_zal_zw; //czas autoamt zal na zw + u16 t_slave; + u16 t_czek_na_master; + u16 t_zbierania; + + + }; + +struct spz_3f_2_logic +{ + u8 *Pob; + u8 Pob_bit_mask; + u8 *Blokada; + u8 Blokada_bit_mask; + u8 *W1_ON; + u8 W1_ON_bit_mask; + u8 *W2_ON; + u8 W2_ON_bit_mask; + u8 *W3_ON; + u8 W3_ON_bit_mask; + u8 *WYL_1; + u8 WYL_1_bit_mask; + u8 *WYL_2; + u8 WYL_2_bit_mask; + u8 *WYL_3; + u8 WYL_3_bit_mask; + u8 *FAZ_1; + u8 FAZ_1_bit_mask; + u8 *FAZ_3; + u8 FAZ_3_bit_mask; + u8 *kasowanie; + u8 kasowanie_bit_mask; + u8 *gotowosc; + u8 gotowosc_bit_mask; + u8 *wydluzenie_we; + u8 wydluzenie_we_bit_mask; + u8 *sck_we; + u8 sck_we_bit_mask; + + u8 *master_slave; + u8 master_slave_bit_mask; + u8 *wyl_2; + u8 wyl_2_bit_mask; + u8 *we_zal_slave; + u8 we_zal_slave_bit_mask; + u8 *we_zal; + u8 we_zal_bit_mask; + u8 *we_przysp; + u8 we_przysp_bit_mask; + + u8 *wy_pob; + u8 wy_pob_bit_mask; + u8 *Z; + u8 Z_bit_mask; + u8 *ZGODA_1F; + u8 ZGODA_1F_bit_mask; + u8 *t_akcji; + u8 t_akcji_bit_mask; + u8 *t_o; + u8 t_o_bit_mask; + u8 *t_prz_bez; + u8 t_prz_bez_bit_mask; + u8 *t_blokady; + u8 t_blokady_bit_mask; + u8 *proba1; + u8 proba1_bit_mask; + u8 *proba2; + u8 proba2_bit_mask; + u8 *proba3; + u8 proba3_bit_mask; + u8 *proba4; + u8 proba4_bit_mask; + u8 *proba5; + u8 proba5_bit_mask; + u8 *proba6; + u8 proba6_bit_mask; + u8 *proba7; + u8 proba7_bit_mask; + u8 *proba8; + u8 proba8_bit_mask; + u8 *udany; + u8 udany_bit_mask; + u8 *nieudany; + u8 nieudany_bit_mask; + u8 *rozwojowe; + u8 rozwojowe_bit_mask; + u8 *wy_blokady; + u8 wy_blokady_bit_mask; + u8 *error; + u8 error_bit_mask; + u8 *brakSCK; + u8 brak_SCK_bit_mask; + u8 *niegotowy; + u8 niegotowy_bit_mask; + u8 *zal_zw; + u8 zal_zw_bit_mask; + + u8 *wy_zal_slave; + u8 wy_zal_slave_bit_mask; + u8 *wy_jest_master; + u8 wy_jest_master_bit_mask; + u8 *wy_jest_slave; + u8 wy_jest_slave_bit_mask; + + u8 *wy_czek_na_master; + u8 wy_czek_na_master_bit_mask; + + u8 *wy_czek_na_SCK; + u8 wy_czek_na_SCK_bit_mask; + + u8 *t_prz_bez_1f; + u8 t_prz_bez_1f_bit_mask; + + struct Nastawy_przeliczone_spz_3f_2 nast_;// struktura z parametramio + + u8 byl_wyl_f1; + u8 byl_wyl_f2; + u8 byl_wyl_f3; + + u8 byl_odpad_f1; + u8 byl_odpad_f2; + u8 byl_odpad_f3; + + u8 wylaczyla_f1; + u8 wylaczyla_f2; + u8 wylaczyla_f3; + + u8 W_ON1_old; + u8 W_ON2_old; + u8 W_ON3_old; + u8 we_zal_old; + u8 byl_zalacz; + u8 blokuj_slave; + u8 ile_fazowy; + u8 jest_blok_trw; + + u8 start_log; + u8 spr_wylaczenia; + u8 jednofazowy_L1; + u8 jednofazowy_L2; + u8 jednofazowy_L3; + + enum _stan_wew_spz_3f stan_wew; + enum _master_slave tryb_master_slave; + u16 licznik_pob; + u16 licznik_beznap; + u16 limit_beznap; + u16 limit_beznap_1faz; + u16 licznik_zalacz; + u16 licznik_odwzb; + u16 licznik_udany; + u16 wydluzenie; + u16 licznik_SCK; + u16 licznik_akcji; + u16 licznik_zal_zw; + u8 licznik_SPZ; + u16 licznik_czek_na_master; + + u16 licznik_master_slave; + u16 licznik_zbierania; + u8 jednofazowy; + u8 wyj_zgoda_jednofazowy; + u8 wyj_zgoda_trojfazowy; + +}; + +struct spz_3f_2_io +{ + u32 Pob_in; + u32 Blokada_in; + u32 W1_ON_in; + u32 W2_ON_in; + u32 W3_ON_in; + u32 WYL_1_in; + u32 WYL_2_in; + u32 WYL_3_in; + u32 FAZ_1_in; + u32 FAZ_3_in; + u32 kasowanie_in; + u32 gotowosc_in; + u32 wydluzenie_in; + u32 sck_in; + u32 master_slave_in; + u32 wyl_2_in; + u32 we_zal_slave_in; + u32 zal_in; + u32 przysp_in; + + u32 wy_pob_out; + u32 Z_out; + u32 ZGODA_1F_out; + u32 t_akcji_out; + u32 t_o_out; + u32 t_prz_bez_out; + u32 t_blokady_out; + u32 proba1_out; + u32 proba2_out; + u32 proba3_out; + u32 proba4_out; + u32 proba5_out; + u32 proba6_out; + u32 proba7_out; + u32 proba8_out; + u32 udany_out; + u32 nieudany_out; + u32 rozwojowe_out; + u32 wy_blokady_out; + u32 error_out; + u32 brak_SCK_out; + u32 niegotowy_out; + u32 zal_zw_out; + u32 wy_zal_slave_out; //nowe + u32 wy_jest_slave_out; //nowe + u32 wy_jest_master_out; //nowe + u32 wy_czek_na_master; + u32 wy_czek_na_sck; + u32 t_prz_bez_1f_out; + +}__attribute__((__packed__)); + +struct spz_3f_2_params +{ + u32 bity; // nastawy bitowe + double licz_p; // liczba prob (od 1 do 5) + double t1; // czas przerwy beznapieciowej proby 1 + double t2; // czas przerwy beznapieciowej proby 2 + double t3; // czas przerwy beznapieciowej proby 3 + double t4; // czas przerwy beznapieciowej proby 4 + double t5; // czas przerwy beznapieciowej proby 5 + double t6; // czas przerwy beznapieciowej proby 6 + double t7; // czas przerwy beznapieciowej proby 7 + double t8; // czas przerwy beznapieciowej proby 8 + double to; // czas oczekiwania na odpowiedz wylacznika + double tb; // czas blokowania + double tz; // czas impulsu zalaczajacego + double tp1; // czas przerwy jednofazowej + double twy; // czas wydluzenia przerwy beznap + double troz; // czas przerwy beznapieciowej w zwarciu rozwojowym + double tsck; // czas oczekiwania na SCK + double takcji; //czas akcji + double t_zal_zw; + double t_slave; + double t_czek_na_master; + double t_zbierania; +}__attribute__((__packed__)); + +struct spz_3f_2_args +{ + struct spz_3f_2_io io; + struct spz_3f_2_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void spz_3f_2(void *args, void *logic); +extern int spz_3f_2_initlog(void *arguments, void *logic); + +#endif /* SPZ_3F_2_H_ */ diff --git a/src/sqrt.c b/src/sqrt.c new file mode 100644 index 0000000..82fd3fd --- /dev/null +++ b/src/sqrt.c @@ -0,0 +1,35 @@ +/* + * sqrt.c + * + * Created on: 16-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "sqrt.h" + +int sqrt_initlog(void *arguments, void *logic) +{ + struct sqrt_args *args = (struct sqrt_args *)arguments; + struct sqrt_logic *log = (struct sqrt_logic *)logic; + + if(set_float_ptr(args->io.in_float_in,&log->fin)) + return -1; + + if(set_float_ptr(args->io.out_float_out,&log->fout)) + return -1; + + return 0; +} + +void sqrt_(void *arguments, void *logic) +{ + struct sqrt_logic *log = (struct sqrt_logic *)logic; + + *log->fout=sqrtf(*log->fin); +} diff --git a/src/sqrt.h b/src/sqrt.h new file mode 100644 index 0000000..6f449c1 --- /dev/null +++ b/src/sqrt.h @@ -0,0 +1,39 @@ +/* + * sqrt.h + * + * Created on: 16-02-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef SQRT_H_ +#define SQRT_H_ + +#include "tdefs.h" + +struct sqrt_logic +{ + float *fin; + float *fout; +}; + +struct sqrt_io +{ + u32 in_float_in; + u32 out_float_out; +}__attribute__((__packed__)); + +struct sqrt_params +{ +}__attribute__((__packed__)); + +struct sqrt_args +{ + struct sqrt_io io; + struct sqrt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void sqrt_(void *args, void *logic); +extern int sqrt_initlog(void *arguments, void *logic); + +#endif /* SQRT_H_ */ diff --git a/src/sr.c b/src/sr.c new file mode 100644 index 0000000..bc11365 --- /dev/null +++ b/src/sr.c @@ -0,0 +1,49 @@ +/* + * sr.c + * + * Created on: 12-05-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "sr.h" + +int sr_initlog(void *arguments, void *logic) +{ + struct sr_args *args = (struct sr_args *)arguments; + struct sr_logic *log = (struct sr_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + log->s_ptr = log_manager.nets_data + (args->io.s_in >> 3); + log->s_bit_no = args->io.s_in & 0x07; + + log->r_ptr = log_manager.nets_data + (args->io.r_in >> 3); + log->r_bit_no = args->io.r_in & 0x07; + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->s_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->r_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void sr(void *arguments, void *logic) +{ + //struct sr_args *args = (struct sr_args *)arguments; + struct sr_logic *log = (struct sr_logic *)logic; + + if((*log->s_ptr & (1<s_bit_no))) + *log->out_ptr|=(1<out_bit_no); + else if((*log->r_ptr & (1<r_bit_no))) + *log->out_ptr&=~(1<out_bit_no); +} diff --git a/src/sr.h b/src/sr.h new file mode 100644 index 0000000..877fdf7 --- /dev/null +++ b/src/sr.h @@ -0,0 +1,47 @@ +/* + * sr.h + * + * + * Created on: 12-05-2014 + * Author: KJ + */ + +#ifndef SR_H_ +#define SR_H_ + +#include "tdefs.h" + +struct sr_logic +{ + u8 *out_ptr; + u8 out_bit_no; + + u8 *s_ptr; + u8 s_bit_no; + + u8 *r_ptr; + u8 r_bit_no; +}; + +struct sr_io +{ + u32 s_in; + u32 r_in; + u32 out; +}__attribute__((__packed__)); + +struct sr_params +{ +}__attribute__((__packed__)); + +struct sr_args +{ + struct sr_io io; + struct sr_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void sr(void *args, void *logic); +extern int sr_initlog(void *args, void *logic); + +#endif /* SR_H_ */ diff --git a/src/sum_e_3f.c b/src/sum_e_3f.c new file mode 100644 index 0000000..6395066 --- /dev/null +++ b/src/sum_e_3f.c @@ -0,0 +1,131 @@ +/* + * sum_e_3f.c + * + * Created on: 30-07-2019 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "sum_e_3f.h" + +int sum_e_3f_initlog(void *arguments, void *logic) +{ + struct sum_e_3f_args *args = (struct sum_e_3f_args *)arguments; + struct sum_e_3f_logic *log = (struct sum_e_3f_logic *)logic; + + if(set_float_ptr(args->io.in_l1_orta_a_float_in,&log->in_l1_orta_a)) + return -1; + if(set_float_ptr(args->io.in_l1_ortb_a_float_in,&log->in_l1_ortb_a)) + return -1; + if(set_float_ptr(args->io.in_l1_orta_b_float_in,&log->in_l1_orta_b)) + return -1; + if(set_float_ptr(args->io.in_l1_ortb_b_float_in,&log->in_l1_ortb_b)) + return -1; + + if(set_float_ptr(args->io.in_l2_orta_a_float_in,&log->in_l2_orta_a)) + return -1; + if(set_float_ptr(args->io.in_l2_ortb_a_float_in,&log->in_l2_ortb_a)) + return -1; + if(set_float_ptr(args->io.in_l2_orta_b_float_in,&log->in_l2_orta_b)) + return -1; + if(set_float_ptr(args->io.in_l2_ortb_b_float_in,&log->in_l2_ortb_b)) + return -1; + + if(set_float_ptr(args->io.in_l3_orta_a_float_in,&log->in_l3_orta_a)) + return -1; + if(set_float_ptr(args->io.in_l3_ortb_a_float_in,&log->in_l3_ortb_a)) + return -1; + if(set_float_ptr(args->io.in_l3_orta_b_float_in,&log->in_l3_orta_b)) + return -1; + if(set_float_ptr(args->io.in_l3_ortb_b_float_in,&log->in_l3_ortb_b)) + return -1; + + if(set_float_ptr(args->io.in_3i0_orta_a_float_in,&log->in_3i0_orta_a)) + return -1; + if(set_float_ptr(args->io.in_3i0_ortb_a_float_in,&log->in_3i0_ortb_a)) + return -1; + if(set_float_ptr(args->io.in_3i0_orta_b_float_in,&log->in_3i0_orta_b)) + return -1; + if(set_float_ptr(args->io.in_3i0_ortb_b_float_in,&log->in_3i0_ortb_b)) + return -1; + + if(set_float_ptr(args->io.out_l1_orta_float_out,&log->out_l1_orta)) + return -1; + if(set_float_ptr(args->io.out_l1_ortb_float_out,&log->out_l1_ortb)) + return -1; + if(set_float_ptr(args->io.out_l1_esk_float_out,&log->out_l1_esk)) + return -1; + + if(set_float_ptr(args->io.out_l2_orta_float_out,&log->out_l2_orta)) + return -1; + if(set_float_ptr(args->io.out_l2_ortb_float_out,&log->out_l2_ortb)) + return -1; + if(set_float_ptr(args->io.out_l2_esk_float_out,&log->out_l2_esk)) + return -1; + + if(set_float_ptr(args->io.out_l3_orta_float_out,&log->out_l3_orta)) + return -1; + if(set_float_ptr(args->io.out_l3_ortb_float_out,&log->out_l3_ortb)) + return -1; + if(set_float_ptr(args->io.out_l3_esk_float_out,&log->out_l3_esk)) + return -1; + + if(set_float_ptr(args->io.out_3i0_orta_float_out,&log->out_3i0_orta)) + return -1; + if(set_float_ptr(args->io.out_3i0_ortb_float_out,&log->out_3i0_ortb)) + return -1; + if(set_float_ptr(args->io.out_3i0_esk_float_out,&log->out_3i0_esk)) + return -1; + + return 0; +} + +void sum_e_3f(void *arguments, void *logic) +{ + struct sum_e_3f_logic *log = (struct sum_e_3f_logic *)logic; + struct sum_e_3f_args *args = (struct sum_e_3f_args *)arguments; + float m1 = args->params.m1; + float m2 = args->params.m2; + + if(m1!=1.0f || m2!=1.0f) + { + *log->out_l1_orta=(*log->in_l1_orta_a * m1) + (*log->in_l1_orta_b * m2); + *log->out_l1_ortb=(*log->in_l1_ortb_a * m1) + (*log->in_l1_ortb_b * m2); + *log->out_l1_esk=((*log->out_l1_orta * *log->out_l1_orta) + (*log->out_l1_ortb * *log->out_l1_ortb))/2; + + *log->out_l2_orta=(*log->in_l2_orta_a * m1) + (*log->in_l2_orta_b * m2); + *log->out_l2_ortb=(*log->in_l2_ortb_a * m1) + (*log->in_l2_ortb_b * m2); + *log->out_l2_esk=((*log->out_l2_orta * *log->out_l2_orta) + (*log->out_l2_ortb * *log->out_l2_ortb))/2; + + *log->out_l3_orta=(*log->in_l3_orta_a * m1) + (*log->in_l3_orta_b * m2); + *log->out_l3_ortb=(*log->in_l3_ortb_a * m1) + (*log->in_l3_ortb_b * m2); + *log->out_l3_esk=((*log->out_l3_orta * *log->out_l3_orta) + (*log->out_l3_ortb * *log->out_l3_ortb))/2; + + *log->out_3i0_orta=(*log->in_3i0_orta_a * m1) + (*log->in_3i0_orta_b * m2); + *log->out_3i0_ortb=(*log->in_3i0_ortb_a * m1) + (*log->in_3i0_ortb_b * m2); + *log->out_3i0_esk=((*log->out_3i0_orta * *log->out_3i0_orta) + (*log->out_3i0_ortb * *log->out_3i0_ortb))/2; + } + else + { + *log->out_l1_orta=(*log->in_l1_orta_a) + (*log->in_l1_orta_b); + *log->out_l1_ortb=(*log->in_l1_ortb_a) + (*log->in_l1_ortb_b); + *log->out_l1_esk=((*log->out_l1_orta * *log->out_l1_orta) + (*log->out_l1_ortb * *log->out_l1_ortb))/2; + + *log->out_l2_orta=(*log->in_l2_orta_a) + (*log->in_l2_orta_b); + *log->out_l2_ortb=(*log->in_l2_ortb_a) + (*log->in_l2_ortb_b); + *log->out_l2_esk=((*log->out_l2_orta * *log->out_l2_orta) + (*log->out_l2_ortb * *log->out_l2_ortb))/2; + + *log->out_l3_orta=(*log->in_l3_orta_a) + (*log->in_l3_orta_b); + *log->out_l3_ortb=(*log->in_l3_ortb_a) + (*log->in_l3_ortb_b); + *log->out_l3_esk=((*log->out_l3_orta * *log->out_l3_orta) + (*log->out_l3_ortb * *log->out_l3_ortb))/2; + + *log->out_3i0_orta=(*log->in_3i0_orta_a) + (*log->in_3i0_orta_b); + *log->out_3i0_ortb=(*log->in_3i0_ortb_a) + (*log->in_3i0_ortb_b); + *log->out_3i0_esk=((*log->out_3i0_orta * *log->out_3i0_orta) + (*log->out_3i0_ortb * *log->out_3i0_ortb))/2; + } +} diff --git a/src/sum_e_3f.h b/src/sum_e_3f.h new file mode 100644 index 0000000..d74e921 --- /dev/null +++ b/src/sum_e_3f.h @@ -0,0 +1,100 @@ +/* + * sum_e_3f.h + * + * Created on: 30-07-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef SUM_E_3F_H_ +#define SUM_E_3F_H_ + +#include "tdefs.h" + +struct sum_e_3f_logic +{ + float *in_l1_orta_a; + float *in_l1_ortb_a; + float *in_l1_orta_b; + float *in_l1_ortb_b; + + float *in_l2_orta_a; + float *in_l2_ortb_a; + float *in_l2_orta_b; + float *in_l2_ortb_b; + + float *in_l3_orta_a; + float *in_l3_ortb_a; + float *in_l3_orta_b; + float *in_l3_ortb_b; + + float *in_3i0_orta_a; + float *in_3i0_ortb_a; + float *in_3i0_orta_b; + float *in_3i0_ortb_b; + + float *out_l1_orta; + float *out_l1_ortb; + float *out_l1_esk; + + float *out_l2_orta; + float *out_l2_ortb; + float *out_l2_esk; + + float *out_l3_orta; + float *out_l3_ortb; + float *out_l3_esk; + + float *out_3i0_orta; + float *out_3i0_ortb; + float *out_3i0_esk; +}; + +struct sum_e_3f_io +{ + u32 in_l1_orta_a_float_in; + u32 in_l1_ortb_a_float_in; + u32 in_l1_orta_b_float_in; + u32 in_l1_ortb_b_float_in; + u32 in_l2_orta_a_float_in; + u32 in_l2_ortb_a_float_in; + u32 in_l2_orta_b_float_in; + u32 in_l2_ortb_b_float_in; + u32 in_l3_orta_a_float_in; + u32 in_l3_ortb_a_float_in; + u32 in_l3_orta_b_float_in; + u32 in_l3_ortb_b_float_in; + u32 in_3i0_orta_a_float_in; + u32 in_3i0_ortb_a_float_in; + u32 in_3i0_orta_b_float_in; + u32 in_3i0_ortb_b_float_in; + u32 out_l1_orta_float_out; + u32 out_l1_ortb_float_out; + u32 out_l1_esk_float_out; + u32 out_l2_orta_float_out; + u32 out_l2_ortb_float_out; + u32 out_l2_esk_float_out; + u32 out_l3_orta_float_out; + u32 out_l3_ortb_float_out; + u32 out_l3_esk_float_out; + u32 out_3i0_orta_float_out; + u32 out_3i0_ortb_float_out; + u32 out_3i0_esk_float_out; +}__attribute__((__packed__)); + +struct sum_e_3f_params +{ + double m1; + double m2; +}__attribute__((__packed__)); + +struct sum_e_3f_args +{ + struct sum_e_3f_io io; + struct sum_e_3f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void sum_e_3f(void *args, void *logic); +extern int sum_e_3f_initlog(void *arguments, void *logic); + +#endif /* SUM_E_3F_H_ */ diff --git a/src/sum_float.c b/src/sum_float.c new file mode 100644 index 0000000..f9d82c3 --- /dev/null +++ b/src/sum_float.c @@ -0,0 +1,42 @@ +/* + * sum_float.c + * + * Created on: 01-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "sum_float.h" + +int sum_float_initlog(void *arguments, void *logic) +{ + struct sum_float_args *args = (struct sum_float_args *)arguments; + struct sum_float_logic *log = (struct sum_float_logic *)logic; + + if(set_float_ptr(args->io.in1_float_in,&log->in1)) + return -1; + + if(set_float_ptr(args->io.in2_float_in,&log->in2)) + return -1; + + if(set_float_ptr(args->io.in3_float_in,&log->in3)) + return -1; + + if(set_float_ptr(args->io.out_float_out,&log->out)) + return -1; + + return 0; +} + +void sum_float(void *arguments, void *logic) +{ + struct sum_float_logic *log = (struct sum_float_logic *)logic; +// struct mux_float_args *args = (struct mux_float_args *)arguments; + + *log->out=*log->in1+*log->in2+*log->in3; +} diff --git a/src/sum_float.h b/src/sum_float.h new file mode 100644 index 0000000..d61e9ea --- /dev/null +++ b/src/sum_float.h @@ -0,0 +1,43 @@ +/* + * sum_float.h + * + * Created on: 01-03-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef SUM_FLOAT_H_ +#define SUM_FLOAT_H_ + +#include "tdefs.h" + +struct sum_float_logic +{ + float *in1; + float *in2; + float *in3; + float *out; +}; + +struct sum_float_io +{ + u32 in1_float_in; + u32 in2_float_in; + u32 in3_float_in; + u32 out_float_out; +}__attribute__((__packed__)); + +struct sum_float_params +{ +}__attribute__((__packed__)); + +struct sum_float_args +{ + struct sum_float_io io; + struct sum_float_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void sum_float(void *args, void *logic); +extern int sum_float_initlog(void *arguments, void *logic); + +#endif /* SUM_FLOAT_H_ */ diff --git a/src/sum_flt_3f.c b/src/sum_flt_3f.c new file mode 100644 index 0000000..3e853ea --- /dev/null +++ b/src/sum_flt_3f.c @@ -0,0 +1,64 @@ +/* + * sum_flt_3f.c + * + * Created on: 30-07-2019 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "sum_flt_3f.h" + +int sum_flt_3f_initlog(void *arguments, void *logic) +{ + struct sum_flt_3f_args *args = (struct sum_flt_3f_args *)arguments; + struct sum_flt_3f_logic *log = (struct sum_flt_3f_logic *)logic; + + if(set_float_ptr(args->io.in_l1a_float_in,&log->in_l1a)) + return -1; + if(set_float_ptr(args->io.in_l1b_float_in,&log->in_l1b)) + return -1; + + if(set_float_ptr(args->io.in_l2a_float_in,&log->in_l2a)) + return -1; + if(set_float_ptr(args->io.in_l2b_float_in,&log->in_l2b)) + return -1; + + if(set_float_ptr(args->io.in_l3a_float_in,&log->in_l3a)) + return -1; + if(set_float_ptr(args->io.in_l3b_float_in,&log->in_l3b)) + return -1; + + if(set_float_ptr(args->io.in_3i0a_float_in,&log->in_3i0a)) + return -1; + if(set_float_ptr(args->io.in_3i0b_float_in,&log->in_3i0b)) + return -1; + + if(set_float_ptr(args->io.out_l1_float_out,&log->out_l1)) + return -1; + if(set_float_ptr(args->io.out_l2_float_out,&log->out_l2)) + return -1; + if(set_float_ptr(args->io.out_l3_float_out,&log->out_l3)) + return -1; + if(set_float_ptr(args->io.out_3i0_float_out,&log->out_3i0)) + return -1; + + return 0; +} + +void sum_flt_3f(void *arguments, void *logic) +{ + struct sum_flt_3f_logic *log = (struct sum_flt_3f_logic *)logic; + struct sum_flt_3f_args *args = (struct sum_flt_3f_args *)arguments; + float m1 = args->params.m1; + float m2 = args->params.m2; + + *log->out_l1=*log->in_l1a*m1 + *log->in_l1b*m2; + *log->out_l2=*log->in_l2a*m1 + *log->in_l2b*m2; + *log->out_l3=*log->in_l3a*m1 + *log->in_l3b*m2; + *log->out_3i0=*log->in_3i0a*m1 + *log->in_3i0b*m2; +} diff --git a/src/sum_flt_3f.h b/src/sum_flt_3f.h new file mode 100644 index 0000000..2906661 --- /dev/null +++ b/src/sum_flt_3f.h @@ -0,0 +1,61 @@ +/* + * sum_flt_3f.h + * + * Created on: 30-07-2019 + * Author: Krzysztof Jakubczyk + */ + +#ifndef SUM_FLT_3F_H_ +#define SUM_FLT_3F_H_ + +#include "tdefs.h" + +struct sum_flt_3f_logic +{ + float *in_l1a; + float *in_l1b; + float *in_l2a; + float *in_l2b; + float *in_l3a; + float *in_l3b; + float *in_3i0a; + float *in_3i0b; + float *out_l1; + float *out_l2; + float *out_l3; + float *out_3i0; +}; + +struct sum_flt_3f_io +{ + u32 in_l1a_float_in; + u32 in_l1b_float_in; + u32 in_l2a_float_in; + u32 in_l2b_float_in; + u32 in_l3a_float_in; + u32 in_l3b_float_in; + u32 in_3i0a_float_in; + u32 in_3i0b_float_in; + u32 out_l1_float_out; + u32 out_l2_float_out; + u32 out_l3_float_out; + u32 out_3i0_float_out; +}__attribute__((__packed__)); + +struct sum_flt_3f_params +{ + double m1; + double m2; +}__attribute__((__packed__)); + +struct sum_flt_3f_args +{ + struct sum_flt_3f_io io; + struct sum_flt_3f_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void sum_flt_3f(void *args, void *logic); +extern int sum_flt_3f_initlog(void *arguments, void *logic); + +#endif /* SUM_FLT_3F_H_ */ diff --git a/src/sv.c b/src/sv.c new file mode 100644 index 0000000..0a32809 --- /dev/null +++ b/src/sv.c @@ -0,0 +1,515 @@ +/* + * sv.c + * + * Created on: 21-01-2021 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "eth.h" +#include "goose.h" +#include "sv.h" +#include "analog_in.h" + +#include + +#include "comm.h" +#include "../ethernet/ports/am1808/include/lwiplib.h" +#include "../ethernet/emac.h" +#include "../ethernet/hw_types.h" +#include "ports/am1808/include/netif/sitaraif.h" +#include +//#include +#include +#include + +struct sv_queue svqueue = { .cur = 0, .next = 0, .lost = 0 }; + +void iec61850_sv_recv(struct goose_frame *gfr,int len,char type) +{ + if(!(svqueue.status[svqueue.next] & (SV_TO_PROCESS))) + { + if(htons(gfr->len)==len && len<=SV_MAX_FR_SIZE) + { + memcpy(svqueue.buf[svqueue.next],(char*)gfr,len); + svqueue.status[svqueue.next]|=SV_TO_PROCESS; + svqueue.next++; + svqueue.next%=SV_QUEUE_DEPTH; + } + } + else + svqueue.lost++; +} + +int sv_initlog(void *arguments, void *logic) +{ + struct sv_args *args = (struct sv_args *)arguments; + struct sv_logic *log = (struct sv_logic *)logic; + + u8 i; + int values[6]; + u8 dstaddr[6]; + + if(set_float_ptr(args->io.sv_cnt_float_out,&log->sv_cnt)) + return -1; + + if(set_float_ptr(args->io.lost_float_out,&log->lost)) + return -1; + + if(set_float_ptr(args->io.IL1_float_out,&log->IL[0])) + return -1; + if(set_float_ptr(args->io.IL2_float_out,&log->IL[1])) + return -1; + if(set_float_ptr(args->io.IL3_float_out,&log->IL[2])) + return -1; + if(set_float_ptr(args->io.IL4_float_out,&log->IL[3])) + return -1; + if(set_float_ptr(args->io.UL1_float_out,&log->UL[0])) + return -1; + if(set_float_ptr(args->io.UL2_float_out,&log->UL[1])) + return -1; + if(set_float_ptr(args->io.UL3_float_out,&log->UL[2])) + return -1; + if(set_float_ptr(args->io.UL4_float_out,&log->UL[3])) + return -1; + + if(set_pointer_out_ptr(args->io.IL1_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][0][0])) + return -1; + if(set_pointer_out_ptr(args->io.IL2_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][1][0])) + return -1; + if(set_pointer_out_ptr(args->io.IL3_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][2][0])) + return -1; + if(set_pointer_out_ptr(args->io.IL4_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_I_CARD_ADDR][3][0])) + return -1; + + if(set_pointer_out_ptr(args->io.UL1_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][0][0])) + return -1; + if(set_pointer_out_ptr(args->io.UL2_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][1][0])) + return -1; + if(set_pointer_out_ptr(args->io.UL3_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][2][0])) + return -1; + if(set_pointer_out_ptr(args->io.UL4_buf_ptr_out,(u32*)&bus_an_samples_buf[SV_U_CARD_ADDR][3][0])) + return -1; + + if(set_pointer_out_ptr(args->io.I_params_ptr_out,(u32*)&log->I_params)) + return -1; + + if(set_pointer_out_ptr(args->io.U_params_ptr_out,(u32*)&log->U_params)) + return -1; + + if(set_bit_ptr_struct(args->io.kob_out,&log->kob)) + return -1; + if(set_bit_ptr_struct(args->io.no_sync_out,&log->no_sync)) + return -1; + + if(set_bit_ptr_struct(args->io.IL1_qual_good_out,&log->qual_good[0])) + return -1; + if(set_bit_ptr_struct(args->io.IL2_qual_good_out,&log->qual_good[1])) + return -1; + if(set_bit_ptr_struct(args->io.IL3_qual_good_out,&log->qual_good[2])) + return -1; + if(set_bit_ptr_struct(args->io.IL4_qual_good_out,&log->qual_good[3])) + return -1; + if(set_bit_ptr_struct(args->io.UL1_qual_good_out,&log->qual_good[4])) + return -1; + if(set_bit_ptr_struct(args->io.UL2_qual_good_out,&log->qual_good[5])) + return -1; + if(set_bit_ptr_struct(args->io.UL3_qual_good_out,&log->qual_good[6])) + return -1; + if(set_bit_ptr_struct(args->io.UL4_qual_good_out,&log->qual_good[7])) + return -1; + + if(set_bit_ptr_struct(args->io.IL1_test_out,&log->test[0])) + return -1; + if(set_bit_ptr_struct(args->io.IL2_test_out,&log->test[1])) + return -1; + if(set_bit_ptr_struct(args->io.IL3_test_out,&log->test[2])) + return -1; + if(set_bit_ptr_struct(args->io.IL4_test_out,&log->test[3])) + return -1; + if(set_bit_ptr_struct(args->io.UL1_test_out,&log->test[4])) + return -1; + if(set_bit_ptr_struct(args->io.UL2_test_out,&log->test[5])) + return -1; + if(set_bit_ptr_struct(args->io.UL3_test_out,&log->test[6])) + return -1; + if(set_bit_ptr_struct(args->io.UL4_test_out,&log->test[7])) + return -1; + + + svqueue.lost=0; + + + log->sv_id=(char*)&args->params.strings_start; + u8 len = strlen(log->sv_id) + 1; + if(len%4) + len+=4-(len%4); + + log->sv_dstaddr=log->sv_id+len; + + if( 6 == sscanf( log->sv_dstaddr, "%x-%x-%x-%x-%x-%x%*c", + &values[0], &values[1], &values[2], + &values[3], &values[4], &values[5] ) ) + { + for( i = 0; i < 6; ++i ) + dstaddr[i] = (u8) values[i]; + } + else + return -1; + + emac_hash_add(EMAC_0_BASE,dstaddr); + + log->U_params.znam_wtor=args->params.U_znam_wtor; + log->U_params.znam_pierw=args->params.U_znam_pierw; + log->U_params.jednostka=0; //V + + log->I_params.znam_wtor=args->params.I_znam_wtor; + log->I_params.znam_pierw=args->params.I_znam_pierw; + log->I_params.jednostka=1; //A + + log->i_mul = 0.001f / log->I_params.znam_pierw; + log->i_mul2 = 32767.0f / (/*log->I_params.znam_wtor**/args->params.I_zakr*sqrtf(2)); + log->I_params.multiplier=1.0/log->i_mul2; + log->I_params.nast.mnoznik=log->I_params.multiplier; + + log->u_mul = 0.01f / log->U_params.znam_pierw; + log->u_mul2 = 32767.0f / (/*log->U_params.znam_wtor**/args->params.U_zakr*sqrtf(2)); + log->U_params.multiplier=1.0/log->u_mul2; + log->U_params.nast.mnoznik=log->U_params.multiplier; + + return 0; +} + + +void sv(void *arguments, void *logic) +{ + struct sv_logic *log = (struct sv_logic *)logic; + struct sv_args *args = (struct sv_args *)arguments; + struct goose_frame *gfr; + u8 *pdu; + struct sv_seqdata *sv_data_ptr; + u8 *pdu_prev; + u8 matched_crit; + int len,pdu_len; + int bytes_left, bytes_left_prev; + int err; + u8 tag; + u8 i; + u32 cfg_rev; + u32 smp_cnt; + u32 smp_synch; + u32 snum; + + *log->sv_cnt=log->sv_processed; + *log->lost=svqueue.lost + log->lost_smp_cnt; + + while(svqueue.status[svqueue.cur] & SV_TO_PROCESS) + { + err = 0; + gfr = (struct goose_frame *)svqueue.buf[svqueue.cur]; + + if(htons(gfr->appid)==args->params.sv_appid) + { + matched_crit = 0; + bytes_left=htons(gfr->len)-8; + pdu = (u8 *)&gfr->pdu_tag; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + + if(len<0) + { + err=1; + break; + } + + if(tag==SV_PDU) + { + pdu_len = len; + pdu_prev = pdu; + bytes_left_prev = bytes_left; + bytes_left = pdu_len; + + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + if(len<0) + { + err=1; + break; + } + + switch(tag) + { + case SV_ASDU: + while(bytes_left>=2 && !err) + { + len = asn_parse(&pdu,&bytes_left,&tag); + if(len<0) + { + err=1; + break; + } + switch(tag) + { + case SV_ASDU_DATA: + break; + + case SV_TAG_ID: + + // process data (compare LEN bytes with STR) + if(len!=strlen(log->sv_id) || memcmp(log->sv_id,pdu,len)) + err=1; + else + matched_crit++; + pdu+=len; + bytes_left-=len; + break; + + case SV_TAG_CONFREF: + if(len>4) + err=1; + else + { + cfg_rev=0; + + for(i=0;iparams.sv_cfg_rev==cfg_rev) + matched_crit++; + else + err=1; + } + + bytes_left-=len; + break; + + case SV_TAG_SMPCNT: + if(len>4) + err=1; + else + { + smp_cnt=0; + + for(i=0;i4) + err=1; + else + { + smp_synch=0; + + for(i=0;ino_sync); + else + set_struct(&log->no_sync); + + u8 *ptr; + ptr=(u8*)sv_data_ptr; + + snum = (log->smp_cnt_prev+1) % (SAMPLES_PER_MS*1000); + + while(snum!=smp_cnt) + { + for(i=0;i<4;i++) + { + bus_an_samples_buf[SV_I_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767; + bus_an_samples_buf[SV_U_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767; + } + + snum++; + snum%=(SAMPLES_PER_MS*1000); + log->lost_smp_cnt++; + } + + log->smp_cnt_prev = smp_cnt; + + for(i=0;i<8;i++) + { + pr=(int)ptr[0]<<24; + pr|=(int)ptr[1]<<16; + pr|=(int)ptr[2]<<8; + pr|=(int)ptr[3]; + + qual=(int)ptr[4]<<24; + qual|=(int)ptr[5]<<16; + qual|=(int)ptr[6]<<8; + qual|=(int)ptr[7]; + + check_and_set_struct(!(qual & SV_QUALITY_VALIDITY_MASK),&log->qual_good[i]); + check_and_set_struct(qual & SV_QUALITY_TEST,&log->test[i]); + + if(i<4) + { + pr_fl=pr*log->i_mul; + *log->IL[i]=pr_fl; + + pr_fl*=log->i_mul2; + + if(pr_fl>32767) + pr_fl=32767; + else if(pr_fl<-32767) + pr_fl=-32767; + + bus_an_samples_buf[SV_I_CARD_ADDR][i][smp_cnt%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=(u16)(pr_fl+32767); + } + else + { + pr_fl=pr*log->u_mul; + *log->UL[i-4]=pr_fl; + + pr_fl*=log->u_mul2; + + if(pr_fl>32767) + pr_fl=32767; + else if(pr_fl<-32767) + pr_fl=-32767; + + bus_an_samples_buf[SV_U_CARD_ADDR][i-4][smp_cnt%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=(u16)(pr_fl+32767); + } + ptr+=8; + } + + log->sv_processed++; + break; + } + + pdu=pdu_prev+pdu_len; + bytes_left=bytes_left_prev-pdu_len; + break; // only one PDU checked + } + else + { + pdu+=len; + bytes_left-=len; + } + } + } + + svqueue.status[svqueue.cur]=0; + svqueue.cur=((svqueue.cur+1)%SV_QUEUE_DEPTH); + } + + if(err || matched_crit!=5) // no valid sv in this cycle + { + if(log->sv_timeout_cnt>=SV_WAIT_LOOP_CYCLES) + { + clear_struct(&log->kob); + set_struct(&log->no_sync); + for(i=0;i<8;i++) + { + clear_struct(&log->qual_good[i]); + clear_struct(&log->test[i]); + } + + if(bus_an_cur_sample_num<(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES)) + snum=SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2-((SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES)-bus_an_cur_sample_num); + else + snum=bus_an_cur_sample_num-(SAMPLES_PER_MS*LOOP_CYCLE_MS*SV_WAIT_LOOP_CYCLES); + + while(snum!=bus_an_cur_sample_num) + { + for(i=0;i<4;i++) + { + bus_an_samples_buf[SV_I_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767; + bus_an_samples_buf[SV_U_CARD_ADDR][i][snum%(SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2)]=0+32767; + } + + snum++; + snum%=(SAMPLES_PER_MS*1000); + } + + for(i=0;i<4;i++) + { + bus_an_samples_buf[SV_I_CARD_ADDR][i][bus_an_cur_sample_num]=0+32767; + bus_an_samples_buf[SV_U_CARD_ADDR][i][bus_an_cur_sample_num]=0+32767; + } + log->smp_cnt_prev = bus_an_cur_sample_num; + } + else + log->sv_timeout_cnt++; + } + else + { + log->sv_timeout_cnt=0; + set_struct(&log->kob); + } + +// +// TODO: sprawdzac dodatkowo czy jest probka z danym tstamp przed obiegiem? A co gdy brak synchro TZX lub MU? +// + +} diff --git a/src/sv.h b/src/sv.h new file mode 100644 index 0000000..d0cd248 --- /dev/null +++ b/src/sv.h @@ -0,0 +1,169 @@ +/* + * sv.h + * + * Created on: 21-01-2021 + * Author: Krzysztof Jakubczyk + */ + +#ifndef SV_H_ +#define SV_H_ + +#include "analog_in.h" +#include "tdefs.h" +#include "helper.h" +#include "comm.h" +#include "config.h" + +#define SV_NORMAL 2 +#define SV_VLAN 3 + + + +#define SV_PDU 0x60 +#define SV_ASDU 0xA2 +#define SV_ASDU_DATA 0x30 +#define SV_TAG_ID 0x80 +#define SV_TAG_SMPCNT 0x82 +#define SV_TAG_CONFREF 0x83 +#define SV_TAG_SMPSYNCH 0x85 +#define SV_TAG_SEQDATA 0x87 + + +#define SV_QUALITY_TEST 0x800 +#define SV_QUALITY_VALIDITY_MASK 0x3 + + +struct sv_seqdata +{ + int32_t CurA_Amp; + int32_t CurA_Q; + int32_t CurB_Amp; + int32_t CurB_Q; + int32_t CurC_Amp; + int32_t CurC_Q; + int32_t CurN_Amp; + int32_t CurN_Q; + int32_t VoltA_Amp; + int32_t VoltA_Q; + int32_t VoltB_Amp; + int32_t VoltB_Q; + int32_t VoltC_Amp; + int32_t VoltC_Q; + int32_t VoltN_Amp; + int32_t VoltN_Q; +}; + +struct sv_queue +{ + u32 cur; + u32 next; + u32 lost; + char buf[SV_QUEUE_DEPTH][SV_MAX_FR_SIZE]; + char status[SV_QUEUE_DEPTH]; +}; + +#define SV_TO_PROCESS 0x02 + +struct sv_logic +{ + float *sv_cnt; + float *IL[4]; + float *UL[4]; + char *sv_id; + char *sv_dstaddr; + u32 sv_processed; + float *lost; + struct analog_in_params_extended I_params; + struct analog_in_params_extended U_params; + struct binary_io kob; + struct binary_io no_sync; + struct binary_io qual_good[8]; + struct binary_io test[8]; + float i_mul; + float i_mul2; + float u_mul; + float u_mul2; + u32 smp_cnt_prev; + u32 sv_timeout_cnt; + u32 lost_smp_cnt; +}; + +struct sv_io +{ + u32 lost_float_out; + u32 sv_cnt_float_out; + + u32 I_params_ptr_out; + u32 IL1_float_out; + u32 IL1_buf_ptr_out; + u32 IL1_qual_good_out; + u32 IL1_test_out; + + u32 IL2_float_out; + u32 IL2_buf_ptr_out; + u32 IL2_qual_good_out; + u32 IL2_test_out; + + u32 IL3_float_out; + u32 IL3_buf_ptr_out; + u32 IL3_qual_good_out; + u32 IL3_test_out; + + u32 IL4_float_out; + u32 IL4_buf_ptr_out; + u32 IL4_qual_good_out; + u32 IL4_test_out; + + u32 U_params_ptr_out; + u32 UL1_float_out; + u32 UL1_buf_ptr_out; + u32 UL1_qual_good_out; + u32 UL1_test_out; + + u32 UL2_float_out; + u32 UL2_buf_ptr_out; + u32 UL2_qual_good_out; + u32 UL2_test_out; + + u32 UL3_float_out; + u32 UL3_buf_ptr_out; + u32 UL3_qual_good_out; + u32 UL3_test_out; + + u32 UL4_float_out; + u32 UL4_buf_ptr_out; + u32 UL4_qual_good_out; + u32 UL4_test_out; + + u32 kob_out; + u32 no_sync_out; +}__attribute__((__packed__)); + + +struct sv_params +{ + u32 bits; + u32 sv_appid; + u32 sv_cfg_rev; + double I_znam_wtor; ///< watosc znamionowa wtorna + double I_znam_pierw; ///< wartosc znamionowa pierwotna + double I_zakr; + double U_znam_wtor; ///< watosc znamionowa wtorna + double U_znam_pierw; ///< wartosc znamionowa pierwotna + double U_zakr; + u32 strings_start; +}__attribute__((__packed__)); + +struct sv_args +{ + struct sv_io io; + struct sv_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void sv(void *args, void *logic); +extern int sv_initlog(void *arguments, void *logic); + +extern void iec61850_sv_recv(struct goose_frame *gfr,int len,char type); + +#endif diff --git a/src/ti/ipc/Notify.h b/src/ti/ipc/Notify.h new file mode 100644 index 0000000..6638dec --- /dev/null +++ b/src/ti/ipc/Notify.h @@ -0,0 +1,6 @@ +#ifndef NOTIFY_H_ +#define NOTIFY_H_ + +// Mock for Notify.h + +#endif /* NOTIFY_H_ */ diff --git a/src/ti/sysbios/hal/Hwi.h b/src/ti/sysbios/hal/Hwi.h new file mode 100644 index 0000000..87944e7 --- /dev/null +++ b/src/ti/sysbios/hal/Hwi.h @@ -0,0 +1,6 @@ +#ifndef HWI_H_ +#define HWI_H_ + +// Mock for Hwi.h + +#endif /* HWI_H_ */ diff --git a/src/ti/sysbios/hal/Timer.h b/src/ti/sysbios/hal/Timer.h new file mode 100644 index 0000000..650be9f --- /dev/null +++ b/src/ti/sysbios/hal/Timer.h @@ -0,0 +1,10 @@ +#ifndef TIMER_H_ +#define TIMER_H_ + +// Mock for Timer.h +#include + +typedef void* Timer_Handle; +typedef uint32_t UInt32; + +#endif /* TIMER_H_ */ diff --git a/src/ti/sysbios/knl/Semaphore.h b/src/ti/sysbios/knl/Semaphore.h new file mode 100644 index 0000000..d8d0368 --- /dev/null +++ b/src/ti/sysbios/knl/Semaphore.h @@ -0,0 +1,7 @@ +#ifndef SEMAPHORE_H_ +#define SEMAPHORE_H_ + +// Mock for Semaphore.h +typedef void* Semaphore_Handle; + +#endif /* SEMAPHORE_H_ */ diff --git a/src/tp.c b/src/tp.c new file mode 100644 index 0000000..e1dc97c --- /dev/null +++ b/src/tp.c @@ -0,0 +1,70 @@ +/* + * tp.c + * + * Created on: 30-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "tp.h" + +int tp_initlog(void *arguments, void *logic) +{ + struct tp_args *args = (struct tp_args *)arguments; + struct tp_logic *log = (struct tp_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_no = args->io.in & 0x07; + + log->reset_ptr = log_manager.nets_data + (args->io.reset >> 3); + log->reset_bit_no = args->io.reset & 0x07; + + log->t_cnt=args->params.t_delay; + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->reset_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void tp(void *arguments, void *logic) +{ + struct tp_args *args = (struct tp_args *)arguments; + struct tp_logic *log = (struct tp_logic *)logic; + + if(*log->reset_ptr & (1<reset_bit_no)) + { + log->t_cnt=args->params.t_delay; + *log->out_ptr&=~(1<out_bit_no); + } + else + { + if(*log->in_ptr & (1<in_bit_no)) + { + *log->out_ptr|=(1<out_bit_no); + log->t_cnt=0; + } + else + { + if(log->t_cnt>=args->params.t_delay) + *log->out_ptr&=~(1<out_bit_no); + else + { + *log->out_ptr|=(1<out_bit_no); + log->t_cnt+=LOOP_CYCLE_MS; + } + } + } +} diff --git a/src/tp.h b/src/tp.h new file mode 100644 index 0000000..fe10c44 --- /dev/null +++ b/src/tp.h @@ -0,0 +1,50 @@ +/* + * tp.h + * + * + * Created on: 30-04-2014 + * Author: KJ + */ + +#ifndef TP_H_ +#define TP_H_ + +#include "tdefs.h" + +struct tp_logic +{ + u8 *out_ptr; + u8 out_bit_no; + + u8 *in_ptr; + u8 in_bit_no; + + u8 *reset_ptr; + u8 reset_bit_no; + + u32 t_cnt; +}; + +struct tp_io +{ + u32 in; + u32 reset; + u32 out; +}__attribute__((__packed__)); + +struct tp_params +{ + u32 t_delay; +}__attribute__((__packed__)); + +struct tp_args +{ + struct tp_io io; + struct tp_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void tp(void *args, void *logic); +extern int tp_initlog(void *args, void *logic); + +#endif /* TP_H_ */ diff --git a/src/tz.c b/src/tz.c new file mode 100644 index 0000000..c4df007 --- /dev/null +++ b/src/tz.c @@ -0,0 +1,55 @@ +/* + * tz.c + * + * Created on: 30-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "tz.h" + +int tz_initlog(void *arguments, void *logic) +{ + struct tz_args *args = (struct tz_args *)arguments; + struct tz_logic *log = (struct tz_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + log->in_ptr = log_manager.nets_data + (args->io.in >> 3); + log->in_bit_no = args->io.in & 0x07; + + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void tz(void *arguments, void *logic) +{ + struct tz_args *args = (struct tz_args *)arguments; + struct tz_logic *log = (struct tz_logic *)logic; + + if(!(*log->in_ptr & (1<in_bit_no))) + { + *log->out_ptr&=~(1<out_bit_no); + log->t_cnt=0; + } + else + { + if(log->t_cnt>=args->params.t_delay) + *log->out_ptr|=(1<out_bit_no); + else + { + *log->out_ptr&=~(1<out_bit_no); + log->t_cnt+=LOOP_CYCLE_MS; + } + } +} diff --git a/src/tz.h b/src/tz.h new file mode 100644 index 0000000..2adec42 --- /dev/null +++ b/src/tz.h @@ -0,0 +1,46 @@ +/* + * tz.h + * + * + * Created on: 30-04-2014 + * Author: KJ + */ + +#ifndef TZ_H_ +#define TZ_H_ + +#include "tdefs.h" + +struct tz_logic +{ + u8 *out_ptr; + u8 out_bit_no; + + u8 *in_ptr; + u8 in_bit_no; + + u32 t_cnt; +}; + +struct tz_io +{ + u32 in; + u32 out; +}__attribute__((__packed__)); + +struct tz_params +{ + u32 t_delay; +}__attribute__((__packed__)); + +struct tz_args +{ + struct tz_io io; + struct tz_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void tz(void *args, void *logic); +extern int tz_initlog(void *args, void *logic); + +#endif /* TZ_H_ */ diff --git a/src/tz_8.c b/src/tz_8.c new file mode 100644 index 0000000..3391ac3 --- /dev/null +++ b/src/tz_8.c @@ -0,0 +1,110 @@ +/* + * tz.c + * + * Created on: 30-04-2014 + * Author: KJ + */ + + + +#include "tdefs.h" +#include "misc.h" + +#include "tz_8.h" + +//struct lg_men log_manager; //USUN + +int tz_8_initlog(void *arguments, void *logic) +{ + struct tz_8_args *args = (struct tz_8_args *)arguments; + struct tz_8_logic *log = (struct tz_8_logic *)logic; + int i = 0; + + log->out_ptr[0] = log_manager.nets_data + (args->io.out1 >> 3); + log->out_bit_no[0] = args->io.out1 & 0x07; + + log->out_ptr[1] = log_manager.nets_data + (args->io.out2 >> 3); + log->out_bit_no[1] = args->io.out2 & 0x07; + + log->out_ptr[2] = log_manager.nets_data + (args->io.out3 >> 3); + log->out_bit_no[2] = args->io.out3 & 0x07; + + log->out_ptr[3] = log_manager.nets_data + (args->io.out4 >> 3); + log->out_bit_no[3] = args->io.out4 & 0x07; + + log->out_ptr[4] = log_manager.nets_data + (args->io.out5 >> 3); + log->out_bit_no[4] = args->io.out5 & 0x07; + + log->out_ptr[5] = log_manager.nets_data + (args->io.out6 >> 3); + log->out_bit_no[5] = args->io.out6 & 0x07; + + log->out_ptr[6] = log_manager.nets_data + (args->io.out7 >> 3); + log->out_bit_no[6] = args->io.out7 & 0x07; + + log->out_ptr[7] = log_manager.nets_data + (args->io.out8 >> 3); + log->out_bit_no[7] = args->io.out8 & 0x07; + + log->in_ptr[0] = log_manager.nets_data + (args->io.in1 >> 3); + log->in_bit_no[0] = args->io.in1 & 0x07; + + log->in_ptr[1] = log_manager.nets_data + (args->io.in2 >> 3); + log->in_bit_no[1] = args->io.in2 & 0x07; + + log->in_ptr[2] = log_manager.nets_data + (args->io.in3 >> 3); + log->in_bit_no[2] = args->io.in3 & 0x07; + + log->in_ptr[3] = log_manager.nets_data + (args->io.in4 >> 3); + log->in_bit_no[3] = args->io.in4 & 0x07; + + log->in_ptr[4] = log_manager.nets_data + (args->io.in5 >> 3); + log->in_bit_no[4] = args->io.in5 & 0x07; + + log->in_ptr[5] = log_manager.nets_data + (args->io.in6 >> 3); + log->in_bit_no[5] = args->io.in6 & 0x07; + + log->in_ptr[6] = log_manager.nets_data + (args->io.in7 >> 3); + log->in_bit_no[6] = args->io.in7 & 0x07; + + log->in_ptr[7] = log_manager.nets_data + (args->io.in8 >> 3); + log->in_bit_no[7] = args->io.in8 & 0x07; + + for (i = 0; i < 8; i++) + { + if(log->out_ptr[i] >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in_ptr[i] >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + log->t_cnt[i] = 0; + } + + return 0; +} + +void tz_8(void *arguments, void *logic) +{ + struct tz_8_args *args = (struct tz_8_args *)arguments; + struct tz_8_logic *log = (struct tz_8_logic *)logic; + + int i = 0; + + for (i = 0; i < 8; i++) + { + if(!(*log->in_ptr[i] & (1<in_bit_no[i]))) + { + *log->out_ptr[i]&=~(1<out_bit_no[i]); + log->t_cnt[i]=0; + } + else + { + if(log->t_cnt[i]>=args->params.t_delay) + *log->out_ptr[i]|=(1<out_bit_no[i]); + else + { + *log->out_ptr[i]&=~(1<out_bit_no[i]); + log->t_cnt[i]+=LOOP_CYCLE_MS; + } + } + } +} diff --git a/src/tz_8.h b/src/tz_8.h new file mode 100644 index 0000000..ec0d6aa --- /dev/null +++ b/src/tz_8.h @@ -0,0 +1,77 @@ +/* + * tz.h + * + * + * Created on: 30-04-2014 + * Author: KJ + */ + +#ifndef TZ8_H_ +#define TZ8_H_ + +/* +#include +#define u8 uint8_t +#define u16 uint16_t +#define u32 uint32_t + +struct binary_io +{ + int yyyy; +}; + +struct lg_men +{ + u8 * nets_data; +}; +*/ +#include "tdefs.h" + +struct tz_8_logic +{ + u8 *out_ptr[8]; + u8 out_bit_no[8]; + + u8 *in_ptr[8]; + u8 in_bit_no[8]; + + u32 t_cnt[8]; + +}; + +struct tz_8_io +{ + u32 in1; + u32 in2; + u32 in3; + u32 in4; + u32 in5; + u32 in6; + u32 in7; + u32 in8; + u32 out1; + u32 out2; + u32 out3; + u32 out4; + u32 out5; + u32 out6; + u32 out7; + u32 out8; +}__attribute__((__packed__)); + +struct tz_8_params +{ + u32 t_delay; +}__attribute__((__packed__)); + +struct tz_8_args +{ + struct tz_8_io io; + struct tz_8_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void tz_8(void *args, void *logic); +extern int tz_8_initlog(void *args, void *logic); + +#endif /* TZ8_H_ */ diff --git a/src/u_fail.c b/src/u_fail.c new file mode 100644 index 0000000..62e00f4 --- /dev/null +++ b/src/u_fail.c @@ -0,0 +1,349 @@ +/* + * u_fail.c + * + * Created on: 29-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "u_fail.h" +#include "helper.h" +#include + +int u_fail_initlog(void *arguments, void *logic) +{ + struct u_fail_args *args = (struct u_fail_args *)arguments; + struct u_fail_logic *log = (struct u_fail_logic *)logic; + + log->stan_bl = (u8 *)(log_manager.nets_data + (args->io.stan_bl_in >> 3)); + if((u8 *)log->stan_bl > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->stan_bl_bit = (args->io.stan_bl_in & 0x07); + + log->I1_orta = (float *)(log_manager.nets_data + (args->io.I1_orta_float_in >> 3)); + if((u8 *)log->I1_orta > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1_ortb = (float *)(log_manager.nets_data + (args->io.I1_ortb_float_in >> 3)); + if((u8 *)log->I1_ortb > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I1 = (float *)(log_manager.nets_data + (args->io.I1_float_in >> 3)); + if((u8 *)log->I1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I2 = (float *)(log_manager.nets_data + (args->io.I2_float_in >> 3)); + if((u8 *)log->I2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->I0 = (float *)(log_manager.nets_data + (args->io.I0_float_in >> 3)); + if((u8 *)log->I0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U1 = (float *)(log_manager.nets_data + (args->io.U1_float_in >> 3)); + if((u8 *)log->U1 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U2 = (float *)(log_manager.nets_data + (args->io.U2_float_in >> 3)); + if((u8 *)log->U2 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->U0 = (float *)(log_manager.nets_data + (args->io.U0_float_in >> 3)); + if((u8 *)log->U0 > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->NHS = (u8 *)(log_manager.nets_data + (args->io.NHS_in >> 3)); + if((u8 *)log->NHS > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->NHS_bit = (args->io.NHS_in & 0x07); + + log->W_ON = (u8 *)(log_manager.nets_data + (args->io.W_ON_in >> 3)); + if((u8 *)log->W_ON > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->W_ON_bit = (args->io.W_ON_in & 0x07); + + log->test = (u8 *)(log_manager.nets_data + (args->io.test_in >> 3)); + if((u8 *)log->test > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->test_bit = (args->io.test_in & 0x07); + + log->Bl_Ufail = (u8 *)(log_manager.nets_data + (args->io.Bl_Ufail_out >> 3)); + if((u8 *)log->Bl_Ufail > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->Bl_Ufail_bit = (args->io.Bl_Ufail_out & 0x07); + + log->Syg_Ufail = (u8 *)(log_manager.nets_data + (args->io.Syg_Ufail_out >> 3)); + if((u8 *)log->Syg_Ufail > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->Syg_Ufail_bit = (args->io.Syg_Ufail_out & 0x07); + + log->nast_.on = args->params.bity & 0x0001; + log->nast_.IU0 = args->params.bity & 0x002; + log->nast_.IU1 = args->params.bity & 0x004; + log->nast_.IU2 = args->params.bity & 0x008; + log->nast_.bl = args->params.bity & 0x0010; + + log->nast_.Iomin = args->params.Iomin * args->params.Iomin; + log->nast_.Iokh = args->params.Iokh * args->params.Iokh; + log->nast_.Uomin = args->params.Uomin * args->params.Uomin; + log->nast_.I2min = args->params.I2min * args->params.I2min; + log->nast_.I2kh = args->params.I2kh * args->params.I2kh; + log->nast_.U2min = args->params.U2min * args->params.U2min; + + log->nast_.U0_dbl = args->params.U0_dbl * args->params.U0_dbl; + log->nast_.U1_dbl = args->params.U1_dbl * args->params.U1_dbl; + log->nast_.U2_dbl = args->params.U2_dbl * args->params.U2_dbl; + log->nast_.DU1 = args->params.DU1; + log->nast_.DI1k = 2 * args->params.DI1 * args->params.DI1; + log->nast_.DI1 = args->params.DI1; + log->nast_.I1max = args->params.I1max * args->params.I1max; + + log->nast_.tz = (uint16_t)(100 * (args->params.tbl-0.005f)); + + return 0; +} + +void u_fail(void *arguments, void *logic) +{ + struct u_fail_logic *log = (struct u_fail_logic *)logic; + + u8 local; + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + log->w.I0 = *log->I0; + log->w.I1 = *log->I1; + log->w.I2 = *log->I2; + log->w.U0 = *log->U0; + log->w.U1 = *log->U1; + log->w.U2 = *log->U2; + + //------------------------------------------- + //przekaznik skladowych zerowych + if(log->nast_.IU0) //jeeli kryterium aktywne + { + if (log->dw.Syg_Ufail) + { + local = !log->dw.U_OK && log->dw.Ufail_0; + } else { + local = (log->w.U0 > log->nast_.Uomin) && !((log->w.I0 > log->nast_.Iomin) && (log->w.I0 > (log->nast_.Iokh * log->w.I1))) && (*log->W_ON & (1<W_ON_bit)); + } + + sprawdz_P( + &log->dw.Ufail_0, + local, + !local, + &log->dw.licz0, + 3,3 + ); + } + + //------------------------------------------- + //przekaznik skladowych przeciwnych + if(log->nast_.IU2) //jeeli kryterium aktywne + { + if (log->dw.Syg_Ufail) + { + local = !log->dw.U_OK && log->dw.Ufail_2; + } else { + local = (log->w.U2 > log->nast_.U2min) && !((log->w.I2 > log->nast_.I2min) && (log->w.I2 > (log->nast_.I2kh * log->w.I1))) && (*log->W_ON & (1<W_ON_bit)); + } + sprawdz_P( + &log->dw.Ufail_2, + local, + !local, + &log->dw.licz2, + 3,3 + ); + } + + + //------------------------------------------- + //przekaznik skladowych zgodnych + if(log->nast_.IU1) //jeeli kryterium aktywne + { + + + log->dw.I1p = sqrtf(log->w.I1); + local = (fabs(log->dw.I1_old[2] - log->dw.I1p) > log->nast_.DI1) || (log->w.I1 > log->nast_.I1max); + sprawdz_P( + &log->dw.UI1_Iw, + local, + !local, + &log->dw.liczI1, + 3,48 + ); + + log->dw.U1p = sqrtf(log->w.U1); + local = (log->dw.U1_old - log->dw.U1p) > log->nast_.DU1; + sprawdz_P( + &log->dw.UI1_dUw, + local, + !local, + &log->dw.liczdU1, + 3,3 + ); + + + local = (log->w.U1 > log->nast_.U1_dbl) || (log->w.U0 > log->nast_.U0_dbl) || (log->w.U2 > log->nast_.U2_dbl); + sprawdz_P( + &log->dw.UI1_Uw, + local, + !local, + &log->dw.liczU1, + 3,3 + ); + + + if (log->dw.Syg_Ufail) + { + log->dw.Ufail_1 &= !log->dw.U_OK; + } else { + log->dw.Ufail_1 |= log->dw.UI1_dUw; + log->dw.Ufail_1 &= !(log->dw.UI1_Iw || log->dw.UI1_Uw || log->dw.UI1_Iwk || !(*log->W_ON & (1<W_ON_bit))); + } + } + + + if(log->dw.Ufail_0 || log->dw.Ufail_1 || log->dw.Ufail_2 || (*log->NHS & (1<NHS_bit))) + *log->Bl_Ufail|=(1<Bl_Ufail_bit); + else + *log->Bl_Ufail&=~(1<Bl_Ufail_bit); + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + check_and_set(*log->test & (1<test_bit),log->Bl_Ufail,log->Bl_Ufail_bit); + } + // + + + // realizacja blokady dzialania przekaznika + if(log->nast_.bl && (*log->stan_bl & (1<stan_bl_bit))) { + *log->Bl_Ufail&=~(1<Bl_Ufail_bit); + log->dw.Syg_Ufail = 0; + log->dw.Ufail_0 = log->dw.Ufail_1 = log->dw.Ufail_2 = 0; + } + // + + + //---------------------------------- + + + } else { + *log->Bl_Ufail&=~(1<Bl_Ufail_bit); + } + +} + +void u_fail_100hz(void *arguments, void *logic) +{ + struct u_fail_logic *log = (struct u_fail_logic *)logic; + + u8 local; + float local1,local2; + + + if(log->nast_.on) // jesli zabezpieczenie aktywne + { + + if(log->nast_.IU1) + { + log->w.I1_ort_a = *log->I1_orta; + log->w.I1_ort_b = *log->I1_ortb; + + log->dw.idx_bufor++; + if(log->dw.idx_bufor>9) log->dw.idx_bufor = 0; + log->dw.I1_old[0] = log->dw.buforI[log->dw.idx_bufor][0]; + log->dw.I1_old[1] = log->dw.buforI[log->dw.idx_bufor][1]; + log->dw.I1_old[2] = log->dw.buforI[log->dw.idx_bufor][2]; + log->dw.U1_old = log->dw.buforU[log->dw.idx_bufor]; + log->dw.buforI[log->dw.idx_bufor][0] = log->w.I1_ort_a; + log->dw.buforI[log->dw.idx_bufor][1] = log->w.I1_ort_b; + log->dw.buforI[log->dw.idx_bufor][2] = log->dw.I1p; + log->dw.buforU[log->dw.idx_bufor] = log->dw.U1p; + + + local1 = log->w.I1_ort_a - log->dw.I1_old[0]; + local2 = log->w.I1_ort_b - log->dw.I1_old[1]; + local = (local1*local1 + local2*local2) > log->nast_.DI1k; + + + sprawdz_P( + &log->dw.UI1_Iwk, + local, + !local, + &log->dw.liczI1k, + 3,4 + ); + + + } + + + local = log->w.U0 < log->nast_.U0_dbl && log->w.U1 > log->nast_.U1_dbl && log->w.U2 < log->nast_.U2_dbl; + + sprawdz_P( + &log->dw.U_OK, + local, + !local, + &log->dw.liczOK, + 6,3 + ); + + + + + //realizacja przekaznika czasowego + if ((*log->Bl_Ufail & (1<Bl_Ufail_bit))) + { + log->dw.liczt++; + if (log->dw.liczt>log->nast_.tz) + { + log->dw.Syg_Ufail = 1; + log->dw.liczt--; + } + } else { + log->dw.Syg_Ufail = 0; + log->dw.liczt = 0; + } + + + + + if(log->dw.Syg_Ufail) + *log->Syg_Ufail|=(1<Syg_Ufail_bit); + else + *log->Syg_Ufail&=~(1<Syg_Ufail_bit); + + } else { + *log->Syg_Ufail&=~(1<Syg_Ufail_bit); + } +} + +void u_fail_20hz(void *arguments, void *logic) +{ + struct u_fail_logic *log = (struct u_fail_logic *)logic; + + if(!(*log->Bl_Ufail & (1<Bl_Ufail_bit)) && log->dw.wsk_rej) // przypisanie parametrow zaklocenia jesli bylo zadzialanie i zabezpieczenie sie odwzbudzilo + { + // log->dw.R.I0 = sqrt(log->dw.I0); + // log->dw.R.I1 = sqrt(log->dw.I1); +// log->dw.R.I2 = sqrt(log->dw.I2); + // log->dw.R.U0 = sqrt(log->dw.U0); + // log->dw.R.U1 = sqrt(log->dw.U1); +// log->dw.R.U2 = sqrt(log->dw.U2); +// log->dw.R.moment_wystapienia = log->dw.czas_; // wpisanie czasu chwili zadzialania zabezpieczenia + // log->dw.R.dlugosc = (float)log->dw.dl_ * 0.01f; // wpisanie czasu trwania zwarcia + // log->dw.R.czy_nowa = true; // sygnalizacja powstania nowej rejestracji + log->dw.wsk_rej = 0; // zerowanie wskaznika powstania nowego zaklocenia + } +} diff --git a/src/u_fail.h b/src/u_fail.h new file mode 100644 index 0000000..6e77b26 --- /dev/null +++ b/src/u_fail.h @@ -0,0 +1,164 @@ +/* + * u_fail.h + * + * Created on: 29-07-2016 + * Author: Krzysztof Jakubczyk + */ + +#ifndef U_FAIL_H_ +#define U_FAIL_H_ + +#include "tdefs.h" + +struct wejscia_u_fail +{ + float I1_ort_a; ///< skladowa ortogonalna a skladowej zgodnej pradu + float I1_ort_b; ///< skladowa ortogonalna b skladowej zgodnej pradu + float I1; ///< skladowa zgodna pradu + float I2; ///< skladowa przeciwna pradu + float I0; ///< skladowa zerowa pradu + float U1; ///< skladowa zgodna napiecia + float U2; ///< skladowa przeciwna napiecia + float U0; ///< skladowa zerowa napiecia +}; + +struct Nastawy_przeliczone_u_fail + { + float Iomin;///io.stan_bl_in,&log->stan_bl,&log->stan_bl_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.test_in,&log->test,&log->test_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.W_out,&log->W,&log->W_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.Z_out,&log->Z,&log->Z_bit_mask)) + return -1; + + if(set_bit_ptr_mask(args->io.P_out,&log->P,&log->P_bit_mask)) + return -1; + + if(set_float_ptr(args->io.I_float_in,&log->I)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = ((args->params.t_ - 0.005f) * 100); //czas dzialania + log->nast_.full_period = args->params.filter_type; + return 0; +} + +void under_1ph(void *arguments, void *logic) +{ +// struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct under_1ph_logic *log = (struct under_1ph_logic *)logic; + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob, + *log->Inast_.limit1_, + *log->I>log->nast_.limit2_, + &log->dw.lp, + log->nast_.full_period?20:3,3); //realizacja przekaznika + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob = (*log->test & log->test_bit_mask)?1:0; + } + + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & log->stan_bl_bit_mask)) /*|| czy_start()*/) + log->dw.pob = 0; + // + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad && log->dw.pob) + { + log->dw.zad = 1; +// dw.czas_ = time; + } + + check_and_set_mask(log->dw.pob,log->P,log->P_bit_mask); + check_and_set_mask(log->dw.zad,log->Z,log->Z_bit_mask); + check_and_set_mask(log->dw.zad && log->nast_.w_,log->W,log->W_bit_mask); + } else { + *log->W&=~log->W_bit_mask; + *log->Z&=~log->Z_bit_mask; + *log->P&=~log->P_bit_mask; + } +} + +void under_1ph_100hz(void *arguments, void *logic) +{ +// struct ov_1ph_args *args = (struct ov_1ph_args *)arguments; + struct under_1ph_logic *log = (struct under_1ph_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + } +} diff --git a/src/under_1ph.c_ b/src/under_1ph.c_ new file mode 100644 index 0000000..54aa814 --- /dev/null +++ b/src/under_1ph.c_ @@ -0,0 +1,68 @@ +/* + * under_1ph.c + * + * Created on: 12-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "../tdefs.h" +#include "../misc.h" + +#include "under_1ph.h" + +int under_1ph_initlog(void *arguments, void *logic) +{ + struct under_1ph_args *args = (struct under_1ph_args *)arguments; + struct under_1ph_logic *log = (struct under_1ph_logic *)logic; + + log->action_ptr = log_manager.nets_data + (args->io.action_out >> 3); + log->action_bit_no = args->io.action_out & 0x07; + + if(log->action_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->trig_ptr = log_manager.nets_data + (args->io.trig_out >> 3); + log->trig_bit_no = args->io.trig_out & 0x07; + + if(log->trig_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8))) + return -1; + + log->esk_ptr = (float *)(log_manager.nets_data + (args->io.esk_float_in >> 3)); + if((u8 *)log->esk_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(float))) + return -1; + + log->limit_val1 = args->params.trig_val * args->params.trig_val; + log->limit_val2 = log->limit_val1 * args->params.kp * args->params.kp; + + return 0; +} + +void under_1ph(void *arguments, void *logic) +{ + struct under_1ph_args *args = (struct under_1ph_args *)arguments; + struct under_1ph_logic *log = (struct under_1ph_logic *)logic; + + if(*log->trig_ptr & (1<trig_bit_no)) + { + if(!(*log->action_ptr & (1<action_bit_no))) + { + log->time_cnt++; + if(log->time_cnt>args->params.t_delay) + *log->action_ptr|=(1<action_bit_no); + } + + if(*log->esk_ptr > log->limit_val2) + { + *log->action_ptr&=~(1<action_bit_no); + *log->trig_ptr&=~(1<trig_bit_no); + } + } + else + { + if(*log->esk_ptr < log->limit_val1) + { + *log->trig_ptr|=(1<trig_bit_no); + log->time_cnt=0; + } + } +} diff --git a/src/under_1ph.h b/src/under_1ph.h new file mode 100644 index 0000000..55d25f1 --- /dev/null +++ b/src/under_1ph.h @@ -0,0 +1,86 @@ +/* + * under_1ph.h + * + * Created on: 08-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef UNDER_1PH_H_ +#define UNDER_1PH_H_ + +#include "tdefs.h" + +struct dane_wewnetrzne_under1ph +{ +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) +// uint32_t dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) +// float max_; // maksymalna watosc zaklocenia (rejestr tymczasowy - przed zapisem) + short lp; ///io.stan_bl,&log->stan_bl,&log->stan_bl_bit_no)) + return -1; + + if(set_bit_ptr(args->io.test_in,&log->test,&log->test_bit_no)) + return -1; + + if(set_bit_ptr(args->io.W_out,&log->W,&log->W_bit_no)) + return -1; + + if(set_bit_ptr(args->io.Z_out,&log->Z,&log->Z_bit_no)) + return -1; + + if(set_bit_ptr(args->io.P_out,&log->P,&log->P_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL1_out,&log->PL1,&log->PL1_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL2_out,&log->PL2,&log->PL2_bit_no)) + return -1; + + if(set_bit_ptr(args->io.PL3_out,&log->PL3,&log->PL3_bit_no)) + return -1; + + if(set_float_ptr(args->io.I1_float_in,&log->I1)) + return -1; + + if(set_float_ptr(args->io.I2_float_in,&log->I2)) + return -1; + + if(set_float_ptr(args->io.I3_float_in,&log->I3)) + return -1; + + log->nast_.on_ = (args->params.bity & 0x0001)?1:0; + log->nast_.w_ = (args->params.bity & 0x0002)?1:0; + log->nast_.bl_ = (args->params.bity & 0x0004)?1:0; + log->nast_.or_ = (args->params.bity & 0x0008)?1:0; + log->nast_.limit1_ = args->params.Wr_ * args->params.Wr_; // wyliczenie wartosci rozruchowej algorytmu + log->nast_.limit2_ = log->nast_.limit1_ * args->params.kp_ * args->params.kp_; // wyliczenie wartosci powrotowej algorytmu + log->nast_.t_ = (args->params.t_ - 0.005f) * 100; //czas dzialania + log->nast_.full_period = args->params.filter_type; + //*((uint8_t *)REG_FUNCTION_ON_OFF + id - 0x2400) = nast_.on_; + + return 0; +} + +void under_3ph(void *arguments, void *logic) +{ + struct under_3ph_logic *log = (struct under_3ph_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + sprawdz_P(&log->dw.pob1, + *log->I1nast_.limit1_, + *log->I1>log->nast_.limit2_, + &log->dw.lp1, + log->nast_.full_period?20:5,5); //realizacja przekaznika faza 1 + + sprawdz_P(&log->dw.pob2, + *log->I2nast_.limit1_, + *log->I2>log->nast_.limit2_, + &log->dw.lp2, + log->nast_.full_period?20:5,5); //realizacja przekaznika faza 2 + + sprawdz_P(&log->dw.pob3, + *log->I3nast_.limit1_, + *log->I3>log->nast_.limit2_, + &log->dw.lp3, + log->nast_.full_period?20:5,5); //realizacja przekaznika faza 3 + + + //obsluga wejscia testu przekaznika + if (czy_test_R()) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = (*log->test & (1<test_bit_no))?1:0; + } + // + + // realizacja blokady dzialania przekaznika + if((log->nast_.bl_ && (*log->stan_bl & (1<stan_bl_bit_no))) /*| czy_start()*/) + { + log->dw.pob1 = log->dw.pob2 = log->dw.pob3 = 0; + } + // + + if(log->nast_.or_) // zaleznie od nastawienia logiki pobudzen "and" lub "or" + { + log->dw.pob = log->dw.pob1 || log->dw.pob2 || log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "or" + } else { + log->dw.pob = log->dw.pob1 && log->dw.pob2 && log->dw.pob3;// identyfikacja pobudzenia przekaznika dla "and" + } + + + if (!log->nast_.t_) // realizacja przekaznika bezzwlocznego + if (!log->dw.zad & log->dw.pob) + { + log->dw.zad = 1; + //log->dw.czas_ = time; + } + + check_and_set(log->dw.pob,log->P,log->P_bit_no); + check_and_set(log->dw.pob1,log->PL1,log->PL1_bit_no); + check_and_set(log->dw.pob2,log->PL2,log->PL2_bit_no); + check_and_set(log->dw.pob3,log->PL3,log->PL3_bit_no); + check_and_set(log->dw.zad,log->Z,log->Z_bit_no); + check_and_set(log->dw.zad && log->nast_.w_,log->W,log->W_bit_no); + } else { + *log->W&=~(1<W_bit_no); + *log->Z&=~(1<Z_bit_no); + *log->P&=~(1<P_bit_no); + *log->PL1&=~(1<PL1_bit_no); + *log->PL2&=~(1<PL2_bit_no); + *log->PL3&=~(1<PL3_bit_no); + } +} + +void under_3ph_100hz(void *arguments, void *logic) +{ + struct under_3ph_logic *log = (struct under_3ph_logic *)logic; + + if(log->nast_.on_) // jesli zabezpieczenie aktywne + { + + // realizacja przekaznika czasowego + if(log->dw.pob) log->dw.licz_t_++; else log->dw.licz_t_ = 0; // odliczanie czasu dzialania + if(log->dw.licz_t_ > log->nast_.t_) + { // sprawdzenie przekroczenia nastawionego czasu + log->dw.zad = 1; // ustawienie zadzialania po odliczeniu czasu + log->dw.licz_t_--; + } else log->dw.zad = 0; + // + // realizacja rejestratora parametrow ostatniego zaklocenia + /* float wej1 = dane_analogowe[nast_.adr.kwampl1]; + float wej2 = dane_analogowe[nast_.adr.kwampl2]; + float wej3 = dane_analogowe[nast_.adr.kwampl3]; + + switch(status_rej) + { + case oczekiwanie: + if (dw.pob) + { + dw.max1_ = wej1; + dw.max2_ = wej2; + dw.max3_ = wej3; + dw.dl_ = 0; // start odliczania czasu dlugosci trwania zaklocenia + status_rej = sledzenie; + if (dw.zad) + { + status_rej = sledzenie_zad; + } + } + break; + case sledzenie: + if (dw.zad) + { + status_rej = sledzenie_zad; + dw.czas_ = time; // zapisanie do bufora tymczasowego czasu wyslania impulsu wylacz dla rejestracji + } + if (dw.pob) + { + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + dw.dl_++; + } else { + status_rej = oczekiwanie; + } + break; + case sledzenie_zad: + if (!dw.pob) + { + dw.wsk_rej = 1; // zapamietanie faktu powstania zaklocenia z wylaczeniem + status_rej = zapis; + } else { + dw.dl_++; + if(dw.max1_ < wej1) dw.max1_ = wej1;// szukanie wartosci maksymalnej + if(dw.max2_ < wej2) dw.max2_ = wej2;// szukanie wartosci maksymalnej + if(dw.max3_ < wej3) dw.max3_ = wej3;// szukanie wartosci maksymalnej + } + break; + case zapis: + if (!dw.wsk_rej) + { + status_rej = oczekiwanie; + } + break; + }*/ + } +} + diff --git a/src/under_3ph.h b/src/under_3ph.h new file mode 100644 index 0000000..431e28e --- /dev/null +++ b/src/under_3ph.h @@ -0,0 +1,106 @@ +/* + * under_3ph.h + * + * Created on: 29-11-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef UNDER_3PH_H_ +#define UNDER_3PH_H_ + +#include "tdefs.h" + + struct Nastawy_przeliczone_under3ph + { + float limit1_; ///< wartosc rozruchowa + float limit2_; ///< wartosc powrotu + u32 t_; ///< opoznienie czasu zadzialania przekaznika + u8 on_; ///< wartosc nastawy aktywnosci przekaznika 1- aktywny + u8 w_; ///< wartosc nastawy dzialania na wylaczeniee 1 - wylaczenie 0 - tylko sygnalizacja + u8 bl_; ///< wartosc nastawy aktywnosci blokady dzialania 1- blokada aktywna + u8 or_; ///< logika pobudzen "0" oznacza dzialanie w trybie "and", a "1" w trybie "or" + u8 full_period; + }; + + struct dane_wewnetrzne_under3ph + { +// Wyjscia_rejestratora R; ///< dane ostatniego zaklocenia +// Time czas_; // moment wystapienia zaklocenia (rejestr tymczasowy - przed zapisem) + u32 dl_; // dlugosc czasu trwania zaklocenia (rejestr tymczasowy - przed zapisem) + float max1_; // maksymalna watosc wejscia 1 (rejestr tymczasowy - przed zapisem) + float max2_; // maksymalna watosc wejscia 2 (rejestr tymczasowy - przed zapisem) + float max3_; // maksymalna watosc wejscia 3 (rejestr tymczasowy - przed zapisem) + short lp1; ///out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + log->sample_ptr = (float *)(log_manager.nets_data + (args->io.sample_float_in >> 3)); + if((u8*)log->sample_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data)-sizeof(float))) + return -1; + + return 0; +} + +void valf_gt(void *arguments, void *logic) +{ + struct valf_gt_args *args = (struct valf_gt_args *)arguments; + struct valf_gt_logic *log = (struct valf_gt_logic *)logic; + + if(args->io.out) + { + if(*log->sample_ptr > args->params.trig_val) + *log->out_ptr |= (1<out_bit_no); + else + *log->out_ptr &= ~(1<out_bit_no); + } +} diff --git a/src/valf_gt.h b/src/valf_gt.h new file mode 100644 index 0000000..1ffef0a --- /dev/null +++ b/src/valf_gt.h @@ -0,0 +1,41 @@ +/* + * valf_gt.h + * + * Created on: 07-05-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef VALF_GT_H_ +#define VALF_GT_H_ + +#include "tdefs.h" + +struct valf_gt_logic +{ + float *sample_ptr; + u8 *out_ptr; + u8 out_bit_no; +}; + +struct valf_gt_io +{ + u32 sample_float_in; + u32 out; +}__attribute__((__packed__)); + +struct valf_gt_params +{ + double trig_val; +}__attribute__((__packed__)); + +struct valf_gt_args +{ + struct valf_gt_io io; + struct valf_gt_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void valf_gt(void *args, void *logic); +extern int valf_gt_initlog(void *arguments, void *logic); + +#endif /* VALF_GT_H_ */ diff --git a/src/virt_in.c b/src/virt_in.c new file mode 100644 index 0000000..4f4746c --- /dev/null +++ b/src/virt_in.c @@ -0,0 +1,29 @@ +/* + * virt_in.c + * + * Created on: 14-10-2014 + * Author: Krzysztof Jakubczyk + */ + +#include "tdefs.h" +#include "misc.h" + +#include "virt_in.h" + +int virt_in_initlog(void *arguments, void *logic) +{ + struct virt_in_args *args = (struct virt_in_args *)arguments; + struct virt_in_logic *log = (struct virt_in_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(args->params.in_num>=MAX_VIRT_IN_COUNT) + return -1; + + return 0; +} + diff --git a/src/virt_in.h b/src/virt_in.h new file mode 100644 index 0000000..46d06d5 --- /dev/null +++ b/src/virt_in.h @@ -0,0 +1,44 @@ +/* + * virt_in.h + * + * Created on: 14-10-2014 + * Author: Krzysztof Jakubczyk + */ + +#ifndef VIRT_IN_H_ +#define VIRT_IN_H_ + +#include "tdefs.h" + +#define MAX_VIRT_IN_COUNT 64 +#define VIRT_IN_IMPULSE_TIME 150 +#define VIRT_IN_IMPULSE_TIME_100HZ 15 + +struct virt_in_logic +{ + u8 *out_ptr; + u8 out_bit_no; + u8 impulse_cnt; +}; + +struct virt_in_io +{ + u32 out; +}__attribute__((__packed__)); + +struct virt_in_params +{ + u32 in_num; + u32 impulse; +}__attribute__((__packed__)); + +struct virt_in_args +{ + struct virt_in_io io; + struct virt_in_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int virt_in_initlog(void *arguments, void *logic); + +#endif /* VIRT_IN_H_ */ diff --git a/src/virt_in_drv.c b/src/virt_in_drv.c new file mode 100644 index 0000000..6133d84 --- /dev/null +++ b/src/virt_in_drv.c @@ -0,0 +1,125 @@ +/* + * virt_in_drv.c + * + * Created on: 15-10-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "virt_in_drv.h" +#include "virt_in.h" +#include + +u32 virt_in_states = 0; +u32 virt_in_mask = 0; + +u32 virt_in2_states = 0; +u32 virt_in2_mask = 0; + +int virt_in_drv_initlog(void *arguments, void *logic) +{ + struct virt_in_drv_logic *log = (struct virt_in_drv_logic *)logic; + struct virt_in_args *v_args; + struct virt_in_logic *v_log; + + int i; + + log->virt_in_count=0; + + //Semaphore_post(spi_semaphore); + //virt_in_states = 0; + + virt_in_mask = 0; + virt_in2_mask = 0; + for(i=0;ielement_num[log->virt_in_count]>(u8 *)(log_manager.buf+sizeof(log_manager.buf)-sizeof(u16))) + return -1; + + v_args = (struct virt_in_args *)log_manager.log_element[i].fun_args_ptr; + v_log = (struct virt_in_logic *)log_manager.log_element[i].fun_log_ptr; + + if(v_args->params.in_num<32) + { + if(virt_in_states & (1<params.in_num)) + *v_log->out_ptr|=(1<out_bit_no); + + virt_in_mask|=(1<params.in_num); + } + else + { + if(virt_in2_states & (1<<(v_args->params.in_num-32))) + *v_log->out_ptr|=(1<out_bit_no); + + virt_in2_mask|=(1<<(v_args->params.in_num-32)); + } + log->element_num[log->virt_in_count]=i; + log->virt_in_count++; + } + } + + + return 0; +} + +void virt_in_drv_100hz(void *arguments, void *logic) +{ + struct virt_in_drv_logic *log = (struct virt_in_drv_logic *)logic; + struct virt_in_args *v_args; + struct virt_in_logic *v_log; + int i; + + for(i=0;ivirt_in_count;i++) + { + v_args = (struct virt_in_args *)log_manager.log_element[log->element_num[i]].fun_args_ptr; + v_log = (struct virt_in_logic *)log_manager.log_element[log->element_num[i]].fun_log_ptr; + + if(v_args->params.in_num<32) + { + if(virt_in_states & (1<params.in_num)) + { + *v_log->out_ptr|=(1<out_bit_no); + if(v_args->params.impulse) + { + if(v_log->impulse_cnt++>=VIRT_IN_IMPULSE_TIME_100HZ) + { + virt_in_states&=~(1<params.in_num); + *v_log->out_ptr&=~(1<out_bit_no); + v_log->impulse_cnt=0; + } + } + } + else + { + *v_log->out_ptr&=~(1<out_bit_no); + v_log->impulse_cnt=0; + } + } + else + { + if(virt_in2_states & (1<<(v_args->params.in_num-32))) + { + *v_log->out_ptr|=(1<out_bit_no); + if(v_args->params.impulse) + { + if(v_log->impulse_cnt++>=VIRT_IN_IMPULSE_TIME_100HZ) + { + virt_in2_states&=~(1<<(v_args->params.in_num-32)); + *v_log->out_ptr&=~(1<out_bit_no); + v_log->impulse_cnt=0; + } + } + } + else + { + *v_log->out_ptr&=~(1<out_bit_no); + v_log->impulse_cnt=0; + } + } + } +} diff --git a/src/virt_in_drv.h b/src/virt_in_drv.h new file mode 100644 index 0000000..97a09aa --- /dev/null +++ b/src/virt_in_drv.h @@ -0,0 +1,43 @@ +/* + * virt_in_drv.h + * + * + * Created on: 15-10-2014 + * Author: KJ + */ + +#ifndef VIRT_IN_DRV_H_ +#define VIRT_IN_DRV_H_ + +#include "tdefs.h" + +extern u32 virt_in_states; +extern u32 virt_in_mask; +extern u32 virt_in2_states; +extern u32 virt_in2_mask; + +struct virt_in_drv_logic +{ + u8 virt_in_count; + u16 element_num[]; +}; + +struct virt_in_drv_io +{ +}__attribute__((__packed__)); + +struct virt_in_drv_params +{ +}__attribute__((__packed__)); + +struct virt_in_drv_args +{ + struct virt_in_drv_io io; + struct virt_in_drv_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern int virt_in_drv_initlog(void *args, void *logic); +extern void virt_in_drv_100hz(void *args, void *logic); + +#endif /* VIRT_IN_DRV_H_ */ diff --git a/src/xor.c b/src/xor.c new file mode 100644 index 0000000..faa5f26 --- /dev/null +++ b/src/xor.c @@ -0,0 +1,62 @@ +/* + * xor.c + * + * Created on: 29-04-2014 + * Author: KJ + */ + + +#include "tdefs.h" +#include "misc.h" + +#include "xor.h" + +int xor_initlog(void *arguments, void *logic) +{ + struct xor_args *args = (struct xor_args *)arguments; + struct xor_logic *log = (struct xor_logic *)logic; + + log->out_ptr = log_manager.nets_data + (args->io.out >> 3); + log->out_bit_no = args->io.out & 0x07; + + log->in1_ptr = log_manager.nets_data + (args->io.in1 >> 3); + log->in1_bit_no = args->io.in1 & 0x07; + + log->in2_ptr = log_manager.nets_data + (args->io.in2 >> 3); + log->in2_bit_no = args->io.in2 & 0x07; + + if(log->out_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in1_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + if(log->in2_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data))) + return -1; + + return 0; +} + +void xor(void *arguments, void *logic) +{ + struct xor_args *args = (struct xor_args *)arguments; + struct xor_logic *log = (struct xor_logic *)logic; + + if(args->io.out) + { + if(*log->in1_ptr & (1<in1_bit_no)) + { + if(*log->in2_ptr & (1<in2_bit_no)) + *log->out_ptr&=~(1<out_bit_no); + else + *log->out_ptr|=(1<out_bit_no); + } + else + { + if(*log->in2_ptr & (1<in2_bit_no)) + *log->out_ptr|=(1<out_bit_no); + else + *log->out_ptr&=~(1<out_bit_no); + } + } +} diff --git a/src/xor.h b/src/xor.h new file mode 100644 index 0000000..5542777 --- /dev/null +++ b/src/xor.h @@ -0,0 +1,47 @@ +/* + * xor.h + * + * + * Created on: 29-04-2014 + * Author: KJ + */ + +#ifndef XOR_H_ +#define XOR_H_ + +#include "tdefs.h" + +struct xor_logic +{ + u8 *out_ptr; + u8 out_bit_no; + + u8 *in1_ptr; + u8 in1_bit_no; + + u8 *in2_ptr; + u8 in2_bit_no; +}; + +struct xor_io +{ + u32 in1; + u32 in2; + u32 out; +}__attribute__((__packed__)); + +struct xor_params +{ +}__attribute__((__packed__)); + +struct xor_args +{ + struct xor_io io; + struct xor_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void xor(void *args, void *logic); +extern int xor_initlog(void *args, void *logic); + +#endif /* XOR_H_ */ diff --git a/src/zzr.c b/src/zzr.c new file mode 100644 index 0000000..47ac68e --- /dev/null +++ b/src/zzr.c @@ -0,0 +1,285 @@ +/* + * zzr.c + * + * Created on: 11-07-2017 + * Author: Krzysztof Jakubczyk + */ + +#include + +#include "tdefs.h" +#include "misc.h" + +#include "helper.h" +#include "zzr.h" + +int zzr_initlog(void *arguments, void *logic) +{ + struct zzr_args *args = (struct zzr_args *)arguments; + struct zzr_logic *log = (struct zzr_logic *)logic; + + if(set_float_ptr(args->io.InL1_orta,&log->InL1_orta)) + return -1; + + if(set_float_ptr(args->io.InL1_ortb,&log->InL1_ortb)) + return -1; + + if(set_float_ptr(args->io.InL1_esk,&log->InL1_esk)) + return -1; + + if(set_float_ptr(args->io.InL2_orta,&log->InL2_orta)) + return -1; + + if(set_float_ptr(args->io.InL2_ortb,&log->InL2_ortb)) + return -1; + + if(set_float_ptr(args->io.InL2_esk,&log->InL2_esk)) + return -1; + + if(set_float_ptr(args->io.InL3_orta,&log->InL3_orta)) + return -1; + + if(set_float_ptr(args->io.InL3_ortb,&log->InL3_ortb)) + return -1; + + if(set_float_ptr(args->io.InL3_esk,&log->InL3_esk)) + return -1; + + if(set_float_ptr(args->io.In2L1_orta,&log->In2L1_orta)) + return -1; + + if(set_float_ptr(args->io.In2L1_ortb,&log->In2L1_ortb)) + return -1; + + if(set_float_ptr(args->io.In2L1_esk,&log->In2L1_esk)) + return -1; + + if(set_float_ptr(args->io.In2L2_orta,&log->In2L2_orta)) + return -1; + + if(set_float_ptr(args->io.In2L2_ortb,&log->In2L2_ortb)) + return -1; + + if(set_float_ptr(args->io.In2L2_esk,&log->In2L2_esk)) + return -1; + + if(set_float_ptr(args->io.In2L3_orta,&log->In2L3_orta)) + return -1; + + if(set_float_ptr(args->io.In2L3_ortb,&log->In2L3_ortb)) + return -1; + + if(set_float_ptr(args->io.In2L3_esk,&log->In2L3_esk)) + return -1; + + if(set_float_ptr(args->io.Iy_orta,&log->Iy_orta)) + return -1; + + if(set_float_ptr(args->io.Iy_ortb,&log->Iy_ortb)) + return -1; + + if(set_float_ptr(args->io.Iy_esk,&log->Iy_esk)) + return -1; + + if(set_bit_ptr_struct(args->io.bl_in,&log->bl)) + return -1; + + if(set_bit_ptr_struct(args->io.test_in,&log->test)) + return -1; + + if(set_bit_ptr_struct(args->io.P_out,&log->P)) + return -1; + + if(set_bit_ptr_struct(args->io.W_out,&log->W)) + return -1; + + if(set_float_ptr(args->io.Id_float_out,&log->Id_out)) + return -1; + + if(set_float_ptr(args->io.Ir_float_out,&log->Ir_out)) + return -1; + + log->kamNa=args->params.kamN; + log->kamN2a=args->params.kamN2; + log->kamYa=args->params.kamY; + log->td_ms=(u32)(args->params.td * 1000); + + return 0; +} + +void zzr(void *arguments, void *logic) +{ + struct zzr_logic *log = (struct zzr_logic *)logic; + struct zzr_args *args = (struct zzr_args *)arguments; + float maxf,maxf2,Id_orta,Id_ortb,Isum_am_orta,Isum_am_ortb,Id,Ir,Isum_am2_orta,Isum_am2_ortb,Isum_am1_ortb,Isum_am1_orta,Ir1,Ir2; + u8 trig, untrig; + + if(!check_struct(&log->bl) && (args->params.bits & ZZR_AKT)) + { + Isum_am_orta=log->kamNa*(*log->InL1_orta + *log->InL2_orta + *log->InL3_orta); + Isum_am_ortb=log->kamNa*(*log->InL1_ortb + *log->InL2_ortb + *log->InL3_ortb); + Isum_am1_orta=Isum_am_orta; + Isum_am1_ortb=Isum_am_ortb; + + if (args->params.bits & ZZR_AT) + { + Isum_am2_orta=log->kamN2a*(*log->In2L1_orta + *log->In2L2_orta + *log->In2L3_orta); + Isum_am2_ortb=log->kamN2a*(*log->In2L1_ortb + *log->In2L2_ortb + *log->In2L3_ortb); + Isum_am_orta+=Isum_am2_orta; + Isum_am_ortb+=Isum_am2_ortb; + } + + Id_orta = Isum_am_orta + (log->kamYa * *log->Iy_orta); + Id_ortb = Isum_am_ortb + (log->kamYa * *log->Iy_ortb); + Id = ((Id_orta * Id_orta) + (Id_ortb*Id_ortb)) / 2; + Id = sqrtf(Id); + + switch((enum zzr_mode)args->params.mode) + { + case low_imped_ip_max: // 1 + maxf=*log->InL1_esk; + if(*log->InL2_esk>maxf) + maxf=*log->InL2_esk; + if(*log->InL3_esk>maxf) + maxf=*log->InL3_esk; + + maxf=sqrtf(maxf); + maxf*=log->kamNa; + + maxf2=0; + + if(args->params.bits & ZZR_AT) + { + maxf2=*log->In2L1_esk; + if(*log->In2L2_esk>maxf2) + maxf2=*log->In2L2_esk; + if(*log->In2L3_esk>maxf2) + maxf2=*log->In2L3_esk; + + maxf2=sqrtf(maxf2); + maxf2*=log->kamN2a; + } + + if(maxf2>maxf) + maxf=maxf2; + + + Ir=(sqrtf(*log->Iy_esk)*log->kamYa + maxf)*0.5; + + break; + case low_imped_sum_ip: // 0 + default: + + if (args->params.bits & ZZR_AT) + { + Ir1 = ((Isum_am1_orta * Isum_am1_orta) + (Isum_am1_ortb * Isum_am1_ortb)); + Ir2 = ((Isum_am2_orta * Isum_am2_orta) + (Isum_am2_ortb * Isum_am2_ortb)); + if(Ir2>Ir1) + Ir = Ir2 / 2; + else + Ir = Ir1 / 2; + } + else + Ir = ((Isum_am_orta * Isum_am_orta) + (Isum_am_ortb * Isum_am_ortb)) / 2; + + Ir = sqrtf(Ir); + break; + } + + *log->Ir_out=Ir; + *log->Id_out=Id; + + switch((enum zzr_characteristic)args->params.characteristic) + { + case linear_advanced: + if(Irparams.Ir_m2) + { + trig = (Id > args->params.Id_max) ? 1 : (Id > (args->params.Id_start + args->params.m1*Ir)) ? 1 : 0; + untrig = (Id <= (0.95*args->params.Id_max) && Id < 0.95*(args->params.Id_start + args->params.m1*Ir))?1:0; + } + else + { + float tmp; + tmp = args->params.Id_start + args->params.m1*args->params.Ir_m2; + trig = (Id > args->params.Id_max) ? 1 : (Id > (tmp + args->params.m2*(Ir-args->params.Ir_m2))) ? 1 : 0; + untrig = (Id <= (0.95*args->params.Id_max) && Id < 0.95*(tmp + args->params.m2*(Ir-args->params.Ir_m2)))?1:0; + } + + sprawdz_P( + &log->P_ch, + trig, + untrig, + &log->P_ch_cnt, + 20,3 + ); + + if (czy_test_R()) + { + log->P_ch = check_struct(&log->test)?1:0; + } + + if(log->P_ch) + { + set_struct(&log->P); + log->t_cnt+=LOOP_CYCLE_MS; + if(log->t_cnt>log->td_ms) + { + set_struct(&log->W); + log->t_cnt-=LOOP_CYCLE_MS; + } + } + else + { + clear_struct(&log->P); + clear_struct(&log->W); + log->t_cnt=20; + } + break; + + case linear_simple: + default: + trig = (Id > args->params.Id_max) ? 1 : (Id > (args->params.Id_start + args->params.m1*Ir)) ? 1 : 0; + untrig = (Id <= (0.95*args->params.Id_max) && Id < 0.95*(args->params.Id_start + args->params.m1*Ir))?1:0; + sprawdz_P( + &log->P_ch, + trig, + untrig, + &log->P_ch_cnt, + 3,3 + ); + + if (czy_test_R()) + { + log->P_ch = check_struct(&log->test)?1:0; + } + + if(log->P_ch) + { + set_struct(&log->P); + log->t_cnt+=LOOP_CYCLE_MS; + if(log->t_cnt>log->td_ms) + { + set_struct(&log->W); + log->t_cnt-=LOOP_CYCLE_MS; + } + } + else + { + clear_struct(&log->P); + clear_struct(&log->W); + log->t_cnt=0; + } + + break; + } + } + else + { + *log->Ir_out=0; + *log->Id_out=0; + log->t_cnt=0; + clear_struct(&log->P); + clear_struct(&log->W); + } + +} diff --git a/src/zzr.h b/src/zzr.h new file mode 100644 index 0000000..52a24fd --- /dev/null +++ b/src/zzr.h @@ -0,0 +1,133 @@ +/* + * zzr.h + * + * Created on: 11-07-2017 + * Author: Krzysztof Jakubczyk + */ + +#ifndef ZZR_H_ +#define ZZR_H_ + +#include "tdefs.h" +#include "config.h" + +#define ZZR_AKT 0x00000001 +#define ZZR_AT 0x00000002 + +enum zzr_mode +{ + low_imped_sum_ip, + low_imped_ip_max +}; + +enum zzr_characteristic +{ + linear_simple, + linear_advanced +}; + +struct zzr_logic +{ + struct binary_io bl; + struct binary_io test; + struct binary_io P; + struct binary_io W; + + float *InL1_orta; + float *InL1_ortb; + float *InL1_esk; + float *InL2_orta; + float *InL2_ortb; + float *InL2_esk; + float *InL3_orta; + float *InL3_ortb; + float *InL3_esk; + float *In2L1_orta; + float *In2L1_ortb; + float *In2L1_esk; + float *In2L2_orta; + float *In2L2_ortb; + float *In2L2_esk; + float *In2L3_orta; + float *In2L3_ortb; + float *In2L3_esk; + float *Iy_orta; + float *Iy_ortb; + float *Iy_esk; + + float *kamN_out; + float *kamY_out; + float *Id_out; + float *Ir_out; + + float kamNa; + float kamN2a; + float kamYa; + + u8 P_ch; + short P_ch_cnt; + + u32 td_ms; + u32 t_cnt; +}; + +struct zzr_io +{ + u32 bl_in; + u32 InL1_orta; + u32 InL1_ortb; + u32 InL1_esk; + u32 InL2_orta; + u32 InL2_ortb; + u32 InL2_esk; + u32 InL3_orta; + u32 InL3_ortb; + u32 InL3_esk; + + u32 In2L1_orta; + u32 In2L1_ortb; + u32 In2L1_esk; + u32 In2L2_orta; + u32 In2L2_ortb; + u32 In2L2_esk; + u32 In2L3_orta; + u32 In2L3_ortb; + u32 In2L3_esk; + + u32 Iy_orta; + u32 Iy_ortb; + u32 Iy_esk; + u32 test_in; + u32 Id_float_out; + u32 Ir_float_out; + u32 P_out; + u32 W_out; +}__attribute__((__packed__)); + +struct zzr_params +{ + u32 bits; + u32 mode; + double kamN; + double kamY; + u32 characteristic; + double Id_start; + double m1; + double m2; + double Ir_m2; + double Id_max; + double td; + double kamN2; +}__attribute__((__packed__)); + +struct zzr_args +{ + struct zzr_io io; + struct zzr_params params; +// u16 crc; +}__attribute__((__packed__)); + +extern void zzr(void *args, void *logic); +extern int zzr_initlog(void *arguments, void *logic); + +#endif /* ZZR_H_ */ diff --git a/stub_sdk/ti/sysbios/hal/Timer.h b/stub_sdk/ti/sysbios/hal/Timer.h new file mode 100644 index 0000000..637ce0b --- /dev/null +++ b/stub_sdk/ti/sysbios/hal/Timer.h @@ -0,0 +1 @@ +// Dummy file to satisfy #include diff --git a/stub_sdk/ti/sysbios/knl/Semaphore.h b/stub_sdk/ti/sysbios/knl/Semaphore.h new file mode 100644 index 0000000..587cb5c --- /dev/null +++ b/stub_sdk/ti/sysbios/knl/Semaphore.h @@ -0,0 +1,7 @@ +#ifndef SEMAPHORE_STUB_H +#define SEMAPHORE_STUB_H + +// Dummy type definition for Semaphore_Handle +typedef void* Semaphore_Handle; + +#endif // SEMAPHORE_STUB_H