From 20a0a62cc8ff5bfe14bce41f40e7ebc3eedbcead Mon Sep 17 00:00:00 2001 From: Razvalyaev Date: Fri, 17 Jan 2025 10:19:40 +0300 Subject: [PATCH] =?UTF-8?q?#3=20=D0=9F=D0=B5=D1=80=D0=B5=D1=81=D1=82=D1=80?= =?UTF-8?q?=D1=83=D0=BA=D1=82=D1=83=D1=80=D0=B8=D1=80=D0=BE=D0=B2=D0=B0?= =?UTF-8?q?=D0=BD=20=D0=BF=D1=80=D0=BE=D0=B5=D0=BA=D1=82,=20=D0=BD=D0=B0?= =?UTF-8?q?=D1=87=D0=B0=D1=82=D0=B0=20=D1=80=D0=B0=D0=B1=D0=BE=D1=82=D0=B0?= =?UTF-8?q?=20=D0=BD=D0=B0=D0=B4=20=D1=83=D0=BB=D1=83=D1=87=D1=88=D0=B5?= =?UTF-8?q?=D0=BD=D0=B8=D0=B5=D0=BC=20=D0=BC=D0=BE=D0=B4=D1=83=D0=BB=D1=8F?= =?UTF-8?q?=20=D0=A8=D0=98=D0=9C=20(open=20#4)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Папка app_wrapper содержит модули для управления приложением МК: инициализция, входы/выходы, вызов функций приложения - Папка xilinx_wrapper содержит модули для имитации xilinx (на будущее, хочу вытащить в отдельные sfunction) - Папка Src содержит исходный код приложения МК - В корне Inu файлы для запуска приложения (модулей app_wrapper) и run_bat для компиляции sfunction --- Inu/MCU.c | 4 +- Inu/Src2/551/VectorControl/abc_to_alphabeta.c | 23 - Inu/Src2/551/VectorControl/abc_to_alphabeta.h | 39 - Inu/Src2/551/VectorControl/abc_to_dq.c | 39 - Inu/Src2/551/VectorControl/abc_to_dq.h | 42 - Inu/Src2/551/VectorControl/alg_pll.c | 577 ---- Inu/Src2/551/VectorControl/alg_pll.h | 188 -- Inu/Src2/551/VectorControl/alphabeta_to_dq.c | 24 - Inu/Src2/551/VectorControl/alphabeta_to_dq.h | 32 - .../551/VectorControl/dq_to_alphabeta_cos.c | 39 - .../551/VectorControl/dq_to_alphabeta_cos.h | 40 - Inu/Src2/551/VectorControl/params_pll.h | 41 - Inu/Src2/551/VectorControl/regul_power.c | 88 - Inu/Src2/551/VectorControl/regul_power.h | 30 - Inu/Src2/551/VectorControl/regul_turns.c | 143 - Inu/Src2/551/VectorControl/regul_turns.h | 29 - Inu/Src2/551/VectorControl/smooth.c | 180 -- Inu/Src2/551/VectorControl/smooth.h | 78 - Inu/Src2/551/VectorControl/teta_calc.c | 91 - Inu/Src2/551/VectorControl/teta_calc.h | 36 - Inu/Src2/551/VectorControl/vector_control.c | 297 -- Inu/Src2/551/VectorControl/vector_control.h | 82 - Inu/Src2/551/main/281xEvTimersInit.c | 636 ---- Inu/Src2/551/main/281xEvTimersInit.h | 18 - Inu/Src2/551/main/CAN_project.h | 256 -- Inu/Src2/551/main/Main.c | 126 - Inu/Src2/551/main/PWMTMSHandle.c | 510 --- Inu/Src2/551/main/PWMTMSHandle.h | 25 - Inu/Src2/551/main/PWMTools.c | 2438 --------------- Inu/Src2/551/main/PWMTools.h | 54 - Inu/Src2/551/main/adc_internal.h | 33 - Inu/Src2/551/main/adc_tools.c | 1412 --------- Inu/Src2/551/main/adc_tools.h | 402 --- Inu/Src2/551/main/alarm_log.c | 196 -- Inu/Src2/551/main/alarm_log.h | 16 - Inu/Src2/551/main/alg_simple_scalar.c | 976 ------ Inu/Src2/551/main/alg_simple_scalar.h | 101 - Inu/Src2/551/main/alg_uf_const.c | 80 - Inu/Src2/551/main/alg_uf_const.h | 34 - Inu/Src2/551/main/another_bs.c | 458 --- Inu/Src2/551/main/another_bs.h | 21 - Inu/Src2/551/main/break_regul.c | 204 -- Inu/Src2/551/main/break_regul.h | 70 - Inu/Src2/551/main/calc_rms_vals.c | 265 -- Inu/Src2/551/main/calc_rms_vals.h | 66 - Inu/Src2/551/main/calc_tempers.c | 284 -- Inu/Src2/551/main/calc_tempers.h | 16 - Inu/Src2/551/main/can_bs2bs.c | 83 - Inu/Src2/551/main/can_bs2bs.h | 21 - Inu/Src2/551/main/can_protocol_ukss.h | 63 - Inu/Src2/551/main/control_station_project.c | 2493 --------------- Inu/Src2/551/main/control_station_project.h | 109 - Inu/Src2/551/main/detect_error_3_phase.c | 167 - Inu/Src2/551/main/detect_error_3_phase.h | 148 - Inu/Src2/551/main/detect_errors.c | 1606 ---------- Inu/Src2/551/main/detect_errors.h | 80 - Inu/Src2/551/main/detect_errors_adc.c | 310 -- Inu/Src2/551/main/detect_errors_adc.h | 45 - Inu/Src2/551/main/detect_overload.c | 92 - Inu/Src2/551/main/detect_overload.h | 32 - Inu/Src2/551/main/detect_phase_break.c | 112 - Inu/Src2/551/main/detect_phase_break.h | 36 - Inu/Src2/551/main/detect_phase_break2.c | 203 -- Inu/Src2/551/main/detect_phase_break2.h | 50 - Inu/Src2/551/main/digital_filters.c | 103 - Inu/Src2/551/main/digital_filters.h | 19 - Inu/Src2/551/main/edrk_main.c | 2736 ----------------- Inu/Src2/551/main/edrk_main.h | 1838 ----------- Inu/Src2/551/main/f281xbmsk.h | 244 -- Inu/Src2/551/main/f281xpwm.c | 288 -- Inu/Src2/551/main/f281xpwm.h | 163 - Inu/Src2/551/main/limit_lib.c | 50 - Inu/Src2/551/main/limit_lib.h | 15 - Inu/Src2/551/main/limit_power.c | 237 -- Inu/Src2/551/main/limit_power.h | 21 - Inu/Src2/551/main/logs_hmi.c | 1150 ------- Inu/Src2/551/main/logs_hmi.h | 86 - Inu/Src2/551/main/manch.h | 182 -- Inu/Src2/551/main/master_slave.c | 586 ---- Inu/Src2/551/main/master_slave.h | 34 - Inu/Src2/551/main/message2.c | 907 ------ Inu/Src2/551/main/message2.h | 33 - Inu/Src2/551/main/message2can.c | 278 -- Inu/Src2/551/main/message2can.h | 18 - Inu/Src2/551/main/message2test.c | 678 ---- Inu/Src2/551/main/message2test.h | 33 - Inu/Src2/551/main/message_modbus.c | 897 ------ Inu/Src2/551/main/message_modbus.h | 61 - Inu/Src2/551/main/message_terminals_can.c | 128 - Inu/Src2/551/main/message_terminals_can.h | 15 - Inu/Src2/551/main/modbus_hmi.c | 393 --- Inu/Src2/551/main/modbus_hmi.h | 39 - Inu/Src2/551/main/modbus_hmi_read.c | 222 -- Inu/Src2/551/main/modbus_hmi_read.h | 15 - Inu/Src2/551/main/modbus_hmi_update.c | 2022 ------------ Inu/Src2/551/main/modbus_hmi_update.h | 39 - Inu/Src2/551/main/modbus_svu_update.c | 710 ----- Inu/Src2/551/main/modbus_svu_update.h | 17 - Inu/Src2/551/main/not_use/log_to_mem.c | 249 -- Inu/Src2/551/main/not_use/log_to_mem.h | 201 -- Inu/Src2/551/main/optical_bus.c | 413 --- Inu/Src2/551/main/optical_bus.h | 109 - Inu/Src2/551/main/optical_bus_tools.c | 791 ----- Inu/Src2/551/main/optical_bus_tools.h | 14 - Inu/Src2/551/main/overheat_limit.c | 80 - Inu/Src2/551/main/overheat_limit.h | 13 - Inu/Src2/551/main/params.h | 182 -- Inu/Src2/551/main/params_alg.h | 218 -- Inu/Src2/551/main/params_bsu.h | 123 - Inu/Src2/551/main/params_hwp.h | 9 - Inu/Src2/551/main/params_motor.h | 59 - Inu/Src2/551/main/params_norma.h | 70 - Inu/Src2/551/main/params_protect_adc.h | 25 - Inu/Src2/551/main/params_pwm24.h | 48 - Inu/Src2/551/main/params_temper_p.h | 58 - Inu/Src2/551/main/pll_tools.c | 105 - Inu/Src2/551/main/pll_tools.h | 22 - Inu/Src2/551/main/project.c | 1049 ------- Inu/Src2/551/main/project.h | 74 - Inu/Src2/551/main/project_setup.h | 414 --- Inu/Src2/551/main/protect_levels.h | 119 - Inu/Src2/551/main/pump_control.c | 255 -- Inu/Src2/551/main/pump_control.h | 27 - Inu/Src2/551/main/pwm_logs.c | 476 --- Inu/Src2/551/main/pwm_logs.h | 16 - Inu/Src2/551/main/pwm_test_lines.c | 36 - Inu/Src2/551/main/pwm_test_lines.h | 63 - Inu/Src2/551/main/ramp_zadanie_tools.c | 417 --- Inu/Src2/551/main/ramp_zadanie_tools.h | 20 - Inu/Src2/551/main/sbor_shema.c | 1764 ----------- Inu/Src2/551/main/sbor_shema.h | 24 - Inu/Src2/551/main/sim_model.c | 222 -- Inu/Src2/551/main/sim_model.h | 21 - Inu/Src2/551/main/sync_tools.c | 520 ---- Inu/Src2/551/main/sync_tools.h | 113 - Inu/Src2/551/main/synhro_tools.c | 144 - Inu/Src2/551/main/synhro_tools.h | 19 - Inu/Src2/551/main/temper_p_tools.c | 77 - Inu/Src2/551/main/temper_p_tools.h | 22 - Inu/Src2/551/main/tk_Test.c | 369 --- Inu/Src2/551/main/tk_Test.h | 12 - Inu/Src2/551/main/ukss_tools.c | 605 ---- Inu/Src2/551/main/ukss_tools.h | 33 - Inu/Src2/551/main/uom_tools.c | 149 - Inu/Src2/551/main/uom_tools.h | 15 - Inu/Src2/551/main/v_pwm24_v2.c | 948 ------ Inu/Src2/551/main/v_pwm24_v2.h | 161 - Inu/Src2/551/main/v_rotor.c | 1095 ------- Inu/Src2/551/main/v_rotor.h | 185 -- Inu/Src2/551/main/v_rotor_22220.c | 666 ---- Inu/Src2/551/main/v_rotor_22220.h | 54 - Inu/Src2/551/main/vector.h | 254 -- Inu/Src2/551/main/xPlatesAddress.h | 21 - Inu/Src2/VectorControl/abc_to_alphabeta.c | 23 - Inu/Src2/VectorControl/abc_to_alphabeta.h | 39 - Inu/Src2/VectorControl/abc_to_dq.c | 38 - Inu/Src2/VectorControl/abc_to_dq.h | 42 - Inu/Src2/VectorControl/alphabeta_to_abc.c | 24 - Inu/Src2/VectorControl/alphabeta_to_abc.h | 38 - Inu/Src2/VectorControl/alphabeta_to_dq.c | 22 - Inu/Src2/VectorControl/alphabeta_to_dq.h | 32 - Inu/Src2/VectorControl/dq_to_alphabeta_cos.c | 39 - Inu/Src2/VectorControl/dq_to_alphabeta_cos.h | 40 - .../VectorControl/efficiency_compensation.c | 107 - .../VectorControl/efficiency_compensation.h | 13 - Inu/Src2/VectorControl/params_motor.h | 23 - Inu/Src2/VectorControl/pwm_vector_regul.c | 666 ---- Inu/Src2/VectorControl/pwm_vector_regul.h | 81 - Inu/Src2/VectorControl/regul_power.c | 269 -- Inu/Src2/VectorControl/regul_power.h | 32 - Inu/Src2/VectorControl/regul_turns.c | 185 -- Inu/Src2/VectorControl/regul_turns.h | 12 - Inu/Src2/VectorControl/teta_calc.c | 262 -- Inu/Src2/VectorControl/teta_calc.h | 39 - Inu/Src2/main/IQmathLib.c | 182 -- Inu/Src2/main/IQmathLib.h | 661 ---- Inu/Src2/main/PWMTools.c | 719 ----- Inu/Src2/main/PWMTools.h | 19 - Inu/Src2/main/adc_tools.c | 528 ---- Inu/Src2/main/adc_tools.h | 137 - Inu/Src2/main/dmctype.h | 31 - Inu/Src2/main/my_filter.c | 122 - Inu/Src2/main/my_filter.h | 76 - Inu/Src2/main/params.h | 181 -- Inu/Src2/main/pid_reg3.c | 107 - Inu/Src2/main/pid_reg3.h | 117 - Inu/Src2/main/rotation_speed.h | 44 - Inu/Src2/main/svgen_dq.h | 38 - Inu/Src2/main/svgen_dq_v2.c | 120 - Inu/Src2/main/v_pwm24.c | 1635 ---------- Inu/Src2/main/v_pwm24.h | 190 -- Inu/Src2/main/vector.h | 256 -- Inu/Src2/main/word_structurs.h | 64 - Inu/Src2/main/xp_write_xpwm_time.c | 361 --- Inu/Src2/main/xp_write_xpwm_time.h | 183 -- .../app_includes.h} | 2 +- .../init28335.c => app_wrapper/app_init.c} | 26 +- .../init28335.h => app_wrapper/app_init.h} | 3 +- .../param.c => app_wrapper/app_io.c} | 2 +- .../param.h => app_wrapper/app_io.h} | 0 .../app_wrapper.c} | 20 +- .../app_wrapper.h} | 1 - Inu/{main_matlab => app_wrapper}/def.h | 10 +- .../device_support/ReadMe.txt | 0 .../device_support/include/C28x_FPU_FastRTS.h | 0 .../device_support/include/DSP281x_Adc.h | 0 .../include/DSP281x_CpuTimers.h | 0 .../include/DSP281x_DefaultISR.h | 0 .../device_support/include/DSP281x_DevEmu.h | 0 .../device_support/include/DSP281x_Device.h | 0 .../device_support/include/DSP281x_ECan.h | 0 .../device_support/include/DSP281x_Ev.h | 0 .../device_support/include/DSP281x_Examples.h | 0 .../include/DSP281x_GlobalPrototypes.h | 0 .../device_support/include/DSP281x_Gpio.h | 0 .../device_support/include/DSP281x_Mcbsp.h | 0 .../device_support/include/DSP281x_PieCtrl.h | 0 .../device_support/include/DSP281x_PieVect.h | 0 .../include/DSP281x_SWPrioritizedIsrLevels.h | 0 .../device_support/include/DSP281x_Sci.h | 0 .../device_support/include/DSP281x_Spi.h | 0 .../device_support/include/DSP281x_SysCtrl.h | 0 .../device_support/include/DSP281x_XIntrupt.h | 0 .../device_support/include/DSP281x_Xintf.h | 0 .../device_support/include/DSP2833x_Adc.h | 0 .../include/DSP2833x_CpuTimers.h | 0 .../device_support/include/DSP2833x_DMA.h | 0 .../device_support/include/DSP2833x_DevEmu.h | 0 .../device_support/include/DSP2833x_Device.h | 0 .../device_support/include/DSP2833x_ECan.h | 0 .../device_support/include/DSP2833x_ECap.h | 0 .../device_support/include/DSP2833x_EPwm.h | 0 .../device_support/include/DSP2833x_EQep.h | 0 .../device_support/include/DSP2833x_Gpio.h | 0 .../device_support/include/DSP2833x_I2c.h | 0 .../device_support/include/DSP2833x_Mcbsp.h | 0 .../device_support/include/DSP2833x_PieCtrl.h | 0 .../device_support/include/DSP2833x_PieVect.h | 0 .../device_support/include/DSP2833x_Sci.h | 0 .../device_support/include/DSP2833x_Spi.h | 0 .../device_support/include/DSP2833x_SysCtrl.h | 0 .../include/DSP2833x_XIntrupt.h | 0 .../device_support/include/DSP2833x_Xintf.h | 0 .../device_support/include/IQmathLib.h | 0 .../include/SimSupport_GlobalPrototypes.h | 0 .../device_support/include/dmctype.h | 0 .../device_support/source/C28x_FPU_FastRTS.c | 0 .../source/C28x_FPU_FastRTS.obj | Bin .../source/DSP281x_GlobalVariableDefs.c | 0 .../source/DSP2833x_GlobalVariableDefs.c | 0 .../source/DSP2833x_GlobalVariableDefs.obj | Bin .../device_support/source/IQmathLib_matlab.c | 0 Inu/main_matlab/pwm_sim.c | 158 - Inu/mcu_wrapper.c | 22 +- Inu/mcu_wrapper_conf.h | 40 +- run_mex.bat => Inu/run_mex.bat | 28 +- Inu/{main_matlab => xilinx_wrapper}/adc_sim.c | 0 Inu/{main_matlab => xilinx_wrapper}/adc_sim.h | 0 Inu/xilinx_wrapper/pwm_sim.c | 281 ++ Inu/{main_matlab => xilinx_wrapper}/pwm_sim.h | 25 +- allmex.m | 20 +- inu_23550.slx | Bin 86468 -> 86559 bytes 262 files changed, 391 insertions(+), 52038 deletions(-) delete mode 100644 Inu/Src2/551/VectorControl/abc_to_alphabeta.c delete mode 100644 Inu/Src2/551/VectorControl/abc_to_alphabeta.h delete mode 100644 Inu/Src2/551/VectorControl/abc_to_dq.c delete mode 100644 Inu/Src2/551/VectorControl/abc_to_dq.h delete mode 100644 Inu/Src2/551/VectorControl/alg_pll.c delete mode 100644 Inu/Src2/551/VectorControl/alg_pll.h delete mode 100644 Inu/Src2/551/VectorControl/alphabeta_to_dq.c delete mode 100644 Inu/Src2/551/VectorControl/alphabeta_to_dq.h delete mode 100644 Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.c delete mode 100644 Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.h delete mode 100644 Inu/Src2/551/VectorControl/params_pll.h delete mode 100644 Inu/Src2/551/VectorControl/regul_power.c delete mode 100644 Inu/Src2/551/VectorControl/regul_power.h delete mode 100644 Inu/Src2/551/VectorControl/regul_turns.c delete mode 100644 Inu/Src2/551/VectorControl/regul_turns.h delete mode 100644 Inu/Src2/551/VectorControl/smooth.c delete mode 100644 Inu/Src2/551/VectorControl/smooth.h delete mode 100644 Inu/Src2/551/VectorControl/teta_calc.c delete mode 100644 Inu/Src2/551/VectorControl/teta_calc.h delete mode 100644 Inu/Src2/551/VectorControl/vector_control.c delete mode 100644 Inu/Src2/551/VectorControl/vector_control.h delete mode 100644 Inu/Src2/551/main/281xEvTimersInit.c delete mode 100644 Inu/Src2/551/main/281xEvTimersInit.h delete mode 100644 Inu/Src2/551/main/CAN_project.h delete mode 100644 Inu/Src2/551/main/Main.c delete mode 100644 Inu/Src2/551/main/PWMTMSHandle.c delete mode 100644 Inu/Src2/551/main/PWMTMSHandle.h delete mode 100644 Inu/Src2/551/main/PWMTools.c delete mode 100644 Inu/Src2/551/main/PWMTools.h delete mode 100644 Inu/Src2/551/main/adc_internal.h delete mode 100644 Inu/Src2/551/main/adc_tools.c delete mode 100644 Inu/Src2/551/main/adc_tools.h delete mode 100644 Inu/Src2/551/main/alarm_log.c delete mode 100644 Inu/Src2/551/main/alarm_log.h delete mode 100644 Inu/Src2/551/main/alg_simple_scalar.c delete mode 100644 Inu/Src2/551/main/alg_simple_scalar.h delete mode 100644 Inu/Src2/551/main/alg_uf_const.c delete mode 100644 Inu/Src2/551/main/alg_uf_const.h delete mode 100644 Inu/Src2/551/main/another_bs.c delete mode 100644 Inu/Src2/551/main/another_bs.h delete mode 100644 Inu/Src2/551/main/break_regul.c delete mode 100644 Inu/Src2/551/main/break_regul.h delete mode 100644 Inu/Src2/551/main/calc_rms_vals.c delete mode 100644 Inu/Src2/551/main/calc_rms_vals.h delete mode 100644 Inu/Src2/551/main/calc_tempers.c delete mode 100644 Inu/Src2/551/main/calc_tempers.h delete mode 100644 Inu/Src2/551/main/can_bs2bs.c delete mode 100644 Inu/Src2/551/main/can_bs2bs.h delete mode 100644 Inu/Src2/551/main/can_protocol_ukss.h delete mode 100644 Inu/Src2/551/main/control_station_project.c delete mode 100644 Inu/Src2/551/main/control_station_project.h delete mode 100644 Inu/Src2/551/main/detect_error_3_phase.c delete mode 100644 Inu/Src2/551/main/detect_error_3_phase.h delete mode 100644 Inu/Src2/551/main/detect_errors.c delete mode 100644 Inu/Src2/551/main/detect_errors.h delete mode 100644 Inu/Src2/551/main/detect_errors_adc.c delete mode 100644 Inu/Src2/551/main/detect_errors_adc.h delete mode 100644 Inu/Src2/551/main/detect_overload.c delete mode 100644 Inu/Src2/551/main/detect_overload.h delete mode 100644 Inu/Src2/551/main/detect_phase_break.c delete mode 100644 Inu/Src2/551/main/detect_phase_break.h delete mode 100644 Inu/Src2/551/main/detect_phase_break2.c delete mode 100644 Inu/Src2/551/main/detect_phase_break2.h delete mode 100644 Inu/Src2/551/main/digital_filters.c delete mode 100644 Inu/Src2/551/main/digital_filters.h delete mode 100644 Inu/Src2/551/main/edrk_main.c delete mode 100644 Inu/Src2/551/main/edrk_main.h delete mode 100644 Inu/Src2/551/main/f281xbmsk.h delete mode 100644 Inu/Src2/551/main/f281xpwm.c delete mode 100644 Inu/Src2/551/main/f281xpwm.h delete mode 100644 Inu/Src2/551/main/limit_lib.c delete mode 100644 Inu/Src2/551/main/limit_lib.h delete mode 100644 Inu/Src2/551/main/limit_power.c delete mode 100644 Inu/Src2/551/main/limit_power.h delete mode 100644 Inu/Src2/551/main/logs_hmi.c delete mode 100644 Inu/Src2/551/main/logs_hmi.h delete mode 100644 Inu/Src2/551/main/manch.h delete mode 100644 Inu/Src2/551/main/master_slave.c delete mode 100644 Inu/Src2/551/main/master_slave.h delete mode 100644 Inu/Src2/551/main/message2.c delete mode 100644 Inu/Src2/551/main/message2.h delete mode 100644 Inu/Src2/551/main/message2can.c delete mode 100644 Inu/Src2/551/main/message2can.h delete mode 100644 Inu/Src2/551/main/message2test.c delete mode 100644 Inu/Src2/551/main/message2test.h delete mode 100644 Inu/Src2/551/main/message_modbus.c delete mode 100644 Inu/Src2/551/main/message_modbus.h delete mode 100644 Inu/Src2/551/main/message_terminals_can.c delete mode 100644 Inu/Src2/551/main/message_terminals_can.h delete mode 100644 Inu/Src2/551/main/modbus_hmi.c delete mode 100644 Inu/Src2/551/main/modbus_hmi.h delete mode 100644 Inu/Src2/551/main/modbus_hmi_read.c delete mode 100644 Inu/Src2/551/main/modbus_hmi_read.h delete mode 100644 Inu/Src2/551/main/modbus_hmi_update.c delete mode 100644 Inu/Src2/551/main/modbus_hmi_update.h delete mode 100644 Inu/Src2/551/main/modbus_svu_update.c delete mode 100644 Inu/Src2/551/main/modbus_svu_update.h delete mode 100644 Inu/Src2/551/main/not_use/log_to_mem.c delete mode 100644 Inu/Src2/551/main/not_use/log_to_mem.h delete mode 100644 Inu/Src2/551/main/optical_bus.c delete mode 100644 Inu/Src2/551/main/optical_bus.h delete mode 100644 Inu/Src2/551/main/optical_bus_tools.c delete mode 100644 Inu/Src2/551/main/optical_bus_tools.h delete mode 100644 Inu/Src2/551/main/overheat_limit.c delete mode 100644 Inu/Src2/551/main/overheat_limit.h delete mode 100644 Inu/Src2/551/main/params.h delete mode 100644 Inu/Src2/551/main/params_alg.h delete mode 100644 Inu/Src2/551/main/params_bsu.h delete mode 100644 Inu/Src2/551/main/params_hwp.h delete mode 100644 Inu/Src2/551/main/params_motor.h delete mode 100644 Inu/Src2/551/main/params_norma.h delete mode 100644 Inu/Src2/551/main/params_protect_adc.h delete mode 100644 Inu/Src2/551/main/params_pwm24.h delete mode 100644 Inu/Src2/551/main/params_temper_p.h delete mode 100644 Inu/Src2/551/main/pll_tools.c delete mode 100644 Inu/Src2/551/main/pll_tools.h delete mode 100644 Inu/Src2/551/main/project.c delete mode 100644 Inu/Src2/551/main/project.h delete mode 100644 Inu/Src2/551/main/project_setup.h delete mode 100644 Inu/Src2/551/main/protect_levels.h delete mode 100644 Inu/Src2/551/main/pump_control.c delete mode 100644 Inu/Src2/551/main/pump_control.h delete mode 100644 Inu/Src2/551/main/pwm_logs.c delete mode 100644 Inu/Src2/551/main/pwm_logs.h delete mode 100644 Inu/Src2/551/main/pwm_test_lines.c delete mode 100644 Inu/Src2/551/main/pwm_test_lines.h delete mode 100644 Inu/Src2/551/main/ramp_zadanie_tools.c delete mode 100644 Inu/Src2/551/main/ramp_zadanie_tools.h delete mode 100644 Inu/Src2/551/main/sbor_shema.c delete mode 100644 Inu/Src2/551/main/sbor_shema.h delete mode 100644 Inu/Src2/551/main/sim_model.c delete mode 100644 Inu/Src2/551/main/sim_model.h delete mode 100644 Inu/Src2/551/main/sync_tools.c delete mode 100644 Inu/Src2/551/main/sync_tools.h delete mode 100644 Inu/Src2/551/main/synhro_tools.c delete mode 100644 Inu/Src2/551/main/synhro_tools.h delete mode 100644 Inu/Src2/551/main/temper_p_tools.c delete mode 100644 Inu/Src2/551/main/temper_p_tools.h delete mode 100644 Inu/Src2/551/main/tk_Test.c delete mode 100644 Inu/Src2/551/main/tk_Test.h delete mode 100644 Inu/Src2/551/main/ukss_tools.c delete mode 100644 Inu/Src2/551/main/ukss_tools.h delete mode 100644 Inu/Src2/551/main/uom_tools.c delete mode 100644 Inu/Src2/551/main/uom_tools.h delete mode 100644 Inu/Src2/551/main/v_pwm24_v2.c delete mode 100644 Inu/Src2/551/main/v_pwm24_v2.h delete mode 100644 Inu/Src2/551/main/v_rotor.c delete mode 100644 Inu/Src2/551/main/v_rotor.h delete mode 100644 Inu/Src2/551/main/v_rotor_22220.c delete mode 100644 Inu/Src2/551/main/v_rotor_22220.h delete mode 100644 Inu/Src2/551/main/vector.h delete mode 100644 Inu/Src2/551/main/xPlatesAddress.h delete mode 100644 Inu/Src2/VectorControl/abc_to_alphabeta.c delete mode 100644 Inu/Src2/VectorControl/abc_to_alphabeta.h delete mode 100644 Inu/Src2/VectorControl/abc_to_dq.c delete mode 100644 Inu/Src2/VectorControl/abc_to_dq.h delete mode 100644 Inu/Src2/VectorControl/alphabeta_to_abc.c delete mode 100644 Inu/Src2/VectorControl/alphabeta_to_abc.h delete mode 100644 Inu/Src2/VectorControl/alphabeta_to_dq.c delete mode 100644 Inu/Src2/VectorControl/alphabeta_to_dq.h delete mode 100644 Inu/Src2/VectorControl/dq_to_alphabeta_cos.c delete mode 100644 Inu/Src2/VectorControl/dq_to_alphabeta_cos.h delete mode 100644 Inu/Src2/VectorControl/efficiency_compensation.c delete mode 100644 Inu/Src2/VectorControl/efficiency_compensation.h delete mode 100644 Inu/Src2/VectorControl/params_motor.h delete mode 100644 Inu/Src2/VectorControl/pwm_vector_regul.c delete mode 100644 Inu/Src2/VectorControl/pwm_vector_regul.h delete mode 100644 Inu/Src2/VectorControl/regul_power.c delete mode 100644 Inu/Src2/VectorControl/regul_power.h delete mode 100644 Inu/Src2/VectorControl/regul_turns.c delete mode 100644 Inu/Src2/VectorControl/regul_turns.h delete mode 100644 Inu/Src2/VectorControl/teta_calc.c delete mode 100644 Inu/Src2/VectorControl/teta_calc.h delete mode 100644 Inu/Src2/main/IQmathLib.c delete mode 100644 Inu/Src2/main/IQmathLib.h delete mode 100644 Inu/Src2/main/PWMTools.c delete mode 100644 Inu/Src2/main/PWMTools.h delete mode 100644 Inu/Src2/main/adc_tools.c delete mode 100644 Inu/Src2/main/adc_tools.h delete mode 100644 Inu/Src2/main/dmctype.h delete mode 100644 Inu/Src2/main/my_filter.c delete mode 100644 Inu/Src2/main/my_filter.h delete mode 100644 Inu/Src2/main/params.h delete mode 100644 Inu/Src2/main/pid_reg3.c delete mode 100644 Inu/Src2/main/pid_reg3.h delete mode 100644 Inu/Src2/main/rotation_speed.h delete mode 100644 Inu/Src2/main/svgen_dq.h delete mode 100644 Inu/Src2/main/svgen_dq_v2.c delete mode 100644 Inu/Src2/main/v_pwm24.c delete mode 100644 Inu/Src2/main/v_pwm24.h delete mode 100644 Inu/Src2/main/vector.h delete mode 100644 Inu/Src2/main/word_structurs.h delete mode 100644 Inu/Src2/main/xp_write_xpwm_time.c delete mode 100644 Inu/Src2/main/xp_write_xpwm_time.h rename Inu/{mcu_app_includes.h => app_wrapper/app_includes.h} (96%) rename Inu/{main_matlab/init28335.c => app_wrapper/app_init.c} (94%) rename Inu/{main_matlab/init28335.h => app_wrapper/app_init.h} (83%) rename Inu/{main_matlab/param.c => app_wrapper/app_io.c} (99%) rename Inu/{main_matlab/param.h => app_wrapper/app_io.h} (100%) rename Inu/{main_matlab/main_matlab.c => app_wrapper/app_wrapper.c} (96%) rename Inu/{main_matlab/main_matlab.h => app_wrapper/app_wrapper.h} (76%) rename Inu/{main_matlab => app_wrapper}/def.h (96%) rename Inu/{main_matlab => app_wrapper}/device_support/ReadMe.txt (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/C28x_FPU_FastRTS.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Adc.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_CpuTimers.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_DefaultISR.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_DevEmu.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Device.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_ECan.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Ev.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Examples.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_GlobalPrototypes.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Gpio.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Mcbsp.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_PieCtrl.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_PieVect.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_SWPrioritizedIsrLevels.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Sci.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Spi.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_SysCtrl.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_XIntrupt.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP281x_Xintf.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_Adc.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_CpuTimers.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_DMA.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_DevEmu.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_Device.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_ECan.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_ECap.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_EPwm.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_EQep.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_Gpio.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_I2c.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_Mcbsp.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_PieCtrl.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_PieVect.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_Sci.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_Spi.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_SysCtrl.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_XIntrupt.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/DSP2833x_Xintf.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/IQmathLib.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/SimSupport_GlobalPrototypes.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/include/dmctype.h (100%) rename Inu/{main_matlab => app_wrapper}/device_support/source/C28x_FPU_FastRTS.c (100%) rename Inu/{main_matlab => app_wrapper}/device_support/source/C28x_FPU_FastRTS.obj (100%) rename Inu/{main_matlab => app_wrapper}/device_support/source/DSP281x_GlobalVariableDefs.c (100%) rename Inu/{main_matlab => app_wrapper}/device_support/source/DSP2833x_GlobalVariableDefs.c (100%) rename Inu/{main_matlab => app_wrapper}/device_support/source/DSP2833x_GlobalVariableDefs.obj (100%) rename Inu/{main_matlab => app_wrapper}/device_support/source/IQmathLib_matlab.c (100%) delete mode 100644 Inu/main_matlab/pwm_sim.c rename run_mex.bat => Inu/run_mex.bat (84%) rename Inu/{main_matlab => xilinx_wrapper}/adc_sim.c (100%) rename Inu/{main_matlab => xilinx_wrapper}/adc_sim.h (100%) create mode 100644 Inu/xilinx_wrapper/pwm_sim.c rename Inu/{main_matlab => xilinx_wrapper}/pwm_sim.h (77%) diff --git a/Inu/MCU.c b/Inu/MCU.c index 2060518..6cca00c 100644 --- a/Inu/MCU.c +++ b/Inu/MCU.c @@ -167,10 +167,10 @@ static void mdlStart(SimStruct *S) static void mdlInitializeSampleTimes(SimStruct *S) { // Шаг дискретизации - hmcu.SimSampleTime = mxGetPr(ssGetSFcnParam(S,NPARAMS-1))[0]; + hmcu.sSimSampleTime = mxGetPr(ssGetSFcnParam(S,NPARAMS-1))[0]; // Register one pair for each sample time - ssSetSampleTime(S, 0, hmcu.SimSampleTime); + ssSetSampleTime(S, 0, hmcu.sSimSampleTime); ssSetOffsetTime(S, 0, 0.0); } diff --git a/Inu/Src2/551/VectorControl/abc_to_alphabeta.c b/Inu/Src2/551/VectorControl/abc_to_alphabeta.c deleted file mode 100644 index e0ea922..0000000 --- a/Inu/Src2/551/VectorControl/abc_to_alphabeta.c +++ /dev/null @@ -1,23 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library -#include "abc_to_alphabeta.h" - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(abc_to_alphabeta_calc,".fast_run"); -void abc_to_alphabeta_calc(ABC_TO_ALPHABETA *v) -{ - static _iq iq_1_sqrt3 = _IQ(0.57735026918962576450914878050196); // = 1/sqrt(3) - static _iq iq_2_sqrt3 = _IQ(1.1547005383792515290182975610039); // =2/sqrt(3) - - v->Ualpha = v->Ua; - v->Ubeta = _IQmpy(iq_1_sqrt3,v->Ua) + _IQmpy(iq_2_sqrt3,v->Ub); - -} - - - -///////////////////////////////////////////////// diff --git a/Inu/Src2/551/VectorControl/abc_to_alphabeta.h b/Inu/Src2/551/VectorControl/abc_to_alphabeta.h deleted file mode 100644 index 8d3f23b..0000000 --- a/Inu/Src2/551/VectorControl/abc_to_alphabeta.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef __ABC_ALPHABETA_H__ -#define __ABC_ALPHABETA_H__ - - - -typedef struct { _iq Ua; //phase A voltage, input - _iq Ub; //phase B voltage, input - _iq Uc; //phase C voltage, input -// _iq Tetta; //phase angle, input - _iq Ualpha; // axis d voltage, output - _iq Ubeta; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - }ABC_TO_ALPHABETA; - - - - - -typedef ABC_TO_ALPHABETA *ABC_TO_ALPHABETA_handle; - -#define ABC_TO_ALPHABETA_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(unsigned long))abc_to_alphabeta_calc\ - } - - -void abc_to_alphabeta_calc(ABC_TO_ALPHABETA_handle); - - - - - - - - -#endif // end __ABC_ALPHABETA_H diff --git a/Inu/Src2/551/VectorControl/abc_to_dq.c b/Inu/Src2/551/VectorControl/abc_to_dq.c deleted file mode 100644 index 8044f2c..0000000 --- a/Inu/Src2/551/VectorControl/abc_to_dq.c +++ /dev/null @@ -1,39 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library -#include "abc_to_dq.h" - - - - - - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(abc_to_dq_calc,".fast_run"); -void abc_to_dq_calc(ABC_TO_DQ *v) -{ - static _iq iq_two_third_pi = _IQ(6.283185307179586476925286766559/3.0); - static _iq iq_two_third = _IQ(2.0/3.0); - - v->Id = _IQmpy(iq_two_third,_IQmpy(v->Ia, _IQsin(v->Tetta)) + _IQmpy(v->Ib, _IQsin(v->Tetta - iq_two_third_pi)) + _IQmpy(v->Ic, _IQsin(v->Tetta + iq_two_third_pi))); - v->Iq = _IQmpy(iq_two_third,_IQmpy(v->Ia, _IQcos(v->Tetta)) + _IQmpy(v->Ib, _IQcos(v->Tetta - iq_two_third_pi)) + _IQmpy(v->Ic, _IQcos(v->Tetta + iq_two_third_pi))); -} - - -#pragma CODE_SECTION(abc_to_dq_calc_v2,".fast_run"); -void abc_to_dq_calc_v2(ABC_TO_DQ *v) -{ - static _iq iq_two_third_pi = _IQ(6.283185307179586476925286766559/3.0); - static _iq iq_two_third = _IQ(2.0/3.0); - - v->Id = _IQmpy(iq_two_third,_IQmpy(v->Ia, _IQcos(v->Tetta)) + _IQmpy(v->Ib, _IQcos(v->Tetta - iq_two_third_pi)) + _IQmpy(v->Ic, _IQcos(v->Tetta + iq_two_third_pi))); - v->Iq = _IQmpy(iq_two_third,_IQmpy(-v->Ia, _IQsin(v->Tetta)) - _IQmpy(v->Ib, _IQcos(v->Tetta - iq_two_third_pi)) - _IQmpy(v->Ic, _IQcos(v->Tetta + iq_two_third_pi))); -} - - -///////////////////////////////////////////////// - - diff --git a/Inu/Src2/551/VectorControl/abc_to_dq.h b/Inu/Src2/551/VectorControl/abc_to_dq.h deleted file mode 100644 index 57e3de6..0000000 --- a/Inu/Src2/551/VectorControl/abc_to_dq.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef __ABC_DQ_H__ -#define __ABC_DQ_H__ - - - -typedef struct { _iq Ia; //phase A voltage, input - _iq Ib; //phase B voltage, input - _iq Ic; //phase C voltage, input - _iq Tetta; //phase angle, input - _iq Id; // axis d voltage, output - _iq Iq; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - void (*calc_v2)(); // Pointer to calculation function - }ABC_TO_DQ; - - - - - -typedef ABC_TO_DQ *ABC_TO_DQ_handle; - -#define ABC_TO_DQ_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(Uint32))abc_to_dq_calc, \ - (void (*)(Uint32))abc_to_dq_calc_v2 } - - -void abc_to_dq_calc(ABC_TO_DQ_handle); -void abc_to_dq_calc_v2(ABC_TO_DQ_handle); - - - - - - - - -#endif // end __ABC_DQ_H diff --git a/Inu/Src2/551/VectorControl/alg_pll.c b/Inu/Src2/551/VectorControl/alg_pll.c deleted file mode 100644 index b7f3a9c..0000000 --- a/Inu/Src2/551/VectorControl/alg_pll.c +++ /dev/null @@ -1,577 +0,0 @@ -#include "IQmathLib.h" -#include "alg_pll.h" -#include "params_pll.h" - -#include "params_norma.h" - -//#define NORMA_ACP 3000 -//#define FREQ_PWM_VIPR 1975 - -//#define SIZE_PLL_AVG 50 - -//_iq w_in_avg[SIZE_PLL_AVG]; -//_iq w_out_avg[SIZE_PLL_AVG]; - -#define DETECT_PLL_D (2000.0/NORMA_ACP) // ampl -#define DETECT_PLL_Q (500.0/NORMA_ACP) // zero - -_iq iqDetect_PLL_d=_IQ(DETECT_PLL_D); -_iq iqDetect_PLL_q=_IQ(DETECT_PLL_Q); - -#define MAX_COUNT_ERR_PLL 5000 //20 - -#ifdef USE_SMOOTH_FOR_CALC_WC -SMOOTH mysmooth=SMOOTH_DEFAULTS; -#endif - - -// -/////////////////////////////////////////// -//PLL_REC pll2 = PLL_REC_DEFAULT; -/////////////////////////////////////////// - - -ABC_TO_ALPHABETA abc_to_alphabeta_u_input = ABC_TO_ALPHABETA_DEFAULTS; -ALPHABETA_TO_DQ alphabeta_to_dq_u_input = ALPHABETA_TO_DQ_DEFAULTS; -PIDREG3 pidUdq = PIDREG3_DEFAULTS; - -//int count_wait_pll=0; -int count_err_pll = 0; - - -//int c_start=0; -//int c_stop=0; - -#define MAX_TIME_WAIT_PLL 5000 - - -//_iq iqUab=0, iqUbc=0, iqUca=0; - - -//_iq koef_Um_filter = _IQ(0.000125/0.09); - - -//_iq koef_AddIq_minus_1_filter = _IQ(0.00034/0.009);//9576244354248046875 - -#pragma CODE_SECTION(minus_plus_2_pi,".fast_run"); -_iq minus_plus_2_pi(_iq a) -{ - - while (a>=CONST_IQ_2PI) - a -= CONST_IQ_2PI; - - while (a<=-CONST_IQ_2PI) - a += CONST_IQ_2PI; - - return a; -} - - -#pragma CODE_SECTION(minus_plus_2_pi_v2,".fast_run"); -_iq minus_plus_2_pi_v2(_iq a) -{ - - while (a>=CONST_IQ_2PI) - a -= CONST_IQ_2PI; - - while (a<0) - a += CONST_IQ_2PI; - - return a; -} - - - -#pragma CODE_SECTION(AB_BC_CA_To_ABC,".fast_run"); -void AB_BC_CA_To_ABC(_iq U_AB, _iq U_BC, _iq U_CA, _iq *Ua, _iq *Ub, _iq *Uc) -{ -// static _iq c2 = _IQ(2.0); - -// static _iq c13_sqrt = _IQ(1.7320508075688772935274463415059 / 3.0); - - *Ua = U_AB; - *Ub = U_BC; - *Uc = U_CA; -/* - - - *Ua = _IQmpy(c13_sqrt, (_IQmpy(c2,U_AB)+U_BC));// 2*U_AB/3+U_BC/3; - *Ub = _IQmpy(c13_sqrt, (_IQmpy(c2,U_BC)+U_CA));// 2*U_BC/3+U_CA/3; - *Uc = _IQmpy(c13_sqrt, (_IQmpy(c2,U_CA)+U_AB));// 2*U_CA/3+U_AB/3; -*/ -} - - - - -///////////////////////////////////////////////// -#pragma CODE_SECTION(PLLController,".fast_run"); -///////////////////////////////////////////////// -void PLLController(PLL_REC *v) -{ - static unsigned int count_wait_pll=0; - static _iq prev_Tetta_z=0; - _iq Tetta_z_t=0; - static int prev_flag_find_pll = 0; - static int flag_reset_Tetta_p = 0; -// static _iq prev_Tetta_v2 = 0; - - - v->vars.Uab = v->input.Input_U_AB - v->vars.iqZeroUAB; - v->vars.Ubc = v->input.Input_U_BC - v->vars.iqZeroUBC; - v->vars.Uca = v->input.Input_U_CA - v->vars.iqZeroUCA; - - v->vars.sum_zeroU_AB_BC_CA = v->vars.Uab + v->vars.Ubc + v->vars.Uca; - - if (v->setup.rotation_u_cba) - { - AB_BC_CA_To_ABC(v->vars.Uab, v->vars.Uca, v->vars.Ubc, &v->vars.Ua, &v->vars.Ub, &v->vars.Uc); - } - else - { - AB_BC_CA_To_ABC(v->vars.Uab,v->vars.Ubc,v->vars.Uca, &v->vars.Ua, &v->vars.Ub, &v->vars.Uc); - } -#ifdef ROTATION_U_CBA - -#else - -#endif - - v->vars.sum_zeroU_A_B_C = v->vars.Ua + v->vars.Ub + v->vars.Uc; - - abc_to_alphabeta_u_input.Ua = v->vars.Ua; - abc_to_alphabeta_u_input.Ub = v->vars.Ub; - abc_to_alphabeta_u_input.Uc = v->vars.Uc; - - abc_to_alphabeta_u_input.calc(&abc_to_alphabeta_u_input); - - - v->vars.Ualpha = abc_to_alphabeta_u_input.Ualpha; - v->vars.Ubeta = abc_to_alphabeta_u_input.Ubeta; - - - alphabeta_to_dq_u_input.Ualpha = v->vars.Ualpha; - alphabeta_to_dq_u_input.Ubeta = v->vars.Ubeta; - alphabeta_to_dq_u_input.Tetta = v->vars.Tetta; - - alphabeta_to_dq_u_input.calc(&alphabeta_to_dq_u_input); - - - v->vars.pll_Ud = alphabeta_to_dq_u_input.Ud; - v->vars.pll_Uq = alphabeta_to_dq_u_input.Uq; - - -// pidUdq.Fdb = v->pll_Ud;//.pll_Uq; //err = Ref - Fdb -// pidUdq.Ref = 0; - - pidUdq.Ref = v->vars.pll_Uq; //err = Ref - Fdb - pidUdq.Fdb = 0; - - pidUdq.calc(&pidUdq); - v->vars.wc = pidUdq.Out; - - - if (prev_flag_find_pll==0) - { - flag_reset_Tetta_p = 0; - } - - if (v->output.flag_find_pll) - { -#ifdef USE_SMOOTH_FOR_CALC_WC - mysmooth.input = _IQtoIQ23(v->vars.wc); - mysmooth.add(&mysmooth); - mysmooth.calc(&mysmooth); - v->vars.w_shtrih = _IQ23toIQ(mysmooth.av); -#else - v->vars.w_shtrih = v->vars.wc; -#endif - } - else - { - v->vars.w_shtrih = v->vars.wc; - } - - - v->vars.Tetta += v->vars.wc; - v->vars.Tetta = minus_plus_2_pi(v->vars.Tetta); // +- 2PI - - if (v->output.flag_find_pll) - { - v->vars.dwc = v->vars.wc - v->vars.w_shtrih; - - v->vars.Tetta_p += v->vars.dwc; - v->vars.Tetta_p = minus_plus_2_pi(v->vars.Tetta_p); // +- 2PI - - v->vars.dTetta = v->vars.Tetta - v->vars.Tetta_p;// + iq_05Pi; - - v->vars.dTetta = minus_plus_2_pi(v->vars.dTetta); // +- 2PI - - v->vars.Tetta_z = v->vars.dTetta; - -// if (v->Tetta_z>=iq_05Pi && prev_Tetta_zTetta_p = 0; - - Tetta_z_t = minus_plus_2_pi_v2(v->vars.Tetta_z); - - if ( (Tetta_z_t>=0) && (Tetta_z_t(CONST_IQ_2PI-CONST_IQ_PI05)) ) ) - { - if (flag_reset_Tetta_p==0) - { - v->vars.Tetta_p = 0; -// flag_reset_Tetta_p = 1; - } - } - - - prev_Tetta_z = Tetta_z_t; - -#ifdef USE_FILTER_TETTA -//use filter teta - v->vars.Tetta_v2 = v->vars.Tetta_z;//v->Tetta; -#else -//use mgnoven teta - v->vars.Tetta_v2 = v->vars.Tetta; -#endif - v->vars.delta_Tetta_c = v->vars.Tetta_z - v->vars.Tetta; - -// prev_Tetta_v2 = v->Tetta_v2; - - } - else - { - v->vars.Tetta_v2 = v->vars.Tetta; - flag_reset_Tetta_p = 0; - } - - -// PLL OK? -//count_wait_pll=0 -//new alg find pll - if (v->vars.w_shtrih >= v->vars.find_min_w_strih && v->vars.w_shtrih <= v->vars.find_max_w_strih) - { - if (v->vars.count_wait_pll_w_shtrih < v->vars.max_time_wait_pll_w_strih) - v->vars.count_wait_pll_w_shtrih++; - } - else - { - if (v->vars.count_wait_pll_w_shtrih>0) - v->vars.count_wait_pll_w_shtrih--; - } - - if (v->vars.count_wait_pll_w_shtrih == v->vars.max_time_wait_pll_w_strih) - v->output.flag_find_pll = 1; - - if (v->vars.count_wait_pll_w_shtrih == 0) - v->output.flag_find_pll = 0; - -//end new alg find pll - - - - if ( (_IQabs(v->vars.pll_Uq)<=_IQabs(iqDetect_PLL_q)) // zero - && (_IQabs(v->vars.pll_Ud)>=_IQabs(iqDetect_PLL_d) ) //ampl - ) - { - count_err_pll=0; - if (count_wait_plloutput.flag_find_pll = 1; - } - else - { - if (count_err_pll>=MAX_COUNT_ERR_PLL) - { - // fail find pll - count_wait_pll=0; - - v->output.flag_find_pll = 0; - - if (v->output.flag_find_pll==1) - { - v->vars.pll_Uq = 0; - v->vars.pll_Ud = 0; - } - - } - else - { - count_err_pll++; - if ((v->output.flag_find_pll==0) && (count_wait_pll>0)) - count_wait_pll--; - } - } - -// end PLL Ok? - - - v->vars.pi_teta_u_out = pidUdq.Out; - v->vars.pi_teta_u_i = pidUdq.Ui; - v->vars.pi_teta_u_p = pidUdq.Up; - - - prev_flag_find_pll = v->output.flag_find_pll; -} - -////////////////////////////////////////////////// -////////////////////////////////////////////////// -////////////////////////////////////////////////// -///////////////////////////////////////////////// -//#pragma CODE_SECTION(pll_get_freq,".fast_run"); -///////////////////////////////////////////////// -void pll_get_freq_float(PLL_REC *v) -{ - - if (v->output.flag_find_pll) - { - v->output.int_freq_net = _IQtoF( v->vars.w_shtrih) * v->setup.freq_run_pll / PI * 50.00; // freq*100 - } - else - { - v->output.int_freq_net = 0; - } - -} - -////////////////////////////////////////////////// -////////////////////////////////////////////////// -void pll_get_freq_iq(PLL_REC *v) -{ - - if (v->output.flag_find_pll) - { - v->output.iq_freq_net = v->vars.w_shtrih;//_IQtoF( v->vars.w_shtrih) * v->setup.freq_run_pll / PI * 50.00; // freq*100 - } - else - { - v->output.iq_freq_net = 0; - } - -} - -////////////////////////////////////////////////// -//#pragma CODE_SECTION(detect_phase_count,".fast_run"); -void detect_phase_count(PLL_REC *v) -{ - static _iq prev_Ua = 0; - static int prev_flag_find_pll=0; - - - -// abc_to_dq.Ua - - if ((v->output.flag_find_pll != prev_flag_find_pll) && (v->output.flag_find_pll == 1)) - { - prev_Ua = v->vars.Ua; - v->vars.enable_detect_phase_count = 1; - v->vars.error_phase_count = 0; - } - - - if (v->output.flag_find_pll==0) - v->vars.enable_detect_phase_count = 0; - - - if (v->output.flag_find_pll) - { - if (v->vars.enable_detect_phase_count) - { - if ( (prev_Ua<0) && (v->vars.Ua>=0) ) - { - - if (v->vars.Uc > v->vars.Ub) - v->vars.enable_detect_phase_count = 0; - - if (v->vars.Ub > v->vars.Uc) - { - v->vars.enable_detect_phase_count = 0; - v->vars.error_phase_count = 1; - } - } - } - } - - - prev_flag_find_pll = v->output.flag_find_pll; - prev_Ua = v->vars.Ua; - -} -///////////////////////////////////////////////// - - -//////////////////////////////////////////////// - -#pragma CODE_SECTION(Find_zero_Uabc,".fast_run"); -void Find_zero_Uabc(PLL_REC *v) -{ - static int c_a=0; -// static int c_b=0; -// static int c_c=0; - - static _iq sum_a=0; - static _iq sum_b=0; - static _iq sum_c=0; - - _iq22 sum_t,c_t; - - _iq22 sum_div; - - -// AB_BC_CA_To_ABC(analog.iqUin_AB-iqZeroUABC, analog.iqUin_BC-iqZeroUABC, analog.iqUin_CA-iqZeroUABC); - - - sum_a += v->input.Input_U_AB; // analog.iqUin_AB; - sum_b += v->input.Input_U_BC; // analog.iqUin_BC; - sum_c += v->input.Input_U_CA; // analog.iqUin_CA; - - c_a++; - - if (c_a >= v->vars.count_sum_find_zero_uabc) - { - sum_div = v->vars.sum_div_find_zero_uabc; - - sum_t = _IQtoIQ22(sum_a); - c_t = _IQ22div(sum_t,sum_div); - v->vars.iqZeroUAB = _IQ22toIQ(c_t); - - sum_t = _IQtoIQ22(sum_b); - c_t = _IQ22div(sum_t,sum_div); - v->vars.iqZeroUBC = _IQ22toIQ(c_t); - - sum_t = _IQtoIQ22(sum_c); - c_t = _IQ22div(sum_t,sum_div); - v->vars.iqZeroUCA = _IQ22toIQ(c_t); - - sum_a = 0; - sum_b = 0; - sum_c = 0; - c_a = 0; - } - -} - - - - - -void pll_init(PLL_REC *v) -{ - v->output.status = STATUS_PLL_NOT_INITED; - - abc_to_alphabeta_u_input.Ua = 0; - abc_to_alphabeta_u_input.Ub = 0; - abc_to_alphabeta_u_input.Uc = 0; - abc_to_alphabeta_u_input.Ualpha = 0; - abc_to_alphabeta_u_input.Ubeta = 0; - - alphabeta_to_dq_u_input.Tetta = 0; - alphabeta_to_dq_u_input.Ualpha = 0; - alphabeta_to_dq_u_input.Ubeta = 0; - alphabeta_to_dq_u_input.Ud = 0; - alphabeta_to_dq_u_input.Uq = 0; - - v->vars.count_wait_pll_w_shtrih = 0; - - v->vars.pll_Ud = 0; - v->vars.pll_Uq = 0; - v->vars.Tetta = 0; - v->vars.Tetta_p = 0; - - v->vars.Ua = 0; - v->vars.Ub = 0; - v->vars.Uc = 0; - - v->vars.Ualpha = 0; - v->vars.Ubeta = 0; - -// count_wait_pll = 0; - count_err_pll = 0; - v->output.flag_find_pll = 0; - - pidUdq.Kp = _IQ(v->setup.pid_kp_pll); - pidUdq.Ki = _IQ(v->setup.pid_ki_pll); - - pidUdq.Kc = _IQ(PID_KC_PLL); - pidUdq.Kd = _IQ(0.0); - - pidUdq.OutMax = _IQ(K_PLL_MAX); - pidUdq.OutMin = _IQ(K_PLL_MIN); - - pidUdq.Err = 0; - pidUdq.Out = 0; - pidUdq.OutPreSat = 0; - pidUdq.SatErr = 0; - - if (v->setup.freq_run_pll == 0) - v->setup.freq_run_pll = DEFAULT_FREQ_RUN_PLL; - - pidUdq.Ui = _IQ(2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll)); - -// iqDetect_PLL_d = iqDetect_PLL_d; -// iqDetect_PLL_q = iqDetect_PLL_q; - -#ifdef USE_SMOOTH_FOR_CALC_WC - mysmooth.init(&mysmooth); -#endif - - - v->vars.count_sum_find_zero_uabc = v->setup.freq_run_pll/DEFAULT_FREQ_NET; //79*2 //1975/50*2 - v->vars.sum_div_find_zero_uabc = _IQ22(v->vars.count_sum_find_zero_uabc); - - v->vars.find_max_w_strih = _IQ(FIND_MAX_W_STRIH*2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll)); - v->vars.find_min_w_strih = _IQ(FIND_MIN_W_STRIH*2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll)); - - v->vars.max_time_wait_pll_w_strih = (v->vars.count_sum_find_zero_uabc * MAX_PERIOD_WAIT_PLL_W_SHTRIH); - - v->output.status = STATUS_PLL_INITED; - -} - - - - -#pragma CODE_SECTION(read_error_find_pll,".fast_run2"); -int read_error_find_pll(PLL_REC *v) -{ -// static int enable_detect_pll_err=0; -// static int prev_flag_find_pll=0; - static int err_pll=0; - - - err_pll = 0; -/* - if ((v->output.flag_find_pll!=prev_flag_find_pll) - && (v->output.flag_find_pll==1)) - { - enable_detect_pll_err = 1; - } - prev_flag_find_pll = v->output.flag_find_pll; - - if (v->input.enable_find_pll==0) - enable_detect_pll_err = 0; - - if ((enable_detect_pll_err) && (v->output.flag_find_pll==0) && (v->input.enable_find_pll==1)) - { - err_pll = 1; - } -*/ - return err_pll; -} - - - - -#pragma CODE_SECTION(pll_calc,".fast_run"); -void pll_calc(PLL_REC *v) -{ - if (v->output.status >= STATUS_PLL_INITED) - { - Find_zero_Uabc(v); - PLLController(v); -// detect_phase_count(v); - v->output.status = STATUS_PLL_OK; - } - -} diff --git a/Inu/Src2/551/VectorControl/alg_pll.h b/Inu/Src2/551/VectorControl/alg_pll.h deleted file mode 100644 index 0a63dde..0000000 --- a/Inu/Src2/551/VectorControl/alg_pll.h +++ /dev/null @@ -1,188 +0,0 @@ -#ifndef __ALG_PLL_H__ -#define __ALG_PLL_H__ - -#include "IQmathLib.h" -#include "math_pi.h" -#include "pid_reg3.h" -#include "abc_to_alphabeta.h" -#include "alphabeta_to_dq.h" -#include "smooth.h" -#include "smooth.h" - - -#define DEFAULT_FREQ_NET 50.00 // Hz - -#define DEFAULT_FREQ_RUN_PLL 4000 // Hz - -#define DEFAULT_PID_KP_PLL 0.0375 -#define DEFAULT_PID_KI_PLL 0.0128 - -#define STATUS_PLL_OK 10 - -#define STATUS_PLL_ERROR 2 -#define STATUS_PLL_INITED 1 -#define STATUS_PLL_NOT_INITED 0 - -#define FIND_MAX_W_STRIH 1.5 //0.12 //75Hz -#define FIND_MIN_W_STRIH 0.5 //0.045 //33Hz - -#define MAX_PERIOD_WAIT_PLL_W_SHTRIH 3 - - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// - -typedef struct { - float pid_kp_pll; // . Kp - float pid_ki_pll; // . Ki - int freq_run_pll; // , . - int rotation_u_cba; // : 0 - A-B-C, 1 - A-C-B - } PLL_SETUP; - -#define PLL_SETUP_DEFAULT {DEFAULT_PID_KP_PLL, DEFAULT_PID_KI_PLL, DEFAULT_FREQ_RUN_PLL,0} - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// - -typedef struct { _iq Input_U_AB; // Uab - _iq Input_U_BC; // Ubc - _iq Input_U_CA; // Uca - } PLL_INPUT; - -#define PLL_INPUT_DEFAULT {0, 0, 0} - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// - -typedef struct { int flag_find_pll; - int int_freq_net; - _iq iq_freq_net; - int status; - } PLL_OUTPUT; - -#define PLL_OUTPUT_DEFAULT {0, 0, 0, STATUS_PLL_NOT_INITED} - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// - -typedef struct { - int enable_detect_phase_count; - int error_phase_count; - - _iq pll_Ud; - _iq pll_Uq; - _iq Tetta; - _iq Tetta_v2; - - _iq wc; - _iq dwc; - _iq w_shtrih; - - _iq Tetta_z; - _iq Tetta_p; - _iq dTetta; - - _iq zeroPLL; - _iq pi_teta_u_out; - _iq pi_teta_u_p; - _iq pi_teta_u_i; - _iq add_teta; - _iq add_teta2; - - _iq Ua; - _iq Ub; - _iq Uc; - - - _iq Uab; - _iq Ubc; - _iq Uca; - - _iq Ualpha; - _iq Ubeta; - - _iq iqZeroUAB; - _iq iqZeroUBC; - _iq iqZeroUCA; - - _iq sum_zeroU_AB_BC_CA; - _iq sum_zeroU_A_B_C; - - _iq delta_Tetta_c; - _iq22 sum_div_find_zero_uabc; - int count_sum_find_zero_uabc; - - _iq find_max_w_strih; - _iq find_min_w_strih; - - int count_wait_pll_w_shtrih; - int max_time_wait_pll_w_strih;//MAX_TIME_WAIT_PLL_W_SHTRIH - - int enable_find_pll; - - - }PLL_VARS;//39 - -#define PLL_VARS_DEFAULT {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} - - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// - -typedef struct { PLL_INPUT input; - PLL_OUTPUT output; - PLL_SETUP setup; - PLL_VARS vars; - void (*init)(); // Pointer to calculation function - void (*calc_pll)(); // Pointer to calculation function - void (*get_freq_float)(); // Pointer to calculation function - void (*get_freq_iq)(); - }PLL_REC; - -typedef PLL_REC *PLL_REC_handle; - -#define PLL_REC_DEFAULT {\ - PLL_INPUT_DEFAULT,\ - PLL_OUTPUT_DEFAULT,\ - PLL_SETUP_DEFAULT,\ - PLL_VARS_DEFAULT,\ - (void (*)(unsigned long))pll_init,\ - (void (*)(unsigned long))pll_calc,\ - (void (*)(unsigned long))pll_get_freq_float,\ - (void (*)(unsigned long))pll_get_freq_iq \ - } - -void pll_init(PLL_REC_handle); -void pll_calc(PLL_REC_handle); -void pll_get_freq_float(PLL_REC_handle); -void pll_get_freq_iq(PLL_REC_handle); - -void Find_zero_Uabc(PLL_REC_handle); -void PLLController(PLL_REC *v); -void AB_BC_CA_To_ABC(_iq U_AB, _iq U_BC, _iq U_CA, _iq *Ua, _iq *Ub, _iq *Uc); -void detect_phase_count(PLL_REC *v); -int read_error_find_pll(PLL_REC *v); - - - -_iq minus_plus_2_pi(_iq a); -_iq minus_plus_2_pi_v2(_iq a); - - - - - - - -#endif - - - - - - diff --git a/Inu/Src2/551/VectorControl/alphabeta_to_dq.c b/Inu/Src2/551/VectorControl/alphabeta_to_dq.c deleted file mode 100644 index ffa5a73..0000000 --- a/Inu/Src2/551/VectorControl/alphabeta_to_dq.c +++ /dev/null @@ -1,24 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library - -#include "alphabeta_to_dq.h" - - - - - - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(alphabeta_to_dq_calc,".fast_run"); -void alphabeta_to_dq_calc(ALPHABETA_TO_DQ *v) -{ - - v->Ud = _IQmpy(v->Ualpha, _IQcos(v->Tetta)) + _IQmpy(v->Ubeta, _IQsin(v->Tetta)); - v->Uq = -_IQmpy(v->Ualpha, _IQsin(v->Tetta)) + _IQmpy(v->Ubeta, _IQcos(v->Tetta)); - -} -///////////////////////////////////////////////// diff --git a/Inu/Src2/551/VectorControl/alphabeta_to_dq.h b/Inu/Src2/551/VectorControl/alphabeta_to_dq.h deleted file mode 100644 index 8484e14..0000000 --- a/Inu/Src2/551/VectorControl/alphabeta_to_dq.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef __ALPHABETA_DQ_H__ -#define __ALPHABETA_DQ_H__ - - - -typedef struct { _iq Ualpha; //phase A voltage, input - _iq Ubeta; //phase B voltage, input - _iq Tetta; //phase angle, input - _iq Ud; // axis d voltage, output - _iq Uq; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - }ALPHABETA_TO_DQ; - - - - - -typedef ALPHABETA_TO_DQ *ALPHABETA_TO_DQ_handle; - -#define ALPHABETA_TO_DQ_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(unsigned long))alphabeta_to_dq_calc \ - } - - -void alphabeta_to_dq_calc(ALPHABETA_TO_DQ_handle); - - -#endif // end __ALPHABETA_DQ_H diff --git a/Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.c b/Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.c deleted file mode 100644 index d075df4..0000000 --- a/Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.c +++ /dev/null @@ -1,39 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library -#include "dq_to_alphabeta_cos.h" - - - - - - -///////////////////////////////////////////////// - - -//#pragma CODE_SECTION(dq_to_alphabeta_calc,".fast_run2"); -void dq_to_alphabeta_calc(DQ_TO_ALPHABETA_handle v) -{ - - v->Ualpha = _IQmpy(v->Ud, _IQcos(v->Tetta)) + _IQmpy(v->Uq, _IQsin(v->Tetta)); - v->Ubeta = -_IQmpy(v->Ud, _IQsin(v->Tetta)) + _IQmpy(v->Uq, _IQcos(v->Tetta)); - -} - - -//#pragma CODE_SECTION(dq_to_alphabeta_calc2,".fast_run2"); -void dq_to_alphabeta_calc2(DQ_TO_ALPHABETA_handle v) -{ - - v->Ualpha = _IQmpy(v->Ud, _IQsin(v->Tetta)) + _IQmpy(v->Uq, _IQcos(v->Tetta)); - v->Ubeta = -_IQmpy(v->Ud, _IQcos(v->Tetta)) + _IQmpy(v->Uq, _IQsin(v->Tetta)); - -} - -//#pragma CODE_SECTION(dq_to_alphabeta_calc_cos,".fast_run2"); -void dq_to_alphabeta_calc_cos(DQ_TO_ALPHABETA_handle v) -{ - - v->Ualpha = _IQmpy(v->Ud, _IQcos(v->Tetta)) - _IQmpy(v->Uq, _IQsin(v->Tetta)); - v->Ubeta = _IQmpy(v->Ud, _IQsin(v->Tetta)) + _IQmpy(v->Uq, _IQcos(v->Tetta)); - -} -///////////////////////////////////////////////// diff --git a/Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.h b/Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.h deleted file mode 100644 index b261ced..0000000 --- a/Inu/Src2/551/VectorControl/dq_to_alphabeta_cos.h +++ /dev/null @@ -1,40 +0,0 @@ - - - -#ifndef __DQ_ALPHABETA_H__ -#define __DQ_ALPHABETA_H__ - -#include "IQmathLib.h" - -typedef struct { _iq Ualpha; //phase A voltage, input - _iq Ubeta; //phase B voltage, input - _iq Tetta; //phase angle, input - _iq Ud; // axis d voltage, output - _iq Uq; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - void (*calc2)(); // Pointer to calculation function. Like in MATLAB - void (*calc_cos)(); // Pointer to calculation function, Ualpha = Uq*Cos(Tetta) - }DQ_TO_ALPHABETA; - - - - - -typedef DQ_TO_ALPHABETA *DQ_TO_ALPHABETA_handle; - -#define DQ_TO_ALPHABETA_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(Uint32))dq_to_alphabeta_calc, \ - (void (*)(Uint32))dq_to_alphabeta_calc2, \ - (void (*)(Uint32))dq_to_alphabeta_calc_cos \ - } - - -void dq_to_alphabeta_calc(DQ_TO_ALPHABETA_handle); -void dq_to_alphabeta_calc2(DQ_TO_ALPHABETA_handle); -void dq_to_alphabeta_calc_cos(DQ_TO_ALPHABETA_handle); - -#endif // end __DQ_ALPHABETA_H__ diff --git a/Inu/Src2/551/VectorControl/params_pll.h b/Inu/Src2/551/VectorControl/params_pll.h deleted file mode 100644 index 2ec9466..0000000 --- a/Inu/Src2/551/VectorControl/params_pll.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef __PARAMS_PLL_H__ -#define __PARAMS_PLL_H__ - - -//#define USE_SMOOTH_FOR_CALC_WC 1 // . - - -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -// stend params -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -//////////// -//PLL -/////////// - -//23470 params -//#define PID_KP_PLL 0.00375 -//#define PID_KI_PLL 0.128 - -//ship1 -#define DEFAULT_PID_KP_PLL 0.0375 -#define DEFAULT_PID_KI_PLL 0.0128 - - -// - -#define PID_KC_PLL 1.000 //0.16 //0.05 //0.1 //20 //200 -// -#define K_PLL_MAX 10.0 //1000000 -#define K_PLL_MIN -10.0 //-1000000 -// - -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -// end params -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// - -#endif - diff --git a/Inu/Src2/551/VectorControl/regul_power.c b/Inu/Src2/551/VectorControl/regul_power.c deleted file mode 100644 index d78f216..0000000 --- a/Inu/Src2/551/VectorControl/regul_power.c +++ /dev/null @@ -1,88 +0,0 @@ -/* - * regul_power.c - * - * Created on: 16 . 2020 . - * Author: star - */ -#include "IQmathLib.h" -#include "regul_power.h" - -#include -#include -#include -#include -#include -#include "math.h" - -#include "mathlib.h" - -#define TIME_RMP_SLOW 20.0 //sec -#define TIME_RMP_FAST 20.0 //sec - -POWER power = POWER_DEFAULTS; - -void init_Pvect(void) { - power.pidP.Ref = 0; - power.pidP.Kp = _IQ(1); - power.pidP.Ki = _IQ(0.1); - power.pidP.Kc = _IQ(0.5); - power.pidP.OutMax = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP); - power.pidP.OutMin = -_IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP); - - power.Pzad_rmp = 0; - power.koef_fast = _IQ(FROT_NOMINAL / (float)NORMA_FROTOR / TIME_RMP_FAST / (float)FREQ_PWM); - power.koef_slow = _IQ(FROT_NOMINAL / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM); - power.Iq_out_max = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP); - power.Pnominal = _IQ(P_NOMINAL * 1000.0 / NORMA_ACP / NORMA_ACP); -} - -#pragma CODE_SECTION(vector_power,".fast_run"); -_iq vector_power(_iq Pzad, _iq P_measured, int n_alg, unsigned int master, - _iq Iq_measured, _iq Iq_limit, _iq *Iq_zad, int reset) -{ - static int prev_n_alg = 0; - _iq Iq_out = 0; - _iq koef_rmp = 0; - if (n_alg != ALG_MODE_FOC_POWER || !edrk.Go || master != MODE_MASTER || reset) { - power.pidP.Ui = Iq_measured; - power.pidP.Out = Iq_measured; - if (reset) { power.Pzad_rmp = 0; } - } - if (n_alg == ALG_MODE_FOC_OBOROTS) { - Pzad = power.Pnominal; - } - if (master == MODE_SLAVE) { - power.Pzad_rmp = P_measured; - return *Iq_zad; - } - - // , - //.. - if (n_alg == ALG_MODE_FOC_POWER && prev_n_alg != ALG_MODE_FOC_POWER) { - power.Pzad_rmp = P_measured; - } - - if((_IQabs(power.Pzad_rmp) <= _IQabs(Pzad)) && - (((Pzad >= 0) && (power.Pzad_rmp >= 0)) || ((Pzad < 0) && (power.Pzad_rmp < 0)))) - { - koef_rmp = power.koef_slow; - } - else - { - koef_rmp = power.koef_fast; - } - power.Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, power.Pzad_rmp, Pzad); - - power.pidP.OutMax = Iq_limit; - power.pidP.OutMin = -Iq_limit; - power.pidP.Ref = power.Pzad_rmp; - power.pidP.Fdb = P_measured; - power.pidP.calc(&power.pidP); - Iq_out = power.pidP.Out; - Iq_out = _IQsat(Iq_out, Iq_limit, -Iq_limit); - *Iq_zad = Iq_out; - - prev_n_alg = n_alg; - - return Iq_out; -} diff --git a/Inu/Src2/551/VectorControl/regul_power.h b/Inu/Src2/551/VectorControl/regul_power.h deleted file mode 100644 index 80d3e06..0000000 --- a/Inu/Src2/551/VectorControl/regul_power.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * regul_power.h - * - * Created on: 16 . 2020 . - * Author: star - */ - -#ifndef SRC_VECTORCONTROL_NIO12_REGUL_POWER_H_ -#define SRC_VECTORCONTROL_NIO12_REGUL_POWER_H_ - -#include "pid_reg3.h" - -typedef struct { - PIDREG3 pidP; - _iq Pzad_rmp; - _iq koef_fast; - _iq koef_slow; - _iq Iq_out_max; - _iq Pnominal; -} POWER; - -#define POWER_DEFAULTS {PIDREG3_DEFAULTS, 0,0,0,0,0} - -_iq vector_power(_iq Pzad, _iq P_measured, int mode, unsigned int master, - _iq Iq_measured, _iq Iq_limit, _iq* Frot_zad, int reset); -void init_Pvect(void); - -extern POWER power; - -#endif /* SRC_VECTORCONTROL_NIO12_REGUL_POWER_H_ */ diff --git a/Inu/Src2/551/VectorControl/regul_turns.c b/Inu/Src2/551/VectorControl/regul_turns.c deleted file mode 100644 index 95ca611..0000000 --- a/Inu/Src2/551/VectorControl/regul_turns.c +++ /dev/null @@ -1,143 +0,0 @@ -#include "DSP281x_Device.h" -#include "IQmathLib.h" - -#include "regul_turns.h" - -#include -#include -#include -#include -#include -#include -#include -#include "math.h" -#include "mathlib.h" -#include "pid_reg3.h" -#include "vector_control.h" - -#pragma DATA_SECTION(turns,".fast_vars1"); -TURNS turns = TURNS_DEFAULTS; - - //IQ_OUT_NOM TODO:set Iq nominal - -#define IQ_165_RPM 2306867 //165/ -#define IQ_170_RPM 2376772 //170/ -#define IQ_5_RPM 69905 //5/ - -#define TIME_RMP_FAST 10.0 //sec -#define TIME_RMP_SLOW 30.0 //sec -#define F_DEST 3.0 //Hz - -void init_Fvect() -{ - turns.pidFvect.Ref = 0; - turns.pidFvect.Kp = _IQ(5); // //_IQ(30); - turns.pidFvect.Ki = _IQ(0.005); //_IQ(0.008);//_IQ(0.002); // - turns.pidFvect.Kc = _IQ(0.5); - turns.pidFvect.OutMax = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP); - turns.pidFvect.OutMin = -_IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP); - - turns.Fzad_rmp = 0; - turns.Fnominal = _IQ(FROT_MAX / NORMA_FROTOR); - turns.koef_fast = - _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 1.0); - turns.koef_slow = - _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 1.0); - turns.Iq_out_max = _IQ(MOTOR_CURRENT_MAX / NORMA_ACP); - turns.Id_out_max = 0;//_IQ(ID_OUT_NOM / NORMA_ACP); - turns.mzz_zad_int = 0; -} - -void reset_F_pid() -{ - turns.pidFvect.Up = 0; - turns.pidFvect.Up1 = 0; - turns.pidFvect.Ui = 0; - turns.pidFvect.Ud = 0; - turns.pidFvect.Out = 0; -} - -//#pragma CODE_SECTION(vector_turns,".fast_run2"); -void vector_turns(_iq Fzad, _iq Frot, - _iq Iq_measured, _iq Iq_limit, int n_alg, - unsigned int master, _iq *Iq_zad, int reset) -{ - static int prev_n_alg = 0; - _iq koef_rmp; //, koef_spad; - _iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat; - _iq deltaVar; - -// turns.mzz_zad_int = zad_intensiv_q(35480, 35480, turns.mzz_zad_int, Iq_limit); - turns.mzz_zad_int = Iq_limit; - - turns.pidFvect.OutMax = turns.mzz_zad_int; - turns.pidFvect.OutMin = -turns.mzz_zad_int; - - // - if (Fzad >= 0 && Frot > 0) - { - turns.pidFvect.OutMin = 0; - } - if (Fzad <= 0 && Frot < 0) - { - turns.pidFvect.OutMax = 0; - } - if (reset) { turns.Fzad_rmp = Frot;} - - if ((n_alg < ALG_MODE_FOC_OBOROTS) || (!edrk.Go)) - // -, - { // - turns.Fzad_rmp = Frot; -// prev_Fzad = Frot; - reset_F_pid(); // , - - turns.pidFvect.Ui = Iq_measured; - turns.pidFvect.Out = Iq_measured; - *Iq_zad = Iq_measured; - - if (!edrk.Go) - { - *Iq_zad = 0; - } - - return; - } - if (master == MODE_SLAVE) { - turns.Fzad_rmp = Frot; - turns.pidFvect.Ui = Iq_measured; - turns.pidFvect.Out = Iq_measured; - return; - } - // - if (n_alg == ALG_MODE_FOC_POWER) { - Fzad = turns.Fnominal; - } - // - // - if (n_alg == ALG_MODE_FOC_OBOROTS && prev_n_alg != ALG_MODE_FOC_OBOROTS) { - turns.Fzad_rmp = Frot; - } - - if (_IQabs(turns.Fzad_rmp) <= _IQabs(Fzad) - && (((Fzad >= 0) && (turns.Fzad_rmp >= 0)) - || ((Fzad < 0) && (turns.Fzad_rmp < 0)))) - { - koef_rmp = turns.koef_slow; - } - else - { - koef_rmp = turns.koef_fast; - } - - turns.Fzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, turns.Fzad_rmp, Fzad); - - turns.pidFvect.Ref = turns.Fzad_rmp; - turns.pidFvect.Fdb = Frot; - - turns.pidFvect.calc(&turns.pidFvect); - Iq_out_unsat = turns.pidFvect.Out; - - Iq_out_sat = _IQsat(Iq_out_unsat, turns.mzz_zad_int, -turns.mzz_zad_int); //Test - *Iq_zad = Iq_out_sat; - - prev_n_alg = n_alg; -} diff --git a/Inu/Src2/551/VectorControl/regul_turns.h b/Inu/Src2/551/VectorControl/regul_turns.h deleted file mode 100644 index f765b88..0000000 --- a/Inu/Src2/551/VectorControl/regul_turns.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef REGUL_TURNS -#define REGUL_TURNS -#include "IQmathLib.h" -#include "pid_reg3.h" - -typedef struct { - PIDREG3 pidFvect; - - _iq Fzad_rmp; - _iq Fnominal; - _iq koef_fast; - _iq koef_slow; - _iq Iq_out_max; - _iq Id_out_max; - _iq mzz_zad_int; -} TURNS; - -#define TURNS_DEFAULTS {PIDREG3_DEFAULTS, 0,0,0,0,0,0,0} - -void init_Fvect(void); -void vector_turns(_iq Fzad, _iq Frot, - _iq Iq, _iq Iq_limit, int mode, - unsigned int master, _iq *Iq_zad, int reset); - -extern TURNS turns; - -#endif //REGUL_TURNS - - diff --git a/Inu/Src2/551/VectorControl/smooth.c b/Inu/Src2/551/VectorControl/smooth.c deleted file mode 100644 index dcf63c8..0000000 --- a/Inu/Src2/551/VectorControl/smooth.c +++ /dev/null @@ -1,180 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library - -#include "smooth.h" -#include "math_pi.h" -#include "math_pi.h" - - -#define SIZE_SMOOTH_INPUT 180 - - -#pragma CODE_SECTION(my_mean,".fast_run"); -_iq23 my_mean(int cnt, SMOOTH *v) -{ - _iq23 summ = 0; - int start; - - - start = v->current_pos_buf_input; - if (start==0) - start = (MAX_SIZE_SMOOTH_INPUT-1); - - while(cnt>0) - { - cnt--; - start--; - summ += v->buf_input[start]; - if (start==0) - start = (MAX_SIZE_SMOOTH_INPUT-1); - } - return summ; - -} - - - -void smooth_init(SMOOTH *v) -{ - int i=0; - - v->c = 1; - v->av = 0; - v->w = _IQ23(WINDOW_START); - - for (i=0;ibuf_input[i] = 0; - } -// current_pos_buf_input = 0; - v->current_pos_buf_input = 0; - -} - -#pragma CODE_SECTION(smooth_add,".fast_run"); -void smooth_add(SMOOTH *v) -{ - volatile int i; - - i = v->current_pos_buf_input; - v->buf_input[i] = v->input; - - v->current_pos_buf_input++; - if (v->current_pos_buf_input>=MAX_SIZE_SMOOTH_INPUT) - v->current_pos_buf_input = 0; - -} - - -#pragma CODE_SECTION(smooth_calc,".fast_run"); -void smooth_calc(SMOOTH *v) -{ - _iq23 e=0; - _iq23 summ=0; - _iq23 w_new; - long w_int; - - w_int = _IQ23int(v->w); - - if (v->c <= (WINDOW_START*2)) - { - summ = my_mean(v->c,v); - v->av = _IQ23div(summ,_IQ23(v->c)); - e = 0; - } - else - { - e = _IQ23div(CONST_IQ_2PI,v->av) - v->w; //(2*pi*fs/av) - w - v->ee = v->ee + _IQ23mpy(v->kp,(e - v->e0)) + _IQ23mpy(v->ki, e); - w_new = v->w + v->ee; - - if (w_new>_IQ23(SIZE_SMOOTH_INPUT)) - w_new = _IQ23(SIZE_SMOOTH_INPUT); - - w_int = _IQ23int(w_new); - summ = my_mean(w_int,v); - v->w = _IQ23(w_int); - v->av = _IQ23div(summ, v->w); - - } - - if (v->cc++; - - v->e0 = e; - v->w_int = w_int; -} - - -#pragma CODE_SECTION(smooth_simple_calc,".fast_run"); -void smooth_simple_calc(SMOOTH *v) -{ - volatile _iq23 summ=0; - - if (v->c <= v->w_int_simple ) - { - summ = my_mean(v->c, v); - v->summ = summ; - v->av = _IQ23div(summ,_IQ23(v->c)); - } - else - { - summ = my_mean(v->w_int_simple, v); - v->summ = summ; - v->av = _IQ23div(summ, _IQ23(v->w_int_simple)); - } - - if (v->cc++; - - -} - - - - -void iq_smooth (_iq23 *input, _iq23 *output, int n, int window) -{ - int i,j,z,k1,k2,hw; - int fm1,fm2; - - _iq23 tmp; - - - fm1 = window/2; - fm2 = fm1*2; - if ((window-fm2)==0) - window++; - - hw = (window-1)/2; - output[0] = input[0]; - - for (i=1;i(n-1)) - { - k1=i-n+i+1; - k2=n-1; - z=k2-k1+1; - } - else - { - k1=i-hw; - k2=i+hw; - z=window; - } - - for (j=k1;j<=k2;j++) - { - tmp=tmp + input[j]; - } - output[i] = _IQ23div(tmp,_IQ23(z)); - } - -} diff --git a/Inu/Src2/551/VectorControl/smooth.h b/Inu/Src2/551/VectorControl/smooth.h deleted file mode 100644 index c91e6f1..0000000 --- a/Inu/Src2/551/VectorControl/smooth.h +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef __SMOOTH_H__ -#define __SMOOTH_H__ - -#define WINDOW_START 79.0 //39.0 -#define MAX_SIZE_SMOOTH_INPUT 180 - -typedef struct { int current_pos_buf_input; - _iq23 kp; - _iq23 ki; - int c; - int w_int_simple; - _iq23 w; - long w_int; - _iq23 ee; - _iq23 e0; - _iq23 av; - _iq23 input; - _iq23 summ; - _iq23 buf_input[MAX_SIZE_SMOOTH_INPUT]; - void (*init)(); // Pointer to calculation function - void (*add)(); // Pointer to calculation function - void (*calc)(); // Pointer to calculation function - void (*simple_calc)(); // Pointer to calculation function - - }SMOOTH; - - - - - -typedef SMOOTH *SMOOTH_handle; - -#define SMOOTH_DEFAULTS { 0, \ - _IQ23(0.1), \ - _IQ23(0.01), \ - 1, \ - 1, \ - _IQ23(WINDOW_START), \ - WINDOW_START, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - {0},\ - (void (*)(unsigned long))smooth_init,\ - (void (*)(unsigned long))smooth_add,\ - (void (*)(unsigned long))smooth_calc,\ - (void (*)(unsigned long))smooth_simple_calc\ - } - - -void smooth_calc(SMOOTH_handle); -void smooth_init(SMOOTH_handle); -void smooth_add(SMOOTH_handle); -void smooth_simple_calc(SMOOTH_handle); - - - - - - -void iq_smooth (_iq23 *input, _iq23 *output, int n, int window); - - - - -#endif // end __ABC_ALPHABETA_H - - - - - - - - - - diff --git a/Inu/Src2/551/VectorControl/teta_calc.c b/Inu/Src2/551/VectorControl/teta_calc.c deleted file mode 100644 index 8a64daf..0000000 --- a/Inu/Src2/551/VectorControl/teta_calc.c +++ /dev/null @@ -1,91 +0,0 @@ -#include "IQmathLib.h" - -#include "teta_calc.h" - -#include -#include -#include -#include -#include - -#include "mathlib.h" -#include "pid_reg3.h" - - -#define CONST_IQ_2PI 105414357 -#define PI 3.1415926535897932384626433832795 - -#define MAX_Ud_Pid_Out_Id 176160 //0.2 ~ 167772 //0.21 ~ 176160 -#define BPSI_START 0.17 //0.15 - -TETTA_CALC tetta_calc = TETTA_CALC_DEF; - -void init_teta_calc_struct() -{ - float Tr_cm = (L_M + L_SIGMA_R) / (R_ROTOR_SHTRIH / SLIP_NOM); -// tetta_calc.k_r = _IQ(1 / FREQ_PWM / Tr_cm); -// tetta_calc.k_t = _IQ(R_ROTOR / (L_M + L_SIGMA_R) / 2.0 / 3.14 / 50 / NORMA_FROTOR); - tetta_calc.k_r = _IQ(0.005168); //_IQ(0.015); - tetta_calc.k_t = _IQ(0.0074); //_IQ(0.0045); - tetta_calc.Imds = 0; - tetta_calc.theta = 0; - tetta_calc.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM); -} - -//#pragma CODE_SECTION(calc_teta_Id,".fast_run"); -void calc_teta_Id(_iq Frot, _iq Id, _iq Iq, _iq *theta_out, _iq *theta_to_slave, _iq *Fsl_out, _iq *Fstator_out, - unsigned int master, int reset) { - - _iq Fsl, Fst; - _iq add_to_tic = 0; - _iq to_slave = 0; - - if (reset) { - tetta_calc.Imds = 0; - } - - tetta_calc.Imds = tetta_calc.Imds + _IQmpy(tetta_calc.k_r, (Id - tetta_calc.Imds)); - - if (master == MODE_SLAVE){ - return; - } - - Fsl = _IQmpy(tetta_calc.k_t, Iq); - if (tetta_calc.Imds != 0) { - Fsl = _IQdiv(Fsl, tetta_calc.Imds); - } else { - Fsl = 0; - } - - if (Fsl > MAX_Ud_Pid_Out_Id) { Fsl = MAX_Ud_Pid_Out_Id;} - if (Fsl < -MAX_Ud_Pid_Out_Id) { Fsl = -MAX_Ud_Pid_Out_Id;} -// if (Fsl < 0) { Fsl = 0;} - - Fst = Frot * POLUS + Fsl; - add_to_tic = _IQmpy(Fst, tetta_calc.hz_to_angle); - tetta_calc.theta += add_to_tic; - to_slave = tetta_calc.theta + add_to_tic; - - if (tetta_calc.theta > CONST_IQ_2PI) { - tetta_calc.theta -= CONST_IQ_2PI; - } else if (tetta_calc.theta < 0) { - tetta_calc.theta += CONST_IQ_2PI; - } - - if (to_slave > CONST_IQ_2PI) { - to_slave -= CONST_IQ_2PI; - } else if (to_slave < 0) { - to_slave += CONST_IQ_2PI; - } - - *Fsl_out = Fsl; - *theta_out = tetta_calc.theta; - *theta_to_slave = to_slave; - *Fstator_out = Fst; - -// logpar.log26 = (int16)(_IQtoIQ15(Fsl)); -// logpar.log27 = (int16)(_IQtoIQ15(tetta_calc.Imds)); -// logpar.log28 = (int16)(_IQtoIQ15(Iq)); -// logpar.log3 = (int16)(_IQtoIQ15(Id)); -} - diff --git a/Inu/Src2/551/VectorControl/teta_calc.h b/Inu/Src2/551/VectorControl/teta_calc.h deleted file mode 100644 index d2e7b03..0000000 --- a/Inu/Src2/551/VectorControl/teta_calc.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef TETA_CALC -#define TETA_CALC - -#include "IQmathLib.h" -#include "pid_reg3.h" - -void calc_teta_Id(_iq Frot, _iq Id, _iq Iq, _iq *tetta_out, _iq *theta_to_slave, _iq *Fsl_out, _iq *Fstator_out, - unsigned int master, int reset); -void init_teta_calc_struct(void); - -// k_r = Ts / Tr_cm -// Tr_cm = Lr / Rr -// Lr - -// Rr - -// -// k_t = 1 / (Tr_cm * 2 * Pi * f_b) -// f_b = NORMA_FROT -// K = Ts * f_b -// f_b - (12 ) -// Ts - (840 ) - -typedef struct { - _iq Imds; - _iq theta; - - _iq hz_to_angle; - _iq k_r; - _iq k_t; -} TETTA_CALC; - -#define TETTA_CALC_DEF {0,0,0,0,0} - -extern TETTA_CALC tetta_calc; - -#endif //TETA_CALC - diff --git a/Inu/Src2/551/VectorControl/vector_control.c b/Inu/Src2/551/VectorControl/vector_control.c deleted file mode 100644 index 901b75d..0000000 --- a/Inu/Src2/551/VectorControl/vector_control.c +++ /dev/null @@ -1,297 +0,0 @@ -/* - * vector_control.c - * - * Created on: 16 . 2020 . - * Author: star - */ -#include "IQmathLib.h" - -#include "vector_control.h" - -#include -#include -#include -#include -#include -#include -#include "math.h" -#include "mathlib.h" -#include "filter_v1.h" -#include "abc_to_dq.h" -#include "regul_power.h" -#include "regul_turns.h" -#include "teta_calc.h" - -//#define CALC_TWO_WINDINGS - -#define I_ZERO_LEVEL_IQ 27962 // 111848 ~ 20A //279620 ~ 50A //55924 ~ 10A - - -#pragma DATA_SECTION(vect_control,".fast_vars"); -VECTOR_CONTROL vect_control = VECTOR_CONTROL_DEFAULTS; - - -void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad, - _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1, - _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2, int reset); -void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2); -void analog_dq_calc(_iq winding_displacement); -_iq calcId(_iq Iq_limit, _iq Iq, int reset, int prepare_stop_PWM); -inline void calcUdUqCompensation(_iq Frot); - -void initVectorControl() { - vect_control.pidD1.Kp = _IQ(0.3);//_IQ(0.2); - vect_control.pidD1.Ki = _IQ(0.01);//_IQ(0.9); - vect_control.pidD1.OutMax = _IQ(0.9); - vect_control.pidD1.OutMin = -_IQ(0.9); - vect_control.pidQ1.Kp = _IQ(0.3); - vect_control.pidQ1.Ki = _IQ(0.01); - vect_control.pidQ1.OutMax = _IQ(0.9); - vect_control.pidQ1.OutMin = -_IQ(0.9); - vect_control.pidD2.Kp = _IQ(0.3); - vect_control.pidD2.Ki = _IQ(0.3); - vect_control.pidD2.OutMax = _IQ(0.9); - vect_control.pidD2.OutMin = -_IQ(0.9); - vect_control.pidQ2.Kp = _IQ(0.3); - vect_control.pidQ2.Ki = _IQ(0.3); - vect_control.pidQ2.OutMax = _IQ(0.9); - vect_control.pidQ2.OutMin = -_IQ(0.9); - -// vect_control.iqId_nominal = _IQ(MOTOR_CURRENT_NOMINAL * sqrtf(1 - COS_FI * COS_FI) / NORMA_ACP); - vect_control.iqId_nominal = _IQ(MOTOR_CURRENT_NOMINAL * 0.4 / NORMA_ACP); - vect_control.iqId_min = _IQ(200 / NORMA_ACP); - vect_control.iqId_start = _IQ(200.0 / NORMA_ACP); - vect_control.koef_rmp_Id = (_iq)(vect_control.iqId_nominal / FREQ_PWM); - vect_control.koef_filt_I = _IQ(0.5); - - - vect_control.koef_Ud_comp = _IQ((L_SIGMA_S + L_M * L_SIGMA_R / (L_M + L_SIGMA_R)) * 2 * 3.14 * NORMA_FROTOR); //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r) - vect_control.koef_Uq_comp = _IQ((L_M + L_SIGMA_S) * 2 * 3.14 * NORMA_FROTOR); //Lm + Lsigm_s -// vect_control.koef_Ud_comp = _IQ(0.0002369 * 2 * 3.14 * NORMA_FROTOR); //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r) -// vect_control.koef_Uq_comp = _IQ(0.0043567 * 2 * 3.14 * NORMA_FROTOR); //Lm + Lsigm_s - vect_control.koef_zero_Uzad = _IQ(0.993); //_IQ(0.993); //_IQ(0.03); - init_Pvect(); - init_Fvect(); - init_teta_calc_struct(); -} - -void vectorControlConstId (_iq Pzad, _iq Fzad, int direction, _iq Frot, - int n_alg, unsigned int master, _iq mzz_zad, - _iq winding_displacement, - _iq theta_from_master, _iq Iq_from_master, _iq P_from_slave, - _iq *theta_to_slave, _iq *Iq_to_slave, _iq *P_to_master, - int reset, int prepare_stop_PWM) { - _iq Iq_zad = 0, Iq_zad_power = 0, Id_zad = 0; - _iq P_measured = 0; - static _iq Ud_zad1 = 0, Uq_zad1 = 0, Ud_zad2 = 0, Uq_zad2 = 0; - -// if(direction < 0) { Frot = -Frot; } - - if (reset) { - Ud_zad1 = 0; - Uq_zad1 = 0; - Ud_zad2 = 0; - Uq_zad2 = 0; - } - analog_dq_calc(winding_displacement); - - P_measured = vect_control.iqPvsi1 + vect_control.iqPvsi2; - *P_to_master = P_measured; - P_measured += P_from_slave; - - - - vector_power(Pzad, P_measured, n_alg, master, (vect_control.iqIq1 + vect_control.iqIq2), - edrk.zadanie.iq_Izad_rmp, &Iq_zad_power, reset); - vector_turns(Fzad, Frot, (vect_control.iqIq1 + vect_control.iqIq2), - Iq_zad_power, n_alg, master, &Iq_zad, reset); - - Id_zad = calcId(edrk.zadanie.iq_Izad, Iq_zad, reset, prepare_stop_PWM); - - if (master == MODE_SLAVE) { - vect_control.iqTheta = theta_from_master; - *theta_to_slave = theta_from_master; - Iq_zad = Iq_from_master; - Iq_zad_power = Iq_from_master; - } else { -// calc_teta_Id(Frot, vect_control.iqId1, vect_control.iqIq1, &vect_control.iqTheta, theta_to_slave, -// &vect_control.iqFsl, &vect_control.iqFstator, master, reset); - calc_teta_Id(Frot, Id_zad, Iq_zad, &vect_control.iqTheta, theta_to_slave, - &vect_control.iqFsl, &vect_control.iqFstator, master, reset); - } - - calcUdUqCompensation(Frot); - - if (prepare_stop_PWM && Id_zad == 0) { - vect_control.iqUdKm = _IQmpy(vect_control.iqUdKm, vect_control.koef_zero_Uzad); - vect_control.iqUqKm = _IQmpy(vect_control.iqUqKm, vect_control.koef_zero_Uzad); - } else { - Idq_to_Udq_2_windings((Id_zad >> 1), (Iq_zad >> 1), - vect_control.iqId1, vect_control.iqIq1, &Ud_zad1, &Uq_zad1, - vect_control.iqId2, vect_control.iqIq2, &Ud_zad2, &Uq_zad2, reset); - - vect_control.iqUdKm = Ud_zad1 + vect_control.iqUdCompensation; - vect_control.iqUqKm = Uq_zad1 + vect_control.iqUqCompensation; - } - - vect_control.sqrtIdq1 = _IQsqrt(_IQmpy(vect_control.iqId1, vect_control.iqId1) + _IQmpy(vect_control.iqIq1, vect_control.iqIq1)); - analog_Udq_calc(Ud_zad1, Uq_zad1, Ud_zad2, Uq_zad2); - *Iq_to_slave = Iq_zad; - - vect_control.Iq_zad1 = Iq_zad; - vect_control.Id_zad1 = Id_zad; - -} - - -#pragma CODE_SECTION(analog_dq_calc,".fast_run"); -void analog_dq_calc(_iq winding_displacement) -{ - ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS; - - abc_dq_converter.Ia = analog.iqIu_1; - abc_dq_converter.Ib = analog.iqIv_1; - abc_dq_converter.Ic = analog.iqIw_1; - abc_dq_converter.Tetta = vect_control.iqTheta + winding_displacement; - abc_dq_converter.calc(&abc_dq_converter); - vect_control.iqId1 = abc_dq_converter.Id; - vect_control.iqIq1 = abc_dq_converter.Iq; - vect_control.iqId1_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqId1_filt, vect_control.iqId1); - vect_control.iqIq1_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqIq1_filt, vect_control.iqIq1); - - abc_dq_converter.Ia = analog.iqIu_2; - abc_dq_converter.Ib = analog.iqIv_2; - abc_dq_converter.Ic = analog.iqIw_2; - abc_dq_converter.Tetta = vect_control.iqTheta + winding_displacement; - abc_dq_converter.calc(&abc_dq_converter); - vect_control.iqId2 = abc_dq_converter.Id; - vect_control.iqIq2 = abc_dq_converter.Iq; - vect_control.iqId2_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqId2_filt, vect_control.iqId2); - vect_control.iqIq2_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqIq2_filt, vect_control.iqIq2); - - - if (_IQabs(vect_control.iqId1) < I_ZERO_LEVEL_IQ) { vect_control.iqId1 = 0; } - if (_IQabs(vect_control.iqIq1) < I_ZERO_LEVEL_IQ) { vect_control.iqIq1 = 0; } - if (_IQabs(vect_control.iqId2) < I_ZERO_LEVEL_IQ) { vect_control.iqId2 = 0; } - if (_IQabs(vect_control.iqIq2) < I_ZERO_LEVEL_IQ) { vect_control.iqIq2 = 0; } - - vect_control.iqPvsi1 = _IQmpy(_IQmpy(vect_control.iqIq1, _IQabs(vect_control.iqUq1)), 25165824L); - vect_control.iqPvsi2 = _IQmpy(_IQmpy(vect_control.iqIq2, _IQabs(vect_control.iqUq2)), 25165824L); - -} - -#pragma CODE_SECTION(analog_dq_calc_external,".fast_run"); -void analog_dq_calc_external(_iq winding_displacement, _iq theta) -{ - ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS; - - abc_dq_converter.Ia = analog.iqIu_1; - abc_dq_converter.Ib = analog.iqIv_1; - abc_dq_converter.Ic = analog.iqIw_1; - abc_dq_converter.Tetta = theta + winding_displacement; - abc_dq_converter.calc(&abc_dq_converter); - vect_control.iqId1 = abc_dq_converter.Id; - vect_control.iqIq1 = abc_dq_converter.Iq; - - - abc_dq_converter.Ia = analog.iqIu_2; - abc_dq_converter.Ib = analog.iqIv_2; - abc_dq_converter.Ic = analog.iqIw_2; - abc_dq_converter.Tetta = theta + winding_displacement; - abc_dq_converter.calc(&abc_dq_converter); - vect_control.iqId2 = abc_dq_converter.Id; - vect_control.iqIq2 = abc_dq_converter.Iq; - - if (_IQabs(vect_control.iqId1) < I_ZERO_LEVEL_IQ) { vect_control.iqId1 = 0; } - if (_IQabs(vect_control.iqIq1) < I_ZERO_LEVEL_IQ) { vect_control.iqIq1 = 0; } - if (_IQabs(vect_control.iqId2) < I_ZERO_LEVEL_IQ) { vect_control.iqId2 = 0; } - if (_IQabs(vect_control.iqIq2) < I_ZERO_LEVEL_IQ) { vect_control.iqIq2 = 0; } - - vect_control.iqPvsi1 = _IQmpy(_IQmpy(vect_control.iqIq1, _IQabs(vect_control.iqUq1)), 25165824L); - vect_control.iqPvsi2 = _IQmpy(_IQmpy(vect_control.iqIq2, _IQabs(vect_control.iqUq2)), 25165824L); - -} - -void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad, - _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1, - _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2, int reset) -{ - if (reset) { - vect_control.pidD1.Ui = 0; - vect_control.pidD1.Out = 0; - vect_control.pidQ1.Ui = 0; - vect_control.pidQ1.Out = 0; -#ifdef CALC_TWO_WINDINGS - vect_control.pidD2.Ui = 0; - vect_control.pidD2.Out = 0; - vect_control.pidQ2.Ui = 0; - vect_control.pidQ2.Out = 0; -#endif - } - vect_control.pidD1.Ref = Id_zad; - vect_control.pidD1.Fdb = Id_measured1; - vect_control.pidD1.calc(&vect_control.pidD1); - *Ud_zad1 = vect_control.pidD1.Out; - - vect_control.pidQ1.Ref = Iq_zad; - vect_control.pidQ1.Fdb = Iq_measured1; - vect_control.pidQ1.calc(&vect_control.pidQ1); - *Uq_zad1 = vect_control.pidQ1.Out; -#ifdef CALC_TWO_WINDINGS - vect_control.pidD2.Ref = Id_zad; - vect_control.pidD2.Fdb = Id_measured2; - vect_control.pidD2.calc(&vect_control.pidD2); - *Ud_zad2 = vect_control.pidD2.Out; - - vect_control.pidQ2.Ref = Iq_zad; - vect_control.pidQ2.Fdb = Iq_measured2; - vect_control.pidQ2.calc(&vect_control.pidQ2); - *Uq_zad2 = vect_control.pidQ2.Out; -#else - *Ud_zad2 = *Ud_zad1; - *Uq_zad2 = *Ud_zad1; -// *Uq_zad2 = *Uq_zad1; -#endif -} - -#pragma CODE_SECTION(analog_Udq_calc,".fast_run"); -void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2) -{ - _iq Uzpt = filter.iqU_1_long + filter.iqU_2_long; - vect_control.iqUd1 = _IQmpy(Ud1, _IQmpy(Uzpt, 8388608L)); // 8388608 = _IQ(0.5) - vect_control.iqUq1 = _IQmpy(Uq1, _IQmpy(Uzpt, 8388608L)); - vect_control.iqUd2 = _IQmpy(Ud2, _IQmpy(Uzpt, 8388608L)); - vect_control.iqUq2 = _IQmpy(Uq2, _IQmpy(Uzpt, 8388608L)); - -} - -_iq calcId(_iq Iq_limit, _iq Iq, int reset, int prepare_stop_PWM) { - static _iq Id_rmp = 0; - _iq Id_zad = 0; - if (reset) { - Id_rmp = 0; - } - - - if (prepare_stop_PWM) { - Id_zad = 0; - } else if (Iq < vect_control.iqId_min) { - Id_zad = vect_control.iqId_min; - } else if (Iq > vect_control.iqId_nominal) { - Id_zad = vect_control.iqId_nominal; - } else { - Id_zad = Iq; - } -// Id_zad = Iq_limit < vect_control.iqId_nominal ? Iq_limit : vect_control.iqId_nominal; - Id_rmp = zad_intensiv_q(vect_control.koef_rmp_Id, vect_control.koef_rmp_Id << 1, Id_rmp, Id_zad); - return Id_rmp; -} - -void calcUdUqCompensation(_iq Frot) { - _iq Uzpt = (filter.iqU_1_long + filter.iqU_2_long) >> 1; - _iq UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), vect_control.iqIq1 + vect_control.iqIq2); - _iq UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), vect_control.iqId1 + vect_control.iqId2); - vect_control.iqUdCompensation = -_IQdiv(UdVolt, Uzpt); - vect_control.iqUqCompensation = _IQdiv(UqVolt, Uzpt); -} - diff --git a/Inu/Src2/551/VectorControl/vector_control.h b/Inu/Src2/551/VectorControl/vector_control.h deleted file mode 100644 index 3307fb1..0000000 --- a/Inu/Src2/551/VectorControl/vector_control.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * vector_control.h - * - * Created on: 16 . 2020 . - * Author: star - */ - -#ifndef SRC_VECTORCONTROL_NIO12_VECTOR_CONTROL_H_ -#define SRC_VECTORCONTROL_NIO12_VECTOR_CONTROL_H_ - -#include "pid_reg3.h" -#include "regul_power.h" -#include "regul_turns.h" - -typedef struct { - PIDREG3 pidD1; - PIDREG3 pidQ1; - PIDREG3 pidD2; - PIDREG3 pidQ2; - - _iq iqId1; - _iq iqIq1; - _iq iqId2; - _iq iqIq2; - _iq iqUd1; - _iq iqUq1; - _iq iqUd2; - _iq iqUq2; - _iq iqUdKm; - _iq iqUqKm; - _iq iqUdCompensation; - _iq iqUqCompensation; - - _iq iqId1_filt; - _iq iqIq1_filt; - _iq iqId2_filt; - _iq iqIq2_filt; - - _iq iqPvsi1; - _iq iqPvsi2; - _iq iqTheta; - _iq iqFsl; - _iq iqFstator; - _iq iqId_nominal; - _iq iqId_min; - _iq iqId_start; - _iq koef_rmp_Id; - _iq koef_filt_I; - _iq koef_Ud_comp; - _iq koef_Uq_comp; - _iq koef_zero_Uzad; - _iq add_tetta; - - _iq sqrtIdq1; - _iq sqrtIdq2; - - _iq Iq_zad1; - _iq Id_zad1; - - _iq add_bpsi; - -} VECTOR_CONTROL; - -#define VECTOR_CONTROL_DEFAULTS {PIDREG3_DEFAULTS, PIDREG3_DEFAULTS, \ - PIDREG3_DEFAULTS, PIDREG3_DEFAULTS, \ - 0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} - -void vectorControlConstId (_iq Pzad, _iq Fzad, int direction, _iq Frot, - int n_alg, unsigned int master, _iq mzz_zad, - _iq winding_displacement, - _iq theta_from_master, _iq Iq_from_master, _iq P_from_slave, - _iq *theta_to_slave, _iq *Iq_to_slave, _iq *P_to_master, - int reset, int prepare_stop_PWM); - -void analog_dq_calc_external(_iq winding_displacement, _iq theta); -void initVectorControl(); - -extern VECTOR_CONTROL vect_control; - - -#endif /* SRC_VECTORCONTROL_NIO12_VECTOR_CONTROL_H_ */ diff --git a/Inu/Src2/551/main/281xEvTimersInit.c b/Inu/Src2/551/main/281xEvTimersInit.c deleted file mode 100644 index 59b7005..0000000 --- a/Inu/Src2/551/main/281xEvTimersInit.c +++ /dev/null @@ -1,636 +0,0 @@ -// TI File $Revision: /main/3 $ -// Checkin $Date: July 2, 2007 11:32:13 $ -//########################################################################### -// -// FILE: Example_281xEvTimerPeriod.c -// -// TITLE: DSP281x Event Manager GP Timer example program. -// -// ASSUMPTIONS: -// -// This program requires the DSP281x V1.00 header files. -// As supplied, this project is configured for "boot to H0" operation. -// -// Other then boot mode pin configuration, no other hardware configuration -// is required. -// -// DESCRIPTION: -// -// This program sets up EVA Timer 1, EVA Timer 2, EVB Timer 3 -// and EVB Timer 4 to fire an interrupt on a period overflow. -// A count is kept each time each interrupt passes through -// the interrupt service routine. -// -// EVA Timer 1 has the shortest period while EVB Timer4 has the -// longest period. -// -// Watch Variables: -// -// EvaTimer1InterruptCount; -// EvaTimer2InterruptCount; -// EvbTimer3InterruptCount; -// EvbTimer4InterruptCount; -// -//########################################################################### -// $TI Release: DSP281x C/C++ Header Files V1.20 $ -// $Release Date: July 27, 2009 $ -//########################################################################### - -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "DSP281x_Examples.h" // DSP281x Examples Include File - -#include <281xEvTimersInit.h> -#include - -#include "TuneUpPlane.h" -#include "profile_interrupt.h" - -// Prototype statements for functions found within this file. -interrupt void eva_timer1_isr(void); -interrupt void eva_timer2_isr(void); -interrupt void evb_timer3_isr(void); -interrupt void evb_timer4_isr(void); - - -// Global counts used in this example -Uint32 EvaTimer1InterruptCount = 0; -Uint32 EvaTimer2InterruptCount = 0; -Uint32 EvbTimer3InterruptCount = 0; -Uint32 EvbTimer4InterruptCount = 0; - -//unsigned int enable_profile_led1_Timer1 = 1; -//unsigned int enable_profile_led1_Timer2 = 1; -//unsigned int enable_profile_led1_Timer3 = 1; -//unsigned int enable_profile_led1_Timer4 = 1; -// -//unsigned int enable_profile_led2_Timer1 = 0; -//unsigned int enable_profile_led2_Timer2 = 0; -//unsigned int enable_profile_led2_Timer3 = 0; -//unsigned int enable_profile_led2_Timer4 = 0; - -//Pointers to handler functions -void (*timer1_handler)() = NULL; -void (*timer2_handler)() = NULL; -void (*timer3_handler)() = NULL; -void (*timer4_handler)() = NULL; - -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -void init_eva_timer1(int freq, void (*timer_handler)()) -{ - // Initialize EVA Timer 1: - // Setup Timer 1 Registers (EV A) - EvaRegs.GPTCONA.all = 0; - - // Set the Period for the GP timer 1 to 0x0200; - EvaRegs.T1PR = 0x0200; // Period - EvaRegs.T1CMPR = 0x0000; // Compare Reg - - // Enable Period interrupt bits for GP timer 1 - // Count up, x128, internal clk, enable compare, use own period - EvaRegs.EVAIMRA.bit.T1PINT = 0;//1; - EvaRegs.EVAIFRA.all = BIT7; -// EvaRegs.EVAIFRA.bit.T1PINT = 1; - - // Clear the counter for GP timer 1 - EvaRegs.T1CNT = 0x0000; -// EvaRegs.T1PR = (float64)HSPCLK/(float64)(freq / 2); - EvaRegs.T1PR = (float64)HSPCLK/(float64)(freq); - EvaRegs.T1CON.all = FREE_RUN_FLAG + TIMER_CONT_UP + TIMER_CLK_PRESCALE_X_1 - + TIMER_ENABLE_BY_OWN + TIMER_ENABLE + TIMER_ENABLE_COMPARE; // - - // Start EVA ADC Conversion on timer 1 Period interrupt - EvaRegs.GPTCONA.bit.T1TOADC = 2; - - // Save pointer to handler in variable - timer1_handler = timer_handler; - - EALLOW; // This is needed to write to EALLOW protected registers - PieVectTable.T1PINT = &eva_timer1_isr; - EDIS; // This is needed to disable write to EALLOW protected registers - - // Enable PIE group 2 interrupt 4 for T1PINT -// PieCtrlRegs.PIEIER2.all = M_INT4; - PieCtrlRegs.PIEIER2.bit.INTx4 = 1; - - // Enable CPU INT2 for T1PINT, INT3 for T2PINT, INT4 for T3PINT - // and INT5 for T4PINT: - // IER |= M_INT2; -} - -void stop_eva_timer1() -{ - IER &= ~(M_INT2); - EvaRegs.EVAIMRA.bit.T1PINT = 0; -} - -void start_eva_timer1() -{ - IER |= (M_INT2); - EvaRegs.EVAIFRA.all = BIT7; - EvaRegs.EVAIMRA.bit.T1PINT = 1; -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -void init_eva_timer2(int freq, void (*timer_handler)()) -{ - // Initialize EVA Timer 2: - // Setup Timer 2 Registers (EV A) - EvaRegs.GPTCONA.all = 0; - - // Set the Period for the GP timer 2 to 0x0200; - EvaRegs.T2PR = 0x0400; // Period - EvaRegs.T2CMPR = 0x0000; // Compare Reg - - // Enable Period interrupt bits for GP timer 2 - // Count up, x128, internal clk, enable compare, use own period - EvaRegs.EVAIMRB.bit.T2PINT = 0;//1; - EvaRegs.EVAIFRB.all = BIT0; - // EvaRegs.EVAIFRB.bit.T2PINT = 1; - - // Clear the counter for GP timer 2 - EvaRegs.T2CNT = 0x0000; -// EvaRegs.T2PR = (float64)HSPCLK/(float64)(freq / 2); - EvaRegs.T2PR = (float64)HSPCLK/(float64)(freq); - EvaRegs.T2CON.all = FREE_RUN_FLAG + TIMER_CONT_UP + TIMER_CLK_PRESCALE_X_1 - + TIMER_ENABLE_BY_OWN + TIMER_ENABLE + TIMER_ENABLE_COMPARE; // - - // Start EVA ADC Conversion on timer 2 Period interrupt - EvaRegs.GPTCONA.bit.T2TOADC = 2; - - // Save pointer to handler in variable - timer2_handler = timer_handler; - - EALLOW; // This is needed to write to EALLOW protected registers - PieVectTable.T2PINT = &eva_timer2_isr; - EDIS; // This is needed to disable write to EALLOW protected registers - - // Enable PIE group 3 interrupt 1 for T2PINT -// PieCtrlRegs.PIEIER3.all = M_INT1; - PieCtrlRegs.PIEIER3.bit.INTx1 = 1;//M_INT1; - - // Enable CPU INT2 for T1PINT, INT3 for T2PINT, INT4 for T3PINT - // and INT5 for T4PINT: -// IER |= (M_INT3); -} - -void stop_eva_timer2() -{ - IER &= ~(M_INT3); - EvaRegs.EVAIMRB.bit.T2PINT = 0; -} - -void start_eva_timer2() -{ - IER |= (M_INT3); - EvaRegs.EVAIFRB.all = BIT0; - EvaRegs.EVAIMRB.bit.T2PINT = 1; -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -void init_evb_timer3(int freq, void (*timer_handler)()) -{ - // Initialize EVB Timer 3: - // Setup Timer 3 Registers (EV B) - EvbRegs.GPTCONB.all = 0; - - // Set the Period for the GP timer 3 to 0x0200; - EvbRegs.T3PR = 0x0800; // Period - EvbRegs.T3CMPR = 0x0000; // Compare Reg - - // Enable Period interrupt bits for GP timer 3 - // Count up, x128, internal clk, enable compare, use own period - EvbRegs.EVBIMRA.bit.T3PINT = 0;//1; - EvbRegs.EVBIFRA.all = BIT7; - // EvbRegs.EVBIFRA.bit.T3PINT = 1; - - // Clear the counter for GP timer 3 - EvbRegs.T3CNT = 0x0000; -// EvbRegs.T3PR = (float64)HSPCLK/(float64)(freq / 2); - EvbRegs.T3PR = (float64)HSPCLK/(float64)(freq); - EvbRegs.T3CON.all = FREE_RUN_FLAG + TIMER_CONT_UP + TIMER_CLK_PRESCALE_X_1 - + TIMER_ENABLE_BY_OWN + TIMER_ENABLE + TIMER_ENABLE_COMPARE; -// EvbRegs.T3CON.all = SOFT_STOP_FLAG + TIMER_CONT_UP + TIMER_CLK_PRESCALE_X_1 - // + TIMER_ENABLE_BY_OWN + TIMER_ENABLE + TIMER_ENABLE_COMPARE; - - // Save pointer to handler in variable - timer3_handler = timer_handler; - - // Start EVA ADC Conversion on timer 3 Period interrupt - EvbRegs.GPTCONB.bit.T3TOADC = 2; - - EALLOW; // This is needed to write to EALLOW protected registers - PieVectTable.T3PINT = &evb_timer3_isr; - EDIS; // This is needed to disable write to EALLOW protected registers - - // Enable PIE group 4 interrupt 4 for T3PINT -// PieCtrlRegs.PIEIER4.all = M_INT4; - PieCtrlRegs.PIEIER4.bit.INTx4 = 1; - - // Enable CPU INT2 for T1PINT, INT3 for T2PINT, INT4 for T3PINT - // and INT5 for T4PINT: - // IER |= M_INT4; -} - -void stop_evb_timer3() -{ - IER &= ~(M_INT4); - EvbRegs.EVBIMRA.bit.T3PINT = 0; -} - -void start_evb_timer3() -{ - IER |= (M_INT4); - - // Make sure PIEACK for group 2 is clear (default after reset) -// PieCtrlRegs.PIEACK.all = M_INT4; - EvbRegs.EVBIFRA.all = BIT7; - EvbRegs.EVBIMRA.bit.T3PINT = 1; -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -void init_evb_timer4(int freq, void (*timer_handler)()) -{ - // Initialize EVB Timer 4: - // Setup Timer 4 Registers (EV B) - EvbRegs.GPTCONB.all = 0; - - // Set the Period for the GP timer 4 to 0x0200; - EvbRegs.T4PR = 0x1000; // Period - EvbRegs.T4CMPR = 0x0000; // Compare Reg - - // Enable Period interrupt bits for GP timer 4 - // Count up, x128, internal clk, enable compare, use own period - EvbRegs.EVBIMRB.bit.T4PINT = 0;//1; - EvbRegs.EVBIFRB.all = BIT0; -// EvbRegs.EVBIFRB.bit.T4PINT = 1; - - // Clear the counter for GP timer 4 - EvbRegs.T4CNT = 0x0000; -// EvbRegs.T4PR = (float64)HSPCLK/(float64)(freq / 2); - EvbRegs.T4PR = (float64)HSPCLK/(float64)(freq); - EvbRegs.T4CON.all = FREE_RUN_FLAG + TIMER_CONT_UP + TIMER_CLK_PRESCALE_X_1 - + TIMER_ENABLE_BY_OWN + TIMER_ENABLE + TIMER_ENABLE_COMPARE; - - // Start EVA ADC Conversion on timer 4 Period interrupt - EvbRegs.GPTCONB.bit.T4TOADC = 2; - - // Save pointer to handler in variable - timer4_handler = timer_handler; - - EALLOW; // This is needed to write to EALLOW protected registers - PieVectTable.T4PINT = &evb_timer4_isr; - EDIS; // This is needed to disable write to EALLOW protected registers - - // Enable PIE group 5 interrupt 1 for T4PINT -// PieCtrlRegs.PIEIER5.all = M_INT1; - PieCtrlRegs.PIEIER5.bit.INTx1 = 1; - - // Enable CPU INT2 for T1PINT, INT3 for T2PINT, INT4 for T3PINT - // and INT5 for T4PINT: - // IER |= M_INT5; -} - -void stop_evb_timer4() -{ - IER &= ~(M_INT5); - EvbRegs.EVBIMRB.bit.T4PINT = 0; -} - -void start_evb_timer4() -{ - IER |= (M_INT5); - EvbRegs.EVBIFRB.all = BIT0; - EvbRegs.EVBIMRB.bit.T4PINT = 1; -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(eva_timer1_isr,".fast_run2"); -interrupt void eva_timer1_isr(void) -{ - // Set interrupt priority: - volatile Uint16 TempPIEIER = PieCtrlRegs.PIEIER2.all; - IER |= M_INT2; - IER &= MINT2; // Set "global" priority - PieCtrlRegs.PIEIER2.all &= MG24; // Set "group" priority - PieCtrlRegs.PIEACK.all = 0xFFFF; // Enable PIE interrupts - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer1) - i_led1_on_off_special(1); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer1) - i_led2_on_off_special(1); -#endif - EINT; - - // Insert ISR Code here....... - if(timer1_handler) - { - timer1_handler(); - } - - // Next line for debug only (remove after inserting ISR Code): -// ESTOP0; - EvaRegs.EVAIFRA.all = BIT7; - // Restore registers saved: - DINT; - PieCtrlRegs.PIEIER2.all = TempPIEIER; -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer1) - i_led1_on_off_special(0); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer1) - i_led2_on_off_special(0); -#endif - -// -// -// IER &= ~(M_INT2);// stop_eva_timer1(); -// // Enable more interrupts from this timer -// EvaRegs.EVAIMRA.bit.T1PINT = 1; -// -// // Note: To be safe, use a mask value to write to the entire -// // EVAIFRA register. Writing to one bit will cause a read-modify-write -// // operation that may have the result of writing 1's to clear -// // bits other then those intended. -//// EvaRegs.EVAIFRA.bit.T1PINT = 1; -// EvaRegs.EVAIFRA.all = BIT7; -// -// // Acknowledge interrupt to receive more interrupts from PIE group 2 -// PieCtrlRegs.PIEACK.all = PIEACK_GROUP2; -// -// // EINT; -// -// -// if(timer1_handler) -// { -// timer1_handler(); -// } - -// DINT; -// IER |= (M_INT2);//start_eva_timer1(); -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(eva_timer2_isr,".fast_run2"); -interrupt void eva_timer2_isr(void) -{ - - // Set interrupt priority: - volatile Uint16 TempPIEIER = PieCtrlRegs.PIEIER3.all; - IER |= M_INT3; - IER &= MINT3; // Set "global" priority - PieCtrlRegs.PIEIER3.all &= MG31; // Set "group" priority - PieCtrlRegs.PIEACK.all = 0xFFFF; // Enable PIE interrupts - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer2) - i_led1_on_off_special(1); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer2) - i_led2_on_off_special(1); -#endif - - EINT; - - // Insert ISR Code here....... - if(timer2_handler) - { - timer2_handler(); - } - - // Next line for debug only (remove after inserting ISR Code): -// ESTOP0; - EvaRegs.EVAIFRB.all = BIT0; - // Restore registers saved: - DINT; - PieCtrlRegs.PIEIER3.all = TempPIEIER; - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer2) - i_led1_on_off_special(0); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer2) - i_led2_on_off_special(0); -#endif - - - -// -// -// // IER &= ~(M_INT3);// stop_eva_timer2(); -// -// // Enable more interrupts from this timer -// EvaRegs.EVAIMRB.bit.T2PINT = 1; -// -// // Note: To be safe, use a mask value to write to the entire -// // EVAIFRB register. Writing to one bit will cause a read-modify-write -// // operation that may have the result of writing 1's to clear -// // bits other then those intended. -// EvaRegs.EVAIFRB.all = BIT0; -//// EvaRegs.EVAIFRB.bit.T2PINT = 1; -// -// -// // Acknowledge interrupt to receive more interrupts from PIE group 3 -// PieCtrlRegs.PIEACK.all = PIEACK_GROUP3; -// -// -//// EnableInterrupts(); -// -// if(timer2_handler) -// { -// timer2_handler(); -// } - - -// DINT; - // IER |= (M_INT3);//start_eva_timer2(); - - -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(evb_timer3_isr,".fast_run2"); -interrupt void evb_timer3_isr(void) -{ - // Set interrupt priority: - volatile Uint16 TempPIEIER = PieCtrlRegs.PIEIER4.all; - IER |= M_INT4; - IER &= MINT4; // Set "global" priority - PieCtrlRegs.PIEIER4.all &= MG44; // Set "group" priority - PieCtrlRegs.PIEACK.all = 0xFFFF; // Enable PIE interrupts - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer3) - i_led1_on_off_special(1); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer3) - i_led2_on_off_special(1); -#endif - - EINT; - - // Insert ISR Code here....... - if(timer3_handler) - { - timer3_handler(); - } - - // Next line for debug only (remove after inserting ISR Code): -// ESTOP0; - -// EvbRegs.EVBIMRA.bit.T3PINT = 1; - EvbRegs.EVBIFRA.all = BIT7; -// PieCtrlRegs.PIEACK.all = PIEACK_GROUP4; // Enable PIE interrupts - // Restore registers saved: - DINT; - PieCtrlRegs.PIEIER4.all = TempPIEIER; - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer3) - i_led1_on_off_special(0); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer3) - i_led2_on_off_special(0); -#endif - - -// -// -// IER &= ~(M_INT4);//stop_evb_timer3(); -// -// // Enable more interrupts from this timer -// EvbRegs.EVBIMRA.bit.T3PINT = 1; -// -// -// // IER &= ~(M_INT4); -// //EvbTimer3InterruptCount++; -// // Note: To be safe, use a mask value to write to the entire -// // EVBIFRA register. Writing to one bit will cause a read-modify-write -// // operation that may have the result of writing 1's to clear -// // bits other then those intended. -// EvbRegs.EVBIFRA.all = BIT7; -// -// // Acknowledge interrupt to receive more interrupts from PIE group 4 -// PieCtrlRegs.PIEACK.all = PIEACK_GROUP4; -// -// EINT; -// -// -// if(timer3_handler) -// { -// timer3_handler(); -// } -// // IFR &= ~(M_INT4); // ! -// // IER |= (M_INT4); -// -// DINT; -// IER |= (M_INT4);//start_evb_timer3(); -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(evb_timer4_isr,".fast_run2"); -interrupt void evb_timer4_isr(void) -{ - // Set interrupt priority: - volatile Uint16 TempPIEIER = PieCtrlRegs.PIEIER5.all; - IER |= M_INT5; - IER &= MINT5; // Set "global" priority - PieCtrlRegs.PIEIER5.all &= MG51; // Set "group" priority - PieCtrlRegs.PIEACK.all = 0xFFFF; // Enable PIE interrupts - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer4) - i_led1_on_off_special(1); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer4) - i_led2_on_off_special(1); -#endif - - EINT; - - // Insert ISR Code here....... - if(timer4_handler) - { - timer4_handler(); - } - - // Next line for debug only (remove after inserting ISR Code): -// ESTOP0; - EvbRegs.EVBIFRB.all = BIT0; - // Restore registers saved: - DINT; - PieCtrlRegs.PIEIER5.all = TempPIEIER; - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.timer4) - i_led1_on_off_special(0); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.timer4) - i_led2_on_off_special(0); -#endif - -// -// -// -// IER &= ~(M_INT5);//stop_evb_timer4(); -// -// EvbRegs.EVBIMRB.bit.T4PINT = 1; -// //EvbTimer4InterruptCount++; -// // Note: To be safe, use a mask value to write to the entire -// // EVBIFRB register. Writing to one bit will cause a read-modify-write -// // operation that may have the result of writing 1's to clear -// // bits other then those intended. -// EvbRegs.EVBIFRB.all = BIT0; -// -// // Acknowledge interrupt to receive more interrupts from PIE group 5 -// PieCtrlRegs.PIEACK.all = PIEACK_GROUP5; -// EINT; -// -// -// if(timer4_handler) -// { -// timer4_handler(); -// } -// -// DINT; -// IER |= (M_INT5);//start_evb_timer4(); -} -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////// - - -//=========================================================================== -// No more. -//=========================================================================== diff --git a/Inu/Src2/551/main/281xEvTimersInit.h b/Inu/Src2/551/main/281xEvTimersInit.h deleted file mode 100644 index 2498fc9..0000000 --- a/Inu/Src2/551/main/281xEvTimersInit.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef EV_TIMERS -#define EV_TIMERS - -void init_eva_timer1(int freqHz, void (*timer_handler)()); -void init_eva_timer2(int freqHz, void (*timer_handler)()); -void init_evb_timer3(int freqHz, void (*timer_handler)()); -void init_evb_timer4(int freqHz, void (*timer_handler)()); - -void start_eva_timer1(); -void stop_eva_timer1(); -void start_eva_timer2(); -void stop_eva_timer2(); -void start_evb_timer3(); -void stop_evb_timer3(); -void start_evb_timer4(); -void stop_evb_timer4(); - -#endif //EV_TIMERS diff --git a/Inu/Src2/551/main/CAN_project.h b/Inu/Src2/551/main/CAN_project.h deleted file mode 100644 index 3454f1e..0000000 --- a/Inu/Src2/551/main/CAN_project.h +++ /dev/null @@ -1,256 +0,0 @@ -/* - * CAN_project.h - * - * Created on: 21 2020 . - * Author: yura - */ - -#ifndef SRC_MAIN_CAN_PROJECT_H_ -#define SRC_MAIN_CAN_PROJECT_H_ - - -#include "can_protocol_ukss.h" - -////////////////////////////////////////////////////////////////// -// CAN -// PCH_1 PCH_2 67 -////////////////////////////////////////////////////////////////// -#define CAN_BASE_ADR_MPU_PCH_1 0x0CEB0E1 -#define CAN_BASE_ADR_MPU_PCH_2 0x0CEB0E1 //0x0CEB0E2 -////////////////////////////////////// -#define CAN_BASE_ADR_TERMINAL_PCH_1 0x0EEEE01 -#define CAN_BASE_ADR_TERMINAL_PCH_2 0x0EEEE03 -////////////////////////////////////// -#define CAN_BASE_ADR_UNITS_PCH_1 0x235500 -#define CAN_BASE_ADR_UNITS_PCH_2 0x235500 -////////////////////////////////////// -#define CAN_BASE_ADR_ALARM_LOG_PCH_1 0x0BCDEF01 -#define CAN_BASE_ADR_ALARM_LOG_PCH_2 0x0BCDEF02 -////////////////////////////////////////////////////////////////// - -//#define CAN_PROTOCOL_VERSION 1 -#define CAN_PROTOCOL_VERSION 2 - -#define CAN_SPEED_BITS 125000 -//#define CAN_SPEED_BITS 250000 -//#define CAN_SPEED_BITS 500000 - - - -#define ENABLE_CAN_SEND_TO_UKSS_FROM_MAIN 1 -#define ENABLE_CAN_SEND_TO_MPU_FROM_MAIN 1 -#define ENABLE_CAN_SEND_TO_TERMINAL_FROM_MAIN 0//1 -#define ENABLE_CAN_SEND_TO_TERMINAL_OSCIL 0//1 -#define ENABLE_CAN_SEND_TO_ANOTHER_BSU_FROM_MAIN 1 - - -////////////////////////////////////////////////////// - -#ifndef ENABLE_CAN_SEND_TO_UKSS_FROM_MAIN -#define ENABLE_CAN_SEND_TO_UKSS_FROM_MAIN 0 -#endif - -#ifndef ENABLE_CAN_SEND_TO_MPU_FROM_MAIN -#define ENABLE_CAN_SEND_TO_MPU_FROM_MAIN 0 -#endif - - -#ifndef ENABLE_CAN_SEND_TO_TERMINAL_FROM_MAIN -#define ENABLE_CAN_SEND_TO_TERMINAL_FROM_MAIN 0 -#endif - -#ifndef ENABLE_CAN_SEND_TO_TERMINAL_OSCIL -#define ENABLE_CAN_SEND_TO_TERMINAL_OSCIL 0 -#endif - -#ifndef ENABLE_CAN_SEND_TO_ANOTHER_BSU_FROM_MAIN -#define ENABLE_CAN_SEND_TO_ANOTHER_BSU_FROM_MAIN 0 -#endif - -/////////////////////////////////////////////////////////////////// -// setup ukss boxs -/////////////////////////////////////////////////////////////////// - -// -#define USE_UKSS_0 1 -#define USE_UKSS_1 1 -#define USE_UKSS_2 1 -#define USE_UKSS_3 1 -#define USE_UKSS_4 1 -//#define USE_UKSS_5 1 -//#define USE_UKSS_6 1 -//#define USE_UKSS_7 1 -//#define USE_UKSS_8 1 -//#define USE_UKSS_9 1 -//#define USE_UKSS_10 1 -//#define USE_UKSS_11 1 -//#define USE_UKSS_12 1 -//#define USE_UKSS_13 1 -//#define USE_UKSS_14 1 -//#define USE_UKSS_15 1 - - - -#define OFFSET_CAN_ADR_UNITS 0x10 - -// - -#if USE_UKSS_0 -#define ZADATCHIK_CAN 0 //Zadatchik -#endif - -#if USE_UKSS_1 -#define VPU_CAN 1 //VPU -#endif - -#if USE_UKSS_2 -#define UMU_CAN_DEVICE 2 //Voltage control UMU -#endif - -#if USE_UKSS_3 -#define BKSSD_CAN_DEVICE 3 //AC DRIVE -#endif - - -#if USE_UKSS_4 -#define ANOTHER_BSU1_CAN_DEVICE 4 //Another BSU1 -// Unites revers box or not -// revers box used only on CAN between BS1 BS2 -//#define USE_R_B_4 1 //0 -#endif - - -//#if USE_UKSS_5 -//#define ANOTHER_BSU2_CAN_DEVICE 5 //Another BSU2 -//// Unites revers box or not -//// revers box used only on CAN between BS1 BS2 -////#define USE_R_B_5 1 //0 -//#endif - - - - -/////////////////////////////////////////////////////////////////// -// setup mpu boxes -/////////////////////////////////////////////////////////////////// - -// -#define USE_MPU_0 1 -#define USE_MPU_1 1 -//#define USE_MPU_2 1 -//#define USE_MPU_3 1 - - -#define OFFSET_CAN_ADR_MPU 0x10 - -//#define MPU_CAN_DEVICE 2 // MPU -#define TIME_PAUSE_CAN_FROM_MPU 1000 - - - - -/////////////////////////////////////////////////////////////////// -// setup terminal boxes -/////////////////////////////////////////////////////////////////// -// -#define USE_TERMINAL_1_OSCIL 1 -#define USE_TERMINAL_1_CMD 1 -#define USE_TERMINAL_2_OSCIL 1 -#define USE_TERMINAL_2_CMD 1 - - -#define OFFSET_CAN_ADR_TERMINAL 0x10 -#define TIME_PAUSE_CAN_FROM_TERMINAL 2 - -/////////////////////////////////////////////////////////////////// -// setup ALARM_LOG box -/////////////////////////////////////////////////////////////////// -#define USE_ALARM_LOG_0 1 - -/////////////////////////////////////////////////////////////////// -// setup can_open boxes -/////////////////////////////////////////////////////////////////// -//#define BWC_CAN_DEVICE 7 // Water cooler -//#define INGITIM 9 - -//#define BWC_CAN_FATEC 1 -//#define BWC_CAN_SIEMENS 1 -//#define INGITIM_CAN_OPEN 1 - - -#define CANOPENUNIT_LEN 30 - - -/////////words from zadatchik from Ingitim -#define PDO1_W1_ADR 0x11 -#define PDO1_W2_ADR 0x12 -#define PDO1_W3_ADR 0x13 -#define PDO1_W4_ADR 0x14 -#define PDO2_W1_ADR 0x15 -#define PDO2_W2_ADR 0x16 -#define PDO2_W3_ADR 0x17 -#define PDO2_W4_ADR 0x18 -#define PDO3_W1_ADR 0x19 -#define PDO3_W2_ADR 0x1A -#define PDO3_W3_ADR 0x1B -#define PDO3_W4_ADR 0x1C -#define PDO5_W1_ADR 0x1D -#define PDO5_W2_ADR 0x1E -#define PDO5_W3_ADR 0x1F -#define PDO5_W4_ADR 0x20 -#define PDO6_W1_ADR 0x21 -#define PDO6_W2_ADR 0x22 -#define PDO6_W3_ADR 0x23 -#define PDO6_W4_ADR 0x24 -////////////////////////////////////// - -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -#define UNITS_NUMERATION_FROM_0_OR_1 0 //1 - - -#define MAX_CAN_WAIT_TIMEOUT 7500 // 2500 -//-------------------------------------------------// - -//#define CAN_ADR_TEST_LAMP 0x0CE031 //0x1CE030 - - -/////////////////////////////////////////////////////////////////// - - - - - - -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -// ukss -////////////////////////////////////////////////////////////////// -#define ADR_CAN_NUMBER1_ON_ZAD 0x00 -#define ADR_CAN_NUMBER2_ON_ZAD 0x01 -#define ADR_CAN_LAMPS_ON_ZAD 0x02 -#define ADR_CAN_LAMPS_2_ON_ZAD 0x03 -#define ADR_CAN_KEY_ON_ZAD 0x10 - -////////////////////////////////////// -////////////////////////////////////// - -#define CAN_SPEED_UKSS8 10 -#define CAN_SPEED_VPU1 20 -#define CAN_SPEED_VPU2 20 -#define CAN_SPEED_UMP1 5//30 - -//#define ADR_CAN_SPEED 99 -//#define ADR_CAN_LENGTH_FINISH_ADR 97 -//#define ADR_CAN_CMD 127 -//#define ADR_CAN_KEY_ON_VPU 16 // -//#define ADR_CAN_DOOR 0 // - - -////////////////////////////////////// -////////////////////////////////////// - - - -#endif /* SRC_MAIN_CAN_PROJECT_H_ */ diff --git a/Inu/Src2/551/main/Main.c b/Inu/Src2/551/main/Main.c deleted file mode 100644 index 9716206..0000000 --- a/Inu/Src2/551/main/Main.c +++ /dev/null @@ -1,126 +0,0 @@ -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" - -#include -#include "RS_Functions.h" -#include "xp_project.h" -#include "x_wdog.h" - -void main() -{ - // Step 1. Initialize System Control: - // PLL, WatchDog, enable Peripheral Clocks - // This example function is found in the DSP281x_SysCtrl.c file. - InitSysCtrl(); - - XintfZone0_Timing();//Xilinx Zone - XintfZone6_And7_Timing();//Flash Zone - XintfZone2_Timing();//External RAM Zone - - // Step 2. Initalize GPIO: - // This example function is found in the DSP281x_Gpio.c file and - // illustrates how to set the GPIO to it's default state. - // InitGpio(); // Skipped for this example - - // For this example use the following configuration: - //Gpio_select(); - - // Step 3. Clear all interrupts and initialize PIE vector table: - // Disable CPU interrupts - DINT; - -// status_interrupts = __disable_interrupts(); - - // Initialize PIE control registers to their default state. - // The default state is all PIE interrupts disabled and flags - // are cleared. - // This function is found in the DSP281x_PieCtrl.c file. - - InitPieCtrl(); - -// __disable_interrupts(); - - // Disable CPU interrupts and clear all CPU interrupt flags: - IER = 0x0000; - IFR = 0x0000; - - // Initialize the PIE vector table with pointers to the shell Interrupt - // Service Routines (ISR). - // This will populate the entire table, even if the interrupt - // is not used in this example. This is useful for debug purposes. - // The shell ISR routines are found in DSP281x_DefaultIsr.c. - // This function is found in DSP281x_PieVect.c. - - InitPieVectTable(); - - // Step 4. Initialize all the Device Peripherals: - // This function is found in DSP281x_InitPeripherals.c - // InitPeripherals(); // Not required for this example - - FlashInit(); - - SetupLedsLine(); -//while(1) -{ - i_led2_on_off(0); - i_led1_on_off(1); - DELAY_US(500000); - i_led2_on_off(1); - i_led1_on_off(0); - DELAY_US(500000); - i_led2_on_off(0); - i_led1_on_off(0); -} - - - RS232_TuneUp(RS232_SPEED_A, RS232_SPEED_B); - - KickDog(); - - // , ! - edrk_init_before_main(); - - // , RS232 - project.enable_all_interrupt(); - - - DINT; - i_led2_on_off(1); - i_led1_on_off(1); - DELAY_US(500000); - i_led2_on_off(0); - i_led1_on_off(0); - DELAY_US(500000); - i_led2_on_off(1); - i_led1_on_off(1); - DELAY_US(500000); - i_led2_on_off(0); - i_led1_on_off(0); - DELAY_US(500000); - project.enable_all_interrupt(); - - // - edrk_init_before_loop(); - - // main - for(;;) - { - // , - - edrk_go_main(); - // rs232 - RS232_WorkingWith(1,0,0); -// static int fff=0; -// if (fff) -// { -// Answer(&rs_a, CMD_RS232_POKE); -// fff = 0; -// } - - } - - -} - - diff --git a/Inu/Src2/551/main/PWMTMSHandle.c b/Inu/Src2/551/main/PWMTMSHandle.c deleted file mode 100644 index 2fe71b1..0000000 --- a/Inu/Src2/551/main/PWMTMSHandle.c +++ /dev/null @@ -1,510 +0,0 @@ - -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File - - -#include -#include -#include -#include -#include -#include - -#include "CAN_Setup.h" -#include "global_time.h" -#include "RS_Functions.h" - - - -int - m_PWM = 1, /* 1- , 0- */ - Dpwm = 12500, - Fpwm = 1000, - Dpwm2 = 6250, - Dpwm4 = 3125, - Zpwm = 1; // -//TODO -static int mPWM_a = 0, mPWM_b = 0; // b 1 - ., 0 - . - -PWMGEND pwmd = PWMGEND_DEFAULTS; - - -#if (TMSPWMGEN==1) - -#define DMIN 750 // 15mks Dminimum - -interrupt void PWM_Handler(void) -{ - -// static unsigned int time_tick_sec_mks=0; - static unsigned int pwm_run=0; - - // Enable more interrupts from this timer - EvaRegs.EVAIMRA.bit.T1PINT = 1; - - // Note: To be safe, use a mask value to write to the entire - // EVAIFRA register. Writing to one bit will cause a read-modify-write - // operation that may have the result of writing 1's to clear - // bits other then those intended. - EvaRegs.EVAIFRA.all = BIT7; - - // Acknowledge interrupt to receive more interrupts from PIE group 2 - PieCtrlRegs.PIEACK.all = PIEACK_GROUP2; - - - - // PWM_ticks++; - - if (pwm_run==1) - { -// stop_pwm(); - } - else - { - - pwm_run=1; - - - EnableInterrupts(); - - - // if (time_tick_sec_mks>FREQ_PWM) -// { -// time_tick_sec++; -// time_tick_sec_mks=0; - // } -// else - // time_tick_sec_mks++; - - // rs_a.time_wait_rs_out++; -// rs_b.time_wait_rs_out++; - // rs_a.time_wait_rs_out_mpu++; -// rs_b.time_wait_rs_out_mpu++; - - - global_time.calc(&global_time); - inc_RS_timeout_cicle(); - inc_CAN_timeout_cicle(); - -// led1_on_off(1); - PWM_interrupt(); /* y */ -// led1_on_off(0); - - pwm_run=0; - - } -/* - // Enable more interrupts from this timer - EvaRegs.EVAIMRA.bit.T1PINT = 1; - - // Note: To be safe, use a mask value to write to the entire - // EVAIFRA register. Writing to one bit will cause a read-modify-write - // operation that may have the result of writing 1's to clear - // bits other then those intended. - EvaRegs.EVAIFRA.all = BIT7; - - // Acknowledge interrupt to receive more interrupts from PIE group 2 - PieCtrlRegs.PIEACK.all = PIEACK_GROUP2; - -*/ - -// led2_on_off(0); -// disable(); /* y TMS */ - - -// SCIb_RX_Int_enable(); -// SCIa_RX_Int_enable(); - - - -} -#endif - - -#if (TMSPWMGEN==1) -void init_eva_evb() -{ - -// unsigned int tload; - -// stop_pwm(); - - EALLOW; - -// EVA Configure T1PWM, T2PWM, PWM1-PWM6 -// Initalize the timers - // Initalize EVA Timer1 - - - PieVectTable.T1PINT=&PWM_Handler; - - - EvaRegs.EVAIMRA.bit.T1PINT = 1; - EvaRegs.EVAIFRA.bit.T1PINT = 1; - // Enable PIE group 2 interrupt 4 for T1PINT - PieCtrlRegs.PIEIER2.bit.INTx4 = 1; - - -// EvaRegs.EVAIFRA.bit.T1OFINT=1; -// PieVectTable.T1OFINT = &PWM_Handler2; -// EvaRegs.EVAIMRA.bit.T1OFINT = 1; -// EvaRegs.EVAIFRA.bit.T1OFINT = 1; - // Enable PIE group 2 interrupt 7 for T1PINT -// PieCtrlRegs.PIEIER2.bit.INTx7 = 1; - - -//#ifdef DOUBLE_UPDATE_PWM - -// PieVectTable.T1UFINT = &PWM_Handler2; -// EvaRegs.EVAIMRA.bit.T1UFINT = 1; -// EvaRegs.EVAIFRA.bit.T1UFINT = 1; - - // Enable PIE group 2 interrupt 7 for T1PINT -// PieCtrlRegs.PIEIER2.bit.INTx6 = 1; - -//#endif - - -// EvaRegs.EVAIFRA.bit.T1CINT=1; -// PieVectTable.T1CINT = &PWM_Handler2; -// EvaRegs.EVAIMRA.bit.T1CINT = 1; -// EvaRegs.EVAIFRA.bit.T1CINT = 1; - // Enable PIE group 2 interrupt 7 for T1PINT -// PieCtrlRegs.PIEIER2.bit.INTx5 = 1; - - - IER |= M_INT2; - - EDIS; -// start_pwm(); - - -} -#endif - - -#if (TMSPWMGEN==1) -void setup_tms_pwm_int(int pwm_freq, int one_two, int pwm_protect) -{ - - - float64 pwm_period; -// pwm_tick; - -// int prev_interrupt; - - // init_vector(); - -// f_disable(); /* y TMS */ -// *(int *)(VECT_TABLE + VECT_INT2) = (int)PWM_Handler; /* */ -// SET_IEMASK_INT2(); /* y */ - - pwm_period = (float64)HSPCLK/(float64)pwm_freq/2.0; - - Fpwm = (int)pwm_freq; - Dpwm = (int)pwm_period; - Dpwm2 = (int)(pwm_period/2); - Dpwm4 = (int)(pwm_period/4); - - // stop_pwm(); /* */ - // setup_pwm_out(); - - - - - init_eva_evb(); - - - - - EvbRegs.EVBIFRA.bit.PDPINTB=1; - EvaRegs.EVAIFRA.bit.PDPINTA=1; - -// EVBIFRB - // Initialize PWM module - pwmd.PeriodMax = Dpwm; // Perscaler X1 (T1), ISR period = T x 1 - pwmd.PeriodMin = DMIN; // Perscaler X1 (T1), ISR period = T x 1 - -// ARpwmd.PeriodMax = Dpwm; -// ARpwmd.PeriodMin = DMIN; -// - pwmd.ShiftPhaseA = 0;//Dpwm/6.0; - pwmd.ShiftPhaseB = 0; - - pwmd.init(&pwmd); - -// ARpwmd.init(&pwmd); - // m.m2.bit.WDog_pwm = 0; - - - -/* pwm1.PeriodMax = Dpwm; // Perscaler X1 (T1), ISR period = T x 1 - pwm1.init(&pwm1); - - pwm2.PeriodMax = Dpwm; // Perscaler X1 (T1), ISR period = T x 1 - pwm2.init(&pwm2); */ - - -// if(one_two < 1.5) one_two = 0; -// if(one_two > 1.5) one_two = 0x80000000; - - -// addr_xilinx(WG_COUNT) = pwm_divisor; /* */ -// addr_xilinx(WG_PERIOD) = Dpwm<<16; /* */ -// addr_xilinx(ADR_INT) = one_two; /* */ -// addr_xilinx(WG_PROTECT) = pwm_protect; /* */ - - - -//Invoking the computation function -//svgen_mf1. - -// Initialize the SVGEN_MF module -/* - svgen_mf1.FreqMax = _IQ(6*BASE_FREQ*T); - svgen_mf2.FreqMax = _IQ(6*BASE_FREQ*T); - - - svgen_mf2.Offset=_IQ(0); - svgen_mf1.Offset=_IQ(0); - - svgen_mf1.Alpha = _IQ(0); - svgen_mf2.Alpha = _IQ(0.52359877559829887307710723054658); - - - k=0.1; //0.9; - freq = 0.499; - -*/ -//svgen_mf1.calc(&svgen_mf1); -//svgen_mf2.calc(&svgen_mf2); - -// start_pwm_a(); -// start_pwm_b(); - - - i_WriteMemory(ADR_PWM_DRIVE_MODE, 0x0000); // TMS - -} -#endif - -/********************************************************************/ -/* */ -/********************************************************************/ -void start_tms_pwm_a() -{ - unsigned int mask_tk_lines; - mPWM_a = 1; - - - EALLOW; - EvaRegs.COMCONA.all = 0xa600;//0xA600; // Init COMCONA Register - - EvaRegs.ACTRA.all = 0x0999; - EDIS; - - -} - -void start_tms_pwm_b() -{ - unsigned int mask_tk_lines; - mPWM_b = 1; - - - EALLOW; - EvbRegs.COMCONB.all = 0xa600;//0xA600; // Init COMCONA Register - - EvbRegs.ACTRB.all = 0x0999; - EDIS; - - - -} - - - -void start_tms_pwm(void) -{ - mPWM_a = 1; - mPWM_b = 1; - - - m_PWM = 0; - // m.m1.bit.PWM=0; - // m.m1.bit.PWM_A=0; - // m.m1.bit.PWM_B=0; - - EALLOW; - // addr_xilinx(WG_OUT)=0x00; - - EvaRegs.COMCONA.all = 0xa600;//0xA600; // Init COMCONA Register - EvbRegs.COMCONB.all = 0xa600;//0xA600; // Init COMCONA Register - - EvaRegs.ACTRA.all = 0x0999; - EvbRegs.ACTRB.all = 0x0999; - -// EvaRegs.GPTCONA.bit.TCMPOE=0; -// EvbRegs.GPTCONB.bit.TCMPOE=0; -// EvaRegs.T1CON.bit.TECMPR=1; -// EvbRegs.T3CON.bit.TECMPR=1; - EDIS; - -} - -/********************************************************************/ -/* */ -/********************************************************************/ -void start_select_tms_pwm(unsigned int mask) -{ - unsigned int mask_pwm_a,mask_pwm_b; - unsigned char b,i; - - - EALLOW; - - - - EvaRegs.ACTRA.all = 0x0fff; - EvbRegs.ACTRB.all = 0x0fff; - - EvaRegs.COMCONA.all = 0xa600;//0xA600; // Init COMCONA Register - EvbRegs.COMCONB.all = 0xa600;//0xA600; // Init COMCONA Register - - - mask_pwm_a=0; - for (i=0;i<6;i++) - { - b=(mask >> i) & 1; - if (b==0) - mask_pwm_a |= (1 << (2*i) ); - else - mask_pwm_a |= (3 << (2*i) ); - - - } - - mask_pwm_b=0; - for (i=0;i<6;i++) - { - b=(mask >> (i+8)) & 1; - if (b==0) - mask_pwm_b |= (1 << (2*i) ); - else - mask_pwm_b |= (3 << (2*i) ); - - } - - EvaRegs.ACTRA.all = mask_pwm_a; - EvbRegs.ACTRB.all = mask_pwm_b; - - EDIS; -} - - -/********************************************************************/ -/* */ -/********************************************************************/ -//#pragma CODE_SECTION(stop_pwm,".fast_run"); -void stop_tms_pwm(void) -{ - mPWM_a = 0; - mPWM_b = 0; - - - m_PWM = 1; - // m.m1.bit.PWM=1; - // m.m1.bit.PWM_A=1; - // m.m1.bit.PWM_B=1; - - EALLOW; - // EvaRegs.GPTCONA.bit.TCMPOE=1; - // EvbRegs.GPTCONB.bit.TCMPOE=1; - // EvaRegs.T1CON.bit.TECMPR=0; - // EvbRegs.T3CON.bit.TECMPR=0; - - // addr_xilinx(WG_OUT)=0x0fff; // - EvaRegs.ACTRA.all = 0x0fff; - EvbRegs.ACTRB.all = 0x0fff; - - // EvaRegs.COMCONA.all = 0xa400;//0xA600; // Init COMCONA Register - // EvbRegs.COMCONB.all = 0xa400;//0xA600; // Init COMCONA Register - - // EvaRegs.COMCONA.bit.FCMP6OE=0; - - - - //EvbRegs.COMCONB.bit.FCOMPOE=0; - //EvaRegs.COMCONA.bit.CENABLE=0; - //EvbRegs.COMCONB.bit.CENABLE=0; - - - EDIS; - - -} - -void stop_tms_pwm_a() -{ - unsigned int mask_tk_lines; -// m_PWM = 1; - mPWM_a = 0; - - EALLOW; - EvaRegs.ACTRA.all = 0x0fff; - EDIS; - -} - - -void stop_tms_pwm_b() -{ - unsigned int mask_tk_lines; - m_PWM = 1; - mPWM_b = 0; - - - - EALLOW; - EvbRegs.ACTRB.all = 0x0fff; - EDIS; - -} - -void setup_tms_pwm_out(void) -{ -//int b; -#if (TMSPWMGEN==1) - EALLOW; - -// GpioMuxRegs.GPDMUX.bit.T3CTRIP_PDPB_GPIOD5=0; -// GpioMuxRegs.GPDDIR.bit.GPIOD5=0; -// GpioDataRegs.GPDSET.bit.GPIOD5=1; - - -// GpioDataRegs.GPDCLEAR.bit.GPIOD5=1; - - - - GpioMuxRegs.GPAMUX.bit.PWM1_GPIOA0=1; - GpioMuxRegs.GPAMUX.bit.PWM2_GPIOA1=1; - GpioMuxRegs.GPAMUX.bit.PWM3_GPIOA2=1; - GpioMuxRegs.GPAMUX.bit.PWM4_GPIOA3=1; - GpioMuxRegs.GPAMUX.bit.PWM5_GPIOA4=1; - GpioMuxRegs.GPAMUX.bit.PWM6_GPIOA5=1; - - GpioMuxRegs.GPBMUX.bit.PWM7_GPIOB0=1; - GpioMuxRegs.GPBMUX.bit.PWM8_GPIOB1=1; - GpioMuxRegs.GPBMUX.bit.PWM9_GPIOB2=1; - GpioMuxRegs.GPBMUX.bit.PWM10_GPIOB3=1; - GpioMuxRegs.GPBMUX.bit.PWM11_GPIOB4=1; - GpioMuxRegs.GPBMUX.bit.PWM12_GPIOB5=1; - - EDIS; - // -// write_memory(adr_oe_buf_v,0); -#endif -} - - - diff --git a/Inu/Src2/551/main/PWMTMSHandle.h b/Inu/Src2/551/main/PWMTMSHandle.h deleted file mode 100644 index ab1675c..0000000 --- a/Inu/Src2/551/main/PWMTMSHandle.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef _PWMTMSHANDLE_H -#define _PWMTMSHANDLE_H - -void setup_tms_pwm_out(void); -void start_tms_pwm(void); -void stop_tms_pwm(void); - -void start_tms_pwm_b(); -void start_tms_pwm_a(); -void stop_tms_pwm_a(); -void stop_tms_pwm_b(); - - -void setup_tms_pwm_int(int pwm_freq, int one_two, int pwm_protect); - -void start_break_pwm(void); -void stop_break_pwm(void); - - - - - - -#endif - diff --git a/Inu/Src2/551/main/PWMTools.c b/Inu/Src2/551/main/PWMTools.c deleted file mode 100644 index b62f7b3..0000000 --- a/Inu/Src2/551/main/PWMTools.c +++ /dev/null @@ -1,2438 +0,0 @@ -#include -#include -#include -#include -#include -#include //22.06 -#include -#include -#include -#include -//#include -#include -#include -#include //22.06 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "f281xpwm.h" - -//#include "SpaceVectorPWM.h" -#include "CAN_Setup.h" -#include "global_time.h" -#include "IQmathLib.h" -#include "mathlib.h" -#include "oscil_can.h" -#include "rmp_cntl_v1.h" -#include "uf_alg_ing.h" -#include "vhzprof.h" -#include "vector_control.h" -#include "MemoryFunctions.h" -#include "RS_Functions.h" -#include "TuneUpPlane.h" -#include "xp_write_xpwm_time.h" -#include "pwm_test_lines.h" -#include "detect_errors.h" -#include "modbus_table_v2.h" -#include "params_alg.h" -#include "v_rotor_22220.h" -#include "log_to_memory.h" -#include "log_params.h" -#include "limit_power.h" -#include "pwm_logs.h" -#include "optical_bus_tools.h" -#include "ramp_zadanie_tools.h" -#include "pll_tools.h" - - -///////////////////////////////////// -#if (_SIMULATE_AC==1) -#include "sim_model.h" -#endif - -//#pragma DATA_SECTION(freq1,".fast_vars1"); -//_iq freq1; - -//#pragma DATA_SECTION(k1,".fast_vars1"); -//_iq k1 = 0; - -#define ENABLE_LOG_INTERRUPTS 0 //1 - - -#if (ENABLE_LOG_INTERRUPTS) - -#pragma DATA_SECTION(log_interrupts,".slow_vars"); -#define MAX_COUNT_LOG_INTERRUPTS 100 -unsigned int log_interrupts[MAX_COUNT_LOG_INTERRUPTS+2] = {0}; - - - - -///////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////// - - -void add_log_interrupts(int cmd) -{ - static int count_log_interrupst = 0; - - if (count_log_interrupst>=MAX_COUNT_LOG_INTERRUPTS) - count_log_interrupst = 0; - - - log_interrupts[count_log_interrupst++] = cmd; - log_interrupts[count_log_interrupst++] = EvbRegs.T3CNT; - - -//#if (ENABLE_LOG_INTERRUPTS) -// add_log_interrupts(0); -//#endif - -} - -#endif //if (ENABLE_LOG_INTERRUPTS) - - - -#pragma DATA_SECTION(iq_U_1_save,".fast_vars1"); -_iq iq_U_1_save = 0; -#pragma DATA_SECTION(iq_U_2_save,".fast_vars1"); -_iq iq_U_2_save = 0; - - -unsigned int enable_calc_vector = 0; - - - -//WINDING winding1 = WINDING_DEFAULT; - -//#define COUNT_SAVE_LOG_OFF 50 // -#define COUNT_START_IMP 5 //10 - -#define CONST_005 838860 -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// - - -#pragma CODE_SECTION(global_time_interrupt,".fast_run2"); -void global_time_interrupt(void) -{ - -// inc_RS_timeout_cicle(); -// inc_CAN_timeout_cicle(); - -#if(_ENABLE_PWM_LINES_FOR_TESTS) -// PWM_LINES_TK_18_ON; -#endif - - if (edrk.disable_interrupt_timer3) - return; - -//i_led1_on_off(1); - - - if (sync_data.latch_interrupt && sync_data.enabled_interrupt) - { - // ! - // - start_sync_interrupt(); - } - -#if (ENABLE_LOG_INTERRUPTS) - add_log_interrupts(1); -#endif - - global_time.calc(&global_time); - -#if (ENABLE_LOG_INTERRUPTS) - add_log_interrupts(101); -#endif - -/* -static unsigned int oldest_time = 0, time_pause = TIME_PAUSE_MODBUS_REMOUTE; -control_station_test_alive_all_control(); - if (detect_pause_milisec(time_pause,&oldest_time)) - modbusNetworkSharing(0); - - RS232_WorkingWith(0,1); -*/ - - -//i_led1_on_off(0); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - // PWM_LINES_TK_18_OFF; -#endif -} - -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -//#define get_tics_timer_pwm2(k) {time_buf2[k] = (EvbRegs.T3CNT-start_tics_4timer);k++;} -//unsigned int time_buf2[10] = {0}; - - -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -#define PAUSE_INC_TIMEOUT_CICLE 10 // FREQPWM/10 -void pwm_inc_interrupt(void) -{ - static unsigned int t_inc = 0; - - if (t_inc>=PAUSE_INC_TIMEOUT_CICLE) - { - inc_RS_timeout_cicle(); - inc_CAN_timeout_cicle(); - t_inc = 0; - } - else - t_inc++; -} -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(pwm_analog_ext_interrupt,".fast_run2"); -void pwm_analog_ext_interrupt(void) -{ -// static int count_timer_buf2=0, start_tics_4timer = 0, c_rms = 0; -// static _iq prev_Iu=0, prev_Ua=0; - //static _iq iq_50hz_norma = _IQ(50.0/NORMA_FROTOR); - -// i_led2_on(); - - // -// start_tics_4timer = EvaRegs.T1CNT; - -// count_timer_buf2 = 0; -// get_tics_timer_pwm2(count_timer_buf2); - - if (edrk.SumSbor == 1) { -// detect_protect_adc(uf_alg.tetta_bs, uf_alg.tetta_bs); - } - - - - calc_pll_50hz(); - -// -// if (c_rms>=9) -// { -// edrk.test_rms_Iu = calc_rms(analog.iqIu,prev_Iu,edrk. f_stator); -// edrk.test_rms_Ua = calc_rms(analog.iqUin_A1B1,prev_Ua, iq_50hz_norma); -// -// prev_Iu = analog.iqIu; -// prev_Ua = analog.iqUin_A1B1; -// c_rms = 0; -// } -// else -// c_rms++; - -// fill_RMS_buff_interrupt(uf_alg.tetta_bs, uf_alg.tetta_bs); - - // get_tics_timer_pwm2(count_timer_buf2); -// i_led2_off(); - - -// global_time.calc(&global_time); - -} -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// - -inline void init_regulators() -{ -// if(f.Mode != 0) -// { -//// pwm_vector_model_titov(0, 0, /*rotor.iqW*/0, 0); -// } -} - -#define select_working_channels(go_a, go_b) {go_a = !f.Obmotka1; \ - go_b = !f.Obmotka2;} - -#define MAX_COUNT_WAIT_GO_0 FREQ_PWM // 1 . - - -#define PAUSE_ERROR_DETECT_UPDATE_OPTBUS_DATA 900// ((unsigned int)(1*FREQ_PWM*2)) // ~1sec //50 -#define MAX_TIMER_WAIT_SET_TO_ZERO_ZADANIE 27000 //((unsigned int)(30*FREQ_PWM*2)) // 60 sec -//#define MAX_TIMER_WAIT_BOTH_READY2 108000 //(120*FREQ_PWM*2) // 120 sec -#define MAX_TIMER_WAIT_BOTH_READY2 216000 //(120*FREQ_PWM*2) // 240 sec -#define MAX_TIMER_WAIT_MASTER_SLAVE 4500 // 5 sec - -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -//#define _ENABLE_PWM_LED2_PROFILE 1 - - -#if (_ENABLE_PWM_LED2_PROFILE) -unsigned int profile_pwm[30]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0}; -unsigned int pos_profile_pwm = 0; -#endif - -/////////////////////////////////////////////////////////////////// -#define _ENABLE_LOG_TICS_PWM 0//1 -#define _ENABLE_SLOW_PWM 0//1 -#define _ENABLE_INTERRUPT_PWM_LED2 0//1 - -#if (_ENABLE_LOG_TICS_PWM==1) - -static int c_run=0; -static int c_run_start=0; -static int i_log; - - -#pragma DATA_SECTION(time_buf,".slow_vars"); -#define MAX_COUNT_TIME_BUF 50 -int time_buf[MAX_COUNT_TIME_BUF] = {0}; - -//#define get_tics_timer_pwm(flag,k) {if (flag) {time_buf[k] = (unsigned int)(EvbRegs.T3CNT-start_tics_4timer);k++;}else{time_buf[k] = -1; k++;}} - -#define set_tics_timer_pwm(flag,k) { time_buf[k] = flag;k++; } - -//#define get_tics_timer_pwm(flag,k) if (flag) ? {time_buf[k] = (EvbRegs.T3CNT-start_tics_4timer);k++;} : {time_buf[k] = -1; k++;}; -static int count_timer_buf=0; - -#else - -#define get_tics_timer_pwm(flag) asm(" NOP;") -#define set_tics_timer_pwm(flag,k) asm(" NOP;") -//static int count_timer_buf=0; - -#endif - - -#if(_ENABLE_SLOW_PWM) -static int slow_pwm_pause = 0; -#endif - -unsigned int count_time_buf = 0; -int stop_count_time_buf=0; -unsigned int log_wait; - -unsigned int end_tics_4timer, start_tics_4timer; -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////// -#if (_ENABLE_LOG_TICS_PWM==1) -//#pragma CODE_SECTION(get_tics_timer_pwm,".fast_run"); -void get_tics_timer_pwm(unsigned int flag) -{ - unsigned int delta; - - if (flag) - { - delta = (unsigned int)(EvbRegs.T3CNT-start_tics_4timer); - if (count_timer_buf>=3) - time_buf[count_timer_buf] = delta - time_buf[count_timer_buf-2]; - else - time_buf[count_timer_buf] = delta; - time_buf[count_timer_buf] = time_buf[count_timer_buf]*33/1000; - count_timer_buf++; - } - else - { - time_buf[count_timer_buf] = -1; - count_timer_buf++; - } -} -#else - -#endif - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////// -// -/////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(calc_rotors,".fast_run"); -void calc_rotors(int flag) -{ - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_17_ON; -#endif - - update_rot_sensors(); - - - - set_tics_timer_pwm(6,count_timer_buf); - get_tics_timer_pwm(flag); - - -#if(C_cds_in_number>=1) - project.cds_in[0].read_pbus(&project.cds_in[0]); - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_17_OFF; -#endif - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_19_ON; -#endif - -#if(SENSOR_ALG==SENSOR_ALG_23550) -// 23550 - RotorMeasureDetectDirection(); - RotorMeasure();// -#endif - -#if(SENSOR_ALG==SENSOR_ALG_22220) -// 22220 - Rotor_measure_22220(); - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_19_OFF; -#endif - - set_tics_timer_pwm(7,count_timer_buf); - get_tics_timer_pwm(flag); - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_20_ON; -#endif - -// RotorMeasurePBus(); -#if(SENSOR_ALG==SENSOR_ALG_23550) -// 23550 - RotorDirectionFilter(WRotorPBus.RotorDirectionInstant, &WRotorPBus.RotorDirectionSlow, &WRotorPBus.RotorDirectionSlow2, &WRotorPBus.RotorDirectionCount); -#endif - -#if(SENSOR_ALG==SENSOR_ALG_22220) - // 22220 - // nothing -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_20_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_20_ON; -#endif - -#if(SENSOR_ALG==SENSOR_ALG_23550) -// 23550 - select_values_wrotor(); -#endif -#if(SENSOR_ALG==SENSOR_ALG_22220) -// 22220 - select_values_wrotor_22220(); - -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_20_OFF; -#endif - - set_tics_timer_pwm(8,count_timer_buf); - get_tics_timer_pwm(flag); - -#endif //(C_cds_in_number>=1) - - - edrk.rotor_direction = WRotor.RotorDirectionSlow; - -#if(SENSOR_ALG==SENSOR_ALG_23550) -// 23550 - edrk.iq_f_rotor_hz = WRotor.iqWRotorSumFilter; -#endif - -#if(SENSOR_ALG==SENSOR_ALG_22220) -// 22220 - edrk.iq_f_rotor_hz = WRotor.iqWRotorSum; -#endif - -} - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(calc_zadanie_rampa,".fast_run"); -void calc_zadanie_rampa(void) -{ -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_19_ON; -#endif - - - // - load_current_ramp_oborots_power(); - - if (edrk.StartGEDfromControl==0) - ramp_all_zadanie(2); // - else - if (edrk.flag_wait_set_to_zero_zadanie || edrk.flag_block_zadanie || edrk.Status_Ready.bits.ready_final==0 || /*edrk.StartGEDfromControl==0 ||*/ edrk.run_razbor_shema == 1) - ramp_all_zadanie(1); // , , edrk.StartGEDfromZadanie - else - ramp_all_zadanie(0); // - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_19_OFF; -#endif - -} - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -#pragma CODE_SECTION(async_pwm_ext_interrupt,".fast_run2"); -void async_pwm_ext_interrupt(void) -{ - -#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) - PWM_LINES_TK_19_ON; -#endif - if (edrk.run_to_pwm_async) - { - PWM_interrupt(); - edrk.run_to_pwm_async = 0; - } - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) - PWM_LINES_TK_19_OFF; -#endif - -} -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// -void run_detect_fast_error(void) -{ - - detect_error_u_zpt_fast(); - detect_error_u_in(); - -} -//////////////////////////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(PWM_interrupt_main,".fast_run"); -void PWM_interrupt_main(void) -{ - -#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) - PWM_LINES_TK_16_ON; -#endif - - norma_adc_nc(0); - - edrk.run_to_pwm_async = 1; - -#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) - PWM_LINES_TK_16_OFF; -#endif - -} -//////////////////////////////////////////////////////////////////////////////// - - -#define MAX_COUNT_COUNTSTARTGEDFROMZADANIE FREQ_PWM //3000 // edrk.StartGEDfromZadanie - - - -#pragma CODE_SECTION(PWM_interrupt,".fast_run"); -void PWM_interrupt(void) -{ - - static unsigned int pwm_run = 0; - static _iq Uzad1=0, Fzad=0, Uzad2=0, Izad_out = 0, Uzad_from_master = 0; - -// static int count_step_ram_off = 0; -// static int count_start_impuls = 0; - - static int prevGo = -1; - static volatile unsigned int go_a = 0; - static volatile unsigned int go_b = 0; - - static int prev_go_a = 0; - static int prev_go_b = 0; - - static _iq iq_U_1_prev = 0; - static _iq iq_U_2_prev = 0; - static unsigned int prev_timer = 0; - unsigned int cur_timer; - static unsigned int count_lost_interrupt=0; - static int en_rotor = 1;//1; - - static unsigned long timer_wait_set_to_zero_zadanie = 0; - static unsigned long timer_wait_both_ready2 = 0; - - static unsigned int prev_error_controller = 0,error_controller=0; - - static unsigned long time_delta = 0; - - static unsigned int run_calc_uf = 0, prev_run_calc_uf = 0, count_wait_go_0 = 0; - - int pwm_enable_calc_main = 0, pwm_up_down = 0, err_interr = 0, slow_error = 0; - - static unsigned int count_err_read_opt_bus = 0, prev_edrk_Kvitir = 0; - - static unsigned int count_wait_read_opt_bus = 0, old_count_ok = 0, data_ready_optbus = 0, count_ok_read_opt_bus = 0; - -// static T_cds_optical_bus_data_in buff[25]={0}; - static unsigned int flag_last_error_read_opt_bus = 0, sum_count_err_read_opt_bus1=0; - - static unsigned int count_read_slave = 0, flag1_change_moment_read_optbus = 0, flag2_change_moment_read_optbus = 0; - - static unsigned int count_updated_sbus = 0, prev_ManualDischarge = 0; - - static unsigned int prev_flag_detect_update_optbus_data=0, flag_detect_update_optbus_data = 0, pause_error_detect_update_optbus_data = 0; - static unsigned int timer_wait_to_master_slave = 0; - static unsigned int prev_master = 0; - - static int pwm_enable_calc_main_log = 1; - - static int what_pwm = 0; - - int localStartGEDfromZadanie; - static unsigned int countStartGEDfromZadanie = 0; - - -// OPTICAL_BUS_CODE_STATUS optbus_status = {0}; - static STATUS_DATA_READ_OPT_BUS optbus_status; - _iq wd; - -// if (edrk.disable_interrupt_sync==0) -// start_sync_interrupt(); - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_16_ON; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_16_ON; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS) -// PWM_LINES_TK_16_ON; -#endif - - - -#if (_ENABLE_INTERRUPT_PWM_LED2) -i_led2_on_off(1); -#endif - - if (edrk.disable_interrupt_pwm) - { - pwm_inc_interrupt(); - return; - } - - if (flag_special_mode_rs==1) - { - calc_norm_ADC_0(1); - calc_norm_ADC_1(1); - pwm_inc_interrupt(); - - return; - } - -#if (_ENABLE_PWM_LED2_PROFILE) - pos_profile_pwm = 0; -#endif - - - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - - -// if (xpwm_time.what_next_interrupt==PWM_LOW_LEVEL_INTERRUPT) -// { -//#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) -// PWM_LINES_TK_17_ON; -//#endif -// -// i_sync_pin_on(); -// -// } -// else -// { -//#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) -// PWM_LINES_TK_17_OFF; -//#endif -// -// i_sync_pin_off(); -// } - - - -//////////////// -//PWN_COUNT_RUN_PER_INTERRUPT PWM_TWICE_INTERRUPT_RUN - err_interr = detect_level_interrupt(edrk.flag_second_PCH); - - if (err_interr) - edrk.errors.e3.bits.ERR_INT_PWM_LONG |=1; - - if (xpwm_time.one_or_two_interrupts_run == PWM_ONE_INTERRUPT_RUN) - pwm_up_down = 2; - else - if (xpwm_time.where_interrupt == PWM_LOW_LEVEL_INTERRUPT) - { - pwm_up_down = 0; - } - else - pwm_up_down = 1; - - // sync line - if (pwm_up_down==2 || pwm_up_down==0) - { -// what_pwm = 0; - // i_sync_pin_on(); - calculate_sync_detected(); - } - -///////////////// -#if (ENABLE_LOG_INTERRUPTS) - add_log_interrupts(3); -#endif - - -#if (_ENABLE_LOG_TICS_PWM==1) - - count_timer_buf = 0; - // optical_read_data.timer=0; -#endif - - -#if (_FLOOR6==0) -// if (edrk.Stop==0) -// i_led1_on_off(1); -#else - // i_led1_on_off(1); -#endif - - - edrk.into_pwm_interrupt = 1; - - // - start_tics_4timer = EvbRegs.T3CNT; - cur_timer = global_time.pwm_tics; - if (prev_timer>cur_timer) - { - if ((prev_timer-cur_timer)<2) - { -// stop_pwm(); - edrk.count_lost_interrupt++; - } - } - else - { - if ((cur_timer==prev_timer) || (cur_timer-prev_timer)>2) - { -// stop_pwm(); - edrk.count_lost_interrupt++; - } - } - prev_timer = cur_timer; - // - - set_tics_timer_pwm(1,count_timer_buf); - get_tics_timer_pwm(1); - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_17_ON; -#endif - -#if (_SIMULATE_AC==1) - calc_norm_ADC_0_sim(0); -#else - calc_norm_ADC_0(0); // norma Pwm_main() -#endif - run_detect_fast_error(); // - - - - ///////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////// - if (edrk.Kvitir==0 && prev_edrk_Kvitir==1) - { - count_err_read_opt_bus = 0; - edrk.sum_count_err_read_opt_bus = 0; - } - - set_tics_timer_pwm(2,count_timer_buf); - get_tics_timer_pwm(1); - - ////////////////////////////// -// inc_RS_timeout_cicle(); - //////////////////////////////// - //////////////////////////////////////////// - //////////////////////////////////////////// -// inc_CAN_timeout_cicle(); - //////////////////////////////////////////// - - if (edrk.ms.another_bs_maybe_on==1 && - (edrk.auto_master_slave.local.bits.master || edrk.auto_master_slave.local.bits.slave) ) - { - - flag_detect_update_optbus_data = 1; - - if (prev_flag_detect_update_optbus_data == 0) - pause_error_detect_update_optbus_data = 0; - - - count_updated_sbus = optical_read_data.data_was_update_between_pwm_int; - - // PAUSE_ERROR_DETECT_UPDATE_OPTBUS_DATA OPT_BUS - // OPT_BUS - if (pause_error_detect_update_optbus_data=1) -// project.cds_in[0].read_pbus(&project.cds_in[0]); -//#endif - -#if(C_cds_in_number>=2) - project.cds_in[1].read_pbus(&project.cds_in[1]); -#endif - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_17_OFF; -#endif - - - - set_tics_timer_pwm(10,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - if (pwm_run == 1) - { - // - , ? - soft_stop_x24_pwm_all(); - edrk.errors.e9.bits.ERR_INT_PWM_VERY_LONG |=1; - } - else - { - pwm_run = 1; - -// detect_I_M_overload(); -// if (edrk.from_rs.bits.ACTIVE) -// edrk.Go = edrk.StartGEDRS; - -// project_read_errors_controller(); // ADR_ERRORS_TOTAL_INFO - - if ( (edrk.errors.e0.all) - || (edrk.errors.e1.all) - || (edrk.errors.e2.all) - || (edrk.errors.e3.all) - || (edrk.errors.e4.all) - || (edrk.errors.e5.all) - || (edrk.errors.e6.all) - || (edrk.errors.e7.all) - || (edrk.errors.e8.all) - || (edrk.errors.e9.all) - || (edrk.errors.e10.all) - || (edrk.errors.e11.all) - || (edrk.errors.e12.all) - ) - edrk.Stop |= 1; - else - edrk.Stop = 0; - - - - project.read_errors_controller(); - error_controller = (project.controller.read.errors.all | project.controller.read.errors_buses.bit.slave_addr_error | project.controller.read.errors_buses.bit.count_error_pbus); -// project.controller.read.errors.all = error_controller; - - - - if(error_controller && prev_error_controller==0) - { - edrk.errors.e11.bits.ERROR_CONTROLLER_BUS |= 1; - svgen_set_time_keys_closed(&svgen_pwm24_1); - svgen_set_time_keys_closed(&svgen_pwm24_2); - - write_swgen_pwm_times(PWM_MODE_RELOAD_FORCE); - -// xerror(main_er_ID(1),(void *)0); - } - prev_error_controller = error_controller;//project.controller.read.errors.all; - - - if (pwm_enable_calc_main==0)// - - { - - if (en_rotor) - { -#if (_SIMULATE_AC==1) -// calc_rotors_sim(); -#else - calc_rotors(pwm_enable_calc_main_log); // -#endif - - - - } - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - calc_zadanie_rampa(); - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - calc_norm_ADC_1(1); // - - calc_power_full(); - - calc_all_limit_koeffs(); - - } - - ///////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////// - - - if (pwm_enable_calc_main)// - { - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_18_ON; -#endif - - if (edrk.Obmotka1 == 0) - go_a = 1; - else - go_a = 0; - - if (edrk.Obmotka2 == 0) - go_b = 1; - else - go_b = 0; - - ////////////////////////// - - if (optical_read_data.data.cmd.bit.start_pwm && edrk.auto_master_slave.local.bits.slave ) - edrk.StartGEDfromSyncBus = 1; - else - edrk.StartGEDfromSyncBus = 0; - - edrk.master_Uzad = _IQ15toIQ( optical_read_data.data.pzad_or_wzad); - edrk.master_theta = _IQ12toIQ( optical_read_data.data.angle_pwm); - edrk.master_Izad = _IQ15toIQ( optical_read_data.data.iq_zad_i_zad); - edrk.master_Iq = _IQ15toIQ( optical_read_data.data.iq_zad_i_zad); - - set_tics_timer_pwm(11,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - ///////////////////////// - - if ((edrk.auto_master_slave.local.bits.slave==1 && edrk.auto_master_slave.local.bits.master==0) - || (edrk.auto_master_slave.local.bits.slave==0 && edrk.auto_master_slave.local.bits.master==1) ) - { - - if (edrk.auto_master_slave.local.bits.master != prev_master) - timer_wait_to_master_slave = 0; - - // . - if (timer_wait_to_master_slave>MAX_TIMER_WAIT_MASTER_SLAVE) - { - edrk.Status_Ready.bits.MasterSlaveActive = 1; - - if (edrk.auto_master_slave.local.bits.master) - edrk.MasterSlave = MODE_MASTER; - else - edrk.MasterSlave = MODE_SLAVE; - } - else - { - edrk.Status_Ready.bits.MasterSlaveActive = 0; - edrk.MasterSlave = MODE_DONTKNOW; - timer_wait_to_master_slave++; - } - prev_master = edrk.auto_master_slave.local.bits.master; - } - else - { - - edrk.Status_Ready.bits.MasterSlaveActive = 0; - edrk.MasterSlave = MODE_DONTKNOW; - - timer_wait_to_master_slave = 0; - } - - - set_tics_timer_pwm(12,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) { - if (edrk.MasterSlave == MODE_MASTER) { - if (get_start_ged_from_zadanie()) { - edrk.prepare_stop_PWM = 0; - //edrk.StartGEDfromZadanie = 1; - localStartGEDfromZadanie = 1; - } else { - edrk.prepare_stop_PWM = 1; - if (edrk.k_stator1 < 41943) { //335544 ~ 2% - //edrk.StartGEDfromZadanie = 0; - localStartGEDfromZadanie = 0; - } - } - } else { - if (get_start_ged_from_zadanie()) { - //edrk.StartGEDfromZadanie = 1; - localStartGEDfromZadanie = 1; - } else { - if (edrk.k_stator1 < 41943) { //335544 ~ 2% - //edrk.StartGEDfromZadanie = 0; - localStartGEDfromZadanie = 0; - } - } - edrk.prepare_stop_PWM = optical_read_data.data.cmd.bit.prepare_stop_PWM; - } - } else { - //edrk.StartGEDfromZadanie = - localStartGEDfromZadanie = get_start_ged_from_zadanie(); - } - - // localStartGEDfromZadanie=1 edrk.StartGEDfromZadanie - if (localStartGEDfromZadanie && edrk.prevStartGEDfromZadanie==0) - { - if (countStartGEDfromZadanieMAX_TIMER_WAIT_BOTH_READY2) - edrk.errors.e1.bits.VERY_LONG_BOTH_READY2 |= 1; - else - timer_wait_both_ready2++; - - } - else - timer_wait_both_ready2 = 0; - - - if (optical_read_data.data.cmd.bit.rascepitel_cmd==CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON - && optical_read_data.data.cmd.bit.ready_cmd == CODE_READY_CMD_READY2) // - { - edrk.flag_block_zadanie = 0; - edrk.flag_wait_set_to_zero_zadanie = 0; - } - - if (optical_read_data.data.cmd.bit.rascepitel_cmd==CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_OFF - && optical_read_data.data.cmd.bit.ready_cmd == CODE_READY_CMD_READY1) // - { - edrk.flag_block_zadanie = 0; - edrk.flag_wait_set_to_zero_zadanie = 0; - } - - - if (edrk.StartGEDfromZadanie==0 && edrk.flag_block_zadanie - && (optical_read_data.data.cmd.bit.rascepitel_cmd==CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF)) - // - edrk.you_can_on_rascepitel = 1; - - - if (edrk.flag_wait_set_to_zero_zadanie) - { - if (timer_wait_set_to_zero_zadanie>MAX_TIMER_WAIT_SET_TO_ZERO_ZADANIE) - { - // , - // !!!! - edrk.errors.e1.bits.ANOTHER_BS_NOT_ON_RASCEPITEL |= 1; - - } - else - timer_wait_set_to_zero_zadanie++; - - } - else - timer_wait_set_to_zero_zadanie = 0; - - - // , . - if (edrk.errors.e1.bits.ANOTHER_BS_NOT_ON_RASCEPITEL) - edrk.flag_wait_set_to_zero_zadanie = 0; - - - edrk.StartGED = ((edrk.StartGEDfromControl==1) && (edrk.StartGEDfromZadanie==1) && (edrk.flag_block_zadanie==0)); - - - if (edrk.MasterSlave == MODE_MASTER) - { - edrk.GoWait = ( (edrk.StartGED ) && (edrk.Stop == 0) - && (project.controller.read.errors.all==0) && - (slow_error==0) && - (edrk.Status_Ready.bits.ready_final) - && edrk.Status_Ready.bits.MasterSlaveActive - && edrk.warnings.e9.bits.BREAKER_GED_ON==0 - ); - } - else - if (edrk.MasterSlave == MODE_SLAVE) - { - edrk.GoWait = ( (edrk.StartGED && edrk.StartGEDfromSyncBus) && (edrk.Stop == 0) - && (project.controller.read.errors.all==0) && - (slow_error==0) && - (edrk.Status_Ready.bits.ready_final) - && edrk.Status_Ready.bits.MasterSlaveActive - ); - } - else - edrk.GoWait = 0; - - // if (edrk.GoWait==0 && edrk.Go == 0 && - - - // edrk.Go - if (edrk.GoWait) - { - if (count_wait_go_0>=MAX_COUNT_WAIT_GO_0) - edrk.Go = edrk.GoWait; - else - { - edrk.Go = 0; - edrk.errors.e7.bits.VERY_FAST_GO_0to1 |=1; // ! edrk.Go!!! - } - } - else - { - if (edrk.Go) - count_wait_go_0 = 0; - - edrk.Go = 0; - if (count_wait_go_0=(MAX_COUNT_TIME_BUF-1)) - count_time_buf = 0; - - log_wait = 0; - if (edrk.MasterSlave == MODE_MASTER) - log_wait |= 0x1; - if (edrk.MasterSlave == MODE_SLAVE) - log_wait |= 0x2; - if (edrk.StartGED) - log_wait |= 0x4; - if (edrk.Stop) - log_wait |= 0x8; - if (edrk.Status_Ready.bits.ready_final) - log_wait |= 0x10; - if (edrk.Status_Ready.bits.MasterSlaveActive) - log_wait |= 0x20; - if (edrk.GoWait) - log_wait |= 0x40; - if (edrk.Go) - log_wait |= 0x80; - if (project.controller.read.errors.all==0) - log_wait |= 0x100; - if (slow_error) - log_wait |= 0x200; - if (edrk.StartGEDfromSyncBus) - log_wait |= 0x400; - - - time_buf[count_time_buf] = log_wait; - - if (edrk.errors.e7.bits.VERY_FAST_GO_0to1) - stop_count_time_buf = 1; - } -*/ -#endif - - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_GO) - if (edrk.StartGEDfromSyncBus) - { - PWM_LINES_TK_17_ON; - } - else - { - PWM_LINES_TK_17_OFF; - } - - if (edrk.StartGEDfromZadanie) - { - PWM_LINES_TK_18_ON; - } - else - { - PWM_LINES_TK_18_OFF; - } - if (edrk.flag_block_zadanie) - { - PWM_LINES_TK_19_ON; - } - else - { - PWM_LINES_TK_19_OFF; - } - - if (edrk.StartGEDfromControl) - { - PWM_LINES_TK_16_ON; - } - else - { - PWM_LINES_TK_16_OFF; - } - -#endif - - - - set_tics_timer_pwm(15,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - ////////////////////////////////// - ////////////////////////////////// - - if(edrk.Go == 1) - { - if (edrk.Go != prevGo) - { - edrk.count_run++; -// clear_mem(FAST_LOG); -// count_start_impuls = 0; -// count_step = 0; - f.count_step_ram_off = COUNT_SAVE_LOG_OFF; -// count_step_run = 0; - - // set_start_mem(FAST_LOG); - // set_start_mem(SLOW_LOG); - - // logpar.start_write_fast_log = 1; - - init_uf_const(); - init_simple_scalar(); - - Fzad = 0; - Uzad1 = 0; - Uzad2 = 0; - - clear_logpar(); - } - else - { - - if (f.count_start_impuls < COUNT_START_IMP) - { - f.count_start_impuls++; - } - else - { - f.count_start_impuls = COUNT_START_IMP; - - f.flag_record_log = 1; - enable_calc_vector = 1; - - } - - } - } - else // (edrk.Go == 0) - { - - if (f.count_step_ram_off > 0) - { - f.count_step_ram_off--; - f.flag_record_log = 1; - } else { - f.flag_record_log = 0; - } - - // - if (edrk.ManualDischarge && prev_ManualDischarge!=edrk.ManualDischarge) - edrk.Discharge = 1; - - prev_ManualDischarge =edrk.ManualDischarge; - - if (f.count_start_impuls == 0) - { - - if (edrk.Discharge || (edrk.ManualDischarge ) ) - { - break_resistor_managment_calc(); - soft_start_x24_break_1(); - } - else - { - - if (f.count_step_ram_off > 0) - { - break_resistor_recup_calc(edrk.zadanie.iq_set_break_level); - // soft_start_x24_break_1(); - } - else - { - // - soft_stop_x24_pwm_all(); - - } - } - - } - - set_tics_timer_pwm(16,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - if (f.count_start_impuls==COUNT_START_IMP) - { - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) - { - if (edrk.flag_second_PCH == 0) { - wd = uf_alg.winding_displacement_bs1; - } else { - wd = uf_alg.winding_displacement_bs2; - } - - vectorControlConstId(edrk.zadanie.iq_power_zad_rmp, edrk.zadanie.iq_oborots_zad_hz_rmp, - WRotor.RotorDirectionSlow, WRotor.iqWRotorSumFilter, - edrk.Mode_ScalarVectorUFConst, - edrk.MasterSlave, edrk.zadanie.iq_Izad, wd, - edrk.master_theta, edrk.master_Iq, edrk.iq_power_kw_another_bs, - &edrk.tetta_to_slave, &edrk.Iq_to_slave, &edrk.P_to_master, - 0, 1); - - test_calc_vect_dq_pwm24_Ing(vect_control.iqTheta, vect_control.iqUdKm, vect_control.iqUqKm, - edrk.disable_alg_u_disbalance, - edrk.zadanie.iq_kplus_u_disbalance_rmp, edrk.zadanie.iq_k_u_disbalance_rmp, - filter.iqU_1_fast, filter.iqU_2_fast, - 0, - edrk.Uzad_max, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.Uzad_to_slave); - analog.PowerFOC = edrk.P_to_master; - Fzad = vect_control.iqFstator; - Izad_out = edrk.Iq_to_slave; - } else { - test_calc_simple_dq_pwm24_Ing(Fzad, 0, 0, - 0, 0, filter.iqU_1_fast, filter.iqU_2_fast, - 1, - edrk.Uzad_max, - edrk.master_theta, - edrk.master_Uzad, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.tetta_to_slave, - &edrk.Uzad_to_slave); - } - } - else - { - if (f.count_start_impuls==COUNT_START_IMP-1) - { - - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) - { - if (edrk.flag_second_PCH == 0) { - wd = uf_alg.winding_displacement_bs1; - } else { - wd = uf_alg.winding_displacement_bs2; - } - - vectorControlConstId(edrk.zadanie.iq_power_zad_rmp, edrk.zadanie.iq_oborots_zad_hz_rmp, - WRotor.RotorDirectionSlow, WRotor.iqWRotorSumFilter, - edrk.Mode_ScalarVectorUFConst, - edrk.MasterSlave, edrk.zadanie.iq_Izad, wd, - edrk.master_theta, edrk.master_Iq, edrk.iq_power_kw_another_bs, - &edrk.tetta_to_slave, &edrk.Iq_to_slave, &edrk.P_to_master, - 0, 1); - - test_calc_vect_dq_pwm24_Ing(vect_control.iqTheta, vect_control.iqUdKm, vect_control.iqUqKm, - edrk.disable_alg_u_disbalance, - edrk.zadanie.iq_kplus_u_disbalance_rmp, edrk.zadanie.iq_k_u_disbalance_rmp, - filter.iqU_1_fast, filter.iqU_2_fast, - 0, - edrk.Uzad_max, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.Uzad_to_slave); - analog.PowerFOC = edrk.P_to_master; - Fzad = vect_control.iqFstator; - Izad_out = edrk.Iq_to_slave; - } else { - test_calc_simple_dq_pwm24_Ing(Fzad, 0, 0, - 0, 0, filter.iqU_1_fast, filter.iqU_2_fast, - 1, - edrk.Uzad_max, - edrk.master_theta, - edrk.master_Uzad, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.tetta_to_slave, - &edrk.Uzad_to_slave); - } - - } - else - { - if (f.count_start_impuls==COUNT_START_IMP-2) - { - // middle -// svgen_set_time_keys_closed(&svgen_pwm24_1); -// svgen_set_time_keys_closed(&svgen_pwm24_2); - svgen_set_time_middle_keys_open(&svgen_pwm24_1); - svgen_set_time_middle_keys_open(&svgen_pwm24_2); - } - else - // - { - svgen_set_time_keys_closed(&svgen_pwm24_1); - svgen_set_time_keys_closed(&svgen_pwm24_2); - } - - Fzad = 0; - - } - } - - - if (f.count_start_impuls > 0) { - f.count_start_impuls -= 1; - } else { - f.count_start_impuls = 0; - } - enable_calc_vector = 0; - - - - Uzad1 = 0; - Uzad2 = 0; - - } // end if Go==1 - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_18_OFF; -#endif - - - } // end pwm_enable_calc_main one interrupt one period only - -/* - * - // if ((m.m0.bit.EnableGoA == 1) && (f.Obmotka1 == 0)) - - - // if ((m.m0.bit.EnableGoB == 1) && (f.Obmotka2 == 0)) - if (f.Obmotka2 == 0) - { - go_b = 1; - } - else - { - go_b = 0; - } - - if (go_a == 0 && prev_go_a != go_a) - { - // 1 - soft_stop_x24_pwm_1(); - } - - if (go_a == 1 && prev_go_a != go_a) - { - // 1 - soft_start_x24_pwm_1(); - } - - if (go_b == 0 && prev_go_b != go_b) - { - // 2 - soft_stop_x24_pwm_2(); - } - - if (go_b == 1 && prev_go_b != go_b) - { - // 2 - soft_start_x24_pwm_2(); - } - - prev_go_a = go_a; - prev_go_b = go_b; - * - * - */ - - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - - if (pwm_enable_calc_main) // - { - - - if (f.count_start_impuls==1 && edrk.Go==1) - { - // - svgen_set_time_keys_closed(&svgen_pwm24_1); - svgen_set_time_keys_closed(&svgen_pwm24_2); - // soft_start_x24_pwm_1_2(); - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) { - if (edrk.flag_second_PCH == 0) { - wd = uf_alg.winding_displacement_bs1; - } else { - wd = uf_alg.winding_displacement_bs2; - } - vectorControlConstId(edrk.zadanie.iq_power_zad_rmp, edrk.zadanie.iq_oborots_zad_hz_rmp, - WRotor.RotorDirectionSlow, WRotor.iqWRotorSumFilter, - edrk.Mode_ScalarVectorUFConst, - edrk.MasterSlave, edrk.zadanie.iq_Izad, wd, - edrk.master_theta, edrk.master_Iq, edrk.P_from_slave, - &edrk.tetta_to_slave, &edrk.Iq_to_slave, &edrk.P_to_master, 1, edrk.prepare_stop_PWM); - } - } - - if (f.count_start_impuls==2 && edrk.Go==1) - { - // - if (go_a == 1 && go_b == 1) { - // start_pwm(); edrk.Go - soft_start_x24_pwm_1_2(); - } else if (go_a == 1) { - soft_start_x24_pwm_1(); - } else if (go_b == 1) { - soft_start_x24_pwm_2(); - } - - // enable work break -#if (DISABLE_WORK_BREAK==1) - -#else -// if (edrk.disable_break_work==0) - { - soft_start_x24_break_1(); - } -#endif - - } // end if (count_start_impuls==5) - - - if (f.count_start_impuls==3 && edrk.Go==1) - { - // - svgen_set_time_middle_keys_open(&svgen_pwm24_1); - svgen_set_time_middle_keys_open(&svgen_pwm24_2); - } - - - - if (f.count_start_impuls==4 && edrk.Go==1) - { - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) - { - -// void simple_scalar(int n_alg, int n_wind_pump, _iq Frot_pid, _iq Frot,_iq fzad,_iq mzz_zad, _iq bpsi_const, _iq fzad_provorot, -// _iq iqIm_1, _iq iqIm_2, _iq iqUin, _iq Iin, _iq powerzad, _iq power_pid, -// _iq *Fz, _iq *Uz1) - - if (edrk.flag_second_PCH == 0) { - wd = uf_alg.winding_displacement_bs1; - } else { - wd = uf_alg.winding_displacement_bs2; - } - - vectorControlConstId(0, 0, - WRotor.RotorDirectionSlow, WRotor.iqWRotorSumFilter, - edrk.Mode_ScalarVectorUFConst, - edrk.MasterSlave, edrk.zadanie.iq_Izad, wd, - edrk.master_theta, edrk.master_Iq, edrk.iq_power_kw_another_bs, - &edrk.tetta_to_slave, &edrk.Iq_to_slave, &edrk.P_to_master, - 0, edrk.prepare_stop_PWM); - - test_calc_vect_dq_pwm24_Ing(vect_control.iqTheta, vect_control.iqUdKm, vect_control.iqUqKm, - edrk.disable_alg_u_disbalance, - edrk.zadanie.iq_kplus_u_disbalance_rmp, edrk.zadanie.iq_k_u_disbalance_rmp, - filter.iqU_1_fast, filter.iqU_2_fast, - 0, - edrk.Uzad_max, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.Uzad_to_slave); - Fzad = vect_control.iqFstator; - Izad_out = edrk.Iq_to_slave; - } else { - test_calc_simple_dq_pwm24_Ing(Fzad, 0, 0, - 0, 0, filter.iqU_1_fast, filter.iqU_2_fast, - 0, - edrk.Uzad_max, - edrk.master_theta, - edrk.master_Uzad, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.tetta_to_slave, &edrk.Uzad_to_slave); - - - simple_scalar(1,0, WRotor.RotorDirectionSlow, - WRotor.iqWRotorSumFilter2, WRotor.iqWRotorSumFilter, - 0, - 0, - 0, edrk.iq_bpsi_normal, - 0, - // analog.iqU_1_long+analog.iqU_2_long, - edrk.zadanie.iq_ZadanieU_Charge_rmp+edrk.zadanie.iq_ZadanieU_Charge_rmp, - 0, - edrk.zadanie.iq_power_zad_rmp, 0, - edrk.zadanie.iq_limit_power_zad_rmp, edrk.Mode_ScalarVectorUFConst, - 0,0, edrk.count_bs_work+1, - &Fzad, &Uzad1, &Uzad2, &Izad_out); - } - - } - - if (f.count_start_impuls == COUNT_START_IMP && edrk.Go==1) - { - if (pwm_enable_calc_main) // - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_21_ON; -#endif - - - //break_resistor_recup_calc(edrk.zadanie.iq_ZadanieU_Charge); - break_resistor_recup_calc(edrk.zadanie.iq_set_break_level); - - set_tics_timer_pwm(17,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - run_calc_uf = 1; - - // , middle - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_UF_CONST) - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_ON; -#endif - uf_const(&Fzad,&Uzad1,&Uzad2); -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_ON; -#endif - - test_calc_simple_dq_pwm24_Ing(Fzad, - Uzad1, - edrk.disable_alg_u_disbalance, - edrk.zadanie.iq_kplus_u_disbalance, - edrk.zadanie.iq_k_u_disbalance, - filter.iqU_1_fast, - filter.iqU_2_fast, - 0, - edrk.Uzad_max, - edrk.master_theta, - edrk.master_Uzad, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, - &edrk.tetta_to_slave, - &edrk.Uzad_to_slave); -// rmp_freq.DesiredInput = alg_pwm24.freq1; -// rmp_freq.calc(&rmp_freq); -// Fzad = rmp_freq.Out; -// -// vhz1.Freq = Fzad; -// vhz1.calc(&vhz1); -// -// -// Uzad1 = alg_pwm24.k1; -// Uzad2 = alg_pwm24.k1; - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_OFF; -#endif - - // test_calc_pwm24(Uzad1, Uzad2, Fzad); - // analog_dq_calc_const(); - - } // end ALG_MODE_UF_CONST - else - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_POWER) - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_ON; -#endif - simple_scalar(1, - 0, - WRotor.RotorDirectionSlow, - WRotor.iqWRotorSumFilter, //rotor_22220.iqFlong * rotor_22220.direct_rotor; - WRotor.iqWRotorSumFilter, //rotor_22220.iqFout * rotor_22220.direct_rotor; - //0, 0, - edrk.zadanie.iq_oborots_zad_hz_rmp, - edrk.all_limit_koeffs.sum_limit, - edrk.zadanie.iq_Izad_rmp, - edrk.iq_bpsi_normal, - analog.iqIm, -// analog.iqU_1_long+analog.iqU_2_long, - edrk.zadanie.iq_ZadanieU_Charge_rmp+edrk.zadanie.iq_ZadanieU_Charge_rmp, - analog.iqIin_sum, - edrk.zadanie.iq_power_zad_rmp, - edrk.iq_power_kw_full_znak,//(filter.PowerScalar+edrk.iq_power_kw_another_bs), - edrk.zadanie.iq_limit_power_zad_rmp, edrk.Mode_ScalarVectorUFConst, - edrk.master_Izad, - edrk.MasterSlave, - edrk.count_bs_work+1, - &Fzad, - &Uzad1, - &Uzad2, - &Izad_out); - - set_tics_timer_pwm(18,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_ON; -#endif - - - - if (edrk.cmd_disable_calc_km_on_slave) - Uzad_from_master = edrk.master_Uzad; - else - { -#if (DISABLE_CALC_KM_ON_SLAVE==1) - Uzad_from_master = edrk.master_Uzad; -#else - Uzad_from_master = Uzad1; -#endif - - } - - test_calc_simple_dq_pwm24_Ing(Fzad, Uzad1, edrk.disable_alg_u_disbalance, - edrk.zadanie.iq_kplus_u_disbalance, edrk.zadanie.iq_k_u_disbalance, filter.iqU_1_fast, filter.iqU_2_fast, - 0, - edrk.Uzad_max, - edrk.master_theta, - Uzad_from_master, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.tetta_to_slave, &edrk.Uzad_to_slave); -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_OFF; -#endif - - set_tics_timer_pwm(19,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - if (edrk.flag_second_PCH == 0) { - wd = uf_alg.winding_displacement_bs1; - } else { - wd = uf_alg.winding_displacement_bs2; - } - - analog_dq_calc_external(wd, uf_alg.tetta); - - } // end ALG_MODE_SCALAR_OBOROTS - else - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) - { - -// void simple_scalar(int n_alg, int n_wind_pump, _iq Frot_pid, _iq Frot,_iq fzad,_iq mzz_zad, _iq bpsi_const, _iq fzad_provorot, -// _iq iqIm_1, _iq iqIm_2, _iq iqUin, _iq Iin, _iq powerzad, _iq power_pid, -// _iq *Fz, _iq *Uz1) - - if (edrk.flag_second_PCH == 0) { - wd = uf_alg.winding_displacement_bs1; - } else { - wd = uf_alg.winding_displacement_bs2; - } -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_ON; -#endif - vectorControlConstId(edrk.zadanie.iq_power_zad_rmp, edrk.zadanie.iq_oborots_zad_hz_rmp, - WRotor.RotorDirectionSlow, WRotor.iqWRotorSumFilter, - edrk.Mode_ScalarVectorUFConst, - edrk.MasterSlave, edrk.zadanie.iq_Izad, wd, - edrk.master_theta, edrk.master_Iq, edrk.iq_power_kw_another_bs, - &edrk.tetta_to_slave, &edrk.Iq_to_slave, &edrk.P_to_master, - 0, edrk.prepare_stop_PWM); -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_ON; -#endif - - test_calc_vect_dq_pwm24_Ing(vect_control.iqTheta, vect_control.iqUdKm, vect_control.iqUqKm, - edrk.disable_alg_u_disbalance, - edrk.zadanie.iq_kplus_u_disbalance_rmp, edrk.zadanie.iq_k_u_disbalance_rmp, - filter.iqU_1_fast, filter.iqU_2_fast, - 0, - edrk.Uzad_max, - edrk.MasterSlave, - edrk.flag_second_PCH, - &edrk.Kplus, &edrk.Uzad_to_slave); - - analog.PowerFOC = edrk.P_to_master; - Fzad = vect_control.iqFstator; - Izad_out = edrk.Iq_to_slave; - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_20_OFF; -#endif - } // end ALG_MODE_FOC_OBOROTS - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_21_OFF; -#endif - - } // end pwm_enable_calc_main - - } // end (count_start_impuls == COUNT_START_IMP && edrk.Go==1) - else - { - run_calc_uf = 0; - if (pwm_enable_calc_main) // - { - - } - svgen_pwm24_1.Ta_imp = 0; - svgen_pwm24_1.Tb_imp = 0; - svgen_pwm24_1.Tc_imp = 0; - svgen_pwm24_2.Ta_imp = 0; - svgen_pwm24_2.Tb_imp = 0; - svgen_pwm24_2.Tc_imp = 0; - - } // end else (count_start_impuls == COUNT_START_IMP && edrk.Go==1) - - prevGo = edrk.Go; - - - - ////////////////////////////////// - optical_write_data.data.cmd.bit.start_pwm = edrk.Go; - optical_write_data.data.cmd.bit.prepare_stop_PWM = edrk.prepare_stop_PWM; - - optical_write_data.data.angle_pwm = _IQtoIQ12(edrk.tetta_to_slave + vect_control.add_tetta); - optical_write_data.data.pzad_or_wzad = _IQtoIQ15(edrk.Uzad_to_slave); - optical_write_data.data.iq_zad_i_zad = _IQtoIQ15(edrk.Izad_out); - - optical_bus_update_data_write(); - - set_tics_timer_pwm(20,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - - if (edrk.ms.another_bs_maybe_on==1 && edrk.auto_master_slave.local.bits.master) - { - // i_led2_on(); - } - - ////////////////////////////////// - ////////////////////////////////// - - edrk.Izad_out = Izad_out; - - if (edrk.MasterSlave==MODE_SLAVE) - { - edrk.f_stator = Fzad; - edrk.k_stator1 = edrk.Uzad_to_slave;//Uzad1; - edrk.k_stator2 = edrk.Uzad_to_slave;//Uzad2; - - } - else - if (edrk.MasterSlave==MODE_MASTER) - { - edrk.f_stator = Fzad; - edrk.k_stator1 = edrk.Uzad_to_slave;//Uzad1; - edrk.k_stator2 = edrk.Uzad_to_slave; - } - else - { - edrk.f_stator = 0; - edrk.k_stator1 = 0; - edrk.k_stator2 = 0; - } - - } // end pwm_enable_calc_main - - - - - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - /////////////////////////////////////////// - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_22_ON; -#endif - - if (xpwm_time.one_or_two_interrupts_run == PWM_ONE_INTERRUPT_RUN) - write_swgen_pwm_times(PWM_MODE_RELOAD_FORCE); - else - { - if (edrk.Go==1) - { - if (f.count_start_impuls==COUNT_START_IMP-1) - { - if (pwm_enable_calc_main) - write_swgen_pwm_times(PWM_MODE_RELOAD_LEVEL_HIGH); - else - write_swgen_pwm_times(PWM_MODE_RELOAD_LEVEL_LOW); - } - else -// if (pwm_enable_calc_main) - write_swgen_pwm_times(PWM_MODE_RELOAD_FORCE); - } - else - { - if (f.count_start_impuls==COUNT_START_IMP-3) - { - if (pwm_enable_calc_main) - write_swgen_pwm_times(PWM_MODE_RELOAD_LEVEL_HIGH); - else - write_swgen_pwm_times(PWM_MODE_RELOAD_LEVEL_LOW); - - } - else - write_swgen_pwm_times(PWM_MODE_RELOAD_FORCE); - } - - - - // if (pwm_enable_calc_main) - // prev_run_calc_uf = run_calc_uf; - - - - } -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_22_OFF; -#endif - - - set_tics_timer_pwm(21,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - - // test write oscil buf - - if ( pwm_enable_calc_main==0) // - { - - run_write_logs(); - - } - - - // i_led2_on(); - // - if (edrk.SumSbor == 1) { - detect_protect_adc(uf_alg.tetta_bs, uf_alg.tetta_bs); - //// get_tics_timer_pwm(pwm_enable_calc_main,count_timer_buf); - } - - // fill_RMS_buff_interrupt(uf_alg.tetta_bs, uf_alg.tetta_bs); - - set_tics_timer_pwm(24,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - - // out_I_over_1_6.calc(&out_I_over_1_6); - // i_led2_off(); - - - pwm_run = 0; - - - } // end if pwm_run==1 - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_16_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_16_ON; -#endif - - if (pwm_enable_calc_main==0) // - { - - - // pwm_analog_ext_interrupt(); - -// inc_RS_timeout_cicle(); -// inc_CAN_timeout_cicle(); - -#if (_SIMULATE_AC==1) - sim_model_execute(); -#endif - - } - - pwm_analog_ext_interrupt(); - pwm_inc_interrupt(); - - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_16_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_16_ON; -#endif - - - - - set_tics_timer_pwm(25,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - - -#if (_ENABLE_SLOW_PWM) -// pause_1000(slow_pwm_pause); -#endif - - set_tics_timer_pwm(26,count_timer_buf); - get_tics_timer_pwm(pwm_enable_calc_main_log); - -///////////////////////////////////////////////// - // - end_tics_4timer = EvbRegs.T3CNT; - - if (end_tics_4timer>start_tics_4timer) - { - time_delta = (end_tics_4timer - start_tics_4timer); - time_delta = time_delta * 33/1000; - if (pwm_enable_calc_main) - edrk.period_calc_pwm_int1 = time_delta;//(end_tics_4timer - start_tics_4timer)*33/1000; - else - edrk.period_calc_pwm_int2 = time_delta;//(end_tics_4timer - start_tics_4timer)*33/1000; - } - // - ///////////////////////////////////////////////// - - // get_tics_timer_pwm(pwm_enable_calc_main,count_timer_buf); - - - - - -#if (_ENABLE_LOG_TICS_PWM==1) - - for (i_log=count_timer_buf;i_log=10000) - c_run=c_run_start; - else - c_run++; -#endif - -#if (ENABLE_LOG_INTERRUPTS) - add_log_interrupts(103); -#endif - - - -// i_sync_pin_off(); - edrk.into_pwm_interrupt = 0; - -#if (_ENABLE_INTERRUPT_PWM_LED2) -i_led2_on_off(0); -#endif - - - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - // PWM_LINES_TK_16_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_16_OFF; -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_16_OFF; -#endif - - -#if (_ENABLE_PWM_LED2_PROFILE) - i_led2_on_off(0); - if (pwm_enable_calc_main==0) - profile_pwm[pos_profile_pwm] = 2; -#endif - - -} -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// - - - -#pragma CODE_SECTION(fix_pwm_freq_synchro_ain,".fast_run"); -void fix_pwm_freq_synchro_ain(void) -{ - unsigned int new_freq; - static unsigned int delta_freq = 1; -// if (f.Sync_input_or_output == SYNC_INPUT) - { - sync_inc_error(); - - if (sync_data.disable_sync || sync_data.timeout_sync_signal == 1 || sync_data.enable_do_sync == 0) - { - - new_freq = xpwm_time.pwm_tics; - i_WriteMemory(ADR_PWM_PERIOD, new_freq); - - return; - } - - if (sync_data.pwm_freq_plus_minus_zero==1) - { - - - //Increment xtics - new_freq = xpwm_time.pwm_tics + delta_freq; - i_WriteMemory(ADR_PWM_PERIOD, new_freq); // Saw period in tics. 1 tic = 16.67 nsec - - // change_freq_pwm(VAR_FREQ_PWM_XTICS); - - - } - - if (sync_data.pwm_freq_plus_minus_zero==-1) - { - //4464 - //Decrement xtics - new_freq = xpwm_time.pwm_tics - delta_freq; - i_WriteMemory(ADR_PWM_PERIOD, new_freq); // Saw period in tics. 1 tic = 16.67 nsec - - // change_freq_pwm(VAR_FREQ_PWM_XTICS); - - } - - if (sync_data.pwm_freq_plus_minus_zero==0) - { - new_freq = xpwm_time.pwm_tics; - i_WriteMemory(ADR_PWM_PERIOD, new_freq); - // change_freq_pwm(VAR_FREQ_PWM_XTICS); - } - - } - - - -} - -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// - -/* -void slow_vector_update() -{ - _iq iqKzad = 0; - - freq1 = _IQ (f.fzad/F_STATOR_MAX);//f.iqFRotorSetHz; - iqKzad = _IQ(f.kzad); - k1 = zad_intensiv_q(20000, 20000, k1, iqKzad); - -} -*/ - -void detect_work_revers(int direction, _iq fzad, _iq frot) -{ - static int prev_revers = 0; - int flag_revers; - // - if (direction == -1 && fzad > 0) - { - flag_revers = 1; - } - else - if (direction == 1 && fzad < 0) - { - flag_revers = 1; - } - else - { - flag_revers = 0; - } - - if (flag_revers && prev_revers==0) - edrk.count_revers++; - - prev_revers = flag_revers; - -} - - -void calc_power_full(void) -{ - _iq power_full_abs, power_one_abs, power_full_abs_f, power_one_abs_f; - - // - power_one_abs = _IQabs(filter.PowerScalar); - power_one_abs_f = _IQabs(filter.PowerScalarFilter2); - power_full_abs = power_one_abs + _IQabs(edrk.iq_power_kw_another_bs); - power_full_abs_f = power_one_abs_f + _IQabs(edrk.iq_power_kw_another_bs); - - if (edrk.oborots>=0) - { - edrk.iq_power_kw_full_znak = power_full_abs; - edrk.iq_power_kw_one_znak = power_one_abs; - edrk.iq_power_kw_full_filter_znak = power_full_abs_f; - edrk.iq_power_kw_one_filter_znak = power_one_abs_f; - } - else - { - edrk.iq_power_kw_full_znak = -power_full_abs; - edrk.iq_power_kw_one_znak = -power_one_abs; - edrk.iq_power_kw_full_filter_znak = -power_full_abs_f; - edrk.iq_power_kw_one_filter_znak = -power_one_abs_f; - } - - edrk.iq_power_kw_full_abs = power_full_abs; - edrk.iq_power_kw_one_abs = power_one_abs; - edrk.iq_power_kw_full_filter_abs = power_full_abs_f; - edrk.iq_power_kw_one_filter_abs = power_one_abs_f; - -} diff --git a/Inu/Src2/551/main/PWMTools.h b/Inu/Src2/551/main/PWMTools.h deleted file mode 100644 index 67f30b2..0000000 --- a/Inu/Src2/551/main/PWMTools.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef PWMTOOLS_H -#define PWMTOOLS_H -#include -#include - - -////////////////////////////////////////////////// -////////////////////////////////////////////////// -////////////////////////////////////////////////// - - -void InitPWM(void); -void PWM_interrupt(void); -void PWM_interrupt_main(void); - - - -void stop_wdog(void); -void start_wdog(void); - - -void global_time_interrupt(void); -void optical_bus_read_write_interrupt(void); -void pwm_analog_ext_interrupt(void); -void pwm_inc_interrupt(void); - -void fix_pwm_freq_synchro_ain(void); -void async_pwm_ext_interrupt(void); - - - -void calc_rotors(int flag); - -void detect_work_revers(int direction, _iq fzad, _iq frot); - -void calc_power_full(void); - - - -////////////////////////////////////////////////// -////////////////////////////////////////////////// -////////////////////////////////////////////////// -////////////////////////////////////////////////// - -extern PWMGEND pwmd; - -//extern int var_freq_pwm_xtics; -//extern int var_period_max_xtics; -//extern int var_period_min_xtics; - - - -#endif //PWMTOOLS_H - diff --git a/Inu/Src2/551/main/adc_internal.h b/Inu/Src2/551/main/adc_internal.h deleted file mode 100644 index c74a174..0000000 --- a/Inu/Src2/551/main/adc_internal.h +++ /dev/null @@ -1,33 +0,0 @@ - - -#define ADC_usDELAY 8000L -#define ADC_usDELAY2 20L - - - - - -// Determine when the shift to right justify the data takes place -// Only one of these should be defined as 1. -// The other two should be defined as 0. -#define POST_SHIFT 0 // Shift results after the entire sample table is full -#define INLINE_SHIFT 1 // Shift results as the data is taken from the results regsiter -#define NO_SHIFT 0 // Do not shift the results - -// ADC start parameters -#define ADC_MODCLK 0x3 // HSPCLK = SYSCLKOUT/2*ADC_MODCLK2 = 150/(2*3) = 25MHz -#define ADC_CKPS 0x0 // ADC module clock = HSPCLK/1 = 25MHz/(1) = 25MHz -#define ADC_SHCLK 0x1 // S/H width in ADC module periods = 2 ADC cycle -#define AVG 1000 // Average sample limit -#define ZOFFSET 0x00 // Average Zero offset -#define BUF_SIZE 100 // Sample buffer size - - - -#define FREQ_ADC 15000.0//26.08.2009//73000.0 - - -#define read_ADC(c) (*(&AdcRegs.ADCRESULT0+c)>>4) - -#define SDVIG_K_FILTER_S 2 //1//(27.08.2009) //3 - diff --git a/Inu/Src2/551/main/adc_tools.c b/Inu/Src2/551/main/adc_tools.c deleted file mode 100644 index 80b3b8d..0000000 --- a/Inu/Src2/551/main/adc_tools.c +++ /dev/null @@ -1,1412 +0,0 @@ -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "IQmathLib.h" - -#include -#include -#include -#include -#include - -#include "mathlib.h" -#include "filter_v1.h" -#include "xp_project.h" - -//#include "spartan_tools.h" - - - -//#define LOG_ACP_TO_BUF 1 - -#ifdef LOG_ACP_TO_BUF - -#define SIZE_BUF_LOG_ACP 500 -#pragma DATA_SECTION(BUF_ADC,".slow_vars") -int BUF_ADC[SIZE_BUF_LOG_ACP]; -#pragma DATA_SECTION(BUF_ADC_2,".slow_vars") -int BUF_ADC_2[SIZE_BUF_LOG_ACP]; - -#endif - - - -#if (USE_INTERNAL_ADC==1) - -#if(C_adc_number==1) -unsigned int const R_ADC[COUNT_ARR_ADC_BUF][16] = { R_ADC_DEFAULT_0,R_ADC_DEFAULT_INTERNAL }; -unsigned int const K_LEM_ADC[COUNT_ARR_ADC_BUF][16] = { K_LEM_ADC_DEFAULT_0, K_LEM_ADC_DEFAULT_INTERNAL}; -float const K_NORMA_ADC[COUNT_ARR_ADC_BUF][16] = { NORMA_ADC_DEFAULT_0, NORMA_ADC_DEFAULT_INTERNAL}; -#endif -#if(C_adc_number==2) -unsigned int const R_ADC[COUNT_ARR_ADC_BUF][16] = { R_ADC_DEFAULT_0, R_ADC_DEFAULT_1,R_ADC_DEFAULT_INTERNAL }; -unsigned int const K_LEM_ADC[COUNT_ARR_ADC_BUF][16] = { K_LEM_ADC_DEFAULT_0, K_LEM_ADC_DEFAULT_1, K_LEM_ADC_DEFAULT_INTERNAL }; -float const K_NORMA_ADC[COUNT_ARR_ADC_BUF][16] = { NORMA_ADC_DEFAULT_0, NORMA_ADC_DEFAULT_1, NORMA_ADC_DEFAULT_INTERNAL }; -#endif -#if(C_adc_number==3) -unsigned int const R_ADC[COUNT_ARR_ADC_BUF][16] = { R_ADC_DEFAULT_0, R_ADC_DEFAULT_1, R_ADC_DEFAULT_2,R_ADC_DEFAULT_INTERNAL }; -unsigned int const K_LEM_ADC[COUNT_ARR_ADC_BUF][16] = { K_LEM_ADC_DEFAULT_0, K_LEM_ADC_DEFAULT_1, K_LEM_ADC_DEFAULT_2, K_LEM_ADC_DEFAULT_INTERNAL }; -float const K_NORMA_ADC[COUNT_ARR_ADC_BUF][16] = { NORMA_ADC_DEFAULT_0, NORMA_ADC_DEFAULT_1, NORMA_ADC_DEFAULT_2, NORMA_ADC_DEFAULT_INTERNAL }; -#endif - -#else - -#if(C_adc_number==1) -#pragma DATA_SECTION(R_ADC,".slow_vars") -unsigned int R_ADC[COUNT_ARR_ADC_BUF][16] = { R_ADC_DEFAULT_0 }; -#pragma DATA_SECTION(K_LEM_ADC,".slow_vars") -unsigned int K_LEM_ADC[COUNT_ARR_ADC_BUF][16] = { K_LEM_ADC_DEFAULT_0}; -#pragma DATA_SECTION(K_NORMA_ADC,".slow_vars") -float K_NORMA_ADC[COUNT_ARR_ADC_BUF][16] = { NORMA_ADC_DEFAULT_0}; -#endif -#if(C_adc_number==2) -#pragma DATA_SECTION(R_ADC,".slow_vars") -unsigned int R_ADC[COUNT_ARR_ADC_BUF][16] = { R_ADC_DEFAULT_0, R_ADC_DEFAULT_1 }; -#pragma DATA_SECTION(K_LEM_ADC,".slow_vars") -unsigned int K_LEM_ADC[COUNT_ARR_ADC_BUF][16] = { K_LEM_ADC_DEFAULT_0, K_LEM_ADC_DEFAULT_1 }; -#pragma DATA_SECTION(K_NORMA_ADC,".slow_vars") -float K_NORMA_ADC[COUNT_ARR_ADC_BUF][16] = { NORMA_ADC_DEFAULT_0, NORMA_ADC_DEFAULT_1 }; -#endif -#if(C_adc_number==3) -#pragma DATA_SECTION(R_ADC,".slow_vars") -unsigned int R_ADC[COUNT_ARR_ADC_BUF][16] = { R_ADC_DEFAULT_0, R_ADC_DEFAULT_1, R_ADC_DEFAULT_2 }; -#pragma DATA_SECTION(K_LEM_ADC,".slow_vars") -unsigned int K_LEM_ADC[COUNT_ARR_ADC_BUF][16] = { K_LEM_ADC_DEFAULT_0, K_LEM_ADC_DEFAULT_1, K_LEM_ADC_DEFAULT_2 }; -#pragma DATA_SECTION(K_NORMA_ADC,".slow_vars") -float K_NORMA_ADC[COUNT_ARR_ADC_BUF][16] = { NORMA_ADC_DEFAULT_0, NORMA_ADC_DEFAULT_1, NORMA_ADC_DEFAULT_2 }; -#endif - -#endif - -//unsigned int const R_ADC_1[16] = R_ADC_DEFAULT_1; -//unsigned int const K_LEM_ADC_1[16] = K_LEM_ADC_DEFAULT_1; - - - - -#if (USE_INTERNAL_ADC==1) -int error_ADC[COUNT_ARR_ADC_BUF][16] = { {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} }; -#else - -#if(C_adc_number==1) -int error_ADC[COUNT_ARR_ADC_BUF][16] = { {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} }; -#endif -#if(C_adc_number==2) -int error_ADC[COUNT_ARR_ADC_BUF][16] = { {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} }; -#endif -#if(C_adc_number==3) -int error_ADC[COUNT_ARR_ADC_BUF][16] = { {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} }; -#endif - -#endif - - -#pragma DATA_SECTION(ADC_f,".fast_vars"); -int ADC_f[COUNT_ARR_ADC_BUF][16]; - -#pragma DATA_SECTION(ADC_fast,".fast_vars"); -int ADC_fast[COUNT_ARR_ADC_BUF][16][COUNT_ARR_ADC_BUF_FAST_POINT]; - - -#pragma DATA_SECTION(ADC_sf,".fast_vars"); -int ADC_sf[COUNT_ARR_ADC_BUF][16]; - -#pragma DATA_SECTION(analog,".fast_vars"); -ANALOG_VALUE analog = ANALOG_VALUE_DEFAULT; - -#pragma DATA_SECTION(filter,".fast_vars"); -ANALOG_VALUE filter = ANALOG_VALUE_DEFAULT; - -#pragma DATA_SECTION(analog_zero,".fast_vars"); -ANALOG_VALUE analog_zero = ANALOG_VALUE_DEFAULT; - - - -unsigned int const level_err_ADC_PLUS[16] = level_err_ADC_PLUS_default; -unsigned int const level_err_ADC_MINUS[16] = level_err_ADC_MINUS_default; - - -#pragma DATA_SECTION(err_adc_protect,".fast_vars"); -#pragma DATA_SECTION(mask_err_adc_protect,".fast_vars"); -ERR_ADC_PROTECT err_adc_protect[COUNT_ARR_ADC_BUF],mask_err_adc_protect[COUNT_ARR_ADC_BUF]; - - - -_iq koef_Im_filter=0; -_iq koef_Power_filter=0; -_iq koef_Power_filter2=0; - -#pragma DATA_SECTION(k_norm_ADC,".slow_vars") -_iq19 k_norm_ADC[COUNT_ARR_ADC_BUF][16]; - -#pragma DATA_SECTION(iq19_zero_ADC,".fast_vars"); -_iq19 iq19_zero_ADC[COUNT_ARR_ADC_BUF][16]; - - -#pragma DATA_SECTION(zero_ADC,".slow_vars") -int zero_ADC[COUNT_ARR_ADC_BUF][16]; - - -#pragma DATA_SECTION(iq19_k_norm_ADC,".fast_vars"); -_iq19 iq19_k_norm_ADC[COUNT_ARR_ADC_BUF][16]; - -#pragma DATA_SECTION(iq_norm_ADC,".fast_vars"); -_iq iq_norm_ADC[COUNT_ARR_ADC_BUF][16]; - -#pragma DATA_SECTION(iq_norm_ADC_sf,".fast_vars"); -_iq iq_norm_ADC_sf[COUNT_ARR_ADC_BUF][16]; - - -#pragma DATA_SECTION(koef_Uzpt_long_filter,".fast_vars"); -_iq koef_Uzpt_long_filter=0; - -#pragma DATA_SECTION(koef_Uzpt_fast_filter,".fast_vars"); -_iq koef_Uzpt_fast_filter=0; - -#pragma DATA_SECTION(koef_Uin_filter,".fast_vars"); -_iq koef_Uin_filter=0; - - -void fast_detect_protect_ACP(); -//void fast_read_all_adc_one(int cc); -//void fast_read_all_adc_more(void); - - -#if (USE_INTERNAL_ADC==1) -#pragma CODE_SECTION(adc_isr,".fast_run"); -interrupt void adc_isr(void) -{ -// unsigned char k; - static char l_ir=0; - static char step_acp=0; - -//i_led1_on_off(1); - -// i_led1_on_off(1); - - project.adc->read_pbus(&project.adc[0]); - - ADC_f[0][0] = project.adc[0].read.pbus.adc_value[0]; - ADC_f[0][1] = project.adc[0].read.pbus.adc_value[1]; - ADC_f[0][2] = project.adc[0].read.pbus.adc_value[2]; - ADC_f[0][3] = project.adc[0].read.pbus.adc_value[3]; - ADC_f[0][4] = project.adc[0].read.pbus.adc_value[4]; - ADC_f[0][5] = project.adc[0].read.pbus.adc_value[5]; - ADC_f[0][6] = project.adc[0].read.pbus.adc_value[6]; - ADC_f[0][7] = project.adc[0].read.pbus.adc_value[7]; - ADC_f[0][8] = project.adc[0].read.pbus.adc_value[8]; - ADC_f[0][9] = project.adc[0].read.pbus.adc_value[9]; - ADC_f[0][10] = project.adc[0].read.pbus.adc_value[10]; - ADC_f[0][11] = project.adc[0].read.pbus.adc_value[11]; - ADC_f[0][12] = project.adc[0].read.pbus.adc_value[12]; - ADC_f[0][13] = project.adc[0].read.pbus.adc_value[13]; - ADC_f[0][14] = project.adc[0].read.pbus.adc_value[14]; - ADC_f[0][15] = project.adc[0].read.pbus.adc_value[15]; - - - ADC_sf[0][0] += (ADC_f[0][0] - ADC_sf[0][0]) >> Shift_Filter; - ADC_sf[0][1] += (ADC_f[0][1] - ADC_sf[0][1]) >> Shift_Filter; - ADC_sf[0][2] += (ADC_f[0][2] - ADC_sf[0][2]) >> Shift_Filter; - ADC_sf[0][3] += (ADC_f[0][3] - ADC_sf[0][3]) >> Shift_Filter; - - -/* - - - if (ADC_sf[2][0]>ERR_LEVEL_ADC_PLUS || ADC_sf[2][0]ERR_LEVEL_ADC_PLUS || ADC_sf[2][1]ERR_LEVEL_ADC_PLUS || ADC_f[2]ERR_LEVEL_ADC_PLUS || ADC_sf[2][3]ERR_LEVEL_ADC_PLUS || ADC_f[8]ERR_LEVEL_ADC_PLUS || ADC_f[9]ERR_LEVEL_ADC_PLUS || ADC_f[10]ERR_LEVEL_ADC_PLUS_6 || ADC_f[2][11]maxU) maxU = buf_U1_3point[1]; -// if (buf_U1_3point[2]>maxU) maxU = buf_U1_3point[2]; -// -// if (buf_U1_3point[1]= component_Ready) - detect_zero_analog(i); - } - -// zero_ADC[1][2] = 2010;//1976; // uab -// zero_ADC[1][3] = 2010;//1989; // ubc -// zero_ADC[1][4] = 2010;//1994; // uca - - - zero_ADC[0][0]=zero_ADC[0][2];//2042;//1992;//1835; //uzpt - zero_ADC[0][1]=zero_ADC[0][2];//2042;//1992;//1835; //uzpt - - - -#if (COUNT_ARR_ADC_BUF>1) - zero_ADC[1][1]=zero_ADC[1][15]; - zero_ADC[1][2]=zero_ADC[1][15]; - zero_ADC[1][3]=zero_ADC[1][15]; - zero_ADC[1][4]=zero_ADC[1][15]; - zero_ADC[1][5]=zero_ADC[1][15]; - zero_ADC[1][6]=zero_ADC[1][15]; - zero_ADC[1][7]=zero_ADC[1][15]; - zero_ADC[1][8]=zero_ADC[1][15]; - zero_ADC[1][9]=zero_ADC[1][15]; - zero_ADC[1][10]=zero_ADC[1][15]; - zero_ADC[1][11]=zero_ADC[1][15]; - zero_ADC[1][12]=zero_ADC[1][15]; - zero_ADC[1][13]=zero_ADC[1][15]; - zero_ADC[1][14]=zero_ADC[1][15]; -#endif - - - for (k=0;k<16;k++) - { - for (i=0;i2200) || (zero_ADC[i][k]<1900)) - zero_ADC[i][k] = DEFAULT_ZERO_ADC; - } - } - - - - for (k=0;k<16;k++) - { - for (i=0;ierror_counts.adc_0) -// if (ADC_sf[i][k] >= ERR_LEVEL_ADC_PLUS) detect_protect_ACP_plus (i, k); - // if (ADC_sf[i][k] <= ERR_LEVEL_ADC_MINUS) detect_protect_ACP_minus(i, k); - } - } - - -} - -#if (USE_INTERNAL_ADC==1) -#pragma CODE_SECTION(fast_detect_protect_ACP_internal,".fast_run"); -void fast_detect_protect_ACP_internal(void) -{ - int k; - k=0; - if (ADC_sf[COUNT_ARR_ADC_BUF-1][k] >= ERR_LEVEL_ADC_PLUS) detect_protect_ACP_plus (2, k); - if (ADC_sf[COUNT_ARR_ADC_BUF-1][k] <= ERR_LEVEL_ADC_MINUS) detect_protect_ACP_minus(2, k); - k=1; - if (ADC_sf[COUNT_ARR_ADC_BUF-1][k] >= ERR_LEVEL_ADC_PLUS) detect_protect_ACP_plus (2, k); - if (ADC_sf[COUNT_ARR_ADC_BUF-1][k] <= ERR_LEVEL_ADC_MINUS) detect_protect_ACP_minus(2, k); - k=3; - if (ADC_sf[COUNT_ARR_ADC_BUF-1][k] >= ERR_LEVEL_ADC_PLUS) detect_protect_ACP_plus (2, k); - if (ADC_sf[COUNT_ARR_ADC_BUF-1][k] <= ERR_LEVEL_ADC_MINUS) detect_protect_ACP_minus(2, k); - - -} -#endif - - -#pragma CODE_SECTION(fast_detect_protect_ACP,".fast_run"); -void fast_detect_protect_ACP() -{ - int i,k; - -// for (i=0;i<2;i++) - { -// if (project.adc[i].status == component_Ready) -#if(C_adc_number>=1) - i = 0; - for (k=0;k<14;k++) - { - if (ADC_f[i][k] >= ERR_LEVEL_ADC_PLUS) detect_protect_ACP_plus (i, k); - if (ADC_f[i][k] <= ERR_LEVEL_ADC_MINUS) detect_protect_ACP_minus(i, k); - } -#endif -#if(C_adc_number>=2) - i = 1; - for (k=2;k<5;k++) - { - if (ADC_f[i][k] >= ERR_LEVEL_ADC_PLUS) detect_protect_ACP_plus (i, k); - if (ADC_f[i][k] <= ERR_LEVEL_ADC_MINUS) detect_protect_ACP_minus(i, k); - } -#endif -#if(C_adc_number>=3) - i = 2; - for (k=0;k<15;k++) - { - if (ADC_f[i][k] >= ERR_LEVEL_ADC_PLUS) detect_protect_ACP_plus (i, k); - if (ADC_f[i][k] <= ERR_LEVEL_ADC_MINUS) detect_protect_ACP_minus(i, k); - } -#endif - - } - -} - -#pragma CODE_SECTION(norma_adc,".fast_run"); -inline _iq norma_adc(int plane, int chan) -{ -// return _IQ19toIQ(_IQ19mpy((iq19_zero_ADC[n_norm] - ((long)ADC_sf[plane][chan]<<19) ),iq19_k_norm_ADC[n_norm])); - return _IQ19toIQ(_IQ19mpy((((long)ADC_f[plane][chan]<<19) - iq19_zero_ADC[plane][chan]),iq19_k_norm_ADC[plane][chan])); -} - - - -#if (USE_INTERNAL_ADC==1) -#pragma CODE_SECTION(norma_adc_internal_sf,".fast_run2"); -_iq norma_adc_internal_sf(int l) -{ - return _IQ19toIQ(_IQ19mpy((((long)ADC_sf[2][l]<<19) - iq19_zero_ADC[2][l]),iq19_k_norm_ADC[2][l])); -} -#endif - - -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -// -//#pragma CODE_SECTION(fast_read_all_adc_one,".fast_run"); -//void fast_read_all_adc_one(int cc) -//{ -// int i,k; -// int t; -// -// i_led1_on_off(1); -// -// project.adc[0].read_pbus(&project.adc[0]); -// -// for (k=0;k<16;k++) -// { -// t = project.adc[0].read.pbus.adc_value[k]; -// ADC_fast[0][k][cc] = t; -// -// // save max values -// if (t>ADC_fast[0][k][1] || cc==3) -// ADC_fast[0][k][1] = t; -// // save min values -// if (tflags.bit.error==0 ) -// { -// -// -// -// fast_read_all_adc_one(3); -// pause_1000(p); -// fast_read_all_adc_one(4); -// pause_1000(p); -// fast_read_all_adc_one(5); -// pause_1000(p); -// fast_read_all_adc_one(6); -// pause_1000(p); -// fast_read_all_adc_one(7); -// pause_1000(p); -// fast_read_all_adc_one(8); -// -// -// -// for (k=0;k<16;k++) -// { -// ADC_fast[0][k][0] = (-ADC_fast[0][k][1] - ADC_fast[0][k][2] + ADC_fast[0][k][3] + ADC_fast[0][k][4] -// +ADC_fast[0][k][5] + ADC_fast[0][k][6] + ADC_fast[0][k][7] + ADC_fast[0][k][8]) >> 2; // 4 -// } -// -// } -// else -// { -// for (k=0;k<16;k++) -// { -// ADC_fast[0][k][0] = 5000; // error -// } -// } -// -// -// if (project.adc[1].status == component_Ready -// && project.controller.read.errors.bit.error_pbus == 0 -// && project.controller.read.errors_buses.bit.slave_addr_error==0 -// && project.x_parallel_bus->flags.bit.error==0 ) -// { -// -// fast_read_all_adc_two(); -// } -// else -// { -// for (k=0;k<16;k++) -// { -// ADC_fast[1][k][0] = 5000; // error -// } -// } -// -//} - -///////////////////////////////////////////////////////// -// -//#pragma CODE_SECTION(norma_fast_adc,".fast_run"); -//void norma_fast_adc(void) -//{ -// int i,k; -//// int bb; -// -//#ifdef LOG_ACP_TO_BUF -// static int c_log=0; -// static int n_log_acp_p=0; -// static int n_log_acp_c=2; -// static int n_log_acp_p_2=0; -// static int n_log_acp_c_2=2; -// -//#endif -// -// for (i=0;iflags.bit.error==0 ) -// { -// for (k=0;k<16;k++) -// { -// iq_norm_ADC[i][k] = _IQ19toIQ(_IQ19mpy((-iq19_zero_ADC[i][k] + ((long)ADC_fast[i][k][0]<<19) ),iq19_k_norm_ADC[i][k])); -// } -// } -// else -// { -// for (k=0;k<16;k++) -// { -// iq_norm_ADC[i][k] = 0; -// } -// -// } -// -// } -// -//#ifdef LOG_ACP_TO_BUF -// if (c_log>=SIZE_BUF_LOG_ACP) -// c_log=0; -// BUF_ADC[c_log]=ADC_fast[n_log_acp_p][n_log_acp_c][0]; -// BUF_ADC_2[c_log]=ADC_fast[n_log_acp_p_2][n_log_acp_c_2][3]; -// c_log++; -//#endif -// -////i_led2_off(); -//} - -///////////////////////////////////////////////////////// - -/* -#pragma CODE_SECTION(norma_all_adc,".fast_run"); -void norma_all_adc(void) -{ - int i,k; -// int bb; -#ifdef LOG_ACP_TO_BUF - static int c_log=0; - static int n_log_acp_p=0; - static int n_log_acp_c=2; - static int n_log_acp_p_2=0; - static int n_log_acp_c_2=5; - -#endif - - for (i=0;iread_pbus(&project.adc[i]); - - if ( project.adc[i].status == component_Ready - && project.controller.read.errors.bit.error_pbus == 0 - && project.controller.read.errors_buses.bit.slave_addr_error==0 - && project.x_parallel_bus->flags.bit.error==0 ) - { - for (k=0;k<16;k++) - { - -// ADC_f[i][k] = (int)pr->data.adc[i].acc_short[k]; - -#ifdef ADC_READ_FROM_PARALLEL_BUS - ADC_f[i][k] = project.adc[i].read.pbus.adc_value[k]; - ADC_sf[i][k] += (((int)(ADC_f[i][k] - ADC_sf[i][k]))>>SDVIG_K_FILTER_S); -#else -// ADC_f[i][k] = project.adc[i].fpga.read.channels[k].value.acc_short;//iq_norm_ADC[i][k] = _IQ19toIQ(_IQ19mpy((iq19_zero_ADC[i][k] - ((long)project.adc[i].fpga.read.channels[k].value.acc_short<<19) ),iq19_k_norm_ADC[i][k])); -#endif - iq_norm_ADC[i][k] = _IQ19toIQ(_IQ19mpy((-iq19_zero_ADC[i][k] + ((long)ADC_f[i][k]<<19) ),iq19_k_norm_ADC[i][k])); -// iq_norm_ADC_sf[i][k] = _IQ19toIQ(_IQ19mpy((iq19_zero_ADC[i][k] - ((long)ADC_sf[i][k]<<19) ),iq19_k_norm_ADC[i][k])); - } - } - else - { - for (k=0;k<16;k++) - { - ADC_f[i][k] = 5000;//DEFAULT_ZERO_ADC; - ADC_sf[i][k] = 5000;//DEFAULT_ZERO_ADC; - iq_norm_ADC[i][k] = 0; - } - - } - - } - -#ifdef LOG_ACP_TO_BUF - if (c_log>=SIZE_BUF_LOG_ACP) - c_log=0; - BUF_ADC[c_log]=ADC_f[n_log_acp_p][n_log_acp_c]; - BUF_ADC_2[c_log]=ADC_f[n_log_acp_p_2][n_log_acp_c_2]; - c_log++; -#endif - - -#if (USE_INTERNAL_ADC==1) - iq_norm_ADC[COUNT_ARR_ADC_BUF-1][0] = norma_adc_internal_sf(0); - iq_norm_ADC[COUNT_ARR_ADC_BUF-1][1] = norma_adc_internal_sf(1); - iq_norm_ADC[COUNT_ARR_ADC_BUF-1][3] = norma_adc_internal_sf(3); -#endif -//i_led2_off(); -} -*/ -//////////////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(norma_adc_nc,".fast_run"); -void norma_adc_nc(int nc) -{ - int k; -// int bb; - - project.read_errors_controller(); - project.adc[nc].read_pbus(&project.adc[nc]); - - if ( project.adc[nc].status == component_Ready - && project.controller.read.errors.bit.error_pbus == 0 - && project.controller.read.errors_buses.bit.slave_addr_error==0 - && project.x_parallel_bus->flags.bit.error==0 ) - { - for (k=0;k<16;k++) - { - - ADC_f[nc][k] = project.adc[nc].read.pbus.adc_value[k]; - ADC_sf[nc][k] += (((int)(ADC_f[nc][k] - ADC_sf[nc][k]))>>SDVIG_K_FILTER_S); - iq_norm_ADC[nc][k] = _IQ19toIQ(_IQ19mpy((-iq19_zero_ADC[nc][k] + ((long)ADC_f[nc][k]<<19) ),iq19_k_norm_ADC[nc][k])); - } - } - else - { - for (k=0;k<16;k++) - { - ADC_f[nc][k] = 5000;//DEFAULT_ZERO_ADC; - ADC_sf[nc][k] = 5000;//DEFAULT_ZERO_ADC; - iq_norm_ADC[nc][k] = 0; - } - - } -} - -//////////////////////////////////////////////////////////////////// - - -#pragma CODE_SECTION(calc_norm_ADC_1,".fast_run"); -void calc_norm_ADC_1(int run_norma) -{ - _iq a1,a2,a3; - -#if (USE_ADC_1) - - if (run_norma) - norma_adc_nc(1); - - -#if (_FLOOR6==1) - - analog.T_U01 = - analog.T_U02 = - analog.T_U03 = - analog.T_U04 = - analog.T_U05 = - analog.T_U06 = - analog.T_U07 = - analog.T_Water_external = - analog.T_Water_internal = - - analog.P_Water_internal = - - analog.T_Air_01 = - analog.T_Air_02 = - analog.T_Air_03 = - analog.T_Air_04 = 0; - -#else - analog.T_U01 = iq_norm_ADC[1][1]; - analog.T_U02 = iq_norm_ADC[1][2]; - analog.T_U03 = iq_norm_ADC[1][3]; - analog.T_U04 = iq_norm_ADC[1][4]; - analog.T_U05 = iq_norm_ADC[1][5]; - analog.T_U06 = iq_norm_ADC[1][6]; - analog.T_U07 = iq_norm_ADC[1][7]; - analog.T_Water_external = iq_norm_ADC[1][9]; - analog.T_Water_internal = iq_norm_ADC[1][8]; - - analog.P_Water_internal = iq_norm_ADC[1][14]; - - analog.T_Air_01 = iq_norm_ADC[1][10]; - analog.T_Air_02 = iq_norm_ADC[1][11]; - analog.T_Air_03 = iq_norm_ADC[1][12]; - analog.T_Air_04 = iq_norm_ADC[1][13]; - - -#endif -#else - - analog.T_U01 = - analog.T_U02 = - analog.T_U03 = - analog.T_U04 = - analog.T_U05 = - analog.T_U06 = - analog.T_U07 = - analog.T_Water_external = - analog.T_Water_internal = - - analog.P_Water_internal = - - analog.T_Air_01 = - analog.T_Air_02 = - analog.T_Air_03 = - analog.T_Air_04 = 0; - -#endif -// analog.iqI_vozbud = iq_norm_ADC[1][13]; - -} - -//////////////////////////////////////////////////////////////////// -#pragma CODE_SECTION(calc_norm_ADC_0,".fast_run"); -void calc_norm_ADC_0(int run_norma) -{ - _iq a1,a2,a3; - -#if (USE_ADC_0) - - if (run_norma) - norma_adc_nc(0); - -#if (_FLOOR6) - analog.iqU_1 = iq_norm_ADC[0][0] - analog_zero.iqU_1 + analog.iqU_1_imit; - analog.iqU_2 = iq_norm_ADC[0][1] - analog_zero.iqU_2 + analog.iqU_1_imit; -#else - analog.iqU_1 = iq_norm_ADC[0][0] - analog_zero.iqU_1; - analog.iqU_2 = iq_norm_ADC[0][1] - analog_zero.iqU_2; -#endif - analog.iqIu_1 = iq_norm_ADC[0][2]; - analog.iqIv_1 = iq_norm_ADC[0][3]; - analog.iqIw_1 = iq_norm_ADC[0][4]; - - analog.iqIu_2 = iq_norm_ADC[0][5]; - analog.iqIv_2 = iq_norm_ADC[0][6]; - analog.iqIw_2 = iq_norm_ADC[0][7]; - - analog.iqIin_1 = -iq_norm_ADC[0][9]; // - analog.iqIin_2 = -iq_norm_ADC[0][9]; // - - analog.iqUin_A1B1 = iq_norm_ADC[0][10]; - -// 23550.1 - -// 23550.1 - - analog.iqUin_B1C1 = iq_norm_ADC[0][11]; // 23550.1 - analog.iqUin_A2B2 = iq_norm_ADC[0][12]; // 23550.1 - -// 23550.3 bs1 bs2 - -// analog.iqUin_B1C1 = iq_norm_ADC[0][12]; // 23550.3 -// analog.iqUin_A2B2 = iq_norm_ADC[0][11]; // 23550.3 -// - analog.iqUin_B2C2 = iq_norm_ADC[0][13]; - - analog.iqIbreak_1 = iq_norm_ADC[0][14]; - analog.iqIbreak_2 = iq_norm_ADC[0][15]; - -#else - analog.iqU_1 = analog.iqIu_1 = analog.iqIu_2 = analog.iqIv_1 = analog.iqIv_2 = - analog.iqIw_1 = analog.iqIw_2 = analog.iqIin_1 = analog.iqIin_2 = analog.iqUin_A1B1 = - analog.iqUin_B1C1 = analog.iqUin_A2B2 = analog.iqUin_B2C2 = analog.iqIbreak_1 = analog.iqIbreak_2 - = 0; -#endif - - analog.iqUin_C1A1 = -(analog.iqUin_A1B1 + analog.iqUin_B1C1); - analog.iqUin_C2A2 = -(analog.iqUin_A2B2 + analog.iqUin_B2C2); - - - - filter.iqU_1_long = exp_regul_iq(koef_Uzpt_long_filter, filter.iqU_1_long, analog.iqU_1); - filter.iqU_2_long = exp_regul_iq(koef_Uzpt_long_filter, filter.iqU_2_long, analog.iqU_2); - - -// analog.iqU_1_fast = filter_U1_3point(analog.iqU_1_fast); - filter.iqU_1_fast = exp_regul_iq(koef_Uzpt_fast_filter, filter.iqU_1_fast, analog.iqU_1); - filter.iqU_2_fast = exp_regul_iq(koef_Uzpt_fast_filter, filter.iqU_2_fast, analog.iqU_2); - - -// filter.iqUzpt_2_2_fast = exp_regul_iq(koef_Uzpt_fast_filter, filter.iqUzpt_2_2_fast, analog.iqUzpt_2_2); - - - -//15 - - - analog.iqIm_1 = im_calc(analog.iqIu_1, analog.iqIv_1, analog.iqIw_1); - analog.iqIm_2 = im_calc(analog.iqIu_2, analog.iqIv_2, analog.iqIw_2); - - analog.iqIu = analog.iqIu_1+analog.iqIu_2; - analog.iqIv = analog.iqIv_1+analog.iqIv_2; - analog.iqIw = analog.iqIw_1+analog.iqIw_2; - - analog.iqIm = im_calc(analog.iqIu, analog.iqIv, analog.iqIw); - - - analog.iqIin_sum = analog.iqIin_1+analog.iqIin_2; - -// analog.iqIm_3 = im_calc(analog.iqIa1_1_fir_n+analog.iqIa2_1_fir_n, analog.iqIb1_1_fir_n+analog.iqIb2_1_fir_n, analog.iqIc1_1_fir_n+analog.iqIc2_1_fir_n); - - analog.iqUin_m1 = im_calc(analog.iqUin_A1B1, analog.iqUin_B1C1, analog.iqUin_C1A1); - analog.iqUin_m2 = im_calc(analog.iqUin_A2B2, analog.iqUin_B2C2, analog.iqUin_C2A2); - -// analog.iqUin_m2 = im_calc(analog.UinA2, analog.UinB2, analog.UinC2); - - filter.iqUin_m1 = exp_regul_iq(koef_Uin_filter, filter.iqUin_m1, analog.iqUin_m1); - filter.iqUin_m2 = exp_regul_iq(koef_Uin_filter, filter.iqUin_m2, analog.iqUin_m2); - - - -// i_led1_on_off(0); -// i_led1_on_off(1); - -//1 - - filter.iqIm_1 = exp_regul_iq(koef_Im_filter, filter.iqIm_1, analog.iqIm_1); - filter.iqIm_2 = exp_regul_iq(koef_Im_filter, filter.iqIm_2, analog.iqIm_2); - filter.iqIm = exp_regul_iq(koef_Im_filter, filter.iqIm, analog.iqIm); - - filter.iqIin_sum = exp_regul_iq(koef_Im_filter, filter.iqIin_sum, analog.iqIin_sum); - -//3 -// filter_batter2_Iin.InpVarCurr = (analog.iqIin_1)-ZERO_I_IN; - // filter_batter2_Iin.calc(&filter_batter2_Iin); - -// filter.iqIin = _IQmpy(filter_batter2_Iin.Out,_IQ_09); - - - filter.iqIin_1 = exp_regul_iq(koef_Im_filter, filter.iqIin_1, analog.iqIin_1); - filter.iqIin_2 = exp_regul_iq(koef_Im_filter, filter.iqIin_2, analog.iqIin_2); - - a1 = analog.iqU_1+analog.iqU_2; - a2 = analog.iqIin_1; - a3 = _IQmpy(a1,a2); - analog.PowerScalar = a3; -// filter.Power = analog.iqU_1+analog.iqU_2; - filter.PowerScalar = exp_regul_iq(koef_Power_filter, filter.PowerScalar, analog.PowerScalar); - filter.PowerScalarFilter2 = exp_regul_iq(koef_Power_filter2, filter.PowerScalarFilter2, filter.PowerScalar); - -} - - - -#pragma DATA_SECTION(acp_zero,".slow_vars") -_iq19 acp_zero[16]; -#pragma DATA_SECTION(acp_summ,".slow_vars") -long acp_summ[16]; -/********************************************************************/ -/* y */ -/********************************************************************/ -void detect_zero_analog(int nc) -{ - long i,k; - - - _iq koef_zero_ADC_filter = _IQ19(0.00002/0.0003185); - - - for (k=0;k<16;k++) - { - acp_zero[k] = 0; - acp_summ[k] = 0; - } - - - - for (i=0; i=1) -#define R_ADC_DEFAULT_0 { 271, 271, 876, 876, 876, 876, 876, 876, 249, 249, 301, 301, 301, 301, 301, 301 } -#define K_LEM_ADC_DEFAULT_0 { 7200, 7200, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 8400, 8400, 8400, 8400, 5000, 5000 } -#define NORMA_ADC_DEFAULT_0 { NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP } -#endif - -#if(C_adc_number>=2) -#define R_ADC_DEFAULT_1 { 1, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190 } -#define K_LEM_ADC_DEFAULT_1 { 1, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1 } -#define NORMA_ADC_DEFAULT_1 { NORMA_ACP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_TEMPER_MILL_AMP, NORMA_ACP_P, NORMA_ACP } -#endif - -#if(C_adc_number>=3) -#define R_ADC_DEFAULT_2 { 271, 271, 887, 887, 887, 887, 887, 887, 250, 250, 3125, 3125, 3125, 3125, 309, 309 } -#define K_LEM_ADC_DEFAULT_2 { 7200, 7200, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 60000, 60000, 60000, 60000, 5000, 5000 } -#define NORMA_ADC_DEFAULT_2 { NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP } -#endif - -// 23550.1 - -//#if(C_adc_number>=1) -//#define R_ADC_DEFAULT_0 { 271, 271, 887, 887, 887, 887, 887, 887, 250, 250, 312, 312, 312, 312, 309, 309 } -//#define K_LEM_ADC_DEFAULT_0 { 7200, 7200, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 8400, 8400, 8400, 8400, 5000, 5000 } -//#define NORMA_ADC_DEFAULT_0 { NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP } -//#endif -// -//#if(C_adc_number>=2) -//#define R_ADC_DEFAULT_1 { 1, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190, 6190 } -//#define K_LEM_ADC_DEFAULT_1 { 1, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1 } -//#define NORMA_ADC_DEFAULT_1 { NORMA_ACP, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_TEMPER, NORMA_ACP_P, NORMA_ACP } -//#endif -// -//#if(C_adc_number>=3) -//#define R_ADC_DEFAULT_2 { 271, 271, 887, 887, 887, 887, 887, 887, 250, 250, 3125, 3125, 3125, 3125, 309, 309 } -//#define K_LEM_ADC_DEFAULT_2 { 7200, 7200, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 5000, 60000, 60000, 60000, 60000, 5000, 5000 } -//#define NORMA_ADC_DEFAULT_2 { NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP } -//#endif - - - - -#if (USE_INTERNAL_ADC==1) -#define R_ADC_DEFAULT_INTERNAL { 100,100,100,100,100,100,100,100,1248,1248,1248,100,100,100,100,100 } -#define K_LEM_ADC_DEFAULT_INTERNAL { 30,30,30,30,10,10,10,10,621,621,621,100,10,10,10,10 } -#define NORMA_ADC_DEFAULT_INTERNAL { NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP, NORMA_ACP } -#endif - - - - - -/* - //awa3 - //14 out1 - 0 - 11 - //15 out2 - 0 - 11 - //8 - 0 - 20 | 0 - 200 / - 0V - 1.5V / 0 - 200 / - //9 - 0 - 20 | 0 - 200 / - 0V - 1.5V / 0 - 200 / - - //10 - 0 - 20 | 0 - 200 / - 0V - 1.5V / 0 - 200 / - - //11 - 0 - 20 | 0 - 200 / - 0V - 1.5V / 0 - 200 / - //12 - 4 - 20 | 0 - 10 / - 0.3V - 1.5V / 0 - 10 / - - //13 - 4 - 20 | 0 - 10 / - 0.3V - 1.5V / 0 - 10 / -*/ - - -typedef union -{ - - struct - { - unsigned int c0_plus :1; /* 0 + */ - unsigned int c1_plus :1; /* 0 + */ - unsigned int c2_plus :1; /* 0 + */ - unsigned int c3_plus :1; /* 0 + */ - unsigned int c4_plus :1; /* 0 + */ - unsigned int c5_plus :1; /* 0 + */ - unsigned int c6_plus :1; /* 0 + */ - unsigned int c7_plus :1; /* 0 + */ - unsigned int c8_plus :1; /* 0 + */ - unsigned int c9_plus :1; /* 0 + */ - unsigned int c10_plus :1; /* 0 + */ - unsigned int c11_plus :1; /* 0 + */ - unsigned int c12_plus :1; /* 0 + */ - unsigned int c13_plus :1; /* 0 + */ - unsigned int c14_plus :1; /* 0 + */ - unsigned int c15_plus :1; /* 0 + */ - } bit; /* */ - unsigned long all; /* */ - -} ERR_ADC_PLUS_PROTECT; - - -typedef union -{ - - struct - { - unsigned int c0_minus :1; /* 0 - */ - unsigned int c1_minus :1; /* 0 - */ - unsigned int c2_minus :1; /* 0 - */ - unsigned int c3_minus :1; /* 0 - */ - unsigned int c4_minus :1; /* 0 - */ - unsigned int c5_minus :1; /* 0 - */ - unsigned int c6_minus :1; /* 0 - */ - unsigned int c7_minus :1; /* 0 - */ - unsigned int c8_minus :1; /* 0 - */ - unsigned int c9_minus :1; /* 0 - */ - unsigned int c10_minus :1; /* 0 - */ - unsigned int c11_minus :1; /* 0 - */ - unsigned int c12_minus :1; /* 0 - */ - unsigned int c13_minus :1; /* 0 - */ - unsigned int c14_minus :1; /* 0 - */ - unsigned int c15_minus :1; /* 0 - */ - - } bit; /* */ - unsigned int all; /* */ - -} ERR_ADC_MINUS_PROTECT; - - -typedef struct -{ - ERR_ADC_PLUS_PROTECT plus; - ERR_ADC_MINUS_PROTECT minus; -} ERR_ADC_PROTECT; - - -/* y y */ -typedef struct -{ - _iq iqU_1; - _iq iqU_2; - - _iq iqU_1_fast; - _iq iqU_2_fast; - - _iq iqU_1_long; - _iq iqU_2_long; - - _iq iqIu_1; - _iq iqIv_1; - _iq iqIw_1; - - _iq iqIu_2; - _iq iqIv_2; - _iq iqIw_2; - - _iq iqIu_1_rms; - _iq iqIv_1_rms; - _iq iqIw_1_rms; - - _iq iqIu_2_rms; - _iq iqIv_2_rms; - _iq iqIw_2_rms; - - _iq iqIu; - _iq iqIv; - _iq iqIw; - - _iq iqIin_1; - _iq iqIin_2; - - _iq iqUin_A1B1; - _iq iqUin_B1C1; - _iq iqUin_C1A1; - - _iq iqUin_A2B2; - _iq iqUin_B2C2; - _iq iqUin_C2A2; - - _iq iqUin_A1B1_rms; - _iq iqUin_B1C1_rms; - _iq iqUin_C1A1_rms; - - _iq iqUin_A2B2_rms; - _iq iqUin_B2C2_rms; - _iq iqUin_C2A2_rms; - - _iq iqUin_m1; - _iq iqUin_m2; - - _iq iqIbreak_1; - _iq iqIbreak_2; //39 - - _iq T_U01; - _iq T_U02; - _iq T_U03; - _iq T_U04; - _iq T_U05; - _iq T_U06; - _iq T_U07; - - _iq T_Water_external; - _iq T_Water_internal; - - _iq T_Air_01; - _iq T_Air_02; - _iq T_Air_03; - _iq T_Air_04; - - _iq P_Water_internal; //53 - - - _iq iqI_vozbud; - - _iq iqIin_sum; - - _iq iqIm_1; - _iq iqIm_2; - - _iq iqIm; - - - _iq iqM; - - _iq PowerScalar; - _iq PowerScalarFilter2; - _iq PowerFOC; - - _iq iqU_1_imit; //63 - - - /* - _iq iqUzpt_1_2; //uzpt1 bs2 - _iq iqUzpt_2_2; //uzpt2 bs2 - _iq iqUzpt_1_2_fast; //uzpt1 bs2 - _iq iqUzpt_2_2_fast; //uzpt2 bs2 - _iq iqUzpt_1_2_long; //uzpt1 bs2 - _iq iqUzpt_2_2_long; //uzpt2 bs2 - _iq iqIin_1_1; //Iin AF1 BS1 - _iq iqIin_2_1; //Iin AF2 BS1 - _iq iqIin_3_1; //Iin AF3 BS1 - _iq iqIin_4_1; //Iin AF4 BS1 - _iq iqIin_5_1; //Iin AF5 BS1 - _iq iqIin_6_1; //Iin AF6 BS1 - _iq iqIin_1_2; //Iin AF1 BS2 - _iq iqIin_2_2; //Iin AF2 BS2 - _iq iqIin_3_2; //Iin AF3 BS2 - _iq iqIin_4_2; //Iin AF4 BS2 - _iq iqIin_5_2; //Iin AF5 BS2 - _iq iqIin_6_2; //Iin AF6 BS2 - _iq iqUin_AB; // AB - _iq iqUin_BC; // BC - _iq iqUin_CA; // CA - _iq iqUin_AB_sf; // AB - _iq iqUin_BC_sf; // BC - _iq iqUin_CA_sf; // CA - _iq iqT_WATER_in; // - _iq iqT_WATER_out; // - _iq iqT_AIR_in_up; // () - _iq iqT_AIR_in_down;// () - _iq iqP_WATER_in; // - _iq iqP_WATER_out; // - - _iq iqT_BK1_BK12; // BK1_BK12 - _iq iqT_BK13_BK24; // BK13_BK24 - - _iq iqUin_m1; // - - - _iq iqIu_1_1; //Iu AF1 BS1 - _iq iqIu_1_2; //Iu AF2 BS1 - _iq iqIv_1_1; //Iv AF3 BS1 - _iq iqIv_1_2; //Iv AF4 BS1 - _iq iqIw_1_1; //Iw AF5 BS1 - _iq iqIw_1_2; //Iw AF6 BS1 - - - - _iq iqIu_2_1; //Iu AF1 BS2 - _iq iqIu_2_2; //Iu AF2 BS2 - _iq iqIv_2_1; //Iv AF3 BS2 - _iq iqIv_2_2; //Iv AF4 BS2 - _iq iqIw_2_1; //Iw AF5 BS2 - _iq iqIw_2_2; //Iw AF6 BS2 - - _iq iqIm_1; - _iq iqIm_2; - - _iq iqWexp; - _iq iqWout; - - _iq iqM; -*/ -} ANALOG_VALUE; - - -#define ANALOG_VALUE_DEFAULT {0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0, 0,0, 0} -/* y y */ - - -#define ERR_LEVEL_ADC_PLUS 3950 //+1270A //2950 // +650A //3467 // 3367 //3367 //3267 // 0xfff-0x29c -#define ERR_LEVEL_ADC_MINUS 150 //-1270A //1150 //-650A // 267 //367 - -#define ERR_LEVEL_ADC_PLUS_6 3800 //3783 //3623~1150 // 3462 ~ 1050 A // 3320 ~ 960A //3680 //3267 // 0xfff-0x29c -#define ERR_LEVEL_ADC_MINUS_6 1000 //267 //367 - -#define MIN_DETECT_UD_ZERO 2300 - - -#define level_err_ADC_PLUS_default {ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,\ - ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,\ - ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,\ - ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS,ERR_LEVEL_ADC_PLUS} - -#define level_err_ADC_MINUS_default {ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,\ - ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,\ - ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,\ - ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS,ERR_LEVEL_ADC_MINUS} - - -extern ANALOG_VALUE analog; -extern ANALOG_VALUE filter; -extern ANALOG_VALUE analog_zero; - -//void calc_norm_ADC(int fast); -void calc_norm_ADC_0(int run_norma); -void calc_norm_ADC_1(int run_norma); -void Init_Adc_Variables(void); -void norma_adc_nc(int nc); - - - -extern int ADC_f[COUNT_ARR_ADC_BUF][16]; -extern int zero_ADC[COUNT_ARR_ADC_BUF][16]; - -extern ERR_ADC_PROTECT err_adc_protect[COUNT_ARR_ADC_BUF],mask_err_adc_protect[COUNT_ARR_ADC_BUF]; - -extern unsigned int R_ADC[COUNT_ARR_ADC_BUF][16]; -extern unsigned int K_LEM_ADC[COUNT_ARR_ADC_BUF][16]; -extern float K_NORMA_ADC[COUNT_ARR_ADC_BUF][16]; - -//void norma_all_adc(void); -extern _iq koef_Uzpt_long_filter, koef_Uzpt_fast_filter, koef_Uin_filter, koef_Im_filter, koef_Power_filter, koef_Power_filter2; - -void detect_zero_analog(int nc); - - -#if (USE_INTERNAL_ADC==1) - -void Init_Internal_Adc(void); - -#endif - - -#endif // end _ADC_TOOLS diff --git a/Inu/Src2/551/main/alarm_log.c b/Inu/Src2/551/main/alarm_log.c deleted file mode 100644 index 7dd9e2e..0000000 --- a/Inu/Src2/551/main/alarm_log.c +++ /dev/null @@ -1,196 +0,0 @@ -/* - * alarm_log.c - * - * Created on: 11 . 2024 . - * Author: yura - */ - -#include "edrk_main.h" -#include "alarm_log_can.h" -#include "alarm_log.h" -#include "log_params.h" -#include "logs_hmi.h" -#include "global_time.h" - - -#define PAUSE_AUTO_SAVE_ALARM_LOG_SECONDS 20 // 20 sec -#define PAUSE_AUTO_STOP_ALARM_LOG_SECONDS 5 // 5 sec - -void send_alarm_log_pult(void) -{ - static int prev_imit_send_alarm_log_pult = 0; - int to_store = 0; - - static int flag_wait_alarm = 0, prev_Ready2 = 0, flag_store_log = 0, flag_store_log_prepare = 0, flag_auto_logs = 0, flag_stop_alarm = 0; - - static unsigned int pause_store_logs = 0, pause_stop_logs = 0; - - - - // - if (edrk.imit_send_alarm_log_pult && prev_imit_send_alarm_log_pult == 0) - flag_store_log = 1; - prev_imit_send_alarm_log_pult = edrk.imit_send_alarm_log_pult; - - - // - - - // ? - if (prev_Ready2==0 && edrk.Status_Ready.bits.ready_final) - { - flag_wait_alarm = 1; - flag_stop_alarm = 0; - } - - if (prev_Ready2==1 && edrk.Status_Ready.bits.ready_final==0) - { - // , - init_timer_sec(&pause_stop_logs); - - flag_stop_alarm = 1; - } - prev_Ready2 = edrk.Status_Ready.bits.ready_final; - - - // - if (flag_wait_alarm && edrk.summ_errors) - { - // - flag_store_log_prepare = 1; - flag_wait_alarm = 0; - flag_stop_alarm = 0; - init_timer_sec(&pause_store_logs); - } - -// // , -// if (flag_stop_alarm) -// { -// -// -// } - - // , - if (flag_stop_alarm && detect_pause_sec(PAUSE_AUTO_STOP_ALARM_LOG_SECONDS, &pause_stop_logs)) - { - flag_stop_alarm = 0; - flag_wait_alarm = 0; - } - - - if (flag_store_log_prepare) - { - flag_auto_logs = detect_pause_sec(PAUSE_AUTO_SAVE_ALARM_LOG_SECONDS, &pause_store_logs); - } - - if (flag_auto_logs) - { - flag_store_log_prepare = 0; - flag_auto_logs = 0; - // - flag_store_log = 1; - - flag_wait_alarm = 0; - flag_stop_alarm = 0; - } - - - - - - - - // - if (flag_store_log) - { - - if (edrk.pult_cmd.log_what_memory >= 2) - to_store = 2; - else - if (edrk.pult_cmd.log_what_memory >= 1) - to_store = 1; - else - to_store = 0; - - log_to_HMI.sdusb = to_store; - - - // - if (log_to_HMI.sdusb) - { - log_to_HMI.send_log = 3; - } - - flag_store_log = 0; - - } - - - - -} - - -void test_send_alarm_log(int alarm_cmd) -{ - static unsigned int points_alarm_log = 1000; - // static unsigned int nchannel_alarm_log = 30; - static int prev_alarm_cmd = 0; - static int local_alarm_cmd = 0; - - - - if (alarm_cmd && prev_alarm_cmd==0 && alarm_log_can.stage==0) - { -// i_led2_on(); - - alarm_log_can.post_points = COUNT_SAVE_LOG_OFF;//100; // - alarm_log_can.oscills = log_params.BlockSizeErr;//nchannel_alarm_log; // - - alarm_log_can.global_enable = 1; - - alarm_log_can.start_adr_temp = (int *)0xc0000; // , . - - // alarm_log_can.finish_adr_temp = (int *)0xa0000; // , . - - alarm_log_can.start_adr_real_logs = (int *)log_params.start_address_log;//(int *)START_ADDRESS_LOG; // - alarm_log_can.finish_adr_real_log = (int *)log_params.end_address_log;//(int *)logpar.; // - - alarm_log_can.current_adr_real_log = (int *)log_params.addres_mem; - - - alarm_log_can.temp_points = points_alarm_log; // . - // alarm_log_can.real_points = 1000; // , . - - alarm_log_can.step = 450; // mks - local_alarm_cmd = alarm_cmd; -// alarm_log_can.status_alarm = alarm_cmd;//cmd_alarm_log; // - alarm_log_can.start = 0x1; - alarm_log_can.stop = 0x1; - alarm_log_can.copy2temp = 0x1; - - - alarm_log_can.clear(&alarm_log_can); -// alarm_log_can.send(&alarm_log_can); - -// i_led2_off(); - } - else - local_alarm_cmd = 0; - - alarm_log_can.status_alarm = local_alarm_cmd;//cmd_alarm_log; // - alarm_log_can.send(&alarm_log_can); - - if (alarm_log_can.stage) - { -// i_led2_on_off(1); - } - else - { -// i_led2_on_off(0); - } - - prev_alarm_cmd = alarm_cmd; - -} -//////////////////////////////////////////////////////////// diff --git a/Inu/Src2/551/main/alarm_log.h b/Inu/Src2/551/main/alarm_log.h deleted file mode 100644 index c8fef58..0000000 --- a/Inu/Src2/551/main/alarm_log.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * alarm_log.h - * - * Created on: 11 . 2024 . - * Author: yura - */ - -#ifndef SRC_MAIN_ALARM_LOG_H_ -#define SRC_MAIN_ALARM_LOG_H_ - -void test_send_alarm_log(int alarm_cmd); -void send_alarm_log_pult(void); - - - -#endif /* SRC_MAIN_ALARM_LOG_H_ */ diff --git a/Inu/Src2/551/main/alg_simple_scalar.c b/Inu/Src2/551/main/alg_simple_scalar.c deleted file mode 100644 index 7fc3c8f..0000000 --- a/Inu/Src2/551/main/alg_simple_scalar.c +++ /dev/null @@ -1,976 +0,0 @@ -/* - * alg_simple_scalar.c - * - * Created on: 26 . 2020 . - * Author: Yura - */ - - -#include -#include -//#include -#include -#include -#include -#include -#include -//#include "log_to_mem.h" -#include "IQmathLib.h" -#include "math_pi.h" -#include "mathlib.h" -#include "params_pwm24.h" -#include "filter_v1.h" -#include "log_to_memory.h" - - -#pragma DATA_SECTION(simple_scalar1,".slow_vars"); -ALG_SIMPLE_SCALAR simple_scalar1 = ALG_SIMPLE_SCALAR_DEFAULT; - -_iq koefBpsi = _IQ(0.05); //0.05 - -void init_simple_scalar(void) -{ - simple_scalar1.mzz_add_1 = _IQ(MZZ_ADD_1/NORMA_MZZ); - simple_scalar1.mzz_add_2 = _IQ(MZZ_ADD_2/NORMA_MZZ); - simple_scalar1.mzz_add_3 = _IQ(MZZ_ADD_3/NORMA_MZZ); - - simple_scalar1.poluses = _IQ(POLUS); - simple_scalar1.iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ); - - simple_scalar1.powerzad_add = _IQ(POWERZAD_ADD_MAX); - simple_scalar1.powerzad_dec = _IQ(POWERZAD_DEC); - -// simple_scalar1.k_freq_for_pid = _IQ(1.0); - simple_scalar1.k_freq_for_pid = _IQ(450.0/FREQ_PWM); - - simple_scalar1.iq_add_kp_df = _IQ(ADD_KP_DF); - simple_scalar1.iq_add_ki_df = _IQ(ADD_KI_DF); - - simple_scalar1.min_mzz_for_df = _IQ(MIN_MZZ_FOR_DF/NORMA_MZZ); - - simple_scalar1.pidF_Kp = _IQ(PID_KP_F); - simple_scalar1.pidF_Ki = _IQ(PID_KI_F); - - - - simple_scalar1.pidIm1.Kp=_IQ(PID_KP_IM); - simple_scalar1.pidIm1.Ki=_IQ(PID_KI_IM); - - simple_scalar1.pidIm_Ki = simple_scalar1.pidIm1.Ki; - - simple_scalar1.pidIm1.Kc=_IQ(PID_KC_IM); - simple_scalar1.pidIm1.Kd=_IQ(PID_KD_IM); - - - simple_scalar1.pidIm1.OutMax=_IQ(K_STATOR_MAX); - simple_scalar1.pidIm1.OutMin=_IQ(K_STATOR_MIN); - -////////////// - - - simple_scalar1.pidF.Kp=_IQ(PID_KP_F); - simple_scalar1.pidF.Ki=_IQ(PID_KI_F); - simple_scalar1.pidF.Kc=_IQ(PID_KC_F); - simple_scalar1.pidF.Kd=_IQ(PID_KD_F); - - - simple_scalar1.pidF.OutMax=_IQ(500/NORMA_MZZ); - simple_scalar1.pidF.OutMin=_IQ(0); - // iq_MAX_DELTA_pidF = _IQ(MAX_DELTA_pidF/NORMA_WROTOR); -///////////////////////// -// simple_scalar1.pidPower_Kp = _IQ(PID_KP_POWER); -// simple_scalar1.pidPower_Ki = _IQ(PID_KI_POWER); - - - // iq_add_kp_dpower = _IQ(ADD_KP_DPOWER); - // iq_add_ki_dpower = _IQ(ADD_KI_DPOWER); - - simple_scalar1.pidPower.Kp=_IQ(PID_KP_POWER); - simple_scalar1.pidPower.Ki=_IQ(PID_KI_POWER); - simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid); // Ki - - simple_scalar1.pidPower.Kc=_IQ(PID_KC_POWER); - simple_scalar1.pidPower.Kd=_IQ(PID_KD_POWER); - - simple_scalar1.pidPower.OutMax=_IQ(500/NORMA_MZZ); - simple_scalar1.pidPower.OutMin=_IQ(0); - - - simple_scalar1.iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek 5% - - - // . - simple_scalar1.min_bpsi = _IQ(BPSI_MINIMAL/NORMA_FROTOR); - simple_scalar1.max_bpsi = _IQ(BPSI_MAXIMAL/NORMA_FROTOR); - -} - -/***************************************************************/ -/* / y 3 - - vector_moment(real Frot, - - real fzad, - - real mzz_zad, - - - real *Fz, - - yy - real *Uz1, - - . y 1 - real *Uz2) - - . y 2 - - - y y , y fzad - - y y = 0.5 - - yy 3- . - y y y */ -/****************************************************************/ - - -//#pragma CODE_SECTION(simple_scalar,".fast_run"); -void simple_scalar(int n_alg, - int n_wind_pump, - int direction, - _iq Frot_pid, - _iq Frot, - _iq fzad, - _iq iqKoefOgran, - _iq mzz_zad, - _iq bpsi_const, - _iq iqIm, - _iq iqUin, - _iq Iin, - _iq powerzad, - _iq power_pid, - _iq power_limit, - int mode_oborots_power, - _iq Izad_from_master, - int master, - int count_bs_work, - _iq *Fz, - _iq *Uz1, - _iq *Uz2, - _iq *Izad_out) -{ - - _iq mzz, dF, dI1, Izad, Uz_t1, Kpred_Ip, pred_Ip;//, znak_moment; - _iq dI2, Uz_t2; - - _iq pkk=0,ikk=0; - _iq Im_regul=0; - - - - static _iq bpsi=0; - // static _iq IQ_POLUS=0; - - - static _iq mzz_zad_int=0; - static _iq mzzi=0; - - static _iq I1_i=0; - static _iq I2_i=0; - - static _iq Im1=0; - static _iq Im2=0; - - static _iq Uze_t1=0; - static _iq Uze_t2=0; - - // static _iq fzad_ogr=0; - - - -// static _iq koef_Uz_t_filter=0; - static _iq dI1_prev=0; - static _iq Uz_t1_prev=0; - - static _iq dF_prev = 0; - static _iq mzz_prev = 0; - - // static _iq mzz_add_1, mzz_add_2; - - static _iq fzad_int=0;//, fzad_add_max;//,iq_mzz_max_for_fzad ; - static _iq fzad_add=0; //fzad_dec - _iq halfIm1, halfIm2; - - static _iq powerzad_int=0, powerzad_add_max=0, pidFOutMax = 0, pidFOutMin = 0 ; -//powerzad_dec powerzad_add -// static _iq koef_bpsi=0; - // static _iq min_bpsi=0; - static int flag_uz_t1=0; - - // static _iq correct_err=0; -// static _iq iq_dF_min1=0; -// static _iq iq_dF_min2=0; - _iq pred_dF,Kpred_dF; - static _iq dF_PREDEL_LEVEL2 = 0,dF_PREDEL_LEVEL1=0; - _iq Uze_ogr=0; - - // static _iq iq_spad_k=1; - static _iq myq_temp=0; - static _iq bpsi_filter=0; - static _iq _iq_koef_im_on_tormog = _IQ(KOEF_IM_ON_TORMOG); - static _iq _iq_koef_im_on_tormog_max_temper_break = _IQ(KOEF_IM_ON_TORMOG_WITH_MAX_TEMPER_BREAK); - static _iq n_iq_koef_im_on_tormog = CONST_IQ_1, t_iq_koef_im_on_tormog = CONST_IQ_1; - - static _iq _iq_koef_im_on_tormog_add =_IQ(0.0005), _iq_koef_im_on_tormog_dec = _IQ(0.01); - -// static _iq F_revers_level00= _IQ(70.0/60.0/NORMA_FROTOR); - static _iq F_revers_level0 = _IQ(90.0/60.0/NORMA_FROTOR); - static _iq F_revers_level1 = _IQ(100.0/60.0/NORMA_FROTOR); - static _iq F_revers_level2 = _IQ(110.0/60.0/NORMA_FROTOR); - static _iq F_revers_level3 = _IQ(120.0/60.0/NORMA_FROTOR); - static _iq F_revers_level4 = _IQ(130.0/60.0/NORMA_FROTOR); - static _iq F_revers_level5 = _IQ(140.0/60.0/NORMA_FROTOR); - static _iq F_revers_level6 = _IQ(150.0/60.0/NORMA_FROTOR); - static _iq F_revers_level7 = _IQ(160.0/60.0/NORMA_FROTOR); - static _iq F_revers_level8 = _IQ(170.0/60.0/NORMA_FROTOR); - static _iq F_revers_level9 = _IQ(180.0/60.0/NORMA_FROTOR); - static _iq F_revers_level10 = _IQ(190.0/60.0/NORMA_FROTOR); - static _iq F_revers_level11 = _IQ(200.0/60.0/NORMA_FROTOR); - static _iq F_revers_level12 = _IQ(210.0/60.0/NORMA_FROTOR); - static _iq F_revers_level13 = _IQ(220.0/60.0/NORMA_FROTOR); - - static _iq kF_revers_level00 = _IQ(0.65); - static _iq kF_revers_level0 = _IQ(0.70); - static _iq kF_revers_level1 = _IQ(0.75); - static _iq kF_revers_level2 = _IQ(0.78); - static _iq kF_revers_level3 = _IQ(0.80); - static _iq kF_revers_level4 = _IQ(0.82); - static _iq kF_revers_level5 = _IQ(0.84); - static _iq kF_revers_level6 = _IQ(0.86); - static _iq kF_revers_level7 = _IQ(0.88); - static _iq kF_revers_level8 = _IQ(0.90); - static _iq kF_revers_level9 = _IQ(0.91); - static _iq kF_revers_level10 = _IQ(0.92); - static _iq kF_revers_level11 = _IQ(0.93); - static _iq kF_revers_level12 = _IQ(0.94); - static _iq kF_revers_level13 = _IQ(0.96); - - - static _iq P_level0 = _IQ(70.0/60.0/NORMA_FROTOR); - static _iq P_level1 = _IQ(150.0/60.0/NORMA_FROTOR); - static _iq P_level2 = _IQ(160.0/60.0/NORMA_FROTOR); - static _iq P_level3 = _IQ(170.0/60.0/NORMA_FROTOR); - static _iq P_level4 = _IQ(180.0/60.0/NORMA_FROTOR); - static _iq P_level5 = _IQ(190.0/60.0/NORMA_FROTOR); - static _iq P_level6 = _IQ(200.0/60.0/NORMA_FROTOR); - static _iq P_level7 = _IQ(210.0/60.0/NORMA_FROTOR); - static _iq P_level8 = _IQ(220.0/60.0/NORMA_FROTOR); - static _iq P_level9 = _IQ(230.0/60.0/NORMA_FROTOR); -// static _iq P_level9 = _IQ(300.0/60.0/NORMA_FROTOR); - - static _iq kP_level0 = _IQ(0.9); - static _iq kP_level1 = _IQ(0.9); - static _iq kP_level2 = _IQ(0.9); - static _iq kP_level3 = _IQ(0.85); - static _iq kP_level4 = _IQ(0.8); - static _iq kP_level5 = _IQ(0.75); - static _iq kP_level6 = _IQ(0.7); - static _iq kP_level7 = _IQ(0.65); - static _iq kP_level8 = _IQ(0.6); - static _iq kP_level9 = _IQ(0.55); - - static _iq pid_kp_power = _IQ(PID_KP_POWER); - static _iq add_mzz_outmax_pidp = _IQ(100.0/NORMA_MZZ); - _iq new_pidP_OutMax = 0; - - _iq k_ogr_p_koef_1 = 0; - _iq k_ogr_p_koef_2 = 0; - - _iq k_ogr_n = 0; - - - _iq Frot_pid_abs; - - _iq d_m=0; - - _iq iq_decr_mzz_power; - - _iq level1_power_ain_decr_mzz, level2_power_ain_decr_mzz; - _iq new_power_limit = 0; - - static _iq koef_Power_filter2 = _IQ(1.0/(FREQ_PWM*EXP_FILTER_KOEF_OGRAN_POWER_LIMIT));//2.2 //30000;// 0,0012//16777;//0,001//13981; - - - - - - Frot_pid_abs = _IQabs(Frot_pid); - -// pidPower.Kp -// -// if (Frot_pid_abs>=P_level0) -// { -// k_ogr_p_koef_1 = CONST_IQ_1 - _IQdiv( (Frot_pid_abs-P_level0) , (P_level9-P_level0) ); -// if (k_ogr_p_koef_1<0) k_ogr_p_koef_1 = 0; -// } -// else -// k_ogr_p_koef_1 = CONST_IQ_1; -// -// //k_ogr_p_koef_1 1 0 -// -// k_ogr_p_koef_2 = CONST_IQ_01 + _IQmpy(CONST_IQ_09, k_ogr_p_koef_1); - -// simple_scalar1.pidPower.Kp = _IQmpy (pid_kp_power, k_ogr_p_koef_2);// _IQ(PID_KP_POWER) - - simple_scalar1.pidPower.Kp = pid_kp_power;//_IQmpy (pid_kp_power, k_ogr_p_koef_2);// _IQ(PID_KP_POWER) - - if (mode_oborots_power == ALG_MODE_SCALAR_OBOROTS) - { - if (simple_scalar1.cmd_new_calc_p_limit) - { - simple_scalar1.flag_decr_mzz_power = 0; - simple_scalar1.iq_decr_mzz_power_filter = CONST_IQ_1; - simple_scalar1.iq_decr_mzz_power = CONST_IQ_1; - new_power_limit = power_limit; - } - else - { - // , - // . - // simple_scalar1.iq_decr_mzz_power_filter 1.0 - , - // 1-MAX_KOEF_OGRAN_POWER_LIMIT - - new_power_limit = power_limit - simple_scalar1.sdvig_power_limit; - if (new_power_limit=(level2_power_ain_decr_mzz-level1_power_ain_decr_mzz)) - d_m = CONST_IQ_1; - else - d_m = _IQdiv(d_m,(level2_power_ain_decr_mzz - level1_power_ain_decr_mzz)); - } - - if (d_m<0) - d_m=0; // - - if (d_m>CONST_IQ_1) - d_m=CONST_IQ_1; // - - // 1.0 0.0 MAX_KOEF_OGRAN_POWER_LIMIT 0.0 - d_m = _IQmpy(d_m, MAX_KOEF_OGRAN_POWER_LIMIT); // - - simple_scalar1.iq_decr_mzz_power = CONST_IQ_1 - d_m;// 1.0 - . MAX_KOEF_OGRAN_POWER_LIMIT . . - - if (simple_scalar1.iq_decr_mzz_power<0) - simple_scalar1.iq_decr_mzz_power=0; - - - simple_scalar1.iq_decr_mzz_power_filter = exp_regul_iq(koef_Power_filter2, - simple_scalar1.iq_decr_mzz_power_filter, - simple_scalar1.iq_decr_mzz_power); - - if (simple_scalar1.iq_decr_mzz_power_filter<0) - simple_scalar1.iq_decr_mzz_power_filter = 0; - - - if (d_m>0) - simple_scalar1.flag_decr_mzz_power = 1; - else - simple_scalar1.flag_decr_mzz_power=0; - - } - } - else - { - simple_scalar1.flag_decr_mzz_power = 0; - simple_scalar1.iq_decr_mzz_power_filter = CONST_IQ_1; - simple_scalar1.iq_decr_mzz_power = CONST_IQ_1; - new_power_limit = power_limit; - } - -#if (ENABLE_DECR_MZZ_POWER_IZAD) - if (simple_scalar1.disable_KoefOgranIzad==0) - simple_scalar1.iqKoefOgranIzad = _IQmpy(iqKoefOgran,simple_scalar1.iq_decr_mzz_power_filter); - else - simple_scalar1.iqKoefOgranIzad = iqKoefOgran; -#else - simple_scalar1.iqKoefOgranIzad = iqKoefOgran; -#endif - //static _iq _iq_1 = _IQ(1.0); - - // static _iq mzz_int_level1_on_F=0; - - -// mzz = _IQsat(mzz,mzz_zad_int,0); - - - simple_scalar1.mzz_zad_in1 = mzz_zad; - simple_scalar1.Izad_from_master = Izad_from_master; - - iqKoefOgran = _IQsat(iqKoefOgran,CONST_IQ_1,0); - - /* y y */ - if ( (Frot==0) && (fzad==0) ) - { - mzzi = 0; - fzad_int = 0; - powerzad_int = 0; - bpsi_filter = 0; - pidFOutMax = pidFOutMin = 0; - n_iq_koef_im_on_tormog = CONST_IQ_1;//_IQ(1.0); - simple_scalar1.iq_decr_mzz_power_filter = CONST_IQ_1; - - } - - if (mzz_zad==0) - { - bpsi_filter = 0; - mzz=0; - I1_i=0; - mzzi=0; - mzz_zad_int = 0; - fzad_int = 0; - powerzad_int = 0; - - simple_scalar1.pidIm1.Up1 = 0; - simple_scalar1.pidIm1.Ui = 0; - - simple_scalar1.pidF.Up1 = 0; - simple_scalar1.pidF.Ui = 0; - - simple_scalar1.pidPower.Up1 = 0; - simple_scalar1.pidPower.Ui = 0; - - Uze_t1=0; - Uze_t2=0; - - dI1_prev = 0; - Uz_t1_prev = 0; - - dF_prev = 0; - mzz_prev = 0; - - - // fzad - fzad_add = _IQ(FZAD_ADD_MAX/NORMA_FROTOR); - // fzad - // fzad_dec = _IQ(FZAD_DEC/NORMA_FROTOR); -// -// - // mzz_max - // iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ); - - - // . Uz_t_filter - // koef_Uz_t_filter = _IQ(0.001/0.5); //0.0333 - - - - // . mzz - // koef_bpsi = _IQ((0.6/NORMA_WROTOR)/(200.0/NORMA_MZZ)); - - flag_uz_t1=0; - - - // . . - // correct_err = _IQ(2.5/NORMA_WROTOR); - - // . . . - // iq_dF_min1 = _IQ(1.0/NORMA_WROTOR); - - // iq_dF_min2 = _IQ(1.5/NORMA_WROTOR); - - // . Km - // iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek 5% - -// iq_spad_k = _IQ(0.9965); //0.993 ~ 0.4 sek 5% - - -// dF_PREDEL_LEVEL1 = _IQ(0.5/NORMA_WROTOR); -// dF_PREDEL_LEVEL2 = _IQ(1.5/NORMA_WROTOR); - - // mzz_int_level1_on_F = _IQ(1.0/NORMA_WROTOR); -// mzz_int_level2_on_F = _IQ(1.5/NORMA_WROTOR); - - - } - - - // - if (direction==0) - { - // - n_iq_koef_im_on_tormog = CONST_IQ_1;//_IQ(1.0); - } - else - if (direction==-1 && fzad <= 0) - { -// , - if (Frot_pid<-F_revers_level13) - n_iq_koef_im_on_tormog = kF_revers_level13; - else - if (Frot_pid<-F_revers_level12) - n_iq_koef_im_on_tormog = kF_revers_level12; - else - if (Frot_pid<-F_revers_level11) - n_iq_koef_im_on_tormog = kF_revers_level11; - else - if (Frot_pid<-F_revers_level10) - n_iq_koef_im_on_tormog = kF_revers_level10; - else - if (Frot_pid<-F_revers_level9) - n_iq_koef_im_on_tormog = kF_revers_level9; - else - if (Frot_pid<-F_revers_level8) - n_iq_koef_im_on_tormog = kF_revers_level8; - else - if (Frot_pid<-F_revers_level7) - n_iq_koef_im_on_tormog = kF_revers_level7; - else - if (Frot_pid<-F_revers_level6) - n_iq_koef_im_on_tormog = kF_revers_level6; - else - if (Frot_pid<-F_revers_level5) - n_iq_koef_im_on_tormog = kF_revers_level5; - else - if (Frot_pid<-F_revers_level4) - n_iq_koef_im_on_tormog = kF_revers_level4; - else - if (Frot_pid<-F_revers_level3) - n_iq_koef_im_on_tormog = kF_revers_level3; - else - if (Frot_pid<-F_revers_level2) - n_iq_koef_im_on_tormog = kF_revers_level2; - else - if (Frot_pid<-F_revers_level1) - n_iq_koef_im_on_tormog = kF_revers_level1; - if (Frot_pid<-F_revers_level0) - n_iq_koef_im_on_tormog = kF_revers_level0; - else - n_iq_koef_im_on_tormog = kF_revers_level00; - - } - else - if (direction==1 && fzad>=0) - { - // , - n_iq_koef_im_on_tormog = CONST_IQ_1;//_IQ(1.0); - } - else - { - // _iq_koef_im_on_tormog -// mzz_zad = _IQmpy(mzz_zad, _iq_koef_im_on_tormog); - - if (edrk.warnings.e9.bits.BREAK_TEMPER_ALARM == 1) - // , - n_iq_koef_im_on_tormog = _iq_koef_im_on_tormog_max_temper_break; - else - n_iq_koef_im_on_tormog = _iq_koef_im_on_tormog; - } - - t_iq_koef_im_on_tormog = zad_intensiv_q(_iq_koef_im_on_tormog_add, - _iq_koef_im_on_tormog_dec, - t_iq_koef_im_on_tormog, - n_iq_koef_im_on_tormog); - - - mzz_zad = _IQmpy(mzz_zad, t_iq_koef_im_on_tormog); - - simple_scalar1.mzz_zad_in2 = mzz_zad; - - /* */ - if (n_alg==1) - { - - mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad); - -// if (Frot_pid>mzz_int_level1_on_F) -// mzz_zad_int = zad_intensiv_q(mzz_add_1, mzz_add_1, mzz_zad_int, mzz_zad); -// else -// mzz_zad_int = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz_zad_int, mzz_zad); - - } - - - if (n_alg==2) - mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad); - -// myq_temp = _IQdiv(mzz_zad, simple_scalar1.iq_mzz_max_for_fzad); -// myq_temp = _IQmpy( myq_temp, fzad_add_max); -// fzad_add = myq_temp; - - fzad_int = zad_intensiv_q(fzad_add, fzad_add, fzad_int, fzad ); - - - - - powerzad_int = zad_intensiv_q(simple_scalar1.powerzad_add, simple_scalar1.powerzad_add, powerzad_int, powerzad); - - if (n_alg==1) - { - /* y */ - if (mzz_zad_int>=0) - { - dF = fzad_int - Frot_pid;//*direction; - -////////// Power PI ////////////// - - - //if (_IQabs(simple_scalar1.pidF.Out)) - - k_ogr_n = (_IQabs(power_pid) - _IQabs(powerzad_int)); - // if (k_ogr_n<0) k_ogr_n = 0; - - k_ogr_n = CONST_IQ_1 - _IQdiv(k_ogr_n, _IQabs(powerzad_int)); - - simple_scalar1.k_ogr_n = _IQsat(k_ogr_n,CONST_IQ_1,-CONST_IQ_1); - - - // pidP OutMax - new_pidP_OutMax = _IQabs(simple_scalar1.pidF.Out)+add_mzz_outmax_pidp; - new_pidP_OutMax = _IQsat(new_pidP_OutMax, mzz_zad_int, add_mzz_outmax_pidp ); // 100 simple_scalar1.pidF.Out - - // -// new_pidP_OutMax = mzz_zad_int; - - simple_scalar1.pidPower.OutMax = new_pidP_OutMax; - simple_scalar1.pidPower.OutMin = 0; - - -// pidPower.Kp = _IQmpy( _IQdiv(iq_add_kp_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Kp); -// pidPower.Ki = _IQmpy( _IQdiv(iq_add_ki_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Ki); - -// simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid); - - - simple_scalar1.pidPower.Ref = _IQabs(powerzad_int); // - - simple_scalar1.pidPower.Fdb = _IQabs(power_pid); - simple_scalar1.pidPower.calc(&simple_scalar1.pidPower); - - - // Saturate the integral output - - if (simple_scalar1.pidPower.Ui > simple_scalar1.pidPower.OutMax) - simple_scalar1.pidPower.Ui = simple_scalar1.pidPower.OutMax; - else if (simple_scalar1.pidPower.Ui < simple_scalar1.pidPower.OutMin) - simple_scalar1.pidPower.Ui = simple_scalar1.pidPower.OutMin; - - -////////////////////////////// -////////////////////////////// - - - // . - // pidF.OutMax=mzz_zad_int; - // - - pidFOutMax = zad_intensiv_q(simple_scalar1.mzz_add_3, simple_scalar1.mzz_add_1, pidFOutMax, simple_scalar1.pidPower.Out); - pidFOutMin = zad_intensiv_q(simple_scalar1.mzz_add_3, simple_scalar1.mzz_add_1, pidFOutMin, simple_scalar1.pidPower.Out); - - -// fzad - if (direction==-1 && fzad <= 0) - { - pidFOutMax = 0; - simple_scalar1.pidF.OutMax = 0;//simple_scalar1.pidPower.Out; - simple_scalar1.pidF.OutMin = -pidFOutMin;//-simple_scalar1.pidPower.Out; - - } - else - if (direction==1 && fzad>=0) - { - pidFOutMin = 0; - simple_scalar1.pidF.OutMax = pidFOutMax;//simple_scalar1.pidPower.Out; - simple_scalar1.pidF.OutMin = 0;//-simple_scalar1.pidPower.Out; - } - else - { - simple_scalar1.pidF.OutMax = pidFOutMax;//simple_scalar1.pidPower.Out; - simple_scalar1.pidF.OutMin = -pidFOutMin;//-simple_scalar1.pidPower.Out; - } - -/* -// pzad - if (direction==-1 && powerzad <= 0) - { - - - } - else - if (direction==1 && powerzad>=0) - { - - } - else - { - - } -*/ - -// pidF.OutMax = mzz_zad; - if (count_bs_work==2) - simple_scalar1.pidF.Kp = simple_scalar1.pidF_Kp;//_IQmpy( _IQdiv(simple_scalar1.iq_add_kp_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Kp); - else - simple_scalar1.pidF.Kp = _IQmpy2(simple_scalar1.pidF_Kp); - - simple_scalar1.pidF.Ki = simple_scalar1.pidF_Ki;//_IQmpy( _IQdiv(simple_scalar1.iq_add_ki_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Ki); - - simple_scalar1.pidF.Ki = _IQmpy(simple_scalar1.pidF.Ki,simple_scalar1.k_freq_for_pid); - -///////////////////////// - -// if (_IQabs(dF)iq_dF_min2) -// { -// m.m1.bit.w_rotor_ust = 0; -// } -////////////////////////////////// - - // dF - //fzad_int = - simple_scalar1.pidF.Ref = _IQmpy(fzad_int, iqKoefOgran); - - simple_scalar1.pidF.Fdb = Frot_pid;//*direction; - simple_scalar1.pidF.calc(&simple_scalar1.pidF); - - - // Saturate the integral output - - if (simple_scalar1.pidF.Ui > simple_scalar1.pidF.OutMax) - simple_scalar1.pidF.Ui = simple_scalar1.pidF.OutMax; - else if (simple_scalar1.pidF.Ui < simple_scalar1.pidF.OutMin) - simple_scalar1.pidF.Ui = simple_scalar1.pidF.OutMin; -///////////////////////////////////// - - mzz = _IQabs(simple_scalar1.pidF.Out); // !!! - -/////////////////////////////////////// - - - -// -// mzz = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz, pidF.Out); - -// mzzi = zad_intensiv_q(mzz_add_2, mzz_add_2, mzzi, mzz); - - // mzz - mzz = _IQsat(mzz,mzz_zad_int,0); - - } - else - { - mzz = 0; - } - - } - - if (n_alg==2) - { - mzz = mzz_zad_int; - } - - - if (master == MODE_SLAVE) - { - mzz = Izad_from_master; - // mzz - mzz = _IQsat(mzz,mzz_zad_int,0); - } - - - *Izad_out = mzz; - /* y I_PREDEL_LEVEL1 - y */ - -/* - pred_Ip = (filter.I_3+filter.I_6)-I_PREDEL_LEVEL1; - - - if (pred_Ip<0) - Kpred_Ip=0.0; // - else - { - // - if (pred_Ip>=(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1)) - Kpred_Ip=1; - else - Kpred_Ip = pred_Ip/(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1); - - } - - // - Izad = mzz * (1-Kpred_Ip); - */ - - - - Izad = _IQmpy(mzz, simple_scalar1.iqKoefOgranIzad); - -// if ((n_alg==1) || (n_alg==2)) -// { -// -// Im1 = iqIm_1; -// Im2 = iqIm_2; -// -// if (n_wind_pump==0) // -// { -// -// halfIm1 = Im1 >> 1; -// halfIm2 = Im2 >> 1; -// -// if (Im1>halfIm2) //if (Im1>IQdiv(Im2,_IQ(2.0))) -// { -// Im_regul=Im1; -// simple_scalar1.UpravIm1=1; -// simple_scalar1.UpravIm2=0; -// } -// else -// { -// if (Im2>halfIm1) -// { -// Im_regul=Im2; -// simple_scalar1.UpravIm2=1; -// simple_scalar1.UpravIm1=0; -// } -// else -// { -// Im_regul=Im1; //Im1 -// simple_scalar1.UpravIm1=1;//1 -// simple_scalar1.UpravIm2=0;//0 -// } -// } -// } -// -// if (n_wind_pump==1) // 1 2- -// { -// Im_regul=Im2; -// simple_scalar1.UpravIm1=0; -// simple_scalar1.UpravIm2=1; -// } -// -// if (n_wind_pump==2) // 2 1- -// { -// Im_regul=Im1; -// simple_scalar1.UpravIm1=1; -// simple_scalar1.UpravIm2=0; -// } - - Im_regul = iqIm; - - simple_scalar1.Im_regul = Im_regul; - simple_scalar1.Izad = Izad; - - dI1 = (Izad - Im_regul ); - - simple_scalar1.pidIm1.Ki = simple_scalar1.pidIm_Ki; - simple_scalar1.pidIm1.Ki = _IQmpy(simple_scalar1.pidIm1.Ki,simple_scalar1.k_freq_for_pid); - - - simple_scalar1.pidIm1.Ref = _IQdiv(Izad,iqUin); - simple_scalar1.pidIm1.Fdb = _IQdiv(Im_regul,iqUin); - simple_scalar1.pidIm1.calc(&simple_scalar1.pidIm1); - - - Uz_t1 = simple_scalar1.pidIm1.Out; - - // Km - if (Uz_t1Uz_t1) Uze_t1 = Uze_ogr; - else - Uze_t1 = Uz_t1; - } - else - { - Uze_t1 = Uz_t1; - } - - Uze_t1 = _IQsat(Uze_t1,simple_scalar1.pidIm1.OutMax, simple_scalar1.pidIm1.OutMin); - -// } - - - /* */ - *Uz1 = Uze_t1; - *Uz2 = Uze_t1; - - - bpsi = bpsi_const + simple_scalar1.add_bpsi; - - // . ~ -// bpsi = _IQmpy(koef_bpsi,mzz); - - - bpsi = _IQsat(bpsi,simple_scalar1.max_bpsi, simple_scalar1.min_bpsi); - -#ifdef BAN_ROTOR_REVERS_DIRECT -// - if (analog.filter_direct_rotor==-1) - // , - *Fz = bpsi; - else - // , - *Fz = _IQmpy(Frot,IQ_POLUS) + bpsi; - -#else - - if (simple_scalar1.pidF.Out < 0) - { - bpsi_filter = exp_regul_iq(koefBpsi, bpsi_filter, -bpsi); - } - else - if (simple_scalar1.pidF.Out > 0) - { - bpsi_filter = exp_regul_iq(koefBpsi, bpsi_filter, bpsi); - } - else - bpsi_filter = exp_regul_iq(koefBpsi, bpsi_filter, 0); - - -// *Fz = _IQmpy(Frot*direction,simple_scalar1.poluses) + bpsi_filter; - *Fz = _IQmpy(Frot, simple_scalar1.poluses) + bpsi_filter; - - - simple_scalar1.bpsi_curent = bpsi_filter; - -#endif - - - simple_scalar1.mzz_zad_int = mzz_zad_int; - simple_scalar1.Uze_t1 = Uze_t1; - simple_scalar1.iqKoefOgran = iqKoefOgran; - simple_scalar1.Fz = *Fz; - simple_scalar1.direction = direction; - simple_scalar1.fzad_int = fzad_int; - - - -// if (n_alg==2) -// { -// -// *Fz = fzad_provorot; -// /* bpsi - , -// y y */ -// } - - -} - - - diff --git a/Inu/Src2/551/main/alg_simple_scalar.h b/Inu/Src2/551/main/alg_simple_scalar.h deleted file mode 100644 index f05ca38..0000000 --- a/Inu/Src2/551/main/alg_simple_scalar.h +++ /dev/null @@ -1,101 +0,0 @@ -/* - * alg_simple_scalar.h - * - * Created on: 26 . 2020 . - * Author: Vladislav - */ - -#ifndef SRC_MAIN_ALG_SIMPLE_SCALAR_H_ -#define SRC_MAIN_ALG_SIMPLE_SCALAR_H_ - -#include "IQmathLib.h" -#include "pid_reg3.h" - -typedef struct { PIDREG3 pidIm1; - PIDREG3 pidF; - PIDREG3 pidPower; - - _iq mzz_add_1; - _iq mzz_add_2; - _iq poluses; - _iq fzad_add_max; - _iq fzad_dec; - - _iq powerzad_add; - _iq powerzad_dec; - _iq min_bpsi; - _iq koef_Uz_t_filter; - _iq iq_spad_k; - - _iq iq_mzz_max_for_fzad; - _iq k_freq_for_pid; - _iq iq_add_kp_df; - _iq iq_add_ki_df; - _iq min_mzz_for_df; - - _iq pidF_Kp; - _iq pidF_Ki; - int UpravIm1; - int UpravIm2; - _iq pidIm_Ki; - - _iq mzz_zad_in1; - _iq mzz_zad_in2; - - _iq mzz_zad_int; - _iq Im_regul; - _iq Izad; - _iq Izad_from_master; - _iq bpsi_curent; - _iq Uze_t1; - - _iq iqKoefOgran; - _iq Fz; - int direction; - _iq fzad_int; - - _iq add_bpsi; - _iq max_bpsi; - - _iq mzz_add_3; - - _iq k_ogr_n; - _iq iq_decr_mzz_power; - _iq iq_decr_mzz_power_filter; - int flag_decr_mzz_power; - - _iq iqKoefOgranIzad; - int disable_KoefOgranIzad; - _iq add_power_limit; - _iq sdvig_power_limit; - - int cmd_new_calc_p_limit; - - -} ALG_SIMPLE_SCALAR; - -#define ALG_SIMPLE_SCALAR_DEFAULT {PIDREG3_DEFAULTS,PIDREG3_DEFAULTS,PIDREG3_DEFAULTS,\ - 0,0,0,0,0,\ - 0,0,0,0,0,\ - 0,0,0,0,0,\ - 0,0,0,0,0,\ - 0,0,0,0,0,0,0, \ - 0,0, 0,0,0, 0,0, 0, 0, 0,0,0,0,0,0, 0,0 \ -} - - -extern ALG_SIMPLE_SCALAR simple_scalar1; - - - -void simple_scalar(int n_alg, int n_wind_pump, int direction, - _iq Frot_pid, _iq Frot,_iq fzad,_iq mzz_zad, _iq bpsi_const, - _iq iqKoefOgran, - _iq iqIm, _iq iqUin, _iq Iin, _iq powerzad, _iq power_pid, - _iq power_limit, int mode_oborots_power, - _iq Izad_from_master, int master, int count_bs_work, - _iq *Fz, _iq *Uz1, _iq *Uz2, _iq *Izad); - -void init_simple_scalar(void); - -#endif /* SRC_MAIN_ALG_SIMPLE_SCALAR_H_ */ diff --git a/Inu/Src2/551/main/alg_uf_const.c b/Inu/Src2/551/main/alg_uf_const.c deleted file mode 100644 index 3e5b8fa..0000000 --- a/Inu/Src2/551/main/alg_uf_const.c +++ /dev/null @@ -1,80 +0,0 @@ -/* - * alg_uf_const.c - * - * Created on: 26 . 2020 . - * Author: Yura - */ -#include -#include -#include -#include -#include - -#include "mathlib.h" - - - - -#define T_NARAST 15 // . - -//VHZPROF vhz1 = VHZPROF_DEFAULTS; - - -ALG_UF_CONST uf_const1 = ALG_UF_CONST_DEFAULT; - -void init_uf_const(void) -{ - uf_const1.freq = 0; - uf_const1.km = 0; - - uf_const1.zad_plus_km = _IQ(1.0/(FREQ_PWM*T_NARAST)); - - - uf_const1.rmp_freq.RampLowLimit = _IQ(-4); //0 - uf_const1.rmp_freq.RampHighLimit = _IQ(4); - - uf_const1.rmp_freq.RampPlus = _IQ(0.0002); - - uf_const1.rmp_freq.RampMinus = _IQ(-0.0002); - uf_const1.rmp_freq.DesiredInput = 0; - uf_const1.rmp_freq.Out = 0; - - uf_const1.max_km = _IQ(K_STATOR_MAX); - -} - - -void uf_const(_iq *Fz, _iq *Uz1, _iq *Uz2) -{ - - -// vhz1.HighFreq = _IQ(f.fmax_uf/F_STATOR_MAX); -///////////// - - uf_const1.km = edrk.zadanie.iq_kzad_rmp; -// uf_const1.km = zad_intensiv_q(uf_const1.zad_plus_km, uf_const1.zad_plus_km, uf_const1.km, edrk.zadanie.iq_kzad); -// uf_const1.km = _IQsat(uf_const1.km,uf_const1.max_km,0); - *Uz1 = uf_const1.km; - *Uz2 = uf_const1.km; - - -///////////////// - uf_const1.freq = edrk.zadanie.iq_fzad_rmp; - -// uf_const1.rmp_freq.DesiredInput = uf_const1.freq; -// uf_const1.rmp_freq.calc(&uf_const1.rmp_freq); - *Fz = uf_const1.freq; - -/* - vhz1.Freq = Fzad; - vhz1.calc(&vhz1); - - - - *Fz = rmp_freq.Out; - */ - - -} - - diff --git a/Inu/Src2/551/main/alg_uf_const.h b/Inu/Src2/551/main/alg_uf_const.h deleted file mode 100644 index 50276f0..0000000 --- a/Inu/Src2/551/main/alg_uf_const.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * alg_uf_const.h - * - * Created on: 26 . 2020 . - * Author: Yura - */ - -#ifndef SRC_MAIN_ALG_UF_CONST_H_ -#define SRC_MAIN_ALG_UF_CONST_H_ - - -#include "IQmathLib.h" -#include "rmp_cntl_v1.h" - - -typedef struct { - RMP_V1 rmp_freq; - _iq freq; - _iq km; - _iq zad_plus_km; - _iq max_km; -} ALG_UF_CONST; - -extern ALG_UF_CONST uf_const1; - -#define ALG_UF_CONST_DEFAULT {RMP_V1_DEFAULTS, 0, 0, 0, 0 } - - - - -void init_uf_const(void); -void uf_const(_iq *Fz, _iq *Uz1, _iq *Uz2); - -#endif /* SRC_MAIN_ALG_UF_CONST_H_ */ diff --git a/Inu/Src2/551/main/another_bs.c b/Inu/Src2/551/main/another_bs.c deleted file mode 100644 index 542df74..0000000 --- a/Inu/Src2/551/main/another_bs.c +++ /dev/null @@ -1,458 +0,0 @@ -/* - * another_bs.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#include - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include -#include "adc_tools.h" -#include "CAN_project.h" -#include "CAN_Setup.h" -#include "global_time.h" -#include "v_rotor.h" -#include "ukss_tools.h" -#include "another_bs.h" -#include "control_station_project.h" -#include "control_station.h" -#include "can_bs2bs.h" -#include "sync_tools.h" -#include "vector_control.h" -#include "master_slave.h" -#include "digital_filters.h" - - -////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////// -void read_data_from_bs(void) -{ - int g; - - if (control_station.alive_control_station[CONTROL_STATION_ANOTHER_BS]) - { - g = Unites[ANOTHER_BSU1_CAN_DEVICE][5]; - edrk.int_koef_ogran_power_another_bs = g; - edrk.iq_koef_ogran_power_another_bs = ((float)edrk.int_koef_ogran_power_another_bs); - - g = Unites[ANOTHER_BSU1_CAN_DEVICE][6]; - edrk.power_kw_another_bs = g; - edrk.iq_power_kw_another_bs = _IQ(((float)edrk.power_kw_another_bs * 1000.0)/NORMA_ACP/NORMA_ACP); - - g = Unites[ANOTHER_BSU1_CAN_DEVICE][7]; - edrk.active_post_upravl_another_bs = g; - - g = Unites[ANOTHER_BSU1_CAN_DEVICE][9]; - edrk.Ready1_another_bs = g; - - g = Unites[ANOTHER_BSU1_CAN_DEVICE][10]; - edrk.Ready2_another_bs = g; - - g = Unites[ANOTHER_BSU1_CAN_DEVICE][11]; - edrk.MasterSlave_another_bs = g; - - g = Unites[ANOTHER_BSU1_CAN_DEVICE][3] & 0x1; - edrk.ump_cmd_another_bs = g; - - g = (Unites[ANOTHER_BSU1_CAN_DEVICE][3] & 0x2) >> 1; - edrk.qtv_cmd_another_bs = g; - - g = Unites[ANOTHER_BSU1_CAN_DEVICE][13]; - edrk.errors_another_bs_from_can = g; - } - else - { - edrk.errors_another_bs_from_can = 0; - } - - -} - - -////////////////////////////////////////////////////////// -unsigned int read_cmd_sbor_from_bs(void) -{ - unsigned int g; - g = Unites[ANOTHER_BSU1_CAN_DEVICE][4]; - return g; - - -} - - - - -void UpdateTableSecondBS(void) -{ - int cmd; - int i,k; - - static unsigned int counter_sum_errors = 0; - - Unites2SecondBS[0]++; - - Unites2SecondBS[1] = global_time.miliseconds; - Unites2SecondBS[2] = edrk.flag_second_PCH; - Unites2SecondBS[3] = (edrk.to_shema.bits.QTV_ON_OFF << 1) | (edrk.to_shema.bits.UMP_ON_OFF); - Unites2SecondBS[4] = edrk.SumSbor; - Unites2SecondBS[5] = edrk.int_koef_ogran_power; - Unites2SecondBS[6] = (int)edrk.power_kw; - Unites2SecondBS[7] = (int)edrk.active_post_upravl; - Unites2SecondBS[8] = (int)edrk.power_kw; - Unites2SecondBS[9] = edrk.Status_Ready.bits.ready1; - Unites2SecondBS[10] = edrk.Status_Ready.bits.ready_final; - Unites2SecondBS[11] = edrk.MasterSlave; - - Unites2SecondBS[12] = _IQtoF(vect_control.iqId_min) * NORMA_ACP; - - Unites2SecondBS[13] = pause_detect_error(&counter_sum_errors, 10, edrk.summ_errors); - - - for (i=0;i -#include -#include -#include -#include - -#include "IQmathLib.h" -#include "pid_reg3.h" -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File - - -#pragma DATA_SECTION(break_result_1,".fast_vars"); -_iq break_result_1 = 0; - -#pragma DATA_SECTION(break_result_2,".fast_vars"); -_iq break_result_2 = 0; - -#pragma DATA_SECTION(break_result_3,".fast_vars"); -_iq break_result_3 = 0; - -#pragma DATA_SECTION(break_result_4,".fast_vars"); -_iq break_result_4 = 0; - -#pragma DATA_SECTION(koef_Krecup,".fast_vars"); -_iq koef_Krecup = 0; - -#pragma DATA_SECTION(koef_Min_recup,".fast_vars"); -_iq koef_Min_recup = 0; - -BREAK_REGUL break_reg = BREAK_REGUL_DEFAULTS; - -_iq calc_recup(_iq voltage, _iq recup_level, _iq max_recup_level); -//#pragma DATA_SECTION(pidF,".fast_vars"); -//PIDREG3 pidF=PIDREG3_DEFAULTS; - -void break_resistor_managment_init(unsigned int freq_pwm) -{ - volatile float percent;//, percent2; - _iq iq_percent1 = 0, iq_percent2 = 0;// iq_percent3 = 0, iq_percent4 = 0; - - break_reg.pwm_tics = (FREQ_INTERNAL_GENERATOR_XILINX_TMS / freq_pwm ); - break_reg.pwm_minimal_tics = (float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0; - break_reg.pwm_max_tics = break_reg.pwm_tics - break_reg.pwm_minimal_tics; - - - // - . IQ15. - percent = (break_reg.pwm_max_tics * RECUP_PERCENT_MIN/100.0) * 32768.0;// _IQ15 - iq_percent1 = percent; - - // - . IQ15. - percent = (break_reg.pwm_max_tics * RECUP_PERCENT_MAX/100.0) * 32768.0; - iq_percent2 = percent; - - // - - koef_Krecup = _IQdiv((iq_percent1 - iq_percent2),(IQ_RECUP_LEVEL_MIN - IQ_RECUP_LEVEL_MAX)); - koef_Min_recup = iq_percent1; // -} - - -//int break_resistor_managment_calc(int flag_on_off) -#pragma CODE_SECTION(break_resistor_managment_calc,".fast_run"); -void break_resistor_managment_calc() -{ - static unsigned int break_counter = 0;//MAX_BREAK_IMPULSE + 1; - - if(edrk.Discharge && edrk.from_shema_filter.bits.QTV_ON_OFF==0 - && edrk.from_shema_filter.bits.UMP_ON_OFF==0)// && edrk.to_shema.bits.QTV_ON == 0) - { - if (break_counter < MAX_BREAK_IMPULSE) - { - break_counter++; - - - if ((filter.iqU_1_fast > BREAK_INSENSITIVE_LEVEL_MIN) - || edrk.ManualDischarge || edrk.NoDetectUZeroDischarge) - { - if (edrk.Obmotka1 == 0) - break_result_1 = 300; - else - break_result_1 = 0; - } - else - { - break_result_1 = 0; - } - - if ((filter.iqU_2_fast > BREAK_INSENSITIVE_LEVEL_MIN) - || edrk.ManualDischarge || edrk.NoDetectUZeroDischarge ) - { - if (edrk.Obmotka2 == 0) - break_result_2 = 300; - else - break_result_2 = 0; - } - else - { - break_result_2 = 0; - } - -// if (filter.iqU_3_fast < BREAK_INSENSITIVE_LEVEL_MIN) -// { -// break_result_3 = 0; -// } -// else -// { -// break_result_3 = 300; -//// break_result_3 = calc_stop_run_recup(filter.iqU_3_fast); -// } -// -// if (filter.iqU_4_fast < BREAK_INSENSITIVE_LEVEL_MIN) -// { -// break_result_4 = 0; -// } -// else -// { -// break_result_4 = 300; -//// break_result_4 = calc_stop_run_recup(filter.iqU_4_fast); -// } - - if((!break_result_1 && !break_result_2 && !break_result_3 && !break_result_4) - || (break_counter >= (MAX_BREAK_IMPULSE - 1))) - { - edrk.Discharge = 0; - break_counter = 0; - } - } - else - { - break_result_1 = 0; - break_result_2 = 0; - break_result_3 = 0; - break_result_4 = 0; - break_counter = 0; - edrk.Discharge = 0; - } - } - -} - -#pragma CODE_SECTION(calc_recup,".fast_run"); -_iq calc_recup(_iq voltage, _iq recup_level, _iq max_recup_level) -{ - if (voltage <= recup_level) - { - return 0; - } - else - { - voltage = _IQsat(voltage,max_recup_level,recup_level); - voltage = _IQmpy(koef_Krecup,voltage - recup_level) + koef_Min_recup; - return voltage >> 15; - } -} - -#pragma CODE_SECTION(break_resistor_recup_calc,".fast_run"); -void break_resistor_recup_calc(_iq Uzpt_nominal) -{ - _iq Uzpt_start_recup = Uzpt_nominal + IQ_DELTA_U_START_RECUP; - _iq Uzpt_max_open_break_keys = Uzpt_nominal + IQ_DELTA_U_MAX_OPEN_BREAK_KEYS; - - if (/*edrk.Go &&*/ (edrk.SborFinishOk || edrk.Status_Ready.bits.ImitationReady2) ) - { - break_result_1 = calc_recup(filter.iqU_1_fast, Uzpt_start_recup, Uzpt_max_open_break_keys); - break_result_2 = calc_recup(filter.iqU_2_fast, Uzpt_start_recup, Uzpt_max_open_break_keys); - } -} - -#pragma CODE_SECTION(break_resistor_managment_update,".fast_run"); -void break_resistor_managment_update() -{ - if (break_result_1 < break_reg.pwm_minimal_tics) - break_result_1 = 0; - - if (break_result_2 < break_reg.pwm_minimal_tics) - break_result_2 = 0; - -// if (break_result_3 < break_reg.pwm_minimal_tics) -// break_result_3 = 0; -// -// if (break_result_4 < break_reg.pwm_minimal_tics) -// break_result_4 = 0; - - if (break_result_1 > break_reg.pwm_max_tics) - break_result_1 = break_reg.pwm_max_tics; - - if (break_result_2 > break_reg.pwm_max_tics) - break_result_2 = break_reg.pwm_max_tics; - -// if (break_result_3 > break_reg.pwm_max_tics) -// break_result_3 = break_reg.pwm_max_tics; -// -// if (break_result_4 > break_reg.pwm_max_tics) -// break_result_4 = break_reg.pwm_max_tics; -} - -//#pragma CODE_SECTION(break_resistor_set_closed,".fast_run"); -void break_resistor_set_closed() -{ - break_result_1 = 0; - break_result_2 = 0; - break_result_3 = 0; - break_result_4 = 0; -} diff --git a/Inu/Src2/551/main/break_regul.h b/Inu/Src2/551/main/break_regul.h deleted file mode 100644 index 1e79cdd..0000000 --- a/Inu/Src2/551/main/break_regul.h +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef _BREAK_REGUL_H -#define _BREAK_REGUL_H - -#ifdef __cplusplus - extern "C" { -#endif - -typedef struct { - unsigned int pwm_tics; - unsigned int pwm_minimal_tics; - unsigned int pwm_max_tics; -} BREAK_REGUL; - -#define BREAK_REGUL_DEFAULTS {0,0,0} - - -#define BREAK_INSENSITIVE_LEVEL_MIN 279620 //50V -#define BREAK_INSENSITIVE_LEVEL_MAX 4194304//500V//5872025//700 V//2516582//300 V//4194304//500 V//2516582 // 838860 //100 V -#define PERCENT_BREAK_MAX 13421772//80 percent -#define PERCENT_BREAK_MIN 838860//5 percent -#define MAX_RECUP_LEVEL 5872025 //700 V -#define K_RECUP_LEVEL 56 //5.6 /// 838860 / 4697620 //700 V -#define MAX_BREAK_IMPULSE 9000 - -#define RECUP_PERCENT_MIN 10.0 -#define RECUP_PERCENT_MAX 90.0 - -//#define IQ_RECUP_LEVEL_MIN_ACCUM 16777216 //3000V -//#define IQ_RECUP_LEVEL_MAX_ACCUM 17895697 //3200V - -#define IQ_RECUP_LEVEL_MIN 15658734L //2800V //15099494L //2700V //9507089L //1700V //8947848L //1600V // -#define IQ_RECUP_LEVEL_MAX 16777216L //3000V //16777216L //16217975L //2900V //11184810L //2000V //17895697 //3200V 10066329 //1800V // - -#define IQ_DELTA_U_START_RECUP 559240 //100V //279620LL //50V -#define IQ_DELTA_U_MAX_OPEN_BREAK_KEYS 1677721LL//300 V - - -#define IQ_MIN_VOLTAGE_STOP_RUN 279620L //50V -#define IQ_MAX_VOLTAGE_STOP_RUN 16777216L //3000V - -#define BRK_STOP_RUN_PERCENT_MIN 5 -#define BRK_STOP_RUN_PERCENT_MAX 50 - -//int break_resistor_managment_calc(int flag_on_off); -void break_resistor_managment_calc(void); -void break_resistor_managment_init(unsigned int freq_pwm); -void break_resistor_managment_update(void); -void break_resistor_recup_calc(_iq Uzpt_nominal); -void break_resistor_set_closed(void); -_iq calc_stop_run_recup(_iq voltage); - - -//extern PIDREG3 break_struct_1; -//extern PIDREG3 break_struct_2; - - - - -extern _iq break_result_1; -extern _iq break_result_2; -extern _iq break_result_3; -extern _iq break_result_4; - -extern _iq kp_break; - -#ifdef __cplusplus - } -#endif - -#endif /* _BREAK_REGUL_H */ diff --git a/Inu/Src2/551/main/calc_rms_vals.c b/Inu/Src2/551/main/calc_rms_vals.c deleted file mode 100644 index 7363ce2..0000000 --- a/Inu/Src2/551/main/calc_rms_vals.c +++ /dev/null @@ -1,265 +0,0 @@ -/* - * calc_rms_vals.c - * - * Created on: 14 . 2020 . - * Author: star - */ -#include -#include -#include -#include -#include - -#include "IQmathLib.h" -#include "mathlib.h" - -#define NUM_OF_CHANNELS 6 - -//#define USE_CALC_U_IN_RMS 1 -//#define USE_CALC_I_OUT_RMS 1 - - -#pragma DATA_SECTION(in_U_rms_calc_buffer,".slow_vars") -RMS_BUFFER in_U_rms_calc_buffer[NUM_OF_CHANNELS] = {RMS_BUFFER_DEFAULTS, RMS_BUFFER_DEFAULTS, \ - RMS_BUFFER_DEFAULTS, RMS_BUFFER_DEFAULTS, \ - RMS_BUFFER_DEFAULTS, RMS_BUFFER_DEFAULTS}; - -#pragma DATA_SECTION(out_I_rms_calc_buffer,".slow_vars") -RMS_BUFFER_WITH_THINNING out_I_rms_calc_buffer[6] = {RMS_BUFFER_WITH_THINNING_DEFAULTS, RMS_BUFFER_WITH_THINNING_DEFAULTS, \ - RMS_BUFFER_WITH_THINNING_DEFAULTS, RMS_BUFFER_WITH_THINNING_DEFAULTS, \ - RMS_BUFFER_WITH_THINNING_DEFAULTS, RMS_BUFFER_WITH_THINNING_DEFAULTS}; - -/////////////////////////////////////////////////// -void init_Uin_rms(void) -{ - int k,l; - // - for (k=0;kvalues[b->position++] = val; - if (b->position >= b->array_size) { b->position = 0;} -} - -#define add_to_buff_def(b, val) { (b)->values[(b)->position++] = (val); if ((b)->position >= (b)->array_size) { (b)->position = 0;}} - -static void calc_Uin_rms(void); -static void calc_Iout_rms(void); - -#define CONST_PI 52707178 - -//#pragma CODE_SECTION(fill_rms_array_IQ15,".fast_run1"); -void fill_rms_array_IQ15(RMS_BUFFER_WITH_THINNING *v) { - static _iq norma_f_rot = NORMA_FROTOR; - int period_by_teta = 0; -// int freq = _IQtoF(v->signal_freq) * NORMA_FROTOR; -// int freq = _IQtoF(v->signal_freq * norma_f_rot); //Max IQ24 = 127. - int freq = (v->signal_freq * norma_f_rot) / (1LL << GLOBAL_Q); // 10 , - int count_miss_writing = 100; - if (freq != 0) { - freq *= v->elements_in_period; //"" - count_miss_writing = (int)(v->freq_pwm / freq); - if (count_miss_writing > 100) {count_miss_writing = 100;} - } - - if (v->use_teta) { - v->internal.teta_period_counter += 1; - if ((v->teta < CONST_PI && v->internal.teta_prev > CONST_PI) || - (v->teta > CONST_PI && v->internal.teta_prev < CONST_PI)) { - v->internal.zero_teta_period = v->internal.teta_period_counter; - v->internal.teta_period_counter = 0; - } - v->internal.teta_prev = v->teta; - period_by_teta = v->internal.zero_teta_period / v->elements_in_period; - if (period_by_teta != 0) { - count_miss_writing = (count_miss_writing + period_by_teta) / 2; - } - } - - - if (v->internal.miss_write_counter++ < count_miss_writing) { - return; - } - - v->internal.miss_write_counter = 0; - v->values[v->position++] = _IQtoIQ15(v->val); - if (v->position >= v->array_size) { - v->position = 0; - } -} - -void fill_RMS_buff_interrupt(_iq teta_ch1, _iq teta_ch2) { -#ifdef USE_CALC_U_IN_RMS -// add_to_buff(&in_U_rms_calc_buffer[0], analog.iqUin_A1B1); -// add_to_buff(&in_U_rms_calc_buffer[1], analog.iqUin_B1C1); -// add_to_buff(&in_U_rms_calc_buffer[2], analog.iqUin_C1A1); -// add_to_buff(&in_U_rms_calc_buffer[3], analog.iqUin_A2B2); -// add_to_buff(&in_U_rms_calc_buffer[4], analog.iqUin_B2C2); -// add_to_buff(&in_U_rms_calc_buffer[5], analog.iqUin_C2A2); -// - add_to_buff_def(&in_U_rms_calc_buffer[0], analog.iqUin_A1B1); //define 10 - add_to_buff_def(&in_U_rms_calc_buffer[1], analog.iqUin_B1C1); - add_to_buff_def(&in_U_rms_calc_buffer[2], analog.iqUin_C1A1); - add_to_buff_def(&in_U_rms_calc_buffer[3], analog.iqUin_A2B2); - add_to_buff_def(&in_U_rms_calc_buffer[4], analog.iqUin_B2C2); - add_to_buff_def(&in_U_rms_calc_buffer[5], analog.iqUin_C2A2); -#endif //USE_CALC_U_IN_RMS -#ifdef USE_CALC_I_OUT_RMS - out_I_rms_calc_buffer[0].val = analog.iqIu_1; - out_I_rms_calc_buffer[0].teta = teta_ch1; - out_I_rms_calc_buffer[0].freq_pwm = FREQ_PWM * 2; - out_I_rms_calc_buffer[0].signal_freq = edrk.f_stator; - out_I_rms_calc_buffer[0].add_value(&out_I_rms_calc_buffer[0]); - out_I_rms_calc_buffer[1].val = analog.iqIv_1; - out_I_rms_calc_buffer[1].teta = teta_ch1; - out_I_rms_calc_buffer[1].freq_pwm = FREQ_PWM * 2; - out_I_rms_calc_buffer[1].signal_freq = edrk.f_stator; - out_I_rms_calc_buffer[1].add_value(&out_I_rms_calc_buffer[1]); - out_I_rms_calc_buffer[2].val = analog.iqIw_1; - out_I_rms_calc_buffer[2].teta = teta_ch1; - out_I_rms_calc_buffer[2].freq_pwm = FREQ_PWM * 2; - out_I_rms_calc_buffer[2].signal_freq = edrk.f_stator; - out_I_rms_calc_buffer[2].add_value(&out_I_rms_calc_buffer[2]); - - out_I_rms_calc_buffer[3].val = analog.iqIu_1; - out_I_rms_calc_buffer[3].teta = teta_ch2; - out_I_rms_calc_buffer[3].freq_pwm = FREQ_PWM * 2; - out_I_rms_calc_buffer[3].signal_freq = edrk.f_stator; - out_I_rms_calc_buffer[3].add_value(&out_I_rms_calc_buffer[3]); - out_I_rms_calc_buffer[4].val = analog.iqIu_1; - out_I_rms_calc_buffer[4].teta = teta_ch2; - out_I_rms_calc_buffer[4].freq_pwm = FREQ_PWM * 2; - out_I_rms_calc_buffer[4].signal_freq = edrk.f_stator; - out_I_rms_calc_buffer[4].add_value(&out_I_rms_calc_buffer[4]); - out_I_rms_calc_buffer[5].val = analog.iqIu_1; - out_I_rms_calc_buffer[5].teta = teta_ch2; - out_I_rms_calc_buffer[5].freq_pwm = FREQ_PWM * 2; - out_I_rms_calc_buffer[5].signal_freq = edrk.f_stator; - out_I_rms_calc_buffer[5].add_value(&out_I_rms_calc_buffer[5]); -#endif //USE_CALC_I_OUT_RMS -} - - -void calc_Uin_rms(void) { - - RMS_CALC_ARRAY rc = RMS_CALC_DEFAULTS; - - rc.size_array = in_U_rms_calc_buffer[0].array_size; - rc.data_array = in_U_rms_calc_buffer[0].values; - analog.iqUin_A1B1_rms = rc.calc(&rc); - rc.size_array = in_U_rms_calc_buffer[1].array_size; - rc.data_array = in_U_rms_calc_buffer[1].values; - analog.iqUin_B1C1_rms = rc.calc(&rc); - rc.size_array = in_U_rms_calc_buffer[2].array_size; - rc.data_array = in_U_rms_calc_buffer[2].values; - analog.iqUin_C1A1_rms = rc.calc(&rc); - - rc.size_array = in_U_rms_calc_buffer[3].array_size; - rc.data_array = in_U_rms_calc_buffer[3].values; - analog.iqUin_A2B2_rms = rc.calc(&rc); - rc.size_array = in_U_rms_calc_buffer[4].array_size; - rc.data_array = in_U_rms_calc_buffer[4].values; - analog.iqUin_B2C2_rms = rc.calc(&rc); - rc.size_array = in_U_rms_calc_buffer[5].array_size; - rc.data_array = in_U_rms_calc_buffer[5].values; - analog.iqUin_C2A2_rms = rc.calc(&rc); - -} - - -void calc_Iout_rms() { - int i = 0; - _iq results[NUM_OF_CHANNELS]; - RMS_CALC_ARRAY_THINNING rc_Iout = RMS_CALC_THINNING_DEFAULTS; - - //Calc rms - for (i = 0; i < NUM_OF_CHANNELS; i++) { - rc_Iout.data_array = out_I_rms_calc_buffer[i].values; - rc_Iout.last_elem_position = out_I_rms_calc_buffer[i].position; - rc_Iout.size_array = out_I_rms_calc_buffer[i].array_size; - rc_Iout.signal_period = out_I_rms_calc_buffer[i].elements_in_period; - results[i] = rc_Iout.calc(&rc_Iout); - } - analog.iqIu_1_rms = results[0]; - analog.iqIv_1_rms = results[1]; - analog.iqIw_1_rms = results[2]; - analog.iqIu_2_rms = results[3]; - analog.iqIv_2_rms = results[4]; - analog.iqIw_2_rms = results[5]; -} - -void calc_RMS_values_main() { -#ifdef USE_CALC_U_IN_RMS - calc_Uin_rms(); -#endif -// i_led2_off(); -// i_led2_on(); -#ifdef USE_CALC_I_OUT_RMS - calc_Iout_rms(); -#endif -} - -//float signal_amplitude = 141; -//_iq data_arr[RMS_BUFFER_SIZE]; -//_iq16 data_arr_IQ16[RMS_BUFFER_SIZE]; -//_iq position = 0; -//_iq result_simple = 0; -//_iq result_simple_t = 0; -// -//RMS_BUFFER_WITH_THINNING buffer_var_freq = RMS_BUFFER_WITH_THINNING_DEFAULTS; -//int freq_mian_signal = 1; -//int counter_wait_change_freq = 0; -//#define WAIT_CHANGE_FREQ_TICS 2000 -/* -void test_calc_rms (_iq teta) { - _iq amplitude = _IQ (signal_amplitude / NORMA_ACP); - _iq current_val = 0; - _iq add_var_freq = 0; - static _iq add_50hz = _IQ(6.28 * 50.0 / FREQ_PWM / 2.0); - static _iq teta_50hz = 0, teta_Low = 0; - RMS_CALC_ARRAY rc = RMS_CALC_DEFAULTS; - - RMS_CALC_ARRAY_THINNING rct = RMS_CALC_THINNING_DEFAULTS; - - current_val = _IQmpy(amplitude, _IQcos(teta_50hz)); - data_arr[position] = current_val; - data_arr_IQ16[position] = _IQtoIQ16(current_val); - position += 1; - if (position >= RMS_BUFFER_SIZE) position = 0; - teta_50hz += add_50hz; - rc.data_array = data_arr; - rc.size_array = RMS_BUFFER_SIZE; - result_simple = rc.calc(&rc); - - if (counter_wait_change_freq >= WAIT_CHANGE_FREQ_TICS) { - if (freq_mian_signal < 20) { freq_mian_signal +=1;} - counter_wait_change_freq = 0; - } else { - counter_wait_change_freq += 1; - } - - add_var_freq = _IQ(6.28 * freq_mian_signal / FREQ_PWM / 2.0); - teta_Low += add_var_freq; - current_val = _IQmpy(amplitude, _IQcos(teta_Low)); - buffer_var_freq.val = current_val; - buffer_var_freq.signal_freq = _IQ((float)freq_mian_signal / NORMA_FROTOR); - buffer_var_freq.teta = teta_Low; - buffer_var_freq.add_value(&buffer_var_freq); - - - - rct.data_array = buffer_var_freq.values; - rct.size_array = buffer_var_freq.array_size; - rct.last_elem_position = buffer_var_freq.position; - rct.signal_period = buffer_var_freq.array_size; - result_simple_t = rct.calc(&rct); -} -*/ diff --git a/Inu/Src2/551/main/calc_rms_vals.h b/Inu/Src2/551/main/calc_rms_vals.h deleted file mode 100644 index a01b3da..0000000 --- a/Inu/Src2/551/main/calc_rms_vals.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * calc_rms_vals.h - * - * Created on: 14 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_CALC_RMS_VALS_H_ -#define SRC_MAIN_CALC_RMS_VALS_H_ - - -#include - -#include "IQmathLib.h" - -#define RMS_BUFFER_SIZE (FREQ_PWM * 2 / 50) -//#define RMS_BUFFER_SIZE 18 //For FREQ_PWM 450 and signal frequency 50 Hz - - -typedef struct { - _iq values[RMS_BUFFER_SIZE]; - int array_size; - int position; -} RMS_BUFFER; - -#define RMS_BUFFER_DEFAULTS {{0}, RMS_BUFFER_SIZE, 0} - -#define RMS_THINNING_BUFFER_SIZE 40 // 30 -#define RMS_THINNING_PERIOD (RMS_THINNING_BUFFER_SIZE * 3 / 4) -typedef struct { - _iq val; //in - _iq16 values[RMS_THINNING_BUFFER_SIZE]; - int array_size; - int position; - int elements_in_period; // 1 - - int freq_pwm; //in - _iq signal_freq; //in - int use_teta; // teta - _iq teta; //in - - struct { - int miss_write_counter; - int teta_period_counter; - _iq teta_prev; - int zero_teta_period; -// int zero_teta_counter_prev; - } internal; - void (*add_value)(); -} RMS_BUFFER_WITH_THINNING; - - -#define RMS_BUFFER_WITH_THINNING_DEFAULTS {0, {0}, RMS_THINNING_BUFFER_SIZE,0, \ - RMS_THINNING_PERIOD,\ - (FREQ_PWM * 2),0,0,0, {0,0,0,0}, \ - fill_rms_array_IQ15} - -void fill_rms_array_IQ15(RMS_BUFFER_WITH_THINNING *v); -void fill_RMS_buff_interrupt(_iq teta_ch1, _iq teta_ch2); -void calc_RMS_values_main(); -void init_Uin_rms(void); - - -void test_calc_rms (_iq teta); - -#endif /* SRC_MAIN_CALC_RMS_VALS_H_ */ diff --git a/Inu/Src2/551/main/calc_tempers.c b/Inu/Src2/551/main/calc_tempers.c deleted file mode 100644 index a5988cc..0000000 --- a/Inu/Src2/551/main/calc_tempers.c +++ /dev/null @@ -1,284 +0,0 @@ -/* - * calc_tempers.c - * - * Created on: 4 . 2020 . - * Author: star - */ - -#include -#include -#include -#include -#include - -#include "CAN_Setup.h" -#include "IQmathLib.h" - -int calc_max_temper_acdrive_bear(void); -int calc_max_temper_acdrive_winding(void); -int calc_max_temper_edrk_u(void); -int calc_max_temper_edrk_water(void); -int calc_max_temper_edrk_air(void); -int calc_min_temper_edrk_air(void); - - - - -//////////////////////////////////////////////////////////////////// -int calc_max_temper_acdrive_bear(void) -{ - int i, max_t=0; - - for (i=0;imax_t) - max_t = edrk.temper_acdrive.winding.filter_real_int_temper[i]; - - return max_t; - -} -//////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////// -int calc_max_temper_acdrive_winding(void) -{ - int i, max_t=0; - - for (i=0;imax_t) - max_t = edrk.temper_acdrive.winding.filter_real_int_temper[i]; - - return max_t; - -} -//////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////// -int calc_max_temper_edrk_u(void) -{ - int i, max_t=0; - - for (i=0;i<7;i++) - if (edrk.temper_edrk.real_int_temper_u[i]>max_t) - max_t = edrk.temper_edrk.real_int_temper_u[i]; - - return max_t; - -} -//////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -int calc_max_temper_edrk_water(void) -{ - int i, max_t=0; - - for (i=0;i<2;i++) - if (edrk.temper_edrk.real_int_temper_water[i]>max_t) - max_t = edrk.temper_edrk.real_int_temper_water[i]; - - return max_t; - -} -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -int calc_max_temper_edrk_air(void) -{ - int i, max_t=0; - - for (i=0;i<4;i++) - if (edrk.temper_edrk.real_int_temper_air[i]>max_t) - max_t = edrk.temper_edrk.real_int_temper_air[i]; - - return max_t; - -} -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -int calc_min_temper_edrk_air(void) -{ - int i, min_t=1000; - - for (i=0;i<4;i++) - if (edrk.temper_edrk.real_int_temper_air[i] -#include -#include -#include -#include - -#include "control_station.h" -#include "CAN_Setup.h" -#include "global_time.h" -#include "IQmathLib.h" -#include "mathlib.h" -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" -#include "xp_project.h" -#include "another_bs.h" - - - - - - -#pragma DATA_SECTION(Unites2SecondBS, ".slow_vars") -int Unites2SecondBS[SIZE_ARR_CAN_UNITES_BS2BS]={0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0\ - }; - - - -int max_count_send_to_can2second_bs = SIZE_ARR_CAN_UNITES_BS2BS; - - - - - -void SendAll2SecondBS(unsigned int pause) -{ - static int time_tick_modbus_can=0; - static unsigned int old_PWM_ticks=0; - static int count_write_to_modbus_can=0; - static int time_send_to_can=0; - int real_mbox, n_box; - - - - -//send to another BS - if (detect_pause_milisec(pause, &old_PWM_ticks)) - { - // if (edrk.flag_second_PCH==0) - // n_box = ANOTHER_BSU2_CAN_DEVICE; - // else - n_box = ANOTHER_BSU1_CAN_DEVICE; - - time_tick_modbus_can=0; - time_send_to_can=0; - - UpdateTableSecondBS(); - - real_mbox = get_real_out_mbox (UNITS_TYPE_BOX, n_box); - - if (CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_ON)) - { - CAN_cycle_send( UNITS_TYPE_BOX, n_box, 0, &Unites2SecondBS[0], max_count_send_to_can2second_bs, CAN_BOX_STANDART_ADR, CAN_BOX_PRIORITY_NORMAL);// 40 . - } - } - - - -} - - - diff --git a/Inu/Src2/551/main/can_bs2bs.h b/Inu/Src2/551/main/can_bs2bs.h deleted file mode 100644 index b3d69cf..0000000 --- a/Inu/Src2/551/main/can_bs2bs.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * can_bs2bs.h - * - * Created on: 27 . 2020 . - * Author: stud - */ - -#ifndef SRC_MAIN_CAN_BS2BS_H_ -#define SRC_MAIN_CAN_BS2BS_H_ - -#define SIZE_ARR_CAN_UNITES_BS2BS 50 //100 - - -extern int Unites2SecondBS[SIZE_ARR_CAN_UNITES_BS2BS]; - -void SendAll2SecondBS(unsigned int pause); - - - - -#endif /* SRC_MAIN_CAN_BS2BS_H_ */ diff --git a/Inu/Src2/551/main/can_protocol_ukss.h b/Inu/Src2/551/main/can_protocol_ukss.h deleted file mode 100644 index 18bf157..0000000 --- a/Inu/Src2/551/main/can_protocol_ukss.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * can_protocol_ukss.h - * - * Created on: 23 . 2024 . - * Author: yura - */ - -#ifndef SRC_MAIN_CAN_PROTOCOL_UKSS_H_ -#define SRC_MAIN_CAN_PROTOCOL_UKSS_H_ - - - -#define CAN_PROTOCOL_UKSS 2 // 2 - - -#ifndef CAN_PROTOCOL_UKSS -#define CAN_PROTOCOL_UKSS 1 -#endif - - - - -#if (CAN_PROTOCOL_UKSS == 2) - - -#define ADR_CYCLES_TIMER_MAIN 96 // . CAN, * 10 m -#define ADR_CYCLES_TIMER_ADD 97 // . CAN, * 10 m -#define ADR_CYCLES_PAUSE_MAIN 98 // . CAN, * 10 m -#define ADR_CYCLES_PAUSE_ADD 99 // . CAN, * 10 m -#define ADR_CYCLES_REPEATE_MAIN 100 // . CAN, * 10 m -#define ADR_CYCLES_REPEATE_ADD 101 // . CAN, * 10 m -#define ADR_CYCLES_REPEATE_DIGIO 102 // . , - -#define ADR_LIGHT_LED_1 104 // 1 -#define ADR_LIGHT_LED_2 105 // 2 -#define ADR_LIGHT_LED_3 106 // 3 -#define ADR_LIGHT_LED_4 107 // 4 -#define ADR_LIGHT_LED_5 108 // 5 -#define ADR_LIGHT_LED_6 109 // 6 -#define ADR_LIGHT_LED_7 110 // 7 - - - -#define ADR_COUNT_CYCLES_MAIN 120 // . CAN -#define ADR_COUNT_CYCLES_ADD 121 // . CAN -#define ADR_COUNT_FULL_CYCLES_MAIN 122 //- . CAN -#define ADR_COUNT_FULL_CYCLES_ADD 123 //- . CAN - -#define ADR_PROTOCOL_VERSION 125 // -#define ADR_UKSS_NUMBER 126 // - -#endif - - - - - - - - - - -#endif /* SRC_MAIN_CAN_PROTOCOL_UKSS_H_ */ diff --git a/Inu/Src2/551/main/control_station_project.c b/Inu/Src2/551/main/control_station_project.c deleted file mode 100644 index 877b4fb..0000000 --- a/Inu/Src2/551/main/control_station_project.c +++ /dev/null @@ -1,2493 +0,0 @@ -/* - * control_station_project.c - * - * Created on: 1 . 2020 . - * Author: Yura - */ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "control_station.h" -#include "CAN_Setup.h" -#include "global_time.h" -#include "IQmathLib.h" -#include "mathlib.h" -#include "modbus_table_v2.h" -#include "vector_control.h" -#include "RS_Functions.h" -#include "adc_tools.h" -#include "RS_Function_terminal.h" -#include "alg_simple_scalar.h" - - - - - - -/*CONTROL_STATION_TERMINAL_RS232 - CONTROL_STATION_TERMINAL_CAN, - - CONTROL_STATION_INGETEAM_PULT_RS485, - CONTROL_STATION_MPU_SVU_CAN, - CONTROL_STATION_MPU_KEY_CAN, - CONTROL_STATION_MPU_SVU_RS485, - CONTROL_STATION_MPU_KEY_RS485, - CONTROL_STATION_ZADATCHIK_CAN, - - - - CONTROL_STATION_CMD_GO = 0,// cmd_go / - CONTROL_STATION_CMD_SET_IZAD,// - CONTROL_STATION_CMD_SET_ROTOR,// - CONTROL_STATION_CMD_CHARGE,// - CONTROL_STATION_CMD_UNCHARGE,// - CONTROL_STATION_CMD_CHECKBACK,// - CONTROL_STATION_CMD_TEST_LEDS - */ - - -#define DEC_ZAD_OBOROTS 1 -#define INC_ZAD_OBOROTS 1 - - - - - -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void detect_active_from_all_signals(void) -{ - if (control_station.alive_control_station[CONTROL_STATION_TERMINAL_RS232]) - { - // control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_ACTIVE_CONTROL] = control_station - } - - -} -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -float plus_minus_oborots(int key_plus, int key_minus, float ff, int ufconst_vector, int flag) -{ - static float ff_add=0, ff_dec=0; - - float ff_add1=0, ff_dec1=0; - float ff_add2=0, ff_dec2=0; - float ff_add3=0, ff_dec3=0; - - static int prev_key_plus=0, prev_key_minus=0, count_press2 = 0, level_d1 = 2; - - static int direct_zad=0, direct_cur=0, only_plus=0, only_minus=0; - static unsigned int old_time_plus_minus=0, count_press = 0; - - if (flag==1) - { - count_press = 0; - - ff_dec = 0; - ff_add = 0; - - prev_key_plus = key_plus; - prev_key_minus = key_minus; - - return ff; - } - - if (ff>0) - { -// if (key_minus || key_plus) - only_plus = 1; - only_minus = 0; - } - else - if (ff<0) - { -// if (key_minus || key_plus) - only_minus = 1; - only_plus = 0; - } - else - { - if (key_minus==0 && key_plus==0) - { - only_plus = 0; - only_minus = 0; - } - - } - - -// -// -// if (key_plus) -// { -// if (ff>0) -// direct_zad = 1; -// if (ff<0) -// direct_zad = -1; -// if (ff==0) -// direct_zad = 0; -// -// } -// if (key_minus) -// { -// if (ff>0) -// direct_zad = -1; -// if (ff<0) -// direct_zad = 1; -// if (ff==0) -// direct_zad = 0; -// } - - - if (ufconst_vector==0) - { - ff_add1 = INC_ZAD_OBOROTS*10.0; - ff_dec1 = DEC_ZAD_OBOROTS*10.0; - - ff_add2 = INC_ZAD_OBOROTS*30.0; - ff_dec2 = DEC_ZAD_OBOROTS*30.0; - ff_add3 = INC_ZAD_OBOROTS*60.0; - ff_dec3 = DEC_ZAD_OBOROTS*60.0; - - } - else - { - ff_add1 = INC_ZAD_OBOROTS; - ff_dec1 = DEC_ZAD_OBOROTS; - - ff_add2 = INC_ZAD_OBOROTS*1.0; - ff_dec2 = DEC_ZAD_OBOROTS*1.0; - ff_add3 = INC_ZAD_OBOROTS*2.0; - ff_dec3 = DEC_ZAD_OBOROTS*2.0; - } - - - if (detect_pause_milisec(250,&old_time_plus_minus)) - { - if (key_minus || key_plus) - { - if (count_press<300) - count_press++; - - if (count_press2<10) - count_press2++; - - } - else - { - count_press = 0; - count_press2 = 0; - } - - if (count_press==1) - { -// ff_dec = 0; -// ff_add = 0; - ff_dec = ff_dec1; - ff_add = ff_add1; - level_d1 = 4; - } - - if (count_press==6) - { - ff_dec = ff_dec1; - ff_add = ff_add1; - level_d1 = 2; - } - - if (count_press==15) - { - ff_dec = ff_dec1; - ff_add = ff_add1; - level_d1 = 0; - } -// if (count_press==60) -// { -// ff_dec = ff_dec2; -// ff_add = ff_add2; -// level_d1 = 0; -// } - - if (count_press==30) - { - ff_dec = ff_dec3; - ff_add = ff_add3; - } - - - if (key_minus) - { - if ((count_press2>=level_d1) || (count_press==1)) - { - if (ff>ff_dec) - { - ff=ff-ff_dec; - if (ff0) - ff = 0; - } - else - if (ff<=ff_dec && ff>0) - ff=0; - else - if (ff<0 && only_plus==0) - ff=ff-ff_add; - else - if (ff==0 && only_plus==0) - ff=ff-DEAD_ZONE_ZADANIE_OBOROTS_ROTOR; - - count_press2 = 0; - } - } - else - if (key_plus) - { - if ((count_press2>=level_d1) || (count_press==1)) - { - if (ff==0 && only_minus==0) - ff=ff+DEAD_ZONE_ZADANIE_OBOROTS_ROTOR; - else - if (ff>=0 && only_minus==0) - ff=ff+ff_add; - else - if (ff>=-ff_dec && ff<0) - ff=0; - else - if (ff<-ff_dec) - { - ff=ff+ff_dec; - if (ff>-DEAD_ZONE_ZADANIE_OBOROTS_ROTOR && ff<0) - ff = 0; - } - - count_press2 = 0; - } - } - - } - - if (ufconst_vector==0) - ff = my_satur_float(ff,fast_round(MAX_ZADANIE_F*100.0),fast_round(-MAX_ZADANIE_F*100.0), 0); - else - ff = my_satur_float(ff, MAX_ZADANIE_OBOROTS_ROTOR, MIN_ZADANIE_OBOROTS_ROTOR, 0); - - - prev_key_plus = key_plus; - prev_key_minus = key_minus; - - - return ff; -} - -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void parse_parameters_from_one_control_station_another_bs(int cc) -{ - int i;//, pos_numbercmd; - static int prev_checkback = 0; - static int flag_wait_revers_go = 0; - - _iq _iq_ff; - static unsigned int prev_charge=0, prev_uncharge=0, key_local_charge=0, key_local_uncharge=0; - static unsigned int old_time_pult_ing = 0; - float ff=0, ff_add=0, ff_dec=0; - int key_plus = 0, key_minus = 0; - - - - - if (edrk.ms.err_signals.alive_can_to_another_bs==0) - { - - // - // edrk.zadanie_from_another_bs.int_fzad = Unites[ANOTHER_BSU1_CAN_DEVICE][3]; - // edrk.zadanie_from_another_bs.int_kzad = Unites[ANOTHER_BSU1_CAN_DEVICE][4]; - // edrk.zadanie_from_another_bs.int_Izad = Unites[ANOTHER_BSU1_CAN_DEVICE][5]; - // edrk.zadanie_from_another_bs.int_oborots_zad = Unites[ANOTHER_BSU1_CAN_DEVICE][6]; - // edrk.zadanie_from_another_bs.int_power_zad = Unites[ANOTHER_BSU1_CAN_DEVICE][7]; - // // - // edrk.zadanie_from_another_bs.iq_fzad = _IQ15toIQ(edrk.zadanie_from_another_bs.int_fzad); - // edrk.zadanie_from_another_bs.iq_kzad = _IQ15toIQ(edrk.zadanie_from_another_bs.int_kzad); - // edrk.zadanie_from_another_bs.iq_Izad = _IQ15toIQ(edrk.zadanie_from_another_bs.int_Izad); - // edrk.zadanie_from_another_bs.oborots_zad = edrk.zadanie_from_another_bs.int_oborots_zad; - // edrk.zadanie_from_another_bs.iq_power_zad = _IQ15toIQ(edrk.zadanie_from_another_bs.int_power_zad); - - - } - else - { - - - - } - - - if (control_station.alive_control_station[cc]) - { - - } - else - { - for (i=0;i 0 && Unites[ANOTHER_BSU1_CAN_DEVICE][12] < 600) { - vect_control.iqId_min = _IQ((float)Unites[ANOTHER_BSU1_CAN_DEVICE][12] / NORMA_ACP); - } -/* - - control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL] = control_station.active_control_station[CONTROL_STATION_VPU_CAN]; - - -// scalar, vector, ufconst - control_station.array_cmd[cc][CONTROL_STATION_CMD_UFCONST_VECTOR] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_UFCONST_VECTOR]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SCALAR_FOC] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_SCALAR_FOC]; - - - control_station.array_cmd[cc][CONTROL_STATION_CMD_ROTOR_POWER] = 0; - -// -// if - - if ((detect_pause_milisec(100,&old_time_pult_ing)) && control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]) - { - -// control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = control_station.raw_array_data[cc][4].all; - ff = control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]; - - key_plus = edrk.from_vpu.bits.PLUS; - key_minus = edrk.from_vpu.bits.MINUS; - - ff = plus_minus_oborots(key_plus, key_minus, ff, control_station.array_cmd[cc][CONTROL_STATION_CMD_UFCONST_VECTOR]); - - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR] = ff; - } - - -// -// pos_numbercmd = 15; // , - - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHECKBACK] = edrk.from_vpu.bits.KVITIR; - -// , - key_local_charge = 0;//edrk.from_shema.bits.SBOR_SHEMA; - key_local_uncharge = 0;//edrk.from_shema.bits.RAZBOR_SHEMA; - - if (key_local_charge && prev_charge==0) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - prev_charge = key_local_charge; - - - if (key_local_uncharge && prev_uncharge==0) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - prev_uncharge = key_local_uncharge; - - -/////////////////////////////////////////// -// - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_IZAD] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_SET_IZAD]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_KM] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_SET_KM]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_U_ZARYAD] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_SET_U_ZARYAD]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_K_U_DISBALANCE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_SET_K_U_DISBALANCE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_K_PLUS_U_DISBALANCE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_SET_K_PLUS_U_DISBALANCE]; - - -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]!=0) -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 1; -// else -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 0; - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_GO]; - -// -// control_station.array_cmd[cc][CONTROL_STATION_CMD_TEST_LEDS] = 0; -// -// prev_checkback = control_station.raw_array_data[cc][pos_numbercmd].bits.bit1; - - // rs232 - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_PUMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_ENABLE_ON_CHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_QTV]; - - // 0 - auto on - rand pump - // 1 - auto on 1 pump - // 2 - auto on 2 pump - // 3 - manual on 1 pump - // 4 - manual on 2 pump - - // - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MODE_PUMP]; - - - if (control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]==0) - return; -*/ - -} -///////////////////////////////////////////////////////////// - -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void parse_parameters_from_one_control_station_pult_vpu(int cc) -{ - int i;//, pos_numbercmd; - static int prev_checkback = 0, prev_key_oborots = 0; - static int flag_wait_revers_go = 0; - - _iq _iq_ff; - static unsigned int prev_charge=0, prev_uncharge=0, key_local_charge=0, key_local_uncharge=0; - static unsigned int old_time_pult_ing = 0, old_time_pult_ing2 = 0; - float ff=0, ff_add=0, ff_dec=0; - int key_plus = 0, key_minus = 0; - - - if (control_station.alive_control_station[cc]) - { - - } - else - { - for (i=0;i=0) - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = -control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - - - - - -// -// pos_numbercmd = 15; // , - - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHECKBACK] = edrk.from_vpu.bits.KVITIR; - -// , - key_local_charge = 0;//edrk.from_shema.bits.SBOR_SHEMA; - key_local_uncharge = 0;//edrk.from_shema.bits.RAZBOR_SHEMA; - - if (key_local_charge && prev_charge==0) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - prev_charge = key_local_charge; - - - if (key_local_uncharge) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - prev_uncharge = key_local_uncharge; - - - - -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]!=0) -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 1; -// else -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 0; -// -// control_station.array_cmd[cc][CONTROL_STATION_CMD_TEST_LEDS] = 0; -// -// prev_checkback = control_station.raw_array_data[cc][pos_numbercmd].bits.bit1; - - - // rs232 - if (control_station.alive_control_station[CONTROL_STATION_TERMINAL_RS232]) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_GO]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_PUMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_ENABLE_ON_CHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_QTV]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_UMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_MANUAL_DISCHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_STOP_LOGS] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_STOP_LOGS]; - - } - else - if (control_station.alive_control_station[CONTROL_STATION_INGETEAM_PULT_RS485]) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_GO]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_PUMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_ENABLE_ON_CHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_QTV]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_UMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MANUAL_DISCHARGE]; - - edrk.Status_Ready.bits.ImitationReady2 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][181].all; - edrk.Obmotka1 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][194].all; - edrk.Obmotka2 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][195].all; - edrk.disable_alg_u_disbalance = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][193].all; - - - edrk.Run_Rascepitel_from_RS = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][182].all; - edrk.stop_logs_rs232 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][183].all; - edrk.stop_slow_log = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][184].all; - edrk.disable_limit_power_from_svu = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][185].all; - edrk.disable_uom = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][186].all; - - - - - } - - - // 0 - auto on - rand pump - // 1 - auto on 1 pump - // 2 - auto on 2 pump - // 3 - manual on 1 pump - // 4 - manual on 2 pump - - // - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MODE_PUMP]; - - - if (control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]==0) - return; - - -} -///////////////////////////////////////////////////////////// - - - -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void parse_parameters_from_one_control_station_pult_zadat4ik(int cc) -{ - int i;//, pos_numbercmd; - static int prev_checkback = 0, prev_key_oborots = 0; - static int flag_wait_revers_go = 0; - - float ff=0, ff_add=0, ff_dec=0; - int key_plus = 0, key_minus = 0; - - _iq _iq_ff; - static unsigned int prev_charge=0, prev_uncharge=0, key_local_charge=0, key_local_uncharge=0; - static unsigned int old_time_pult_ing = 0, old_time_pult_ing2 = 0; - - if (control_station.alive_control_station[cc]) - { - - } - else - { - for (i=0;i=0) - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = -control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - -// -// pos_numbercmd = 15; // , - - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHECKBACK] = edrk.from_zadat4ik.bits.KVITIR; - -// , - key_local_charge = edrk.from_shema_filter.bits.SBOR_SHEMA; - key_local_uncharge = edrk.from_shema_filter.bits.RAZBOR_SHEMA; - - - if (key_local_uncharge && key_local_charge) - { - key_local_uncharge = 0; - key_local_charge = 0; - //edrk.errors.e1.bits.BOTH_KEYS_CHARGE_DISCHARGE |=1; - } - - if (key_local_charge && prev_charge==0) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - prev_charge = key_local_charge; - - - if (key_local_uncharge) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - prev_uncharge = key_local_uncharge; - - -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]!=0) -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 1; -// else -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 0; - -// -// control_station.array_cmd[cc][CONTROL_STATION_CMD_TEST_LEDS] = 0; -// -// prev_checkback = control_station.raw_array_data[cc][pos_numbercmd].bits.bit1; - - - // rs232 - if (control_station.alive_control_station[CONTROL_STATION_TERMINAL_RS232]) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_GO]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_PUMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_ENABLE_ON_CHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_QTV]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_UMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_MANUAL_DISCHARGE]; - } - else - if (control_station.alive_control_station[CONTROL_STATION_INGETEAM_PULT_RS485]) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_GO]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_PUMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_ENABLE_ON_CHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_QTV]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_UMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MANUAL_DISCHARGE]; - - edrk.Status_Ready.bits.ImitationReady2 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][181].all; - edrk.Obmotka1 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][194].all; - edrk.Obmotka2 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][195].all; - edrk.disable_alg_u_disbalance = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][193].all; - - edrk.Run_Rascepitel_from_RS = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][182].all; - edrk.stop_logs_rs232 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][183].all; - edrk.stop_slow_log = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][184].all; - edrk.disable_limit_power_from_svu = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][185].all; - edrk.disable_uom = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][186].all; - - } - - - // 0 - auto on - rand pump - // 1 - auto on 1 pump - // 2 - auto on 2 pump - // 3 - manual on 1 pump - // 4 - manual on 2 pump - - // - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MODE_PUMP]; - - - if (control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]==0) - return; - - -} -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void parse_parameters_from_one_control_station_pult_ingeteam(int cc) -{ - int i, zad_power; //pos_numbercmd - static int prev_checkback = 0; - static int flag_wait_revers_go = 0; - float ff=0, ff_add=0, ff_dec=0; - int key_plus = 0, key_minus = 0; - _iq _iq_ff=0; - static unsigned int prev_key_local_charge_key=0, prev_key_local_uncharge_key=0, - key_local_charge_key=0, key_local_uncharge_key=0; - static unsigned int prev_key_local_charge_display=0, prev_key_local_uncharge_display=0, - key_local_charge_display=0, key_local_uncharge_display=0; - - static unsigned int prev_charge=0, prev_uncharge=0, key_local_charge=0, key_local_uncharge=0; - - static unsigned int old_time_pult_ing = 0, old_time_pult_ing2 = 0; - static unsigned int prev_key_local_charge_uncharge_display=0, key_local_charge_uncharge_display=0; - - if (control_station.alive_control_station[cc]) - { - - } - else - { - for (i=0;i=0) - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = -control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - - } - - - - } - - -// -// pos_numbercmd = 15; // , - - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHECKBACK] = control_station.raw_array_data[cc][1].all; - - -// , - key_local_charge_key = edrk.from_ing2.bits.KEY_SBOR; - key_local_uncharge_key = edrk.from_ing2.bits.KEY_RAZBOR; - - - -// , - key_local_charge_uncharge_display = control_station.raw_array_data[cc][2].all; - - - prev_key_local_charge_key = key_local_charge_key; - prev_key_local_uncharge_key = key_local_uncharge_key; - - - if (prev_key_local_charge_uncharge_display != key_local_charge_uncharge_display) - { - if (key_local_charge_uncharge_display==1) - { - key_local_charge_display = 1; - key_local_uncharge_display = 0; - } - else - { - key_local_charge_display = 0; - key_local_uncharge_display =1; - } - } - else - { - key_local_charge_display = 0; - key_local_uncharge_display = 0; - } - - prev_key_local_charge_uncharge_display = key_local_charge_uncharge_display; - - - prev_key_local_charge_display = key_local_charge_display; - prev_key_local_uncharge_display = key_local_uncharge_display; - - key_local_charge = key_local_charge_key || key_local_charge_display; - key_local_uncharge = key_local_uncharge_key || key_local_uncharge_display; - - if (key_local_uncharge_key && key_local_charge_key) - { - key_local_uncharge_key = 0; - key_local_charge_key = 0; -// edrk.errors.e1.bits.BOTH_KEYS_CHARGE_DISCHARGE |=1; - } - - - - if (edrk.Status_Ready.bits.ImitationReady2==0) // , .. - { - if (key_local_charge && prev_charge==0) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - - - - if (key_local_uncharge) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - - } - else - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - } - - prev_charge = key_local_charge; - prev_uncharge = key_local_uncharge; - - - -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]!=0) -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 1; -// else -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 0; -// - -// -// control_station.array_cmd[cc][CONTROL_STATION_CMD_TEST_LEDS] = 0; -// -// prev_checkback = control_station.raw_array_data[cc][pos_numbercmd].bits.bit1; - - - // rs232 - if (control_station.alive_control_station[CONTROL_STATION_TERMINAL_RS232]) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_PUMP] - || control_station.raw_array_data[cc][188].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] - || control_station.raw_array_data[cc][191].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_QTV] - || control_station.raw_array_data[cc][189].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_UMP] - || control_station.raw_array_data[cc][190].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_MANUAL_DISCHARGE] - || control_station.raw_array_data[cc][180].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_GO] - || !control_station.raw_array_data[cc][192].all; - } - else - { - - edrk.Status_Ready.bits.ImitationReady2 = control_station.raw_array_data[cc][181].all; - edrk.Run_Rascepitel_from_RS = control_station.raw_array_data[cc][182].all; - edrk.stop_logs_rs232 = control_station.raw_array_data[cc][183].all; - edrk.stop_slow_log = control_station.raw_array_data[cc][184].all; - edrk.disable_limit_power_from_svu = control_station.raw_array_data[cc][185].all; - edrk.disable_uom = control_station.raw_array_data[cc][186].all; - - - - edrk.Obmotka1 = control_station.raw_array_data[cc][194].all; - edrk.Obmotka2 = control_station.raw_array_data[cc][195].all; - edrk.disable_alg_u_disbalance = control_station.raw_array_data[cc][193].all; - - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.raw_array_data[cc][188].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.raw_array_data[cc][189].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.raw_array_data[cc][190].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.raw_array_data[cc][191].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.raw_array_data[cc][180].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = !control_station.raw_array_data[cc][192].all; - - } - - - - //control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.raw_array_data[cc][188].all; -// control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.raw_array_data[cc][189].all; - //control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.raw_array_data[cc][190].all; - //control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.raw_array_data[cc][191].all; -// control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.raw_array_data[cc][180].all; -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = !control_station.raw_array_data[cc][192].all; - - - - // 0 - auto on - rand pump - // 1 - auto on 1 pump - // 2 - auto on 2 pump - // 3 - manual on 1 pump - // 4 - manual on 2 pump - - if (control_station.raw_array_data[cc][9].all<=2) // _(0)_(1) - { - if (control_station.raw_array_data[cc][9].all==0) // (0) 1_2(1) - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = 0; - if (control_station.raw_array_data[cc][9].all==1) // _1 - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = 1; - if (control_station.raw_array_data[cc][9].all==2) // _2 - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = 2; - } - else - { - if (control_station.raw_array_data[cc][9].all==3) // _1_2 - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = 3; - if (control_station.raw_array_data[cc][9].all==4) // _1_2 - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = 4; - } - - - if (control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]==0) - return; - -} -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void parse_parameters_from_one_control_station_MPU_SVU(int cc) -{ - int i, zad_power, limit_power; - _iq _iq_ff; - - static unsigned int old_time_MPU_SVU = 0; - - static int prev_checkback_3 = 0; - static int flag_wait_revers_go_3 = 0; - static unsigned int prev_charge_3 = 0, prev_uncharge_3 = 0, cmd_local_charge_3 = 0, cmd_local_uncharge_3 = 0; - static unsigned int old_time_MPU_SVU_3 = 0; - - static int prev_checkback_4 = 0; - static int flag_wait_revers_go_4 = 0; - static unsigned int prev_charge_4 = 0, prev_uncharge_4 = 0, cmd_local_charge_4 = 0, cmd_local_uncharge_4 = 0; - static unsigned int old_time_MPU_SVU_4 = 0; - - - - if (control_station.alive_control_station[cc]) - { - - } - else - { - for (i=0;i=0) - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = -control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - - } - } - // - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHECKBACK] = control_station.raw_array_data[cc][0].all; - - - if (cc==CONTROL_STATION_MPU_SVU_CAN) - { - // , - if (edrk.flag_second_PCH == 0) { - cmd_local_charge_3 = control_station.raw_array_data[cc][6].all; - cmd_local_uncharge_3 = control_station.raw_array_data[cc][9].all; - } else { - cmd_local_charge_3 = control_station.raw_array_data[cc][7].all;//modbus_table_can_in[129].all; //control_station.raw_array_data[cc][7].all; - cmd_local_uncharge_3 = control_station.raw_array_data[cc][10].all; - } - - - if (cmd_local_charge_3 && cmd_local_uncharge_3) - { - cmd_local_charge_3 = 0; - cmd_local_uncharge_3 = 0; - // edrk.errors.e1.bits.BOTH_KEYS_CHARGE_DISCHARGE |=1; - } - - - - if (cmd_local_charge_3 && prev_charge_3==0) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - prev_charge_3 = cmd_local_charge_3; - - - if (cmd_local_uncharge_3) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - - prev_uncharge_3 = cmd_local_uncharge_3; - } - - if (cc==CONTROL_STATION_MPU_KEY_CAN) - { - // , - if (edrk.flag_second_PCH == 0) { - cmd_local_charge_4 = control_station.raw_array_data[cc][6].all; - cmd_local_uncharge_4 = control_station.raw_array_data[cc][9].all; - } else { - cmd_local_charge_4 = control_station.raw_array_data[cc][7].all;//modbus_table_can_in[129].all; //control_station.raw_array_data[cc][7].all; - cmd_local_uncharge_4 = control_station.raw_array_data[cc][10].all; - } - - - if (cmd_local_charge_4 && cmd_local_uncharge_4) - { - cmd_local_charge_4 = 0; - cmd_local_uncharge_4 = 0; - // edrk.errors.e1.bits.BOTH_KEYS_CHARGE_DISCHARGE |=1; - } - - - - if (cmd_local_charge_4 && prev_charge_4==0) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - prev_charge_4 = cmd_local_charge_4; - - - if (cmd_local_uncharge_4) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 1; - control_station.array_cmd[cc][CONTROL_STATION_CMD_CHARGE] = 0; - } - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_UNCHARGE] = 0; - - prev_uncharge_4 = cmd_local_uncharge_4; - } - - control_station.array_cmd[cc][CONTROL_STATION_CMD_BLOCK_BS] = control_station.raw_array_data[cc][4].all; - -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]!=0) -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 1; -// else -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 0; -// -// -// control_station.array_cmd[cc][CONTROL_STATION_CMD_TEST_LEDS] = 0; -// -// prev_checkback = control_station.raw_array_data[cc][pos_numbercmd].bits.bit1; - - - // rs232 - if (control_station.alive_control_station[CONTROL_STATION_TERMINAL_RS232]) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_GO]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_PUMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_ENABLE_ON_CHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_QTV]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_DISABLE_ON_UMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_MANUAL_DISCHARGE]; - } - else - if (control_station.alive_control_station[CONTROL_STATION_INGETEAM_PULT_RS485]) - { - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_GO]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_PUMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_ENABLE_ON_CHARGE]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_QTV] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_QTV]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_DISABLE_ON_UMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_UMP]; - control_station.array_cmd[cc][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MANUAL_DISCHARGE]; - - edrk.Status_Ready.bits.ImitationReady2 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][181].all; - edrk.Obmotka1 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][194].all; - edrk.Obmotka2 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][195].all; - edrk.disable_alg_u_disbalance = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][193].all; - - edrk.Run_Rascepitel_from_RS = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][182].all; - edrk.stop_logs_rs232 = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][183].all; - edrk.stop_slow_log = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][184].all; - edrk.disable_limit_power_from_svu = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][185].all; - edrk.disable_uom = control_station.raw_array_data[CONTROL_STATION_INGETEAM_PULT_RS485][186].all; - - - } - - - // 0 - auto on - rand pump - // 1 - auto on 1 pump - // 2 - auto on 2 pump - // 3 - manual on 1 pump - // 4 - manual on 2 pump - - // - control_station.array_cmd[cc][CONTROL_STATION_CMD_MODE_PUMP] = control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MODE_PUMP]; - - - edrk.breaker_on = control_station.raw_array_data[cc][17].all; - - edrk.break_tempers[0] = control_station.raw_array_data[cc][18].all; - edrk.break_tempers[1] = control_station.raw_array_data[cc][19].all; - edrk.break_tempers[2] = control_station.raw_array_data[cc][20].all; - edrk.break_tempers[3] = control_station.raw_array_data[cc][21].all; - - - if (control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]==0) - return; - -} -///////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void parse_parameters_from_one_control_station_terminal_rs232(int cc) -{ - static int pos_numbercmd = (sizeof(CMD_ANALOG_DATA_STRUCT) >> 1); - int i, fint; - static int prev_checkback = 0, lock_ImitationReady2 = 0; - static int flag_wait_revers_go = 0; - static unsigned int prev_charge=0, prev_uncharge=0, key_local_charge=0, key_local_uncharge=0; - float ff; - _iq _iq_ff; - int zad_power=0; - - if (control_station.alive_control_station[cc]) - { - - } - else - { - for (i=0;i=0) - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - else - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = -control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_LIMIT_POWER]; - } - - //control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR] = (int)control_station.raw_array_data[cc][0].all; - //control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER] = my_satur_int(control_station.raw_array_data[cc][2].all, MAX_ZADANIE_POWER, MIN_ZADANIE_POWER); - - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_KM] = control_station.raw_array_data[cc][1].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_IZAD] = my_satur_int(control_station.raw_array_data[cc][3].all, MAX_ZADANIE_I_M, 0, 0); - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_U_ZARYAD] = control_station.raw_array_data[cc][4].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_K_U_DISBALANCE] = control_station.raw_array_data[cc][5].all; - control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_K_PLUS_U_DISBALANCE] = control_station.raw_array_data[cc][6].all; - - - - if (control_station.raw_array_data[cc][8].all > 0 && control_station.raw_array_data[cc][8].all <= 600 && edrk.MasterSlave == MODE_MASTER) { - vect_control.iqId_min = _IQ((float)control_station.raw_array_data[cc][8].all / NORMA_ACP); - } - - fint = (int)control_station.raw_array_data[cc][9].all; - if (fint>=-360 && fint<=360 && edrk.MasterSlave == MODE_MASTER) - { - vect_control.add_tetta = _IQ(fint / 180.0 * PI); - } - else - vect_control.add_tetta = 0; - - fint = (int)control_station.raw_array_data[cc][10].all; - if (fint>=0) - { - edrk.t_slow_log = fint; - } - else - edrk.t_slow_log = 0; - - fint = (int)control_station.raw_array_data[cc][11].all; - if (fint) - { - simple_scalar1.add_bpsi = _IQ(fint/1000.0/NORMA_FROTOR);//_IQ(0.05/NORMA_FROTOR); - } - else - simple_scalar1.add_bpsi = 0; - - fint = (int)control_station.raw_array_data[cc][12].all; - if (fint) - { - simple_scalar1.add_power_limit = _IQ(fint*1000.0/(NORMA_MZZ*NORMA_MZZ));//_IQ(0.05/NORMA_FROTOR); - } - else - simple_scalar1.add_power_limit = 0; - - - fint = (int)control_station.raw_array_data[cc][13].all; - if (fint) - { - simple_scalar1.sdvig_power_limit = _IQ(fint*1000.0/(NORMA_MZZ*NORMA_MZZ));//_IQ(0.05/NORMA_FROTOR); - } - else - simple_scalar1.sdvig_power_limit = 0; - - -//#if (_FLOOR6) - fint = (int)control_station.raw_array_data[cc][14].all; - if (fint>=0 && fint<=3200) - { - analog.iqU_1_imit = _IQ(fint/NORMA_ACP); - } - else - analog.iqU_1_imit = 0; -//#else -// analog.iqU_1_imit = 0; -//#endif - - prev_checkback = control_station.array_cmd[cc][CONTROL_STATION_CMD_CHECKBACK];//control_station.raw_array_data[cc][pos_numbercmd].bits.bit1; - // edrk.KvitirRS = 1; - - control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.raw_array_data[cc][pos_numbercmd].bits.bit5; - -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_ROTOR_POWER]==0) -// { -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]!=0) -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.raw_array_data[cc][pos_numbercmd].bits.bit5; -// else -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 0; -// } -// else -// { -// if (control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_POWER]!=0) -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = control_station.raw_array_data[cc][pos_numbercmd].bits.bit5; -// else -// control_station.array_cmd[cc][CONTROL_STATION_CMD_GO] = 0; -// } - - -/* - edrk.from_rs.bits.ACTIVE = control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]; - - - if (edrk.from_rs.bits.ACTIVE==0) - return; - - edrk.from_can.bits.ACTIVE = 0; - -*/ - - - - if (control_station.array_cmd[cc][CONTROL_STATION_CMD_ACTIVE_CONTROL]==0) - return; - - // CAN RS232 - if (control_station.array_cmd[CONTROL_STATION_TERMINAL_RS232][CONTROL_STATION_CMD_ACTIVE_CONTROL] && cc==CONTROL_STATION_TERMINAL_CAN) - return; - - - - - -// edrk.DirectOUT = control_station.raw_array_data[cc][pos_numbercmd].bits.bit7; - -// edrk.DirectNagrevOff = control_station.raw_array_data[cc][pos_numbercmd].bits.bit8; -// edrk.DirectBlockKeyOff = control_station.raw_array_data[cc][pos_numbercmd].bits.bit9; -//// edrk.DirectPumpON = control_station.raw_array_data[cc][pos_numbercmd].bits.bit10; -// edrk.DirectZaryadOn = control_station.raw_array_data[cc][pos_numbercmd].bits.bit11; - - -// edrk.SetSpeed = control_station.raw_array_data[cc][pos_numbercmd].bits.bit14; - - - - - - //////////////////////////// - ///////////////////////// - - -} - -////////////////////////////////////////////////////// -////////////////////////////////////////////////////// -////////////////////////////////////////////////////// -void parse_data_from_master_to_alg(void) -{ - - if (edrk.MasterSlave==MODE_SLAVE) - { - control_station.active_array_cmd[CONTROL_STATION_CMD_SET_ROTOR] = control_station.array_cmd[CONTROL_STATION_ANOTHER_BS][CONTROL_STATION_CMD_SET_ROTOR]; - control_station.active_array_cmd[CONTROL_STATION_CMD_SET_KM] = control_station.array_cmd[CONTROL_STATION_ANOTHER_BS][CONTROL_STATION_CMD_SET_KM]; - control_station.active_array_cmd[CONTROL_STATION_CMD_SET_IZAD] = control_station.array_cmd[CONTROL_STATION_ANOTHER_BS][CONTROL_STATION_CMD_SET_IZAD]; - control_station.active_array_cmd[CONTROL_STATION_CMD_SET_POWER] = control_station.array_cmd[CONTROL_STATION_ANOTHER_BS][CONTROL_STATION_CMD_SET_POWER]; - } - -} -////////////////////////////////////////////////////// -////////////////////////////////////////////////////// -////////////////////////////////////////////////////// -////////////////////////////////////////////////////// -////////////////////////////////////////////////////// -////////////////////////////////////////////////////// - -void parse_analog_data_from_active_control_station_to_alg(void) -{ - float ff, ff1; - _iq _iq_ff; - int i; - -// edrk.disable_interrupt_sync = control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_INTERRUPT_SYNC]; -// edrk.disable_interrupt_timer2 = control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_INTERRUPT_TIMER2]; - - edrk.NoDetectUZeroDischarge = control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_QTV]; - - // if (current_active_control==CONTROL_STATION_TERMINAL_RS232) - edrk.StartGEDfromControl = control_station.active_array_cmd[CONTROL_STATION_CMD_GO]; - - - -////////////////// - if (control_station.active_array_cmd[CONTROL_STATION_CMD_UFCONST_VECTOR]==0) - edrk.Mode_ScalarVectorUFConst = ALG_MODE_UF_CONST; - else - if (control_station.active_array_cmd[CONTROL_STATION_CMD_SCALAR_FOC]==0) - { - if (control_station.active_array_cmd[CONTROL_STATION_CMD_ROTOR_POWER]==0) - edrk.Mode_ScalarVectorUFConst = ALG_MODE_SCALAR_OBOROTS; - else - edrk.Mode_ScalarVectorUFConst = ALG_MODE_SCALAR_POWER; - } - else - { - if (control_station.active_array_cmd[CONTROL_STATION_CMD_ROTOR_POWER]==0) - edrk.Mode_ScalarVectorUFConst = ALG_MODE_FOC_OBOROTS; - else - edrk.Mode_ScalarVectorUFConst = ALG_MODE_FOC_POWER; - } - - - ////////////////////////////////////////////////////// -// edrk.W_from_RS = control_station.array_cmd[cc][CONTROL_STATION_CMD_SET_ROTOR]; //DataAnalog1; -// ff = control_station.active_array_cmd[CONTROL_STATION_CMD_SET_I_VOZBUD];//0;//DataAnalog4; -// edrk.I_zad_vozb_add_from_RS = my_satur_float(ff,MAX_ZADANIE_I_VOZBUD,0); - - - ff = control_station.active_array_cmd[CONTROL_STATION_CMD_SET_U_ZARYAD]; - - edrk.zadanie.ZadanieU_Charge = my_satur_float(ff,MAX_ZADANIE_U_CHARGE,0,0); - edrk.zadanie.iq_ZadanieU_Charge = _IQ(edrk.zadanie.ZadanieU_Charge/NORMA_ACP); - - - if (edrk.Status_Ready.bits.ready_final==0) - { - // , - for (i=0;idigit_data.Byte01.bit_data.bit0 && flag_wait_revers_go) - edrk.StartGEDRS = 0; - if (pcommand->digit_data.Byte01.bit_data.bit0==0) - edrk.StartGEDRS = 0; - if (pcommand->digit_data.Byte01.bit_data.bit0==0 && flag_wait_revers_go) - flag_wait_revers_go = 0; - if (pcommand->digit_data.Byte01.bit_data.bit0==1 && flag_wait_revers_go==0) - edrk.StartGEDRS = 1; - -// edrk.StartGEDRS = pcommand->digit_data.Byte01.bit_data.bit0; -*/ -// end StartGED - - - - -//////////////// - - - - -// edrk.from_rs.bits.RAZBOR_SHEMA = pcommand->digit_data.Byte01.bit_data.bit5; - - - -// SBOR SHEMA -/* - if (edrk.summ_errors) - { - flag_wait_revers_sbor = 1; - } - - if (flag_wait_revers_sbor==1) - edrk.from_can.bits.SBOR_SHEMA = 0; - - if (pcommand->digit_data.Byte01.bit_data.bit4 && flag_wait_revers_sbor) - edrk.from_can.bits.SBOR_SHEMA = 0; - - if (pcommand->digit_data.Byte01.bit_data.bit4==0) - edrk.from_can.bits.SBOR_SHEMA = 0; - - if (pcommand->digit_data.Byte01.bit_data.bit4==0 && flag_wait_revers_sbor) - flag_wait_revers_sbor = 0; - - if (pcommand->digit_data.Byte01.bit_data.bit4==1 && flag_wait_revers_sbor==0) - edrk.from_can.bits.SBOR_SHEMA = pcommand->digit_data.Byte01.bit_data.bit4; - - prev_byte01_bit4 = pcommand->digit_data.Byte01.bit_data.bit4; -*/ -// end SBOR SHEMA - - - - - -// if (edrk.from_rs.bits.RAZBOR_SHEMA) - // edrk.from_rs.bits.SBOR_SHEMA = 0; - - //edrk.SborRS = pcommand->digit_data.Byte01.bit_data.bit4; - - -// edrk.to_shema.bits.QTV_ON = pcommand->digit_data.Byte02.bit_data.bit3; - - - - -// edrk.RemouteFromRS = pcommand->digit_data.Byte01.bit_data.bit3; - - - - -// edrk.VozbudOnOffFromRS = pcommand->digit_data.Byte01.bit_data.bit1; -// edrk.enable_set_vozbud = pcommand->digit_data.Byte01.bit_data.bit1; -// edrk.SborRS = pcommand->digit_data.Byte01.bit_data.bit2; -// edrk.RazborRS = pcommand->digit_data.Byte01.bit_data.bit3; -// edrk.DirectOUT = pcommand->digit_data.Byte01.bit_data.bit4; - -// edrk.StartGED = pcommand->digit_data.Byte01.bit_data.bit6; - - -// f.flag_distance = pcommand->digit_data.Byte01.bit_data.bit6; -// f.Set_power = pcommand->digit_data.Byte01.bit_data.bit7; - - - // f.Down50 = pcommand->digit_data.Byte02.bit_data.bit2; -// f.Up50 = pcommand->digit_data.Byte02.bit_data.bit3; -// f.Ciclelog = pcommand->digit_data.Byte02.bit_data.bit4; - - // if (SPEED_SELECT_ZADAT==1) -// f.Provorot = pcommand->digit_data.Byte02.bit_data.bit5; - - - - - - - - - - - - - -} - - - -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void parse_parameters_from_all_control_station(void) -{ - - parse_parameters_from_one_control_station_terminal_rs232(CONTROL_STATION_TERMINAL_RS232); - parse_parameters_from_one_control_station_terminal_rs232(CONTROL_STATION_TERMINAL_CAN); - load_parameters_from_can_control_station_to_rs232(); - - if (edrk.get_new_data_from_hmi==1) // ? - { - func_unpack_answer_from_Ingeteam(CONTROL_STATION_INGETEAM_PULT_RS485); - parse_parameters_from_one_control_station_pult_ingeteam(CONTROL_STATION_INGETEAM_PULT_RS485); - edrk.get_new_data_from_hmi = 2; // modbus - } - parse_parameters_from_one_control_station_pult_zadat4ik(CONTROL_STATION_ZADATCHIK_CAN); - parse_parameters_from_one_control_station_pult_vpu(CONTROL_STATION_VPU_CAN); - parse_parameters_from_one_control_station_another_bs(CONTROL_STATION_ANOTHER_BS); -// unpack_answer_from_MPU_SVU_CAN(CONTROL_STATION_MPU_SVU_CAN); - unpack_answer_from_MPU_SVU_CAN(CONTROL_STATION_MPU_KEY_CAN); //4 - unpack_answer_from_MPU_SVU_CAN(CONTROL_STATION_MPU_SVU_CAN); //3 -// unpack_answer_from_MPU_SVU_RS(CONTROL_STATION_MPU_SVU_RS485); - parse_parameters_from_one_control_station_MPU_SVU(CONTROL_STATION_MPU_SVU_CAN); //3 - parse_parameters_from_one_control_station_MPU_SVU(CONTROL_STATION_MPU_KEY_CAN); //4 -// parse_parameters_from_one_control_station_MPU_SVU(CONTROL_STATION_MPU_SVU_RS485); - - -} - -///////////////////////////////////////////////////////////// -// -///////////////////////////////////////////////////////////// -void load_parameters_from_active_control_station(int current_control) -{ - int i; - - if (current_control>=CONTROL_STATION_LAST || current_control<0 ) - { - // - ! ! - for (i=0;i0 - CONTROL_STATION_CMD_SET_K_PLUS_U_DISBALANCE, //kplus_u_disbalance, =0, , . , <>0 . - CONTROL_STATION_CMD_MODE_PUMP, // // // 0 - auto on - rand pump - // 1 - auto on 1 pump - // 2 - auto on 2 pump - // 3 - manual on 1 pump - // 4 - manual on 2 pump - // 5- manual - CONTROL_STATION_CMD_DISABLE_ON_PUMP, - CONTROL_STATION_CMD_ENABLE_ON_CHARGE, - CONTROL_STATION_CMD_DISABLE_ON_QTV, - CONTROL_STATION_CMD_MANUAL_DISCHARGE, - CONTROL_STATION_CMD_DISABLE_ON_UMP, - CONTROL_STATION_CMD_WDOG_OFF, - CONTROL_STATION_CMD_SET_LIMIT_POWER,// - CONTROL_STATION_CMD_BLOCK_BS, // - CONTROL_STATION_CMD_DISABLE_INTERRUPT_SYNC, - CONTROL_STATION_CMD_DISABLE_INTERRUPT_TIMER2, - CONTROL_STATION_CMD_DISABLE_RASCEPITEL, // , - CONTROL_STATION_CMD_PWM_TEST_LINES, // 96 , !!! - CONTROL_STATION_CMD_STOP_LOGS, // - CONTROL_STATION_CMD_LAST // , , , . -}; - - -void control_station_test_alive_all_control(void); -int control_station_select_active(void); -int get_current_station_control(void); -void load_parameters_from_active_control_station(int current_control); -void parse_parameters_from_all_control_station(void); -void parse_parameters_from_one_control_station_terminal_rs232(int cc); -void parse_parameters_from_one_control_station_pult_ingeteam(int cc); -void parse_parameters_from_one_control_station_pult_zadat4ik(int cc); -void parse_parameters_from_one_control_station_pult_vpu(int cc); -void parse_parameters_from_one_control_station_another_bs(int cc); -void parse_parameters_from_one_control_station_MPU_SVU(int cc); - -void parse_analog_data_from_active_control_station_to_alg(void); -void parse_data_from_master_to_alg(void); - -void load_parameters_from_can_control_station_to_rs232(void); - - -#endif /* SRC_MAIN_CONTROL_STATION_PROJECT_H_ */ diff --git a/Inu/Src2/551/main/detect_error_3_phase.c b/Inu/Src2/551/main/detect_error_3_phase.c deleted file mode 100644 index 9b6fd87..0000000 --- a/Inu/Src2/551/main/detect_error_3_phase.c +++ /dev/null @@ -1,167 +0,0 @@ -/* - * detect_error_3_phase.c - * - * Created on: 7 . 2020 . - * Author: star - */ - -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "IQmathLib.h" - - -#include -#include - -#include "global_time.h" - -static int detect_system_asymmetry(DETECT_PROTECT_3_PHASE *v); -static int detect_system_asymmetry_rms(DETECT_PROTECT_3_PHASE *v); - -int detect_error_3_phase(DETECT_PROTECT_3_PHASE *v) { - int err = 0; - int asymmetry = 0; - int break_channel = 0; - - v->errors.all = 0; - v->over_limit.all = 0; - if (v->setup.timers_inited == 0) { - v->setup.timers_inited = 1; - init_timer_milisec(&v->timer_low_minus_10); - init_timer_milisec(&v->timer_low_minus_20); - init_timer_milisec(&v->timer_high_plus_10); - init_timer_milisec(&v->timer_high_plus_20); - } - - if (v->setup.use.bits.phase_U != 0 && - v->iqVal_U > v->setup.levels.iqVal_U_max) { - v->errors.bits.phase_U_max = 1; - v->over_limit.bits.phase_U_max = 1; - err = 1; - } - if (v->setup.use.bits.phase_V != 0 && - v->iqVal_V > v->setup.levels.iqVal_V_max) { - v->errors.bits.phase_V_max = 1; - v->over_limit.bits.phase_V_max = 1; - err = 1; - } - if (v->setup.use.bits.phase_W != 0 && - v->iqVal_W > v->setup.levels.iqVal_W_max) { - v->errors.bits.phase_W_max = 1; - v->over_limit.bits.phase_W_max = 1; - err = 1; - } - //-------------------------------------------------------------------------// - if (v->setup.use.bits.module) { - if (v->iqVal_mod > v->setup.levels.iqVal_module_max) { - v->errors.bits.module_max = 1; - v->over_limit.bits.module_max = 1; - err = 1; - } - } - //-------------------------------------------------------------------------// - if (v->setup.use.bits.detect_minus_10 != 0) { - if (v->iqVal_mod < v->setup.levels.iqNominal_minus10) { - if (detect_pause_milisec(PAUSE_VAL_MINIS_10_S, &v->timer_low_minus_10)){ - v->errors.bits.module_10_percent_low = 1; - err = 1; - } - v->over_limit.bits.module_10_percent_low = 1; - } else { - init_timer_milisec(&v->timer_low_minus_10); - } - v->new_timer_low_minus_10 = v->timer_low_minus_10; - } - - if (v->setup.use.bits.detect_minus_20 != 0) { - if (v->iqVal_mod < v->setup.levels.iqNominal_minus20) { - if (detect_pause_milisec(PAUSE_VAL_MINIS_20_S, &v->timer_low_minus_20)) { - v->errors.bits.module_20_percent_low = 1; - err = 1; - } - v->over_limit.bits.module_20_percent_low = 1; - } else { - init_timer_milisec(&v->timer_low_minus_20); - } - v->new_timer_low_minus_20 = v->timer_low_minus_20; - } - - if (v->setup.use.bits.detect_plus_10 != 0) { - if (v->iqVal_mod > v->setup.levels.iqNominal_plus10) { - if (detect_pause_milisec(PAUSE_VAL_PLUS_10_S, &v->timer_high_plus_10)) { - v->errors.bits.module_10_percent_hi = 1; - err = 1; - } - v->over_limit.bits.module_10_percent_hi = 1; - } else { - init_timer_milisec(&v->timer_high_plus_10); - } - v->new_timer_high_plus_10 = v->timer_high_plus_10; - } - - if (v->setup.use.bits.detect_plus_20 != 0) { - if (v->iqVal_mod > v->setup.levels.iqNominal_plus20) { - if (detect_pause_milisec(PAUSE_VAL_PLUS_20_S, &v->timer_high_plus_20)) { - v->errors.bits.module_20_percent_hi = 1; - err = 1; - } - v->over_limit.bits.module_20_percent_hi = 1; - } else { - init_timer_milisec(&v->timer_high_plus_20); - } - v->new_timer_high_plus_20 = v->timer_high_plus_20; - } - // 3- . 3- 0 - if (v->setup.use.bits.system_asymmetry_by_summ != 0) { - asymmetry = detect_system_asymmetry(v); - if (asymmetry != 0) { - v->errors.bits.system_asymmetry = 1; - err = 1; - } - } - // 3- . 3- - if (v->setup.use.bits.system_asymmetry_by_delta != 0) { - asymmetry = detect_system_asymmetry_rms(v); - if (asymmetry != 0) { - v->errors.bits.system_asymmetry = 1; - err = 1; - } - } - // . , - if (v->setup.use.bits.break_phase != 0 && v->break_phase != 0) { - v->break_phase->iqIu = v->iqVal_U; - v->break_phase->iqIv = v->iqVal_V; - v->break_phase->iqIw = v->iqVal_W; - v->break_phase->iqImod = v->iqVal_mod; - v->break_phase->teta = v->iqTeta; - break_channel = v->break_phase->calc(v->break_phase); - if (break_channel) { - v->errors.bits.break_phase = 1; - err = 1; - switch (break_channel) { - case 1: v->errors.bits.break_phase_U = 1; break; - case 2: v->errors.bits.break_phase_V = 1; break; - case 3: v->errors.bits.break_phase_W = 1; break; - default: break; - } - } - } - - - return err; -} - -int detect_system_asymmetry(DETECT_PROTECT_3_PHASE *v) { - _iq sum = v->iqVal_U + v->iqVal_V + v->iqVal_W; - return _IQabs(sum) > v->setup.levels.iqAsymmetry_delta ? 1 : 0; -} - -int detect_system_asymmetry_rms(DETECT_PROTECT_3_PHASE *v) { - _iq d1 = _IQabs(v->iqVal_U - v->iqVal_V); - _iq d2 = _IQabs(v->iqVal_V - v->iqVal_W); - _iq d3 = _IQabs(v->iqVal_U - v->iqVal_W); - return d1 > v->setup.levels.iqAsymmetry_delta || - d2 > v->setup.levels.iqAsymmetry_delta || - d3 > v->setup.levels.iqAsymmetry_delta ? 1 : 0; -} - diff --git a/Inu/Src2/551/main/detect_error_3_phase.h b/Inu/Src2/551/main/detect_error_3_phase.h deleted file mode 100644 index bdb7307..0000000 --- a/Inu/Src2/551/main/detect_error_3_phase.h +++ /dev/null @@ -1,148 +0,0 @@ -/* - * detect_error_3_phase.h - * - * Created on: 7 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_DETECT_ERROR_3_PHASE_H_ -#define SRC_MAIN_DETECT_ERROR_3_PHASE_H_ - -#include - -typedef struct { - _iq iqVal_module_max; - _iq iqVal_U_max; - _iq iqVal_V_max; - _iq iqVal_W_max; - _iq iqNominal_plus10; - _iq iqNominal_plus20; - _iq iqNominal_minus10; - - _iq iqNominal_minus20; - _iq iqAsymmetry_delta; -} PROTECT_LEVELS_3_PHASE; - -#define PROTECT_LEVELS_3_PHASE_DEFAULTS {0,0,0,0,0,0,0, 0,0} - -typedef struct { - PROTECT_LEVELS_3_PHASE levels; - union { - unsigned int all; - struct { - unsigned int phase_U :1; - unsigned int phase_V :1; - unsigned int phase_W :1; - unsigned int module :1; - - unsigned int detect_minus_10 :1; - unsigned int detect_minus_20 :1; - unsigned int detect_plus_10 :1; - unsigned int detect_plus_20 :1; - - unsigned int system_asymmetry_by_summ :1; // 3- 0 - unsigned int system_asymmetry_by_delta :1; // 3- - unsigned int break_phase :1; - unsigned int reserved :5; - } bits; - } use; - unsigned int timers_inited; -} SETUP_3_PHASE_PROTECT; - -#define SETUP_3_PHASE_PROTECT_DEFAULTS {PROTECT_LEVELS_3_PHASE_DEFAULTS, {0}, 0} - -typedef struct { - //In values - _iq iqVal_U; - _iq iqVal_V; - _iq iqVal_W; - _iq iqVal_mod; - _iq iqTeta; // - - unsigned int timer_low_minus_10; - unsigned int timer_low_minus_20; - unsigned int timer_high_plus_10; - unsigned int timer_high_plus_20; - - //Break phase I state values - BREAK_PHASE_I *break_phase; - - //Out values - union { - unsigned int all; - struct { - unsigned int phase_U_max :1; - unsigned int phase_V_max :1; - unsigned int phase_W_max :1; - unsigned int module_max :1; - unsigned int module_10_percent_hi :1; - unsigned int module_20_percent_hi :1; - unsigned int module_10_percent_low :1; - unsigned int module_20_percent_low :1; - - unsigned int system_asymmetry :1; - unsigned int break_phase :1; - unsigned int break_phase_U :1; - unsigned int break_phase_V :1; - unsigned int break_phase_W :1; - unsigned int reserved :3; - - } bits; - } errors; - union { - unsigned int all; - struct { - unsigned int phase_U_max :1; - unsigned int phase_V_max :1; - unsigned int phase_W_max :1; - unsigned int module_max :1; - unsigned int module_10_percent_hi :1; - unsigned int module_20_percent_hi :1; - unsigned int module_10_percent_low :1; - unsigned int module_20_percent_low :1; - - unsigned int system_asymmetry_by_summ :1; - unsigned int break_phase :1; - unsigned int break_phase_U :1; - unsigned int break_phase_V :1; - unsigned int break_phase_W :1; - unsigned int reserved :3; - - } bits; - } over_limit; - unsigned int new_timer_low_minus_10; - unsigned int new_timer_low_minus_20; - unsigned int new_timer_high_plus_10; - unsigned int new_timer_high_plus_20; - - //Setup - SETUP_3_PHASE_PROTECT setup; - - int (*calc_detect_error_3_phase)(); - -} DETECT_PROTECT_3_PHASE; - -#define DETECT_PROTECT_3_PHASE_DEFAULTS {0,0,0,0,0, 0,0,0,0, 0, {0},{0}, 0,0,0,0,\ - SETUP_3_PHASE_PROTECT_DEFAULTS, \ - detect_error_3_phase} - - -#define ADC_PROTECT_LEVELS_DEFAULT {0,0,0,0, 0,0,0,0, 0} - -#define PAUSE_VAL_MINIS_10_S 10000 -#define PAUSE_VAL_MINIS_20_S 1000 -#define PAUSE_VAL_PLUS_10_S 10000 -#define PAUSE_VAL_PLUS_20_S 1000 - -#define PLUS_10_PERCENT 1.1 -#define PLUS_20_PERCENT 1.2 -#define MINUS_10_PERCENT 0.9 -#define MINUS_20_PERCENT 0.8 -#define ASYMMETRY_DELTA_PERCENTS 0.2 - -int detect_error_3_phase(DETECT_PROTECT_3_PHASE *v); - - - - -#endif /* SRC_MAIN_DETECT_ERROR_3_PHASE_H_ */ diff --git a/Inu/Src2/551/main/detect_errors.c b/Inu/Src2/551/main/detect_errors.c deleted file mode 100644 index 07ff17c..0000000 --- a/Inu/Src2/551/main/detect_errors.c +++ /dev/null @@ -1,1606 +0,0 @@ -/* - * detect_errors.c - * - * Created on: 4 . 2020 . - * Author: star - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "v_rotor.h" - - -#include "control_station.h" -#include "DSP281x_Device.h" -#include "master_slave.h" -#include "another_bs.h" -#include "digital_filters.h" - - -void detect_error_from_knopka_avaria(void); -void detect_error_ute4ka_water(void); -void detect_error_t_vipr(void); -void detect_error_power_upc(void); -void detect_error_op_pit(void); -void detect_error_p_water(void); -void detect_error_pump_2(void); -void detect_error_pump_1(void); -void detect_error_pre_ready_pump(void); -void detect_error_fan(void); -void detect_error_block_qtv_from_svu(void); - -void detect_error_predohr_vipr(void); -void detect_error_qtv(void); -void detect_error_pre_charge(void); -void detect_error_block_izol(void); -void detect_error_nagrev(void); -void detect_error_ground(void); -void detect_error_block_door(void); -void detect_error_optical_bus(void); -void detect_error_sync_bus(void); -int get_status_temper_acdrive_winding(int nc); -int get_status_temper_acdrive_winding_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_acdrive_bear(int nc); -int get_status_temper_acdrive_bear_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_air(int nc); -int get_status_temper_air_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_u(int nc); -int get_status_temper_u_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_water(int nc); -int get_status_p_water_max(void); -int get_status_p_water_min(int pump_on_off); -void detect_error_t_water(void); -void detect_error_t_air(void); -void detect_error_t_u(void); -void detect_error_acdrive_winding(void); - -int get_common_state_warning(void); -int get_common_state_overheat(void); -void detect_error_sensor_rotor(void); - - - - -#pragma DATA_SECTION(protect_levels,".slow_vars"); -PROTECT_LEVELS protect_levels = PROTECT_LEVELS_DEFAULTS; - - - - -/////////////////////////////////////////////// -int get_status_temper_acdrive_winding(int nc) -{ - if (edrk.temper_acdrive.winding.filter_real_int_temper[nc]>=ALARM_TEMPER_ACDRIVE_WINDING) - return 4; - if (edrk.temper_acdrive.winding.filter_real_int_temper[nc]>=ABNORMAL_TEMPER_ACDRIVE_WINDING) - return 2; - return 1; -} - -int get_status_temper_acdrive_winding_with_limits(int nc, int alarm, int abnormal) -{ - if (edrk.temper_acdrive.winding.filter_real_int_temper[nc]>=alarm) - return 4; - if (edrk.temper_acdrive.winding.filter_real_int_temper[nc]>=abnormal) - return 2; - return 1; -} -/////////////////////////////////////////////// -int get_status_temper_acdrive_bear(int nc) -{ - - if (edrk.temper_acdrive.bear.filter_real_int_temper[nc]>=ALARM_TEMPER_ACDRIVE_WINDING) - return 4; - if (edrk.temper_acdrive.bear.filter_real_int_temper[nc]>=ABNORMAL_TEMPER_ACDRIVE_WINDING) - return 2; - return 1; -} - -int get_status_temper_acdrive_bear_with_limits(int nc, int alarm, int abnormal) -{ - - if (edrk.temper_acdrive.bear.filter_real_int_temper[nc]>=alarm) - return 4; - if (edrk.temper_acdrive.bear.filter_real_int_temper[nc]>=abnormal) - return 2; - return 1; -} - -/////////////////////////////////////////////// -int get_status_temper_air(int nc) -{ - if (edrk.temper_edrk.real_int_temper_air[nc]>=ALARM_TEMPER_AIR_INT) - return 4; - if (edrk.temper_edrk.real_int_temper_air[nc]>=ABNORMAL_TEMPER_AIR_INT) - return 2; - return 1; -} - -int get_status_temper_air_with_limits(int nc, int alarm, int abnormal) -{ - if (edrk.temper_edrk.real_int_temper_air[nc]>=alarm) - return 4; - if (edrk.temper_edrk.real_int_temper_air[nc]>=abnormal) - return 2; - return 1; -} - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -int get_status_temper_u(int nc) -{ - if (edrk.temper_edrk.real_int_temper_u[nc]>=ALARM_TEMPER_AF) - return 4; - if (edrk.temper_edrk.real_int_temper_u[nc]>=ABNORMAL_TEMPER_AF) - return 2; - return 1; -} - -int get_status_temper_u_with_limits(int nc, int alarm, int abnormal) -{ - if (edrk.temper_edrk.real_int_temper_u[nc]>=alarm) - return 4; - if (edrk.temper_edrk.real_int_temper_u[nc]>=abnormal) - return 2; - return 1; -} - -/////////////////////////////////////////////// -int get_status_temper_water(int nc) -{ - - if (nc==INDEX_T_WATER_EXT) // ext - { - if (edrk.temper_edrk.real_int_temper_water[nc]>=protect_levels.alarm_temper_water_ext) - return 4; - if (edrk.temper_edrk.real_int_temper_water[nc]>=protect_levels.abnormal_temper_water_ext) - return 2; - return 1; - } - - if (nc==INDEX_T_WATER_INT) // int - { - if (edrk.temper_edrk.real_int_temper_water[nc]>=protect_levels.alarm_temper_water_int) - return 4; - if (edrk.temper_edrk.real_int_temper_water[nc]>=protect_levels.abnormal_temper_water_int) - return 2; - return 1; - } - - return 0; -} - -/////////////////////////////////////////////// -int get_status_p_water_max(void) -{ - if (edrk.p_water_edrk.filter_real_int_p_water[0]>=protect_levels.alarm_p_water_max_int) - return 4; - if (edrk.p_water_edrk.filter_real_int_p_water[0]>=protect_levels.abnormal_p_water_max_int) - return 2; - return 1; -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// -int get_status_p_water_min(int pump_on_off) -{ - if (pump_on_off == 0) - { - if (edrk.p_water_edrk.filter_real_int_p_water[0]<=ALARM_P_WATER_MIN_INT_ON_OFF_PUMP) - return 4; - if (edrk.p_water_edrk.filter_real_int_p_water[0]<=ABNORMAL_P_WATER_MIN_INT_ON_OFF_PUMP) - return 2; - return 1; - } - - if (pump_on_off == 1) - { - if (edrk.p_water_edrk.filter_real_int_p_water[0]<=protect_levels.alarm_p_water_min_int) - return 4; - if (edrk.p_water_edrk.filter_real_int_p_water[0]<=protect_levels.abnormal_p_water_min_int) - return 2; - return 1; - } - return 0; -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// -void detect_error_sensor_rotor(void) -{ - static unsigned int count_err1 = 0, count_err2 = 0, count_err3 = 0, count_err4 = 0; - - - if (edrk.Go) - { - // ? - if (edrk.iq_f_rotor_hz==0) - { - // ! - if (pause_detect_error(&count_err3,TIME_WAIT_SENSOR_ROTOR_BREAK_ALL,1)) - { - edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 1; - edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 1; - //edrk.errors.e9.bits.SENSOR_ROTOR_1_2_BREAK |= 1; // ! - } - else - { -// edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 0; -// edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 0; - edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = pause_detect_error(&count_err1,TIME_WAIT_SENSOR_ROTOR_BREAK_ONE_SENSOR, - inc_sensor.break_sensor1); - edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = pause_detect_error(&count_err2,TIME_WAIT_SENSOR_ROTOR_BREAK_ONE_SENSOR, - inc_sensor.break_sensor2); - } - } - else - { - count_err3 = 0; -// edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 0; -// edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 0; - edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK |= pause_detect_error(&count_err1,TIME_WAIT_ERROR,inc_sensor.break_sensor1); - edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK |= pause_detect_error(&count_err2,TIME_WAIT_ERROR,inc_sensor.break_sensor2); - - } - } - else - { - - count_err1 = count_err2 = 0; - count_err3 = 0; - - edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 0; - edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 0; - } - -} -/////////////////////////////////////////////// -#define TIME_WAIT_T_WATER 30 -void detect_error_t_water(void) -{ - static unsigned int count_run = 0, count_run_static = 0; - int status; - - status = get_status_temper_water(INDEX_T_WATER_INT); - if (status==4) - edrk.errors.e2.bits.T_WATER_INT_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_WATER_INT_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_WATER_INT_MAX = 0; - - status = get_status_temper_water(INDEX_T_WATER_EXT); - if (status==4) - edrk.errors.e2.bits.T_WATER_EXT_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_WATER_EXT_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_WATER_EXT_MAX = 0; - -} -/////////////////////////////////////////////// - -void detect_error_t_air(void) -{ - static unsigned int count_run = 0, count_run_static = 0; - int status,i; - - - status = get_status_temper_air_with_limits(0, protect_levels.alarm_temper_air_int_01, - protect_levels.abnormal_temper_air_int_01); - if (status==4) - edrk.errors.e2.bits.T_AIR0_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_AIR0_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_AIR0_MAX = 0; - - - status = get_status_temper_air_with_limits(1, protect_levels.alarm_temper_air_int_02, - protect_levels.abnormal_temper_air_int_02); - if (status==4) - edrk.errors.e2.bits.T_AIR1_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_AIR1_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_AIR1_MAX = 0; - - - status = get_status_temper_air_with_limits(2, protect_levels.alarm_temper_air_int_03, - protect_levels.abnormal_temper_air_int_03); - if (status==4) - edrk.errors.e2.bits.T_AIR2_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_AIR2_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_AIR2_MAX = 0; - - - status = get_status_temper_air_with_limits(3, protect_levels.alarm_temper_air_int_04, - protect_levels.abnormal_temper_air_int_04); - if (status==4) - edrk.errors.e2.bits.T_AIR3_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_AIR3_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_AIR3_MAX = 0; -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// - -void detect_error_t_u(void) -{ - static unsigned int count_run = 0, count_run_static = 0; - int status,i; - - - status = get_status_temper_u_with_limits(0, protect_levels.alarm_temper_u_01, - protect_levels.abnormal_temper_u_01); - if (status == 4) - edrk.errors.e2.bits.T_UO1_MAX |= 1; - if (status == 2) - edrk.warnings.e2.bits.T_UO1_MAX = 1; - if (status == 1) - edrk.warnings.e2.bits.T_UO1_MAX = 0; - - status = get_status_temper_u_with_limits(1, protect_levels.alarm_temper_u_02, - protect_levels.abnormal_temper_u_02); - if (status==4) - edrk.errors.e2.bits.T_UO2_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_UO2_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_UO2_MAX = 0; - - status = get_status_temper_u_with_limits(2, protect_levels.alarm_temper_u_03, - protect_levels.abnormal_temper_u_03); - if (status==4) - edrk.errors.e2.bits.T_UO3_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_UO3_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_UO3_MAX = 0; - - status = get_status_temper_u_with_limits(3, protect_levels.alarm_temper_u_04, - protect_levels.abnormal_temper_u_04); - if (status==4) - edrk.errors.e2.bits.T_UO4_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_UO4_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_UO4_MAX = 0; - - status = get_status_temper_u_with_limits(4, protect_levels.alarm_temper_u_05, - protect_levels.abnormal_temper_u_05); - if (status==4) - edrk.errors.e2.bits.T_UO5_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_UO5_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_UO5_MAX = 0; - - status = get_status_temper_u_with_limits(5, protect_levels.alarm_temper_u_06, - protect_levels.abnormal_temper_u_06); - if (status==4) - edrk.errors.e2.bits.T_UO6_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_UO6_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_UO6_MAX = 0; - - status = get_status_temper_u_with_limits(6, protect_levels.alarm_temper_u_07, - protect_levels.abnormal_temper_u_07); - if (status==4) - edrk.errors.e2.bits.T_UO7_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.T_UO7_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.T_UO7_MAX = 0; - -} - -void detect_error_t_bsu(void) { - -} -/////////////////////////////////////////////// -void detect_error_acdrive_winding(void) -{ -// static unsigned int count_run = 0, count_run_static = 0; - int status, i; - - status = 0; - status |= get_status_temper_acdrive_winding_with_limits( - 0, protect_levels.alarm_temper_acdrive_winding_U1, - protect_levels.abnormal_temper_acdrive_winding_U1); - if (status == 4) { - edrk.errors.e10.bits.T_ACDRIVE_WINDING_U1 = 1; - } - if (status == 2) { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U1 = 1; - } else { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U1 = 0; - } - - status = 0; - status |= get_status_temper_acdrive_winding_with_limits( - 1, protect_levels.alarm_temper_acdrive_winding_V1, - protect_levels.abnormal_temper_acdrive_winding_V1); - if (status == 4) { - edrk.errors.e10.bits.T_ACDRIVE_WINDING_V1 = 1; - } - if (status == 2) { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V1 = 1; - } else { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V1 = 0; - } - - status = 0; - status |= get_status_temper_acdrive_winding_with_limits( - 2, protect_levels.alarm_temper_acdrive_winding_W1, - protect_levels.abnormal_temper_acdrive_winding_W1); - if (status == 4) { - edrk.errors.e10.bits.T_ACDRIVE_WINDING_W1 = 1; - } - if (status == 2) { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W1 = 1; - } else { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W1 = 0; - } - - status = 0; - status |= get_status_temper_acdrive_winding_with_limits( - 3, protect_levels.alarm_temper_acdrive_winding_U2, - protect_levels.abnormal_temper_acdrive_winding_U2); - if (status == 4) { - edrk.errors.e10.bits.T_ACDRIVE_WINDING_U2 = 1; - } - if (status == 2) { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U2 = 1; - } else { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U2 = 0; - } - - status = 0; - status |= get_status_temper_acdrive_winding_with_limits( - 4, protect_levels.alarm_temper_acdrive_winding_V2, - protect_levels.abnormal_temper_acdrive_winding_V2); - if (status == 4) { - edrk.errors.e10.bits.T_ACDRIVE_WINDING_V2 = 1; - } - if (status == 2) { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V2 = 1; - } else { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V2 = 0; - } - - status = 0; - status |= get_status_temper_acdrive_winding_with_limits( - 5, protect_levels.alarm_temper_acdrive_winding_W2, - protect_levels.abnormal_temper_acdrive_winding_W2); - if (status == 4) { - edrk.errors.e10.bits.T_ACDRIVE_WINDING_W2 = 1; - } - if (status == 2) { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W2 = 1; - } else { - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W2 = 0; - } - - if (edrk.errors.e10.bits.T_ACDRIVE_WINDING_U1 || edrk.errors.e10.bits.T_ACDRIVE_WINDING_V1 || - edrk.errors.e10.bits.T_ACDRIVE_WINDING_W1 || edrk.errors.e10.bits.T_ACDRIVE_WINDING_U2 || - edrk.errors.e10.bits.T_ACDRIVE_WINDING_V2 || edrk.errors.e10.bits.T_ACDRIVE_WINDING_W2) { - edrk.errors.e7.bits.T_ACDRIVE_WINDING_MAX |= 1; - } - if (edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U1 || edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V1 || - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W1 || edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U2 || - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V2 || edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W2) { - edrk.warnings.e7.bits.T_ACDRIVE_WINDING_MAX = 1; - } - else - edrk.warnings.e7.bits.T_ACDRIVE_WINDING_MAX = 0; -} - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -void detect_error_acdrive_bear(void) -{ - static unsigned int count_run = 0, count_run_static = 0; - int status,i; - -// status = 0; - - status = get_status_temper_acdrive_bear_with_limits(0, protect_levels.alarm_temper_acdrive_bear_DNE, - protect_levels.abnormal_temper_acdrive_bear_DNE); - if (status & 4) - edrk.errors.e7.bits.T_ACDRIVE_BEAR_MAX_DNE |= 1; - if (status == 2) - edrk.warnings.e7.bits.T_ACDRIVE_BEAR_MAX_DNE = 1; - if (status == 1) - edrk.warnings.e7.bits.T_ACDRIVE_BEAR_MAX_DNE = 0; - - status = get_status_temper_acdrive_bear_with_limits(1, protect_levels.alarm_temper_acdrive_bear_NE, - protect_levels.abnormal_temper_acdrive_bear_NE); - if (status & 4) - edrk.errors.e9.bits.T_ACDRIVE_BEAR_MAX_NE |= 1; - if (status == 2) - edrk.warnings.e9.bits.T_ACDRIVE_BEAR_MAX_NE = 1; - if (status == 1) - edrk.warnings.e9.bits.T_ACDRIVE_BEAR_MAX_NE = 0; - -} - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -#define TIME_WAIT_RUN_PUMP 100 //50 -void detect_error_p_water(void) -{ - static unsigned int count_run = 0, count_run_static = 0; - int status,i; - - if (edrk.from_ing1.bits.NASOS_ON == 1) - { - if (pause_detect_error(&count_run,TIME_WAIT_RUN_PUMP,1)) - { - status = get_status_p_water_max(); - - if (status & 4) - edrk.errors.e2.bits.P_WATER_INT_MAX |= 1; - if (status==2) - edrk.warnings.e2.bits.P_WATER_INT_MAX = 1; - if (status==1) - edrk.warnings.e2.bits.P_WATER_INT_MAX = 0; - - status = get_status_p_water_min(1); - - if (status & 4) - edrk.errors.e2.bits.P_WATER_INT_MIN |= 1; - if (status==2) - edrk.warnings.e2.bits.P_WATER_INT_MIN = 1; - if (status==1) - edrk.warnings.e2.bits.P_WATER_INT_MIN = 0; - - } - } - else - count_run = 0; - - - // test if nasos off - status = get_status_p_water_min(0); - if (status>1) - { - if (pause_detect_error(&count_run_static,TIME_WAIT_RUN_PUMP,1)) - { - if (status==4) - { - if (control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_PUMP]==0) - edrk.errors.e2.bits.P_WATER_INT_MIN |= 1; - } - if (status==2) - edrk.warnings.e2.bits.P_WATER_INT_MIN = 1; - } - } - else - { - count_run_static = 0; - edrk.warnings.e2.bits.P_WATER_INT_MIN = 0; - } - -} - -/////////////////////////////////////////////// - -/////////////////////////////////////////////// - -void detect_error_ground(void) -{ - static unsigned int count_err = 0; - - - if (edrk.from_ing1.bits.ZAZEML_OFF == 0) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - edrk.errors.e5.bits.ERROR_GROUND_NET |= 1; - } - - if (edrk.from_ing1.bits.ZAZEML_ON == 1) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - edrk.errors.e5.bits.ERROR_GROUND_NET |= 1; - } - - if ((edrk.from_ing1.bits.ZAZEML_OFF == 1) && (edrk.from_ing1.bits.ZAZEML_ON == 0)) - count_err = 0; - -} - -/////////////////////////////////////////////// -void detect_error_nagrev(void) -{ - - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.NAGREV_ON == edrk.to_ing.bits.NAGREV_OFF) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - edrk.errors.e5.bits.ERROR_HEAT |= 1; - } - else - count_err = 0; - - -} -/////////////////////////////////////////////// -#define TIME_WAIT_ERROR_BLOCK_DOOR 30 //20 -void detect_error_block_door(void) -{ - static unsigned int count_err = 0; - - if ((edrk.from_ing2.bits.SOST_ZAMKA == 1 && edrk.to_ing.bits.BLOCK_KEY_OFF==1) - || (edrk.from_ing2.bits.SOST_ZAMKA == 0 && edrk.to_ing.bits.BLOCK_KEY_OFF==0)) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_BLOCK_DOOR,1)) - edrk.errors.e1.bits.BLOCK_DOOR |= 1; - } - else - count_err = 0; - -} - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -void detect_error_block_izol(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.BLOCK_IZOL_AVARIA == 1 ) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_IZOL,1)) - edrk.errors.e5.bits.ERROR_ISOLATE |= 1; - } - - if (edrk.from_ing1.bits.BLOCK_IZOL_AVARIA == 0 && edrk.from_ing1.bits.BLOCK_IZOL_NORMA == 0 ) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_IZOL,1)) - edrk.warnings.e5.bits.ERROR_ISOLATE |= 1; - } - else - edrk.warnings.e5.bits.ERROR_ISOLATE = 0; - - if (edrk.from_ing1.bits.BLOCK_IZOL_AVARIA == 0 && edrk.from_ing1.bits.BLOCK_IZOL_NORMA == 1 ) - { - count_err = 0; - } - - if (edrk.cmd_imit_low_isolation) - edrk.errors.e5.bits.ERROR_ISOLATE |= 1; - -} - -/////////////////////////////////////////////// -void detect_error_pre_charge(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.ZARYAD_ON == 1 && edrk.to_ing.bits.ZARYAD_ON == 0) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_CHARGE_ANSWER,1)) - edrk.errors.e6.bits.ERROR_PRE_CHARGE_ANSWER |= 1; - } - - if (edrk.from_ing1.bits.ZARYAD_ON == 0 && edrk.to_ing.bits.ZARYAD_ON == 1) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_CHARGE_ANSWER,1)) - edrk.errors.e6.bits.ERROR_PRE_CHARGE_ANSWER |= 1; - - - if (edrk.from_ing1.bits.ZARYAD_ON == 0 && edrk.to_ing.bits.ZARYAD_ON == 0) - count_err = 0; - - -// edrk.errors.e6.bits.ERROR_PRE_CHARGE_ANSWER |= 1; -// edrk.errors.e6.bits.ERROR_PRE_CHARGE_U |= 1; -} - -/////////////////////////////////////////////// -void detect_error_qtv(void) -{ - static unsigned int count_err_off = 0; - static unsigned int count_err_on = 0; - - - // , - if (edrk.from_shema_filter.bits.QTV_ON_OFF == 1 && edrk.cmd_to_qtv == 0) - { - if (pause_detect_error(&count_err_off,TIME_WAIT_ERROR_QTV,1)) - edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER |= 1; - } - else - { - count_err_off = 0; - } - -// , - if (edrk.from_shema_filter.bits.QTV_ON_OFF == 0 && edrk.cmd_to_qtv == 1) - { - if (pause_detect_error(&count_err_on,TIME_WAIT_ERROR_QTV,1)) - edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER |= 1; - } - else - { - count_err_on = 0; - } - -// -// if (edrk.from_shema.bits.QTV_ON_OFF == 0 && edrk.cmd_to_qtv == 0) -// count_err = 0; - -// edrk.errors.e6.bits.QTV_ERROR_NOT_U |= 1; - -// edrk.errors.e6.bits.QTV_ERROR_NOT_U |= detect_error_u_zpt(); - edrk.errors.e6.bits.QTV_ERROR_NOT_U |= detect_error_u_in(); -// edrk.errors.e6.bits.QTV_ERROR_NOT_U |= detect_error_u_zpt_on_predzaryad(); -} - -/////////////////////////////////////////////// -void detect_error_predohr_vipr(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.VIPR_PREDOHR_NORMA == 0) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - edrk.errors.e5.bits.ERROR_PRED_VIPR |= 1; - - if (edrk.from_ing1.bits.VIPR_PREDOHR_NORMA == 1) - count_err = 0; -} - -/////////////////////////////////////////////// -#define TIME_WAIT_ERROR_UMP_READY 1200 //120 //750 // 75 .. -#define TIME_WAIT_WARNING_UMP_READY 10 -void detect_error_ump(void) -{ - static unsigned int count_err = 0; - static unsigned int count_err2 = 0; - - static unsigned int prev_SumSbor = 0; - static unsigned int StageUMP = 0; - static unsigned int count_UMP_NOT_READY = 0; - int local_warning_ump = 0; - - if (edrk.SumSbor==1) - { - switch (StageUMP) { - case 0: if (edrk.from_shema_filter.bits.UMP_ON_OFF == 1) - StageUMP++; - break; - case 1: if (edrk.from_shema_filter.bits.UMP_ON_OFF == 0) - StageUMP++; - break; - case 2: - break; - case 3: - break; - - - default: break; - } - - if ((edrk.from_shema_filter.bits.READY_UMP == 0) && (StageUMP==0) && control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_UMP]==0) - { - - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_UMP_READY,1)) - edrk.errors.e7.bits.UMP_NOT_READY |= 1; - } - if (edrk.from_shema_filter.bits.READY_UMP == 1) - count_err = 0; - } - else - { - count_err= 0; - - // , ! - if (edrk.from_shema_filter.bits.UMP_ON_OFF==1) - if (pause_detect_error(&count_err2,TIME_WAIT_ERROR,1)) - edrk.errors.e11.bits.ERROR_UMP_NOT_OFF |= 1; - - if (edrk.from_shema_filter.bits.UMP_ON_OFF == 0) - count_err2 = 0; - - // - if (edrk.ump_cmd_another_bs==0) // - local_warning_ump = !edrk.from_shema_filter.bits.READY_UMP; - // edrk.warnings.e7.bits.UMP_NOT_READY = !edrk.from_shema_filter.bits.READY_UMP; - - - - StageUMP = 0; - } - - edrk.warnings.e7.bits.UMP_NOT_READY = filter_digital_input( edrk.warnings.e7.bits.UMP_NOT_READY, - &count_UMP_NOT_READY, - TIME_WAIT_WARNING_UMP_READY, - local_warning_ump); - - prev_SumSbor = edrk.SumSbor; -} - - -#define TIME_WAIT_BLOCK_QTV_FROM_SVU 20 -/////////////////////////////////////////////// -void detect_error_block_qtv_from_svu(void) -{ - static unsigned int count_err = 0; - - - if (edrk.from_shema.bits.SVU_BLOCK_QTV == 1 || control_station.active_array_cmd[CONTROL_STATION_CMD_BLOCK_BS]) - { - if (pause_detect_error(&count_err,TIME_WAIT_BLOCK_QTV_FROM_SVU,1)) - edrk.errors.e7.bits.SVU_BLOCK_ON_QTV |= 1; - } - else - { - count_err = 0; - } - -} - -/////////////////////////////////////////////// -void detect_error_fan(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.VENTIL_ON == 0 && (edrk.to_ing.bits.NASOS_1_ON || edrk.to_ing.bits.NASOS_2_ON)) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_FAN,1)) - edrk.errors.e5.bits.FAN |= 1; - - if (edrk.from_ing1.bits.VENTIL_ON == 1 && (edrk.to_ing.bits.NASOS_1_ON == 0 && edrk.to_ing.bits.NASOS_2_ON == 0)) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_FAN,1)) - edrk.errors.e5.bits.FAN |= 1; - - if (edrk.from_ing1.bits.VENTIL_ON == 0 && (edrk.to_ing.bits.NASOS_1_ON == 0 && edrk.to_ing.bits.NASOS_2_ON == 0)) - count_err = 0; - -} - - - -/////////////////////////////////////////////// -void detect_error_pre_ready_pump(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.NASOS_NORMA == 0) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - { -// edrk.errors.e5.bits.PRE_READY_PUMP |= 1; - edrk.warnings.e5.bits.PRE_READY_PUMP = 1; - } - - if (edrk.from_ing1.bits.NASOS_NORMA == 1) - { - count_err = 0; - edrk.warnings.e5.bits.PRE_READY_PUMP = 0; - } -} -/////////////////////////////////////////////// -void detect_error_pump_1(void) -{ - static unsigned int count_err = 0; - - // , - if (edrk.from_ing1.bits.NASOS_ON == 0 && edrk.to_ing.bits.NASOS_1_ON) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_PUMP,1)) - { -// edrk.errors.e5.bits.PUMP_1 |= 1; - edrk.warnings.e5.bits.PUMP_1 = 1; - } - } - - - // , - if (edrk.from_ing1.bits.NASOS_ON == 1 && edrk.to_ing.bits.NASOS_1_ON==0 && edrk.SelectPump1_2==1) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_PUMP,1)) - edrk.errors.e5.bits.PUMP_1 |= 1; - - - if (edrk.from_ing1.bits.NASOS_ON == 0 && edrk.to_ing.bits.NASOS_1_ON==0 && edrk.SelectPump1_2==1) - { - // - count_err = 0; - } - - if (edrk.from_ing1.bits.NASOS_ON == 1 && edrk.to_ing.bits.NASOS_1_ON==1 && edrk.SelectPump1_2==1) - { - // - count_err = 0; - edrk.warnings.e5.bits.PUMP_1 = 0; - } - - -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// -void detect_error_pump_2(void) -{ - static unsigned int count_err = 0; - - // , - if (edrk.from_ing1.bits.NASOS_ON == 0 && edrk.to_ing.bits.NASOS_2_ON) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_PUMP,1)) -// edrk.errors.e5.bits.PUMP_2 |= 1; - edrk.warnings.e5.bits.PUMP_2 = 1; - } - - if (edrk.from_ing1.bits.NASOS_ON == 1 && edrk.to_ing.bits.NASOS_2_ON==0 && edrk.SelectPump1_2==2) - { - if (pause_detect_error(&count_err,TIME_WAIT_ERROR_PUMP,1)) - edrk.errors.e5.bits.PUMP_2 |= 1; - } - - if (edrk.from_ing1.bits.NASOS_ON == 0 && edrk.to_ing.bits.NASOS_2_ON==0 && edrk.SelectPump1_2==2) - { - // - count_err = 0; - } - - if (edrk.from_ing1.bits.NASOS_ON == 1 && edrk.to_ing.bits.NASOS_2_ON==1 && edrk.SelectPump1_2==2) - { - // - count_err = 0; - edrk.warnings.e5.bits.PUMP_2 = 0; - } -} -/////////////////////////////////////////////// - -/////////////////////////////////////////////// -void detect_error_op_pit(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.OP_PIT_NORMA == 0 ) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - edrk.errors.e5.bits.OP_PIT |= 1; - - if (edrk.from_ing1.bits.OP_PIT_NORMA == 1 ) - count_err = 0; -} -/////////////////////////////////////////////// -void detect_error_power_upc(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.UPC_24V_NORMA == 0 ) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - edrk.errors.e5.bits.POWER_UPC |= 1; - - if (edrk.from_ing1.bits.UPC_24V_NORMA == 1 ) - count_err = 0; -} -/////////////////////////////////////////////// -void detect_error_t_vipr(void) -{ - static unsigned int count_err = 0; - -// if (edrk.from_ing.bits.VIPR_TEMPER_OK == 0 ) -// if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) -// edrk.errors.e5.bits.T_VIPR_MAX |= 1; -// -// if (edrk.from_ing.bits.VIPR_TEMPER_OK == 1 ) -// pause_detect_error(&count_err,TIME_WAIT_ERROR,0); -} -/////////////////////////////////////////////// -void detect_error_ute4ka_water(void) -{ - static unsigned int count_err = 0; - - if (edrk.from_ing1.bits.OHLAD_UTE4KA_WATER == 1 ) - if (pause_detect_error(&count_err,TIME_WAIT_ERROR,1)) - edrk.errors.e5.bits.UTE4KA_WATER |= 1; - - - if (edrk.from_ing1.bits.OHLAD_UTE4KA_WATER == 0 ) - count_err = 0; -} - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -void detect_error_from_knopka_avaria(void) -{ - edrk.errors.e5.bits.KEY_AVARIA |= edrk.from_ing1.bits.ALL_KNOPKA_AVARIA; -} -/////////////////////////////////////////////// -void detect_error_optical_bus(void) -{ -// if () - - - -} - -#define TIME_WAIT_SYNC_SIGNAL 20 // 2 sec -/////////////////////////////////////////////// -void detect_error_sync_bus(void) -{ - static unsigned int count_err = 0; - -// if (sync_data.flag_sync_1_2==0) -// edrk.errors.e7.bits.MASTER_SLAVE_SYNC |= 1; - - if (!edrk.ms.ready1) - { - edrk.warnings.e1.bits.NO_INPUT_SYNC_SIGNAL = 0; - count_err = 0; - return; - } - - // , , - if (sync_data.timeout_sync_signal && optical_read_data.data.cmd.bit.sync_line_detect - && optical_read_data.status==1) - { - edrk.warnings.e1.bits.NO_INPUT_SYNC_SIGNAL = 1; - return; - } - else - edrk.warnings.e1.bits.NO_INPUT_SYNC_SIGNAL = 0; - - - // , , - if (sync_data.timeout_sync_signal && optical_read_data.data.cmd.bit.sync_line_detect==0 - && edrk.ms.another_bs_maybe_on && optical_read_data.status==1) - { - if (pause_detect_error(&count_err,TIME_WAIT_SYNC_SIGNAL,1)) - edrk.errors.e1.bits.NO_INPUT_SYNC_SIGNAL |= 1; - } - else - count_err = 0; - - -} -/////////////////////////////////////////////// -#pragma CODE_SECTION(detect_error_u_zpt_fast,".fast_run"); -int detect_error_u_zpt_fast(void) -{ - int err; - - err = 0; - - - if (analog.iqU_1>=edrk.iqMAX_U_ZPT_Global) - edrk.errors.e0.bits.U_1_MAX |= 1; - - if (analog.iqU_2>=edrk.iqMAX_U_ZPT_Global) - edrk.errors.e0.bits.U_2_MAX |= 1; - - - if (analog.iqU_1>=edrk.iqMAX_U_ZPT) - edrk.errors.e0.bits.U_1_MAX |= 1; - - - if (analog.iqU_2>=edrk.iqMAX_U_ZPT) - edrk.errors.e0.bits.U_2_MAX |= 1; - - - if (edrk.from_shema_filter.bits.QTV_ON_OFF == 1 - // && edrk.to_shema.bits.QTV_ON - ) - { - if (analog.iqU_1<=edrk.iqMIN_U_ZPT) - edrk.errors.e0.bits.U_1_MIN |= 1; - - if (analog.iqU_2<=edrk.iqMIN_U_ZPT) - edrk.errors.e0.bits.U_2_MIN |= 1; - } - - err = (edrk.errors.e0.bits.U_1_MAX || edrk.errors.e0.bits.U_2_MAX || edrk.errors.e0.bits.U_1_MIN || edrk.errors.e0.bits.U_2_MIN); - return err; - -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// -int detect_error_u_zpt(void) -{ - int err; - - err = 0; - - if (edrk.iqMAX_U_ZPT>MINIMAL_LEVEL_ZAD_U) - { - if (analog.iqU_1>=edrk.iqMAX_U_ZPT_Global) - edrk.errors.e0.bits.U_1_MAX |= 1; - - if (analog.iqU_2>=edrk.iqMAX_U_ZPT_Global) - edrk.errors.e0.bits.U_2_MAX |= 1; - } - - - if (edrk.iqMAX_U_ZPT>MINIMAL_LEVEL_ZAD_U && edrk.from_shema_filter.bits.QTV_ON_OFF == 1 - // && edrk.to_shema.bits.QTV_ON - ) - { - if (analog.iqU_1>=edrk.iqMAX_U_ZPT) - edrk.errors.e0.bits.U_1_MAX |= 1; - - - if (analog.iqU_2>=edrk.iqMAX_U_ZPT) - edrk.errors.e0.bits.U_2_MAX |= 1; - } - - if (edrk.from_shema_filter.bits.QTV_ON_OFF == 1 - //&& edrk.to_shema.bits.QTV_ON - ) - { - if (analog.iqU_1<=edrk.iqMIN_U_ZPT) - edrk.errors.e0.bits.U_1_MIN |= 1; - - if (analog.iqU_2<=edrk.iqMIN_U_ZPT) - edrk.errors.e0.bits.U_2_MIN |= 1; - } - - err = (edrk.errors.e0.bits.U_1_MAX || edrk.errors.e0.bits.U_2_MAX || edrk.errors.e0.bits.U_1_MIN || edrk.errors.e0.bits.U_2_MIN); - return err; - -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// -int detect_error_u_zpt_on_predzaryad(void) -{ - int err; - - err = 0; - - if (edrk.iqMAX_U_ZPT>MINIMAL_LEVEL_ZAD_U) - { - if (analog.iqU_1>=edrk.iqMAX_U_ZPT_Predzaryad) - edrk.errors.e0.bits.U_1_MAX |= 1; - - - if (analog.iqU_2>=edrk.iqMAX_U_ZPT_Predzaryad) - edrk.errors.e0.bits.U_2_MAX |= 1; - } - - err = (edrk.errors.e0.bits.U_1_MAX || edrk.errors.e0.bits.U_2_MAX || edrk.errors.e0.bits.U_1_MIN || edrk.errors.e0.bits.U_2_MIN); - return err; - -} - -/////////////////////////////////////////////// -#pragma CODE_SECTION(detect_error_u_in,".fast_run"); -int detect_error_u_in(void) -{ - int err; - static unsigned int count_err_on = 0; - - err = 0; - - if (edrk.iqMAX_U_ZPT>MINIMAL_LEVEL_ZAD_U) - { - if (filter.iqUin_m1>=edrk.iqMAX_U_IN) - edrk.errors.e0.bits.U_IN_MAX |= 1; - - - if (filter.iqUin_m2>=edrk.iqMAX_U_IN) - edrk.errors.e0.bits.U_IN_MAX |= 1; - } - - if (edrk.from_shema_filter.bits.QTV_ON_OFF == 1 && edrk.SumSbor - // && edrk.to_shema.bits.QTV_ON - ) - { - if (control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_QTV]==1) - { - err = 0; - } - else - { - if ((filter.iqUin_m1<=edrk.iqMIN_U_IN) || (filter.iqUin_m2<=edrk.iqMIN_U_IN)) - err = 1; - else - err = 0; - } - -#if (WORK_ON_STEND_D) - if (err) - { - if (pause_detect_error(&count_err_on,TIME_WAIT_ERROR_QTV,1)) - edrk.errors.e0.bits.U_IN_MIN |= 1; - } - else - count_err_on = 0; - - - -#else - - edrk.errors.e0.bits.U_IN_MIN |= err; -#endif - - } - - - - - - err = (edrk.errors.e0.bits.U_IN_MAX || edrk.errors.e0.bits.U_IN_MIN ); - return err; - -} -/////////////////////////////////////////////// -#define MAX_WAIT_AFTER_KVITIR 100//50 -void detect_error_all(void) -{ - unsigned int pause_after_kvitir=0; - - - if (f.count_wait_after_kvitir<=MAX_WAIT_AFTER_KVITIR) - { - f.count_wait_after_kvitir++; - pause_after_kvitir = 0; - } - else - pause_after_kvitir = 1; - - - - detect_error_ute4ka_water(); -// detect_error_t_vipr(); - detect_error_power_upc(); - detect_error_op_pit(); - - detect_error_pump_2(); - detect_error_pump_1(); - - if (edrk.warnings.e5.bits.PUMP_1 && edrk.warnings.e5.bits.PUMP_2) - { - edrk.errors.e5.bits.PUMP_1 |= 1; - edrk.errors.e5.bits.PUMP_2 |= 1; - } - - detect_error_pre_ready_pump(); - detect_error_fan(); - detect_error_qtv(); - detect_error_pre_charge(); - detect_error_block_izol(); - detect_error_nagrev(); - detect_error_ground(); - detect_error_ump(); - - - // , . ! - if (pause_after_kvitir) - { - detect_error_from_knopka_avaria(); - detect_error_from_another_bs(); - } - -#if (_FLOOR6==1) - -#else - detect_error_p_water(); -#endif - - detect_error_t_water(); - detect_error_t_air(); - detect_error_t_u(); - detect_error_acdrive_bear(); - detect_error_acdrive_winding(); - detect_error_block_qtv_from_svu(); - detect_error_block_door(); - - - detect_error_optical_bus(); - detect_error_sync_bus(); - detect_alive_another_bs(); - - edrk.warning = get_common_state_warning(); - edrk.overheat = get_common_state_overheat(); - - edrk.warnings.e10.bits.WARNING_I_OUT_OVER_1_6_NOMINAL = out_I_over_1_6.overload_detected; - -// edrk.errors.e7.bits.ANOTHER_BS_ALARM |= optical_read_data.data.cmd.bit.alarm; - detect_error_sensor_rotor(); - - -} -/////////////////////////////////////////////// -void clear_errors(void) -{ - - clear_errors_master_slave(); - clear_sync_error(); - - edrk.errors.e0.all = 0; - edrk.errors.e1.all = 0; - edrk.errors.e2.all = 0; - edrk.errors.e3.all = 0; - edrk.errors.e4.all = 0; - edrk.errors.e5.all = 0; - edrk.errors.e6.all = 0; - edrk.errors.e7.all = 0; - edrk.errors.e8.all = 0; - edrk.errors.e9.all = 0; - edrk.errors.e10.all = 0; - edrk.errors.e11.all = 0; - edrk.errors.e12.all = 0; - -// edrk.errors.e0.all = 0; -// edrk.errors.e0.all = 0; - edrk.Stop = 0; - - edrk.count_lost_interrupt = 0; - - f.count_wait_after_kvitir = 0; - - -} -/////////////////////////////////////////////// -void clear_warnings(void) -{ - - edrk.warnings.e0.all = 0; - edrk.warnings.e1.all = 0; - edrk.warnings.e2.all = 0; - edrk.warnings.e3.all = 0; - edrk.warnings.e4.all = 0; - edrk.warnings.e5.all = 0; - edrk.warnings.e6.all = 0; - edrk.warnings.e7.all = 0; - edrk.warnings.e8.all = 0; - edrk.warnings.e9.all = 0; - edrk.warnings.e10.all = 0; - edrk.warnings.e11.all = 0; - edrk.warnings.e12.all = 0; -} - -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -void read_plane_errors(void) -{ - if (project.controller.read.errors.bit.pwm_wdog) - edrk.errors.e9.bits.ERR_PWM_WDOG |= 1; - -#if USE_TK_0 -//af1 - if (project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk0_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk0_current || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk1_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk1_current || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk2_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk2_current || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk3_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk3_current || - project.cds_tk[0].read.sbus.time_err_tk_all.bit.tk_3210 || - project.cds_tk[0].read.sbus.lock_status_error.bit.line_err_keys_3210) - edrk.errors.e6.bits.UO2_KEYS |= 1; -//af2 - if (project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk4_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk4_current || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk5_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk5_current || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk6_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk6_current || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk7_ack || - project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk7_current || - project.cds_tk[0].read.sbus.time_err_tk_all.bit.tk_7654 || - project.cds_tk[0].read.sbus.lock_status_error.bit.line_err_keys_7654) - edrk.errors.e6.bits.UO3_KEYS |= 1; -#endif - -#if USE_TK_1 -//af3 - if (project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk0_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk0_current || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk1_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk1_current || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk2_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk2_current || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk3_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk3_current || - project.cds_tk[1].read.sbus.time_err_tk_all.bit.tk_3210 || - project.cds_tk[1].read.sbus.lock_status_error.bit.line_err_keys_3210) - edrk.errors.e6.bits.UO4_KEYS |= 1; -//af4 - if (project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk4_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk4_current || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk5_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk5_current || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk6_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk6_current || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk7_ack || - project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk7_current || - project.cds_tk[1].read.sbus.time_err_tk_all.bit.tk_7654 || - project.cds_tk[1].read.sbus.lock_status_error.bit.line_err_keys_7654) - edrk.errors.e6.bits.UO5_KEYS |= 1; -#endif - -#if USE_TK_2 -//af5 - if (project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk0_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk0_current || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk1_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk1_current || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk2_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk2_current || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk3_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk3_current || - project.cds_tk[2].read.sbus.time_err_tk_all.bit.tk_3210 || - project.cds_tk[2].read.sbus.lock_status_error.bit.line_err_keys_3210) - edrk.errors.e6.bits.UO6_KEYS |= 1; -//af6 - if (project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk4_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk4_current || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk5_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk5_current || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk6_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk6_current || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk7_ack || - project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk7_current || - project.cds_tk[2].read.sbus.time_err_tk_all.bit.tk_7654 || - project.cds_tk[2].read.sbus.lock_status_error.bit.line_err_keys_7654) - edrk.errors.e6.bits.UO7_KEYS |= 1; - - -#endif - -#if USE_TK_3 - - if (project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk0_ack || - project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk0_current || - project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk1_ack || - project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk1_current || - project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk2_ack || - project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk2_current || - project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk3_ack || - project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk3_current || - project.cds_tk[3].read.sbus.time_err_tk_all.bit.tk_3210 || - project.cds_tk[3].read.sbus.lock_status_error.bit.line_err_keys_3210) - edrk.errors.e6.bits.UO1_KEYS |= 1; - -#endif - -//all errors local status - - -#if USE_TK_0 - if (project.cds_tk[0].read.sbus.lock_status_error.bit.err0_local) - edrk.errors.e4.bits.ERR_TK_0 |= 1; -#endif -#if USE_TK_1 - if (project.cds_tk[1].read.sbus.lock_status_error.bit.err0_local) - edrk.errors.e4.bits.ERR_TK_1 |= 1; -#endif -#if USE_TK_2 - if (project.cds_tk[2].read.sbus.lock_status_error.bit.err0_local) - edrk.errors.e4.bits.ERR_TK_2 |= 1; -#endif -#if USE_TK_3 - if (project.cds_tk[3].read.sbus.lock_status_error.bit.err0_local) - edrk.errors.e4.bits.ERR_TK_3 |= 1; -#endif - - -#if USE_IN_0 - if (project.cds_in[0].read.sbus.lock_status_error.bit.err0_local) - edrk.errors.e4.bits.ERR_IN_0 |= 1; -#endif -#if USE_IN_1 - if (project.cds_in[1].read.sbus.lock_status_error.bit.err0_local) - edrk.errors.e4.bits.ERR_IN_1 |= 1; -#endif - -#if USE_OUT_0 - if (project.cds_out[0].read.sbus.lock_status_error.bit.err0_local) - edrk.errors.e4.bits.ERR_OUT_0 |= 1; -#endif - -#if USE_ADC_0 - if (project.adc[0].read.sbus.lock_status_error.all) - edrk.errors.e4.bits.ERR_ADC_0 |= 1; -#endif -#if USE_ADC_1 - if (project.adc[1].read.sbus.lock_status_error.all) - edrk.errors.e4.bits.ERR_ADC_1 |= 1; -#endif -// - if (project.controller.read.errors.bit.status_er0) - edrk.errors.e5.bits.LINE_ERR0 |= 1; - if (project.controller.read.errors.bit.errHWP_trig) - edrk.errors.e5.bits.LINE_HWP |= 1; - - if (project.controller.read.errors.bit.error_pbus - || project.controller.read.errors_buses.bit.slave_addr_error - || project.controller.read.errors_buses.bit.count_error_pbus - || project.x_parallel_bus->flags.bit.error) - edrk.errors.e6.bits.ERR_PBUS |= 1; - - if (project.controller.read.errors_buses.bit.err_sbus) - edrk.errors.e6.bits.ERR_SBUS |= 1; - -/// - -#if USE_HWP_0 - if (project.hwp[0].read.comp_s.minus.all || project.hwp[0].read.comp_s.plus.all) - edrk.errors.e1.bits.HWP_ERROR |= 1; -#endif - - -#if USE_TK_0 - if (project.all_status_plates.tk0 != component_Ready) - edrk.errors.e3.bits.NOT_READY_TK_0 |= 1; -#endif -#if USE_TK_1 - if (project.all_status_plates.tk1 != component_Ready) - edrk.errors.e3.bits.NOT_READY_TK_1 |= 1; -#endif -#if USE_TK_2 - if (project.all_status_plates.tk2 != component_Ready) - edrk.errors.e3.bits.NOT_READY_TK_2 |= 1; -#endif -#if USE_TK_3 - if (project.all_status_plates.tk3 != component_Ready) - edrk.errors.e3.bits.NOT_READY_TK_3 |= 1; -#endif - -#if USE_ADC_0 - if (project.all_status_plates.adc0 != component_Ready) - edrk.errors.e3.bits.NOT_READY_ADC_0 |= 1; -#endif -#if USE_ADC_1 - if (project.all_status_plates.adc1 != component_Ready) - edrk.errors.e3.bits.NOT_READY_ADC_1 |= 1; -#endif - -#if USE_HWP_0 - if (project.all_status_plates.hwp0 != component_Ready) - edrk.errors.e3.bits.NOT_READY_HWP_0 |= 1; -#endif - -#if USE_IN_0 - if (project.all_status_plates.in0 != component_Ready) - edrk.errors.e3.bits.NOT_READY_IN_0 |= 1; -#endif -#if USE_IN_1 - if (project.all_status_plates.in1 != component_Ready) - edrk.errors.e3.bits.NOT_READY_IN_1 |= 1; -#endif - -#if USE_OUT_0 - if (project.all_status_plates.out0 != component_Ready) - edrk.errors.e3.bits.NOT_READY_OUT_0 |= 1; -#endif -} - -int get_common_state_warning() { - return edrk.warnings.e0.all != 0 || edrk.warnings.e1.all != 0 || - edrk.warnings.e2.all != 0 || edrk.warnings.e3.all != 0 || - edrk.warnings.e4.all != 0 || edrk.warnings.e5.all != 0 || - edrk.warnings.e6.all != 0 || edrk.warnings.e7.all != 0 || - edrk.warnings.e8.all != 0 || edrk.warnings.e9.all != 0 || - edrk.warnings.e10.all != 0 || edrk.warnings.e11.all != 0 || - edrk.warnings.e12.all != 0 ? 1 : 0; -} - -int get_common_state_overheat() { - return edrk.warnings.e2.bits.T_AIR0_MAX | edrk.warnings.e2.bits.T_AIR1_MAX | - edrk.warnings.e2.bits.T_AIR2_MAX | edrk.warnings.e2.bits.T_AIR3_MAX | - edrk.warnings.e2.bits.T_UO1_MAX | edrk.warnings.e2.bits.T_UO2_MAX | - edrk.warnings.e2.bits.T_UO3_MAX | edrk.warnings.e2.bits.T_UO4_MAX | - edrk.warnings.e2.bits.T_UO5_MAX | edrk.warnings.e2.bits.T_UO6_MAX | - edrk.warnings.e2.bits.T_UO7_MAX | edrk.warnings.e2.bits.T_WATER_EXT_MAX | - edrk.warnings.e2.bits.T_WATER_INT_MAX | - edrk.errors.e2.bits.T_AIR0_MAX | edrk.errors.e2.bits.T_AIR1_MAX | - edrk.errors.e2.bits.T_AIR2_MAX | edrk.errors.e2.bits.T_AIR3_MAX | - edrk.errors.e2.bits.T_UO1_MAX | edrk.errors.e2.bits.T_UO2_MAX | - edrk.errors.e2.bits.T_UO3_MAX | edrk.errors.e2.bits.T_UO4_MAX | - edrk.errors.e2.bits.T_UO5_MAX | edrk.errors.e2.bits.T_UO6_MAX | - edrk.errors.e2.bits.T_UO7_MAX | edrk.errors.e2.bits.T_WATER_EXT_MAX | - edrk.errors.e2.bits.T_WATER_INT_MAX; -} - diff --git a/Inu/Src2/551/main/detect_errors.h b/Inu/Src2/551/main/detect_errors.h deleted file mode 100644 index 48e3126..0000000 --- a/Inu/Src2/551/main/detect_errors.h +++ /dev/null @@ -1,80 +0,0 @@ -/* - * detect_errors.h - * - * Created on: 4 . 2020 . - * Author: star - */ - -#ifndef SRC_MYLIBS_DETECT_ERRORS_H_ -#define SRC_MYLIBS_DETECT_ERRORS_H_ - - - -#define TIME_WAIT_ERROR 20 // 2 sec -#define TIME_WAIT_ERROR_QTV 100 // 10 sec -#define TIME_WAIT_ERROR_CHARGE_ANSWER 60 // 6 sec -#define TIME_WAIT_ERROR_IZOL 50 //5 sec //200 // 20 sec -#define TIME_WAIT_ERROR_PUMP 100 // 10 sec -#define TIME_WAIT_ERROR_FAN 300 // 30 sec -#define TIME_WAIT_SENSOR_ROTOR_BREAK_ALL 200 // 20 sec -#define TIME_WAIT_SENSOR_ROTOR_BREAK_DIRECTION 10 // 1 sec -#define TIME_WAIT_SENSOR_ROTOR_BREAK_ONE_SENSOR 20 // 2 sec - - - - -#define MINIMAL_LEVEL_ZAD_U 27962 // 10 V - -void clear_errors(void); -void clear_warnings(void); -void detect_error_all(void); -void read_plane_errors(void); -int detect_error_u_zpt_on_predzaryad(void); -int detect_error_u_in(void); -int detect_error_u_zpt_fast(void); - - -void detect_error_from_knopka_avaria(void); -void detect_error_ute4ka_water(void); -void detect_error_t_vipr(void); -void detect_error_power_upc(void); -void detect_error_op_pit(void); -void detect_error_p_water(void); -void detect_error_pump_2(void); -void detect_error_pump_1(void); -void detect_error_pre_ready_pump(void); -void detect_error_fan(void); -void detect_error_block_qtv_from_svu(void); - -void detect_error_predohr_vipr(void); -void detect_error_qtv(void); -void detect_error_pre_charge(void); -void detect_error_block_izol(void); -void detect_error_nagrev(void); -void detect_error_ground(void); -void detect_error_block_door(void); -void detect_error_optical_bus(void); -void detect_error_sync_bus(void); -int get_status_temper_acdrive_winding(int nc); -int get_status_temper_acdrive_winding_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_acdrive_bear(int nc); -int get_status_temper_acdrive_bear_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_air(int nc); -int get_status_temper_air_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_u(int nc); -int get_status_temper_u_with_limits(int nc, int alarm, int abnormal); -int get_status_temper_water(int nc); -int get_status_p_water_max(void); -int get_status_p_water_min(int pump_on_off); -void detect_error_t_water(void); -void detect_error_t_air(void); -void detect_error_t_u(void); -void detect_error_acdrive_winding(void); - -int get_common_state_warning(void); -int get_common_state_overheat(void); -void detect_error_sensor_rotor(void); - - - -#endif /* SRC_MYLIBS_DETECT_ERRORS_H_ */ diff --git a/Inu/Src2/551/main/detect_errors_adc.c b/Inu/Src2/551/main/detect_errors_adc.c deleted file mode 100644 index 634ea2f..0000000 --- a/Inu/Src2/551/main/detect_errors_adc.c +++ /dev/null @@ -1,310 +0,0 @@ -/* - * detect_errors_adc.c - * - * Created on: 7 . 2020 . - * Author: star - */ -#include -#include -#include -#include -#include -#include -#include -#include "digital_filters.h" - -#include "IQmathLib.h" - -//ANALOG_PROTECT_LEVELS analog_protect_levels = ANALOG_PROTECT_LEVELS_DEFAULTS; - -//#pragma DATA_SECTION(analog_protect,".fast_vars"); -#pragma DATA_SECTION(analog_protect,".slow_vars"); -ANALOG_ADC_PROTECT analog_protect = ANALOG_ADC_PROTECT_DEFAULTS; - -//#pragma DATA_SECTION(break_Iout_1_state,".fast_vars"); -#pragma DATA_SECTION(break_Iout_1_state,".slow_vars"); -BREAK_PHASE_I break_Iout_1_state = BREAK_PHASE_I_DEFAULTS; - -//#pragma DATA_SECTION(break_Iout_2_state,".fast_vars"); -#pragma DATA_SECTION(break_Iout_2_state,".slow_vars"); -BREAK_PHASE_I break_Iout_2_state = BREAK_PHASE_I_DEFAULTS; - -int detect_error_Izpt(); -int detect_error_Ibreak(int res_num); -void init_protect_3_phase(void); - -void init_analog_protect_levels(void) { - init_protect_3_phase(); -} - -#define AMPL_TO_RMS 0.709 - -//#define LEVEL_I_1_2_DIBALANCE 1118481 // 200 A -#define LEVEL_I_1_2_DIBALANCE 1677721 // 300 A - -void init_protect_3_phase(void) { - analog_protect.in_voltage[0].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqVal_W_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP); - analog_protect.in_voltage[0].setup.use.all = 0; - analog_protect.in_voltage[0].setup.use.bits.phase_U = 0; - analog_protect.in_voltage[0].setup.use.bits.phase_V = 0; - analog_protect.in_voltage[0].setup.use.bits.phase_W = 0; - analog_protect.in_voltage[0].setup.use.bits.module = 0; - analog_protect.in_voltage[0].setup.use.bits.detect_minus_10 = 1; - analog_protect.in_voltage[0].setup.use.bits.detect_minus_20 = 1; - analog_protect.in_voltage[0].setup.use.bits.detect_plus_10 = 0; - analog_protect.in_voltage[0].setup.use.bits.detect_plus_20 = 1; - analog_protect.in_voltage[0].setup.use.bits.system_asymmetry_by_delta = 1; - - analog_protect.in_voltage[1].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[1].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[1].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[1].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP); - analog_protect.in_voltage[1].setup.use.all = 0; - analog_protect.in_voltage[1].setup.use.bits.phase_U = 0; - analog_protect.in_voltage[1].setup.use.bits.phase_V = 0; - analog_protect.in_voltage[1].setup.use.bits.phase_W = 0; - analog_protect.in_voltage[1].setup.use.bits.module = 0; - analog_protect.in_voltage[1].setup.use.bits.detect_minus_10 = 1; - analog_protect.in_voltage[1].setup.use.bits.detect_minus_20 = 1; - analog_protect.in_voltage[1].setup.use.bits.detect_plus_10 = 0; - analog_protect.in_voltage[1].setup.use.bits.detect_plus_20 = 1; - analog_protect.in_voltage[1].setup.use.bits.system_asymmetry_by_delta = 1; - - ///////////////// - analog_protect.out_I[0].setup.levels.iqVal_module_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[0].setup.levels.iqVal_U_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[0].setup.levels.iqVal_V_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[0].setup.levels.iqVal_W_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[0].setup.use.all = 0; - analog_protect.out_I[0].setup.use.bits.phase_U = 1; - analog_protect.out_I[0].setup.use.bits.phase_V = 1; - analog_protect.out_I[0].setup.use.bits.phase_W = 1; - analog_protect.out_I[0].setup.use.bits.break_phase = 1; - - analog_protect.out_I[1].setup.levels.iqVal_module_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[1].setup.levels.iqVal_U_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[1].setup.levels.iqVal_V_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[1].setup.levels.iqVal_W_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP); - analog_protect.out_I[1].setup.use.all = 0; - analog_protect.out_I[1].setup.use.bits.phase_U = 1; - analog_protect.out_I[1].setup.use.bits.phase_V = 1; - analog_protect.out_I[1].setup.use.bits.phase_W = 1; - analog_protect.out_I[1].setup.use.bits.break_phase = 1; - - analog_protect.iqI_zpt_level = _IQ(LEVEL_ADC_I_ZPT / NORMA_ACP); - analog_protect.iqI_break_level = _IQ(LEVEL_ADC_I_BREAK / NORMA_ACP); -} - -#define min(x,y) (x) < (y) ? (x) : (y) -#define max(x,y) (x) > (y) ? (x) : (y) - -void reinit_protect_I_and_U_settings(void) { - int max_I = 0; - analog_protect.in_voltage[0].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqVal_W_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[0].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[0].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP); - - analog_protect.in_voltage[1].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[1].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[1].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[1].setup.levels.iqVal_W_max = edrk.iqMAX_U_IN; - analog_protect.in_voltage[1].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP); - analog_protect.in_voltage[1].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP); - - max_I = max(protect_levels.alarm_Imax_U02, protect_levels.alarm_Imax_U03); - max_I = max(max_I, protect_levels.alarm_Imax_U04); - analog_protect.out_I[0].setup.levels.iqVal_module_max = _IQ(((float)max_I) / NORMA_ACP); - analog_protect.out_I[0].setup.levels.iqVal_U_max = _IQ(((float)protect_levels.alarm_Imax_U02) / NORMA_ACP); - analog_protect.out_I[0].setup.levels.iqVal_V_max = _IQ(((float)protect_levels.alarm_Imax_U03) / NORMA_ACP); - analog_protect.out_I[0].setup.levels.iqVal_W_max = _IQ(((float)protect_levels.alarm_Imax_U04) / NORMA_ACP); - - max_I = max(protect_levels.alarm_Imax_U05, protect_levels.alarm_Imax_U06); - max_I = max(max_I, protect_levels.alarm_Imax_U07); - analog_protect.out_I[1].setup.levels.iqVal_module_max = _IQ(((float)max_I) / NORMA_ACP); - analog_protect.out_I[1].setup.levels.iqVal_U_max = _IQ(((float)protect_levels.alarm_Imax_U05) / NORMA_ACP); - analog_protect.out_I[1].setup.levels.iqVal_V_max = _IQ(((float)protect_levels.alarm_Imax_U06) / NORMA_ACP); - analog_protect.out_I[1].setup.levels.iqVal_W_max = _IQ(((float)protect_levels.alarm_Imax_U07) / NORMA_ACP); - - analog_protect.iqI_zpt_level = _IQ(protect_levels.alarm_Izpt_max / NORMA_ACP); - analog_protect.iqI_break_level = _IQ(protect_levels.alarm_Imax_U01 / NORMA_ACP); - - if (edrk.SumSbor == 0) { - analog_protect.in_voltage[0].setup.timers_inited = 0; - analog_protect.in_voltage[1].setup.timers_inited = 0; - analog_protect.out_I[0].setup.timers_inited = 0; - analog_protect.out_I[1].setup.timers_inited = 0; - } -} - -#define TIME_DETECT_WARNING_U_PREDELS 100 - -void detect_protect_adc(_iq teta_ch1, _iq teta_ch2) { - static unsigned int timer_U_in1_minus10 = 0; - static unsigned int timer_U_in1_minus20 = 0; - static unsigned int timer_U_in1_plus20 = 0; - - static unsigned int timer_U_in2_minus10 = 0; - static unsigned int timer_U_in2_minus20 = 0; - static unsigned int timer_U_in2_plus20 = 0; - - static unsigned int counter_in1_minus10 = 0; - static unsigned int counter_in2_minus10 = 0; - static unsigned int counter_in1_minus20 = 0; - static unsigned int counter_in2_minus20 = 0; - - - // - //0 - analog_protect.in_voltage[0].errors.all = 0; - analog_protect.in_voltage[0].iqVal_U = analog.iqUin_A1B1_rms; - analog_protect.in_voltage[0].iqVal_V = analog.iqUin_B1C1_rms; - analog_protect.in_voltage[0].iqVal_W = analog.iqUin_C1A1_rms; - analog_protect.in_voltage[0].iqVal_mod = filter.iqUin_m1; - -// analog_protect.in_voltage[0].calc(&analog_protect.in_voltage[0]); - - //1 - analog_protect.in_voltage[1].errors.all = 0; - analog_protect.in_voltage[1].iqVal_U = analog.iqUin_A2B2_rms; - analog_protect.in_voltage[1].iqVal_V = analog.iqUin_B2C2_rms; - analog_protect.in_voltage[1].iqVal_W = analog.iqUin_C2A2_rms; - analog_protect.in_voltage[1].iqVal_mod = filter.iqUin_m2; - -// analog_protect.in_voltage[1].calc(&analog_protect.in_voltage[1]); - - // - edrk.errors.e0.bits.U_A1B1_MAX |= analog_protect.in_voltage[0].errors.bits.phase_U_max; - edrk.errors.e0.bits.U_B1C1_MAX |= analog_protect.in_voltage[0].errors.bits.phase_V_max; - edrk.errors.e0.bits.U_IN_MAX |= analog_protect.in_voltage[0].errors.bits.module_max; - // - edrk.errors.e0.bits.U_A2B2_MAX |= analog_protect.in_voltage[1].errors.bits.phase_U_max; - edrk.errors.e0.bits.U_B2C2_MAX |= analog_protect.in_voltage[1].errors.bits.phase_V_max; - edrk.errors.e0.bits.U_IN_MAX |= analog_protect.in_voltage[1].errors.bits.module_max; - - edrk.warnings.e8.bits.U_IN_20_PROCENTS_HIGH = analog_protect.in_voltage[0].over_limit.bits.module_20_percent_hi || analog_protect.in_voltage[1].over_limit.bits.module_20_percent_hi; - edrk.errors.e8.bits.U_IN_20_PROCENTS_HIGH |= analog_protect.in_voltage[0].errors.bits.module_20_percent_hi; - edrk.errors.e8.bits.U_IN_20_PROCENTS_HIGH |= analog_protect.in_voltage[1].errors.bits.module_20_percent_hi; - - if (edrk.from_shema_filter.bits.QTV_ON_OFF == 1 - // && edrk.to_shema.bits.QTV_ON - ) - { - - // - edrk.errors.e8.bits.U_IN_10_PROCENTS_LOW |= analog_protect.in_voltage[0].errors.bits.module_10_percent_low; - edrk.errors.e8.bits.U_IN_20_PROCENTS_LOW |= analog_protect.in_voltage[0].errors.bits.module_20_percent_low; - - edrk.errors.e8.bits.U_IN_10_PROCENTS_LOW |= analog_protect.in_voltage[1].errors.bits.module_10_percent_low; - edrk.errors.e8.bits.U_IN_20_PROCENTS_LOW |= analog_protect.in_voltage[1].errors.bits.module_20_percent_low; - - edrk.warnings.e8.bits.U_IN_10_PROCENTS_LOW = pause_detect_error(&counter_in1_minus10, - TIME_DETECT_WARNING_U_PREDELS, - analog_protect.in_voltage[0].over_limit.bits.module_10_percent_low); - - edrk.warnings.e8.bits.U_IN_10_PROCENTS_LOW = pause_detect_error(&counter_in2_minus10, - TIME_DETECT_WARNING_U_PREDELS, - analog_protect.in_voltage[1].over_limit.bits.module_10_percent_low); - - edrk.warnings.e8.bits.U_IN_20_PROCENTS_LOW = pause_detect_error(&counter_in1_minus20, - TIME_DETECT_WARNING_U_PREDELS, - analog_protect.in_voltage[0].over_limit.bits.module_20_percent_low); - - edrk.warnings.e8.bits.U_IN_20_PROCENTS_LOW = pause_detect_error(&counter_in2_minus20, - TIME_DETECT_WARNING_U_PREDELS, - analog_protect.in_voltage[1].over_limit.bits.module_20_percent_low); - - edrk.errors.e9.bits.DISBALANCE_Uin_1 |= analog_protect.in_voltage[0].errors.bits.system_asymmetry; - edrk.errors.e9.bits.DISBALANCE_Uin_2 |= analog_protect.in_voltage[1].errors.bits.system_asymmetry; - - } - else - { - edrk.warnings.e8.bits.U_IN_10_PROCENTS_LOW = 0; - edrk.warnings.e8.bits.U_IN_20_PROCENTS_LOW = 0; - } - - // - analog_protect.out_I[0].errors.all = 0; - analog_protect.out_I[0].iqVal_U = analog.iqIu_1; - analog_protect.out_I[0].iqVal_V = analog.iqIv_1; - analog_protect.out_I[0].iqVal_W = analog.iqIw_1; - analog_protect.out_I[0].iqVal_mod = analog.iqIm_1; - analog_protect.out_I[0].break_phase = &break_Iout_1_state; - analog_protect.out_I[0].iqTeta = teta_ch1; - -// analog_protect.out_I[0].calc(&analog_protect.out_I[0]); - - edrk.errors.e1.bits.I_UO2_MAX = analog_protect.out_I[0].errors.bits.phase_U_max; - edrk.errors.e1.bits.I_UO3_MAX = analog_protect.out_I[0].errors.bits.phase_V_max; - edrk.errors.e1.bits.I_UO4_MAX = analog_protect.out_I[0].errors.bits.phase_W_max; - - if (analog_protect.out_I[0].errors.bits.break_phase) { - edrk.errors.e8.bits.LOSS_OUTPUT_U1 = analog_protect.out_I[0].errors.bits.break_phase_U; - edrk.errors.e8.bits.LOSS_OUTPUT_V1 = analog_protect.out_I[0].errors.bits.break_phase_V; - edrk.errors.e8.bits.LOSS_OUTPUT_W1 = analog_protect.out_I[0].errors.bits.break_phase_W; - } - - analog_protect.out_I[1].errors.all = 0; - analog_protect.out_I[1].iqVal_U = analog.iqIu_2; - analog_protect.out_I[1].iqVal_V = analog.iqIv_2; - analog_protect.out_I[1].iqVal_W = analog.iqIw_2; - analog_protect.out_I[1].iqVal_mod = analog.iqIm_2; - analog_protect.out_I[1].break_phase = &break_Iout_2_state; - analog_protect.out_I[1].iqTeta = teta_ch2; - -// analog_protect.out_I[1].calc(&analog_protect.out_I[1]); - - edrk.errors.e1.bits.I_UO5_MAX = analog_protect.out_I[1].errors.bits.phase_U_max; - edrk.errors.e1.bits.I_UO6_MAX = analog_protect.out_I[1].errors.bits.phase_V_max; - edrk.errors.e1.bits.I_UO7_MAX = analog_protect.out_I[1].errors.bits.phase_W_max; - if (analog_protect.out_I[1].errors.bits.break_phase) { - edrk.errors.e8.bits.LOSS_OUTPUT_U2 = analog_protect.out_I[1].errors.bits.break_phase_U; - edrk.errors.e8.bits.LOSS_OUTPUT_V2 = analog_protect.out_I[1].errors.bits.break_phase_V; - edrk.errors.e8.bits.LOSS_OUTPUT_W2 = analog_protect.out_I[1].errors.bits.break_phase_W; - } - - edrk.errors.e8.bits.DISBALANCE_IM1_IM2 |= _IQabs(analog.iqIm_1 - analog.iqIm_2) > LEVEL_I_1_2_DIBALANCE ? 1 : 0; - - //I zpt - edrk.errors.e0.bits.I_1_MAX |= detect_error_Izpt(); - // - edrk.errors.e1.bits.I_BREAK_1_MAX |= detect_error_Ibreak(1); - edrk.errors.e1.bits.I_BREAK_2_MAX |= detect_error_Ibreak(2); -} - -int detect_error_Izpt() { - return analog.iqIin_1 > analog_protect.iqI_zpt_level ? 1 : 0; -} - -int detect_error_Ibreak(int res_num) { - if (res_num == 1) { - return analog.iqIbreak_1 > analog_protect.iqI_break_level || - analog.iqIbreak_1 < -analog_protect.iqI_break_level ? 1 : 0; - } else if (res_num == 2) { - return analog.iqIbreak_2 > analog_protect.iqI_break_level || - analog.iqIbreak_2 < -analog_protect.iqI_break_level ? 1 : 0; - } else { - return 0; - } -} diff --git a/Inu/Src2/551/main/detect_errors_adc.h b/Inu/Src2/551/main/detect_errors_adc.h deleted file mode 100644 index 6aa850d..0000000 --- a/Inu/Src2/551/main/detect_errors_adc.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * detect_errors_adc.h - * - * Created on: 7 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_DETECT_ERRORS_ADC_H_ -#define SRC_MAIN_DETECT_ERRORS_ADC_H_ - -#include - -typedef struct { - SETUP_3_PHASE_PROTECT U_in; - SETUP_3_PHASE_PROTECT I_out; - - _iq iqI_zpt; - _iq iqI_break; - -} ANALOG_PROTECT_LEVELS; - -#define ANALOG_PROTECT_LEVELS_DEFAULTS { SETUP_3_PHASE_PROTECT_DEFAULTS, \ - SETUP_3_PHASE_PROTECT_DEFAULTS, \ - 0,0} - -typedef struct { - DETECT_PROTECT_3_PHASE in_voltage[2]; - DETECT_PROTECT_3_PHASE out_I[2]; - - _iq iqI_zpt_level; - _iq iqI_break_level; -} ANALOG_ADC_PROTECT; - -#define ANALOG_ADC_PROTECT_DEFAULTS { \ - {DETECT_PROTECT_3_PHASE_DEFAULTS,DETECT_PROTECT_3_PHASE_DEFAULTS},\ - {DETECT_PROTECT_3_PHASE_DEFAULTS,DETECT_PROTECT_3_PHASE_DEFAULTS},\ - 0,0 } - -void init_analog_protect_levels(void); -void detect_protect_adc (_iq teta_ch1, _iq teta_ch2); -void reinit_protect_I_and_U_settings(void); - - -extern ANALOG_ADC_PROTECT analog_protect; -#endif /* SRC_MAIN_DETECT_ERRORS_ADC_H_ */ diff --git a/Inu/Src2/551/main/detect_overload.c b/Inu/Src2/551/main/detect_overload.c deleted file mode 100644 index fce38dd..0000000 --- a/Inu/Src2/551/main/detect_overload.c +++ /dev/null @@ -1,92 +0,0 @@ -/* - * detect_overload.c - * - * Created on: 15 . 2020 . - * Author: star - */ -#include -#include -#include -#include -#include -#include "alg_simple_scalar.h" - -#include "IQmathLib.h" - -DETECT_OVERLOAD out_I_over_1_6 = DETECT_OVERLOAD_DEFAULTS; - -#define CALLS_IN_PWM_INT 2 // (1 2) - -void init_detect_overloads(void) { - out_I_over_1_6.level_overload = _IQmpy(I_OUT_NOMINAL_IQ, _IQ(1.6)); - out_I_over_1_6.time_over_tics = (long) 15 * FREQ_PWM * CALLS_IN_PWM_INT; - out_I_over_1_6.time_latch_tics = (long) 45 * FREQ_PWM * CALLS_IN_PWM_INT; - out_I_over_1_6.tics_counter = 0; - out_I_over_1_6.overload_detected = 0; - -} - -int calc_detect_overload(DETECT_OVERLOAD *v) { - if (v->val > v->level_overload) { - v->tics_counter += 1; - if (v->tics_counter > v->time_over_tics) { v->tics_counter = v->time_over_tics;} - } else { - if (v->tics_counter > 0) { v->tics_counter -= 1; } - else {v->tics_counter = 0;} - if (v->overload_detected && v->tics_counter == 0) { - v->overload_detected = 0; - } - } - if (v->tics_counter >= v->time_over_tics) { - v->overload_detected = 1; - v->tics_counter = v->time_latch_tics; - } - return v->overload_detected; -} - -#define LIMIT_DETECT_LEVEL 16273899 // 0.97 //15938355 //95% - -void check_all_power_limits() { - _iq level_I_nominal = 0; - - //edrk.power_limit.bits.limit_by_temper = edrk.temper_limit_koeffs.code_status; - - if (edrk.Go) - { - - level_I_nominal = _IQmpy(LIMIT_DETECT_LEVEL, edrk.zadanie.iq_Izad_rmp); - - if ((filter.iqIm > level_I_nominal) || - out_I_over_1_6.overload_detected) - { - edrk.power_limit.bits.limit_Iout = 1; - } else - { - edrk.power_limit.bits.limit_Iout = 0; - } - } - else - edrk.power_limit.bits.limit_Iout = 0; - -// if (edrk.from_uom.code>1) -// edrk.power_limit.bits.limit_UOM = 1; -// else -// edrk.power_limit.bits.limit_UOM = 0; - -//filter.PowerScalar + edrk.iq_power_kw_another_bs - if ( (edrk.iq_power_kw_full_filter_abs > _IQmpy(LIMIT_DETECT_LEVEL, edrk.zadanie.iq_limit_power_zad_rmp)) - || simple_scalar1.flag_decr_mzz_power - // , FOC, , . - ) - { - edrk.power_limit.bits.limit_from_SVU = 1; - } - else - { - edrk.power_limit.bits.limit_from_SVU = 0; - } - - -} - - diff --git a/Inu/Src2/551/main/detect_overload.h b/Inu/Src2/551/main/detect_overload.h deleted file mode 100644 index 897e24a..0000000 --- a/Inu/Src2/551/main/detect_overload.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * detect_overload.h - * - * Created on: 15 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_DETECT_OVERLOAD_H_ -#define SRC_MAIN_DETECT_OVERLOAD_H_ - -typedef struct { - _iq val; // - _iq level_overload; // - int overload_detected; // - - unsigned long time_over_tics; - unsigned long time_latch_tics; - unsigned long tics_counter; - - int (*calc)(); -} DETECT_OVERLOAD; - -#define DETECT_OVERLOAD_DEFAULTS {0,0,0, 0,0,0, \ - calc_detect_overload } - -void init_detect_overloads(void); -int calc_detect_overload(DETECT_OVERLOAD *v); -void check_all_power_limits(); - -extern DETECT_OVERLOAD out_I_over_1_6; - -#endif /* SRC_MAIN_DETECT_OVERLOAD_H_ */ diff --git a/Inu/Src2/551/main/detect_phase_break.c b/Inu/Src2/551/main/detect_phase_break.c deleted file mode 100644 index 69f0b55..0000000 --- a/Inu/Src2/551/main/detect_phase_break.c +++ /dev/null @@ -1,112 +0,0 @@ -/* - * detect_phase_break.c - * - * Created on: 10 . 2020 . - * Author: star - */ - -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "IQmathLib.h" - -#include - - -#define CONST_IQ_2PI 105414357 // 360 -#define CONST_IQ_PI2 26353589 // 90 - - -static void clear_alg_vars(BREAK_PHASE_I *v); -static int calc_direction(BREAK_PHASE_I *v); -static int calc_error_if_break(BREAK_PHASE_I *v, int num, int field_direction); - -// , -// 0 - -// 1- U -// 2- V -// 3- W -int calc_break_I_phase(BREAK_PHASE_I *v) { - - int field_direction = 1; //1 - forward, 0 - reverse - int err = 0; - - if (v->teta > CONST_IQ_2PI) { - v->teta = CONST_IQ_2PI; - } - if(v->teta < 0) { - v->teta = 0; - } - field_direction = calc_direction(v); - if (v->iqImod < v->config.iqLevelZero) { - clear_alg_vars(v); - return 0; - } - - if (_IQabs(v->iqIu) < v->config.iqLevelZero && - _IQabs(v->iqIv + v->iqIw) < v->config.iqLevelZero && - _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) { - err = calc_error_if_break(v, 0, field_direction); - } else { - v->latch_break_start[0] = 0; - } - if (_IQabs(v->iqIv) < v->config.iqLevelZero && - _IQabs(v->iqIu + v->iqIw) < v->config.iqLevelZero && - _IQabs(v->iqIu) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) { - err = calc_error_if_break(v, 1, field_direction); - } else { - v->latch_break_start[1] = 0; - } - if (_IQabs(v->iqIw) < v->config.iqLevelZero && - _IQabs(v->iqIv + v->iqIu) < v->config.iqLevelZero && - _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIu) > v->config.iqLevelZero) { - err = calc_error_if_break(v, 2, field_direction); - } else { - v->latch_break_start[2] = 0; - } - - return err; -} - -void clear_alg_vars(BREAK_PHASE_I *v) { - int i = 0; - for (i = 0; i < 3; i++) { - v->latch_break_start[i] = 0; - v->latched_teta[i] = 0; - } -} - -int calc_direction(BREAK_PHASE_I *v) { - int direction = 1; - if (v->teta > v->prev_teta) { - if (v->teta - v->prev_teta < CONST_IQ_PI2) { direction = 1;} - else { direction = 0;} - } else { - if (v->prev_teta - v->teta < CONST_IQ_2PI) { direction = 0;} - else { direction = 1;} - } - v->prev_teta = v->teta; - return direction; -} - -int calc_error_if_break(BREAK_PHASE_I *v, int num, int field_direction) { - int err = 0; - if (v->latch_break_start[num] == 0) { - v->latch_break_start[num] = 1; - v->latched_teta[num] = v->teta; - } else { - if (field_direction == 0) { - if (v->latched_teta[num] > v->teta) { - err = v->latched_teta[num] - v->teta > CONST_IQ_PI2 ? num + 1 : 0; - } else { - err = v->teta - v->latched_teta[num] < CONST_IQ_PI2 - CONST_IQ_2PI ? num + 1 : 0; - } - } else { - if (v->latched_teta[num] < v->teta) { - err = v->teta - v->latched_teta[num] > CONST_IQ_PI2 ? num + 1 : 0; - } else { - err = v->latched_teta[num] - v->teta < CONST_IQ_PI2 - CONST_IQ_2PI ? num + 1 : 0; - } - } - } - return err; -} diff --git a/Inu/Src2/551/main/detect_phase_break.h b/Inu/Src2/551/main/detect_phase_break.h deleted file mode 100644 index 66104d8..0000000 --- a/Inu/Src2/551/main/detect_phase_break.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * detect_phase_break.h - * - * Created on: 10 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_DETECT_PHASE_BREAK_H_ -#define SRC_MAIN_DETECT_PHASE_BREAK_H_ - -typedef struct { - _iq iqIu; - _iq iqIv; - _iq iqIw; - _iq iqImod; - _iq teta; - _iq prev_teta; - - _iq latched_teta[3]; - int latch_break_start[3]; - - struct { - _iq iqLevelZero; - - } config; - - int (*calc)(); -} BREAK_PHASE_I; - -#define BREAK_PHASE_I_DEFAULTS {0,0,0,0,0,0, \ - {0,0,0}, {0,0,0}, {0}, \ - calc_break_I_phase } - -int calc_break_I_phase(BREAK_PHASE_I *v); - -#endif /* SRC_MAIN_DETECT_PHASE_BREAK_H_ */ diff --git a/Inu/Src2/551/main/detect_phase_break2.c b/Inu/Src2/551/main/detect_phase_break2.c deleted file mode 100644 index 4cfb921..0000000 --- a/Inu/Src2/551/main/detect_phase_break2.c +++ /dev/null @@ -1,203 +0,0 @@ -/* - * detect_phase_break.c - * - * Created on: 10 . 2020 . - * Author: star - */ - -#include "IQmathLib.h" - -//#include "DSP281x_Examples.h" // DSP281x Examples Include File -//#include "DSP281x_Device.h" // DSP281x Headerfile Include File - -#include "detect_phase_break2.h" - - -#define CONST_IQ_2PI 105414357 // 2*pi 360 -#define CONST_IQ_3_2PI 79060767 // 4/3 pi 270 -#define CONST_IQ_PI2 26353589 // 90 - - - -void check_brocken_phase(BREAK2_PHASE *v) -{ - int i; - int ph_a=0, ph_b=0, ph_c=0; - _iq plus_a, max_a; - - if ( (v->iqCh[0] >= v->iqCh[1] && v->iqCh[0] < v->iqCh[2]) - || (v->iqCh[0] >= v->iqCh[2] && v->iqCh[0] < v->iqCh[1]) ) - ph_a = 1; - else - ph_a = -1; - - if ( (v->iqCh[1] >= v->iqCh[0] && v->iqCh[1] < v->iqCh[2]) - || (v->iqCh[1] >= v->iqCh[2] && v->iqCh[1] < v->iqCh[0]) ) - ph_b = 1; - else - ph_b = -1; - - if ( (v->iqCh[2] >= v->iqCh[0] && v->iqCh[2] < v->iqCh[1]) - || (v->iqCh[2] >= v->iqCh[1] && v->iqCh[2] < v->iqCh[0]) ) - ph_c = 1; - else - ph_c = -1; - - - // plus_a = _IQ(360.0/v->config.freq_pwm * v->freq_signal); - plus_a = _IQmpy(v->config.calc_const, v->freq_signal); - - - v->sum_brocken_out[0] += plus_a*ph_a; - v->sum_brocken_out[1] += plus_a*ph_b; - v->sum_brocken_out[2] += plus_a*ph_c; - - v->plus_a = plus_a; - - for (i=0;i<3;i++) - { - if (v->sum_brocken_out[i]>=CONST_IQ_2PI) v->sum_brocken_out[i] = CONST_IQ_2PI; - if (v->sum_brocken_out[i]<=0) v->sum_brocken_out[i] = 0; - - if (v->sum_brocken_out[i]>CONST_IQ_3_2PI) - v->return_brocken_code |= (1<sum_brocken_out[0]>=max_a) max_a = v->sum_brocken_out[0]; - if (v->sum_brocken_out[1]>=max_a) max_a = v->sum_brocken_out[1]; - if (v->sum_brocken_out[2]>=max_a) max_a = v->sum_brocken_out[2]; - v->sum_brocken_out[3] = max_a; // - -} - - - -void check_i_out_brocken(float freq) -{ - - - -} - - - - - -// , -// 0 - -// 1- U -// 2- V -// 3- W -int calc_break2_phase(BREAK2_PHASE *v) { - -// int field_direction = 1; //1 - forward, 0 - reverse - int err = 0; - - if (v->freq_signal==0) - { - v->sum_brocken_out[0] = 0; - v->sum_brocken_out[1] = 0; - v->sum_brocken_out[2] = 0; - v->sum_brocken_out[3] = 0; - v->brocken_i_out = 0; - } - else - { - if (_IQabs(v->iqCh[0])>v->config.minimal_level - || _IQabs(v->iqCh[1])>v->config.minimal_level - || _IQabs(v->iqCh[2])>v->config.minimal_level ) - { - check_brocken_phase(v); - } - else - { - - v->iqCh[0] = 0; - v->iqCh[1] = 0; - v->iqCh[2] = 0; - - check_brocken_phase(v); - - } - } - -// if (brocken_i_out & 0x1) -// error.power_errors.bit.phase_a_brocken |= 1; -// if (brocken_i_out & 0x2) -// error.power_errors.bit.phase_b_brocken |= 1; -// if (brocken_i_out & 0x4) -// error.power_errors.bit.phase_c_brocken |= 1; -// -// if(is_errors()) set_err_state(); - - -// -// if (v->teta > CONST_IQ_2PI) { -// v->teta = CONST_IQ_2PI; -// } -// if(v->teta < 0) { -// v->teta = 0; -// } -// field_direction = calc_direction(v); -// if (v->iqImod < v->config.iqLevelZero) { -// clear_alg_vars(v); -// return 0; -// } -// -// if (_IQabs(v->iqIu) < v->config.iqLevelZero && -// _IQabs(v->iqIv + v->iqIw) < v->config.iqLevelZero && -// _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) { -// err = calc_error_if_break(v, 0, field_direction); -// } else { -// v->latch_break_start[0] = 0; -// } -// if (_IQabs(v->iqIv) < v->config.iqLevelZero && -// _IQabs(v->iqIu + v->iqIw) < v->config.iqLevelZero && -// _IQabs(v->iqIu) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) { -// err = calc_error_if_break(v, 1, field_direction); -// } else { -// v->latch_break_start[1] = 0; -// } -// if (_IQabs(v->iqIw) < v->config.iqLevelZero && -// _IQabs(v->iqIv + v->iqIu) < v->config.iqLevelZero && -// _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIu) > v->config.iqLevelZero) { -// err = calc_error_if_break(v, 2, field_direction); -// } else { -// v->latch_break_start[2] = 0; -// } - - return err; -} - - - - -void init_break2_phase(BREAK2_PHASE *v) -{ - v->config.iq_freq = _IQ(v->config.freq_pwm / v->config.norma_freq); - v->config.calc_const = _IQdiv(CONST_IQ_2PI, v->config.iq_freq); - v->return_brocken_code = 0; - - -} - -void clear_break2_phase(BREAK2_PHASE *v) -{ - - v->iqCh[0] = 0; - v->iqCh[1] = 0; - v->iqCh[2] = 0; - v->sum_brocken_out[0] = 0; - v->sum_brocken_out[1] = 0; - v->sum_brocken_out[2] = 0; - v->sum_brocken_out[3] = 0; - v->brocken_i_out = 0; - - v->return_brocken_code = 0; -} - - - - - diff --git a/Inu/Src2/551/main/detect_phase_break2.h b/Inu/Src2/551/main/detect_phase_break2.h deleted file mode 100644 index c32608e..0000000 --- a/Inu/Src2/551/main/detect_phase_break2.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * detect_phase_break2.h - * - * Created on: 10 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_DETECT_PHASE_BREAK2_H_ -#define SRC_MAIN_DETECT_PHASE_BREAK2_H_ - -#include "IQmathLib.h" - - -typedef struct { - _iq iqCh[3]; - _iq sum_brocken_out[4]; // 4 - . - _iq freq_signal; - - int brocken_i_out; - int return_brocken_code; - _iq plus_a; - - - struct { - unsigned int freq_pwm; - unsigned int norma_freq; - - _iq minimal_level; - _iq calc_const; - _iq iq_freq; - } config; - - int (*calc)(); - void (*init)(); - void (*clear_error)(); - -} BREAK2_PHASE; - -#define BREAK2_PHASE_DEFAULTS {{0,0,0},\ - {0,0,0,0},\ - 0,0,0,0,\ - 0,0,0,0,0, \ - calc_break2_phase, init_break2_phase, clear_break2_phase } - -void check_brocken_phase(BREAK2_PHASE *v); -int calc_break2_phase(BREAK2_PHASE *v); -void init_break2_phase(BREAK2_PHASE *v); -void clear_break2_phase(BREAK2_PHASE *v); - -#endif /* SRC_MAIN_DETECT_PHASE_BREAK2_H_ */ diff --git a/Inu/Src2/551/main/digital_filters.c b/Inu/Src2/551/main/digital_filters.c deleted file mode 100644 index a94d46b..0000000 --- a/Inu/Src2/551/main/digital_filters.c +++ /dev/null @@ -1,103 +0,0 @@ -/* - * digital_filters.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - - - -//////////////////////////////////////////////////////////////////// -unsigned int filter_digital_input(unsigned int prev_valus, unsigned int *c_plus, unsigned int max_wait, unsigned int flag) -{ - - if (flag) - { - if ((*c_plus)>=max_wait) - { - return 1; - } - else - { - (*c_plus)++; - return (prev_valus); - } - } - else - { - if ((*c_plus)==0) - { - return 0; - } - else - { - (*c_plus)--; - return (prev_valus); - } - } -} -/////////////////////////////////////////////////////////////////// -//TODO: may be move to detect_errors.c -unsigned int pause_detect_error(unsigned int *c_err, unsigned int max_wait,unsigned int flag) -{ - if (flag) - { - if ((*c_err)>=max_wait) - { - return 1; - } - else - { - (*c_err)++; - return 0; - } - } - else - { - (*c_err) = 0; - return 0; - - } - - - -} - - - -////////////////////////////////////////////////////////// - - - -unsigned int filter_err_count(unsigned int *counter, unsigned int max_errors, unsigned int err, unsigned int cmd) -{ - if (cmd==1) - { - (*counter) = 0; - return 0; - } - - if (err) - { - if ((*counter)>=max_errors) - return 1; - else - (*counter)++; - - return 0; - } - - if (err==0) - { - if ((*counter)==0) - return 0; - else - (*counter)--; - - return 0; - } - return 0; -} - - - diff --git a/Inu/Src2/551/main/digital_filters.h b/Inu/Src2/551/main/digital_filters.h deleted file mode 100644 index bc24cac..0000000 --- a/Inu/Src2/551/main/digital_filters.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * digital_filters.h - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#ifndef SRC_MAIN_DIGITAL_FILTERS_H_ -#define SRC_MAIN_DIGITAL_FILTERS_H_ - -unsigned int filter_digital_input(unsigned int prev_valus, unsigned int *c_plus, unsigned int max_wait, unsigned int flag); - -unsigned int pause_detect_error(unsigned int *c_err, unsigned int max_wait,unsigned int flag); - - -unsigned int filter_err_count(unsigned int *counter, unsigned int max_errors, unsigned int err, unsigned int cmd); - - -#endif /* SRC_MAIN_DIGITAL_FILTERS_H_ */ diff --git a/Inu/Src2/551/main/edrk_main.c b/Inu/Src2/551/main/edrk_main.c deleted file mode 100644 index c89a473..0000000 --- a/Inu/Src2/551/main/edrk_main.c +++ /dev/null @@ -1,2736 +0,0 @@ -#include <281xEvTimersInit.h> -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -//#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mathlib.h" -#include "params_hwp.h" - -//#include "modbus_fill_table.h" - -#include "big_dsp_module.h" -#include "control_station.h" -#include "CAN_Setup.h" - -#include "global_time.h" -#include "IQmathLib.h" -#include "mathlib.h" - -#include "modbus_table_v2.h" -#include "oscil_can.h" -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" -#include "alg_pll.h" -#include "vector_control.h" -#include "CRC_Functions.h" -#include "RS_Functions.h" -#include "xp_project.h" -#include "sbor_shema.h" -#include "alarm_log_can.h" -#include "pwm_test_lines.h" -#include "master_slave.h" -#include "xp_write_xpwm_time.h" -#include "v_rotor_22220.h" -#include "log_to_memory.h" -#include "log_params.h" -#include "build_version.h" -#include "profile_interrupt.h" -#include "limit_power.h" -#include "pwm_logs.h" -#include "logs_hmi.h" -#include "alarm_log.h" -#include "can_protocol_ukss.h" - -#include "ukss_tools.h" -#include "another_bs.h" -#include "temper_p_tools.h" -#include "digital_filters.h" -#include "pll_tools.h" -#include "ramp_zadanie_tools.h" -#include "uom_tools.h" -#include "synhro_tools.h" - -#if (_SIMULATE_AC==1) -#include "sim_model.h" -#endif -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -//#pragma DATA_SECTION(ccc, ".slow_vars") -//int ccc[40] = {0,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1}; - - - -#pragma DATA_SECTION(f, ".slow_vars") -FLAG f = FLAG_DEFAULTS; - -int cur1=0; -int cur2=0; - -unsigned int old_time_edrk1 = 0, old_time_edrk2 = 0, prev_flag_special_mode_rs = 0; - -#pragma DATA_SECTION(edrk, ".slow_vars") -EDRK edrk = EDRK_DEFAULT; - - - - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -// PLUS, MINUS -// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -void set_oborots_from_zadat4ik(void) -{ -static unsigned int old_time_edrk3 = 0, prev_PROVOROT; - - - if (!(detect_pause_milisec(100,&old_time_edrk3))) - return; - -} - - -////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -#define RASCEPITEL_MANUAL_ALWAYS_ON_2 1 // 1 -#define TIME_ON_OFF_FOR_IMITATION_RASCEPITEL 50 // 5 . -#define TIME_FILTER_UMP_SIGNALS 5 // 0.5 -#define TIME_FILTER_ALL_SIGNALS 5 // 0.5 - - -#pragma DATA_SECTION(count_wait_filter, ".slow_vars") -unsigned int count_wait_filter[16] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0}; -unsigned int counter_imit_rascepitel = 0; - -void update_input_edrk(void) -{ - static unsigned int flag_imit_rascepitel = 0; - static int st1=0; - -// ANOTHER PCH - edrk.from_second_pch.bits.RASCEPITEL = !FROM_ING_ANOTHER_RASCEPITEL; - edrk.from_second_pch.bits.MASTER = FROM_ING_ANOTHER_MASTER_PCH; - - -// ING -#if (_FLOOR6==1) - - if (st1==0) - { - edrk.from_zadat4ik.all = 0; - edrk.from_vpu.all = 0; - - edrk.from_ing1.bits.ALL_KNOPKA_AVARIA = 0;//!FROM_ALL_KNOPKA_AVARIA; - edrk.from_ing1.bits.BLOCK_IZOL_AVARIA = 0;//!FROM_ING_BLOCK_IZOL_AVARIA; - edrk.from_ing1.bits.BLOCK_IZOL_NORMA = 1;//!FROM_ING_BLOCK_IZOL_NORMA; - edrk.from_ing1.bits.LOCAL_REMOUTE = 1;//0;//!FROM_ING_LOCAL_REMOUTE; - edrk.from_ing1.bits.NAGREV_ON = 1;//!FROM_ING_NAGREV_ON; - edrk.from_ing1.bits.NASOS_NORMA = 1;//!FROM_ING_NASOS_NORMA; - edrk.from_ing1.bits.NASOS_ON = 0;//!FROM_ING_NASOS_ON; - edrk.from_ing1.bits.OHLAD_UTE4KA_WATER = 0;//!FROM_ING_OHLAD_UTE4KA_WATER; - edrk.from_ing1.bits.UPC_24V_NORMA = 1;//!FROM_ING_UPC_24V_NORMA; - edrk.from_ing1.bits.OP_PIT_NORMA = 1;//!FROM_ING_OP_PIT_NORMA; - edrk.from_ing1.bits.VENTIL_ON = 0;//!FROM_ING_VENTIL_ON; - edrk.from_ing1.bits.VIPR_PREDOHR_NORMA = 1;//!FROM_ING_VIPR_PREDOHR_NORMA; - - edrk.from_ing1.bits.ZARYAD_ON = 0;//!FROM_ING_ZARYAD_ON; - edrk.from_ing1.bits.ZAZEML_OFF = 1;//!FROM_ING_ZAZEML_OFF; - edrk.from_ing1.bits.ZAZEML_ON = 0;//!FROM_ING_ZAZEML_ON; - - edrk.from_ing2.bits.KEY_MINUS = 0;//FROM_ING_OBOROTS_MINUS; - edrk.from_ing2.bits.KEY_PLUS = 0;//!FROM_ING_OBOROTS_PLUS; - edrk.from_ing2.bits.KEY_KVITIR = 0;//FROM_ING_LOCAL_KVITIR; - - edrk.from_ing2.bits.KEY_SBOR = 0;//FROM_ING_SBOR_SHEMA; - edrk.from_ing2.bits.KEY_RAZBOR = 0;//FROM_ING_RAZBOR_SHEMA; - - // edrk.from_ing1.bits.RASCEPITEL_ON = 0;//FROM_ING_RASCEPITEL_ON_OFF; - - // edrk.from_ing2.bits.SOST_ZAMKA = !edrk.to_ing.bits.BLOCK_KEY_OFF;//1;//!FROM_ING_SOST_ZAMKA; - - - // SHEMA - edrk.from_shema.bits.RAZBOR_SHEMA = 0;//FROM_BSU_RAZBOR_SHEMA; - edrk.from_shema.bits.SBOR_SHEMA = 0;//FROM_BSU_SBOR_SHEMA; - - edrk.from_shema.bits.ZADA_DISPLAY = 0;//!FROM_BSU_ZADA_DISPLAY; - edrk.from_shema.bits.SVU = 0;//!FROM_BSU_SVU; - // edrk.from_shema.bits.KNOPKA_AVARIA = FROM_ALL_KNOPKA_AVARIA; - edrk.from_shema.bits.QTV_ON_OFF = 0;//!FROM_SHEMA_QTV_ON_OFF; - edrk.from_shema.bits.UMP_ON_OFF = 0;//!FROM_SHEMA_UMP_ON_OFF; - edrk.from_shema.bits.READY_UMP = 1;//!FROM_SHEMA_READY_UMP; - edrk.from_shema.bits.SVU_BLOCK_QTV = 0;//!FROM_SVU_BLOCK_QTV; - st1 = 1; - } - - // - if (edrk.to_ing.bits.RASCEPITEL_ON) - flag_imit_rascepitel = 1; - if (edrk.to_ing.bits.RASCEPITEL_OFF) - flag_imit_rascepitel = 0; - - edrk.from_ing1.bits.RASCEPITEL_ON = imit_signals_rascepitel(&counter_imit_rascepitel, TIME_ON_OFF_FOR_IMITATION_RASCEPITEL, flag_imit_rascepitel, 0); - ///// - - - edrk.from_ing2.bits.SOST_ZAMKA = !edrk.to_ing.bits.BLOCK_KEY_OFF; - if (edrk.to_ing.bits.NASOS_2_ON || edrk.to_ing.bits.NASOS_1_ON) - { - edrk.from_ing1.bits.VENTIL_ON = 1; - edrk.from_ing1.bits.NASOS_ON = 1; - } - else - { - edrk.from_ing1.bits.VENTIL_ON = 0; - edrk.from_ing1.bits.NASOS_ON = 0; - } -#else - - - // ZADAT4IK - if (control_station.alive_control_station[CONTROL_STATION_ZADATCHIK_CAN]) - edrk.from_zadat4ik.all = Unites[ZADATCHIK_CAN][16]; - else - edrk.from_zadat4ik.all = 0; - - if (control_station.alive_control_station[CONTROL_STATION_VPU_CAN]) - edrk.from_vpu.all = Unites[VPU_CAN][16]; - else - edrk.from_vpu.all = 0; - - - - edrk.from_ing1.bits.ALL_KNOPKA_AVARIA = !FROM_ALL_KNOPKA_AVARIA; - edrk.from_ing1.bits.BLOCK_IZOL_AVARIA = !FROM_ING_BLOCK_IZOL_AVARIA; - edrk.from_ing1.bits.BLOCK_IZOL_NORMA = !FROM_ING_BLOCK_IZOL_NORMA; - edrk.from_ing1.bits.LOCAL_REMOUTE = !FROM_ING_LOCAL_REMOUTE; - edrk.from_ing1.bits.NAGREV_ON = !FROM_ING_NAGREV_ON; - edrk.from_ing1.bits.NASOS_NORMA = !FROM_ING_NASOS_NORMA; - edrk.from_ing1.bits.NASOS_ON = !FROM_ING_NASOS_ON; - edrk.from_ing1.bits.OHLAD_UTE4KA_WATER = !FROM_ING_OHLAD_UTE4KA_WATER; - edrk.from_ing1.bits.UPC_24V_NORMA = !FROM_ING_UPC_24V_NORMA; - edrk.from_ing1.bits.OP_PIT_NORMA = !FROM_ING_OP_PIT_NORMA; - edrk.from_ing1.bits.VENTIL_ON = !FROM_ING_VENTIL_ON; - edrk.from_ing1.bits.VIPR_PREDOHR_NORMA = !FROM_ING_VIPR_PREDOHR_NORMA; - - edrk.from_ing1.bits.ZARYAD_ON = !FROM_ING_ZARYAD_ON; - edrk.from_ing1.bits.ZAZEML_OFF = !FROM_ING_ZAZEML_OFF; - edrk.from_ing1.bits.ZAZEML_ON = !FROM_ING_ZAZEML_ON; - - edrk.from_ing2.bits.KEY_MINUS = FROM_ING_OBOROTS_MINUS; - edrk.from_ing2.bits.KEY_PLUS = !FROM_ING_OBOROTS_PLUS; - edrk.from_ing2.bits.KEY_KVITIR = FROM_ING_LOCAL_KVITIR; - - edrk.from_ing2.bits.KEY_SBOR = FROM_ING_SBOR_SHEMA; - edrk.from_ing2.bits.KEY_RAZBOR = FROM_ING_RAZBOR_SHEMA; - -#if(RASCEPITEL_MANUAL_ALWAYS_ON_2) - - // - if (edrk.to_ing.bits.RASCEPITEL_ON) - flag_imit_rascepitel = 1; - if (edrk.to_ing.bits.RASCEPITEL_OFF) - flag_imit_rascepitel = 0; - - edrk.from_ing1.bits.RASCEPITEL_ON = imit_signals_rascepitel(&counter_imit_rascepitel, TIME_ON_OFF_FOR_IMITATION_RASCEPITEL, flag_imit_rascepitel, 0); - -#else - edrk.from_ing1.bits.RASCEPITEL_ON = FROM_ING_RASCEPITEL_ON_OFF; -#endif - edrk.from_ing2.bits.SOST_ZAMKA = !FROM_ING_SOST_ZAMKA; - - -// SHEMA - edrk.from_shema.bits.RAZBOR_SHEMA = FROM_BSU_RAZBOR_SHEMA; - edrk.from_shema.bits.SBOR_SHEMA = FROM_BSU_SBOR_SHEMA; - - if (edrk.from_shema.bits.RAZBOR_SHEMA==1 && edrk.from_shema.bits.SBOR_SHEMA) - { - // - edrk.from_shema.bits.RAZBOR_SHEMA = 0; - edrk.from_shema.bits.SBOR_SHEMA = 0; - } - edrk.from_shema_filter.bits.RAZBOR_SHEMA = filter_digital_input( edrk.from_shema_filter.bits.RAZBOR_SHEMA, - &count_wait_filter[0], - TIME_FILTER_ALL_SIGNALS, - edrk.from_shema.bits.RAZBOR_SHEMA); - - - edrk.from_shema_filter.bits.SBOR_SHEMA = filter_digital_input( edrk.from_shema_filter.bits.SBOR_SHEMA, - &count_wait_filter[1], - TIME_FILTER_ALL_SIGNALS, - edrk.from_shema.bits.SBOR_SHEMA); - - edrk.from_shema.bits.ZADA_DISPLAY = !FROM_BSU_ZADA_DISPLAY; - edrk.from_shema_filter.bits.ZADA_DISPLAY = filter_digital_input( edrk.from_shema_filter.bits.ZADA_DISPLAY, - &count_wait_filter[2], - TIME_FILTER_ALL_SIGNALS, - edrk.from_shema.bits.ZADA_DISPLAY); - - edrk.from_shema.bits.SVU = !FROM_BSU_SVU; - edrk.from_shema_filter.bits.SVU = filter_digital_input( edrk.from_shema_filter.bits.SVU, - &count_wait_filter[3], - TIME_FILTER_ALL_SIGNALS, - edrk.from_shema.bits.SVU); - - -// edrk.from_shema.bits.KNOPKA_AVARIA = FROM_ALL_KNOPKA_AVARIA; - edrk.from_shema.bits.QTV_ON_OFF = !FROM_SHEMA_QTV_ON_OFF; - edrk.from_shema_filter.bits.QTV_ON_OFF = filter_digital_input( edrk.from_shema_filter.bits.QTV_ON_OFF, - &count_wait_filter[4], - TIME_FILTER_ALL_SIGNALS, - edrk.from_shema.bits.QTV_ON_OFF); - - - - /// FROM_SHEMA_UMP_ON_OFF -// edrk.local_ump_on_off = !FROM_SHEMA_UMP_ON_OFF; -// -// if (edrk.local_ump_on_off) -// { -// if (edrk.local_ump_on_off_count>=TIME_FILTER_UMP_SIGNALS) -// edrk.from_shema.bits.UMP_ON_OFF = 1; -// else -// edrk.local_ump_on_off_count++; -// } -// else -// { -// if (edrk.local_ump_on_off_count==0) -// edrk.from_shema.bits.UMP_ON_OFF = 0; -// else -// edrk.local_ump_on_off_count--; -// } -// -// - edrk.from_shema.bits.UMP_ON_OFF = !FROM_SHEMA_UMP_ON_OFF; - edrk.from_shema_filter.bits.UMP_ON_OFF = filter_digital_input( edrk.from_shema_filter.bits.UMP_ON_OFF, - &count_wait_filter[5], - TIME_FILTER_UMP_SIGNALS, - edrk.from_shema.bits.UMP_ON_OFF); - - - - - - - /// FROM_SHEMA_READY_UMP -// edrk.local_ready_ump = !FROM_SHEMA_READY_UMP; -// -// if (edrk.local_ready_ump) -// { -// if (edrk.local_ready_ump_count>=TIME_FILTER_UMP_SIGNALS) -// edrk.from_shema.bits.READY_UMP = 1; -// else -// edrk.local_ready_ump_count++; -// } -// else -// { -// if (edrk.local_ready_ump_count==0) -// edrk.from_shema.bits.READY_UMP = 0; -// else -// edrk.local_ready_ump_count--; -// } -// - - edrk.from_shema.bits.READY_UMP = !FROM_SHEMA_READY_UMP; - edrk.from_shema_filter.bits.READY_UMP = filter_digital_input( edrk.from_shema_filter.bits.READY_UMP, - &count_wait_filter[6], - TIME_FILTER_UMP_SIGNALS, - edrk.from_shema.bits.READY_UMP); - - - - edrk.from_shema.bits.SVU_BLOCK_QTV = !FROM_SVU_BLOCK_QTV; - edrk.from_shema_filter.bits.SVU_BLOCK_QTV = filter_digital_input( edrk.from_shema_filter.bits.SVU_BLOCK_QTV, - &count_wait_filter[7], - TIME_FILTER_ALL_SIGNALS, - edrk.from_shema.bits.SVU_BLOCK_QTV); - -#endif -} - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -void get_where_oborots(void) -{ - -//// if (CAN_timeout[get_real_out_mbox(MPU_TYPE_BOX,0)-1]==0) -// if (CAN_timeout[get_real_in_mbox(MPU_TYPE_BOX,0)]==0) -// { -// edrk.W_from_SVU = modbus_table_can_in[14].all; -// edrk.W_from_DISPLAY = modbus_table_can_in[16].all; -// } -// else -// { -// edrk.W_from_SVU = 0; -// edrk.W_from_DISPLAY = 0; -// } -// -// -// -// -// if (edrk.from_shema.bits.SVU) -// { -// edrk.W_from_all = edrk.W_from_SVU; -// edrk.W_from_ZADAT4IK = edrk.W_from_all; -// } -// else -// { -// if (edrk.from_shema.bits.ZADA_DISPLAY) -// { -// edrk.W_from_all = edrk.W_from_DISPLAY; -// edrk.W_from_ZADAT4IK = edrk.W_from_all; -// } -// else -// edrk.W_from_all = edrk.W_from_ZADAT4IK; -// } - -} - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// - -unsigned int toggle_status_lamp(unsigned int bb1, unsigned int flag) -{ - - if (bb1==1 && flag==0) - { - return 0; - } - - if (bb1==0 && flag==0) - { - return 0; - } - - if (bb1==1 && flag==1) - { - return 0; - } - - if (bb1==0 && flag==1) - { - return 1; - } - return 0; -} - -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -// -//////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -void update_output_edrk(void) -{ - unsigned int b; - static unsigned int time_toggle1=0,time_toggle2=0; - -//to ingetim - - TO_ING_NAGREV_OFF = !edrk.to_ing.bits.NAGREV_OFF; - TO_ING_NASOS_1_ON = !edrk.to_ing.bits.NASOS_1_ON; - TO_ING_NASOS_2_ON = !edrk.to_ing.bits.NASOS_2_ON; - // TO_ING_RESET_BLOCK_IZOL = !edrk.to_ing.bits.RESET_BLOCK_IZOL; - TO_ING_SMALL_LAMPA_AVARIA = edrk.to_ing.bits.SMALL_LAMPA_AVARIA; - - if (edrk.disable_rascepitel_work) - { - - } - else - { - TO_ING_RASCEPITEL_OFF = !edrk.to_ing.bits.RASCEPITEL_OFF; - TO_ING_RASCEPITEL_ON = !edrk.to_ing.bits.RASCEPITEL_ON; - } - -// ANOTHER PCH - TO_SECOND_PCH_MASTER = edrk.to_second_pch.bits.MASTER; - TO_SECOND_PCH_ALARM = !edrk.to_second_pch.bits.ALARM; - - - -//to_shema -// -//#if (ENABLE_USE_QTV==1) -// TO_STEND_QM_ON_INVERS = !edrk.to_shema.bits.QTV_ON; -//#endif - - - TO_ING_LAMPA_ZARYAD = !edrk.Status_Ready.bits.Batt; - TO_ING_ZARYAD_ON = !edrk.to_ing.bits.ZARYAD_ON; - TO_ING_BLOCK_KEY_OFF = !edrk.to_ing.bits.BLOCK_KEY_OFF; - -#if (MODE_QTV_UPRAVLENIE==1) - TO_SHEMA_QTV_ON_OFF = !edrk.to_shema.bits.QTV_ON_OFF; -#endif - - -#if (MODE_QTV_UPRAVLENIE==2) - TO_SHEMA_QTV_OFF = !edrk.to_shema.bits.QTV_OFF; - TO_SHEMA_QTV_ON = !edrk.to_shema.bits.QTV_ON; -#endif - - TO_SHEMA_ENABLE_QTV = !edrk.to_shema.bits.ENABLE_QTV; - TO_SHEMA_UMP_ON_OFF = !edrk.to_shema.bits.UMP_ON_OFF; - - - - - - - -//lamps APL -// if (edrk.Sbor)// && edrk.from_ing.bits2.GED_NAMAGNI4EN==0) -// { -// if (edrk.Status_Ready.bits.ready5==0) -// edrk.to_zadat4ik.APL_LAMS0.bits.SBOR_SIL_SHEMA = 1; -// else -// edrk.to_zadat4ik.APL_LAMS0.bits.SBOR_SIL_SHEMA = 0; -// } -// else -// { -// edrk.to_zadat4ik.APL_LAMS0.bits.SBOR_SIL_SHEMA = 0; -// } - - - - edrk.to_vpu.BIG_LAMS.bits.GOTOV2 = edrk.Status_Ready.bits.ready_final; - edrk.to_vpu.BIG_LAMS.bits.PEREGRUZKA = edrk.to_zadat4ik.BIG_LAMS.bits.OGRAN_POWER; - edrk.to_vpu.BIG_LAMS.bits.PODDERG_OBOROTS = 0;// - edrk.to_vpu.BIG_LAMS.bits.VPU = edrk.to_zadat4ik.APL_LAMS0.bits.OBOROT_VPU; - -} - - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -// -/////////////////////////////////////////////// -/////////////////////////////////////////////// - -/////////////////////////////////////////////// -void update_lamp_alarm(void) -{ - if ((edrk.errors.e0.all) - || (edrk.errors.e1.all) - || (edrk.errors.e2.all) - || (edrk.errors.e3.all) - || (edrk.errors.e4.all) - || (edrk.errors.e5.all) - || (edrk.errors.e6.all) - || (edrk.errors.e7.all) - || (edrk.errors.e8.all) - || (edrk.errors.e9.all) - || (edrk.errors.e10.all) - || (edrk.errors.e11.all) - || (edrk.errors.e12.all) - ) - { - edrk.to_ing.bits.SMALL_LAMPA_AVARIA = 1; - // edrk.to_second_pch.bits.ALARM = 1; - edrk.summ_errors = 1; - edrk.Stop |= 1; - } - else - { - edrk.to_ing.bits.SMALL_LAMPA_AVARIA = 0; - edrk.to_second_pch.bits.ALARM = 0; - edrk.summ_errors = 0; - } - -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// - - - - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -#define TIME_WAIT_RELE_QTV_ON 30 //2 sec -#define TIME_WAIT_RELE_QTV_OFF 30 //2 sec - -#define TIME_WAIT_ANSWER_QTV_ON TIME_WAIT_ERROR_QTV //150 //15 sec -#define TIME_WAIT_ANSWER_QTV_OFF 50 //4 sec - -/////////////////////////////////////////////// -int qtv_on_off(unsigned int flag) -{ -static unsigned int time_wait_rele_on_qtv=0; -static unsigned int time_wait_rele_off_qtv=0; -static unsigned int time_wait_answer_on_qtv=0; -static unsigned int time_wait_answer_off_qtv=0; -static unsigned int count_err_on = 0; - - -int cmd_qtv=0;//,cmd_p2=0; -static int QTV_Ok = 0; -static int prev_error = 0; - - - cmd_qtv = 0; -// cmd_p2 = 0; - - if ( flag==1 && edrk.summ_errors==0) - { - cmd_qtv = 1; - } - else - { - cmd_qtv = 0; - } - - - - edrk.cmd_to_qtv = cmd_qtv; - - if (cmd_qtv) - { - edrk.to_shema.bits.ENABLE_QTV = 1; - edrk.to_shema.bits.QTV_OFF = 1; - - if ((pause_detect_error(&time_wait_rele_on_qtv,TIME_WAIT_RELE_QTV_ON,1)==0) && edrk.from_shema_filter.bits.QTV_ON_OFF==0) - { -#if (MODE_QTV_UPRAVLENIE==2) - edrk.to_shema.bits.QTV_ON = 1; -#endif -#if (MODE_QTV_UPRAVLENIE==1) - edrk.to_shema.bits.QTV_ON_OFF = 1; -#endif - } - else - edrk.to_shema.bits.QTV_ON = 0; - - - if (pause_detect_error(&time_wait_answer_on_qtv,TIME_WAIT_ANSWER_QTV_ON,1)==0) - { - - if (edrk.from_shema_filter.bits.QTV_ON_OFF==1) - QTV_Ok = 1; - - } - else - { - - // , - if (edrk.from_shema_filter.bits.QTV_ON_OFF==0) - { -#if (WORK_ON_STEND_D) - if (pause_detect_error(&count_err_on,TIME_WAIT_ANSWER_QTV_ON,1)) - { - edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER |= 1; - QTV_Ok = 0; - } -#else - edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER |= 1; - QTV_Ok = 0; -#endif - } - else - count_err_on = 0; - - } - - time_wait_rele_off_qtv = 0; - time_wait_answer_off_qtv = 0; - } - else - { - QTV_Ok = 0; - edrk.to_shema.bits.ENABLE_QTV = 0; - time_wait_rele_on_qtv = 0; - time_wait_answer_on_qtv = 0; - - edrk.to_shema.bits.QTV_ON = 0; - - edrk.to_shema.bits.QTV_ON_OFF = 0; - -// if (pause_detect_error(&time_wait_rele_off_qtv,TIME_WAIT_RELE_QTV_OFF,1)==0) -// edrk.to_shema.bits.QTV_OFF = 1; -// else - edrk.to_shema.bits.QTV_OFF = 0; - - - if (pause_detect_error(&time_wait_answer_off_qtv,TIME_WAIT_ANSWER_QTV_OFF,1)==0) - { - - } - else - { - if (edrk.from_shema_filter.bits.QTV_ON_OFF==1) - edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER |= 1; - } - - - if (prev_error!=edrk.summ_errors && edrk.summ_errors) - { - if (pause_detect_error(&time_wait_rele_off_qtv,TIME_WAIT_RELE_QTV_OFF,1)==1) - time_wait_rele_off_qtv = 0; - } - - - } - - prev_error = edrk.summ_errors; - return (QTV_Ok); - - -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// - -/////////////////////////////////////////////// -void detect_kvitir_from_all(void) -{ - - static int prev_kvitir=0; - - edrk.Kvitir = control_station.active_array_cmd[CONTROL_STATION_CMD_CHECKBACK] - || edrk.from_ing2.bits.KEY_KVITIR - || edrk.from_zadat4ik.bits.KVITIR; - - /* - if (edrk.RemouteFromRS) - edrk.Kvitir = edrk.KvitirRS; - - if (edrk.RemouteFromVPU) - edrk.Kvitir = edrk.KvitirVPU; - - if (edrk.RemouteFromDISPLAY) - edrk.Kvitir = edrk.from_display.bits.KVITIR;//edrk.KvitirDISPLAY; - - if (edrk.RemouteFromMPU) - edrk.Kvitir = edrk.KvitirMPU; -*/ - - if (edrk.Kvitir==1 && prev_kvitir==0) - { - - if (edrk.Status_Ready.bits.ready_final==0 && edrk.Go==0 && edrk.Stop == 1) - { - edrk.KvitirProcess = 1; - project.clear_errors_all_plates(); - clear_errors(); - edrk.KvitirProcess = 0; - } - clear_warnings(); - /* edrk.KvitirDISPLAY = 0; - edrk.KvitirVPU = 0; - edrk.KvitirMPU = 0; - edrk.KvitirSVU = 0; - edrk.KvitirRS = 0; - */ - } - - prev_kvitir = edrk.Kvitir; -} - -/////////////////////////////////////////////// -unsigned int get_ready_1(void) -{ - unsigned int r1, r2; - - - - if (project.cds_in[0].status == component_Ready - && project.cds_in[1].status == component_Ready - && project.cds_out[0].status == component_Ready - && project.cds_tk[0].status == component_Ready - && project.cds_tk[1].status == component_Ready - && project.cds_tk[2].status == component_Ready - && project.cds_tk[3].status == component_Ready - && project.adc[0].status == component_Ready - && project.adc[1].status == component_Ready - && project.hwp[0].status == component_Ready - ) - r2 = 1; - else - r2 = 0; - - - r1 = (edrk.ms.ready1 && edrk.from_ing1.bits.NASOS_NORMA - && edrk.from_ing1.bits.ZAZEML_ON==0 && edrk.from_ing1.bits.ZAZEML_OFF==1 - && edrk.from_ing1.bits.VIPR_PREDOHR_NORMA - && edrk.from_ing1.bits.BLOCK_IZOL_NORMA - && edrk.from_ing1.bits.OP_PIT_NORMA - && edrk.from_ing1.bits.UPC_24V_NORMA - && r2); - - return r1; - - -} -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// - - - -void set_zadanie_u_charge(void) -{ - -// edrk.ZadanieU_Charge = edrk.ZadanieU_Charge_RS; - -// edrk.iq_ZadanieU_Charge = _IQ(edrk.ZadanieU_Charge/NORMA_ACP); - - if (edrk.zadanie.ZadanieU_Charge<=100) - { - edrk.iqMIN_U_ZPT = _IQ(-50.0/NORMA_ACP); - edrk.iqMIN_U_IN = _IQ(-50.0/NORMA_ACP); - } - else - { - - edrk.iqMIN_U_ZPT = _IQ(edrk.zadanie.ZadanieU_Charge*MIN_U_PROC/NORMA_ACP); - edrk.iqMIN_U_IN = _IQ(edrk.zadanie.ZadanieU_Charge*MIN_U_PROC/NORMA_ACP); - - } - - if (edrk.zadanie.ZadanieU_ChargeU_D_MAX_ERROR_GLOBAL) - edrk.iqMAX_U_ZPT_Global = U_D_MAX_ERROR_GLOBAL; - } - - edrk.iqMAX_U_ZPT = edrk.iqMAX_U_ZPT_Global;//_IQ(edrk.zadanie.ZadanieU_Charge*MAX_U_PROC/NORMA_ACP); - edrk.iqMAX_U_IN = _IQ(edrk.zadanie.ZadanieU_Charge*MAX_U_PROC/NORMA_ACP); - - edrk.zadanie.iq_set_break_level = _IQ(NOMINAL_U_BREAK_LEVEL/NORMA_ACP); - -} - - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -// -/////////////////////////////////////////////// -/////////////////////////////////////////////// - -//////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -#define TIME_WAIT_SBOR_1 1 -#define TIME_WAIT_SBOR_2 3 - -void get_sumsbor_command(void) -{ - static unsigned int prev_SBOR_SHEMA = 0; - static unsigned int prev_SBOR_SHEMA_ANOTHER_BS = 0; - unsigned int SBOR_SHEMA_ANOTHER_BS = 0; - static unsigned int Sbor, first=1, w_sbor = 0, Sbor_f = 0; - - Sbor = edrk.SumSbor; - - if (Sbor == 0) - edrk.run_razbor_shema = 0; - - SBOR_SHEMA_ANOTHER_BS = read_cmd_sbor_from_bs(); - - // - if (edrk.Status_Ready.bits.ImitationReady2==0 && - control_station.active_array_cmd[CONTROL_STATION_CMD_CHARGE]==1 && (prev_SBOR_SHEMA==0) - && edrk.from_ing1.bits.ALL_KNOPKA_AVARIA==0 && edrk.summ_errors==0 - && control_station.active_array_cmd[CONTROL_STATION_CMD_UNCHARGE]==0 - ) - { - Sbor = 1; - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_UNCHARGE]==1 - // || edrk.from_shema_filter.bits.RAZBOR_SHEMA // - ) - { - edrk.run_razbor_shema = 1; - // Sbor = 0; - } - - - if (edrk.StartGEDfromZadanie==0 && edrk.run_razbor_shema) - Sbor = 0; - - -// ? - if (SBOR_SHEMA_ANOTHER_BS==0 && prev_SBOR_SHEMA_ANOTHER_BS==1) - { - Sbor = 0; - } - - prev_SBOR_SHEMA = control_station.active_array_cmd[CONTROL_STATION_CMD_CHARGE]; - - prev_SBOR_SHEMA_ANOTHER_BS = SBOR_SHEMA_ANOTHER_BS; - - - // ! - if (edrk.from_ing1.bits.ALL_KNOPKA_AVARIA || edrk.summ_errors) - { - Sbor = 0; - } - - if (Sbor) - { -// if (edrk.flag_second_PCH == 0) -// { -// if (w_sbor=40) - level_go_main = 0; - - -} -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -//#pragma CODE_SECTION(get_start_ged_from_zadanie,".fast_run"); -int get_start_ged_from_zadanie(void) -{ - - // uf const - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_UF_CONST) - { - if (edrk.zadanie.iq_fzad_rmp!=0 && edrk.zadanie.iq_kzad_rmp!=0) - return 1; - else - return 0; - } - else - // scalar oborots - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_OBOROTS) - { - if (edrk.MasterSlave==MODE_SLAVE) - { - if (edrk.zadanie.iq_Izad_rmp!=0 - && edrk.zadanie.iq_limit_power_zad_rmp!=0) - return 1; - else - return 0; - } - else - { - if (edrk.zadanie.iq_oborots_zad_hz_rmp!=0 && edrk.zadanie.iq_Izad_rmp!=0 - && edrk.zadanie.iq_power_zad_rmp!=0 && edrk.zadanie.iq_limit_power_zad_rmp!=0) - return 1; - else - return 0; - } - } - else - // scalar power - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_POWER) - { - if (edrk.zadanie.iq_oborots_zad_hz_rmp!=0 && edrk.zadanie.iq_Izad_rmp!=0 - && edrk.zadanie.iq_power_zad_rmp!=0 && edrk.zadanie.iq_limit_power_zad_rmp!=0) - return 1; - else - return 0; - } - else - // foc oborots - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS) - { - if (edrk.zadanie.iq_oborots_zad_hz_rmp!=0 && edrk.zadanie.iq_Izad_rmp!=0 - && edrk.zadanie.iq_power_zad_rmp!=0 && edrk.zadanie.iq_limit_power_zad_rmp!=0) - return 1; - else - return 0; - } - else - // foc power - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) - { - if (edrk.zadanie.iq_oborots_zad_hz_rmp!=0 && edrk.zadanie.iq_Izad_rmp!=0 - && edrk.zadanie.iq_power_zad_rmp!=0 && edrk.zadanie.iq_limit_power_zad_rmp!=0) - return 1; - else - return 0; - } - else - { - return 0; - } - -} -////////////////////////////////////////////////////////// - - -void cross_stend_automats(void) -{ - unsigned int g; - - edrk.to_shema.bits.CROSS_UMP_ON_OFF = 0; - edrk.to_shema.bits.CROSS_QTV_ON_OFF = 0; - -} - - - - -#define MAX_ERRORS_DETECT_CHANGE_ACTIVE_CONTROL 50 -void check_change_post_upravl(void) -{ - static int prev_active_post_upravl = -1, prev_active_post_upravl_another_bs = -1; - int active_post_upravl = -1, active_post_upravl_another_bs = -1; - static unsigned int count_err = 0; - - - active_post_upravl = get_code_active_post_upravl(); - active_post_upravl_another_bs = edrk.active_post_upravl_another_bs; - - - if (edrk.Status_Ready.bits.ready_final && edrk.Ready2_another_bs) - { - if ((active_post_upravl_another_bs==0 || active_post_upravl==0) && (active_post_upravl==2 || active_post_upravl_another_bs==2)) - { - // - - edrk.errors.e9.bits.CHANGE_ACTIVE_CONTROL_TO_LOCAL_FROM_SVU |= - filter_err_count(&count_err, - MAX_ERRORS_DETECT_CHANGE_ACTIVE_CONTROL, - 1, - 0); - } - else - count_err = 0; - } - - - prev_active_post_upravl = active_post_upravl; - prev_active_post_upravl_another_bs = active_post_upravl_another_bs; - - edrk.active_post_upravl = active_post_upravl; -} - - -int get_code_active_post_upravl(void) -{ - - if (control_station.active_control_station[CONTROL_STATION_TERMINAL_RS232]) - return 3; - else - if (control_station.active_control_station[CONTROL_STATION_TERMINAL_CAN]) - return 4; - else - if (control_station.active_control_station[CONTROL_STATION_INGETEAM_PULT_RS485])//(edrk.RemouteFromDISPLAY) - return 0; - else - if (control_station.active_control_station[CONTROL_STATION_MPU_KEY_CAN]) - return 5; - else - if (control_station.active_control_station[CONTROL_STATION_ZADATCHIK_CAN]) - return 6; - else - if (control_station.active_control_station[CONTROL_STATION_VPU_CAN]) - return 1; - else - if (control_station.active_control_station[CONTROL_STATION_MPU_SVU_CAN]) - return 2; - else - return 10; //error -} - - - - -#define MAX_COUNT_DETECTS_ZERO_U_ZPT 5 - -void auto_detect_zero_u_zpt(void) -{ - static unsigned int old_time_u_zpt1=0, count_detects = 0, flag_detect_zero_u_zpt = 0; - - static _iq prev_uzpt1=0; - static _iq prev_uzpt2=0; - static _iq delta_u = _IQ(3.0/NORMA_ACP); - static _iq minimal_detect_u = _IQ(40.0/NORMA_ACP); - - - - if (edrk.SumSbor==0 && flag_detect_zero_u_zpt==0) - { - // , Uzpt - if (detect_pause_sec(5,&old_time_u_zpt1)) - { - if ( filter.iqU_1_long>=minimal_detect_u || - filter.iqU_2_long>=minimal_detect_u || - (prev_uzpt1-filter.iqU_1_long)>=delta_u || - (prev_uzpt2-filter.iqU_2_long)>=delta_u ) - { - // - count_detects = 0; - } - else - { - if (count_detects=0) - { - if (local_oborots>max_oborots) - local_oborots = max_oborots; - } - else - { - if (local_oborots<-max_oborots) - local_oborots = -max_oborots; - } - - float_oborots = zad_intensiv(1.0, 1.0, float_oborots, local_oborots); - - edrk.oborots = float_oborots; - edrk.power_kw = edrk.oborots * koef_p3/(edrk.count_bs_work+1); - - - - -// -// max_oborots = edrk.zadanie.limit_power_zad/koef_p2; -// max_oborots = my_satur_float(max_oborots,MAX_ZADANIE_OBOROTS_ROTOR,MIN_ZADANIE_OBOROTS_ROTOR, 0); -// -// local_oborots = fast_round(_IQtoF(edrk.zadanie.rmp_oborots_imitation_rmp)*60.0*NORMA_FROTOR); -// if (local_oborots>=0) -// { -// if (local_oborots>max_oborots) -// local_oborots = max_oborots; -// } -// else -// { -// if (local_oborots<-max_oborots) -// local_oborots = -max_oborots; -// } - - } - else - { - - local_power = fast_round(_IQtoF(edrk.zadanie.iq_power_zad_rmp) * NORMA_ACP * NORMA_ACP / 1000.0); - if (edrk.count_bs_work==0) - local_power = my_satur_float(local_power, MAX_ZADANIE_POWER/2, MIN_ZADANIE_POWER/2, 0); - else - local_power = my_satur_float(local_power, MAX_ZADANIE_POWER, MIN_ZADANIE_POWER, 0); - - local_oborots = local_power/koef_p3; - float_oborots = zad_intensiv(1.0, 1.0, float_oborots, local_oborots); - edrk.oborots = float_oborots; - edrk.power_kw = local_power/(edrk.count_bs_work+1);//edrk.oborots * koef_p3; - -// local_power = fast_round(_IQtoF(edrk.zadanie.iq_power_zad_rmp) * NORMA_ACP * NORMA_ACP / 1000.0); -// local_oborots = local_power/koef_p1; - } - - -// float_oborots = zad_intensiv(0.5, 0.5, float_oborots, local_oborots); -// edrk.oborots = float_oborots; -// -// if (edrk.oborots>=0) -// edrk.power_kw = edrk.oborots * koef_p2; -// else -// edrk.power_kw = edrk.oborots * koef_p2; - -// -// -// -// if (edrk.oborots>=0) -// edrk.power_kw = edrk.oborots * koef_p; -// else -// edrk.power_kw = edrk.oborots * (+koef_p); - } - else - { - edrk.oborots = fast_round(_IQtoF(WRotor.iqWRotorSumFilter3)*60.0*NORMA_FROTOR); -// local_power = fast_round(_IQtoF(filter.PowerScalarFilter2) * NORMA_ACP * NORMA_ACP / 1000.0); -// -// if (edrk.oborots>=0) -// edrk.power_kw = local_power; -// else -// edrk.power_kw = -local_power; - - edrk.power_kw = fast_round(_IQtoF(edrk.iq_power_kw_one_filter_znak) * NORMA_ACP * NORMA_ACP / 1000.0); - } - - power_kw_full = edrk.power_kw + edrk.power_kw_another_bs; - -// if (power_kw_full < MINIMAL_POWER_TO_DISPLAY) -// edrk.power_kw_full = 0; -// else - edrk.power_kw_full = power_kw_full; - /////////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////// - - - - edrk.Status_Ready.bits.ready1 = get_ready_1(); - - pump_control(); - - - if (control_station_select_active()) - { - - if (filter_count_error_control_station_select_active<30) - filter_count_error_control_station_select_active++; - else - edrk.errors.e7.bits.NOT_VALID_CONTROL_STATION |= 1; - } - else - filter_count_error_control_station_select_active = 0; - - edrk.current_active_control = get_current_station_control(); - - if (edrk.current_active_controlLEVEL_HWP_I_AF) level = LEVEL_HWP_I_AF; - if (level<0) level = 0; - - if (n_af == 0) - { - if (level != prev_level_all) - { - i_af_protect_d = convert_real_to_mv_hwp(2,level); - if (i_af_protect_d>1500) i_af_protect_d = 1500; // max 1500 mV - - update_maz_level_i_af(n_af, i_af_protect_d); - project.write_all_hwp(); - } - prev_level_all = level; - } -// else -// { -// if (level != prev_level_2) -// { -// i_af_protect_d = convert_real_to_mv_hwp(4,level); -// if (i_af_protect_d>1500) i_af_protect_d = 1500; // max 1500 mV -// -// update_maz_level_i_af(n_af, i_af_protect_d); -// project.write_all_hwp(); -// } -// prev_level_2 = level; -// -// } -} - - -void calc_count_build_revers(void) -{ - static unsigned int prev_b = 0, prev_r = 0; - - if (edrk.Status_Ready.bits.ImitationReady2) - { - detect_work_revers(((edrk.oborots>=0) ? 1 : -1), edrk.zadanie.iq_oborots_zad_hz_rmp, edrk.oborots); - } - else - detect_work_revers(WRotor.RotorDirectionSlow, edrk.zadanie.iq_oborots_zad_hz_rmp, WRotor.iqWRotorSumFilter2); - - if (edrk.count_revers != prev_r) - inc_count_revers(); - - if (edrk.count_sbor != prev_b) - inc_count_build(); - - prev_r = edrk.count_revers; - prev_b = edrk.count_sbor; - -} - - -void prepare_logger_pult(void) -{ - - edrk.pult_data.logger_params[0] = fast_round(_IQtoF(simple_scalar1.iq_decr_mzz_power_filter)*1000.0);//edrk.zadanie.oborots_zad; - edrk.pult_data.logger_params[1] = fast_round(_IQtoF(edrk.Kplus)*1000.0); - edrk.pult_data.logger_params[2] = fast_round(_IQtoF(pll1.vars.pll_Ud)*NORMA_ACP); - edrk.pult_data.logger_params[3] = fast_round(_IQtoF(edrk.f_stator)*NORMA_FROTOR*100.0); - edrk.pult_data.logger_params[4] = fast_round(_IQtoF(edrk.k_stator1)*10000.0); - edrk.pult_data.logger_params[5] = fast_round(_IQtoF(pll1.vars.pll_Uq)*NORMA_ACP); - edrk.pult_data.logger_params[6] = edrk.period_calc_pwm_int2; - edrk.pult_data.logger_params[7] = fast_round(_IQtoF(simple_scalar1.pidF.OutMax)*NORMA_ACP);//edrk.power_kw_full; - edrk.pult_data.logger_params[8] = edrk.Sbor_Mode; - edrk.pult_data.logger_params[9] = edrk.Stage_Sbor; - edrk.pult_data.logger_params[10] = fast_round(_IQtoF(edrk.Izad_out)*NORMA_ACP); - edrk.pult_data.logger_params[11] = edrk.period_calc_pwm_int1; - edrk.pult_data.logger_params[12] = fast_round(_IQtoF(simple_scalar1.pidF.Out)*NORMA_ACP); - edrk.pult_data.logger_params[13] = fast_round(_IQtoF(simple_scalar1.pidF.OutMin)*NORMA_ACP); - edrk.pult_data.logger_params[14] = fast_round(_IQtoF(simple_scalar1.pidPower.Out)*NORMA_ACP); - edrk.pult_data.logger_params[15] = fast_round(_IQtoF(simple_scalar1.pidPower.OutMax)*NORMA_ACP); - edrk.pult_data.logger_params[16] = fast_round(_IQtoF(simple_scalar1.pidF.Ref)*NORMA_FROTOR*1000.0);// //modbus_table_can_in[123].all;// ( , ) - edrk.pult_data.logger_params[17] = modbus_table_can_in[124].all;// () - edrk.pult_data.logger_params[18] = modbus_table_can_in[125].all;// () - edrk.pult_data.logger_params[19] = modbus_table_can_in[134].all;// - edrk.pult_data.logger_params[20] = fast_round(_IQtoF(simple_scalar1.bpsi_curent)*NORMA_FROTOR*1000.0); - edrk.pult_data.logger_params[21] = fast_round(_IQtoF(edrk.from_uom.iq_level_value_kwt)*NORMA_ACP*NORMA_ACP/1000.0); - edrk.pult_data.logger_params[22] = fast_round(_IQtoF(simple_scalar1.iqKoefOgran)*1000.0);//fast_round(_IQtoF(rotor_22220.iqFout)*NORMA_FROTOR*1000.0); - edrk.pult_data.logger_params[23] = fast_round(_IQtoF(simple_scalar1.iqKoefOgranIzad)*1000.0); //fast_round(_IQtoF(edrk.zadanie.iq_limit_power_zad)*NORMA_ACP*NORMA_ACP/1000.0); - edrk.pult_data.logger_params[24] = fast_round(_IQtoF(edrk.all_limit_koeffs.sum_limit)*1000.0); - edrk.pult_data.logger_params[25] = fast_round(_IQtoF(edrk.all_limit_koeffs.uom_limit)*1000.0); - edrk.pult_data.logger_params[26] = fast_round(_IQtoF(edrk.all_limit_koeffs.uin_freq_limit)*1000.0); - edrk.pult_data.logger_params[27] = fast_round(_IQtoF(simple_scalar1.pidF.Fdb)*NORMA_FROTOR*1000.0); // - - -} - - -int calc_auto_moto_pump(void) -{ - volatile long sum_minutes_pump1, sum_minutes_pump2, set_delta_minutes, cur_delta_minutes; - - - sum_minutes_pump1 = 0; - if (edrk.pult_data.data_from_pult.moto[12]>=0) - sum_minutes_pump1 += edrk.pult_data.data_from_pult.moto[12] * 1440; - if (edrk.pult_data.data_from_pult.moto[3]>=0) - sum_minutes_pump1 += edrk.pult_data.data_from_pult.moto[3]; - - sum_minutes_pump2 = 0; - if (edrk.pult_data.data_from_pult.moto[13]>=0) - sum_minutes_pump2 += edrk.pult_data.data_from_pult.moto[13] * 1440; - if (edrk.pult_data.data_from_pult.moto[4]>=0) - sum_minutes_pump2 += edrk.pult_data.data_from_pult.moto[4]; - - cur_delta_minutes = sum_minutes_pump1 - sum_minutes_pump2; - - set_delta_minutes = edrk.pult_data.data_to_pult.TimeToChangePump; - - if (set_delta_minutes==0) - { - return 0; - } - - - if (cur_delta_minutes>set_delta_minutes) - { - return 2; - } - else - if (cur_delta_minutes<-set_delta_minutes) - { - return 1; - } - else - if (edrk.pult_data.data_from_pult.LastWorkPump==0) - { - if (cur_delta_minutes>0) - { - return 1; - } - else - if (cur_delta_minutes<=0) - { - return 2; - } - else - return 0; - } - else - { - if (edrk.pult_data.data_from_pult.LastWorkPump == 1) - return 1; - else - if (edrk.pult_data.data_from_pult.LastWorkPump == 2) - return 2; - else - return 0; - } -// -// -// if (cur_delta_minutes>0) -// { -// //T1>T2 -// if (_IQabs(cur_delta_minutes) >= set_delta_minutes) -// { -// // T1+delta>T2 -// return 2; -// } -// else -// return 1; -// } -// else -// { -// //T2>T1 -// if (_IQabs(cur_delta_minutes) >= set_delta_minutes) -// { -// //T2+delta>T1 -// return 1; -// } -// else -// return 2; -// } - -// if (_IQabs(cur_delta_minutes) > set_delta_minutes) -// { -// if (cur_delta_minutes>) -// return 2; -// else -// return 1; -// -// -// } -// if (cur_delta_minutes>=0) -// { -// if (_IQabs(cur_delta_minutes) > set_delta_minutes) -// return 2; -// else -// return 1; -// } -// else -// { -// if (_IQabs(cur_delta_minutes) > set_delta_minutes) -// return 1; -// else -// return 2; -// } - - - -} - - - -void read_can_error(void) -{ - EALLOW; - edrk.canes_reg = ECanaRegs.CANES.all; - edrk.canrec_reg = ECanaRegs.CANREC.all; - edrk.cantec_reg = ECanaRegs.CANTEC.all; - EDIS; - - cmd_clear_can_error(); - -} - - -void clear_can_error(void) -{ - // EALLOW; - - // ECanaRegs.CANES.all=0xffffffff; - InitCanSoft(); - - //EDIS; - -} - -void cmd_clear_can_error(void) -{ - static int prev_cmd_clear_can_error = 0; - - if (edrk.cmd_clear_can_error && prev_cmd_clear_can_error==0) - { - clear_can_error(); - } - prev_cmd_clear_can_error = edrk.cmd_clear_can_error; - -} - - -void check_temper_break(void) -{ - - if ( (edrk.break_tempers[0] > ABNORMAL_TEMPER_BREAK_INT) - || (edrk.break_tempers[1] > ABNORMAL_TEMPER_BREAK_INT) - || (edrk.break_tempers[2] > ABNORMAL_TEMPER_BREAK_INT) - || (edrk.break_tempers[3] > ABNORMAL_TEMPER_BREAK_INT) - ) - edrk.warnings.e9.bits.BREAK_TEMPER_WARNING = 1; - else - { - if ( (edrk.break_tempers[0] < ABNORMAL_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - && (edrk.break_tempers[1] < ABNORMAL_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - && (edrk.break_tempers[2] < ABNORMAL_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - && (edrk.break_tempers[3] < ABNORMAL_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - ) - edrk.warnings.e9.bits.BREAK_TEMPER_WARNING = 0; - } - - - if ( (edrk.break_tempers[0] > ALARM_TEMPER_BREAK_INT) - || (edrk.break_tempers[1] > ALARM_TEMPER_BREAK_INT) - || (edrk.break_tempers[2] > ALARM_TEMPER_BREAK_INT) - || (edrk.break_tempers[3] > ALARM_TEMPER_BREAK_INT) - ) - edrk.warnings.e9.bits.BREAK_TEMPER_ALARM = 1; - else - { - //DELTA_TEMPER_BREAK_INT - if ( (edrk.break_tempers[0] < ALARM_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - && (edrk.break_tempers[1] < ALARM_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - && (edrk.break_tempers[2] < ALARM_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - && (edrk.break_tempers[3] < ALARM_TEMPER_BREAK_INT-DELTA_TEMPER_BREAK_INT) - ) - edrk.warnings.e9.bits.BREAK_TEMPER_ALARM = 0; - } - -} - -#define TIME_FILTER_BREAKER_SIGNALS 10 - -void check_breaker_ged(void) -{ - static unsigned int count_wait_breaker = 0; - - edrk.warnings.e9.bits.BREAKER_GED_ON = filter_digital_input( edrk.warnings.e9.bits.BREAKER_GED_ON, - &count_wait_breaker, - TIME_FILTER_BREAKER_SIGNALS, - edrk.breaker_on); - -// if (edrk.breaker_on) -// edrk.warnings.e9.bits.BREAKER_GED_ON = 1; -// else -// edrk.warnings.e9.bits.BREAKER_GED_ON = 0; - -} diff --git a/Inu/Src2/551/main/edrk_main.h b/Inu/Src2/551/main/edrk_main.h deleted file mode 100644 index 435baca..0000000 --- a/Inu/Src2/551/main/edrk_main.h +++ /dev/null @@ -1,1838 +0,0 @@ - -#ifndef _EDRK_MAIN_H__ -#define _EDRK_MAIN_H__ - - -#include "IQmathLib.h" -#include "rmp_cntl_v1.h" -#include "rmp_cntl_v2.h" - -#include "alg_pll.h" - - -#define TIME_PAUSE_MODBUS_CAN_BS2BS 500 //900 //500 -#define TIME_PAUSE_MODBUS_CAN_ZADATCHIK_VPU 250 //100//100 -#define TIME_PAUSE_MODBUS_CAN_UKSS_SETUP 2500 // -#define TIME_PAUSE_MODBUS_CAN_MPU 1100 //500 -#define TIME_PAUSE_MODBUS_CAN_TERMINALS 2000 //1000 -#define TIME_PAUSE_MODBUS_CAN_OSCIL 5000 - -//#define TIME_PAUSE_MODBUS_CAN_BS2BS 100//20//500 -//#define TIME_PAUSE_MODBUS_CAN_ZADATCHIK_VPU 250//20//100 -//#define TIME_PAUSE_MODBUS_CAN_UKSS_SETUP 5000 -//#define TIME_PAUSE_MODBUS_CAN_MPU 500//20//500 -//#define TIME_PAUSE_MODBUS_CAN_TERMINALS 1000 -//#define TIME_PAUSE_MODBUS_CAN_OSCIL 5000 - - -//#define TIME_PAUSE_MODBUS_CAN_TMS2TMS_VIPR 75 //500 - - - -//#define FROM_OPENDOOR project.cds_in[1].read.pbus.data_in.bit.in15 - - -// IN0 -#define SENSOR_ROTOR_1 project.cds_in[0].read.pbus.data_in.bit.in0 // 1 -#define SENSOR_ROTOR_2 project.cds_in[0].read.pbus.data_in.bit.in1 // 2 -#define SENSOR_ROTOR_3 project.cds_in[0].read.pbus.data_in.bit.in2 // 3 - -#define FROM_ING_LOCAL_REMOUTE project.cds_in[0].read.pbus.data_in.bit.in3 // LOCAL=0/REMOUTE=1 -#define FROM_ING_LOCAL_KVITIR project.cds_in[0].read.pbus.data_in.bit.in4 // - -#define FROM_BSU_RAZBOR_SHEMA project.cds_in[0].read.pbus.data_in.bit.in5 // =0 -#define FROM_BSU_SBOR_SHEMA project.cds_in[0].read.pbus.data_in.bit.in6 // =0 - -#define FROM_ING_RAZBOR_SHEMA project.cds_in[0].read.pbus.data_in.bit.in5 // =0 -#define FROM_ING_SBOR_SHEMA project.cds_in[0].read.pbus.data_in.bit.in6 // =0 - -#define FROM_ING_OBOROTS_MINUS project.cds_in[0].read.pbus.data_in.bit.in7 // -#define FROM_ING_OBOROTS_PLUS project.cds_in[0].read.pbus.data_in.bit.in8 // - - -#define FROM_BSU_ZADA_DISPLAY project.cds_in[0].read.pbus.data_in.bit.in9 // =1/=0 -#define FROM_BSU_SVU project.cds_in[0].read.pbus.data_in.bit.in10 // =0/=1 -#define FROM_SHEMA_QTV_ON_OFF ((project.cds_in[0].read.pbus.data_in.bit.in12)) - -// , . -#define FROM_SVU_BLOCK_QTV project.cds_in[0].read.pbus.data_in.bit.in11 // QTV - -#define FROM_ING_ANOTHER_RASCEPITEL 1//project.cds_in[0].read.pbus.data_in.bit.in12 // -#define FROM_SHEMA_UMP_ON_OFF project.cds_in[0].read.pbus.data_in.bit.in13 // - -#define FROM_SHEMA_READY_UMP project.cds_in[0].read.pbus.data_in.bit.in14 // -#define FROM_ING_RASCEPITEL_ON_OFF ((project.cds_in[0].read.pbus.data_in.bit.in15)) // - - - -///////////////// - -#define FROM_ING_OP_PIT_NORMA project.cds_in[1].read.pbus.data_in.bit.in0 // 0- -#define FROM_ING_OHLAD_UTE4KA_WATER !(project.cds_in[1].read.pbus.data_in.bit.in1) // // 1 - , 0 - -#define FROM_ING_SOST_ZAMKA project.cds_in[1].read.pbus.data_in.bit.in2 // -#define FROM_ING_ZARYAD_ON project.cds_in[1].read.pbus.data_in.bit.in3 // 1- - -#define FROM_ING_VENTIL_ON project.cds_in[1].read.pbus.data_in.bit.in4 // 0- -#define FROM_ING_NASOS_ON project.cds_in[1].read.pbus.data_in.bit.in5 // 0 - -#define FROM_ING_NASOS_NORMA project.cds_in[1].read.pbus.data_in.bit.in6 // 0? 0. -#define FROM_ING_ZAZEML_OFF project.cds_in[1].read.pbus.data_in.bit.in7 // 1-. -#define FROM_ING_NAGREV_ON project.cds_in[1].read.pbus.data_in.bit.in8 // 1- -#define FROM_ING_BLOCK_IZOL_NORMA project.cds_in[1].read.pbus.data_in.bit.in9 // . 1-. -#define FROM_ING_VIPR_PREDOHR_NORMA project.cds_in[1].read.pbus.data_in.bit.in10 // 0 - . -#define FROM_ING_BLOCK_IZOL_AVARIA project.cds_in[1].read.pbus.data_in.bit.in11 // . 0- -#define FROM_ALL_KNOPKA_AVARIA project.cds_in[1].read.pbus.data_in.bit.in12 // 1 - . -#define FROM_ING_ZAZEML_ON project.cds_in[1].read.pbus.data_in.bit.in13 // 0-. - -#define FROM_ING_ANOTHER_MASTER_PCH project.cds_in[1].read.pbus.data_in.bit.in14 // . - -#define FROM_ING_UPC_24V_NORMA project.cds_in[1].read.pbus.data_in.bit.in15 // 24 UPC 0-. - -//#define FROM_REZERV_12 project.cds_in[1].read.pbus.data_in.bit.in12 // - - - - -#define TO_ING_ZARYAD_ON project.cds_out[0].write.sbus.data_out.bit.dout0 // -#define TO_ING_NAGREV_OFF project.cds_out[0].write.sbus.data_out.bit.dout1 // -#define TO_ING_NASOS_1_ON project.cds_out[0].write.sbus.data_out.bit.dout2 // 1 -#define TO_ING_NASOS_2_ON project.cds_out[0].write.sbus.data_out.bit.dout3 // 2 -#define TO_ING_BLOCK_KEY_OFF project.cds_out[0].write.sbus.data_out.bit.dout4 // , - -#define TO_ING_RELOAD_UPC project.cds_out[0].write.sbus.data_out.bit.dout5 //5- - -#define TO_SHEMA_ENABLE_QTV project.cds_out[0].write.sbus.data_out.bit.dout6 // 6 - QTV -#define TO_ING_LAMPA_ZARYAD project.cds_out[0].write.sbus.data_out.bit.dout7 //7- 80 . - - - -#define MODE_QTV_UPRAVLENIE 2 // 1 - , 2 - - - -#if (MODE_QTV_UPRAVLENIE==1) -////////////////////////////////////////////////////////// -// QTV -#define TO_SHEMA_QTV_ON_OFF project.cds_out[0].write.sbus.data_out.bit.dout8 // 8 - QTV -#endif - - -#if (MODE_QTV_UPRAVLENIE==2) -// QTV -#define TO_SHEMA_QTV_ON project.cds_out[0].write.sbus.data_out.bit.dout8 // 8 - QTV -#define TO_SHEMA_QTV_OFF project.cds_out[0].write.sbus.data_out.bit.dout9 // 9 - QTV -/////////////////////////////////////////////////////////// -#endif - -#define TO_ING_SMALL_LAMPA_AVARIA project.cds_out[0].write.sbus.data_out.bit.dout10 // - -#define TO_SECOND_PCH_ALARM project.cds_out[0].write.sbus.data_out.bit.dout11 // 11 - . -#define TO_SECOND_PCH_MASTER project.cds_out[0].write.sbus.data_out.bit.dout12 // 12 - - . - -#define TO_SHEMA_UMP_ON_OFF project.cds_out[0].write.sbus.data_out.bit.dout13 // 13 - -#define TO_ING_RASCEPITEL_OFF project.cds_out[0].write.sbus.data_out.bit.dout14// 14- -#define TO_ING_RASCEPITEL_ON project.cds_out[0].write.sbus.data_out.bit.dout15// 15 - - - - -enum -{ - ALG_MODE_UF_CONST = 1, - ALG_MODE_SCALAR_OBOROTS, - ALG_MODE_SCALAR_POWER, - ALG_MODE_FOC_OBOROTS, - ALG_MODE_FOC_POWER -}; - - -enum -{ - STAGE_SBOR_STATUS_NO_STATUS = 0, - STAGE_SBOR_STATUS_FIRST, - STAGE_SBOR_STATUS_PUMP, - STAGE_SBOR_STATUS_ZARYAD, - STAGE_SBOR_STATUS_UMP_ON, - STAGE_SBOR_STATUS_QTV, - STAGE_SBOR_STATUS_UMP_OFF, - STAGE_SBOR_STATUS_RASCEPITEL_1, - STAGE_SBOR_STATUS_RASCEPITEL_2, - STAGE_SBOR_STATUS_RASCEPITEL_3, - STAGE_SBOR_STATUS_RASCEPITEL_4, - STAGE_SBOR_STATUS_WAIT_READY_ANOTHER, - STAGE_SBOR_STATUS_FINISH -}; - -/* - - - - - -#define TO_ING_KVITIR project.cds_out[0].write.sbus.data_out.bit.dout3 -#define TO_QTV_OFF project.cds_out[0].write.sbus.data_out.bit.dout4 - -#define TO_ING_VOZB_PODKLU4EN project.cds_out[0].write.sbus.data_out.bit.dout5 -#define TO_ING_VOZB_NEPODKLU4EN project.cds_out[0].write.sbus.data_out.bit.dout6 -#define TO_ING_VOZB_READY project.cds_out[0].write.sbus.data_out.bit.dout7 - - - -#define TO_ING_QTV_VLU4EN project.cds_out[0].write.sbus.data_out.bit.dout9 -#define TO_ING_QTV_READY project.cds_out[0].write.sbus.data_out.bit.dout10 -#define TO_ING_START_GED project.cds_out[0].write.sbus.data_out.bit.dout11 -#define TO_ING_SIL_BLOK_OTKL project.cds_out[0].write.sbus.data_out.bit.dout12 -#define TO_ING_SIL_BLOK_VKL project.cds_out[0].write.sbus.data_out.bit.dout13 -#define TO_ING_BLOK_VOZB_WORK project.cds_out[0].write.sbus.data_out.bit.dout15 -#define TO_ING_OSTANOV_GED project.cds_out[0].write.sbus.data_out.bit.dout14 - - - -#define FROM_ING_SIL_BLOK_VKL project.cds_in[1].read.pbus.data_in.bit.in0 -#define FROM_ING_SIL_BLOK_OTKL project.cds_in[1].read.pbus.data_in.bit.in1 -#define FROM_ING_GED_NAMAGNI4EN project.cds_in[1].read.pbus.data_in.bit.in2 -#define FROM_ING_GED_OSTANOVLEN project.cds_in[1].read.pbus.data_in.bit.in3 -#define FROM_ING_QTV_ON project.cds_in[1].read.pbus.data_in.bit.in4 -#define FROM_ING_QTV_OFF project.cds_in[1].read.pbus.data_in.bit.in5 -#define FROM_ING_VOZB_PODKLU4IT project.cds_in[1].read.pbus.data_in.bit.in6 -#define FROM_ING_VOZB_OTKLU4IT project.cds_in[1].read.pbus.data_in.bit.in7 -#define FROM_ING_VOZB_PUSK project.cds_in[1].read.pbus.data_in.bit.in8 - -*/ - -typedef struct -{ - int adc_temper_u[7]; - float real_temper_u[7]; - int real_int_temper_u[7]; - int max_real_int_temper_u; - - int adc_temper_water[2]; - float real_temper_water[2]; - int real_int_temper_water[2]; //0 - internal; 1 - external - int max_real_int_temper_water; - - int adc_temper_air[4]; - float real_temper_air[4]; - int real_int_temper_air[4]; - int max_real_int_temper_air; - int min_real_int_temper_air; - - - - -} TEMPER_EDRK; -#define TEMPER_EDRK_DEFAULT {{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},0,\ - {0,0},{0,0},{0,0},0,\ - {0,0,0,0},{0,0,0,0},{0,0,0,0},0,0\ - } - - -typedef struct -{ - int adc_p_water[1]; - float real_p_water[1]; - int real_int_p_water[1]; - float filter_real_p_water[1]; - int filter_real_int_p_water[1]; - int flag_init_filter_temp[1]; - -} P_WATER_EDRK; -#define P_WATER_EDRK_DEFAULT {{0},{0},{0},{0},{0},{0}} - - - -typedef struct -{ - - struct - { - int adc_temper[6]; - float real_temper[6]; - int real_int_temper[6]; - float filter_real_temper[6]; - int filter_real_int_temper[6]; - int flag_init_filter_temp[6]; - int max_size; - int max_real_int_temper; - } winding; - - struct - { - int adc_temper[2]; - float real_temper[2]; - int real_int_temper[2]; - float filter_real_temper[2]; - int filter_real_int_temper[2]; - int flag_init_filter_temp[2]; - int max_size; - int max_real_int_temper; - } bear; - -} TEMPER_ACDRIVE; - -#define TEMPER_ACDRIVE_DEFAULT {{{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},6,0},\ - {{0,0},{0,0},{0,0},{0,0},{0,0},{0,0},2,0} } -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// - -typedef struct -{ - union { - unsigned int all; - struct { - unsigned int U_1_MAX: 1; - unsigned int U_2_MAX: 1; - unsigned int U_1_MIN: 1; - unsigned int U_2_MIN : 1; - - unsigned int U_A1B1_MAX: 1; - unsigned int U_A2B2_MAX: 1; - unsigned int U_B1C1_MAX:1; - unsigned int U_B2C2_MAX :1; - - unsigned int U_A1B1_MIN :1; - unsigned int U_A2B2_MIN: 1; - unsigned int U_B1C1_MIN:1; - unsigned int U_B2C2_MIN :1; - - unsigned int U_IN_MAX:1; - unsigned int U_IN_MIN:1; - unsigned int I_1_MAX:1; - unsigned int I_2_MAX:1; - - } bits; - } e0; - - union { - unsigned int all; - struct { - unsigned int I_UO2_MAX: 1; - unsigned int I_UO3_MAX: 1; - unsigned int I_UO4_MAX: 1; - unsigned int I_UO5_MAX : 1; - - unsigned int I_UO6_MAX: 1; - unsigned int I_UO7_MAX: 1; - unsigned int I_BREAK_1_MAX:1; - unsigned int I_BREAK_2_MAX :1; - - unsigned int HWP_ERROR :1; - - unsigned int BLOCK_DOOR: 1; - unsigned int NO_INPUT_SYNC_SIGNAL: 1; - unsigned int NO_CONFIRM_ON_RASCEPITEL: 1; - - unsigned int ANOTHER_BS_NOT_ON_RASCEPITEL: 1; - unsigned int ANOTHER_BS_VERY_LONG_WAIT: 1; - unsigned int VERY_LONG_BOTH_READY2: 1; - unsigned int BOTH_KEYS_CHARGE_DISCHARGE: 1; - - - } bits; - } e1; - - - union { - unsigned int all; - struct { - unsigned int T_UO1_MAX:1; - unsigned int T_UO2_MAX :1; - unsigned int T_UO3_MAX :1; - unsigned int T_UO4_MAX :1; - - unsigned int T_UO5_MAX :1; - unsigned int T_UO6_MAX:1; - unsigned int T_UO7_MAX:1; - unsigned int T_WATER_EXT_MAX:1; - - unsigned int T_WATER_INT_MAX:1; - unsigned int P_WATER_INT_MAX: 1; - unsigned int P_WATER_INT_MIN: 1; - unsigned int T_AIR0_MAX :1; - - unsigned int T_AIR1_MAX :1; - unsigned int T_AIR2_MAX :1; - unsigned int T_AIR3_MAX :1; - unsigned int ERROR_RAZBOR_SHEMA :1; - - - } bits; - } e2; - - - union { - unsigned int all; - struct { - unsigned int NOT_READY_TK_0: 1; - unsigned int NOT_READY_TK_1 : 1; - unsigned int NOT_READY_TK_2: 1; - unsigned int NOT_READY_TK_3: 1; - - unsigned int NOT_READY_OUT_0:1; - unsigned int NOT_READY_OUT_1:1; - unsigned int NOT_READY_OUT_2:1; - unsigned int NOT_READY_IN_0 :1; - - unsigned int NOT_READY_IN_1 :1; - unsigned int NOT_READY_IN_2 :1; - unsigned int NOT_READY_ADC_0: 1; - unsigned int NOT_READY_ADC_1: 1; - - unsigned int NOT_READY_HWP_0: 1; - unsigned int NOT_READY_HWP_1: 1; - unsigned int NOT_READY_CONTR: 1; - unsigned int ERR_INT_PWM_LONG:1; - - - } bits; - } e3; - - - union { - unsigned int all; - struct { - - unsigned int ERR_TK_0: 1; - unsigned int ERR_TK_1: 1; - unsigned int ERR_TK_2: 1; - unsigned int ERR_TK_3: 1; - - unsigned int ERR_OUT_0:1; - unsigned int ERR_OUT_1:1; - unsigned int ERR_OUT_2:1; - unsigned int ERR_IN_0 :1; - - unsigned int ERR_IN_1 :1; - unsigned int ERR_IN_2 :1; - unsigned int ERR_ADC_0:1; - unsigned int ERR_ADC_1:1; - - unsigned int ERR_HWP_0:1; - unsigned int ERR_HWP_1:1; - unsigned int ANOTHER_BS_POWER_OFF:1; - unsigned int FAST_OPTICAL_ALARM:1; - } bits; - } e4; - - union { - unsigned int all; - struct { - - unsigned int LINE_ERR0: 1; - unsigned int LINE_HWP : 1; - unsigned int KEY_AVARIA: 1; - unsigned int PUMP_1: 1; - - unsigned int PUMP_2 : 1; - unsigned int FAN : 1; - unsigned int OP_PIT : 1; - unsigned int POWER_UPC :1; - - unsigned int UTE4KA_WATER :1; - unsigned int T_VIPR_MAX :1; - unsigned int ERROR_PRE_CHARGE_ON: 1; - unsigned int PRE_READY_PUMP: 1; - - unsigned int ERROR_GROUND_NET: 1; - unsigned int ERROR_HEAT: 1; - unsigned int ERROR_ISOLATE: 1; - unsigned int ERROR_PRED_VIPR: 1; - - - } bits; - } e5; - - union { - unsigned int all; - struct { - - unsigned int QTV_ERROR_NOT_ANSWER: 1; - unsigned int QTV_ERROR_NOT_U : 1; - unsigned int ERROR_PRE_CHARGE_U: 1; - unsigned int ERROR_PRE_CHARGE_ANSWER: 1; - - unsigned int UO2_KEYS :1; - unsigned int UO3_KEYS :1; - unsigned int UO4_KEYS :1; - unsigned int UO5_KEYS :1; - - unsigned int UO6_KEYS:1; - unsigned int UO7_KEYS:1; - unsigned int UO1_KEYS:1; - unsigned int ERR_PBUS:1; - - unsigned int ERR_SBUS:1; - unsigned int ER_DISBAL_BATT:1; - unsigned int ER_RAZBALANS_ALG:1; - unsigned int RASCEPITEL_ERROR_NOT_ANSWER:1; - - } bits; - } e6; - - union { - unsigned int all; - struct { - - unsigned int MASTER_SLAVE_SYNC: 1; - unsigned int WRITE_OPTBUS: 1; - unsigned int READ_OPTBUS: 1; - unsigned int ANOTHER_PCH_NOT_ANSWER: 1; - - unsigned int AUTO_SET_MASTER: 1; - unsigned int UMP_NOT_READY: 1; - unsigned int ERROR_SBOR_SHEMA: 1; - unsigned int NOT_VALID_CONTROL_STATION:1; - - unsigned int VERY_FAST_GO_0to1:1; - unsigned int T_ACDRIVE_WINDING_MAX:1; - unsigned int T_ACDRIVE_BEAR_MAX_DNE:1; - unsigned int SVU_BLOCK_ON_QTV:1; - - unsigned int UMP_NOT_ANSWER: 1; - unsigned int ANOTHER_RASCEPITEL_ON: 1; - unsigned int CAN2CAN_BS: 1; - unsigned int ANOTHER_BS_ALARM: 1; - - - - } bits; - } e7; - - union { - unsigned int all; - struct { - - unsigned int LOSS_OUTPUT_U1: 1; - unsigned int LOSS_OUTPUT_V1: 1; - unsigned int LOSS_OUTPUT_W1: 1; - unsigned int LOSS_OUTPUT_U2: 1; - - unsigned int LOSS_OUTPUT_V2: 1; - unsigned int LOSS_OUTPUT_W2: 1; - unsigned int LOSS_INPUT_A1B1: 1; - unsigned int LOSS_INPUT_B1C1: 1; - - unsigned int LOSS_INPUT_A2B2: 1; - unsigned int LOSS_INPUT_B2C2: 1; - unsigned int LOW_FREQ_50HZ: 1; - unsigned int U_IN_10_PROCENTS_LOW: 1; - - unsigned int U_IN_20_PROCENTS_LOW: 1; - unsigned int U_IN_20_PROCENTS_HIGH: 1; - unsigned int DISBALANCE_IM1_IM2: 1; - unsigned int WDOG_OPTICAL_BUS: 1; - - } bits; - } e8; - - union { - unsigned int all; - struct { - unsigned int T_ACDRIVE_BEAR_MAX_NE :1; - unsigned int I_GED_MAX :1; - unsigned int CHANGE_ACTIVE_CONTROL_TO_LOCAL_FROM_SVU :1; - unsigned int DISBALANCE_Uin_1 :1; - - unsigned int DISBALANCE_Uin_2 :1; - unsigned int U_IN_FREQ_NOT_NORMA :1; - unsigned int U_IN_FREQ_NOT_STABLE :1; - unsigned int ERR_PWM_WDOG :1; - - unsigned int ERR_INT_PWM_VERY_LONG : 1; - unsigned int SENSOR_ROTOR_1_BREAK : 1; - unsigned int SENSOR_ROTOR_2_BREAK : 1; - unsigned int SENSOR_ROTOR_1_2_BREAK : 1; - - unsigned int SENSOR_ROTOR_BREAK_DIRECTION : 1; - - unsigned int BREAK_TEMPER_WARNING : 1; - unsigned int BREAK_TEMPER_ALARM : 1; - unsigned int BREAKER_GED_ON : 1; - - } bits; - } e9; - union { - unsigned int all; - struct { - unsigned int WARNING_I_OUT_OVER_1_6_NOMINAL :1; - unsigned int T_BSU_Sensor_BK1 :1; - unsigned int T_BSU_Sensor_BK2 :1; - unsigned int T_ACDRIVE_WINDING_U1 :1; - unsigned int T_ACDRIVE_WINDING_V1 :1; - unsigned int T_ACDRIVE_WINDING_W1 :1; - unsigned int T_ACDRIVE_WINDING_U2 :1; - unsigned int T_ACDRIVE_WINDING_V2 :1; - unsigned int T_ACDRIVE_WINDING_W2 :1; - unsigned int res: 7; - - } bits; - } e10; - union { - unsigned int all; - struct { - unsigned int ERROR_PUMP_ON_SBOR:1; - unsigned int ERROR_RESTART_PUMP_1_ON_SBOR:1; - unsigned int ERROR_RESTART_PUMP_2_ON_SBOR:1; - unsigned int ERROR_RESTART_PUMP_ALL_ON_SBOR:1; - - unsigned int ERROR_PRED_ZARYAD:1; - unsigned int ERROR_PRED_ZARYAD_AFTER:1; - unsigned int ERROR_READY_UMP_BEFORE_QTV:1; - unsigned int ERROR_STATUS_QTV:1; - - unsigned int ERROR_UMP_ON_AFTER :1; - unsigned int ERROR_UMP_NOT_ON:1; - unsigned int ERROR_UMP_NOT_OFF :1; - unsigned int ERROR_RASCEPITEL_WAIT_CMD:1; - - unsigned int ERROR_RASCEPITEL_ON_AFTER:1; - unsigned int ERROR_DISABLE_SBOR:1; - unsigned int ERROR_VERY_LONG_SBOR:1; - unsigned int ERROR_CONTROLLER_BUS:1; -// unsigned int :1; - - } bits; - } e11; - union { - unsigned int all; - struct { - unsigned int res: 16; - - } bits; - } e12; - -} ERRORS_EDRK; - - -#define ERRORS_EDRK_DEFAULT {0,0,0,0,0,0,0,0,0,0,0,0} - -//////////////////////////////////////////////////////// - -typedef struct -{ - struct - { - unsigned int alive_can_to_another_bs; - unsigned int alive_sync_line; - unsigned int alive_sync_line_local; - unsigned int alive_opt_bus_read; - unsigned int alive_opt_bus_write; - unsigned int input_master_slave; - unsigned int input_alarm_another_bs; - unsigned int another_rascepitel; - unsigned int fast_optical_alarm; - } err_lock_signals; - struct - { - unsigned int alive_can_to_another_bs; - unsigned int alive_sync_line; - unsigned int alive_sync_line_local; - unsigned int alive_opt_bus_read; - unsigned int alive_opt_bus_write; - unsigned int input_master_slave; - unsigned int input_alarm_another_bs; - unsigned int another_rascepitel; - unsigned int fast_optical_alarm; - } err_signals; - struct - { - unsigned int alive_can_to_another_bs; - unsigned int alive_sync_line; - unsigned int alive_sync_line_local; - unsigned int alive_opt_bus_read; - unsigned int alive_opt_bus_write; - unsigned int input_master_slave; - unsigned int input_alarm_another_bs; - unsigned int another_rascepitel; - unsigned int fast_optical_alarm; - } warning_signals; - struct - { - unsigned int alive_can_to_another_bs; - unsigned int alive_sync_line; - unsigned int alive_sync_line_local; - unsigned int alive_opt_bus_read; - unsigned int alive_opt_bus_write; - unsigned int input_master_slave; - unsigned int input_alarm_another_bs; - unsigned int another_rascepitel; - unsigned int fast_optical_alarm; - } errors_count; - struct - { - unsigned int alive_can_to_another_bs; - unsigned int alive_sync_line; - unsigned int alive_sync_line_local; - unsigned int alive_opt_bus_read; - unsigned int alive_opt_bus_write; - unsigned int input_master_slave; - unsigned int input_alarm_another_bs; - unsigned int another_rascepitel; - unsigned int fast_optical_alarm; - } wait_count; - - - unsigned int sum_err; // - unsigned int sum_warning; // - unsigned int another_bs_maybe_on; // - unsigned int another_bs_maybe_off; // - unsigned int ready1; // 1 - unsigned int ready2; // 2 - unsigned int ready3; // master/slave - unsigned int count_time_wait_ready1; // - unsigned int count_time_wait_ready2; // - unsigned int status; // , : , , . - - - -} MASTER_SLAVE_COM; -#define MASTER_SLAVE_COM_DEFAULT {{0,0,0,0,0,0,0,0},\ - {0,0,0,0,0,0,0,0},\ - {0,0,0,0,0,0,0,0},\ - {0,0,0,0,0,0,0,0},\ - {0,0,0,0,0,0,0,0},\ - 0,0,0,0,0,0,0,0,0} - - - -//////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////// - -typedef union { - unsigned int all; - struct { - - unsigned int master: 1; - unsigned int slave: 1; - unsigned int try_master: 1; - unsigned int try_slave: 1; - - unsigned int nothing: 1; - unsigned int sync1_2: 1; - unsigned int bus_off: 1; - unsigned int in_err: 1; - unsigned int sync_line_detect:1; - unsigned int tick:1; - - - } bits; - - } AUTO_MASTER_SLAVE_DATA; -//////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - - unsigned int ready1: 1; - unsigned int ready2: 1; - unsigned int ready3: 1; - unsigned int ready4: 1; - - unsigned int ready5: 1; - unsigned int ready6: 1; - unsigned int ready7: 1; - unsigned int ready_final: 1; - - unsigned int Batt: 1; - unsigned int ImitationReady2: 1; - unsigned int MasterSlaveActive: 1; // master slave - unsigned int preImitationReady2: 1; - - unsigned int res:4; - } bits; - } STATUS_READY; -//////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - - unsigned int ZARYAD_ON: 1; - unsigned int NAGREV_OFF: 1; - unsigned int NASOS_1_ON: 1; - unsigned int NASOS_2_ON: 1; - - unsigned int BLOCK_KEY_OFF: 1; - unsigned int RESET_BLOCK_IZOL: 1; - unsigned int SMALL_LAMPA_AVARIA: 1; - unsigned int RASCEPITEL_OFF: 1; - - unsigned int RASCEPITEL_ON: 1; - - unsigned int res:7; - } bits; - } TO_ING; -//////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int OP_PIT_NORMA : 1; - unsigned int OHLAD_UTE4KA_WATER: 1; - unsigned int RASCEPITEL_ON: 1; - unsigned int ZARYAD_ON: 1; - - unsigned int VENTIL_ON : 1; - unsigned int NASOS_ON: 1; - unsigned int NASOS_NORMA: 1; - unsigned int ZAZEML_OFF: 1; - - unsigned int NAGREV_ON: 1; - unsigned int BLOCK_IZOL_NORMA: 1; - unsigned int VIPR_PREDOHR_NORMA: 1; - unsigned int UPC_24V_NORMA: 1; - - unsigned int LOCAL_REMOUTE: 1; - unsigned int ZAZEML_ON: 1; - unsigned int ALL_KNOPKA_AVARIA: 1; - unsigned int BLOCK_IZOL_AVARIA: 1; - } bits; - } FROM_ING1; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int KEY_PLUS : 1; - unsigned int KEY_MINUS : 1; - unsigned int KEY_SBOR : 1; - unsigned int KEY_RAZBOR : 1; - - unsigned int KEY_KVITIR : 1; - unsigned int SOST_ZAMKA : 1; - - unsigned int res: 9; - - } bits; - } FROM_ING2; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int MASTER: 1; - unsigned int RASCEPITEL: 1; - unsigned int res:14; - } bits; - } FROM_SECOND_PCH; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int MASTER: 1; - unsigned int ALARM: 1; - - unsigned int res:14; - } bits; - } TO_SECOND_PCH; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int QTV_ON: 1; - unsigned int QTV_OFF: 1; - unsigned int UMP_ON_OFF: 1; - unsigned int ENABLE_QTV: 1; - unsigned int QTV_ON_OFF: 1; - unsigned int CROSS_UMP_ON_OFF: 1; - unsigned int CROSS_QTV_ON_OFF: 1; - - unsigned int res:9; - } bits; - } TO_SHEMA; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int QTV_ON_OFF: 1; - // unsigned int KNOPKA_AVARIA: 1; - unsigned int ZADA_DISPLAY : 1; - unsigned int RAZBOR_SHEMA :1 ; - unsigned int SBOR_SHEMA : 1; - // unsigned int OPENDOOR : 1; - unsigned int SVU : 1; - // unsigned int ACTIVE : 1; - unsigned int READY_UMP : 1; - unsigned int UMP_ON_OFF : 1; - unsigned int SVU_BLOCK_QTV : 1; - - // unsigned int res:10; - } bits; - } FROM_SHEMA; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int KVITIR: 1; - unsigned int PLUS: 1; - unsigned int MINUS : 1; - unsigned int PROVOROT :1 ; - - unsigned int UOM_READY_ACTIVE : 1; - unsigned int UOM_LIMIT_3 : 1; - unsigned int UOM_LIMIT_2 : 1; - unsigned int UOM_LIMIT_1 : 1; - - - unsigned int res:8; - } bits; - } FROM_ZADAT4IK; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int SBOR_SHEMA: 1; - unsigned int RAZBOR_SHEMA: 1; - unsigned int KVITIR: 1; - unsigned int ACTIVE : 1; - unsigned int res:12; - } bits; - } FROM_RS; - //////////////////////////////////////////////////////// - typedef union { - unsigned int all; - struct { - unsigned int SBOR_SHEMA: 1; - unsigned int RAZBOR_SHEMA: 1; - unsigned int KVITIR: 1; - unsigned int ACTIVE : 1; - unsigned int BLOCKED : 1; - unsigned int res:11; - } bits; - } FROM_DISPLAY; - //////////////////////////////////////////////////////// - typedef struct { - - union { - int all; - } OBOROTS1; - - - union { - int all; - } OBOROTS2; - - union { - unsigned int all; - struct { - unsigned int GOTOV1 : 1; - unsigned int GOTOV2 : 1; - unsigned int EMKOST : 1; //For 23550.3 and AVARIA moved up - unsigned int NEISPRAVNOST : 1; - unsigned int PEREGREV : 1; - unsigned int OGRAN_POWER : 1; - unsigned int AVARIA : 1; - unsigned int res:9; - } bits; - } BIG_LAMS; - - union { - unsigned int all; - struct { - unsigned int PCH1_READY1: 1; - unsigned int PCH1_SHEMA_SOBRANA: 1; - unsigned int PCH1_PODKLU4EN: 1; - unsigned int PCH1_MESTNOE: 1; - - unsigned int PCH2_READY1: 1; - unsigned int PCH2_SHEMA_SOBRANA: 1; - unsigned int PCH2_PODKLU4EN: 1; - unsigned int PCH2_MESTNOE: 1; - - unsigned int GED_PEREGREV: 1; - unsigned int PCH1_PCH2_SYNC: 1; - unsigned int GED_PEREGRUZ: 1; - unsigned int OBOROT_SVU: 1; - - unsigned int OBOROT_ZADAT: 1; - unsigned int OBOROT_MONITOR: 1; - unsigned int OBOROT_VPU: 1; - unsigned int HOD: 1; - } bits; - } APL_LAMS0; - - union { - unsigned int all; - struct { - unsigned int PCH_READY1: 1; - unsigned int PCH_SHEMA_SOBRANA: 1; - unsigned int PCH_PODKLU4EN: 1; - unsigned int PCH_MESTNOE: 1; - unsigned int reserv: 12; - } bits; - } APL_LAMS_PCH; - - } TO_ZADAT4IK; - -#define TO_ZADAT4IK_DEFAULT {0,0,0,0,0} - - - //////////////////////////////////////////////////////// - typedef struct { - - union { - int all; - } OBOROTS1; - - - union { - int all; - } OBOROTS2; - - union { - unsigned int all; - struct { - unsigned int VPU: 1; - unsigned int GOTOV2: 1; - unsigned int PODDERG_OBOROTS : 1; - unsigned int PEREGRUZKA :1 ; - unsigned int res:12; - } bits; - } BIG_LAMS; - - } TO_VPU; - -#define TO_VPU_DEFAULT {0,0,0} - - //////////////////////////////////////////////////////// - typedef struct { - - unsigned int level_value; - unsigned int ready; - unsigned int error; - unsigned int code; - _iq iq_level_value; - _iq iq_level_value_kwt; - - - - union { - unsigned int all; - struct { - unsigned int ready: 1; - unsigned int level0: 1; - unsigned int level1: 1; - unsigned int level2: 1; - unsigned int level3: 12; - } bits; - } digital_line; - - } FROM_UOM; - //////////////////////////////////////////////////////// - //////////////////////////////////////////////////////// - //////////////////////////////////////////////////////// - //////////////////////////////////////////////////////// - //////////////////////////////////////////////////////// - typedef struct { - - int int_ZadanieU_Charge; - float ZadanieU_Charge; - _iq iq_ZadanieU_Charge; - _iq iq_ZadanieU_Charge_rmp; - - int int_oborots_zad; - float oborots_zad; - float oborots_zad_hz; - _iq iq_oborots_zad_hz; - _iq iq_oborots_zad_hz_rmp; - - int int_fzad; - float fzad; - _iq iq_fzad; - _iq iq_fzad_rmp; - - int int_kzad; - float kzad; - _iq iq_kzad; - _iq iq_kzad_rmp; - - int int_Izad; - float Izad; - _iq iq_Izad; - _iq iq_Izad_rmp; - - int int_power_zad; - float power_zad; - _iq iq_power_zad; - _iq iq_power_zad_rmp; - - } ZADANIE_FROM_ANOTHER_BS; - -#define ZADANIE_FROM_ANOTHER_BS_DEFAULT {\ - 0,0,0,0, 0,0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0 } - - //////////////////////////////////////////////////////// - -typedef struct { - - RMP_V1 rmp_ZadanieU_Charge; - RMP_V1 rmp_fzad; - RMP_V1 rmp_k_u_disbalance; - RMP_V1 rmp_kplus_u_disbalance; - RMP_V1 rmp_Izad; - - RMP_V2 rmp_powers_zad; - - RMP_V2 rmp_limit_powers_zad; - RMP_V1 rmp_kzad; - - RMP_V2 rmp_oborots_zad_hz; - - RMP_V1 rmp_oborots_imitation; - - _iq rmp_oborots_imitation_rmp; - - - float ZadanieU_Charge; - _iq iq_ZadanieU_Charge; - _iq iq_ZadanieU_Charge_rmp; - - float oborots_zad; - - float oborots_zad_hz; - _iq iq_oborots_zad_hz; - _iq iq_oborots_zad_hz_rmp; - - float fzad; - _iq iq_fzad; - _iq iq_fzad_rmp; - - float kzad; - _iq iq_kzad; - _iq iq_kzad_rmp; - - float k_u_disbalance; - _iq iq_k_u_disbalance; - _iq iq_k_u_disbalance_rmp; - - float kplus_u_disbalance; - _iq iq_kplus_u_disbalance; - _iq iq_kplus_u_disbalance_rmp; - - float Izad; - _iq iq_Izad; - _iq iq_Izad_rmp; - - float power_zad; - _iq iq_power_zad; - _iq iq_power_zad_rmp; - - float limit_power_zad; - _iq iq_limit_power_zad; - _iq iq_limit_power_zad_rmp; - - _iq iq_set_break_level; - - float oborots_zad_no_dead_zone; - -} ZADANIE; - -#define ZADANIE_DEFAULT { RMP_V1_DEFAULTS, RMP_V1_DEFAULTS,RMP_V1_DEFAULTS,\ - RMP_V1_DEFAULTS,RMP_V1_DEFAULTS,\ - RMP_V2_DEFAULTS,\ - RMP_V2_DEFAULTS,RMP_V1_DEFAULTS,\ - RMP_V2_DEFAULTS,\ - RMP_V1_DEFAULTS,\ - 0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0 , 0,0,0, 0,0} - -typedef union { - struct { - unsigned int limit_by_temper:1; - unsigned int limit_Iout:1; - unsigned int limit_UOM:1; // - unsigned int limit_from_SVU:1; - unsigned int limit_from_freq:1; - unsigned int limit_from_uom_fast:1; - unsigned int limit_moment:1; - - unsigned int res:9; - } bits; - unsigned int all; -} POWER_LIMIT; - -#define POWER_LIMIT_DEFAULTS {0} - - -typedef struct { - - _iq temper_limit; - _iq power_limit; - _iq moment_limit; - _iq uin_freq_limit; - _iq uom_limit; - - _iq local_temper_limit; - _iq local_power_limit; - _iq local_moment_limit; - _iq local_uin_freq_limit; - _iq local_uom_limit; - - _iq sum_limit; - _iq local_sum_limit; - int code_status; - -} ALL_LIMIT_KOEFFS; - -#define ALL_LIMIT_KOEFFS_DEFAULTS {0,0,0,0,0, 0,0,0,0,0, 0,0,0} - -typedef struct { - _iq power_units; - _iq area; - _iq water_int; - _iq water_ext; - _iq acdrive_windings; - - _iq acdrive_bears; - _iq sum_limit; - int code_status; -} TEMPERATURE_LIMIT_KOEFFS; - -#define TEMPERATURE_LIMIT_KOEFFS_DEFAULTS {0,0,0,0,0, 0,CONST_IQ_1,0} - -#define COUNT_MOTO_PULT 18 - -typedef struct -{ - - int nPCH; - int TimeToChangePump; - - int count_revers; // - - int count_build; // - - - int LastWorkPump; // - - int moto[COUNT_MOTO_PULT]; - -} t_params_pult_ing_one; - -#define PARAMS_PULT_ING_ONE_DEFAULTS {-1,-1, -1,-1, 0, \ - {-1,-1,-1,-1,-1, -1,-1,-1,-1,-1, -1,-1,-1,-1,-1, -1,-1,-1 } } - -#define PARAMS_PULT_ING_TWO_DEFAULTS {0,0, 0,0, 0, \ - {0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0 } } - -typedef struct -{ - int flagSaveDataPCH; - int flagSaveDataMoto; - int flagSaveSlowLogs; - int flagSaveParamLogs; - - int logger_params[28]; - - t_params_pult_ing_one data; - t_params_pult_ing_one data_from_pult; - t_params_pult_ing_one data_to_pult; - -// int nPCH_from_pult; -// int nPCH_to_pult; -// int nPCH; -// -// int TimeToChangePump_from_pult; -// int TimeToChangePump_to_pult; -// int TimeToChangePump; -// -// int moto_from_pult[COUNT_MOTO_PULT]; -// int mot_to_pult[COUNT_MOTO_PULT]; -// int moto[COUNT_MOTO_PULT]; -// -// int count_revers_from_pult; -// int count_revers_to_pult; - -} t_params_pult_ing; - -#define PARAMS_PULT_ING_DEFAULTS {0,0,0,0, \ - {0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0}, \ - PARAMS_PULT_ING_TWO_DEFAULTS, \ - PARAMS_PULT_ING_ONE_DEFAULTS, \ - PARAMS_PULT_ING_ONE_DEFAULTS \ - } - - - - -typedef struct -{ - int log_what_memory; // 0 - , ; - // 1 - , ; - // 2 - , ; - // 3 - ; - int kvitir; // - int sbor; - int send_log; - int pump_mode; - int sdusb;// 0 - sd, 1 usb - -} t_pult_cmd_ing; - -#define PULT_CMD_ING_DEFAULTS 0,0,0,0,0,0 - - - -//////////////////////////////////////////////////////// -typedef struct -{ - ZADANIE zadanie; - ZADANIE_FROM_ANOTHER_BS zadanie_from_another_bs; - TEMPER_EDRK temper_edrk; - P_WATER_EDRK p_water_edrk; - ERRORS_EDRK errors; - ERRORS_EDRK warnings; - TEMPER_ACDRIVE temper_acdrive; - - POWER_LIMIT power_limit; - TEMPERATURE_LIMIT_KOEFFS temper_limit_koeffs; - ALL_LIMIT_KOEFFS all_limit_koeffs; - - MASTER_SLAVE_COM ms; - - //////// - - struct { - - AUTO_MASTER_SLAVE_DATA local; // 1 - AUTO_MASTER_SLAVE_DATA prev_local;//1 - AUTO_MASTER_SLAVE_DATA remoute;//1 - AUTO_MASTER_SLAVE_DATA prev_remoute;//1 - unsigned int status; - unsigned int prev_status; - } auto_master_slave; // 6 - - - - STATUS_READY Status_Ready; //1 - - TO_ING to_ing; //1 - - FROM_ING1 from_ing1;//1 - - FROM_ING2 from_ing2;//1 - - FROM_SECOND_PCH from_second_pch;//1 - - TO_SECOND_PCH to_second_pch;//1 - - TO_SHEMA to_shema;//1 - - FROM_SHEMA from_shema;//1 - FROM_SHEMA from_shema_filter;//1 - - FROM_ZADAT4IK from_zadat4ik;//1 - - FROM_ZADAT4IK from_vpu;//1 - - FROM_RS from_rs;//1 - - FROM_RS from_can;//1 - - FROM_DISPLAY from_display;//1 - - FROM_DISPLAY from_mpu;//1 - - FROM_DISPLAY from_svu;//1 - - TO_ZADAT4IK to_zadat4ik;//5 - - - - TO_VPU to_vpu;//3 - - FROM_UOM from_uom;//7 -/////////////////////////////////////////////// - - unsigned int Discharge; - unsigned int ManualDischarge; - unsigned int NoDetectUZeroDischarge; - - unsigned int TimeSbor; - unsigned int TimeRazbor; - - unsigned int AutoStartPump; - unsigned int SumStartPump; - unsigned int ManualStartPump; - - - - unsigned int SumSbor; - - int Kvitir; - int KvitirProcess;//10 - - unsigned int prevGo; - unsigned int Go; - unsigned int GoBreak; - - unsigned int GoWait; - - int flag_wait_set_to_zero_zadanie; - int flag_block_zadanie; - int StartGEDfromControl; - int StartGEDfromZadanie; - int prevStartGEDfromZadanie; - - int StartGED; - int test_mode; - int cmd_to_qtv;//20 - int cmd_to_ump;//20 - int prepare_stop_PWM; - - int StartGEDfromSyncBus; - - - int cmd_to_rascepitel; - - int Mode_ScalarVectorUFConst; - int Mode_OborotsOrPower; - - int SelectPump1_2;//25 - - int summ_errors; - int Status_Charge; - - unsigned int Sbor_Mode; - unsigned int Razbor_Mode; - unsigned int time_wait_sbor; - - int Status_Sbor; - int Stage_Sbor; - int StatusPumpFanAll; - - int StatusPump0; - int StatusPump1; - int StatusFunAll; - int StatusPumpAll;//35 - - - int Run_Pred_Zaryad; - int Zaryad_OK; - int Rascepitel_OK; - int Run_QTV; - int Run_Rascepitel; - int Run_Rascepitel_from_RS; - int Run_UMP; - int Zaryad_UMP_Ok; - int Status_UMP_Ok; - - - int Status_QTV_Ok; - int Status_Rascepitel_Ok; - int Status_Perehod_Rascepitel; - int Final_Status_Rascepitel; - int you_can_on_rascepitel; - int RunZahvatRascepitel; - int RunUnZahvatRascepitel; - - _iq iqMAX_U_ZPT; - _iq iqMAX_U_ZPT_Global; - _iq iqMAX_U_ZPT_Predzaryad; - _iq iqMIN_U_ZPT;//50 - - _iq iqMAX_U_IN; - _iq iqMIN_U_IN; - - int SborFinishOk; - int RazborNotFinish; - - int Obmotka1; - int Obmotka2; - - _iq f_stator; - - _iq k_stator1; - _iq k_stator2;//60 - - _iq iq_f_rotor_hz; - float f_rotor_hz; - int oborots; - int rotor_direction; - int power_kw; -// _iq iq_oborots; - - _iq Izad_out; - - unsigned int period_calc_pwm_int1; - unsigned int period_calc_pwm_int2; - - unsigned int count_lost_interrupt; - unsigned int into_pwm_interrupt; - - int disable_alg_u_disbalance; - - _iq Kplus; - _iq Kminus; - - unsigned int Revers; - - _iq Uzad_max; - - _iq iq_bpsi_normal; - - _iq iq_f_provorot;//70 - - int flag_second_PCH; - int test; - - int Stop; - - int warning; - int overheat; - - unsigned int MasterSlave; - - _iq master_theta; - _iq master_Uzad; - _iq master_Iq; - _iq master_Izad; - _iq tetta_to_slave; - _iq Uzad_to_slave; - _iq Iq_to_slave; - _iq P_from_slave; - _iq P_to_master;//82 - - int flag_wait_both_ready2; - - int number_can_box_terminal_cmd; - int number_can_box_terminal_oscil; - - int Provorot; - int int_koef_ogran_power; - _iq iq_koef_ogran_power; - - int int_koef_ogran_power_another_bs; - _iq iq_koef_ogran_power_another_bs; - - int power_kw_another_bs; - _iq iq_power_kw_another_bs; - - int run_razbor_shema; - - int Ready1_another_bs; - int Ready2_another_bs; - - int ump_cmd_another_bs; - int qtv_cmd_another_bs; - - int active_post_upravl; - int active_post_upravl_another_bs; - int MasterSlave_another_bs; - int freq_50hz_1; - int freq_50hz_2; - - _iq test_rms_Iu; - _iq test_rms_Ua; -// - int disable_interrupt_pwm; - - int disable_interrupt_timer1; - int disable_interrupt_timer2; - int disable_interrupt_timer3; - int disable_interrupt_timer4; - - int disable_interrupt_sync; - - int get_new_data_from_hmi; - int flag_enable_update_hmi; - - int flag_disable_pult_485; - - int disable_rascepitel_work; - - int enable_pwm_test_lines; - int count_bs_work; - - unsigned int run_to_pwm_async; - - int power_kw_full; - - int stop_logs_rs232; - int sbor_wait_ump1; - int sbor_wait_ump2; - int flag_enable_on_ump; - - int local_ump_on_off; - int local_ump_on_off_count; - int local_ready_ump; - int local_ready_ump_count; - - t_params_pult_ing pult_data; - - int logs_rotor; - - int stop_slow_log; - int t_slow_log; - int disable_limit_power_from_svu; - int disable_uom; - int disable_break_work; - - int flag_another_bs_first_ready12; - int flag_this_bs_first_ready12; - int enter_to_pump_stage; - - int buildYear; - int buildMonth; - int buildDay; - - int errors_another_bs_from_can; - - unsigned int count_sbor; - unsigned int count_revers; - unsigned int count_run; - - int flag_slow_in_main; - - t_pult_cmd_ing pult_cmd; - - int get_new_data_from_hmi2; - - _iq iq_freq_50hz; - - int imit_limit_freq; - int imit_limit_uom; - int set_limit_uom_50; - - _iq iq_power_kw_full_znak; - _iq iq_power_kw_one_znak; - - _iq iq_power_kw_full_filter_znak; - _iq iq_power_kw_one_filter_znak; - - _iq iq_power_kw_full_abs; - _iq iq_power_kw_one_abs; - - _iq iq_power_kw_full_filter_abs; - _iq iq_power_kw_one_filter_abs; - - unsigned int sum_count_err_read_opt_bus; - - int imit_save_slow_logs; - - int imit_send_alarm_log_pult; - - int current_active_control; - - // int data_to_message2[100]; -//101 - unsigned long canes_reg; - unsigned long cantec_reg; // Transmit Error Counter - unsigned long canrec_reg; // Receive Error Counter - - int cmd_clear_can_error; - - int cmd_very_slow_start; - - int cmd_imit_low_isolation; - - - int breaker_on; - int break_tempers[4]; - - int cmd_disable_calc_km_on_slave; - - -} EDRK; - - - -#define EDRK_DEFAULT { \ - ZADANIE_DEFAULT,\ - ZADANIE_FROM_ANOTHER_BS_DEFAULT,\ - TEMPER_EDRK_DEFAULT, \ - P_WATER_EDRK_DEFAULT, \ - ERRORS_EDRK_DEFAULT, \ - ERRORS_EDRK_DEFAULT, \ - TEMPER_ACDRIVE_DEFAULT, \ - POWER_LIMIT_DEFAULTS, \ - TEMPERATURE_LIMIT_KOEFFS_DEFAULTS, \ - ALL_LIMIT_KOEFFS_DEFAULTS, \ - MASTER_SLAVE_COM_DEFAULT, \ - 0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0, \ - 0,0,0,0, \ - 0,0,0, \ - 0,0,0,0,0,0,0, \ - \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0,0,0,0,0,0,0, 0,0, \ - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, \ - 0,0,0,0, \ - PARAMS_PULT_ING_DEFAULTS, \ - 0,0,0,0,0,0,0,0, \ - 0,0,0,\ - 0, 0,0,0, 0, \ - PULT_CMD_ING_DEFAULTS, 0,0, 0,0,0, 0,0,0,0, 0,0,0,0, \ - 0, 0,0, 0, 0, \ - 0,0,0, 0, 0, 0, \ - 0, {0,0,0,0}, \ - 0 \ - } - - -extern EDRK edrk; -extern PLL_REC pll1; - -//float get_sensor_ing(void); -//float get_i_vozbud(void); -//float get_zad_vozbud(void); - -//unsigned int convert_w_to_mA(float inp); - -void edrk_init(void); - - -void update_input_edrk(void); -void update_output_edrk(void); - - -//float get_amper_vozbud(void); - -//void set_amper_vozbud(float set_curr, float cur_curr); - - - -//void write_dac(int ndac, int Value); - -void run_edrk(void); - - - -void set_oborots_from_zadat4ik(void); -void get_where_oborots(void); - -//void update_errors(void); - - - - - - -//unsigned int zaryad_on_off(unsigned int flag); - - -void update_lamp_alarm(void); - - - - - - -//int get_status_temper_acdrive_winding(int nc); -//int get_status_temper_acdrive_bear(int nc); -//int get_status_temper_air(int nc); -//int get_status_temper_u(int nc); -//int get_status_temper_water(int nc); -//int get_status_p_water_max(void); -//int get_status_p_water_min(int pump_on_off); - -void detect_kvitir_from_all(void); -//void set_status_pump_fan(void); - - - -int qtv_on_off(unsigned int flag); - -///int detect_error_u_zpt(void); -//int detect_error_u_zpt_on_predzaryad(void); - -void set_zadanie_u_charge(void); - - - - - - - - - - - -void edrk_init_variables(void); - -void edrk_init_before_main(void); -void edrk_init_before_loop(void); -void edrk_go_main(void); - -int get_start_ged_from_zadanie(void); - - -//void UpdateTableSecondBS(void); - -unsigned int get_ready_1(void); - -//int detect_zaryad_ump(void); - -void cross_stend_automats(void); - - - -void get_sumsbor_command(void); - - -//unsigned int read_cmd_sbor_from_bs(void); - -//void read_data_from_bs(void); - - -void check_change_post_upravl(void); -int get_code_active_post_upravl(void); - - - -void auto_detect_zero_u_zpt(void); - -void run_can_from_mpu(void); -void set_new_level_i_protect(int n_af, int level); -void update_maz_level_i_af(int n_af, unsigned int new_maz_level); -void calc_count_build_revers(void); -int calc_auto_moto_pump(void); -void prepare_logger_pult(void); -void read_can_error(void); -void clear_can_error(void); -void cmd_clear_can_error(void); -void check_temper_break(void); -void check_breaker_ged(void); - - - - -//extern int ccc[40]; - -void reinit_before_sbor(void); -unsigned int toggle_status_lamp(unsigned int bb1, unsigned int flag); - - -#endif - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Inu/Src2/551/main/f281xbmsk.h b/Inu/Src2/551/main/f281xbmsk.h deleted file mode 100644 index f9dd78f..0000000 --- a/Inu/Src2/551/main/f281xbmsk.h +++ /dev/null @@ -1,244 +0,0 @@ -/* ================================================================================== -File name: F281XBMSK.H - -Originator: Digital Control Systems Group - Texas Instruments -Description: -Header file containing handy bitmasks for setting up register values. -This file defines the bitmasks for F281X. - -Target: TMS320F281x family - -===================================================================================== - History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20: Using DSP281x v. 1.00 or higher ----------------------------------------------------------------------------------- */ - -#ifndef __F281X_BMSK_H__ -#define __F281X_BMSK_H__ - -/*------------------------------------------------------------------------------ - F281X Register TxCON -------------------------------------------------------------------------------*/ -#define FREE_RUN_FLAG 0x8000 -#define SOFT_STOP_FLAG 0x4000 - -#define TIMER_STOP 0x0000 -#define TIMER_CONT_UPDN 0x0800 -#define TIMER_CONT_UP 0x1000 -#define TIMER_DIR_UPDN 0x1800 - -#define TIMER_CLK_PRESCALE_X_1 0x0000 -#define TIMER_CLK_PRESCALE_X_2 0x0100 -#define TIMER_CLK_PRESCALE_X_4 0x0200 -#define TIMER_CLK_PRESCALE_X_8 0x0300 -#define TIMER_CLK_PRESCALE_X_16 0x0400 -#define TIMER_CLK_PRESCALE_X_32 0x0500 -#define TIMER_CLK_PRESCALE_X_64 0x0600 -#define TIMER_CLK_PRESCALE_X_128 0x0700 - -#define TIMER_ENABLE_BY_OWN 0x0000 -#define TIMER_ENABLE_BY_T1 0x0080 - -#define TIMER_ENABLE 0x0040 -#define TIMER_DISABLE 0x0000 - -#define TIMER_CLOCK_SRC_INTERNAL 0x0000 -#define TIMER_CLOCK_SRC_EXTERNAL 0x0010 -#define TIMER_CLOCK_SRC_QEP 0x0030 - -#define TIMER_COMPARE_LD_ON_ZERO 0x0000 -#define TIMER_COMPARE_LD_ON_ZERO_OR_PRD 0x0004 -#define TIMER_COMPARE_LD_IMMEDIATE 0x0008 - -#define TIMER_ENABLE_COMPARE 0x0002 -#define TIMER_SELECT_T1_PERIOD 0x0001 - -/*------------------------------------------------------------------------------ - F281X Register ACTR 0x7413 BIT FIELD MASKS -------------------------------------------------------------------------------*/ -/*------------------------------------------------------------------------------ -Space Vector Direction Commands -------------------------------------------------------------------------------*/ -#define SV_DIRECTION_CW 0x8000 -#define SV_DIRECTION_CCW 0x0000 - -/*------------------------------------------------------------------------------ -Space Vector Generation Vectors -------------------------------------------------------------------------------*/ - -//------------------------------------------------------------------------------ -#define SPACE_VECTOR_0 0x0000 -#define SPACE_VECTOR_1 0x1000 -#define SPACE_VECTOR_2 0x2000 -#define SPACE_VECTOR_3 0x3000 -#define SPACE_VECTOR_4 0x4000 -#define SPACE_VECTOR_5 0x5000 -#define SPACE_VECTOR_6 0x6000 -#define SPACE_VECTOR_7 0x7000 - -/*------------------------------------------------------------------------------ - Compare action definitions -------------------------------------------------------------------------------*/ -#define COMPARE6_FL 0x0000 -#define COMPARE6_AL 0x0400 -#define COMPARE6_AH 0x0800 -#define COMPARE6_FH 0x0C00 -//------------------------------------------------------------------------------ -#define COMPARE5_FL 0x0000 -#define COMPARE5_AL 0x0100 -#define COMPARE5_AH 0x0200 -#define COMPARE5_FH 0x0300 -//------------------------------------------------------------------------------ -#define COMPARE4_FL 0x0000 -#define COMPARE4_AL 0x0040 -#define COMPARE4_AH 0x0080 -#define COMPARE4_FH 0x00C0 -//------------------------------------------------------------------------------ -#define COMPARE3_FL 0x0000 -#define COMPARE3_AL 0x0010 -#define COMPARE3_AH 0x0020 -#define COMPARE3_FH 0x0030 -//------------------------------------------------------------------------------ -#define COMPARE2_FL 0x0000 -#define COMPARE2_AL 0x0004 -#define COMPARE2_AH 0x0008 -#define COMPARE2_FH 0x000C -//------------------------------------------------------------------------------ -#define COMPARE1_FL 0x0000 -#define COMPARE1_AL 0x0001 -#define COMPARE1_AH 0x0002 -#define COMPARE1_FH 0x0003 -//------------------------------------------------------------------------------ - -/*------------------------------------------------------------------------------ - F281X Register COMCONA/COMCONB -------------------------------------------------------------------------------*/ -#define CMPR_ENABLE 0x8000 -#define CMPR_LD_ON_ZERO 0x0000 -#define CMPR_LD_ON_ZERO_OR_PRD 0x2000 -#define CMPR_LD_IMMEDIATE 0x4000 -#define SVENABLE 0x1000 -#define SVDISABLE 0x0000 -#define ACTR_LD_ON_ZERO 0x0000 -#define ACTR_LD_ON_ZERO_OR_PRD 0x0400 -#define ACTR_LD_IMMEDIATE 0x0800 -#define FCOMPOE 0x0100 - -/*------------------------------------------------------------------------------ - F281X Register DBTCON -------------------------------------------------------------------------------*/ -#define DBT_VAL_0 0x0000 -#define DBT_VAL_1 0x0100 -#define DBT_VAL_2 0x0200 -#define DBT_VAL_3 0x0300 -#define DBT_VAL_4 0x0400 -#define DBT_VAL_5 0x0500 -#define DBT_VAL_6 0x0600 -#define DBT_VAL_7 0x0700 -#define DBT_VAL_8 0x0800 -#define DBT_VAL_9 0x0900 -#define DBT_VAL_10 0x0A00 -#define DBT_VAL_11 0x0B00 -#define DBT_VAL_12 0x0C00 -#define DBT_VAL_13 0x0D00 -#define DBT_VAL_14 0x0E00 -#define DBT_VAL_15 0x0F00 - -#define EDBT3_DIS 0x0000 -#define EDBT3_EN 0x0080 -#define EDBT2_DIS 0x0000 -#define EDBT2_EN 0x0040 -#define EDBT1_DIS 0x0000 -#define EDBT1_EN 0x0020 - -#define DBTPS_X32 0x0014 -#define DBTPS_X16 0x0010 -#define DBTPS_X8 0x000C -#define DBTPS_X4 0x0008 -#define DBTPS_X2 0x0004 -#define DBTPS_X1 0x0000 - -/*------------------------------------------------------------------------------ - F281X Register ADCTRL1 -------------------------------------------------------------------------------*/ -#define ADC_SUS_MODE0 0x0000 -#define ADC_SUS_MODE1 0X1000 -#define ADC_SUS_MODE2 0x2000 -#define ADC_SUS_MODE3 0X3000 -#define ADC_RESET_FLAG 0x4000 - -#define ADC_ACQ_PS_1 0x0000 -#define ADC_ACQ_PS_2 0x0100 -#define ADC_ACQ_PS_3 0x0200 -#define ADC_ACQ_PS_4 0x0300 -#define ADC_ACQ_PS_5 0x0400 -#define ADC_ACQ_PS_6 0x0500 -#define ADC_ACQ_PS_7 0x0600 -#define ADC_ACQ_PS_8 0x0700 -#define ADC_ACQ_PS_9 0x0800 -#define ADC_ACQ_PS_10 0x0900 -#define ADC_ACQ_PS_11 0x0A00 -#define ADC_ACQ_PS_12 0x0B00 -#define ADC_ACQ_PS_13 0x0C00 -#define ADC_ACQ_PS_14 0x0D00 -#define ADC_ACQ_PS_15 0x0E00 -#define ADC_ACQ_PS_16 0x0F00 - -#define ADC_CPS_1 0x0000 -#define ADC_CPS_2 0x0080 -#define ADC_CONT_RUN 0x0040 -#define ADC_SEQ_CASC 0x0010 -#define ADC_SEQ_DUAL 0x0000 - -/*------------------------------------------------------------------------------ - F281X Register ADCTRL2 -------------------------------------------------------------------------------*/ -#define ADC_EVB_SOC 0x8000 -#define ADC_RST_SEQ1 0x4000 -#define ADC_SOC_SEQ1 0x2000 - -#define ADC_INT_ENA_SEQ1 0x0800 -#define ADC_INT_MODE_SEQ1 0X0400 -#define ADC_EVA_SOC_SEQ1 0x0100 - -#define ADC_EXT_SOC_SEQ1 0x0080 -#define ADC_RST_SEQ2 0x0040 -#define ADC_SOC_SEQ2 0x0020 - -#define ADC_INT_ENA_SEQ2 0x0008 -#define ADC_INT_MODE_SEQ2 0x0004 -#define ADC_EVB_SOC_SEQ2 0x0001 - -/*------------------------------------------------------------------------------ - F281X Register ADCTRL3 -------------------------------------------------------------------------------*/ -#define ADC_RFDN 0x0080 -#define ADC_BGDN 0x0040 -#define ADC_PWDN 0x0020 - -#define ADC_CLKPS_X_1 0x0000 -#define ADC_CLKPS_X_2 0x0002 -#define ADC_CLKPS_X_4 0x0004 -#define ADC_CLKPS_X_6 0x0006 -#define ADC_CLKPS_X_8 0x0008 -#define ADC_CLKPS_X_10 0x000A -#define ADC_CLKPS_X_12 0x000C -#define ADC_CLKPS_X_14 0x000E -#define ADC_CLKPS_X_16 0x0010 -#define ADC_CLKPS_X_18 0x0012 -#define ADC_CLKPS_X_20 0x0014 -#define ADC_CLKPS_X_22 0x0016 -#define ADC_CLKPS_X_24 0x0018 -#define ADC_CLKPS_X_26 0x001A -#define ADC_CLKPS_X_28 0x001C -#define ADC_CLKPS_X_30 0x001E - -#define ADC_SMODE_SIMULTANEOUS 0x0001 -#define ADC_SMODE_SEQUENTIAL 0x0000 - -#endif // __F281X_BMSK_H__ -// EOF - - diff --git a/Inu/Src2/551/main/f281xpwm.c b/Inu/Src2/551/main/f281xpwm.c deleted file mode 100644 index 1103b7e..0000000 --- a/Inu/Src2/551/main/f281xpwm.c +++ /dev/null @@ -1,288 +0,0 @@ -/* ================================================================================== -File name: F281XPWM.C - -Originator: Digital Control Systems Group - Texas Instruments - -Description: This file contains source for the Full Compare PWM drivers for the F281x - -Target: TMS320F281x family - -===================================================================================== -History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20: Using DSP281x v. 1.00 or higher -----------------------------------------------------------------------------------*/ - -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "IQmathLib.h" - -#include - -#include "DSP281x_Ev.h" -//#include "params.h" - - -void F281X_EV1_PWM_Init(PWMGEN *p) -{ - EvaRegs.T1PR = p->PeriodMax; // Init Timer 1 period Register - EvaRegs.T1CON.all = PWM_INIT_STATE; // Symmetrical Operation - EvaRegs.DBTCONA.all = DBTCON_INIT_STATE; // Init DBTCONA Register - EvaRegs.ACTRA.all = ACTR_INIT_STATE; // Init ACTRA Register - - EvaRegs.COMCONA.all = 0xA600; // Init COMCONA Register - - EvaRegs.CMPR1 = p->PeriodMax; // Init CMPR1 Register - EvaRegs.CMPR2 = p->PeriodMax; // Init CMPR2 Register - EvaRegs.CMPR3 = p->PeriodMax; // Init CMPR3 Register - EALLOW; // Enable EALLOW - GpioMuxRegs.GPAMUX.all |= 0x003F; // Setting PWM1-6 as primary output pins - EDIS; // Disable EALLOW -} - - -void F281X_EV1_PWM_Update(PWMGEN *p) -{ - int16 MPeriod; - int32 Tmp; - -// Compute the timer period (Q0) from the period modulation input (Q15) - Tmp = (int32)p->PeriodMax*(int32)p->MfuncPeriod; // Q15 = Q0*Q15 - MPeriod = (int16)(Tmp>>16) + (int16)(p->PeriodMax>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvaRegs.T1PR = MPeriod; - -// Compute the compare 1 (Q0) from the PWM 1&2 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC1; // Q15 = Q0*Q15 - EvaRegs.CMPR1 = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - -// Compute the compare 2 (Q0) from the PWM 3&4 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC2; // Q15 = Q0*Q15 - EvaRegs.CMPR2 = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - -// Compute the compare 3 (Q0) from the PWM 5&6 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC3; // Q15 = Q0*Q15 - EvaRegs.CMPR3 = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) -} - - -void F281X_EV2_PWM_Init(PWMGEN *p) -{ - EvbRegs.T3PR = p->PeriodMax; // Init Timer 1 period Register - EvbRegs.T3CON.all = PWM_INIT_STATE; // Symmetrical Operation - EvbRegs.DBTCONB.all = DBTCON_INIT_STATE; // Init DBTCONA Register - EvbRegs.ACTRB.all = ACTR_INIT_STATE; // Init ACTRA Register - - EvbRegs.COMCONB.all = 0xA600; // Init COMCONA Register - - EvbRegs.CMPR4 = p->PeriodMax; // Init CMPR1 Register - EvbRegs.CMPR5 = p->PeriodMax; // Init CMPR2 Register - EvbRegs.CMPR6 = p->PeriodMax; // Init CMPR3 Register - EALLOW; // Enable EALLOW - GpioMuxRegs.GPBMUX.all |= 0x003F; // Setting PWM1-6 as primary output pins - EDIS; // Disable EALLOW -} - - -void F281X_EV2_PWM_Update(PWMGEN *p) -{ - int16 MPeriod; - int32 Tmp; - -// Compute the timer period (Q0) from the period modulation input (Q15) - Tmp = (int32)p->PeriodMax*(int32)p->MfuncPeriod; // Q15 = Q0*Q15 - MPeriod = (int16)(Tmp>>16) + (int16)(p->PeriodMax>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvbRegs.T3PR = MPeriod; - -// Compute the compare 1 (Q0) from the PWM 1&2 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC1; // Q15 = Q0*Q15 - EvbRegs.CMPR4 = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - -// Compute the compare 2 (Q0) from the PWM 3&4 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC2; // Q15 = Q0*Q15 - EvbRegs.CMPR5 = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - -// Compute the compare 3 (Q0) from the PWM 5&6 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC3; // Q15 = Q0*Q15 - EvbRegs.CMPR6 = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - -} - - - -void F281X_EVD_PWM_Init(PWMGEND *p) -{ -//unsigned int pna=0,pnb=0; - - EvaRegs.T1PR = p->PeriodMax; // Init Timer 1 period Register - -#ifdef DOUBLE_UPDATE_PWM - EvaRegs.T1CON.all = PWM_INIT_STATE_DOUBLE_UPADTE; // Symmetrical Operation + DOUBLE UPDATE -#else - EvaRegs.T1CON.all = PWM_INIT_STATE; // Symmetrical Operation -#endif - - EvaRegs.DBTCONA.all = DBTCON_INIT_STATE; // Init DBTCONA Register - EvaRegs.ACTRA.all = ACTR_INIT_STATE; // Init ACTRA Register - - EvaRegs.COMCONA.all = 0xa600;//0xA600; // Init COMCONA Register - - EvaRegs.CMPR1 = p->PeriodMax; // Init CMPR1 Register - EvaRegs.CMPR2 = p->PeriodMax; // Init CMPR2 Register - EvaRegs.CMPR3 = p->PeriodMax; // Init CMPR3 Register - EALLOW; // Enable EALLOW - GpioMuxRegs.GPAMUX.all |= 0x003F; // Setting PWM1-6 as primary output pins - EDIS; // Disable EALLOW - - - EvbRegs.T3PR = p->PeriodMax; // Init Timer 1 period Register - -#ifdef DOUBLE_UPDATE_PWM - EvbRegs.T3CON.all = PWM_INIT_STATE_DOUBLE_UPADTE; // Symmetrical Operation + DOUBLE UPDATE -#else - EvbRegs.T3CON.all = PWM_INIT_STATE; // Symmetrical Operation -#endif - - EvbRegs.DBTCONB.all = DBTCON_INIT_STATE; // Init DBTCONA Register - EvbRegs.ACTRB.all = ACTR_INIT_STATE; // Init ACTRA Register - - EvbRegs.COMCONB.all = 0xa600;//0xA600; // Init COMCONA Register - - EvbRegs.CMPR4 = p->PeriodMax; // Init CMPR1 Register - EvbRegs.CMPR5 = p->PeriodMax; // Init CMPR2 Register - EvbRegs.CMPR6 = p->PeriodMax; // Init CMPR3 Register - EALLOW; // Enable EALLOW - GpioMuxRegs.GPBMUX.all |= 0x003F; // Setting PWM1-6 as primary output pins - EDIS; // Disable EALLOW -// pna = p->ShiftPhaseA;//(p->PeriodMax); -// pnb = p->ShiftPhaseB; - - - EvaRegs.T1CNT = 0x0000; - EvbRegs.T3CNT = 0x0000; - -} - -#pragma CODE_SECTION(set_predel_dshim,".fast_run"); -int16 set_predel_dshim(int16 dshim,int16 dmin,int16 dpwm) -{ - if (dshim < dmin) - { - dshim = dmin; - } - - if (dshim > (dpwm - dmin) ) - { - dshim = (dpwm - dmin); - } - return dshim; -} - -#pragma CODE_SECTION(set_predel_dshim_max,".fast_run"); -int16 set_predel_dshim_max(int16 dshim,int16 dmin,int16 dpwm) -{ - int d2; - -/* - if (dshim < dmin) - { - return 0; - } - else - { - if (dshim > (dpwm - dmin) ) - { -// dshim = (dpwm + 1); - return (dpwm + 10); - } - else - return dshim; - - } -*/ - - - d2 = dmin/2; - - if (dshim < d2) - { - dshim = 0; - return dshim; - } - - if (dshim < dmin) - { - dshim = dmin; - return dshim; - } - - - if (dshim > (dpwm - d2) ) - { - dshim = dpwm+dmin; - return dshim; - } - - - if (dshim > (dpwm - dmin) ) - { - dshim = (dpwm - dmin); - return dshim; - } - - return dshim; - - -} - - -//#pragma CODE_SECTION(F281X_EVD_PWM_Update,".fast_run"); -void F281X_EVD_PWM_Update(PWMGEND *p) -{ - int16 MPeriod, Dshim; - int32 Tmp; - - -// Compute the timer period (Q0) from the period modulation input (Q15) - Tmp = (int32)p->PeriodMax*(int32)p->MfuncPeriod; // Q15 = Q0*Q15 - MPeriod = (int16)(Tmp>>16) + (int16)(p->PeriodMax>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvaRegs.T1PR = MPeriod; - -// Compute the compare 1 (Q0) from the PWM 1&2 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC1; // Q15 = Q0*Q15 - Dshim = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvaRegs.CMPR1 = set_predel_dshim(Dshim,(int16)p->PeriodMin,(int16)MPeriod); - -// Compute the compare 2 (Q0) from the PWM 3&4 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC2; // Q15 = Q0*Q15 - Dshim = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvaRegs.CMPR2 = set_predel_dshim(Dshim,(int16)p->PeriodMin,(int16)MPeriod); - -// Compute the compare 3 (Q0) from the PWM 5&6 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC3; // Q15 = Q0*Q15 - Dshim = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvaRegs.CMPR3 = set_predel_dshim(Dshim,(int16)p->PeriodMin,(int16)MPeriod); - - -// Compute the timer period (Q0) from the period modulation input (Q15) -// Tmp = (int32)p->PeriodMax*(int32)p->MfuncPeriod; // Q15 = Q0*Q15 -// MPeriod = (int16)(Tmp>>16) + (int16)(p->PeriodMax>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvbRegs.T3PR = MPeriod; - -// Compute the compare 1 (Q0) from the PWM 1&2 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC4; // Q15 = Q0*Q15 - Dshim = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvbRegs.CMPR4 = set_predel_dshim(Dshim,(int16)p->PeriodMin,(int16)MPeriod); - -// Compute the compare 2 (Q0) from the PWM 3&4 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC5; // Q15 = Q0*Q15 - Dshim = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvbRegs.CMPR5 = set_predel_dshim(Dshim,(int16)p->PeriodMin,(int16)MPeriod); - -// Compute the compare 3 (Q0) from the PWM 5&6 duty cycle ratio (Q15) - Tmp = (int32)MPeriod*(int32)p->MfuncC6; // Q15 = Q0*Q15 - Dshim = (int16)(Tmp>>16) + (int16)(MPeriod>>1); // Q0 = (Q15->Q0)/2 + (Q0/2) - EvbRegs.CMPR6 = set_predel_dshim(Dshim,(int16)p->PeriodMin,(int16)MPeriod); - -} - diff --git a/Inu/Src2/551/main/f281xpwm.h b/Inu/Src2/551/main/f281xpwm.h deleted file mode 100644 index bf9499f..0000000 --- a/Inu/Src2/551/main/f281xpwm.h +++ /dev/null @@ -1,163 +0,0 @@ -/* ================================================================================== -File name: F281XPWM.H - -Originator: Digital Control Systems Group - Texas Instruments -Description: -Header file containing data type and object definitions and -initializers. Also contains prototypes for the functions in F281XPWM.C. - -Target: TMS320F281x family - -===================================================================================== - History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20: Using DSP281x v. 1.00 or higher ----------------------------------------------------------------------------------- */ - -#ifndef __F281X_PWM_H__ -#define __F281X_PWM_H__ - -#include -//#include "DSP281x_Device.h" -/*---------------------------------------------------------------------------- -Initialization constant for the F281X Timer TxCON for PWM Generation. -Sets up the timer to run free upon emulation suspend, continuous up-down mode -prescaler 1, timer enabled. -----------------------------------------------------------------------------*/ //FREE_RUN_FLAG -#define PWM_INIT_STATE (FREE_RUN_FLAG + \ - TIMER_CONT_UPDN + \ - TIMER_CLK_PRESCALE_X_1 + \ - TIMER_ENABLE_BY_OWN + \ - TIMER_ENABLE) - -#define PWM_INIT_STATE_DOUBLE_UPADTE (FREE_RUN_FLAG + \ - TIMER_COMPARE_LD_ON_ZERO_OR_PRD + \ - TIMER_CONT_UPDN + \ - TIMER_CLK_PRESCALE_X_1 + \ - TIMER_ENABLE_BY_OWN + \ - TIMER_ENABLE) - -/*---------------------------------------------------------------------------- -Initialization constant for the F281X ACTRx register for PWM Generation. -Sets up PWM polarities. -----------------------------------------------------------------------------*/ -#define ACTR_INIT_STATE ( COMPARE1_FH + \ - COMPARE2_FH + \ - COMPARE3_FH + \ - COMPARE4_FH + \ - COMPARE5_FH + \ - COMPARE6_FH ) - -#define ACTR_ON_STATE ( COMPARE1_AL + \ - COMPARE2_AH + \ - COMPARE3_AL + \ - COMPARE4_AH + \ - COMPARE5_AL + \ - COMPARE6_AH ) - -/*---------------------------------------------------------------------------- -Initialization constant for the F281X DBTCONx register for PWM Generation. -Sets up the dead band for PWM and sets up dead band values. -----------------------------------------------------------------------------*/ -#define DBTCON_INIT_STATE ( DBT_VAL_10 + \ - EDBT3_EN + \ - EDBT2_EN + \ - EDBT1_EN + \ - DBTPS_X32 ) - - -/*----------------------------------------------------------------------------- -Define the structure of the PWM Driver Object ------------------------------------------------------------------------------*/ -typedef struct { - Uint16 PeriodMax; // Parameter: PWM Half-Period in CPU clock cycles (Q0) - int16 MfuncPeriod; // Input: Period scaler (Q15) - int16 MfuncC1; // Input: PWM 1&2 Duty cycle ratio (Q15) - int16 MfuncC2; // Input: PWM 3&4 Duty cycle ratio (Q15) - int16 MfuncC3; // Input: PWM 5&6 Duty cycle ratio (Q15) - void (*init)(); // Pointer to the init function - void (*update)(); // Pointer to the update function - } PWMGEN ; - - -typedef struct { - Uint16 PeriodMax; // Parameter: PWM Half-Period in CPU clock cycles (Q0) - Uint16 PeriodMin; // Parameter: PWM Half-Period in CPU clock cycles (Q0) - int16 MfuncPeriod; // Input: Period scaler (Q15) - int16 MfuncC1; // Input: PWM 1&2 Duty cycle ratio (Q15) - int16 MfuncC2; // Input: PWM 3&4 Duty cycle ratio (Q15) - int16 MfuncC3; // Input: PWM 5&6 Duty cycle ratio (Q15) - int16 MfuncC4; // Input: PWM 1&2 Duty cycle ratio (Q15) - int16 MfuncC5; // Input: PWM 3&4 Duty cycle ratio (Q15) - int16 MfuncC6; // Input: PWM 5&6 Duty cycle ratio (Q15) - Uint16 ShiftPhaseA; // Parameter: PWM Half-Period in CPU clock cycles (Q0) - Uint16 ShiftPhaseB; // Parameter: PWM Half-Period in CPU clock cycles (Q0) - void (*init)(); // Pointer to the init function - void (*update)(); // Pointer to the update function - } PWMGEND ; - -/*----------------------------------------------------------------------------- -Define a PWMGEN_handle ------------------------------------------------------------------------------*/ -typedef PWMGEN *PWMGEN_handle; -typedef PWMGEND *PWMGEND_handle; - -/*------------------------------------------------------------------------------ -Default Initializers for the F281X PWMGEN Object -------------------------------------------------------------------------------*/ -#define F281X_EV1_FC_PWM_GEN {1000, \ - 0x7FFF, \ - 0x4000, \ - 0x4000, \ - 0x4000, \ - (void (*)(Uint32))F281X_EV1_PWM_Init, \ - (void (*)(Uint32))F281X_EV1_PWM_Update \ - } - -#define F281X_EV2_FC_PWM_GEN {1000, \ - 0x7FFF, \ - 0x4000, \ - 0x4000, \ - 0x4000, \ - (void (*)(Uint32))F281X_EV2_PWM_Init, \ - (void (*)(Uint32))F281X_EV2_PWM_Update \ - } - - -#define F281X_EVD_FC_PWM_GEN {1000, \ - 0, \ - 0x7FFF, \ - 0x4000, \ - 0x4000, \ - 0x4000, \ - 0x4000, \ - 0x4000, \ - 0x4000, \ - 0x000, \ - 0x000, \ - (void (*)(Uint32))F281X_EVD_PWM_Init, \ - (void (*)(Uint32))F281X_EVD_PWM_Update \ - } - -#define PWMGEN1_DEFAULTS F281X_EV1_FC_PWM_GEN -#define PWMGEN2_DEFAULTS F281X_EV2_FC_PWM_GEN -#define PWMGEND_DEFAULTS F281X_EVD_FC_PWM_GEN - -/*------------------------------------------------------------------------------ - Prototypes for the functions in F281XPWM.C -------------------------------------------------------------------------------*/ -void F281X_EV1_PWM_Init(PWMGEN_handle); -void F281X_EV1_PWM_Update(PWMGEN_handle); -void F281X_EV2_PWM_Init(PWMGEN_handle); -void F281X_EV2_PWM_Update(PWMGEN_handle); - -void F281X_EVD_PWM_Init(PWMGEND_handle); -void F281X_EVD_PWM_Update(PWMGEND_handle); - -int16 set_predel_dshim_max(int16 dshim,int16 dmin,int16 dpwm); -int16 set_predel_dshim(int16 dshim,int16 dmin,int16 dpwm); - - -#endif // __F281X_PWM_H__ - diff --git a/Inu/Src2/551/main/limit_lib.c b/Inu/Src2/551/main/limit_lib.c deleted file mode 100644 index 859b644..0000000 --- a/Inu/Src2/551/main/limit_lib.c +++ /dev/null @@ -1,50 +0,0 @@ -/* - * limit_lib.c - * - * Created on: 15 . 2024 . - * Author: yura - */ - - -#include "IQmathLib.h" -#include "math_pi.h" - - - -_iq linear_decrease(float current, int alarm_level, int warnig_level) { - float delta = current - warnig_level; - float max_delta = alarm_level - warnig_level; - if (delta <= 0 || max_delta <= 0) { - return CONST_IQ_1; - } else { - if (delta>max_delta) - return 0; - else - return CONST_IQ_1 - _IQ(delta / max_delta); - } -} - - - - - - - -_iq linear_decrease_iq(_iq current, _iq alarm_level, _iq warnig_level) -{ - _iq delta = current - warnig_level; - - _iq max_delta = alarm_level - warnig_level; - - if (delta <= 0 || max_delta <= 0) { - return CONST_IQ_1; - } else { - if (delta>=max_delta) - return 0; - else - return CONST_IQ_1 - _IQdiv(delta, max_delta); - } -} - - - diff --git a/Inu/Src2/551/main/limit_lib.h b/Inu/Src2/551/main/limit_lib.h deleted file mode 100644 index 3b1e6aa..0000000 --- a/Inu/Src2/551/main/limit_lib.h +++ /dev/null @@ -1,15 +0,0 @@ -/* - * limit_lib.h - * - * Created on: 15 . 2024 . - * Author: yura - */ - -#ifndef SRC_MAIN_LIMIT_LIB_H_ -#define SRC_MAIN_LIMIT_LIB_H_ - -_iq linear_decrease(float current, int alarm_level, int warnig_level); - -_iq linear_decrease_iq(_iq current, _iq alarm_level, _iq warnig_level); - -#endif /* SRC_MAIN_LIMIT_LIB_H_ */ diff --git a/Inu/Src2/551/main/limit_power.c b/Inu/Src2/551/main/limit_power.c deleted file mode 100644 index 840019f..0000000 --- a/Inu/Src2/551/main/limit_power.c +++ /dev/null @@ -1,237 +0,0 @@ -/* - * limit_power.c - * - * Created on: 15 . 2024 . - * Author: yura - */ - -#include "IQmathLib.h" - -#include -#include -#include -#include -#include -#include "mathlib.h" -#include "math_pi.h" - - -#include "limit_power.h" -#include "limit_lib.h" - -#include "pll_tools.h" - -#include "uom_tools.h" - - - - -#define KOEF_50HZ (FREQ_PWM*2.0*50.0/PI) -#define LEVEL_01HZ_IQ _IQ(10.0/KOEF_50HZ) // 0.1 HZ - -_iq level_50hz = _IQmpyI32(LEVEL_01HZ_IQ, 500); -_iq level_minimal_level_work_hz = _IQmpyI32(LEVEL_01HZ_IQ, 350); - -_iq delta_freq_test = 0; - - -//_iq level_01hz = _IQ(LEVEL_01HZ); - - -//#define LEVEL_50HZ (5000.0/KOEF_50HZ) // 50 HZ -//#define LEVEL_05HZ (50.0/KOEF_50HZ) // 50 HZ -// -//#define LEVEL_3HZ (300.0/KOEF_50HZ) // 50 HZ -//#define LEVEL_2HZ (200.0/KOEF_50HZ) // 50 HZ -//#define LEVEL_1HZ (100.0/KOEF_50HZ) // 50 HZ - - - -#define LEVEL1_FREQ_DECR 10 // 1.5 Hz 49.0 -#define LEVEL2_FREQ_DECR 100 // 10 Hz 40 -//#define LEVEL1_FREQ_DECR 15 // 1.5 Hz 48.5 -//#define LEVEL2_FREQ_DECR 100 // 10 Hz 40 - -#define PLUS_LIMIT_KOEFFS 0.0001 -#define MINUS_LIMIT_KOEFFS 0.05 - -#define MAX_COUNT_GO_UOM (FREQ_PWM*5) // 5 sec -#define SET_LIMIT_UOM 0.5 - -void calc_all_limit_koeffs(void) -{ - _iq sum_limit, delta_freq; - - static unsigned int prev_uom = 0; - static int flag_enable_go_uom = 0; - - - static _iq level1_freq_decr = _IQmpyI32(LEVEL_01HZ_IQ, LEVEL1_FREQ_DECR); - static _iq level2_freq_decr = _IQmpyI32(LEVEL_01HZ_IQ, LEVEL2_FREQ_DECR); - - static _iq iq_set_limit_uom = _IQ(SET_LIMIT_UOM); - static unsigned int count_go_uom = 0; - - static _iq iq_plus_limit_koeffs = _IQ(PLUS_LIMIT_KOEFFS); - static _iq iq_minus_limit_koeffs = _IQ(MINUS_LIMIT_KOEFFS); - - static long freq_test = 30; - //*LEVEL_01HZ_IQ; - static _iq minus_delta_freq_test = _IQdiv32(LEVEL_01HZ_IQ); // 0.1/32 - - - static int uom_test = 50; - static int prev_imit_limit_freq = 0, prev_imit_limit_uom = 0; - - static _iq iq_new_uom_level_kwt = 0; - - - update_uom(); - - // temper - edrk.all_limit_koeffs.local_temper_limit = edrk.temper_limit_koeffs.sum_limit; - - - // uin_freq - if (edrk.Status_Ready.bits.ready_final) //|| edrk.imit_limit_freq - { - - get_freq_50hz_iq(); - - // freq = LEVEL_50HZ - edrk.iq_freq_50hz; - - if (edrk.imit_limit_freq && prev_imit_limit_freq == 0) - delta_freq_test = _IQmpyI32(LEVEL_01HZ_IQ, freq_test); - - if (delta_freq_test>0) - { - if (delta_freq_test>0) - delta_freq_test -= minus_delta_freq_test; - if (delta_freq_test<0) - delta_freq_test = 0; - } - - if (edrk.iq_freq_50hz>level_minimal_level_work_hz) - { - edrk.all_limit_koeffs.local_uin_freq_limit = linear_decrease_iq( (level_50hz - edrk.iq_freq_50hz), - level2_freq_decr, level1_freq_decr); - } - else - edrk.all_limit_koeffs.local_uin_freq_limit = CONST_IQ_1; - } - else - { - edrk.all_limit_koeffs.local_uin_freq_limit = CONST_IQ_1; - } - prev_imit_limit_freq = edrk.imit_limit_freq; - - // - /// UOM - // - if (edrk.from_uom.ready || edrk.set_limit_uom_50) - { - if (edrk.set_limit_uom_50) - { - edrk.from_uom.level_value = uom_test; - } - - if (edrk.imit_limit_uom && prev_imit_limit_uom == 0) - edrk.from_uom.level_value++; - - - if (prev_uom!=edrk.from_uom.level_value && edrk.from_uom.level_value > prev_uom) - { - if (edrk.iq_power_kw_full_filter_abs > edrk.from_uom.iq_level_value_kwt) - flag_enable_go_uom = 1; - } - else - flag_enable_go_uom = 0; - - if (flag_enable_go_uom) - { - count_go_uom = MAX_COUNT_GO_UOM; - edrk.all_limit_koeffs.local_uom_limit = iq_set_limit_uom; // - } - - if (count_go_uom) - { - // - count_go_uom--; - } - else - edrk.all_limit_koeffs.local_uom_limit = CONST_IQ_1; // - - prev_uom = edrk.from_uom.level_value; - - } - else - { - - edrk.power_limit.bits.limit_from_uom_fast = 0; - edrk.all_limit_koeffs.uom_limit = CONST_IQ_1; - prev_uom = 0; - } - prev_imit_limit_uom = edrk.imit_limit_uom; - // if () - - - //// temper - edrk.all_limit_koeffs.temper_limit = zad_intensiv_q(iq_plus_limit_koeffs, iq_minus_limit_koeffs, - edrk.all_limit_koeffs.temper_limit, - edrk.all_limit_koeffs.local_temper_limit); - - edrk.power_limit.bits.limit_by_temper = (edrk.all_limit_koeffs.temper_limit -#include -#include -#include - -#include "control_station.h" -#include "global_time.h" -#include "modbus_table_v2.h" -#include "RS_modbus_pult.h" -#include "DSP281x_Device.h" -#include "MemoryFunctions.h" -#include "RS_modbus_svu.h" -#include "log_params.h" -#include "logs_hmi.h" -#include "edrk_main.h" - - -#pragma DATA_SECTION(log_to_HMI, ".logs"); -t_Logs_with_modbus log_to_HMI = LOGS_WITH_MODBUS_DEFAULTS; - - -#define COUNT_FAST_DATA 300//150 -#define COUNT_SLOW_DATA 300 - - - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -#define MAX_SIZE_LOGS_HMI_FULL (END_ADDRESS_LOGS - START_ADDRESS_LOG + 1) //262144 // 0x80000/2 -#define MAX_SIZE_LOGS_HMI_SMALL 10000 - -#define START_ARRAY_LOG_SEND 300 -#define END_ARRAY_LOG_SEND 899 -#define SIZE_ARRAY_LOG_SEND (END_ARRAY_LOG_SEND - START_ARRAY_LOG_SEND + 1) -#define SIZE_BUF_WRITE_LOGS_TO_MODBUS16_REMOUTE 120 - -int writeLogsArray(int flag_next) -{ - int succed = 0; - static unsigned int old_time = 0; - - static int count_write_to_modbus = 0; - static int cur_position_buf_modbus16 = 0; - - - - - - if (!rs_b.flag_LEADING) - { - - ModbusRTUsetDataArrays(modbus_table_analog_in, modbus_table_analog_out); - - if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0) - { - if (log_to_HMI.flag_start_log_array_sent) - { - cur_position_buf_modbus16 = START_ARRAY_LOG_SEND; - log_to_HMI.flag_log_array_sended = 0; - } - else - { - if (flag_next) - cur_position_buf_modbus16 = cur_position_buf_modbus16 + SIZE_BUF_WRITE_LOGS_TO_MODBUS16_REMOUTE; - } - - log_to_HMI.flag_start_log_array_sent = 0; - } - - if (cur_position_buf_modbus16 >= END_ARRAY_LOG_SEND) - { - // ? - cur_position_buf_modbus16 = START_ARRAY_LOG_SEND; - log_to_HMI.flag_log_array_sended = 1; - // log_to_HMI.flag_log_array_sent_process = 0; -// succed = 1; - return succed; - } - -// // . . -// if ((cur_position_buf_modbus16 > ADRESS_END_FIRST_BLOCK) && -// (cur_position_buf_modbus16 < ADRESS_START_PROTECTION_LEVELS)) { -// cur_position_buf_modbus16 = ADRESS_START_PROTECTION_LEVELS; -// } - - if ((cur_position_buf_modbus16 + SIZE_BUF_WRITE_LOGS_TO_MODBUS16_REMOUTE) > (END_ARRAY_LOG_SEND+1)) - count_write_to_modbus = END_ARRAY_LOG_SEND - cur_position_buf_modbus16 + 1; - else - count_write_to_modbus = SIZE_BUF_WRITE_LOGS_TO_MODBUS16_REMOUTE; - - log_to_HMI.n_log_array_sended = (cur_position_buf_modbus16 - START_ARRAY_LOG_SEND)/100 + 1; - log_to_HMI.flag_log_array_sent_process++;// = 1; - - ModbusRTUsend16(&rs_b, 2, - ADR_MODBUS_TABLE_REMOUTE + cur_position_buf_modbus16, - count_write_to_modbus); - - - - // control_station.count_error_modbus_16[CONTROL_STATION_INGETEAM_PULT_RS485]++; - - // control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = 1; - succed = count_write_to_modbus; - } - return succed; - - - -/* - - unsigned long i = 0; - int succed = 0; - -// int *p_log_data = (int*)LOG_START_ADRES; - - ModbusRTUsetDataArrays(modbus_table_analog_in, modbus_table_analog_out); - if (!rs_b.flag_LEADING) - { - ModbusRTUsend16(&rs_b, 2, - log_to_HMI.current_address, - log_to_HMI.count_write_to_modbus + 1); - - if (err_send_log_16 == 0) { //prev message without errors - log_to_HMI.current_address = log_to_HMI.current_address + SIZE_BUF_WRITE_LOG_TO_MODBUS16; - } - if (log_to_HMI.current_address > END_ARRAY_LOG_SEND) { - log_to_HMI.current_address = START_ARRAY_LOG_SEND; -// log_to_HMI.flag_end_of_log = 1; - log_to_HMI.flag_log_array_sent = 1; - } - if ((log_to_HMI.current_address + SIZE_BUF_WRITE_LOG_TO_MODBUS16) > END_ARRAY_LOG_SEND) { - log_to_HMI.count_write_to_modbus = END_ARRAY_LOG_SEND - log_to_HMI.current_address; - } else { - log_to_HMI.count_write_to_modbus = SIZE_BUF_WRITE_LOG_TO_MODBUS16; - } - - err_send_log_16 += 1; - succed = 1; - - } - return succed; -*/ -} - -void prepareWriteLogsArray(void) { -// log_to_HMI.start_log_address = logpar.addres_mem - logpar.count_log_params_fast_log * SIZE_ARRAY_LOG_SEND; -// log_to_HMI.start_log_address = log_params.addres_mem - log_params.BlockSizeErr * SIZE_ARRAY_LOG_SEND; - - if (log_to_HMI.send_log == 1) - log_to_HMI.start_log_address = log_params.start_address_log_slow; - if (log_to_HMI.send_log == 2) - log_to_HMI.start_log_address = log_params.start_address_log; - if (log_to_HMI.send_log == 3) - log_to_HMI.start_log_address = 0;//log_params.start_address_log; - - // - log_to_HMI.max_size_logs_hmi; - -// if (log_to_HMI.start_log_address < log_params.start_address_log) { -// log_to_HMI.start_log_address = log_params.end_address_log - (log_params.start_address_log - log_to_HMI.start_log_address); -// } -// log_to_HMI.log_address_step = SIZE_ARRAY_LOG_SEND;//log_params.BlockSizeErr; -} - - -int fillAnalogDataArrayForLogSend(void) -{ - int i, k, n;// = START_ARRAY_LOG_SEND; - int c_data = 0, lb = 0, type_log; - volatile unsigned long local_pos = 0; - -// unsigned long current_address = log_to_HMI.start_log_address;// + num_of_log; - - k = 0; - n = 0; - for (i = START_ARRAY_LOG_SEND; i <= END_ARRAY_LOG_SEND; i++) { - -// if (log_to_HMI.count_write_to_modbus > log_to_HMI.max_size_logs_hmi) -// break; - - n = log_to_HMI.count_write_to_modbus/SIZE_BUF_WRITE_LOGS_TO_MODBUS16_REMOUTE; - - if (k>=SIZE_BUF_WRITE_LOGS_TO_MODBUS16_REMOUTE) - k = 0; - - if (k==0) - modbus_table_analog_out[i].all = LOWORD(log_to_HMI.count_write_to_modbus); - else - if (k==1) - modbus_table_analog_out[i].all = LOWORD(global_time.miliseconds); - else - if (k==2) - modbus_table_analog_out[i].all = HIWORD(log_to_HMI.start_log_address); - else - if (k==3) - modbus_table_analog_out[i].all = LOWORD(log_to_HMI.start_log_address); - else - if (k==SIZE_BUF_WRITE_LOGS_TO_MODBUS16_REMOUTE-1) - modbus_table_analog_out[i].all = log_to_HMI.tick_step; - else - { - if (log_to_HMI.count_write_to_modbus > log_to_HMI.max_size_logs_hmi) - modbus_table_analog_out[i].all = 0; - else - { -// modbus_table_analog_out[i].all = LOWORD(log_to_HMI.start_log_address); // - if (log_to_HMI.send_log==3) - { - if (log_to_HMI.start_log_address>=(COUNT_FAST_DATA*log_params.BlockSizeErr) ) - { - local_pos = log_to_HMI.max_size_logs_hmi - log_to_HMI.start_log_address;// - (COUNT_FAST_DATA*log_params.BlockSizeErr); - type_log = SLOW_LOG; - } - else - { - local_pos = log_to_HMI.max_size_logs_hmi - log_to_HMI.start_log_address - (COUNT_SLOW_DATA*log_params.BlockSizeErr); - type_log = FAST_LOG; - } - - modbus_table_analog_out[i].all = alarm_log_get_data(local_pos, type_log); - } - else - modbus_table_analog_out[i].all = ReadMemory(log_to_HMI.start_log_address); - - - - log_to_HMI.start_log_address += 1;//log_to_HMI.log_address_step; - log_to_HMI.count_write_to_modbus += 1; - - } - } - -// modbus_table_analog_out[i+1].all = HIWORD(log_to_HMI.start_log_address);//log_to_HMI.count_write_to_modbus;//ReadMemory(log_to_HMI.start_log_address); -// modbus_table_analog_out[i].all = LOWORD(global_time.miliseconds);//ReadMemory(log_to_HMI.start_log_address); -// modbus_table_analog_out[i+1].all = HIWORD(global_time.miliseconds);//log_to_HMI.count_write_to_modbus;//ReadMemory(log_to_HMI.start_log_address); - -// if (k>1 && k 0) { - *(p_memory++) = value; - value += 1; -// if (log_size % 8 == 0) { -// value += 1; -// } - } -} - - - - - - -int alarm_log_get_data(unsigned long pos, int type_log) -{ - //unsigned int i,k; - static volatile unsigned long cur_adr_log, end_log, start_log, addres_mem, temp_length, delta_adr;//clog //real_length - //int *adr_finish_temp, *adr_current; - - -// real_length = al->real_points * al->oscills; - // real_adr = al->start_adr_real_logs; - - if (type_log==FAST_LOG) - { - temp_length = log_params.BlockSizeErr; - cur_adr_log = log_params.addres_mem; - end_log = log_params.end_address_log; - start_log = log_params.start_address_log; - - } - - if (type_log==SLOW_LOG) - { - temp_length = log_params.BlockSizeSlow; - cur_adr_log = log_params.addres_mem_slow; - end_log = log_params.end_address_log_slow; - start_log = log_params.start_address_log_slow; - } - - // - - - addres_mem = cur_adr_log - pos;//temp_length - - // ? - if (addres_memtemp_points * al->oscills; // - al->temp_log_ready = 0; - - - if (al->current_adr_real_log == al->start_adr_real_logs) // , ? - return; - - adr_current = al->current_adr_real_log; // - adr_finish_temp = al->start_adr_temp + temp_length; // temp - // adr_finish temp_log - // , - for (clog=0; clog= al->start_adr_real_logs) ) - { - *adr_finish_temp = *adr_current; // - // - adr_current--; - } - else - *adr_finish_temp = 0; // ! - - // - adr_finish_temp--; - - // ? - if (adr_current < al->start_adr_real_logs) - { - if (al->finish_adr_real_log) // ? - adr_current = al->finish_adr_real_log; // . - else - adr_current = al->start_adr_real_logs - 1; - } - } - - al->temp_log_ready = 1; -*/ - -} - - - - diff --git a/Inu/Src2/551/main/logs_hmi.h b/Inu/Src2/551/main/logs_hmi.h deleted file mode 100644 index e7637f5..0000000 --- a/Inu/Src2/551/main/logs_hmi.h +++ /dev/null @@ -1,86 +0,0 @@ -/* - * logs_hmi.h - * - * Created on: 28 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#ifndef SRC_MAIN_LOGS_HMI_H_ -#define SRC_MAIN_LOGS_HMI_H_ - -#define _LOG_HMI_SMALL_TEST 0//1 - -#define PLACE_STORE_LOG_PULT_SD 1 //SD -#define PLACE_STORE_LOG_PULT_USB 2 //USB Flash - -typedef struct { - - int send_log; - -// int new_send_log_checked; - unsigned long log_size_sent; -// int flag_data_received; - - - -// unsigned int number_of_log; - unsigned long count_write_to_modbus; - -// unsigned long current_address; - unsigned long start_log_address; -// int log_address_step; - - int step; - int progress_bar; - int enable_progress_bar; - - int cleanLogs; - int tick_step; -// int tick_finish; - - int flag_log_array_sended; - int flag_start_log_array_sent; - - int flag_log_array_sent_process; - int count_data_in_buf; - unsigned long count_sended_to_pult; - - unsigned long max_size_logs_hmi; - - int tick_step2; - int tick_step3; - int tick_step4; - int tick_step5; - int n_log_array_sended; - - unsigned long max_size_logs_hmi_small; - unsigned long max_size_logs_hmi_full; - - int saveLogsToSDCard; - int ReportLogOut; - int sdusb; - -} t_Logs_with_modbus; - -#define LOGS_WITH_MODBUS_DEFAULTS {0,0,0,0,0, 0,0,0,0, 0,0,0,0,0, 0, 0,0,0,0,0, 0,0, 0,0,0} -extern t_Logs_with_modbus log_to_HMI; - - - - -#define LOG_START_ADRES 0xA0000UL -#define LOG_END_ADRES 0xF0000UL -#define LOG_BUFFER_SIZE 0x50000UL //0x100UL - -void fillLogArea(); //TODO for testing only - -int alarm_log_get_data(unsigned long pos, int type_log); - -int writeLogsArray(int flag_next); -static void prepareWriteLogsArray(void); -static int fillAnalogDataArrayForLogSend(void); -int sendLogToHMI(int status_ok); -void run_store_slow_logs(void); - - -#endif /* SRC_MAIN_LOGS_HMI_H_ */ diff --git a/Inu/Src2/551/main/manch.h b/Inu/Src2/551/main/manch.h deleted file mode 100644 index d634beb..0000000 --- a/Inu/Src2/551/main/manch.h +++ /dev/null @@ -1,182 +0,0 @@ -#ifndef MANCH_H -#define MANCH_H - -#ifdef __cplusplus -extern "C" { -#endif - - - - -struct MANCH_READ_BITS { // bits description - Uint16 but_0:1; // 1 - Uint16 but_1:1; // 1 - Uint16 but_2:1; // 1 - Uint16 but_3:1; // 1 - Uint16 but_4:1; // 1 - Uint16 but_5:1; // 1 - Uint16 but_6:1; // 1 - Uint16 but_7:1; // 1 - Uint16 press_any_key:1; // 0 -}; - - -union MANCH_READ_REG { - Uint16 all; - struct MANCH_READ_BITS bit; -}; - - -struct MANCH_WRITE_BITS { // bits description - int number0:14; // 1 - int number1:14; // 1 - Uint16 data_control:1; // 1 - Uint16 case_line_recive2:1; // 1 - Uint16 res0:1; // 1 - Uint16 res1:1; // 1 - Uint16 set_ratio_indicator:4; // 1 - union { - Uint16 all; - struct - { - Uint16 lamp_0:1; // 1 - Uint16 lamp_1:1; // 1 - Uint16 lamp_2:1; // 1 - Uint16 lamp_3:1; // 1 - Uint16 lamp_4:1; // 1 - Uint16 lamp_5:1; // 1 - Uint16 lamp_6:1; // 1 - Uint16 lamp_7:1; // 1 - Uint16 lamp_8:1; // 1 - Uint16 lamp_9:1; // 1 - Uint16 lamp_10:1; // 1 - Uint16 lamp_11:1; // 1 - Uint16 lamp_12:1; // 1 - Uint16 lamp_13:1; // 1 - Uint16 lamp_14:1; // 1 - Uint16 lamp_15:1; // 1 - } bit; - } lamps; - union { - Uint16 all; - struct - { - Uint16 lamp_0:1; // 1 - Uint16 lamp_1:1; // 1 - Uint16 lamp_2:1; // 1 - Uint16 lamp_3:1; // 1 - Uint16 lamp_4:1; // 1 - Uint16 lamp_5:1; // 1 - Uint16 lamp_6:1; // 1 - Uint16 lamp_7:1; // 1 - Uint16 lamp_8:1; // 1 - Uint16 lamp_9:1; // 1 - Uint16 lamp_10:1; // 1 - Uint16 lamp_11:1; // 1 - Uint16 lamp_12:1; // 1 - Uint16 lamp_13:1; // 1 - Uint16 lamp_14:1; // 1 - Uint16 lamp_15:1; // 1 - } bit; - } lamps_2; - Uint16 res2:1; // 1 - Uint16 res3:1; // 1 - Uint16 set_ratio_lamp:4; // 1 - Uint16 case_line_receive1:1; -}; - -/* - -struct MANCH_WRITE1_BITS { // bits description - Uint16 number0:16; // 1 -}; - - - -struct MANCH_WRITE2_BITS { // bits description - Uint16 number1:8; // 1 - Uint16 data_control:1; // 1 - Uint16 case_line_recive2:1; // 1 - Uint16 res10:1; // 1 - Uint16 res11:1; // 1 - - Uint16 res1:6; // 1 -}; - -struct MANCH_WRITE3_BITS { // bits description - Uint16 lamp_0:1; // 1 - Uint16 lamp_1:1; // 1 - Uint16 lamp_2:1; // 1 - Uint16 lamp_3:1; // 1 - Uint16 lamp_4:1; // 1 - Uint16 lamp_5:1; // 1 - Uint16 lamp_6:1; // 1 - Uint16 lamp_7:1; // 1 - Uint16 lamp_8:1; // 1 - Uint16 lamp_9:1; // 1 - Uint16 lamp_10:1; // 1 - Uint16 lamp_11:1; // 1 - Uint16 lamp_12:1; // 1 - Uint16 lamp_13:1; // 1 - Uint16 lamp_14:1; // 1 - Uint16 lamp_15:1; // 1 -}; - - -union MANCH_WRITE1_REG { - Uint16 all; - struct MANCH_WRITE1_BITS bit; -}; - -union MANCH_WRITE2_REG { - Uint16 all; - struct MANCH_WRITE2_BITS bit; -}; - -union MANCH_WRITE3_REG { - Uint16 all; - struct MANCH_WRITE3_BITS bit; -}; - -*/ - -typedef volatile struct { // bits description - union MANCH_READ_REG reg1; -} MANCH_READ_REGS; - -/* -typedef volatile struct { // bits description - union MANCH_WRITE1_REG reg1; - union MANCH_WRITE2_REG reg2; - union MANCH_WRITE3_REG reg3; -} MANCH_WRITE_REGS; -*/ - -typedef volatile struct MANCH_WRITE_BITS MANCH_WRITE_REGS; - - - - - -extern MANCH_READ_REGS ManchReadRegs_00; -extern MANCH_READ_REGS ManchReadRegs_01; -extern MANCH_READ_REGS ManchReadRegs_02; -extern MANCH_READ_REGS ManchReadRegs_03; - -extern MANCH_WRITE_REGS ManchWriteRegs_00; -extern MANCH_WRITE_REGS ManchWriteRegs_01; - - - -void read_manch(); -int write_manch(); -void tune_manch_lines_v1(); - - -#ifdef __cplusplus -} -#endif /* extern "C" */ - - -#endif // end of MANCH_H definition - diff --git a/Inu/Src2/551/main/master_slave.c b/Inu/Src2/551/main/master_slave.c deleted file mode 100644 index 4aa4297..0000000 --- a/Inu/Src2/551/main/master_slave.c +++ /dev/null @@ -1,586 +0,0 @@ -/* - * master_slave.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#include - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include -#include "synhro_tools.h" -#include "master_slave.h" - -////////////////////////////////////////////////////////// - -#pragma DATA_SECTION(buf_log_master_slave_status,".slow_vars"); -unsigned int buf_log_master_slave_status[SIZE_LOG_MASTER_SLAVE_STATUS] = {0}; -//AUTO_MASTER_SLAVE_DATA buf2[SIZE_BUF1] = {0}; -//AUTO_MASTER_SLAVE_DATA buf3[SIZE_BUF1] = {0}; -//OPTICAL_BUS_DATA_LOW_CMD buf4[SIZE_BUF1] = {0}; -//OPTICAL_BUS_DATA_LOW_CMD buf5[SIZE_BUF1] = {0}; - - -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// - -void auto_select_master_slave(void) -{ - static unsigned int count_try_master = 0; - static unsigned int count_wait_answer_confirm_mode = 0; - static unsigned int count_wait_slave_try_master = 0; - unsigned int err_confirm_mode = 0; // - static unsigned int c_buf_log_master_slave_status = 0, prev_status = 0; - - - -// logs master_slave_status - if (edrk.auto_master_slave.status != prev_status) - { - c_buf_log_master_slave_status++; - if (c_buf_log_master_slave_status>=SIZE_LOG_MASTER_SLAVE_STATUS) - c_buf_log_master_slave_status = 0; - buf_log_master_slave_status[c_buf_log_master_slave_status] = edrk.auto_master_slave.status; - } - prev_status = edrk.auto_master_slave.status; -//end logs master_slave_status - - if (edrk.ms.ready2==0 && edrk.errors.e7.bits.AUTO_SET_MASTER==0) - { - edrk.auto_master_slave.remoute.all = 0; - edrk.auto_master_slave.local.all = 0; - edrk.auto_master_slave.prev_remoute.all = edrk.auto_master_slave.remoute.all; - edrk.auto_master_slave.prev_local.all = edrk.auto_master_slave.local.all; - - edrk.auto_master_slave.status = 1; - -// if (prev_ready!=edrk.ms.ready2) -// for (c_buf=0;c_buf=SIZE_BUF1) -// c_buf = 0; -// -// buf1[c_buf] = edrk.auto_master_slave.status; -// buf2[c_buf].all = edrk.auto_master_slave.local.all; -// buf3[c_buf].all = edrk.auto_master_slave.remoute.all; -// buf4[c_buf].all = optical_read_data.data.cmd.all; -// buf5[c_buf].all = optical_write_data.data.cmd.all; -// - - // - if (edrk.auto_master_slave.local.bits.try_master==0 || - (edrk.auto_master_slave.prev_local.bits.try_master != edrk.auto_master_slave.local.bits.try_master && edrk.auto_master_slave.local.bits.try_master==1)) - count_try_master = 0; - - // OPTICAL_BUS , - if (edrk.errors.e7.bits.WRITE_OPTBUS==1 || edrk.errors.e7.bits.READ_OPTBUS==1 || - edrk.warnings.e7.bits.WRITE_OPTBUS==1 || edrk.warnings.e7.bits.READ_OPTBUS==1) - { - - if (edrk.errors.e7.bits.WRITE_OPTBUS==1 || edrk.errors.e7.bits.READ_OPTBUS==1) - { - // , - // - - - - edrk.errors.e7.bits.AUTO_SET_MASTER |= 1; - - edrk.auto_master_slave.remoute.bits.nothing = 1; - edrk.auto_master_slave.remoute.bits.master = 0; - edrk.auto_master_slave.remoute.bits.slave = 0; - edrk.auto_master_slave.remoute.bits.try_master = 0; - edrk.auto_master_slave.remoute.bits.try_slave = 0; - - - edrk.auto_master_slave.local.bits.master = 0; - edrk.auto_master_slave.local.bits.slave = 0; - edrk.auto_master_slave.local.bits.try_master = 0; - edrk.auto_master_slave.local.bits.try_slave = 0; - edrk.auto_master_slave.local.bits.nothing = 1; - - edrk.auto_master_slave.status = 10; - } - else - { - // , - // - edrk.warnings.e7.bits.AUTO_SET_MASTER = 1; - - edrk.auto_master_slave.remoute.bits.nothing = 1; - edrk.auto_master_slave.remoute.bits.master = 0; - edrk.auto_master_slave.remoute.bits.slave = 0; - edrk.auto_master_slave.remoute.bits.try_master = 0; - edrk.auto_master_slave.remoute.bits.try_slave = 0; - - - - edrk.auto_master_slave.local.bits.master = 1; - edrk.auto_master_slave.local.bits.slave = 0; - edrk.auto_master_slave.local.bits.try_master = 0; - edrk.auto_master_slave.local.bits.try_slave = 0; - edrk.auto_master_slave.local.bits.nothing = 1; - - edrk.auto_master_slave.status = 2; - } - - edrk.auto_master_slave.remoute.bits.sync_line_detect = 0; - edrk.auto_master_slave.remoute.bits.bus_off = 1; - edrk.auto_master_slave.remoute.bits.sync1_2 = 0; - - } - else - { - edrk.warnings.e7.bits.AUTO_SET_MASTER = 0; - - edrk.auto_master_slave.remoute.bits.bus_off = 0; - - // OPTICAL_BUS - - if (wait_synhro_optical_bus()==1) - { - - edrk.auto_master_slave.status = 50; // wait synhro - - - } - else - { - - - edrk.auto_master_slave.remoute.bits.master = optical_read_data.data.cmd.bit.master; - edrk.auto_master_slave.remoute.bits.slave = optical_read_data.data.cmd.bit.slave; - edrk.auto_master_slave.remoute.bits.try_master = optical_read_data.data.cmd.bit.maybe_master; - edrk.auto_master_slave.remoute.bits.sync1_2 = optical_read_data.data.cmd.bit.sync_1_2; - edrk.auto_master_slave.remoute.bits.sync_line_detect = optical_read_data.data.cmd.bit.sync_line_detect; - edrk.auto_master_slave.remoute.bits.tick = optical_read_data.data.cmd.bit.wdog_tick; - - if (optical_read_data.data.cmd.bit.master==0 && optical_read_data.data.cmd.bit.slave==0) - edrk.auto_master_slave.remoute.bits.nothing = 1; - - - ////////////////////////////////////////////////// - ////////////////////////////////////////////////// - // 1 - - // - if (edrk.auto_master_slave.remoute.bits.master) - { - - // -? - if (edrk.auto_master_slave.local.bits.master) - { - edrk.errors.e7.bits.AUTO_SET_MASTER |= 1; - edrk.auto_master_slave.status = 3; - } - else - { - // , slave - if (edrk.auto_master_slave.local.bits.master==0 && edrk.auto_master_slave.local.bits.slave==0) - { - // edrk.auto_master_slave.local.bits.try_slave = 1; - // slave - edrk.auto_master_slave.local.bits.slave = 1; - // - edrk.auto_master_slave.local.bits.try_master = 0; - edrk.auto_master_slave.status = 4; - } - else - { - edrk.auto_master_slave.status = 21; - } - } - } - else - ////////////////////////////////////////////////// - ////////////////////////////////////////////////// - // 2 - // slave - - if (edrk.auto_master_slave.remoute.bits.slave) - { - - // slave -? - if (edrk.auto_master_slave.local.bits.slave) - { - - // slave - if (edrk.auto_master_slave.prev_remoute.bits.slave==0) - { - if (edrk.Go) - { - // - edrk.errors.e7.bits.AUTO_SET_MASTER |= 1; - edrk.auto_master_slave.status = 5; - } - else - { - // master - edrk.auto_master_slave.local.bits.try_master = 1; - edrk.auto_master_slave.status = 6; - } - } - else - { - edrk.errors.e7.bits.AUTO_SET_MASTER |= 1; - edrk.auto_master_slave.status = 7; - } - - } - else - { - - // , master - if (edrk.auto_master_slave.local.bits.master==0 && edrk.auto_master_slave.local.bits.slave==0 && edrk.auto_master_slave.local.bits.try_master==0) - { - if (edrk.flag_second_PCH==0) - edrk.auto_master_slave.local.bits.try_master = 1; - if (edrk.flag_second_PCH==1) - edrk.auto_master_slave.local.bits.try_master = 1; - - edrk.auto_master_slave.status = 8; - // edrk.auto_master_slave.local.bits.slave = 1; - } - else - // , slave . - if (edrk.auto_master_slave.local.bits.master==0 && edrk.auto_master_slave.local.bits.slave==0 && edrk.auto_master_slave.local.bits.try_master==1) - { - // - edrk.auto_master_slave.local.bits.master = 1; - edrk.auto_master_slave.local.bits.try_master = 0; - edrk.auto_master_slave.status = 9; - // edrk.auto_master_slave.local.bits.slave = 1; - } - else - { - edrk.auto_master_slave.status = 22; - } - - } - } - else - ////////////////////////////////////////////////// - ////////////////////////////////////////////////// - // 3 - // - - if (edrk.auto_master_slave.remoute.bits.master==0 - && edrk.auto_master_slave.remoute.bits.slave==0 - && edrk.auto_master_slave.remoute.bits.try_master) - { - // slave - if (edrk.auto_master_slave.local.bits.slave) - { - // , slave - // , try_master - if (count_wait_slave_try_master -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "control_station.h" -#include "global_time.h" -#include "vector_control.h" -#include "x_basic_types.h" -#include "xp_cds_in.h" -#include "xp_hwp.h" -#include "xp_project.h" -#include "modbus_table_v2.h" -#include "filter_v1.h" -#include "v_rotor_22220.h" -#include "log_params.h" -#include "break_regul.h" -#include "logs_hmi.h" -#include "CAN_Setup.h" -#include "params_temper_p.h" - - -void func_unpack_answer_from_TMS_RS232(CMD_TO_TMS_STRUCT *pcommand) -{ - // y - unsigned int DataOut; - int Data, Data1, Data2, DataAnalog1, DataAnalog2, DataAnalog3, DataAnalog4, i; - unsigned int h; - volatile unsigned char *pByte; - // static int vs11,vs12,vs1; - // static int DataCnt=0; - // int GoT,Assemble_scheme; - // static int prev_temp_Rele1=0, temp_Rele1=0, prev_temp_Rele2=0, temp_Rele2=0; - - static int flag_prev_turn_on = 0; - static int flag_prev_turn_off = 0; - static int prev_byte01_bit4 = 0; - static int prev_byte01_bit1 = 0; - static int flag_wait_revers_sbor = 1; - static int flag_wait_revers_go = 1; - - static unsigned int count_transmited = 0; - - - // y - // - - if ((sizeof(CMD_TO_TMS_STRUCT)-5)>CONTROL_STATION_MAX_RAW_DATA) - xerror(main_er_ID(2),(void *)0); - - - - // , .. RS232 - pByte = (unsigned char *)(pcommand);//->analog_data.analog1_lo; - - - pByte++; - pByte++; - - for (h=0;h>1].all |= ( (*pByte) << 8) & 0xff00; - } - else - control_station.raw_array_data[CONTROL_STATION_TERMINAL_RS232][h>>1].all = ( (*pByte) ) & 0x00ff; - - pByte++; - } - -} - - - -void func_pack_answer_to_TMS(TMS_TO_TERMINAL_STRUCT *reply_a) -{ - // y - unsigned int DataOut; - int Data1, Data2, DataAnalog1, DataAnalog2, DataAnalog3, DataAnalog4, i; - float Data; - unsigned char *pByte; - // static int vs11,vs12,vs1; - // static int DataCnt=0; - // int GoT,Assemble_scheme; - // static int prev_temp_Rele1=0, temp_Rele1=0, prev_temp_Rele2=0, temp_Rele2=0; - - static int flag_prev_turn_on = 0; - static int flag_prev_turn_off = 0; - static int prev_byte01_bit4 = 0; - static int prev_byte01_bit1 = 0; - static int flag_wait_revers_sbor = 1; - static int flag_wait_revers_go = 1; - - static unsigned int count_transmited = 0; - /* const - */ - -// edrk.data_to_message2[1] = _IQtoF(filter.iqU_1_long)*NORMA_ACP; -// edrk.data_to_message2[2] = _IQtoF(filter.iqU_2_long)*NORMA_ACP; - - - //For instance - //reply->digit_data.byte01.byte_data = 0x43; - - - //1 - Data = _IQtoF(filter.iqU_1_long)*NORMA_ACP; - reply_a->analog_data.analog1_lo = LOBYTE(Data); - reply_a->analog_data.analog1_hi = HIBYTE(Data); - //2 - Data = _IQtoF(filter.iqU_2_long)*NORMA_ACP;//(project.adc[0].read.pbus.adc_value[1] - 2330)/4096*3.0/62.2*1000.0; - reply_a->analog_data.analog2_lo = LOBYTE(Data); - reply_a->analog_data.analog2_hi = HIBYTE(Data); - - //3 - Data = _IQtoF(filter.iqUin_m1)*NORMA_ACP;//(project.adc[0].read.pbus.adc_value[2] - 2330)/4096*3.0/62.2*1000.0; - reply_a->analog_data.analog3_lo = LOBYTE(Data); - reply_a->analog_data.analog3_hi = HIBYTE(Data); - -//4 -// Data = edrk.Status_Sbor;//_IQtoF(filter.iqUin_m2)*NORMA_ACP;//(project.adc[0].read.pbus.adc_value[3] - 2330)/4096*3.0/62.2*1000.0; - Data = _IQtoF(filter.iqUin_m2)*NORMA_ACP;//(project.adc[0].read.pbus.adc_value[3] - 2330)/4096*3.0/62.2*1000.0; -// Data = (_IQtoF((filter.Power) * 9000.0)); // - reply_a->analog_data.analog4_lo = LOBYTE(Data); - reply_a->analog_data.analog4_hi = HIBYTE(Data); - -//5 - Data = edrk.power_kw; // -// Data = (_IQtoF((analog.Power) * 9000.0)); // - //_IQtoF(analog.iqIin_1)*NORMA_ACP;//project.adc[0].read.pbus.adc_value[0]; - reply_a->analog_data.analog5_lo = LOBYTE(Data); - reply_a->analog_data.analog5_hi = HIBYTE(Data); - - Data = _IQtoF(analog.iqIin_sum)*NORMA_ACP;//project.adc[0].read.pbus.adc_value[1]; - reply_a->analog_data.analog6_lo = LOBYTE(Data); - reply_a->analog_data.analog6_hi = HIBYTE(Data); - - - Data = _IQtoF(filter.iqIm_1)*NORMA_ACP;//project.adc[0].read.pbus.adc_value[2]; - reply_a->analog_data.analog7_lo = LOBYTE(Data); - reply_a->analog_data.analog7_hi = HIBYTE(Data); - - Data = _IQtoF(filter.iqIm_2)*NORMA_ACP;//project.adc[0].read.pbus.adc_value[3]; - reply_a->analog_data.analog8_lo = LOBYTE(Data); - reply_a->analog_data.analog8_hi = HIBYTE(Data); - - Data = (int)(edrk.temper_edrk.max_real_int_temper_u); - reply_a->analog_data.analog9_lo = LOBYTE(Data); - reply_a->analog_data.analog9_hi = HIBYTE(Data); - - Data = (int) (edrk.temper_edrk.max_real_int_temper_water); - reply_a->analog_data.analog10_lo = LOBYTE(Data); - reply_a->analog_data.analog10_hi = HIBYTE(Data); - - Data = (int) (edrk.p_water_edrk.filter_real_int_p_water[0]); - reply_a->analog_data.analog11_lo = LOBYTE(Data); - reply_a->analog_data.analog11_hi = HIBYTE(Data); - - Data = (int) (edrk.temper_edrk.max_real_int_temper_air);//_IQtoF(edrk.f_stator)*F_STATOR_MAX;// - reply_a->analog_data.analog12_lo = LOBYTE(Data); - reply_a->analog_data.analog12_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.zadanie.iq_ZadanieU_Charge_rmp)*NORMA_ACP);//edrk.I_zad_vozbud;// - reply_a->analog_data.analog13_lo = LOBYTE(Data); - reply_a->analog_data.analog13_hi = HIBYTE(Data); - - Data = edrk.zadanie.oborots_zad;//edrk.I_zad_vozbud_exp;// - reply_a->analog_data.analog14_lo = LOBYTE(Data); - reply_a->analog_data.analog14_hi = HIBYTE(Data); - - Data = edrk.zadanie.power_zad;//edrk.I_cur_vozbud;// - reply_a->analog_data.analog15_lo = LOBYTE(Data); - reply_a->analog_data.analog15_hi = HIBYTE(Data); - - Data = edrk.zadanie.Izad;//edrk.I_cur_vozbud_exp;// - reply_a->analog_data.analog16_lo = LOBYTE(Data); - reply_a->analog_data.analog16_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.Kplus)*1000.0);//edrk.W_zad_mA;// - reply_a->analog_data.analog17_lo = LOBYTE(Data); - reply_a->analog_data.analog17_hi = HIBYTE(Data); - -Data = fast_round(edrk.freq_50hz_1/10.0);//edrk.Zadanie2VozbudING;// - reply_a->analog_data.analog18_lo = LOBYTE(Data); - reply_a->analog_data.analog18_hi = HIBYTE(Data); - -Data =fast_round(_IQtoF(edrk.f_stator)*NORMA_FROTOR*100.0);// 0;//edrk.Zadanie2VozbudMY;// - reply_a->analog_data.analog19_lo = LOBYTE(Data); - reply_a->analog_data.analog19_hi = HIBYTE(Data); - -Data = fast_round(_IQtoF(edrk.k_stator1)*10000.0);// edrk.W_from_all; - reply_a->analog_data.analog20_lo = LOBYTE(Data); - reply_a->analog_data.analog20_hi = HIBYTE(Data); - -Data = _IQtoF(vect_control.iqId1)*NORMA_ACP;//0;//_IQtoF(edrk.test_rms_Iu)*NORMA_ACP; //fast_round(_IQtoF(WRotor.iqWRotorImpulses1)*NORMA_FROTOR*1000.0);// edrk.W_from_DISPLAY; - reply_a->analog_data.analog21_lo = LOBYTE(Data); - reply_a->analog_data.analog21_hi = HIBYTE(Data); - -Data = _IQtoF(vect_control.iqIq1)*NORMA_ACP;// 0;//_IQtoF(edrk.test_rms_Ua)*NORMA_ACP;// fast_round(_IQtoF(WRotor.iqWRotorImpulses2)*NORMA_FROTOR*1000.0);//600.0*1000000000.0/WRotor.iqWRotorImpulses1;//edrk.W_from_SVU; - reply_a->analog_data.analog22_lo = LOBYTE(Data); - reply_a->analog_data.analog22_hi = HIBYTE(Data); - -//Data = fast_round(_IQtoF(WRotor.iqWRotorSumFilter3)*NORMA_FROTOR*100.0*60.0);//edrk.oborots;//fast_round(_IQtoF(WRotorPBus.iqAngle1F)*360.0);//600.0*1000000000.0/WRotor.iqWRotorImpulses2;//edrk.W_from_ZADAT4IK; -Data = _IQtoF(WRotor.iqWRotorSumFilter) * NORMA_FROTOR*600.0;//edrk.oborots; - reply_a->analog_data.analog23_lo = LOBYTE(Data); - reply_a->analog_data.analog23_hi = HIBYTE(Data); - -Data = fast_round(edrk.f_rotor_hz*100.0);//fast_round(_IQtoF(WRotorPBus.iqAngle2F)*360.0);//; - reply_a->analog_data.analog24_lo = LOBYTE(Data); - reply_a->analog_data.analog24_hi = HIBYTE(Data); - -//Data = _IQtoF(edrk.k_stator1)*10000;//; -Data = edrk.period_calc_pwm_int2;//fast_round(_IQtoF(rotor_22220.iqFdirty)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog25_lo = LOBYTE(Data); - reply_a->analog_data.analog25_hi = HIBYTE(Data); - -Data = edrk.power_kw_full; // -//fast_round(_IQtoF(WRotor.iqWRotorCalcBeforeRegul2)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog26_lo = LOBYTE(Data); - reply_a->analog_data.analog26_hi = HIBYTE(Data); - -Data = edrk.Sbor_Mode;//fast_round(_IQtoF(WRotorPBus.iqWRotorCalcBeforeRegul1)*NORMA_FROTOR*1000.0);//edrk.count_lost_interrupt; - reply_a->analog_data.analog27_lo = LOBYTE(Data); - reply_a->analog_data.analog27_hi = HIBYTE(Data); - -Data = edrk.Stage_Sbor;// fast_round(_IQtoF(WRotorPBus.iqWRotorCalcBeforeRegul2)*NORMA_FROTOR*1000.0);; - reply_a->analog_data.analog28_lo = LOBYTE(Data); - reply_a->analog_data.analog28_hi = HIBYTE(Data); - -Data = fast_round(_IQtoF(edrk.Izad_out)*NORMA_ACP);//edrk.I_zad_vozbud;//; - reply_a->analog_data.analog29_lo = LOBYTE(Data); - reply_a->analog_data.analog29_hi = HIBYTE(Data); - -Data = edrk.period_calc_pwm_int1; - reply_a->analog_data.analog30_lo = LOBYTE(Data); - reply_a->analog_data.analog30_hi = HIBYTE(Data); - - Data = (int)edrk.temper_acdrive.winding.max_real_int_temper; - reply_a->analog_data.analog31_lo = LOBYTE(Data); - reply_a->analog_data.analog31_hi = HIBYTE(Data); - Data = (int)edrk.temper_acdrive.bear.max_real_int_temper; - reply_a->analog_data.analog32_lo = LOBYTE(Data); - reply_a->analog_data.analog32_hi = HIBYTE(Data); - - - - Data = (int)(edrk.temper_edrk.real_int_temper_u[0]); - reply_a->analog_data.analog33_lo = LOBYTE(Data); - reply_a->analog_data.analog33_hi = HIBYTE(Data); - - Data = (int)(edrk.temper_edrk.real_int_temper_u[1]); - reply_a->analog_data.analog34_lo = LOBYTE(Data); - reply_a->analog_data.analog34_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_u[2]); - reply_a->analog_data.analog35_lo = LOBYTE(Data); - reply_a->analog_data.analog35_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_u[3]); - reply_a->analog_data.analog36_lo = LOBYTE(Data); - reply_a->analog_data.analog36_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_u[4]); - reply_a->analog_data.analog37_lo = LOBYTE(Data); - reply_a->analog_data.analog37_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_u[5]); - reply_a->analog_data.analog38_lo = LOBYTE(Data); - reply_a->analog_data.analog38_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_u[6]); - reply_a->analog_data.analog39_lo = LOBYTE(Data); - reply_a->analog_data.analog39_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_air[0]); - reply_a->analog_data.analog40_lo = LOBYTE(Data); - reply_a->analog_data.analog40_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_air[1]); - reply_a->analog_data.analog41_lo = LOBYTE(Data); - reply_a->analog_data.analog41_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_air[2]); - reply_a->analog_data.analog42_lo = LOBYTE(Data); - reply_a->analog_data.analog42_hi = HIBYTE(Data); - - Data = (int)(edrk.temper_edrk.real_int_temper_air[3]); - reply_a->analog_data.analog43_lo = LOBYTE(Data); - reply_a->analog_data.analog43_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_water[0]); // external - reply_a->analog_data.analog44_lo = LOBYTE(Data); - reply_a->analog_data.analog44_hi = HIBYTE(Data); - Data = (int)(edrk.temper_edrk.real_int_temper_water[1]); // internal - reply_a->analog_data.analog45_lo = LOBYTE(Data); - reply_a->analog_data.analog45_hi = HIBYTE(Data); - - - Data = fast_round(_IQtoF(simple_scalar1.pidF.OutMax)*NORMA_ACP);//edrk.auto_master_slave.prev_status;//fast_round(_IQtoF(edrk.zadanie.iq_ZadanieU_Charge_rmp)*NORMA_ACP); - reply_a->analog_data.analog46_lo = LOBYTE(Data); - reply_a->analog_data.analog46_hi = HIBYTE(Data); - - - Data = fast_round(_IQtoF(edrk.zadanie.iq_Izad_rmp)*NORMA_ACP); - reply_a->analog_data.analog47_lo = LOBYTE(Data); - reply_a->analog_data.analog47_hi = HIBYTE(Data); - Data = fast_round(_IQtoF(edrk.zadanie.iq_fzad_rmp)*NORMA_FROTOR*100.0); - reply_a->analog_data.analog48_lo = LOBYTE(Data); - reply_a->analog_data.analog48_hi = HIBYTE(Data); - Data = fast_round(_IQtoF(edrk.zadanie.iq_kzad_rmp)*10000.0); - reply_a->analog_data.analog49_lo = LOBYTE(Data); - reply_a->analog_data.analog49_hi = HIBYTE(Data); - Data = fast_round(_IQtoF(edrk.zadanie.iq_oborots_zad_hz_rmp)*NORMA_FROTOR*60.0); - reply_a->analog_data.analog50_lo = LOBYTE(Data); - reply_a->analog_data.analog50_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.zadanie.iq_power_zad_rmp)*NORMA_ACP*NORMA_ACP/1000.0); - reply_a->analog_data.analog51_lo = LOBYTE(Data); - reply_a->analog_data.analog51_hi = HIBYTE(Data); - - Data = _IQtoF(vect_control.iqId2)*NORMA_ACP;//0;//fast_round( _IQtoF(edrk.zadanie.iq_k_u_disbalance_rmp)*100.0); - reply_a->analog_data.analog52_lo = LOBYTE(Data); - reply_a->analog_data.analog52_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.zadanie.iq_limit_power_zad_rmp)*NORMA_ACP*NORMA_ACP/1000.0); - reply_a->analog_data.analog53_lo = LOBYTE(Data); - reply_a->analog_data.analog53_hi = HIBYTE(Data); - - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER) { - Data = fast_round(_IQtoF(turns.pidFvect.Out)*NORMA_ACP); - reply_a->analog_data.analog54_lo = LOBYTE(Data); - reply_a->analog_data.analog54_hi = HIBYTE(Data); - Data = fast_round(_IQtoF(turns.pidFvect.OutMax)*NORMA_ACP); - reply_a->analog_data.analog55_lo = LOBYTE(Data); - reply_a->analog_data.analog55_hi = HIBYTE(Data); - Data =fast_round(_IQtoF(power.pidP.Out)*NORMA_ACP); - reply_a->analog_data.analog56_lo = LOBYTE(Data); - reply_a->analog_data.analog56_hi = HIBYTE(Data); - Data = fast_round(_IQtoF(power.pidP.OutMax)*NORMA_ACP); - } else { - Data = fast_round(_IQtoF(simple_scalar1.pidF.Out)*NORMA_ACP); - reply_a->analog_data.analog54_lo = LOBYTE(Data); - reply_a->analog_data.analog54_hi = HIBYTE(Data); - Data = fast_round(_IQtoF(simple_scalar1.pidF.OutMin)*NORMA_ACP); - reply_a->analog_data.analog55_lo = LOBYTE(Data); - reply_a->analog_data.analog55_hi = HIBYTE(Data); - Data =fast_round(_IQtoF(simple_scalar1.pidPower.Out)*NORMA_ACP); - reply_a->analog_data.analog56_lo = LOBYTE(Data); - reply_a->analog_data.analog56_hi = HIBYTE(Data); - Data = fast_round(_IQtoF(simple_scalar1.pidPower.OutMax)*NORMA_ACP); - } - - reply_a->analog_data.analog57_lo = LOBYTE(Data); - reply_a->analog_data.analog57_hi = HIBYTE(Data); - - - - Data = fast_round(_IQtoF(simple_scalar1.Izad)*NORMA_ACP); - reply_a->analog_data.analog58_lo = LOBYTE(Data); - reply_a->analog_data.analog58_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.master_Iq)*NORMA_ACP); - reply_a->analog_data.analog59_lo = LOBYTE(Data); - reply_a->analog_data.analog59_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.mzz_zad_in2)*NORMA_ACP);//count_transmited++; - reply_a->analog_data.analog60_lo = LOBYTE(Data); - reply_a->analog_data.analog60_hi = HIBYTE(Data); -// - Data = modbus_table_can_in[123].all;// ( , ) - reply_a->analog_data.analog61_lo = LOBYTE(Data); - reply_a->analog_data.analog61_hi = HIBYTE(Data); - - Data = modbus_table_can_in[124].all;// () - reply_a->analog_data.analog62_lo = LOBYTE(Data); - reply_a->analog_data.analog62_hi = HIBYTE(Data); - - Data = modbus_table_can_in[125].all;// () - reply_a->analog_data.analog63_lo = LOBYTE(Data); - reply_a->analog_data.analog63_hi = HIBYTE(Data); - - Data = modbus_table_can_in[134].all;// - reply_a->analog_data.analog64_lo = LOBYTE(Data); - reply_a->analog_data.analog64_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidPower.SatErr)*NORMA_ACP);//project.cds_tk[3].optical_data_in.local_count_error; - reply_a->analog_data.analog65_lo = LOBYTE(Data); - reply_a->analog_data.analog65_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidPower.Fdb)*NORMA_ACP*NORMA_ACP/1000.0);//project.cds_tk[3].optical_data_out.local_count_error; - reply_a->analog_data.analog66_lo = LOBYTE(Data); - reply_a->analog_data.analog66_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidPower.Ref)*NORMA_ACP*NORMA_ACP/1000.0);//////optical_read_data.count_error_wdog; - reply_a->analog_data.analog67_lo = LOBYTE(Data); - reply_a->analog_data.analog67_hi = HIBYTE(Data); - - - Data = fast_round(_IQtoF(simple_scalar1.pidPower.Up)*NORMA_ACP);//edrk.auto_master_slave.status;//fast_round(_IQtoF(edrk.zadanie.iq_kplus_u_disbalance_rmp)*1000.0); - reply_a->analog_data.analog68_lo = LOBYTE(Data); - reply_a->analog_data.analog68_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(pll1.vars.pll_Uq)*NORMA_ACP); - reply_a->analog_data.analog69_lo = LOBYTE(Data); - reply_a->analog_data.analog69_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(pll1.vars.pll_Ud)*NORMA_ACP); - reply_a->analog_data.analog70_lo = LOBYTE(Data); - reply_a->analog_data.analog70_hi = HIBYTE(Data); - -Data = fast_round(_IQtoF(simple_scalar1.bpsi_curent)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog71_lo = LOBYTE(Data); - reply_a->analog_data.analog71_hi = HIBYTE(Data); - -Data = fast_round(_IQtoF(WRotor.iqWRotorSumFilter2)*NORMA_FROTOR*1000.0); //iqFlong - reply_a->analog_data.analog72_lo = LOBYTE(Data); - reply_a->analog_data.analog72_hi = HIBYTE(Data); - -//Data = fast_round(_IQtoF(WRotor.iqWRotorCalc1)*NORMA_FROTOR*1000.0); -Data = fast_round(_IQtoF(edrk.from_uom.iq_level_value_kwt)*NORMA_ACP*NORMA_ACP/1000.0);// ;//edrk.from_uom.level_value;//fast_round(_IQtoF(rotor_22220.iqFdirty)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog73_lo = LOBYTE(Data); - reply_a->analog_data.analog73_hi = HIBYTE(Data); - -//Data = fast_round(_IQtoF(WRotor.iqWRotorCalc2)*NORMA_FROTOR*1000.0); -Data = _IQtoF(vect_control.iqIq2)*NORMA_ACP;//0;//fast_round(_IQtoF(rotor_22220.iqF)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog74_lo = LOBYTE(Data); - reply_a->analog_data.analog74_hi = HIBYTE(Data); - -//Data = fast_round(_IQtoF(WRotor.iqWRotorImpulsesBeforeRegul1)*NORMA_FROTOR*1000.0);// edrk.W_from_DISPLAY; -Data = fast_round(_IQtoF(WRotor.iqWRotorSumFilter)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog75_lo = LOBYTE(Data); - reply_a->analog_data.analog75_hi = HIBYTE(Data); - -//Data = fast_round(_IQtoF(WRotor.iqWRotorImpulsesBeforeRegul2)*NORMA_FROTOR*1000.0);//600.0*1000000000.0/WRotor.iqWRotorImpulses1;//edrk.W_from_SVU; -Data = fast_round(_IQtoF(simple_scalar1.mzz_zad_int)*NORMA_ACP);//;//0;//fast_round(_IQtoF(rotor_22220.iqFlong)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog76_lo = LOBYTE(Data); - reply_a->analog_data.analog76_hi = HIBYTE(Data); - -//Data = fast_round(_IQtoF(WRotor.iqWRotorImpulses1)*NORMA_FROTOR*1000.0);// edrk.W_from_DISPLAY; - Data = _IQtoF(simple_scalar1.Izad)*NORMA_ACP_RMS;// 0;//fast_round(_IQtoF(WRotor.iqWRotorSumRamp)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog77_lo = LOBYTE(Data); - reply_a->analog_data.analog77_hi = HIBYTE(Data); - -//Data = fast_round(_IQtoF(WRotor.iqWRotorImpulses2)*NORMA_FROTOR*1000.0);//600.0*1000000000.0/WRotor.iqWRotorImpulses1;//edrk.W_from_SVU; - Data = WRotor.RotorDirectionSlow; - reply_a->analog_data.analog78_lo = LOBYTE(Data); - reply_a->analog_data.analog78_hi = HIBYTE(Data); - -Data = fast_round(_IQtoF(simple_scalar1.iqKoefOgran)*1000.0);//0;//fast_round(_IQtoF(WRotor.iqWRotorSum)*NORMA_FROTOR*1000.0);// edrk.W_from_DISPLAY; - reply_a->analog_data.analog79_lo = LOBYTE(Data); - reply_a->analog_data.analog79_hi = HIBYTE(Data); - -Data = fast_round(_IQtoF(simple_scalar1.iqKoefOgranIzad)*1000.0);//0;//fast_round(_IQtoF(WRotor.iqWRotorSumFilter)*NORMA_FROTOR*1000.0);//600.0*1000000000.0/WRotor.iqWRotorImpulses1;//edrk.W_from_SVU; - reply_a->analog_data.analog80_lo = LOBYTE(Data); - reply_a->analog_data.analog80_hi = HIBYTE(Data); - - Data = log_params.cur_volume_of_slow_log;//edrk.power_kw_full; // -// Data = (_IQtoF((analog.Power) * 9000.0)); // - //_IQtoF(analog.iqIin_1)*NORMA_ACP;//project.adc[0].read.pbus.adc_value[0]; - reply_a->analog_data.analog81_lo = LOBYTE(Data); - reply_a->analog_data.analog81_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.zadanie.iq_limit_power_zad)*NORMA_ACP*NORMA_ACP/1000.0); - reply_a->analog_data.analog82_lo = LOBYTE(Data); - reply_a->analog_data.analog82_hi = HIBYTE(Data); - - Data = break_result_1;//fast_round(_IQtoF(WRotor.iqWRotorSumFilter3)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog83_lo = LOBYTE(Data); - reply_a->analog_data.analog83_hi = HIBYTE(Data); - - Data = break_result_2;//WRotorPBus.RotorDirectionInstant; - reply_a->analog_data.analog84_lo = LOBYTE(Data); - reply_a->analog_data.analog84_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.all_limit_koeffs.sum_limit)*1000.0);//WRotorPBus.RotorDirectionCount; - reply_a->analog_data.analog85_lo = LOBYTE(Data); - reply_a->analog_data.analog85_hi = HIBYTE(Data); - - - Data = fast_round(_IQtoF(edrk.all_limit_koeffs.uom_limit)*1000.0);//WRotorPBus.RotorDirectionSlow2; - reply_a->analog_data.analog86_lo = LOBYTE(Data); - reply_a->analog_data.analog86_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(edrk.all_limit_koeffs.uin_freq_limit)*1000.0);//fast_round(_IQtoF(WRotor.iqWRotorSumRamp)*NORMA_FROTOR*1000.0); - reply_a->analog_data.analog87_lo = LOBYTE(Data); - reply_a->analog_data.analog87_hi = HIBYTE(Data); - - Data = _IQtoF(simple_scalar1.Im_regul)*NORMA_ACP_RMS;//(edrk.cantec_reg & 0xff);//edrk.pult_data.TimeToChangePump_from_pult;//0;//fast_round(_IQtoF(WRotor.iqWRotorCalc1Ramp)*NORMA_FROTOR*1000.0);;//WRotor.iqWRotorCalc1Ramp - reply_a->analog_data.analog88_lo = LOBYTE(Data); - reply_a->analog_data.analog88_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidPower.Ui)*NORMA_ACP);//(edrk.canrec_reg & 0xff);;//edrk.pult_data.nPCH_from_pult;//0;//fast_round(_IQtoF(WRotor.iqWRotorCalc2Ramp)*NORMA_FROTOR*1000.0);; - reply_a->analog_data.analog89_lo = LOBYTE(Data); - reply_a->analog_data.analog89_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidF.Fdb)*NORMA_FROTOR*1000.0);//(((unsigned long)edrk.canes_reg>>16) & 0x01ff); - reply_a->analog_data.analog90_lo = LOBYTE(Data); - reply_a->analog_data.analog90_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidF.Ref)*NORMA_FROTOR*1000.0);//(((unsigned long)edrk.canes_reg) & 0x3f); - reply_a->analog_data.analog91_lo = LOBYTE(Data); - reply_a->analog_data.analog91_hi = HIBYTE(Data); - - - Data = fast_round(_IQtoF(simple_scalar1.pidF.SatErr)*NORMA_ACP);//CanBusOffError;//0; - reply_a->analog_data.analog92_lo = LOBYTE(Data); - reply_a->analog_data.analog92_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidF.Ui)*NORMA_ACP);//CanTimeOutErrorTR;//0; - reply_a->analog_data.analog93_lo = LOBYTE(Data); - reply_a->analog_data.analog93_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.pidF.Up)*NORMA_ACP);//0; - reply_a->analog_data.analog94_lo = LOBYTE(Data); - reply_a->analog_data.analog94_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.iq_decr_mzz_power)*1000.0);//0;//simple_scalar1.k_ogr_n - reply_a->analog_data.analog95_lo = LOBYTE(Data); - reply_a->analog_data.analog95_hi = HIBYTE(Data); - - Data = fast_round(_IQtoF(simple_scalar1.iq_decr_mzz_power_filter)*1000.0);//fast_round(_IQtoF(edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus1)*NORMA_FROTOR*60.0*450.0*1000.0); - reply_a->analog_data.analog96_lo = LOBYTE(Data); - reply_a->analog_data.analog96_hi = HIBYTE(Data); - -// Data = 0; -// reply_a->analog_data.analog97_lo = LOBYTE(Data); -// reply_a->analog_data.analog97_hi = HIBYTE(Data); - - - - pByte = &reply_a->digit_data.byte01.byte_data; - for (i = 0; i < 59; i++) //zero all dig data - { - *(pByte + i) = 0; - } - - -// reply->digit_data.byte01.byte_data = project.cds_in[1].read.pbus.data_in.all & 0xff; -// reply->digit_data.byte02.byte_data = (project.cds_in[1].read.pbus.data_in.all >> 8) & 0xff; - - reply_a->digit_data.byte01.byte_data = edrk.errors.e0.all & 0xff; - reply_a->digit_data.byte02.byte_data = (edrk.errors.e0.all >> 8) & 0xff; - - reply_a->digit_data.byte03.byte_data = edrk.errors.e1.all & 0xff; - reply_a->digit_data.byte04.byte_data = (edrk.errors.e1.all >> 8) & 0xff; - - reply_a->digit_data.byte05.byte_data = edrk.errors.e2.all & 0xff; - reply_a->digit_data.byte06.byte_data = (edrk.errors.e2.all >> 8) & 0xff; - - reply_a->digit_data.byte07.byte_data = edrk.errors.e3.all & 0xff; - reply_a->digit_data.byte08.byte_data = (edrk.errors.e3.all >> 8) & 0xff; - - reply_a->digit_data.byte09.byte_data = edrk.errors.e4.all & 0xff; - reply_a->digit_data.byte10.byte_data = (edrk.errors.e4.all >> 8) & 0xff; - - reply_a->digit_data.byte11.byte_data = edrk.errors.e5.all & 0xff; - reply_a->digit_data.byte12.byte_data = (edrk.errors.e5.all >> 8) & 0xff; - -//13 - if (edrk.Status_Perehod_Rascepitel) - reply_a->digit_data.byte13.bit_data.bit1 = 1; - else - reply_a->digit_data.byte13.bit_data.bit1 = 0; - - if (edrk.Status_Rascepitel_Ok) - reply_a->digit_data.byte13.bit_data.bit0 = 1; - else - reply_a->digit_data.byte13.bit_data.bit0 = 0; - - reply_a->digit_data.byte13.bit_data.bit2 = edrk.from_second_pch.bits.MASTER; - reply_a->digit_data.byte13.bit_data.bit3 = edrk.from_second_pch.bits.RASCEPITEL; - - reply_a->digit_data.byte13.bit_data.bit4 = edrk.warning; - reply_a->digit_data.byte13.bit_data.bit5 = edrk.overheat; - reply_a->digit_data.byte13.bit_data.bit6 = edrk.summ_errors; - reply_a->digit_data.byte13.bit_data.bit7 = edrk.Status_Ready.bits.ready_final; - - -// reply_a->digit_data.byte13.byte_data = edrk.errors.e6.all & 0xff; -// reply->digit_data.byte14.byte_data = (edrk.errors.e6.all >> 8) & 0xff; -/* - reply->digit_data.byte15.byte_data = edrk.errors.e7.all & 0xff; - reply->digit_data.byte16.byte_data = (edrk.errors.e7.all >> 8) & 0xff; - - reply->digit_data.byte17.byte_data = edrk.errors.e8.all & 0xff; - reply->digit_data.byte18.byte_data = (edrk.errors.e8.all >> 8) & 0xff; -*/ -//IN2 - reply_a->digit_data.byte14.byte_data = edrk.from_ing1.all & 0xff;// project.cds_in[1].read.pbus.data_in.all & 0xFF; - reply_a->digit_data.byte15.byte_data = (edrk.from_ing1.all >> 8) & 0xFF;//(project.cds_in[1].read.pbus.data_in.all >> 8) & 0xFF; - -// status plates - reply_a->digit_data.byte16.bit_data.bit0 = !(project.hwp[0].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneHWP_Chanals_IsReady; - reply_a->digit_data.byte16.bit_data.bit1 = !(project.adc[0].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneHWP_Chanals_IsReady; - reply_a->digit_data.byte16.bit_data.bit2 = !(project.adc[1].status == component_Ready); - reply_a->digit_data.byte16.bit_data.bit3 = !(project.cds_in[0].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneIN0_IsReady; - reply_a->digit_data.byte16.bit_data.bit4 = !(project.cds_in[1].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneIN1_IsReady; -// reply_ans->digit_data.byte21.bit_data.bit5 = !project.cds_in[2].status;//XProject_balzam.IsReady_reg.bit.XPlaneIN2_IsReady; - reply_a->digit_data.byte16.bit_data.bit5 = !(project.cds_out[0].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneOUT0_IsReady; -// reply_ans->digit_data.byte21.bit_data.bit7 = !project.cds_out[1].status;//XProject_balzam.IsReady_reg.bit.XPlaneOUT1_IsReady; -// reply_ans->digit_data.byte22.bit_data.bit0 = !project.cds_out[2].status;//XProject_balzam.IsReady_reg.bit.XPlaneOUT2_IsReady; - reply_a->digit_data.byte16.bit_data.bit6 = !(project.cds_tk[0].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneTK0_IsReady; - reply_a->digit_data.byte16.bit_data.bit7 = !(project.cds_tk[1].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneTK1_IsReady; - reply_a->digit_data.byte17.bit_data.bit0 = !(project.cds_tk[2].status == component_Ready);//XProject_balzam.IsReady_reg.bit.XPlaneTK2_IsReady; - reply_a->digit_data.byte17.bit_data.bit1 = !(project.cds_tk[3].status == component_Ready);//!XProject_balzam.IsReady_reg.bit.XPlaneTK3_IsReady; -// reply_ans->digit_data.byte22.bit_data.bit5 = !project.adc[1].status;//XProject_balzam.IsReady_reg.bit.XPlaneHWP_Chanals_IsReady; - - -//IN1 - reply_a->digit_data.byte17.bit_data.bit2 = project.cds_in[0].read.pbus.data_in.bit.in0; - reply_a->digit_data.byte17.bit_data.bit3 = project.cds_in[0].read.pbus.data_in.bit.in1; - reply_a->digit_data.byte17.bit_data.bit4 = project.cds_in[0].read.pbus.data_in.bit.in2; - reply_a->digit_data.byte17.bit_data.bit5 = project.cds_in[0].read.pbus.data_in.bit.in3; - reply_a->digit_data.byte17.bit_data.bit6 = project.cds_in[0].read.pbus.data_in.bit.in4; - - reply_a->digit_data.byte17.bit_data.bit7 = project.cds_in[0].read.pbus.data_in.bit.in8; - - reply_a->digit_data.byte18.bit_data.bit0 = project.cds_in[0].read.pbus.data_in.bit.in9; - reply_a->digit_data.byte18.bit_data.bit1 = project.cds_in[0].read.pbus.data_in.bit.in10; - reply_a->digit_data.byte18.bit_data.bit2 = project.cds_in[0].read.pbus.data_in.bit.in11; - - -//out - reply_a->digit_data.byte18.bit_data.bit3 = edrk.to_ing.bits.ZARYAD_ON;// project.cds_out[0].write.sbus.data_out.bit.dout0; - - reply_a->digit_data.byte18.bit_data.bit4 = edrk.to_ing.bits.NAGREV_OFF;//project.cds_out[0].write.sbus.data_out.bit.dout1; - reply_a->digit_data.byte18.bit_data.bit5 = edrk.to_ing.bits.NASOS_1_ON;//project.cds_out[0].write.sbus.data_out.bit.dout2; - reply_a->digit_data.byte18.bit_data.bit6 = edrk.to_ing.bits.NASOS_2_ON;//project.cds_out[0].write.sbus.data_out.bit.dout3; - reply_a->digit_data.byte18.bit_data.bit7 = edrk.to_ing.bits.BLOCK_KEY_OFF;//project.cds _out[0].write.sbus.data_out.bit.dout4; - reply_a->digit_data.byte19.bit_data.bit0 = (edrk.to_shema.bits.UMP_ON_OFF || edrk.to_shema.bits.CROSS_UMP_ON_OFF);//project.cds_out[0].write.sbus.data_out.bit.dout5; - reply_a->digit_data.byte19.bit_data.bit1 = edrk.to_shema.bits.QTV_ON || edrk.to_shema.bits.QTV_ON_OFF || edrk.to_shema.bits.CROSS_QTV_ON_OFF;//project.cds_out[0].write.sbus.data_out.bit.dout6; - reply_a->digit_data.byte19.bit_data.bit2 = edrk.to_shema.bits.QTV_OFF;//project.cds_out[0].write.sbus.data_out.bit.dout7; - - reply_a->digit_data.byte19.bit_data.bit3 = edrk.to_ing.bits.RASCEPITEL_OFF; - reply_a->digit_data.byte19.bit_data.bit4 = edrk.to_ing.bits.RASCEPITEL_ON; - reply_a->digit_data.byte19.bit_data.bit5 = sync_data.local_flag_sync_1_2;// - reply_a->digit_data.byte19.bit_data.bit6 = edrk.flag_second_PCH;// - reply_a->digit_data.byte19.bit_data.bit7 = edrk.to_ing.bits.SMALL_LAMPA_AVARIA; //project.cds_out[0].write.sbus.data_out.bit.dout15; - -//20 - reply_a->digit_data.byte20.bit_data.bit0 = edrk.SumSbor; - - reply_a->digit_data.byte20.bit_data.bit1 = edrk.Status_Ready.bits.ready1; - reply_a->digit_data.byte20.bit_data.bit2 = edrk.Status_Ready.bits.ready2; - reply_a->digit_data.byte20.bit_data.bit3 = edrk.Status_Ready.bits.ready3; - reply_a->digit_data.byte20.bit_data.bit4 = edrk.Status_Ready.bits.ready4; - reply_a->digit_data.byte20.bit_data.bit5 = edrk.Status_Ready.bits.ready5; - reply_a->digit_data.byte20.bit_data.bit6 = edrk.Status_Charge; - reply_a->digit_data.byte20.bit_data.bit7 = edrk.Zaryad_OK; - - reply_a->digit_data.byte21.byte_data = edrk.errors.e6.all & 0xff; - reply_a->digit_data.byte22.byte_data = (edrk.errors.e6.all >> 8) & 0xff; - -// reply_a->digit_data.byte21.bit_data.bit0 = edrk.errors.e6.bits.UO6_KEYS; -// reply_a->digit_data.byte21.bit_data.bit1 = edrk.errors.e6.bits.UO7_KEYS; -// reply_a->digit_data.byte21.bit_data.bit2 = edrk.errors.e6.bits.UO1_KEYS; -// reply_a->digit_data.byte21.bit_data.bit3 = edrk.errors.e6.bits.ERR_PBUS; -// reply_a->digit_data.byte21.bit_data.bit4 = edrk.errors.e6.bits.ERR_SBUS; -// reply_a->digit_data.byte21.bit_data.bit5 = edrk.errors.e6.bits.ER_DISBAL_BATT; -// reply_a->digit_data.byte21.bit_data.bit6 = edrk.errors.e6.bits.ER_RAZBALANS_ALG; -// reply_a->digit_data.byte21.bit_data.bit7 = edrk.errors.e6.bits.RASCEPITEL_ERROR_NOT_ANSWER; - - - reply_a->digit_data.byte23.byte_data = edrk.errors.e7.all & 0xff; - reply_a->digit_data.byte24.byte_data = (edrk.errors.e7.all >> 8) & 0xff; - - - - // hwp - reply_a->digit_data.byte25.byte_data = project.hwp[0].read.comp_s.plus.all & 0xff; - reply_a->digit_data.byte26.byte_data = (project.hwp[0].read.comp_s.plus.all >> 8) & 0xff; - - reply_a->digit_data.byte27.byte_data = project.hwp[0].read.comp_s.minus.all & 0xff; - reply_a->digit_data.byte28.byte_data = (project.hwp[0].read.comp_s.minus.all >> 8) & 0xff; - - - reply_a->digit_data.byte29.bit_data.bit0 = control_station.active_control_station[CONTROL_STATION_TERMINAL_RS232]; - reply_a->digit_data.byte29.bit_data.bit1 = control_station.active_control_station[CONTROL_STATION_TERMINAL_CAN]; - reply_a->digit_data.byte29.bit_data.bit2 = control_station.active_control_station[CONTROL_STATION_INGETEAM_PULT_RS485]; - reply_a->digit_data.byte29.bit_data.bit3 = control_station.active_control_station[CONTROL_STATION_MPU_SVU_CAN]; - reply_a->digit_data.byte29.bit_data.bit4 = control_station.active_control_station[CONTROL_STATION_MPU_KEY_CAN]; - reply_a->digit_data.byte29.bit_data.bit5 = control_station.active_control_station[CONTROL_STATION_MPU_SVU_RS485]; - reply_a->digit_data.byte29.bit_data.bit6 = control_station.active_control_station[CONTROL_STATION_MPU_KEY_RS485]; - reply_a->digit_data.byte29.bit_data.bit7 = control_station.active_control_station[CONTROL_STATION_ZADATCHIK_CAN]; - - reply_a->digit_data.byte30.bit_data.bit0 = control_station.alive_control_station[CONTROL_STATION_TERMINAL_RS232]; - reply_a->digit_data.byte30.bit_data.bit1 = control_station.alive_control_station[CONTROL_STATION_TERMINAL_CAN]; - reply_a->digit_data.byte30.bit_data.bit2 = control_station.alive_control_station[CONTROL_STATION_INGETEAM_PULT_RS485]; - reply_a->digit_data.byte30.bit_data.bit3 = control_station.alive_control_station[CONTROL_STATION_MPU_SVU_CAN]; - reply_a->digit_data.byte30.bit_data.bit4 = control_station.alive_control_station[CONTROL_STATION_MPU_KEY_CAN]; - reply_a->digit_data.byte30.bit_data.bit5 = control_station.alive_control_station[CONTROL_STATION_MPU_SVU_RS485]; - reply_a->digit_data.byte30.bit_data.bit6 = control_station.alive_control_station[CONTROL_STATION_MPU_KEY_RS485]; - reply_a->digit_data.byte30.bit_data.bit7 = control_station.alive_control_station[CONTROL_STATION_ZADATCHIK_CAN]; - - - reply_a->digit_data.byte31.byte_data = optical_read_data.data.cmd.all & 0xff; - reply_a->digit_data.byte32.byte_data = (optical_read_data.data.cmd.all >> 8) & 0xff; - - reply_a->digit_data.byte33.byte_data = optical_write_data.data.cmd.all & 0xff; - reply_a->digit_data.byte34.byte_data = (optical_write_data.data.cmd.all >> 8) & 0xff; - - reply_a->digit_data.byte35.bit_data.bit0 = control_station.alive_control_station[CONTROL_STATION_VPU_CAN]; - reply_a->digit_data.byte35.bit_data.bit1 = control_station.active_control_station[CONTROL_STATION_VPU_CAN]; - - reply_a->digit_data.byte35.bit_data.bit2 = edrk.auto_master_slave.local.bits.master; - reply_a->digit_data.byte35.bit_data.bit3 = edrk.auto_master_slave.local.bits.slave; - reply_a->digit_data.byte35.bit_data.bit4 = edrk.auto_master_slave.local.bits.try_master; - - reply_a->digit_data.byte35.bit_data.bit5 = edrk.auto_master_slave.remoute.bits.master; - reply_a->digit_data.byte35.bit_data.bit6 = edrk.auto_master_slave.remoute.bits.slave; - reply_a->digit_data.byte35.bit_data.bit7 = edrk.auto_master_slave.remoute.bits.try_master; - - reply_a->digit_data.byte36.bit_data.bit0 = edrk.Status_Ready.bits.MasterSlaveActive; - reply_a->digit_data.byte36.bit_data.bit1 = edrk.ms.ready1; - reply_a->digit_data.byte36.bit_data.bit2 = edrk.ms.ready2; - reply_a->digit_data.byte36.bit_data.bit3 = edrk.flag_wait_both_ready2; - reply_a->digit_data.byte36.bit_data.bit4 = edrk.Ready1_another_bs; - reply_a->digit_data.byte36.bit_data.bit5 = edrk.Ready2_another_bs; - reply_a->digit_data.byte36.bit_data.bit6 = edrk.flag_another_bs_first_ready12; - reply_a->digit_data.byte36.bit_data.bit7 = edrk.flag_this_bs_first_ready12; - - - - - reply_a->digit_data.byte37.byte_data = edrk.errors.e8.all & 0xff; - reply_a->digit_data.byte38.byte_data = (edrk.errors.e8.all >> 8) & 0xff; - - reply_a->digit_data.byte39.bit_data.bit0 = edrk.RazborNotFinish; - reply_a->digit_data.byte39.bit_data.bit1 = edrk.RunZahvatRascepitel; - reply_a->digit_data.byte39.bit_data.bit2 = edrk.RunUnZahvatRascepitel; - reply_a->digit_data.byte39.bit_data.bit3 = edrk.Run_Rascepitel; - reply_a->digit_data.byte39.bit_data.bit4 = edrk.ms.ready3; - - reply_a->digit_data.byte39.bit_data.bit5 = edrk.StartGEDfromZadanie; - reply_a->digit_data.byte39.bit_data.bit6 = edrk.flag_wait_set_to_zero_zadanie; - reply_a->digit_data.byte39.bit_data.bit7 = edrk.flag_block_zadanie; - reply_a->digit_data.byte40.bit_data.bit0 = edrk.you_can_on_rascepitel; - reply_a->digit_data.byte40.bit_data.bit1 = edrk.StartGEDfromControl; - reply_a->digit_data.byte40.bit_data.bit2 = edrk.StartGED; - reply_a->digit_data.byte40.bit_data.bit3 = edrk.GoWait; - - reply_a->digit_data.byte40.bit_data.bit4 = edrk.stop_logs_rs232; - reply_a->digit_data.byte40.bit_data.bit5 = edrk.stop_slow_log; - - reply_a->digit_data.byte40.bit_data.bit6 = edrk.disable_limit_power_from_svu; - reply_a->digit_data.byte40.bit_data.bit7 = edrk.disable_uom; - - reply_a->digit_data.byte41.byte_data = edrk.errors.e9.all & 0xff; - reply_a->digit_data.byte42.byte_data = (edrk.errors.e9.all >> 8) & 0xff; - - reply_a->digit_data.byte43.byte_data = edrk.errors.e10.all & 0xff; - reply_a->digit_data.byte44.byte_data = (edrk.errors.e10.all >> 8) & 0xff; - - reply_a->digit_data.byte45.byte_data = edrk.errors.e11.all & 0xff; - reply_a->digit_data.byte46.byte_data = (edrk.errors.e11.all >> 8) & 0xff; - - reply_a->digit_data.byte47.byte_data = edrk.errors.e12.all & 0xff; - reply_a->digit_data.byte48.byte_data = (edrk.errors.e12.all >> 8) & 0xff; - - -// reply_a->digit_data.byte49.byte_data = 0; -// reply_a->digit_data.byte50.byte_data = 0; - // reply_a->digit_data.byte49.byte_data = 0; - - // reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.ALL_KNOPKA_AVARIA; - // reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.BLOCK_IZOL_AVARIA; - // reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.BLOCK_IZOL_NORMA; - reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.LOCAL_REMOUTE; - // reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.NAGREV_ON = !FROM_ING_NAGREV_ON; - // reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.NASOS_NORMA = !FROM_ING_NASOS_NORMA; - // reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.NASOS_ON = !FROM_ING_NASOS_ON; - // reply_a->digit_data.byte49.bit_data.bit0 = edrk.from_ing1.bits.OHLAD_UTE4KA_WATER = !FROM_ING_OHLAD_UTE4KA_WATER; - // edrk.from_ing1.bits.UPC_24V_NORMA = !FROM_ING_UPC_24V_NORMA; - //edrk.from_ing1.bits.OP_PIT_NORMA = !FROM_ING_OP_PIT_NORMA; - //edrk.from_ing1.bits.VENTIL_ON = !FROM_ING_VENTIL_ON; - // edrk.from_ing1.bits.VIPR_PREDOHR_NORMA = !FROM_ING_VIPR_PREDOHR_NORMA; - - // edrk.from_ing1.bits.ZARYAD_ON = !FROM_ING_ZARYAD_ON; - // edrk.from_ing1.bits.ZAZEML_OFF = !FROM_ING_ZAZEML_OFF; - // edrk.from_ing1.bits.ZAZEML_ON = !FROM_ING_ZAZEML_ON; - - reply_a->digit_data.byte49.bit_data.bit1 = edrk.from_ing2.bits.KEY_MINUS; - reply_a->digit_data.byte49.bit_data.bit2 = edrk.from_ing2.bits.KEY_PLUS; - reply_a->digit_data.byte49.bit_data.bit3 = edrk.from_ing2.bits.KEY_KVITIR; - - reply_a->digit_data.byte49.bit_data.bit4 = edrk.from_ing2.bits.KEY_SBOR; - reply_a->digit_data.byte49.bit_data.bit5 = edrk.from_ing2.bits.KEY_RAZBOR; - - // edrk.from_ing1.bits.RASCEPITEL_ON = FROM_ING_RASCEPITEL_ON_OFF; - - reply_a->digit_data.byte49.bit_data.bit6 = edrk.from_ing2.bits.SOST_ZAMKA; - reply_a->digit_data.byte49.bit_data.bit7 = edrk.from_shema_filter.bits.RAZBOR_SHEMA; - // - reply_a->digit_data.byte50.bit_data.bit0 = edrk.from_shema_filter.bits.SBOR_SHEMA; - - reply_a->digit_data.byte50.bit_data.bit1 = edrk.from_shema_filter.bits.ZADA_DISPLAY; - reply_a->digit_data.byte50.bit_data.bit2 = edrk.from_shema_filter.bits.SVU; - // edrk.from_shema.bits.KNOPKA_AVARIA = FROM_ALL_KNOPKA_AVARIA; - reply_a->digit_data.byte50.bit_data.bit3 = edrk.from_shema.bits.QTV_ON_OFF; - reply_a->digit_data.byte50.bit_data.bit4 = edrk.from_shema_filter.bits.UMP_ON_OFF; - reply_a->digit_data.byte50.bit_data.bit5 = edrk.from_shema_filter.bits.READY_UMP; - reply_a->digit_data.byte50.bit_data.bit6 = edrk.from_shema.bits.SVU_BLOCK_QTV; - reply_a->digit_data.byte50.bit_data.bit7 = edrk.errors_another_bs_from_can; - - - // reply_a->digit_data.byte44.byte_data = 0; - - - reply_a->digit_data.byte51.bit_data.bit0 = inc_sensor.break_sensor1; - reply_a->digit_data.byte51.bit_data.bit1 = inc_sensor.break_sensor2; - reply_a->digit_data.byte51.bit_data.bit2 = pll1.output.flag_find_pll; - reply_a->digit_data.byte51.bit_data.bit3 = log_params.stop_log_slow; - reply_a->digit_data.byte51.bit_data.bit4 = log_params.stop_log_level_1; - reply_a->digit_data.byte51.bit_data.bit5 = log_params.stop_log_level_2; - reply_a->digit_data.byte51.bit_data.bit6 = log_params.stop_log_slow_level_1; - reply_a->digit_data.byte51.bit_data.bit7 = log_params.stop_log_slow_level_2; - - - - reply_a->digit_data.byte52.bit_data.bit0 = edrk.from_zadat4ik.bits.KVITIR; - reply_a->digit_data.byte52.bit_data.bit1 = edrk.from_zadat4ik.bits.PLUS; - reply_a->digit_data.byte52.bit_data.bit2 = edrk.from_zadat4ik.bits.MINUS; - reply_a->digit_data.byte52.bit_data.bit3 = edrk.from_zadat4ik.bits.PROVOROT; - reply_a->digit_data.byte52.bit_data.bit4 = edrk.from_zadat4ik.bits.UOM_READY_ACTIVE; - reply_a->digit_data.byte52.bit_data.bit5 = edrk.from_zadat4ik.bits.UOM_LIMIT_3; - reply_a->digit_data.byte52.bit_data.bit6 = edrk.from_zadat4ik.bits.UOM_LIMIT_2; - reply_a->digit_data.byte52.bit_data.bit7 = edrk.from_zadat4ik.bits.UOM_LIMIT_1; - - - - reply_a->digit_data.byte53.bit_data.bit0 = edrk.Run_UMP; - reply_a->digit_data.byte53.bit_data.bit1 = edrk.Status_UMP_Ok; - reply_a->digit_data.byte53.bit_data.bit2 = edrk.Zaryad_UMP_Ok; - reply_a->digit_data.byte53.bit_data.bit3 = edrk.cmd_to_ump; - reply_a->digit_data.byte53.bit_data.bit4 = edrk.sbor_wait_ump1; - reply_a->digit_data.byte53.bit_data.bit5 = edrk.sbor_wait_ump2; - reply_a->digit_data.byte53.bit_data.bit6 = edrk.flag_enable_on_ump; - - reply_a->digit_data.byte53.bit_data.bit7 = edrk.local_ump_on_off; - reply_a->digit_data.byte54.bit_data.bit0 = edrk.local_ready_ump; - -/// - reply_a->digit_data.byte54.bit_data.bit1 = (modbus_table_can_in[128].all) ? 1 : 0; //cmd_local_charge PCH 0 - reply_a->digit_data.byte54.bit_data.bit2 = (modbus_table_can_in[131].all) ? 1 : 0; //cmd_local_uncharge PCH 0 - - reply_a->digit_data.byte54.bit_data.bit3 = (modbus_table_can_in[129].all) ? 1 : 0; //cmd_local_charge PCH 1 - reply_a->digit_data.byte54.bit_data.bit4 = (modbus_table_can_in[132].all) ? 1 : 0; //cmd_local_uncharge PCH 1 - - reply_a->digit_data.byte54.bit_data.bit5 = edrk.from_shema_filter.bits.UMP_ON_OFF; - reply_a->digit_data.byte54.bit_data.bit6 = edrk.SumSbor; - - reply_a->digit_data.byte55.bit_data.bit0 = edrk.power_limit.bits.limit_Iout; - reply_a->digit_data.byte55.bit_data.bit1 = edrk.power_limit.bits.limit_UOM; - reply_a->digit_data.byte55.bit_data.bit2 = edrk.power_limit.bits.limit_by_temper; - reply_a->digit_data.byte55.bit_data.bit3 = edrk.power_limit.bits.limit_from_SVU; - reply_a->digit_data.byte55.bit_data.bit4 = edrk.power_limit.bits.limit_from_uom_fast; - reply_a->digit_data.byte55.bit_data.bit5 = edrk.power_limit.bits.limit_from_freq; - reply_a->digit_data.byte55.bit_data.bit6 = edrk.power_limit.bits.limit_moment; - reply_a->digit_data.byte55.bit_data.bit7 = simple_scalar1.flag_decr_mzz_power; - - - reply_a->digit_data.byte56.bit_data.bit0 = (edrk.pult_cmd.log_what_memory & 0x1) ? 1 : 0; - reply_a->digit_data.byte56.bit_data.bit1 = (edrk.pult_cmd.log_what_memory & 0x2) ? 1 : 0; - - reply_a->digit_data.byte56.bit_data.bit2 = edrk.pult_data.flagSaveDataMoto ? 1 : 0; - - reply_a->digit_data.byte56.bit_data.bit3 = (edrk.pult_data.flagSaveSlowLogs) ? 1 : 0; - reply_a->digit_data.byte56.bit_data.bit4 = edrk.pult_cmd.send_log ? 1 : 0; - - reply_a->digit_data.byte56.bit_data.bit5 = (log_to_HMI.send_log==1) ? 1 : 0; - reply_a->digit_data.byte56.bit_data.bit6 = (log_to_HMI.send_log==2) ? 1 : 0; - reply_a->digit_data.byte56.bit_data.bit7 = (log_to_HMI.send_log==3) ? 1 : 0; - -// - reply_a->digit_data.byte57.bit_data.bit0 = (edrk.break_tempers[0] > ABNORMAL_TEMPER_BREAK_INT) ? 1 : 0; - reply_a->digit_data.byte57.bit_data.bit1 = (edrk.break_tempers[1] > ABNORMAL_TEMPER_BREAK_INT) ? 1 : 0; - reply_a->digit_data.byte57.bit_data.bit2 = (edrk.break_tempers[2] > ABNORMAL_TEMPER_BREAK_INT) ? 1 : 0; - reply_a->digit_data.byte57.bit_data.bit3 = (edrk.break_tempers[3] > ABNORMAL_TEMPER_BREAK_INT) ? 1 : 0; - - reply_a->digit_data.byte57.bit_data.bit4 = (edrk.break_tempers[0] > ALARM_TEMPER_BREAK_INT) ? 1 : 0; - reply_a->digit_data.byte57.bit_data.bit5 = (edrk.break_tempers[1] > ALARM_TEMPER_BREAK_INT) ? 1 : 0; - reply_a->digit_data.byte57.bit_data.bit6 = (edrk.break_tempers[2] > ALARM_TEMPER_BREAK_INT) ? 1 : 0; - reply_a->digit_data.byte57.bit_data.bit7 = (edrk.break_tempers[3] > ALARM_TEMPER_BREAK_INT) ? 1 : 0; - - reply_a->digit_data.byte58.bit_data.bit0 = (edrk.breaker_on==1) ? 1 : 0; - - reply_a->digit_data.byte58.bit_data.bit1 = edrk.warnings.e9.bits.BREAK_TEMPER_WARNING; - reply_a->digit_data.byte58.bit_data.bit2 = edrk.warnings.e9.bits.BREAK_TEMPER_ALARM; - reply_a->digit_data.byte58.bit_data.bit3 = edrk.warnings.e9.bits.BREAKER_GED_ON; - - - -// reply_a->digit_data.byte57.byte_data = 0;//(((unsigned long)edrk.canes_reg>>16) & 0x0ff); -// reply_a->digit_data.byte58.byte_data = 0;//(((unsigned long)edrk.canes_reg) & 0x3f); - reply_a->digit_data.byte59.byte_data = 0;//(((unsigned long)edrk.canes_reg>>24) & 0x1); - reply_a->digit_data.byte60.byte_data = 0; - - - - - return; -} - - diff --git a/Inu/Src2/551/main/message2.h b/Inu/Src2/551/main/message2.h deleted file mode 100644 index 0f93c42..0000000 --- a/Inu/Src2/551/main/message2.h +++ /dev/null @@ -1,33 +0,0 @@ -//////////////////////////////////////////// -// message.h -// -// : -// 1. y -// 2. -// 3. y -// -// -// y y y -// . INTEL 386SX Octagon -// TMS320C32 Texas Instruments. -// -// . -// y -// unsigned char = 8 -// TMS320C32 unsigned char = 32 , y -// 8 . -// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -// -// y -// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -//////////////////////////////////////////// - -#ifndef MESSAGE_H -#define MESSAGE_H - -#include "RS_Function_terminal.h" - -//void func_fill_answer_to_TMS(TMS_TO_TERMINAL_STRUCT* reply_ans, CMD_TO_TMS_STRUCT* pcommand); -void func_pack_answer_to_TMS(TMS_TO_TERMINAL_STRUCT* reply_a); -void func_unpack_answer_from_TMS_RS232(CMD_TO_TMS_STRUCT* pcommand); -#endif //MESSAGE_H diff --git a/Inu/Src2/551/main/message2can.c b/Inu/Src2/551/main/message2can.c deleted file mode 100644 index 5b0a883..0000000 --- a/Inu/Src2/551/main/message2can.c +++ /dev/null @@ -1,278 +0,0 @@ -/* - * message2can.c - * - * Created on: 3 . 2020 . - * Author: Yura - */ -#include -#include -#include -#include -#include -#include -#include - -#include "control_station.h" -#include "CAN_Setup.h" -#include "global_time.h" -#include "IQmathLib.h" -#include "DSP281x_Device.h" -#include "x_basic_types.h" -#include "xp_cds_in.h" -#include "xp_hwp.h" -#include "xp_project.h" - - -void detecting_cmd_from_can(void) -{ - - if (control_station.alive_control_station[CONTROL_STATION_TERMINAL_CAN]) - { - func_unpack_answer_from_TMS_CAN((TERMINAL_UNITES_STRUCT_handle)&TerminalUnites[edrk.number_can_box_terminal_cmd][0]); - } - -} - - -void func_unpack_answer_from_TMS_CAN(TERMINAL_UNITES_STRUCT_handle unites_t) -{ - // y - unsigned int DataOut,h; - int Data, Data1, Data2, DataAnalog1, DataAnalog2, DataAnalog3, DataAnalog4,DataAnalog5,DataAnalog6, i; - unsigned char *pByte; - // static int vs11,vs12,vs1; - // static int DataCnt=0; - // int GoT,Assemble_scheme; - // static int prev_temp_Rele1=0, temp_Rele1=0, prev_temp_Rele2=0, temp_Rele2=0; - - static int flag_prev_turn_on = 0; - static int flag_prev_turn_off = 0; - static int prev_byte01_bit4 = 0; - static int prev_byte01_bit1 = 0; - static int flag_wait_revers_sbor = 1; - static int flag_wait_revers_go = 1; - - static unsigned int count_transmited = 0; - - - // - - if (TERMINAL_UNIT_LEN>CONTROL_STATION_MAX_RAW_DATA) - xerror(main_er_ID(2),(void *)0); - - - - // , .. RS232 - // pByte = (unsigned char *)(pcommand);//->analog_data.analog1_lo; - - for (h=0;hbuf[h].all; - } - - /* - - if (control_station.alive_control_station[CONTROL_STATION_TERMINAL_CAN]==0) - { - edrk.from_can.bits.ACTIVE = 0; - return; - } - - if ((control_station.active_control_station[CONTROL_STATION_TERMINAL_RS232]==1)) - { - edrk.from_can.bits.ACTIVE = 0; - return; - } - - // unites_t->buf[0].bits.bit0 - - edrk.test_mode = 0; - - edrk.from_can.bits.ACTIVE = unites_t->buf[6].bits.bit3; - - if (edrk.from_can.bits.ACTIVE==0) - return; - -// f.RScount = SECOND * 3; - - // unites_t-> -// StartGED - - if (edrk.summ_errors) - { - flag_wait_revers_go = 1; - } - - if (flag_wait_revers_go==1) - edrk.StartGEDRS = 0; - if (unites_t->buf[6].bits.bit0 && flag_wait_revers_go) - edrk.StartGEDRS = 0; - if (unites_t->buf[6].bits.bit0==0) - edrk.StartGEDRS = 0; - if (unites_t->buf[6].bits.bit0==0 && flag_wait_revers_go) - flag_wait_revers_go = 0; - if (unites_t->buf[6].bits.bit0==1 && flag_wait_revers_go==0) - edrk.StartGEDRS = 1; - -// edrk.StartGEDRS = pcommand->digit_data.Byte01.bit_data.bit0; - -// end StartGED - - - edrk.Mode_UFConst = unites_t->buf[6].bits.bit2; - - -//////////////// - - if (unites_t->buf[6].bits.bit1 && prev_byte01_bit1==0) - edrk.KvitirRS = 1; - - prev_byte01_bit1 = unites_t->buf[6].bits.bit1; - - -// edrk.from_rs.bits.RAZBOR_SHEMA = pcommand->digit_data.Byte01.bit_data.bit5; - - - -// SBOR SHEMA - if (edrk.summ_errors) - { - flag_wait_revers_sbor = 1; - } - - if (flag_wait_revers_sbor==1) - edrk.from_can.bits.SBOR_SHEMA = 0; - - if (unites_t->buf[6].bits.bit4 && flag_wait_revers_sbor) - edrk.from_can.bits.SBOR_SHEMA = 0; - - if (unites_t->buf[6].bits.bit4==0) - edrk.from_can.bits.SBOR_SHEMA = 0; - - if (unites_t->buf[6].bits.bit4==0 && flag_wait_revers_sbor) - flag_wait_revers_sbor = 0; - - if (unites_t->buf[6].bits.bit4==1 && flag_wait_revers_sbor==0) - edrk.from_can.bits.SBOR_SHEMA = unites_t->buf[6].bits.bit4; - - prev_byte01_bit4 = unites_t->buf[6].bits.bit4; - -// end SBOR SHEMA - - - -// if (edrk.from_rs.bits.RAZBOR_SHEMA) - // edrk.from_rs.bits.SBOR_SHEMA = 0; - - //edrk.SborRS = pcommand->digit_data.Byte01.bit_data.bit4; - - - edrk.SelectPump0_1 = unites_t->buf[6].bits.bit6; - edrk.DirectOUT = unites_t->buf[6].bits.bit7; - - edrk.DirectNagrevOff = unites_t->buf[6].bits.bit8; - edrk.DirectBlockKeyOff = unites_t->buf[6].bits.bit9; - edrk.DirectPumpON = unites_t->buf[6].bits.bit10; - edrk.DirectZaryadOn = unites_t->buf[6].bits.bit11; - -#ifdef STENDD - - // edrk.to_shema.bits.QTV_ON = pcommand->digit_data.Byte02.bit_data.bit3; - -#endif - - edrk.Status_Ready.bits.ImitationReady2 = unites_t->buf[6].bits.bit12; - - edrk.SetSpeed = unites_t->buf[6].bits.bit13; - - - -// edrk.RemouteFromRS = pcommand->digit_data.Byte01.bit_data.bit3; - - - - -// edrk.VozbudOnOffFromRS = pcommand->digit_data.Byte01.bit_data.bit1; -// edrk.enable_set_vozbud = pcommand->digit_data.Byte01.bit_data.bit1; -// edrk.SborRS = pcommand->digit_data.Byte01.bit_data.bit2; -// edrk.RazborRS = pcommand->digit_data.Byte01.bit_data.bit3; -// edrk.DirectOUT = pcommand->digit_data.Byte01.bit_data.bit4; - -// edrk.StartGED = pcommand->digit_data.Byte01.bit_data.bit6; - - -// f.flag_distance = pcommand->digit_data.Byte01.bit_data.bit6; -// f.Set_power = pcommand->digit_data.Byte01.bit_data.bit7; - - f.Obmotka1 = unites_t->buf[6].bits.bit15; - f.Obmotka2 = unites_t->buf[7].bits.bit0; - - edrk.disable_alg_u_disbalance = unites_t->buf[7].bits.bit1; - - // f.Down50 = pcommand->digit_data.Byte02.bit_data.bit2; -// f.Up50 = pcommand->digit_data.Byte02.bit_data.bit3; -// f.Ciclelog = pcommand->digit_data.Byte02.bit_data.bit4; - - // if (SPEED_SELECT_ZADAT==1) -// f.Provorot = pcommand->digit_data.Byte02.bit_data.bit5; - - -// Data1 = pcommand->analog_data.analog1_hi; -// Data2 = pcommand->analog_data.analog1_lo; -// Data = (Data2 + Data1 * 256); -// if (Data > 32767) -// Data = Data - 65536; - DataAnalog1 = unites_t->buf[0].all; - -// Data1 = pcommand->analog_data.analog2_hi; -// Data2 = pcommand->analog_data.analog2_lo; -// Data = (Data2 + Data1 * 256); -// if (Data > 32767) -// Data = Data - 65536; - DataAnalog2 = unites_t->buf[1].all; - -// Data1 = pcommand->analog_data.analog3_hi; -// Data2 = pcommand->analog_data.analog3_lo; -// Data = (Data2 + Data1 * 256); -// if (Data > 32767) -// Data = Data - 65536; - DataAnalog3 = unites_t->buf[2].all; - -// Data1 = pcommand->analog_data.analog4_hi; -// Data2 = pcommand->analog_data.analog4_lo; -// Data = (Data2 + Data1 * 256); -// if (Data > 32767) -// Data = Data - 65536; - DataAnalog4 = unites_t->buf[3].all; - DataAnalog5 = unites_t->buf[4].all; - DataAnalog6 = unites_t->buf[5].all; - - - edrk.W_from_RS = DataAnalog1; - edrk.I_zad_vozb_add_from_RS = 0;//DataAnalog4; - - - if (!edrk.SetSpeed) - { - if (DataAnalog3<0) - edrk.ZadanieU_Charge_RS = 0; - else - edrk.ZadanieU_Charge_RS = DataAnalog3; - } - - if (edrk.SetSpeed) - { - edrk.fzad = DataAnalog1/100.0; - edrk.kzad = DataAnalog2/10000.0; - edrk.k_u_disbalance = _IQ(DataAnalog4/100.0); - edrk.kplus_u_disbalance = _IQ(DataAnalog3/1000.0); - - } -*/ - return; -} - - - - - diff --git a/Inu/Src2/551/main/message2can.h b/Inu/Src2/551/main/message2can.h deleted file mode 100644 index 14ad7da..0000000 --- a/Inu/Src2/551/main/message2can.h +++ /dev/null @@ -1,18 +0,0 @@ -/* - * message2can.h - * - * Created on: 3 . 2020 . - * Author: Yura - */ - -#ifndef SRC_MAIN_MESSAGE2CAN_H_ -#define SRC_MAIN_MESSAGE2CAN_H_ - -#include "CAN_Setup.h" - -void func_unpack_answer_from_TMS_CAN(TERMINAL_UNITES_STRUCT_handle); -void detecting_cmd_from_can(void); - - - -#endif /* SRC_MAIN_MESSAGE2CAN_H_ */ diff --git a/Inu/Src2/551/main/message2test.c b/Inu/Src2/551/main/message2test.c deleted file mode 100644 index fbcff9e..0000000 --- a/Inu/Src2/551/main/message2test.c +++ /dev/null @@ -1,678 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "CAN_Setup.h" -#include "IQmathLib.h" -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "RS_Functions.h" -#include "xp_project.h" - -#include "x_wdog.h" -#include "params_hwp.h" -#include "detect_errors.h" - - -//XilinxV2 - - -void func_fill_answer_to_TMS_test(TMS_TO_TERMINAL_TEST_ALL_STRUCT* reply_ans, CMD_TO_TMS_TEST_ALL_STRUCT* pcommand) -{ - // y - unsigned int crc, DataOut, sinusImpulse, doubleImpulse,adc_plate; - int Data,Data1,Data2/*,bitt, DataAnalog1, DataAnalog2*/, tk0,tk1,tk2,tk3,period1,period2, period3; - - //static int vs11,vs12,vs1; - static int prev_Go = 0; - static int prev_Prepare = 0; - - static int flag_prev_turn_on = 0; - static int flag_prev_turn_off = 0; - static int flag_prev_lamp_on_off = 0; - static int soft_off_enable = 0, soft_on_enable = 0; - static float tk_time_soft_off = 0; - static unsigned long tk_time_soft_off_d = 0; - static unsigned int i_af_protect_a = 0, i_af_protect_d = 0; - int enable_line_err = 0, disable_tk_soft_off, disable_protect_tk_soft_off; - - - stop_wdog(); - - edrk.test_mode = 1; - // const - // - -// TMS_TO_TERMINAL_TEST_ALL_STRUCT* const pcommand = ((TMS_TO_TERMINAL_TEST_ALL_STRUCT*)reply); - - // - y - // ? - -// f.RScount = SECOND*3; - - f.terminal_prepare = pcommand->digit_data.byte05.bit_data.bit1; - soft_off_enable = pcommand->digit_data.byte06.bit_data.bit0; - soft_on_enable = pcommand->digit_data.byte06.bit_data.bit1; -// edrk.direct_write_out = pcommand->digit_data.byte06.bit_data.bit2; - - disable_tk_soft_off = pcommand->digit_data.byte06.bit_data.bit3; - disable_protect_tk_soft_off = pcommand->digit_data.byte06.bit_data.bit4; - - enable_line_err = pcommand->digit_data.byte06.bit_data.bit5; - - // - -#if (CHECK_IN_OUT_TERMINAL==1) - -#if(C_cds_out_number>=1) - project.cds_out[0].write.sbus.data_out.all = ~(pcommand->digit_data.byte07.byte_data | ((pcommand->digit_data.byte08.byte_data) << 8)); -#endif -#if(C_cds_out_number>=2) - project.cds_out[1].write.sbus.data_out.all = ~(pcommand->digit_data.byte09.byte_data | ((pcommand->digit_data.byte10.byte_data) << 8)); -#endif -#if(C_cds_out_number>=3) - project.cds_out[2].write.sbus.data_out.all = ~(pcommand->digit_data.byte11.byte_data | ((pcommand->digit_data.byte12.byte_data) << 8)); -#endif - -#endif //CHECK_IN_OUT - - if (pcommand->digit_data.byte05.bit_data.bit1 == 1) - { - - //xreset_error_all(); - } - - - -// write_dig_out(); - - //calc_norm_ADC(0); - calc_norm_ADC_0(1); - calc_norm_ADC_1(1); - - // - tk0 = (pcommand->digit_data.byte01.byte_data); - tk1 = (pcommand->digit_data.byte02.byte_data); - tk2 = (pcommand->digit_data.byte03.byte_data); - tk3 = (pcommand->digit_data.byte04.byte_data); - - Data1 = pcommand->analog_data.analog1_hi; - Data2 = pcommand->analog_data.analog1_lo; - Data = (Data2 + Data1*256); - period1 = Data; - - Data1 = pcommand->analog_data.analog2_hi; - Data2 = pcommand->analog_data.analog2_lo; - Data = (Data2 + Data1*256); - period2 = Data; - - Data1 = pcommand->analog_data.analog3_hi; - Data2 = pcommand->analog_data.analog3_lo; - Data = (Data2 + Data1*256); - period3 = Data; - - Data1 = pcommand->analog_data.analog4_hi; - Data2 = pcommand->analog_data.analog4_lo; - Data = (Data2 + Data1*256); - // Data = 200; - tk_time_soft_off = Data*100.0; // mks*10->ns - if (tk_time_soft_off>1300000.0) - tk_time_soft_off = 1300000.0; - tk_time_soft_off_d = (unsigned long)(tk_time_soft_off / DIV_TIME_TK_SOFT_OFF); - - if (tk_time_soft_off_d>65535) - tk_time_soft_off_d = 65535; - - - Data1 = pcommand->analog_data.analog5_hi; - Data2 = pcommand->analog_data.analog5_lo; - Data = (Data2 + Data1*256); - i_af_protect_a = Data; - - if (i_af_protect_a>LEVEL_HWP_I_AF) i_af_protect_a = LEVEL_HWP_I_AF; - if (i_af_protect_a<10) i_af_protect_a = 10; - i_af_protect_d = convert_real_to_mv_hwp(4,i_af_protect_a); - if (i_af_protect_d>1500) i_af_protect_d = 1500; // max 1500 mV - - update_maz_level_i_af(0, i_af_protect_d); - project.read_all_hwp(); - - - - if(pcommand->digit_data.byte05.bit_data.bit3 == 1) - doubleImpulse = 1; - else - doubleImpulse = 0; - - if(pcommand->digit_data.byte05.bit_data.bit5 == 1) - sinusImpulse = 1; - else - sinusImpulse = 0; - - if ((pcommand->digit_data.byte05.bit_data.bit0 == 1) && (prev_Go == 0)) - { - if (pcommand->digit_data.byte05.bit_data.bit2 == 1) // - { - update_maz_level_i_af(0, 1500); - project.write_all_hwp(); - clear_errors(); - project.clear_errors_all_plates(); - update_maz_level_i_af(0, i_af_protect_d); - project.write_all_hwp(); - - } - - -// test_tk_ak_one_impulse( tk0, tk1, tk2, tk3, period1, period2); -#if (USE_TK_0) - project.cds_tk[0].write.sbus.protect_error.bit.enable_soft_disconnect = !disable_tk_soft_off; - project.cds_tk[0].write.sbus.protect_error.bit.detect_soft_disconnect = !disable_protect_tk_soft_off; - project.cds_tk[0].write.sbus.protect_error.bit.enable_line_err = enable_line_err; - project.cds_tk[0].write.sbus.time_after_err = tk_time_soft_off_d;//(int)(tk_time_soft_off / DIV_TIME_TK_SOFT_OFF); -#endif -#if (USE_TK_1) - project.cds_tk[1].write.sbus.protect_error.bit.enable_soft_disconnect = !disable_tk_soft_off; - project.cds_tk[1].write.sbus.protect_error.bit.detect_soft_disconnect = !disable_protect_tk_soft_off; - project.cds_tk[1].write.sbus.protect_error.bit.enable_line_err = enable_line_err; - project.cds_tk[1].write.sbus.time_after_err = tk_time_soft_off_d;//(int)(tk_time_soft_off / DIV_TIME_TK_SOFT_OFF); -#endif -#if (USE_TK_3) - project.cds_tk[3].write.sbus.protect_error.bit.enable_soft_disconnect = !disable_tk_soft_off; - project.cds_tk[3].write.sbus.protect_error.bit.detect_soft_disconnect = !disable_protect_tk_soft_off; - project.cds_tk[3].write.sbus.protect_error.bit.enable_line_err = enable_line_err; - project.cds_tk[3].write.sbus.time_after_err = tk_time_soft_off_d;//(int)(tk_time_soft_off / DIV_TIME_TK_SOFT_OFF); -#endif - - - project.write_all_sbus(); - project.write_all_hwp(); - - test_tk_ak_one_impulse( tk0, tk1, tk2, tk3, period1, period2, period3, doubleImpulse, sinusImpulse, soft_off_enable, soft_on_enable); - } - - if ((pcommand->digit_data.byte05.bit_data.bit0 == 1) && - (pcommand->digit_data.byte05.bit_data.bit3 == 1) && (prev_Go == 0)) - { -// test_tk_ak_sinus_period( tk0, tk1, tk2, tk3, period1, period2); - } - prev_Go = pcommand->digit_data.byte05.bit_data.bit0; - - f.Prepare = pcommand->digit_data.byte05.bit_data.bit1; - if (pcommand->digit_data.byte05.bit_data.bit1 != prev_Prepare) - { - if (pcommand->digit_data.byte05.bit_data.bit1==1) - { - stop_wdog(); - update_maz_level_i_af(0, 1500); - project.write_all_hwp(); - clear_errors(); - project.clear_errors_all_plates(); - update_maz_level_i_af(0, i_af_protect_d); - project.write_all_hwp(); - } - } - prev_Prepare = pcommand->digit_data.byte05.bit_data.bit1; - - if (pcommand->digit_data.byte05.bit_data.bit2 == 1) - { - - prev_Go = 0; // Go - } - -// break_all_on_off(pcommand->digit_data.byte05.bit_data.bit5); - -/* - if (pcommand->digit_data.byte05.bit_data.bit5 != flag_prev_turn_off) //turn off - { - if(pcommand->digit_data.byte05.bit_data.bit5 == 1) - { - project.cds_out[0].fpga.Write.Dout.bit.dout2 = 1; - project.cds_out[0].fpga.Write.Dout.bit.dout12 = 1; - cds_out_all(cds_out_WriteAll); - pause_1000(100000); - pause_1000(100000); - pause_1000(100000); - pause_1000(100000); - project.cds_out[0].fpga.Write.Dout.bit.dout2 = 0; - project.cds_out[0].fpga.Write.Dout.bit.dout12 = 0; - cds_out_all(cds_out_WriteAll); - //f.Ready2 = 0; - f.On_Power_QTV = 0; - edrk.Go = 0; - - } - - flag_prev_turn_off = pcommand->digit_data.byte05.bit_data.bit5; - cds_out_all(cds_out_WriteAll); - - } - if ((pcommand->digit_data.byte05.bit_data.bit6 != flag_prev_turn_on) && !f.Stop && -// ((filter.iqU_1_long > 11184810) || (filter.iqU_3_long > 11184810))) //turn_on - ((filter.iqU_1_long > 5590240) || (filter.iqU_3_long > 5590240))) - { - if(pcommand->digit_data.byte05.bit_data.bit6 == 1) - { - project.cds_out[0].fpga.Write.Dout.bit.dout7 = 0; - cds_out_all(cds_out_WriteAll); - pause_1000(100000); - pause_1000(100000); - pause_1000(100000); - pause_1000(100000); - pause_1000(100000); - project.cds_out[0].fpga.Write.Dout.bit.dout7 = 1; - cds_out_all(cds_out_WriteAll); - //f.Ready2 = 1; - f.On_Power_QTV = 1; - } - - flag_prev_turn_on = pcommand->digit_data.byte05.bit_data.bit6; - cds_out_all(cds_out_WriteAll); - - } - if(project.cds_in[1].fpga.input_new.ChanalsPtr.ChanalPtr[7].rd_status != flag_prev_lamp_on_off) //turnig on lamp when power is on - { - if(project.cds_in[1].fpga.input_new.ChanalsPtr.ChanalPtr[7].rd_status == 1) - { - project.cds_out[1].fpga.Write.Dout.bit.dout1 = 0; - cds_out_all(cds_out_WriteAll); - } - else - { - project.cds_out[1].fpga.Write.Dout.bit.dout1 = 1; - cds_out_all(cds_out_WriteAll); - } - flag_prev_lamp_on_off = project.cds_in[1].fpga.input_new.ChanalsPtr.ChanalPtr[7].rd_status; - } - - -*/ - -// run_break = pcommand->digit_data.byte05.bit_data.bit4; - - - if (pcommand->digit_data.byte05.bit_data.bit4 == 1) - { - adc_plate = 1; - - } - else - adc_plate = 0; - - if (pcommand->digit_data.byte05.bit_data.bit6 == 1) - i_sync_pin_on(); - else - i_sync_pin_off(); - - - - Data = project.adc[adc_plate].read.pbus.adc_value[0];//InternalADC[0];//0;//ADC_sf[0];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog1_lo=LOBYTE(Data); - reply_test_all.analog_data.analog1_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[1];// InternalADC[1];//0;//ADC_sf[1];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog2_lo=LOBYTE(Data); - reply_test_all.analog_data.analog2_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[2];// InternalADC[2];//0;//ADC_sf[2];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog3_lo=LOBYTE(Data); - reply_test_all.analog_data.analog3_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[3];// InternalADC[3];//0;//ADC_sf[3];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog4_lo=LOBYTE(Data); - reply_test_all.analog_data.analog4_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[4];// InternalADC[4];//0;//ADC_sf[4];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog5_lo=LOBYTE(Data); - reply_test_all.analog_data.analog5_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[5];// InternalADC[5];//0;//ADC_sf[5];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog6_lo=LOBYTE(Data); - reply_test_all.analog_data.analog6_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[6];// InternalADC[6];//0;//ADC_sf[6];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog7_lo=LOBYTE(Data); - reply_test_all.analog_data.analog7_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[7];//InternalADC[7];//0;//ADC_sf[7];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog8_lo=LOBYTE(Data); - reply_test_all.analog_data.analog8_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[8];//InternalADC[8];//0;//ADC_sf[8];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog9_lo=LOBYTE(Data); - reply_test_all.analog_data.analog9_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[9];//InternalADC[9];//0;//ADC_sf[9];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog10_lo=LOBYTE(Data); - reply_test_all.analog_data.analog10_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[10];//InternalADC[10];//0;//ADC_sf[10];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog11_lo=LOBYTE(Data); - reply_test_all.analog_data.analog11_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[11];//InternalADC[11];//0;//ADC_sf[11];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog12_lo=LOBYTE(Data); - reply_test_all.analog_data.analog12_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[12];//InternalADC[12];//0;//ADC_sf[12];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog13_lo=LOBYTE(Data); - reply_test_all.analog_data.analog13_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[13];//InternalADC[13];//0;//ADC_sf[13];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog14_lo=LOBYTE(Data); - reply_test_all.analog_data.analog14_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[14];//InternalADC[14];//0;//ADC_sf[14];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog15_lo=LOBYTE(Data); - reply_test_all.analog_data.analog15_hi=HIBYTE(Data); - - Data = project.adc[adc_plate].read.pbus.adc_value[15];//InternalADC[15];//0;//ADC_sf[15];//_IQtoF(analog.iqIa1_1_fir_n)*2000.0;// - reply_test_all.analog_data.analog16_lo=LOBYTE(Data); - reply_test_all.analog_data.analog16_hi=HIBYTE(Data); - - - Data = _IQtoF(analog.iqU_1) * NORMA_ACP; - reply_ans->analog_data.analog17_lo=LOBYTE(Data); - reply_ans->analog_data.analog17_hi=HIBYTE(Data); - - Data = _IQtoF(analog.iqU_2) * NORMA_ACP; - reply_ans->analog_data.analog18_lo=LOBYTE(Data); - reply_ans->analog_data.analog18_hi=HIBYTE(Data); - - Data = project.cds_tk[0].read.sbus.time_err_tk_all.bit.tk_3210; - reply_ans->analog_data.analog19_lo=LOBYTE(Data); - reply_ans->analog_data.analog19_hi=HIBYTE(Data); - - Data = project.cds_tk[0].read.sbus.time_err_tk_all.bit.tk_7654; - reply_ans->analog_data.analog20_lo=LOBYTE(Data); - reply_ans->analog_data.analog20_hi=HIBYTE(Data); - - Data = project.cds_tk[1].read.sbus.time_err_tk_all.bit.tk_3210; - reply_ans->analog_data.analog21_lo=LOBYTE(Data); - reply_ans->analog_data.analog21_hi=HIBYTE(Data); - - Data = project.cds_tk[1].read.sbus.time_err_tk_all.bit.tk_7654; - reply_ans->analog_data.analog22_lo=LOBYTE(Data); - reply_ans->analog_data.analog22_hi=HIBYTE(Data); - - Data = project.cds_tk[2].read.sbus.time_err_tk_all.bit.tk_3210; - reply_ans->analog_data.analog23_lo=LOBYTE(Data); - reply_ans->analog_data.analog23_hi=HIBYTE(Data); - - Data = project.cds_tk[2].read.sbus.time_err_tk_all.bit.tk_7654; - reply_ans->analog_data.analog24_lo=LOBYTE(Data); - reply_ans->analog_data.analog24_hi=HIBYTE(Data); - -// Data = project.cds_tk[3].read.sbus.time_err_tk_all.bit.tk_3210; -// reply_ans->analog_data.analog25_lo=LOBYTE(Data); -// reply_ans->analog_data.analog25_hi=HIBYTE(Data); -// -// Data = project.cds_tk[3].read.sbus.time_err_tk_all.bit.tk_7654; -// reply_ans->analog_data.analog26_lo=LOBYTE(Data); -// reply_ans->analog_data.analog26_hi=HIBYTE(Data); - -reply_ans->digit_data.byte01.byte_data = 0; -reply_ans->digit_data.byte02.byte_data = 0; -reply_ans->digit_data.byte03.byte_data = 0; -reply_ans->digit_data.byte04.byte_data = 0; -reply_ans->digit_data.byte05.byte_data = 0; -reply_ans->digit_data.byte06.byte_data = 0; -reply_ans->digit_data.byte07.byte_data = 0; -reply_ans->digit_data.byte08.byte_data = 0; -reply_ans->digit_data.byte09.byte_data = 0; -reply_ans->digit_data.byte10.byte_data = 0; -reply_ans->digit_data.byte11.byte_data = 0; -reply_ans->digit_data.byte12.byte_data = 0; -reply_ans->digit_data.byte13.byte_data = 0; -reply_ans->digit_data.byte14.byte_data = 0; -reply_ans->digit_data.byte15.byte_data = 0; -reply_ans->digit_data.byte16.byte_data = 0; -reply_ans->digit_data.byte17.byte_data = 0; -reply_ans->digit_data.byte18.byte_data = 0; -reply_ans->digit_data.byte19.byte_data = 0; -reply_ans->digit_data.byte20.byte_data = 0; -reply_ans->digit_data.byte21.byte_data = 0; -reply_ans->digit_data.byte22.byte_data = 0; -reply_ans->digit_data.byte23.byte_data = 0; -reply_ans->digit_data.byte24.byte_data = 0; - - - reply_ans->digit_data.byte01.bit_data.bit0 = project.cds_tk[0].read.sbus.current_status_error.bit.err0_local; - reply_ans->digit_data.byte01.bit_data.bit1 = project.cds_tk[1].read.sbus.current_status_error.bit.err0_local; - reply_ans->digit_data.byte01.bit_data.bit2 = project.cds_tk[2].read.sbus.current_status_error.bit.err0_local; - reply_ans->digit_data.byte01.bit_data.bit3 = project.cds_tk[3].read.sbus.current_status_error.bit.err0_local; - - reply_ans->digit_data.byte01.bit_data.bit4 = project.cds_tk[0].read.sbus.current_status_error.bit.err_power; - reply_ans->digit_data.byte01.bit_data.bit5 = project.cds_tk[1].read.sbus.current_status_error.bit.err_power; - reply_ans->digit_data.byte01.bit_data.bit6 = project.cds_tk[2].read.sbus.current_status_error.bit.err_power; - reply_ans->digit_data.byte01.bit_data.bit7 = project.cds_tk[3].read.sbus.current_status_error.bit.err_power; - - reply_ans->digit_data.byte02.bit_data.bit0 = project.cds_in[0].read.sbus.current_status_error.bit.err_power; - reply_ans->digit_data.byte02.bit_data.bit1 = project.cds_in[1].read.sbus.current_status_error.bit.err_power; -#if(C_cds_in_number>=3) - reply_ans->digit_data.byte02.bit_data.bit2 = project.cds_in[2].read.sbus.current_status_error.bit.err_power; -#endif - reply_ans->digit_data.byte02.bit_data.bit3 = project.cds_out[0].read.sbus.current_status_error.bit.err_power; - - reply_ans->digit_data.byte02.bit_data.bit4 = project.cds_out[1].read.sbus.current_status_error.bit.err_power; - reply_ans->digit_data.byte02.bit_data.bit5 = 0; - reply_ans->digit_data.byte02.bit_data.bit6 = project.cds_tk[0].read.sbus.current_status_error.bit.err_switch; - reply_ans->digit_data.byte02.bit_data.bit7 = project.cds_tk[1].read.sbus.current_status_error.bit.err_switch; - - reply_ans->digit_data.byte03.bit_data.bit0 = project.cds_tk[2].read.sbus.current_status_error.bit.err_switch; - reply_ans->digit_data.byte03.bit_data.bit1 = project.cds_tk[3].read.sbus.current_status_error.bit.err_switch; - reply_ans->digit_data.byte03.bit_data.bit2 = project.cds_in[0].read.sbus.current_status_error.bit.err_switch; - reply_ans->digit_data.byte03.bit_data.bit3 = project.cds_in[1].read.sbus.current_status_error.bit.err_switch; - - -#if(C_cds_in_number>=3) - reply_ans->digit_data.byte03.bit_data.bit4 = project.cds_in[2].read.sbus.current_status_error.bit.err_switch; -#endif - reply_ans->digit_data.byte03.bit_data.bit5 = project.cds_out[0].read.sbus.current_status_error.bit.err_switch; - reply_ans->digit_data.byte03.bit_data.bit6 = project.cds_out[1].read.sbus.current_status_error.bit.err_switch; - reply_ans->digit_data.byte03.bit_data.bit7 = 0; - - //TK0 acknolege-current - reply_ans->digit_data.byte04.byte_data = project.cds_tk[0].read.sbus.status_protect_current_ack.all & 0xFF; - reply_ans->digit_data.byte05.byte_data = (project.cds_tk[0].read.sbus.status_protect_current_ack.all >> 8) & 0xFF; - - //TK1 acknolege-current - reply_ans->digit_data.byte06.byte_data = project.cds_tk[1].read.sbus.status_protect_current_ack.all & 0xFF; - reply_ans->digit_data.byte07.byte_data = (project.cds_tk[1].read.sbus.status_protect_current_ack.all >> 8) & 0xFF; - - //TK2 acknolege-current - reply_ans->digit_data.byte08.byte_data = project.cds_tk[2].read.sbus.status_protect_current_ack.all & 0xFF; - reply_ans->digit_data.byte09.byte_data = (project.cds_tk[2].read.sbus.status_protect_current_ack.all >> 8) & 0xFF; - - //TK3 acknolege-current - reply_ans->digit_data.byte10.byte_data = project.cds_tk[3].read.sbus.status_protect_current_ack.all & 0xFF; - reply_ans->digit_data.byte11.byte_data = (project.cds_tk[3].read.sbus.status_protect_current_ack.all >> 8) & 0xFF; - - -//IN1 - reply_ans->digit_data.byte13.byte_data = project.cds_in[0].read.pbus.data_in.all & 0xFF; - reply_ans->digit_data.byte14.byte_data = (project.cds_in[0].read.pbus.data_in.all >> 8) & 0xFF; - -//IN2 - reply_ans->digit_data.byte15.byte_data = project.cds_in[1].read.pbus.data_in.all & 0xFF; - reply_ans->digit_data.byte16.byte_data = (project.cds_in[1].read.pbus.data_in.all >> 8) & 0xFF; - -//IN3 -#if(C_cds_in_number>=3) - reply_ans->digit_data.byte17.byte_data = project.cds_in[2].read.pbus.data_in.all & 0xFF; - reply_ans->digit_data.byte18.byte_data = (project.cds_in[2].read.pbus.data_in.all >> 8) & 0xFF; -#endif - - reply_ans->digit_data.byte19.bit_data.bit0 = get_status_sync_line();//CAN_timeout[UKSS1_CAN_DEVICE]; - //reply.digit_data.byte21.bit_data.bit1 = CAN_timeout[UKSS4_CAN_DEVICE]; - reply_ans->digit_data.byte19.bit_data.bit2 = 0;// CAN_timeout[UKSS2_CAN_DEVICE]; - reply_ans->digit_data.byte19.bit_data.bit3 = 0;// CAN_timeout[UKSS3_CAN_DEVICE]; - reply_ans->digit_data.byte19.bit_data.bit4 = CAN_timeout[get_real_in_mbox(UNITS_TYPE_BOX,ZADATCHIK_CAN)]; - //reply_test_all.digit_data.byte19.bit_data.bit5 = CAN_timeout[VPU2_CAN_DEVICE]; - reply_ans->digit_data.byte19.bit_data.bit6 = CAN_timeout[get_real_in_mbox(UNITS_TYPE_BOX,UMU_CAN_DEVICE)]; - //reply.digit_data.byte21.bit_data.bit7 = CAN_timeout[VPU1_CAN_DEVICE]; - - reply_ans->digit_data.byte20.bit_data.bit0 = project.controller.read.errors.bit.er0_out; - reply_ans->digit_data.byte20.bit_data.bit1 = project.controller.read.errors.bit.er0_trig; - reply_ans->digit_data.byte20.bit_data.bit2 = project.controller.read.errors.bit.errHWP; - reply_ans->digit_data.byte20.bit_data.bit3 = project.controller.read.errors.bit.errHWP_trig; - reply_ans->digit_data.byte20.bit_data.bit4 = project.controller.read.errors.bit.error_pbus; - reply_ans->digit_data.byte20.bit_data.bit5 = project.controller.read.errors.bit.pwm_wdog; - reply_ans->digit_data.byte20.bit_data.bit6 = project.controller.read.errors.bit.status_er0; -// reply_ans->digit_data.byte20.bit_data.bit0 = project.controller.read.errors.bit.er0_out; -// reply_ans->digit_data.byte20.bit_data.bit0 = CAN_timeout[get_real_in_mbox(UNITS_TYPE_BOX,VPU_CAN)]; -// reply_ans->digit_data.byte20.bit_data.bit1 = CAN_timeout[get_real_in_mbox(MPU_TYPE_BOX,0)]; - reply_ans->digit_data.byte20.bit_data.bit2 = 0;//READY_UKSS_6; - reply_ans->digit_data.byte20.bit_data.bit3 = 0;//READY_UKSS_7; - reply_ans->digit_data.byte20.bit_data.bit4 = 0;//READY_UKSS_8; - //reply_test_all.digit_data.byte20.bit_data.bit5 = READY_UKSS_1; - //reply_test_all.digit_data.byte20.bit_data.bit6 = READY_UKSS_2; - //reply_test_all.digit_data.byte20.bit_data.bit7 = READY_UKSS_3; - - - - reply_ans->digit_data.byte21.bit_data.bit0 = !project.hwp[0].status;//XProject_balzam.IsReady_reg.bit.XPlaneHWP_Chanals_IsReady; - reply_ans->digit_data.byte21.bit_data.bit2 = !project.adc[0].status;//XProject_balzam.IsReady_reg.bit.XPlaneHWP_Chanals_IsReady; -#if(C_cds_in_number>=1) - reply_ans->digit_data.byte21.bit_data.bit3 = !project.cds_in[0].status;//XProject_balzam.IsReady_reg.bit.XPlaneIN0_IsReady; -#endif -#if(C_cds_in_number>=2) - reply_ans->digit_data.byte21.bit_data.bit4 = !project.cds_in[1].status;//XProject_balzam.IsReady_reg.bit.XPlaneIN1_IsReady; -#endif -#if(C_cds_in_number>=3) - reply_ans->digit_data.byte21.bit_data.bit5 = !project.cds_in[2].status;//XProject_balzam.IsReady_reg.bit.XPlaneIN2_IsReady; -#endif - -#if(C_cds_out_number>=1) - reply_ans->digit_data.byte21.bit_data.bit6 = !project.cds_out[0].status;//XProject_balzam.IsReady_reg.bit.XPlaneOUT0_IsReady; -#endif -#if(C_cds_out_number>=2) - reply_ans->digit_data.byte21.bit_data.bit7 = !project.cds_out[1].status;//XProject_balzam.IsReady_reg.bit.XPlaneOUT1_IsReady; -#endif -#if(C_cds_out_number>=3) - reply_ans->digit_data.byte22.bit_data.bit0 = !project.cds_out[2].status;//XProject_balzam.IsReady_reg.bit.XPlaneOUT2_IsReady; -#endif - - - reply_ans->digit_data.byte22.bit_data.bit1 = !project.cds_tk[0].status;//XProject_balzam.IsReady_reg.bit.XPlaneTK0_IsReady; - reply_ans->digit_data.byte22.bit_data.bit2 = !project.cds_tk[1].status;//XProject_balzam.IsReady_reg.bit.XPlaneTK1_IsReady; - reply_ans->digit_data.byte22.bit_data.bit3 = !project.cds_tk[2].status;//XProject_balzam.IsReady_reg.bit.XPlaneTK2_IsReady; - reply_ans->digit_data.byte22.bit_data.bit4 = !project.cds_tk[3].status;//!XProject_balzam.IsReady_reg.bit.XPlaneTK3_IsReady; - reply_ans->digit_data.byte22.bit_data.bit5 = !project.adc[1].status;//XProject_balzam.IsReady_reg.bit.XPlaneHWP_Chanals_IsReady; - - reply_ans->digit_data.byte23.byte_data=project.hwp[0].read.comp_s.minus.all & 0x00ff; - reply_ans->digit_data.byte24.byte_data=((project.hwp[0].read.comp_s.minus.all >> 8) & 0x00ff); - - reply_ans->digit_data.byte23.byte_data|=project.hwp[0].read.comp_s.plus.all & 0x00ff; - reply_ans->digit_data.byte24.byte_data|=((project.hwp[0].read.comp_s.plus.all >> 8) & 0x00ff); - -#if (USE_TK_0) - if (project.cds_tk[0].useit) - { -// reply_ans->digit_data.byte22.bit_data.bit0 = project.cds_tk[0].read.sbus.lock_status_error.bit.err0_in; - reply_ans->digit_data.byte25.bit_data.bit0 = project.cds_tk[0].read.sbus.lock_status_error.bit.err_hwp; - reply_ans->digit_data.byte25.bit_data.bit1 = project.cds_tk[0].read.sbus.lock_status_error.bit.err0_local; - reply_ans->digit_data.byte25.bit_data.bit2 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - reply_ans->digit_data.byte25.bit_data.bit3 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - reply_ans->digit_data.byte25.bit_data.bit4 = project.cds_tk[0].read.sbus.lock_status_error.bit.line_err_keys_7654; - reply_ans->digit_data.byte25.bit_data.bit5 = project.cds_tk[0].read.sbus.lock_status_error.bit.line_err_keys_3210; - reply_ans->digit_data.byte25.bit_data.bit6 = project.cds_tk[0].read.sbus.lock_status_error.bit.ErrorSoftShutdownFromErr0; - reply_ans->digit_data.byte25.bit_data.bit7 = project.cds_tk[0].read.sbus.lock_status_error.bit.ErrorSoftShutdownForbidComb; - } - else - reply_ans->digit_data.byte25.byte_data = 0; - -#else - reply_ans->digit_data.byte25.byte_data = 0; -#endif - -#if (USE_TK_1) - if (project.cds_tk[1].useit) - { - reply_ans->digit_data.byte26.bit_data.bit0 = project.cds_tk[1].read.sbus.lock_status_error.bit.err_hwp; - reply_ans->digit_data.byte26.bit_data.bit1 = project.cds_tk[1].read.sbus.lock_status_error.bit.err0_local; - reply_ans->digit_data.byte26.bit_data.bit2 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - reply_ans->digit_data.byte26.bit_data.bit3 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - reply_ans->digit_data.byte26.bit_data.bit4 = project.cds_tk[1].read.sbus.lock_status_error.bit.line_err_keys_7654; - reply_ans->digit_data.byte26.bit_data.bit5 = project.cds_tk[1].read.sbus.lock_status_error.bit.line_err_keys_3210; - reply_ans->digit_data.byte26.bit_data.bit6 = project.cds_tk[1].read.sbus.lock_status_error.bit.ErrorSoftShutdownFromErr0; - reply_ans->digit_data.byte26.bit_data.bit7 = project.cds_tk[1].read.sbus.lock_status_error.bit.ErrorSoftShutdownForbidComb; - } - else - reply_ans->digit_data.byte26.byte_data = 0; -#else - reply_ans->digit_data.byte26.byte_data = 0; -#endif - -#if (USE_TK_2) - if (project.cds_tk[2].useit) - { - reply_ans->digit_data.byte27.bit_data.bit0 = project.cds_tk[2].read.sbus.lock_status_error.bit.err_hwp; - reply_ans->digit_data.byte27.bit_data.bit1 = project.cds_tk[2].read.sbus.lock_status_error.bit.err0_local; - reply_ans->digit_data.byte27.bit_data.bit2 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - reply_ans->digit_data.byte27.bit_data.bit3 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - reply_ans->digit_data.byte27.bit_data.bit4 = project.cds_tk[2].read.sbus.lock_status_error.bit.line_err_keys_7654; - reply_ans->digit_data.byte27.bit_data.bit5 = project.cds_tk[2].read.sbus.lock_status_error.bit.line_err_keys_3210; - reply_ans->digit_data.byte27.bit_data.bit6 = project.cds_tk[2].read.sbus.lock_status_error.bit.ErrorSoftShutdownFromErr0; - reply_ans->digit_data.byte27.bit_data.bit7 = project.cds_tk[2].read.sbus.lock_status_error.bit.ErrorSoftShutdownForbidComb; - } - else - reply_ans->digit_data.byte27.byte_data = 0; -#else - reply_ans->digit_data.byte27.byte_data = 0; -#endif - -#if (USE_TK_3) - if (project.cds_tk[3].useit) - { - reply_ans->digit_data.byte28.bit_data.bit0 = project.cds_tk[3].read.sbus.lock_status_error.bit.err_hwp; - reply_ans->digit_data.byte28.bit_data.bit1 = project.cds_tk[3].read.sbus.lock_status_error.bit.err0_local; - reply_ans->digit_data.byte28.bit_data.bit2 = project.cds_tk[3].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - reply_ans->digit_data.byte28.bit_data.bit3 = project.cds_tk[3].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - reply_ans->digit_data.byte28.bit_data.bit4 = project.cds_tk[3].read.sbus.lock_status_error.bit.line_err_keys_7654; - reply_ans->digit_data.byte28.bit_data.bit5 = project.cds_tk[3].read.sbus.lock_status_error.bit.line_err_keys_3210; - reply_ans->digit_data.byte28.bit_data.bit6 = project.cds_tk[3].read.sbus.lock_status_error.bit.ErrorSoftShutdownFromErr0; - reply_ans->digit_data.byte28.bit_data.bit7 = project.cds_tk[3].read.sbus.lock_status_error.bit.ErrorSoftShutdownForbidComb; - } - else - reply_ans->digit_data.byte28.byte_data = 0; -#else - reply_ans->digit_data.byte28.byte_data = 0; -#endif - - -/* - -//IN1 ready - reply_ans->digit_data.byte21.byte_data = project.cds_in[0].read.pbus.ready_in.all & 0xFF; - reply_ans->digit_data.byte22.byte_data = (project.cds_in[0].read.pbus.ready_in.all >> 8) & 0xFF; - -//TK0 acknolege-current - reply_ans->digit_data.byte23.byte_data = project.cds_tk[0].read.sbus.status_protect_current_ack.all & 0xFF; - reply_ans->digit_data.byte24.byte_data = (project.cds_tk[0].read.sbus.status_protect_current_ack.all >> 8) & 0xFF; -*/ - return; -} - - - - - - - - - - - - - - - - - - - - - diff --git a/Inu/Src2/551/main/message2test.h b/Inu/Src2/551/main/message2test.h deleted file mode 100644 index 3f1406c..0000000 --- a/Inu/Src2/551/main/message2test.h +++ /dev/null @@ -1,33 +0,0 @@ -//////////////////////////////////////////// -// message.h -// -// : -// 1. y -// 2. -// 3. y -// -// -// y y y -// . INTEL 386SX Octagon -// TMS320C32 Texas Instruments. -// -// . -// y -// unsigned char = 8 -// TMS320C32 unsigned char = 32 , y -// 8 . -// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -// -// y -// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -//////////////////////////////////////////// - -#ifndef MESSAGE_TEST_H -#define MESSAGE_TEST_H - -#include "RS_Function_terminal.h" - -void func_fill_answer_to_TMS_test(TMS_TO_TERMINAL_TEST_ALL_STRUCT* reply_ans, CMD_TO_TMS_TEST_ALL_STRUCT* pcommand); - - -#endif //MESSAGE_H diff --git a/Inu/Src2/551/main/message_modbus.c b/Inu/Src2/551/main/message_modbus.c deleted file mode 100644 index 024db30..0000000 --- a/Inu/Src2/551/main/message_modbus.c +++ /dev/null @@ -1,897 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include "CAN_Setup.h" -#include "global_time.h" -#include "modbus_table_v2.h" -#include "oscil_can.h" -#include "RS_modbus_pult.h" -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "CRC_Functions.h" -#include "RS_Function_terminal.h" -#include "RS_modbus_svu.h" -#include "TuneUpPlane.h" -#if (USE_CONTROL_STATION==1) -#include "control_station.h" -#endif -#include -#include - -#include "pwm_test_lines.h" -#include "params.h" -#include "logs_hmi.h" - - -//#include "can_setup_21300.h" -//#include "modbus_can.h" - -//static int err_modbus3=1; -//static int err_modbus16=1; -//static int cmd_3_or_16=0; - -int enable_can = 0; - -void write_all_data_to_mpu_485(int run_force) -{ - static unsigned int time_tick_modbus = 0; - static unsigned int old_PWM_ticks = 0; - static int count_write_to_modbus = 0; - static int cur_position_buf_modbus16 = 0; - - if (global_time.pwm_tics != old_PWM_ticks) - { - if (global_time.pwm_tics > old_PWM_ticks) - time_tick_modbus = time_tick_modbus + (global_time.pwm_tics - old_PWM_ticks); - else - time_tick_modbus++; - } - - if (time_tick_modbus > TIME_PAUSE_MODBUS_MPU) - { - // pause_1000() - - time_tick_modbus = 0; - pause_1000(10); - // SendCommandModbus3(&rs_b, 0x1, 0xc012,1); - // rs_b.flag_LEADING = 0; - if (!rs_b.flag_LEADING) - { - time_tick_modbus = 0; - //Fast answer to SVU when command changed - // if (flag_send_answer_rs) { - // flag_send_answer_rs = 0; - // err_modbus16++; - // SendCommandModbus16(&rs_b,1,210,SIZE_BUF_WRITE_TO_MODBUS16); - // return; - // } - if (err_modbus16 == 0) - cur_position_buf_modbus16 = cur_position_buf_modbus16 + SIZE_BUF_WRITE_TO_MODBUS16_VPU; - - if (cur_position_buf_modbus16 >= SIZE_MODBUS_TABLE) - cur_position_buf_modbus16 = 0; - - if ((cur_position_buf_modbus16 + SIZE_BUF_WRITE_TO_MODBUS16_VPU) > SIZE_MODBUS_TABLE) - count_write_to_modbus = SIZE_MODBUS_TABLE - cur_position_buf_modbus16; - else - count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS16_VPU; - - // count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS16_VPU; - // cur_position_buf_modbus=0; - - err_modbus16++; - - if (err_modbus16 > MAX_COUNT_ERROR_FROM_RS_MPU) - f.RS_MPU_ERROR = 1; - - ModbusRTUsend16(&rs_b, 1, - ADR_MODBUS_TABLE + cur_position_buf_modbus16, - count_write_to_modbus); - // SendCommandModbus16(&rs_b,1,ADR_MODBUS_TABLE+cur_position_buf_modbus16,count_write_to_modbus); - // SendCommandModbus16(&rs_b,1,1,30); - - // - // SendCommandModbus16(&rs_b,1,0xc001,0x64); - // err_modbus - } - } -} - -void read_all_data_from_mpu_485(int run_force) -{ - static unsigned int time_tick_modbus = 0; - static unsigned int old_PWM_ticks = 0; - static unsigned int count_write_to_modbus = 0; - static int cur_position_buf_modbus3 = 0; - - if (global_time.pwm_tics != old_PWM_ticks) - { - if (global_time.pwm_tics > old_PWM_ticks) - time_tick_modbus = time_tick_modbus + (global_time.pwm_tics - old_PWM_ticks); - else - time_tick_modbus++; - } - - old_PWM_ticks = global_time.pwm_tics; - - if (TIME_PAUSE_MODBUS_MPU < time_tick_modbus) - { - // pause_1000() - - pause_1000(10); - // SendCommandModbus3(&rs_b, 0x1, 0xc012,1); - //rs_b.flag_LEADING = 0; - if (!rs_b.flag_LEADING) - { - time_tick_modbus = 0; - - if (err_modbus3 == 0) - cur_position_buf_modbus3 = cur_position_buf_modbus3 + SIZE_BUF_WRITE_TO_MODBUS16_VPU; - - if (cur_position_buf_modbus3 >= SIZE_MODBUS_TABLE) - cur_position_buf_modbus3 = 0; - - if ((cur_position_buf_modbus3 + SIZE_BUF_WRITE_TO_MODBUS16_VPU) > SIZE_MODBUS_TABLE) - count_write_to_modbus = SIZE_MODBUS_TABLE - cur_position_buf_modbus3; - else - count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS16_VPU; - - // count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS16_VPU; - // cur_position_buf_modbus=0; - - err_modbus3++; - - if (err_modbus3 > MAX_COUNT_ERROR_FROM_RS_MPU) - f.RS_MPU_ERROR = 1; - - // SendCommandModbus3(&rs_b,1,ADR_MODBUS_TABLE+cur_position_buf_modbus3,count_write_to_modbus); - ModbusRTUsend3(&rs_b, 1, - ADR_MODBUS_TABLE + cur_position_buf_modbus3, - count_write_to_modbus); - } - } - - // time_tick_modbus++; -} - -void write_all_data_to_mpu_can(int run_force, unsigned int pause) -{ -// static int time_tick_modbus_can = 0; - static unsigned int old_time = 0; - static int count_write_to_modbus_can = 0; -// static int time_send_to_can = 0; - // static unsigned int counter_max_I = 0, counter_max_M = 0; - - static int cur_position_buf_modbus16_can = 0, prev_send_to_can = 0; - int real_mbox; - unsigned int i; - - real_mbox = get_real_out_mbox(MPU_TYPE_BOX, edrk.flag_second_PCH); - - if (prev_send_to_can && CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_OFF)==0) - { - old_time = (unsigned int)global_time.miliseconds; - return; - } - prev_send_to_can = 0; - - if (!detect_pause_milisec(pause,&old_time)) - return; - - -// -// -// if (cmd_3_or_16 == 1 || run_force) -// { -// -// if (global_time.pwm_tics != old_PWM_ticks) -// { -// if (global_time.pwm_tics > old_PWM_ticks) -// time_tick_modbus_can = time_tick_modbus_can + (global_time.pwm_tics - old_PWM_ticks); -// else -// time_tick_modbus_can++; -// } -// -// old_PWM_ticks = global_time.pwm_tics; -// } -// else -// { -// old_PWM_ticks = global_time.pwm_tics; -// return; -// } -// -// -// -// if (CAN_cycle_free(real_mbox)) -// { -// if (time_send_to_can == 0) -// time_send_to_can = time_tick_modbus_can; -// -// // time_tick_modbus_can=0; -// -// } - // if(f.Prepare && CAN_cycle_free(real_mbox)) - // { - // CAN_cycle_send( MPU_TYPE_BOX, 0, 198, &modbus_table_can_out[197].all, 1); - // } - - -// if (time_tick_modbus_can > TIME_PAUSE_MODBUS_CAN) -// { -// time_tick_modbus_can = 0; -// time_send_to_can = 0; -// // pause_1000(300); - - - if (CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_ON)) - { - ///////////////////////////////////////////////////////////////////////////////////////////// - - //////////////////////////////////////////////////////////////////////////////////////////// - - - if (cur_position_buf_modbus16_can >= SIZE_MODBUS_TABLE) - { - cur_position_buf_modbus16_can = 0; - // modbus_table_can_out[ADR_CAN_TEST_PLUS_ONE].all++; - } - - // - if (cur_position_buf_modbus16_can == 0) - { - for (i=0;i= SIZE_MODBUS_TABLE) - count_write_to_modbus_can = SIZE_MODBUS_TABLE - cur_position_buf_modbus16_can; - else - count_write_to_modbus_can = SIZE_BUF_WRITE_TO_MODBUS16_CAN; - - if (CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_ON)) - { - // modbus_table_can_out[0x124].all++; - // CAN_cycle_send(MPU_CAN_DEVICE, cur_position_buf_modbus16_can+1, &modbus_table_can_out[cur_position_buf_modbus16_can].all, count_write_to_modbus_can); - - CAN_cycle_send( - MPU_TYPE_BOX, - edrk.flag_second_PCH, - cur_position_buf_modbus16_can + 1, -// &modbus_table_can_out[cur_position_buf_modbus16_can].all, - &modbus_table_can_out_temp[cur_position_buf_modbus16_can].all, - count_write_to_modbus_can, CAN_BOX_STANDART_ADR, CAN_BOX_PRIORITY_NORMAL); - - cur_position_buf_modbus16_can = cur_position_buf_modbus16_can + SIZE_BUF_WRITE_TO_MODBUS16_CAN; - } - } - -// } - -} - -//void test_rs_can_with_svu_mpu() -//{ -// int test_a, test_b; -// -// test_a = test_rs_live(&rs_a); -// test_b = test_rs_live(&rs_b); -// -// if (test_a == 0 && test_b == 0 && f.status_MODE_WORK_SVU == 0) -// { -// /* RS232 */ -// if (test_can_live_mpu() == 0) -// { -// // if (f.cmd_to_go == 0) f.cmd_to_go = ERROR_CMD_GO_1; -// edrk.Go = 0; /* */ -// } -// } -// -// if (test_a == 0 && test_b == 0 && f.status_MODE_WORK_MPU == 0 /*&& f.status_SPEED_SELECT_KEY==0*/) -// { -// /* RS232 */ -// if (test_can_live_mpu() == 0) -// { -// // if (f.cmd_to_go == 0) f.cmd_to_go = ERROR_CMD_GO_2; -// edrk.Go = 0; /* */ -// } -// } -// -// if (test_a == 2) -// { -// /* RS232 */ -// // edrk.Go=0; /* */ -// resetup_rs(&rs_a); -// } -// -// if (test_b == 2) -// { -// /* RS232 */ -// resetup_rs(&rs_b); -//// flag_waiting_answer = 0; //, , -// // slave , . -// } -// -// if (test_b == 5) -// { -// /* RS232 */ -// resetup_rs(&rs_b); -//// flag_waiting_answer = 0; //, , -// // slave , . -// } -// -// if (test_a == 4 && test_b == 4) //TODO: && SPEED_SELECT_REMOTE==1) -// { -// // RS232 -// if (test_can_live_mpu() == 0) -// { -// // if (f.cmd_to_go == 0) f.cmd_to_go = ERROR_CMD_GO_3; -// edrk.Go = 0; // -// } -// resetup_mpu_rs(&rs_a); -// resetup_mpu_rs(&rs_b); -//// flag_waiting_answer = 0; -// } -// -// if (test_a == 4 && test_b == 4) //TOD: && SPEED_SELECT_REMOTE==0) -// { -// // RS232 -// // if (test_can_live_mpu()==0) -// -// // test_can_live_terminal // -// -// { -// // if (f.cmd_to_go == 0) f.cmd_to_go = ERROR_CMD_GO_20; -// edrk.Go = 0; // -// } -// resetup_mpu_rs(&rs_a); -// resetup_mpu_rs(&rs_b); -// } -// -// if (test_a == 0 && f.status_MODE_WORK_MPU == 0) // && SPEED_SELECT_REMOTE==0) -// { -// /* RS232 */ -// // if (test_can_live_mpu()==0) -// // if (f.cmd_to_go == 0) f.cmd_to_go = ERROR_CMD_GO_4; -// edrk.Go = 0; /* */ -// } -// -// // if (CAN_timeout[]==1) -// // f.CAN_MPU_ERROR=CAN_timeout[MPU_CAN_DEVICE]; -//} - - - -#define TIME_REINIT_PULT_INGETEAM 5 - - -void test_alive_pult_485(void) -{ - static unsigned int time_pause = 0, old_time = 0; - -// -// if (control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485]>control_station.setup_time_detect_active_resend_485[CONTROL_STATION_INGETEAM_PULT_RS485]) -// { -// resetup_mpu_rs(&rs_b); -// control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// } -/* - if (control_station.time_detect_active[CONTROL_STATION_INGETEAM_PULT_RS485]>control_station.setup_time_detect_active_resend_485[CONTROL_STATION_INGETEAM_PULT_RS485]) - { - resetup_mpu_rs(&rs_b); - control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - // control_station.time_detect_active[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - } -*/ - -// if (control_station.time_detect_active[CONTROL_STATION_INGETEAM_PULT_RS485]>control_station.setup_time_detect_active[CONTROL_STATION_INGETEAM_PULT_RS485]) -// { -// resetup_mpu_rs(&rs_b); -// control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// control_station.time_detect_active[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// } - - -/* - if (!detect_pause_milisec(CONTROL_STATION_TIME_WAIT,&old_time)) - return; - - time_pause++; - - if (time_pause>=TIME_REINIT_PULT_INGETEAM) - { - flag_waiting_answer = 0; //, - } -*/ - -} - - -#define MAX_COUNT_WORK_IN_LOG 150 - - -int modbusNetworkSharing(int flag_update_only_hmi) -{ - static unsigned int old_time = 0 , old_time_refresh = 0, time_pause = TIME_PAUSE_MODBUS_REMOUTE; - static unsigned int old_time_status_3 = 0, time_pause_status_3 = 500; - int final_code=0; - static unsigned int status=0; - static int numberInT=0, enable_send_cmd = 0; - - static int run_pause = 1, flag_next = 0, prev_flag_next = 0; - static int last_ok_cmd=0; - static int flag_only_one_cmd=0; - static int status_ok = 1, status_err = 0, flag_work_rs_send_log = 0, count_free = 0, count_work_in_log = 0; - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_RS) - PWM_LINES_TK_20_ON; -#endif - - RS232_WorkingWith(0,1,0); - - // - - // control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] = 1; - - control_station_test_alive_all_control(); - - // if (detect_pause_milisec(100,&old_time_refresh)) - control_station.update_timers(&control_station); - - detecting_cmd_from_can(); - - // test_rs_can_with_svu_mpu(); -// test_alive_pult_485(); - - - // if (rs_b.RS_DataSended==0 && rs_b.RS_DataReadyAnswerAnalyze==0) - // status = 0; - - control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = rs_b.RS_DataSended; - - final_code = 0; - - - switch(status) - { - case 0: - - old_time = global_time.miliseconds; - status = 1; - if (time_pause==0) - status = 2; - - break; - case 1: - // if (numberInT==0) - // { - if (detect_pause_milisec(time_pause,&old_time)) - status = 2; - // } - // else - // status = 2; - - break; - case 2: - enable_send_cmd = 1; - control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - status = 3; - old_time_status_3 = global_time.miliseconds; - - break; - case 3: - // // rs485 - // if (flag_work_rs_send_log) - // { - // status = 0; - // status_ok++; - // if (status_ok<0) status_ok=1; - // } - - // rs485 ! - if (rs_b.RS_DataWillSend2 == 0 && enable_send_cmd == 0) - { - status = 0; - status_ok++; - count_free++; - } - else - if (rs_b.RS_DataReadyAnswerAnalyze) - { -// i_led2_on_off(0); - - control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - control_station.count_ok_modbus[CONTROL_STATION_INGETEAM_PULT_RS485]++; - rs_b.RS_DataReadyAnswerAnalyze = 0; - rs_b.RS_DataWillSend2 = 0; - status = 0; - - if (last_ok_cmd==4) // readAnalogDataFromRemote() - { - edrk.get_new_data_from_hmi = 1;// ? - edrk.get_new_data_from_hmi2 = 1;// ? - } - final_code = last_ok_cmd;//numberInT+1; - - //status_err = 0; - status_ok++; - } - else - { - if ( (control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485]>control_station.setup_time_detect_active_resend_485[CONTROL_STATION_INGETEAM_PULT_RS485]) - || (detect_pause_milisec(time_pause_status_3,&old_time_status_3)) ) - { - resetup_mpu_rs(&rs_b); - control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - rs_b.RS_DataWillSend2 = 0; - status = 0; - control_station.count_error_modbus[CONTROL_STATION_INGETEAM_PULT_RS485]++; - status_err++;// = 1; - } - - } - - if (status_ok<0) status_ok=1; - - break; - case 4: - - break; - case 5: - - break; - case 6: - - break; - default: - break; - - - - } - -/* - if (status==0) - { - old_time = global_time.miliseconds; - status = 1; - if (time_pause==0) - status = 2; - } - - if (status==1) - { -// if (numberInT==0) -// { - if (detect_pause_milisec(time_pause,&old_time)) - status = 2; -// } -// else -// status = 2; - } - - if (status==2) - { - enable_send_cmd = 1; - control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - status = 3; - old_time_status_3 = global_time.miliseconds; - } - - if (status==3) - { - if (rs_b.RS_DataWillSend2 == 0) - { - - count_free++; - - } - - // rs485 - if (flag_work_rs_send_log) - { - status = 0; - status_ok++; - if (status_ok<0) status_ok=1; - } - - if (rs_b.RS_DataReadyAnswerAnalyze) - { -// i_led2_on_off(0); - - control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - control_station.count_ok_modbus[CONTROL_STATION_INGETEAM_PULT_RS485]++; - rs_b.RS_DataReadyAnswerAnalyze = 0; - rs_b.RS_DataWillSend2 = 0; - status = 0; - - if (last_ok_cmd==4) // readAnalogDataFromRemote() - edrk.get_new_data_from_hmi = 1;// ? - final_code = last_ok_cmd;//numberInT+1; - - //status_err = 0; - status_ok++; - if (status_ok<0) status_ok=1; - } - else - { - if ( (control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485]>control_station.setup_time_detect_active_resend_485[CONTROL_STATION_INGETEAM_PULT_RS485]) - || (detect_pause_milisec(time_pause_status_3,&old_time_status_3)) ) - { - resetup_mpu_rs(&rs_b); - control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; - rs_b.RS_DataWillSend2 = 0; - status = 0; - control_station.count_error_modbus[CONTROL_STATION_INGETEAM_PULT_RS485]++; - status_err++;// = 1; - } - - } - - } -*/ -// switch (status) -// { -// case 0 : status = 1; -// break; -// -// case 1 : old_time = global_time.miliseconds; -// status = 2; -// break; -// -// case 2 : -// if (run_pause) -// { -// status = 3; -// run_pause = 0; -// } -// if (detect_pause_milisec(time_pause,&old_time)) -// status = 3; -// break; -// -// case 3 : -// enable_send_cmd = 1; -//// control_station.count_error_modbus_15[CONTROL_STATION_INGETEAM_PULT_RS485]++; -// control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// status = 4; -// break; -// -// -// case 4 : -// if (rs_b.RS_DataReadyAnswerAnalyze) -// { -// control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// control_station.count_ok_modbus[CONTROL_STATION_INGETEAM_PULT_RS485]++; -// rs_b.RS_DataReadyAnswerAnalyze = 0; -// status = 1; -// final_code = numberInT+1; -// } -// else -// { -// if (control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485]>control_station.setup_time_detect_active_resend_485[CONTROL_STATION_INGETEAM_PULT_RS485]) -// { -// resetup_mpu_rs(&rs_b); -// control_station.time_detect_answer_485[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// status = 1; -// control_station.count_error_modbus[CONTROL_STATION_INGETEAM_PULT_RS485]++; -// } -// } -// break; -// -// -// case 5 : break; -// -// -// case 6 : break; -// -// default : break; -// } - - -// -// -// if (control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] == 1) -// { -// if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0 && prev_flag_waiting_answer == 1) -// { -// old_time = global_time.miliseconds; -// } -// -// if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0) -// { -// if (detect_pause_milisec(time_pause,&old_time)) -// { -// control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = 0; -// } -// -// } -// } -// prev_flag_waiting_answer = control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485]; -// -// -// if (control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] == 0 && -// control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0 ) -// { -// enable_send_cmd = 1; -//// numberInT++; -//// if (numberInT>3) -//// numberInT = 1; -// } -// - - -// - if (flag_only_one_cmd) - numberInT=flag_only_one_cmd-1; -// - - - if (enable_send_cmd -// && (log_to_HMI.send_log == 0) - ) - { -//i_led2_on_off(1); - last_ok_cmd = numberInT; - switch (numberInT) - { - - case 0: - - if ((flag_update_only_hmi==0) && (edrk.flag_enable_update_hmi)) - update_tables_HMI_discrete(); - - writeDiscreteDataToRemote(); - edrk.test++; - - numberInT++; - enable_send_cmd = 0; - break; - - case 1: - - if ((flag_update_only_hmi==0) && (edrk.flag_enable_update_hmi)) -// if (edrk.flag_enable_update_hmi) - update_tables_HMI_analog(); - - writeAnalogDataToRemote(); // 1 - -// if (flag_update_only_hmi==1) -// // -// numberInT = 0; -// else - numberInT++; - - enable_send_cmd = 0; - break; - - case 2: - -// if (edrk.flag_enable_update_hmi) -// update_tables_HMI_analog(); - - writeAnalogDataToRemote(); // 2 - -// if (flag_update_only_hmi==1) -// // -// numberInT = 0; -// else - numberInT++; - - enable_send_cmd = 0; - break; - - - case 3: - readAnalogDataFromRemote(); // 1 - numberInT++; - enable_send_cmd = 0; - break; - - case 4: - readAnalogDataFromRemote(); // 2 - - if (log_to_HMI.send_log) - { - numberInT++; - } - else - // - numberInT = 0; - - enable_send_cmd = 0; - count_work_in_log = 0; // - break; - - case 5: - if (log_to_HMI.send_log) - { - time_pause = 2; - // ccc[0] = 1; - flag_work_rs_send_log = !sendLogToHMI(status_ok); - edrk.flag_slow_in_main = 1; - enable_send_cmd = 0; - - if (count_work_in_log>MAX_COUNT_WORK_IN_LOG) - { - count_work_in_log = 0; - numberInT = 0; // - - } - else - { - count_work_in_log++; - // - } - - } - else - { - time_pause = TIME_PAUSE_MODBUS_REMOUTE; - // ccc[0] = 0; - numberInT = 0; - enable_send_cmd = 0; - edrk.flag_slow_in_main = 0; - } - break; - - - default: - enable_send_cmd = 0; - break; - } - - -//i_led2_on_off(0); - - } - //sendLogToHMI(); - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_RS) - PWM_LINES_TK_20_OFF; -#endif - - if (flag_update_only_hmi) - return final_code; - - - return 0; -} - - - -int modbusNetworkSharingCAN(void) -{ -// static unsigned int old_time1 = 0 , time_pause1 = TIME_PAUSE_NETWORK_CAN1; -// static unsigned int old_time2 = 0 , time_pause2 = TIME_PAUSE_NETWORK_CAN2; -// static unsigned int old_time3 = 0 , time_pause3 = TIME_PAUSE_NETWORK_CAN3; - static unsigned int time_pause_modbus_can_terminals = TIME_PAUSE_MODBUS_CAN_TERMINALS; - - - -#if (ENABLE_CAN_SEND_TO_MPU_FROM_MAIN) -// if (detect_pause_milisec(time_pause1,&old_time1)) - static unsigned int time_pause_modbus_can_mpu = TIME_PAUSE_MODBUS_CAN_MPU; - write_all_data_to_mpu_can(1, time_pause_modbus_can_mpu); -#endif - -#if (ENABLE_CAN_SEND_TO_TERMINAL_FROM_MAIN) -// if (detect_pause_milisec(time_pause2,&old_time2)) - write_all_data_to_terminals_can(1, time_pause_modbus_can_terminals); -#endif - - -#if (ENABLE_CAN_SEND_TO_TERMINAL_OSCIL) -// if (detect_pause_milisec(time_pause3,&old_time3)) - oscil_can.pause_can = TIME_PAUSE_MODBUS_CAN_OSCIL; - oscil_can.send(&oscil_can); -#endif - - return 0; -} - diff --git a/Inu/Src2/551/main/message_modbus.h b/Inu/Src2/551/main/message_modbus.h deleted file mode 100644 index 971e5a9..0000000 --- a/Inu/Src2/551/main/message_modbus.h +++ /dev/null @@ -1,61 +0,0 @@ - -#ifndef _MESSAGE_MODBUS_H -#define _MESSAGE_MODBUS_H - - - -// void ReceiveCommandModbus3(RS_DATA *rs_arr); -// void ReceiveCommandModbus16(RS_DATA *rs_arr); -// void ReceiveCommandModbus15(RS_DATA *rs_arr); - -// void SendCommandModbus3(RS_DATA *rs_arr,int adr_contr, unsigned int adr_start,unsigned int count_word); -// void SendCommandModbus16(RS_DATA *rs_arr,int adr_contr, unsigned int adr_start,unsigned int count_word); - -// void ReceiveAnswerCommandModbus16(RS_DATA *rs_arr); -// void ReceiveAnswerCommandModbus3(RS_DATA *rs_arr); - -#define TIME_PAUSE_MODBUS_MPU 250 //100//500 -#define TIME_PAUSE_MODBUS_REMOUTE 20 //100 //500 - -#define TIME_PAUSE_NETWORK_CAN1 444 //500 -#define TIME_PAUSE_NETWORK_CAN2 990 //500 -#define TIME_PAUSE_NETWORK_CAN3 1855 //500 - -//#define START_ADR_ARR 0xc000 -//#define LENGTH_ADR_ARR 0x100 -//#define SIZE_MODBUS_TABLE_DISCRETE_REMOUTE 36 // = 576/16 -#define SIZE_BUF_WRITE_TO_MODBUS1_REMOUTE SIZE_MODBUS_TABLE_DISCRET_REMOUTE // SIZE_MODBUS_TABLE_DISCRET_BITS //576 // 3 modbus . -#define SIZE_BUF_WRITE_TO_MODBUS15_REMOUTE SIZE_MODBUS_TABLE_DISCRET_REMOUTE //SIZE_MODBUS_TABLE_DISCRET_BITS //576 //96 - - -#define SIZE_BUF_WRITE_TO_MODBUS16_VPU 100 // - -#define SIZE_BUF_WRITE_TO_MODBUS16_REMOUTE 120 //100 // , . , SIZE_ANALOG_DATA_REMOUTE -#define SIZE_ANALOG_DATA_REMOUTE 240 //165 // , - - -#define SIZE_BUF_READ_FROM_MODBUS16_REMOUTE 120 //20//36 // , . , SIZE_ANALOG_DATA_FROM_MODBUS16_REMOUTE -#define SIZE_ANALOG_DATA_FROM_MODBUS16_REMOUTE SIZE_ANALOG_DATA_REMOUTE //20//36 // , - - -#define SIZE_BUF_WRITE_TO_MODBUS16_CAN 100 //10 //1000//400//04.04.2012 //100// //800 -#define START_LOG_MODBUS16_ADRES 100 -#define SIZE_BUF_WRITE_LOG_TO_MODBUS16 120 -//#define SIZE_ANALOG_DATA 61 - - -#define MAX_COUNT_ERROR_FROM_RS_MPU 10 - -//void test_rs_can_with_svu_mpu(); -void write_all_data_to_mpu_can(int run_force, unsigned int pause); -void read_all_data_from_mpu_485(int run_force); -void write_all_data_to_mpu_485(int run_force); -extern int enable_can; - -int modbusNetworkSharing(int flag_update_only_hmi); -int modbusNetworkSharingCAN(void); - - - -#endif //_MESSAGE_MODBUS_H - diff --git a/Inu/Src2/551/main/message_terminals_can.c b/Inu/Src2/551/main/message_terminals_can.c deleted file mode 100644 index c21892a..0000000 --- a/Inu/Src2/551/main/message_terminals_can.c +++ /dev/null @@ -1,128 +0,0 @@ -/* - * message_terminals_can.c - * - * Created on: 15 2020 . - * Author: yura - */ -#include -#include -#include -#include - -#include "CAN_Setup.h" -#include "global_time.h" -#include "modbus_table_v2.h" -#include "oscil_can.h" -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "CRC_Functions.h" -#include "RS_Function_terminal.h" -#include "TuneUpPlane.h" -#include "control_station.h" - - - -#define TERMINALS_CAN_TIME_WAIT 500 // -#pragma DATA_SECTION(buf_message_can_cmd,".slow_vars") -int buf_message_can_cmd[sizeof(CMD_TO_TMS_STRUCT)+10]; -#pragma DATA_SECTION(buf_message_can_data,".slow_vars") -int buf_message_can_data[sizeof(TMS_TO_TERMINAL_STRUCT)+10]; -#pragma DATA_SECTION(buf_message_can_data2,".slow_vars") -int buf_message_can_data2[sizeof(TMS_TO_TERMINAL_STRUCT)+10]; - -int *p_buf_message_can_data3; - - -void write_all_data_to_terminals_can(int run_force, unsigned int pause) -{ - static unsigned int old_time = 0; - static unsigned int send_time = 0; - static int prev_send_to_can = 0; - - static int count_sends = 0; - - unsigned long old_t; - unsigned int i; - int real_mbox; - CMD_TO_TMS_STRUCT* pcommand = (CMD_TO_TMS_STRUCT *)(buf_message_can_cmd); - - - real_mbox = get_real_out_mbox(TERMINAL_TYPE_BOX, edrk.number_can_box_terminal_cmd); - - // , , - // .. TERMINALS_CAN_TIME_WAIT . - if (prev_send_to_can && CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_OFF)==0) - { - old_time = (unsigned int)global_time.miliseconds; - return; - } - prev_send_to_can = 0; - - if (!detect_pause_milisec(pause,&old_time)) - return; - - - //func_fill_answer_to_TMS(&reply, pcommand); - func_pack_answer_to_TMS(&reply); - - *(TMS_TO_TERMINAL_STRUCT*)buf_message_can_data2 = reply; // - - - - // reply.digit_data.byte01.byte_data = send_time; -/* reply.digit_data.byte02.byte_data = 0x66; - reply.analog_data.analog60_hi = 0x33; - reply.analog_data.analog60_lo = 0x44; -*/ - reply.analog_data.analog58_hi = HIBYTE((unsigned int)control_station.raw_array_data[CONTROL_STATION_TERMINAL_CAN][0].all); - reply.analog_data.analog58_lo = LOBYTE((unsigned int)control_station.raw_array_data[CONTROL_STATION_TERMINAL_CAN][0].all); - -// control_station.raw_array_data[CONTROL_STATION_TERMINAL_CAN][0].all - - reply.analog_data.analog59_hi = HIBYTE((unsigned int)global_time.miliseconds); - reply.analog_data.analog59_lo = LOBYTE((unsigned int)global_time.miliseconds); - - reply.analog_data.analog60_hi = HIBYTE(count_sends); - reply.analog_data.analog60_lo = LOBYTE(count_sends); - count_sends++; - if (count_sends>32768) count_sends=0; - -// reply.analog_data.analog1_hi = HIBYTE(send_time); - // reply.analog_data.analog1_lo = LOBYTE(send_time); - -// reply.analog_data.analog2_hi = HIBYTE(oscil_can.timer_send); -// reply.analog_data.analog2_lo = LOBYTE(oscil_can.timer_send); - - p_buf_message_can_data3 = (int *)&reply.digit_data; - - - for (i=0;i>1] |= ( (*p_buf_message_can_data3++) << 8) & 0xff00; - } - else - buf_message_can_data[i>>1] = ( (*p_buf_message_can_data3++) ) & 0x00ff; - - } - - if (CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_ON)) - { - - old_t = global_time.microseconds; - CAN_cycle_send( - TERMINAL_TYPE_BOX, - edrk.number_can_box_terminal_cmd, - 0, - &buf_message_can_data[0], ((sizeof(TMS_TO_TERMINAL_STRUCT)-5)>>1), CAN_BOX_STANDART_ADR, CAN_BOX_PRIORITY_NORMAL); - - prev_send_to_can = 1; - - - send_time = (global_time.microseconds - old_t)/100; - } - - -} diff --git a/Inu/Src2/551/main/message_terminals_can.h b/Inu/Src2/551/main/message_terminals_can.h deleted file mode 100644 index 90cf1c6..0000000 --- a/Inu/Src2/551/main/message_terminals_can.h +++ /dev/null @@ -1,15 +0,0 @@ -/* - * message_terminals_can.h - * - * Created on: 22 2020 . - * Author: yura - */ - -#ifndef SRC_MAIN_MESSAGE_TERMINALS_CAN_H_ -#define SRC_MAIN_MESSAGE_TERMINALS_CAN_H_ - -void write_all_data_to_terminals_can(int run_force, unsigned int pause); - - - -#endif /* SRC_MAIN_MESSAGE_TERMINALS_CAN_H_ */ diff --git a/Inu/Src2/551/main/modbus_hmi.c b/Inu/Src2/551/main/modbus_hmi.c deleted file mode 100644 index f28cb25..0000000 --- a/Inu/Src2/551/main/modbus_hmi.c +++ /dev/null @@ -1,393 +0,0 @@ -#include "log_to_memory.h" -#include -#include -#include -#include - -#include "control_station.h" -#include "global_time.h" -#include "modbus_table_v2.h" -#include "RS_modbus_pult.h" -#include "DSP281x_Device.h" -#include "MemoryFunctions.h" -#include "RS_modbus_svu.h" -#include "log_params.h" - - -#pragma DATA_SECTION(modbus_table_discret_in,".logs"); -MODBUS_REG_STRUCT modbus_table_discret_in[SIZE_MODBUS_TABLE_DISCRET_REMOUTE]; //registers 10001-19999 modbus RTU - -#pragma DATA_SECTION(modbus_table_discret_out,".logs"); -MODBUS_REG_STRUCT modbus_table_discret_out[SIZE_MODBUS_TABLE_DISCRET_REMOUTE]; //registers 1-9999 modbus RTU - -#pragma DATA_SECTION(modbus_table_analog_in, ".logs"); -MODBUS_REG_STRUCT modbus_table_analog_in[SIZE_MODBUS_ANALOG_REMOUTE]; //registers 30001-39999 modbus RTU - -#pragma DATA_SECTION(modbus_table_analog_out, ".logs"); -//MODBUS_REG_STRUCT modbus_table_analog_out[700]; //registers 40001-49999 modbus RTU -MODBUS_REG_STRUCT modbus_table_analog_out[SIZE_MODBUS_ANALOG_REMOUTE]; //registers 40001-49999 modbus RTU - -//#pragma DATA_SECTION(modbus_table_analog_out2, ".logs"); -//MODBUS_REG_STRUCT modbus_table_analog_out[700]; //registers 40001-49999 modbus RTU -//MODBUS_REG_STRUCT modbus_table_analog_out2[SIZE_MODBUS_ANALOG_REMOUTE]; //registers 40001-49999 modbus RTU - - -//unsigned int flag_waiting_answer = 1; -//unsigned int flag_message_sent = 0; - - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -void clear_table_remoute(void) -{ - int i; - - for (i=0;i= 0 && adres < SIZE_MODBUS_TABLE_DISCRET_BITS) - { -// word_number = (adres % 16 == 0) && (adres != 0) ? adres / 16 + 1 : -// adres / 16; - word_number = (adres % 16 == 0) && (adres != 0) ? adres / 16 : - adres / 16; - bit_number = adres % 16; - - if (word_number= 0 && adres < SIZE_MODBUS_TABLE_DISCRET_BITS) { - word_number = adres / 16; - bit_number = adres % 16; - return (modbus_table_discret_out[word_number].all >> bit_number) & 1; - } - - return 0; -} - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -int readDiscreteOutputsFromRemote() -{ - int succed = 0; - static unsigned int time_tick_modbus = 0; - static unsigned int old_PWM_ticks = 0; - static unsigned int count_write_to_modbus = 0; - static int cur_position_buf_modbus1 = 0; - - ModbusRTUsetDiscretDataArray(modbus_table_discret_out, modbus_table_discret_out); - -// if (global_time.pwm_tics != old_PWM_ticks) -// { -// if (global_time.pwm_tics > old_PWM_ticks) -// time_tick_modbus = time_tick_modbus + (global_time.pwm_tics - old_PWM_ticks); -// else -// time_tick_modbus++; -// } -// old_PWM_ticks = global_time.pwm_tics; -// if (TIME_PAUSE_MODBUS < time_tick_modbus) -// { - if (!rs_b.flag_LEADING) - { -// time_tick_modbus = 0; - - if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0) - cur_position_buf_modbus1 = cur_position_buf_modbus1 + SIZE_BUF_WRITE_TO_MODBUS1_REMOUTE; - - if (cur_position_buf_modbus1 >= SIZE_MODBUS_TABLE_DISCRET_REMOUTE) - cur_position_buf_modbus1 = 0; - - if ((cur_position_buf_modbus1 + SIZE_BUF_WRITE_TO_MODBUS1_REMOUTE) > SIZE_MODBUS_TABLE_DISCRET_REMOUTE) - count_write_to_modbus = SIZE_MODBUS_TABLE_DISCRET_REMOUTE - cur_position_buf_modbus1; - else - count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS1_REMOUTE; - - ModbusRTUsend1(&rs_b, 2, - ADR_MODBUS_TABLE_REMOUTE + cur_position_buf_modbus1, - count_write_to_modbus); - succed = 1; - // control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = 1; - } -// } - return succed; -} - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -int writeSigleDiscreteDataToRemote(unsigned int adres) -{ - ModbusRTUsetDiscretDataArray(modbus_table_discret_out, modbus_table_discret_out); - if (!rs_b.flag_LEADING && !control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] && - (adres < SIZE_MODBUS_TABLE_DISCRET_BITS)) { - ModbusRTUsend5(&rs_b, 2, ADR_MODBUS_TABLE_REMOUTE + adres); - return 1; - } - return 0; -} - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -int writeSingleAnalogOutputToRemote(unsigned int adres) -{ - ModbusRTUsetDataArrays(modbus_table_analog_in, modbus_table_analog_out); - if (!rs_b.flag_LEADING && !control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] && - (adres < SIZE_MODBUS_ANALOG_REMOUTE)) { - ModbusRTUsend6(&rs_b, 2, ADR_MODBUS_TABLE_REMOUTE + adres); - return 1; - } - return 0; -} - - - - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -int writeDiscreteDataToRemote() -{ - int succed = 0; - static unsigned int old_time = 0; - - static unsigned int count_write_to_modbus = 0; - static int cur_position_buf_modbus15 = 0; - - //ModbusRTUsetDiscretDataArray(modbus_table_discret_out, modbus_table_discret_out); - ModbusRTUsetDiscretDataArray(modbus_table_discret_in, modbus_table_discret_out); - - - if (!rs_b.flag_LEADING) - { - -// if (rs_b.RS_DataReadyAnswerAnalyze) -// { -// cur_position_buf_modbus15 += SIZE_BUF_WRITE_TO_MODBUS15_REMOUTE; -// } - - if (cur_position_buf_modbus15 >= (SIZE_MODBUS_TABLE_DISCRET_REMOUTE)) - cur_position_buf_modbus15 = 0; - - if ((cur_position_buf_modbus15 + SIZE_BUF_WRITE_TO_MODBUS15_REMOUTE) > (SIZE_MODBUS_TABLE_DISCRET_REMOUTE)) - count_write_to_modbus = SIZE_MODBUS_TABLE_DISCRET_REMOUTE - cur_position_buf_modbus15; - else - count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS15_REMOUTE; - - // count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS16; - // cur_position_buf_modbus=0; - - ModbusRTUsend15(&rs_b, 2, ADR_MODBUS_TABLE_REMOUTE + cur_position_buf_modbus15*16, - count_write_to_modbus*16); -// ModbusRTUsend15(&rs_a, 2, ADR_MODBUS_TABLE_REMOUTE + cur_position_buf_modbus15*16, -// count_write_to_modbus*16); - - cur_position_buf_modbus15 += SIZE_BUF_WRITE_TO_MODBUS15_REMOUTE; - - // control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = 1; - // control_station.count_error_modbus_15[CONTROL_STATION_INGETEAM_PULT_RS485]++; - - // hmi_watch_dog = !hmi_watch_dog; //was transmitted, need to change - succed = 1; - - } - return succed; -} - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -#define ADRESS_END_FIRST_BLOCK 20 -#define ADRESS_START_PROTECTION_LEVELS 91 - -int readAnalogDataFromRemote() -{ - int succed = 0; - static unsigned int old_time = 0; - - static unsigned int count_write_to_modbus = 0; - static int cur_position_buf_modbus3 = 0, size_buf = SIZE_BUF_READ_FROM_MODBUS16_REMOUTE; - - - ModbusRTUsetDataArrays(modbus_table_analog_in, modbus_table_analog_out); - - - - if (!rs_b.flag_LEADING) - { - - if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0) - cur_position_buf_modbus3 = cur_position_buf_modbus3 + size_buf; - - if (cur_position_buf_modbus3 >= SIZE_ANALOG_DATA_FROM_MODBUS16_REMOUTE) - cur_position_buf_modbus3 = 0; - // . . - if ((cur_position_buf_modbus3 > ADRESS_END_FIRST_BLOCK) && - (cur_position_buf_modbus3 < ADRESS_START_PROTECTION_LEVELS)) { - cur_position_buf_modbus3 = ADRESS_START_PROTECTION_LEVELS; - } - if((cur_position_buf_modbus3 < ADRESS_END_FIRST_BLOCK) && - (cur_position_buf_modbus3 + size_buf) > ADRESS_END_FIRST_BLOCK) { - count_write_to_modbus = ADRESS_END_FIRST_BLOCK - cur_position_buf_modbus3; - } - - if ((cur_position_buf_modbus3 + size_buf) > SIZE_ANALOG_DATA_FROM_MODBUS16_REMOUTE) - count_write_to_modbus = SIZE_ANALOG_DATA_FROM_MODBUS16_REMOUTE - cur_position_buf_modbus3; - else - count_write_to_modbus = size_buf; - - // count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS16; - // cur_position_buf_modbus=0; - - - // SendCommandModbus3(&rs_b,1,ADR_MODBUS_TABLE+cur_position_buf_modbus3,count_write_to_modbus); - ModbusRTUsend4(&rs_b, 2, - ADR_MODBUS_TABLE_REMOUTE + cur_position_buf_modbus3, - count_write_to_modbus); - // control_station.count_error_modbus_4[CONTROL_STATION_INGETEAM_PULT_RS485]++; - - // control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = 1; - succed = 1; - } - return succed; -} - - -int writeSingleAnalogDataToRemote(int from_adr, int count_wr) -{ - int succed = 0; - static unsigned int old_time = 0; - - static int count_write_to_modbus = 0; - static int cur_position_buf_modbus16 = 0; - - - ModbusRTUsetDataArrays(modbus_table_analog_in, modbus_table_analog_out); - - - if (!rs_b.flag_LEADING) - { - cur_position_buf_modbus16 = from_adr; - -// if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0) -// cur_position_buf_modbus16 = cur_position_buf_modbus16 + SIZE_BUF_WRITE_TO_MODBUS16_REMOUTE; -// -// if (cur_position_buf_modbus16 >= SIZE_ANALOG_DATA_REMOUTE) -// cur_position_buf_modbus16 = 0; - -// // . . -// if ((cur_position_buf_modbus16 > ADRESS_END_FIRST_BLOCK) && -// (cur_position_buf_modbus16 < ADRESS_START_PROTECTION_LEVELS)) { -// cur_position_buf_modbus16 = ADRESS_START_PROTECTION_LEVELS; -// } - - if ((cur_position_buf_modbus16 + count_wr) > SIZE_ANALOG_DATA_REMOUTE) - count_write_to_modbus = SIZE_ANALOG_DATA_REMOUTE - cur_position_buf_modbus16; - else - count_write_to_modbus = count_wr; - - ModbusRTUsend16(&rs_b, 2, - ADR_MODBUS_TABLE_REMOUTE + cur_position_buf_modbus16, - count_write_to_modbus); - - succed = 1; - } - return succed; -} - - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -int writeAnalogDataToRemote() -{ - int succed = 0; - static unsigned int old_time = 0; - - static int count_write_to_modbus = 0; - static int cur_position_buf_modbus16 = 0; - - - ModbusRTUsetDataArrays(modbus_table_analog_in, modbus_table_analog_out); - - - if (!rs_b.flag_LEADING) - { - if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485] == 0) - cur_position_buf_modbus16 = cur_position_buf_modbus16 + SIZE_BUF_WRITE_TO_MODBUS16_REMOUTE; - - if (cur_position_buf_modbus16 >= SIZE_ANALOG_DATA_REMOUTE) - cur_position_buf_modbus16 = 0; - -// // . . -// if ((cur_position_buf_modbus16 > ADRESS_END_FIRST_BLOCK) && -// (cur_position_buf_modbus16 < ADRESS_START_PROTECTION_LEVELS)) { -// cur_position_buf_modbus16 = ADRESS_START_PROTECTION_LEVELS; -// } - - if ((cur_position_buf_modbus16 + SIZE_BUF_WRITE_TO_MODBUS16_REMOUTE) > SIZE_ANALOG_DATA_REMOUTE) - count_write_to_modbus = SIZE_ANALOG_DATA_REMOUTE - cur_position_buf_modbus16; - else - count_write_to_modbus = SIZE_BUF_WRITE_TO_MODBUS16_REMOUTE; - - ModbusRTUsend16(&rs_b, 2, - ADR_MODBUS_TABLE_REMOUTE + cur_position_buf_modbus16, - count_write_to_modbus); - // control_station.count_error_modbus_16[CONTROL_STATION_INGETEAM_PULT_RS485]++; - - // control_station.flag_message_sent[CONTROL_STATION_INGETEAM_PULT_RS485] = 1; - succed = 1; - } - return succed; -} - - - - - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// - - - diff --git a/Inu/Src2/551/main/modbus_hmi.h b/Inu/Src2/551/main/modbus_hmi.h deleted file mode 100644 index 887b537..0000000 --- a/Inu/Src2/551/main/modbus_hmi.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef _MODBUS_HMI -#define _MODBUS_HMI - -#include "modbus_struct.h" - - -int readDiscreteOutputsFromRemote(); -int writeSigleDiscreteDataToRemote(unsigned int adres); -int writeSingleAnalogOutputToRemote(unsigned int adres); -int writeDiscreteDataToRemote(); -int readAnalogDataFromRemote(); -int writeAnalogDataToRemote(); -int writeSingleAnalogDataToRemote(int from_adr, int count_wr); - -void setRegisterDiscreteOutput(int value, int adres); -int getRegisterDiscreteOutput(int adres); - - - - -void clear_table_remoute(void); // clear table - -#define ADRES_LOG_REGISTERS 100 - - -#define SIZE_MODBUS_TABLE_DISCRET_REMOUTE 36 -#define SIZE_MODBUS_TABLE_DISCRET_BITS (SIZE_MODBUS_TABLE_DISCRET_REMOUTE * 16) -#define SIZE_MODBUS_ANALOG_REMOUTE 900 - - -extern MODBUS_REG_STRUCT modbus_table_analog_in[SIZE_MODBUS_ANALOG_REMOUTE]; -extern MODBUS_REG_STRUCT modbus_table_analog_out[SIZE_MODBUS_ANALOG_REMOUTE]; -extern MODBUS_REG_STRUCT modbus_table_discret_in[SIZE_MODBUS_TABLE_DISCRET_REMOUTE]; -extern MODBUS_REG_STRUCT modbus_table_discret_out[SIZE_MODBUS_TABLE_DISCRET_REMOUTE]; - -//extern unsigned int flag_waiting_answer; -//extern unsigned int flag_message_sent; - -#endif //_MODBUS_HMI diff --git a/Inu/Src2/551/main/modbus_hmi_read.c b/Inu/Src2/551/main/modbus_hmi_read.c deleted file mode 100644 index 9718943..0000000 --- a/Inu/Src2/551/main/modbus_hmi_read.c +++ /dev/null @@ -1,222 +0,0 @@ -/* - * modbus_hmi_read.c - * - * Created on: 21 . 2020 . - * Author: star - */ -#include -#include -#include -#include -#include - - -#define DELTA_ABNORMAL_P_WATER_MIN 50 -#define DELTA_ABNORMAL_P_WATER_MAX 100 - -void parse_protect_levels_HMI() { - - if (modbus_table_analog_in[91].all > 0) { - protect_levels.abnormal_temper_acdrive_winding_U1 = modbus_table_analog_in[91].all * 10; - } - if(modbus_table_analog_in[92].all > 0) { - protect_levels.abnormal_temper_acdrive_winding_V1 = modbus_table_analog_in[92].all * 10; - } - if(modbus_table_analog_in[93].all > 0) { - protect_levels.abnormal_temper_acdrive_winding_W1 = modbus_table_analog_in[93].all * 10; - } - if (modbus_table_analog_in[94].all > 0) { - protect_levels.abnormal_temper_acdrive_winding_U2 = modbus_table_analog_in[94].all * 10; - } - if (modbus_table_analog_in[95].all > 0) { - protect_levels.abnormal_temper_acdrive_winding_V2 = modbus_table_analog_in[95].all * 10; - } - if (modbus_table_analog_in[96].all > 0) { - protect_levels.abnormal_temper_acdrive_winding_W2 = modbus_table_analog_in[96].all * 10; - } - if (modbus_table_analog_in[97].all > 0) { - protect_levels.abnormal_temper_acdrive_bear_DNE = modbus_table_analog_in[97].all * 10; - } - if(modbus_table_analog_in[98].all > 0) { - protect_levels.abnormal_temper_acdrive_bear_NE = modbus_table_analog_in[98].all * 10; - } - - if (modbus_table_analog_in[99].all > 0) { - protect_levels.alarm_temper_acdrive_winding_U1 = modbus_table_analog_in[99].all * 10; - } - if (modbus_table_analog_in[100].all > 0) { - protect_levels.alarm_temper_acdrive_winding_V1 = modbus_table_analog_in[100].all * 10; - } - if (modbus_table_analog_in[101].all > 0) { - protect_levels.alarm_temper_acdrive_winding_W1 = modbus_table_analog_in[101].all * 10; - } - if (modbus_table_analog_in[102].all > 0) { - protect_levels.alarm_temper_acdrive_winding_U2 = modbus_table_analog_in[102].all * 10; - } - if (modbus_table_analog_in[103].all > 0) { - protect_levels.alarm_temper_acdrive_winding_V2 = modbus_table_analog_in[103].all * 10; - } - if (modbus_table_analog_in[104].all > 0) { - protect_levels.alarm_temper_acdrive_winding_W2 = modbus_table_analog_in[104].all * 10; - } - if (modbus_table_analog_in[105].all > 0) { - protect_levels.alarm_temper_acdrive_bear_DNE = modbus_table_analog_in[105].all * 10; - } - if (modbus_table_analog_in[106].all > 0) { - protect_levels.alarm_temper_acdrive_bear_NE = modbus_table_analog_in[106].all * 10; - } - - if (modbus_table_analog_in[107].all > 0) { - protect_levels.abnormal_temper_u_01 = modbus_table_analog_in[107].all * 10; - } - if (modbus_table_analog_in[108].all > 0) { - protect_levels.abnormal_temper_u_02 = modbus_table_analog_in[108].all * 10; - } - if (modbus_table_analog_in[109].all > 0) { - protect_levels.abnormal_temper_u_03 = modbus_table_analog_in[109].all * 10; - } - if (modbus_table_analog_in[110].all > 0) { - protect_levels.abnormal_temper_u_04 = modbus_table_analog_in[110].all * 10; - } - if (modbus_table_analog_in[111].all > 0) { - protect_levels.abnormal_temper_u_05 = modbus_table_analog_in[111].all * 10; - } - if (modbus_table_analog_in[112].all > 0) { - protect_levels.abnormal_temper_u_06 = modbus_table_analog_in[112].all * 10; - } - if (modbus_table_analog_in[113].all > 0) { - protect_levels.abnormal_temper_u_07 = modbus_table_analog_in[113].all * 10; - } - if (modbus_table_analog_in[114].all > 0) { - protect_levels.alarm_temper_u_01 = modbus_table_analog_in[114].all * 10; - } - if (modbus_table_analog_in[115].all > 0) { - protect_levels.alarm_temper_u_02 = modbus_table_analog_in[115].all * 10; - } - if (modbus_table_analog_in[116].all > 0) { - protect_levels.alarm_temper_u_03 = modbus_table_analog_in[116].all * 10; - } - if (modbus_table_analog_in[117].all > 0) { - protect_levels.alarm_temper_u_04 = modbus_table_analog_in[117].all * 10; - } - if (modbus_table_analog_in[118].all > 0) { - protect_levels.alarm_temper_u_05 = modbus_table_analog_in[118].all * 10; - } - if (modbus_table_analog_in[119].all > 0) { - protect_levels.alarm_temper_u_06 = modbus_table_analog_in[119].all * 10; - } - if (modbus_table_analog_in[120].all > 0) { - protect_levels.alarm_temper_u_07 = modbus_table_analog_in[120].all * 10; - } - - if (modbus_table_analog_in[123].all > 0) { - protect_levels.abnormal_temper_water_int = modbus_table_analog_in[123].all * 10; - } - if (modbus_table_analog_in[124].all > 0) { - protect_levels.abnormal_temper_water_ext = modbus_table_analog_in[124].all * 10; - } - if (modbus_table_analog_in[125].all > 0) { - protect_levels.alarm_p_water_min_int = modbus_table_analog_in[125].all * 10; - protect_levels.abnormal_p_water_min_int = protect_levels.alarm_p_water_min_int + DELTA_ABNORMAL_P_WATER_MIN; - } - if (modbus_table_analog_in[126].all > 0) { - protect_levels.alarm_temper_water_int = modbus_table_analog_in[126].all * 10; - } - if (modbus_table_analog_in[127].all > 0) { - protect_levels.alarm_temper_water_ext = modbus_table_analog_in[127].all * 10; - } - if (modbus_table_analog_in[128].all > 0) { - protect_levels.alarm_p_water_max_int = modbus_table_analog_in[128].all * 10; - protect_levels.abnormal_p_water_max_int = protect_levels.alarm_p_water_max_int - DELTA_ABNORMAL_P_WATER_MAX; - } - - if (modbus_table_analog_in[129].all > 0) { - protect_levels.abnormal_temper_air_int_01 = modbus_table_analog_in[129].all * 10; - } - if (modbus_table_analog_in[130].all > 0) { - protect_levels.abnormal_temper_air_int_02 = modbus_table_analog_in[130].all * 10; - } - if (modbus_table_analog_in[131].all > 0) { - protect_levels.abnormal_temper_air_int_03 = modbus_table_analog_in[131].all * 10; - } - if (modbus_table_analog_in[132].all > 0) { - protect_levels.abnormal_temper_air_int_04 = modbus_table_analog_in[132].all * 10; - } - if (modbus_table_analog_in[133].all > 0) { - protect_levels.alarm_temper_air_int_01 = modbus_table_analog_in[133].all * 10; - } - if (modbus_table_analog_in[134].all > 0) { - protect_levels.alarm_temper_air_int_02 = modbus_table_analog_in[134].all * 10; - } - if (modbus_table_analog_in[135].all > 0) { - protect_levels.alarm_temper_air_int_03 = modbus_table_analog_in[135].all * 10; - } - if (modbus_table_analog_in[136].all > 0) { - protect_levels.alarm_temper_air_int_04 = modbus_table_analog_in[136].all * 10; - } - - if (modbus_table_analog_in[137].all > 0) { - edrk.iqMIN_U_IN = _IQ(((float)modbus_table_analog_in[137].all) / NORMA_ACP); - } - if (modbus_table_analog_in[138].all > 0) { - edrk.iqMIN_U_IN = _IQ(((float)modbus_table_analog_in[138].all) / NORMA_ACP); - } - - if (modbus_table_analog_in[139].all > 0) { - edrk.iqMIN_U_ZPT = _IQ(((float)modbus_table_analog_in[139].all) / NORMA_ACP); - } - if (modbus_table_analog_in[140].all > 0) { - edrk.iqMIN_U_ZPT = _IQ(((float)modbus_table_analog_in[140].all) / NORMA_ACP); - } - - if (modbus_table_analog_in[142].all > 0) { - edrk.iqMAX_U_IN = _IQ(((float)modbus_table_analog_in[142].all) / NORMA_ACP); - } - if (modbus_table_analog_in[143].all > 0) { - edrk.iqMAX_U_IN = _IQ(((float)modbus_table_analog_in[143].all) / NORMA_ACP); - } - - if (modbus_table_analog_in[144].all > 0) { - edrk.iqMAX_U_ZPT = _IQ(((float)modbus_table_analog_in[144].all) / NORMA_ACP); - } - if (modbus_table_analog_in[145].all > 0) { - edrk.iqMAX_U_ZPT = _IQ(((float)modbus_table_analog_in[145].all) / NORMA_ACP); - } - - if (modbus_table_analog_in[146].all > 0) { - protect_levels.alarm_Izpt_max = modbus_table_analog_in[146].all; - } - - if (modbus_table_analog_in[155].all > 0) { - protect_levels.alarm_Imax_U01 = modbus_table_analog_in[155].all; - } - if (modbus_table_analog_in[156].all > 0) { - protect_levels.alarm_Imax_U02 = modbus_table_analog_in[156].all; - } - if (modbus_table_analog_in[157].all > 0) { - protect_levels.alarm_Imax_U03 = modbus_table_analog_in[157].all; - } - if (modbus_table_analog_in[158].all > 0) { - protect_levels.alarm_Imax_U04 = modbus_table_analog_in[158].all; - } - if (modbus_table_analog_in[159].all > 0) { - protect_levels.alarm_Imax_U05 = modbus_table_analog_in[159].all; - } - if (modbus_table_analog_in[160].all > 0) { - protect_levels.alarm_Imax_U06 = modbus_table_analog_in[160].all; - } - if (modbus_table_analog_in[161].all > 0) { - protect_levels.alarm_Imax_U07 = modbus_table_analog_in[161].all; - } - if (modbus_table_analog_in[162].all > 0) { - protect_levels.alarm_Iged_max = modbus_table_analog_in[162].all; - } - - - - - -} - - - diff --git a/Inu/Src2/551/main/modbus_hmi_read.h b/Inu/Src2/551/main/modbus_hmi_read.h deleted file mode 100644 index 7f3e0db..0000000 --- a/Inu/Src2/551/main/modbus_hmi_read.h +++ /dev/null @@ -1,15 +0,0 @@ -/* - * modbus_hmi_read.h - * - * Created on: 21 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_MODBUS_HMI_READ_H_ -#define SRC_MAIN_MODBUS_HMI_READ_H_ - - -void parse_protect_levels_HMI(void); - - -#endif /* SRC_MAIN_MODBUS_HMI_READ_H_ */ diff --git a/Inu/Src2/551/main/modbus_hmi_update.c b/Inu/Src2/551/main/modbus_hmi_update.c deleted file mode 100644 index 7c1e83c..0000000 --- a/Inu/Src2/551/main/modbus_hmi_update.c +++ /dev/null @@ -1,2022 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "edrk_main.h" -#include "global_time.h" -#include "control_station.h" -#include "CAN_Setup.h" -#include "global_time.h" -#include "RS_Functions.h" -#include "mathlib.h" -#include "logs_hmi.h" -#include "detect_errors.h" -/* -#include "mathlib.h" -#include -#include "IQmathLib.h" -*/ -int hmi_watch_dog = 0; -int prev_kvitir = 0; -int prev_sbor = 0; -int kvitir1 = 0; -int sbor1 = 0; -int razbor1 = 0; - -//30001 ResetErrors command to controller to reset errors 1-reset -//30002 SchemeAssemble Command to change scheme state 0-scheme dissasemble 1- assemble -//30003 IsPowerSetMode 0-control enigine by turnovers, 1- by power -//30004 SpecifiedPower Power set by user -//30005 SpecifiedTurnovers Turnovers set by user - -//30006 UserValueUpdated command to controller to update set value 1-ative - -//30007 ReportGet Command to get report 1-get 0- nothinhg -//30008 ReportArraySaved Sets to 1 when HMI is ready to get array(part of report) -//30009 PumpsControlMode Pumps Control mode. 0 = auto, 1= pump 1, 2= pump 2 - -//30010 MotoHoursPanel () -//30011 MotoHoursFan1 1 () -//30012 MotoHoursFan2 2 () -//30013 MotoHoursPump1 () -//30014 MotoHoursPump2 () -//30015 MotoHoursInvertorCharged "" "" () -//30016 MotoHoursInvertorGo """" () -//30017 MotoHoursInvertorGoFault "" "" () -//30018 MotoHoursInvertorAlarm """" () - -#define COUNT_ANALOG_DATA_FROM_INGETEAM SIZE_ANALOG_DATA_REMOUTE //(18+1) -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -void func_unpack_answer_from_Ingeteam(unsigned int cc) -{ - // y - unsigned int DataOut; - int Data, Data1, Data2, DataAnalog1, DataAnalog2, DataAnalog3, DataAnalog4, i; - unsigned int h; - volatile unsigned char *pByte; - // static int vs11,vs12,vs1; - // static int DataCnt=0; - // int GoT,Assemble_scheme; - // static int prev_temp_Rele1=0, temp_Rele1=0, prev_temp_Rele2=0, temp_Rele2=0; - - static int flag_prev_turn_on = 0; - static int flag_prev_turn_off = 0; - static int prev_byte01_bit4 = 0; - static int prev_byte01_bit1 = 0; - static int flag_wait_revers_sbor = 1; - static int flag_wait_revers_go = 1; - - static unsigned int count_transmited = 0; - - - // y - // - - if (COUNT_ANALOG_DATA_FROM_INGETEAM > CONTROL_STATION_MAX_RAW_DATA) - xerror(main_er_ID(2),(void *)0); - - for (h=1;h=0 && prev_send_log != edrk.pult_cmd.send_log) - { - if (edrk.pult_cmd.send_log) - log_to_HMI.send_log = edrk.pult_cmd.send_log; - - log_to_HMI.sdusb = edrk.pult_cmd.sdusb; - - } -// else -// log_to_HMI.send_log = 0; - - prev_send_log = edrk.pult_cmd.send_log; - - ///////////////// - ///////////////// - ///////////////// - - control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MODE_PUMP] = edrk.pult_cmd.pump_mode; - - -// -// control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_PUMP] = modbus_table_analog_in[188].all; -// control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_QTV] = modbus_table_analog_in[189].all; -// control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_DISABLE_ON_UMP] = modbus_table_analog_in[190].all; -// control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_ENABLE_ON_CHARGE] = modbus_table_analog_in[191].all; -// control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_MANUAL_DISCHARGE] = modbus_table_analog_in[180].all; -// control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_GO] = !modbus_table_analog_in[192].all; -// -// control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_] = modbus_table_analog_in[188].all; - - ///////////////// - ///////////////// - ///////////////// - ///////////////// - - parse_protect_levels_HMI(); - -} - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// - -int update_progress_load_hmi(int proc_load) -{ - static unsigned int old_time_5 = 0; - volatile int perc_load=0, final_code = 0, c_l = 0; - -// return 0; - - update_tables_HMI_on_inited(proc_load); - - - old_time_5 = global_time.miliseconds; - do - { - - if (final_code >= 4) - { - return 1; - } - -// if (control_station.flag_waiting_answer[CONTROL_STATION_INGETEAM_PULT_RS485]==0) -// final_code = modbusNetworkSharing(0); -// else - final_code = modbusNetworkSharing(1); - -// RS232_WorkingWith(0,1,0); - - } - while (detect_pause_milisec(1500, &old_time_5)==0);//(100,&old_time)); - - return 0; - - - -} - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// - - -void update_tables_HMI_on_inited(int perc_load) -{ - Inverter_state state; - static int nn=0, ss=0; - static int prev_edrk_KVITIR=0; - int i,status; - -// log_to_HMI.send_log = modbus_table_analog_in[7].all; - //setRegisterDiscreteOutput(log_to_HMI.flag_log_array_ready_sent, 310); - - // LoadMode Read 00544 00544 - setRegisterDiscreteOutput(0, 544);// - - // Loading ReadWrite 30088 40088 0-10 - modbus_table_analog_out[88].all = perc_load; - - // - modbus_table_analog_out[4].all++; - - - // build version - modbus_table_analog_out[219].all = edrk.buildYear; - modbus_table_analog_out[220].all = edrk.buildMonth; - modbus_table_analog_out[221].all = edrk.buildDay; - - -} - -/////////////////////////////////////////////////// -/// -/////////////////////////////////////////////////// -void update_tables_HMI_discrete(void) -{ - int real_box; - // !!! - // if (edrk.from_display.bits.KVITIR) - // setRegisterDiscreteOutput(edrk.from_display.bits.KVITIR, 301); - setRegisterDiscreteOutput(control_station.array_cmd[CONTROL_STATION_INGETEAM_PULT_RS485][CONTROL_STATION_CMD_CHECKBACK], 513); - // prev_edrk_KVITIR = edrk.from_display.bits.KVITIR; - ///// - - //setRegisterDiscreteOutput(edrk.RemouteFromDISPLAY, 302); - setRegisterDiscreteOutput(control_station.active_control_station[CONTROL_STATION_INGETEAM_PULT_RS485], 514); - - setRegisterDiscreteOutput(hmi_watch_dog, 515); - - - setRegisterDiscreteOutput(edrk.StatusFunAll, 516); - setRegisterDiscreteOutput(edrk.StatusFunAll, 517); - - setRegisterDiscreteOutput(edrk.StatusPump0, 518); - setRegisterDiscreteOutput(edrk.StatusPump1, 519); - - setRegisterDiscreteOutput(edrk.from_shema_filter.bits.SVU,524); - setRegisterDiscreteOutput(edrk.from_shema_filter.bits.ZADA_DISPLAY,525); - // ___ Read 00523 - // ___ Read 00524 - // __ Read 00525 - - - setRegisterDiscreteOutput(edrk.from_ing1.bits.OHLAD_UTE4KA_WATER, 526);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.NASOS_NORMA, 527);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.OP_PIT_NORMA, 528);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.UPC_24V_NORMA, 529);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.ALL_KNOPKA_AVARIA, 530);// - setRegisterDiscreteOutput(edrk.SumSbor, 531);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.ZARYAD_ON, 532);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.VIPR_PREDOHR_NORMA, 533);// - setRegisterDiscreteOutput(!edrk.temper_limit_koeffs.code_status, 534);// - // setRegisterDiscreteOutput(1, 331);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.ZAZEML_ON, 535);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.NAGREV_ON, 536);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.BLOCK_IZOL_NORMA, 537);// - setRegisterDiscreteOutput(edrk.from_ing1.bits.BLOCK_IZOL_AVARIA, 538);// - - ////////////// - // schemeStateOnController ReadWrite 00539 00539 0 - , 1- - // StateAnotherPowerChannel Read 00540 00540 : 0 - , 1 - // InterfaceOpticalBus Read 00541 00541 : 0 - , 1 - - // StateDriver Read 00542 00542 : 0 - , 1 - - // NumberPowerChannel Read 00543 00543 : 0 - , 1 - - - - setRegisterDiscreteOutput(edrk.Status_Ready.bits.ready_final, 539); - setRegisterDiscreteOutput(edrk.errors.e7.bits.ANOTHER_BS_ALARM, 540); - setRegisterDiscreteOutput(optical_read_data.status == 1 && optical_write_data.status == 1 ? 1 : 0, 541); - setRegisterDiscreteOutput(edrk.Status_Rascepitel_Ok, 542); - - if (edrk.flag_second_PCH==0) - setRegisterDiscreteOutput(0, 543); - else - setRegisterDiscreteOutput(1, 543); - - // LoadMode Read 00544 00544 - setRegisterDiscreteOutput(1, 544); // - - // Loading ReadWrite 30088 40088 0-10 - - setRegisterDiscreteOutput(control_station.active_array_cmd[CONTROL_STATION_CMD_BLOCK_BS] - || edrk.from_shema.bits.SVU_BLOCK_QTV, 545); - ////////////// - - - - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk0_ack, 17);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk1_ack, 18);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk2_ack, 19);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk3_ack, 20);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk4_ack, 21);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk5_ack, 22);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk6_ack, 23);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk7_ack, 24);// - - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk0_ack, 25);// - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk1_ack, 26);// - ///////////////////// - if (edrk.flag_second_PCH==0) - setRegisterDiscreteOutput(edrk.errors.e4.bits.ANOTHER_BS_POWER_OFF, 27); - else - setRegisterDiscreteOutput(edrk.errors.e4.bits.ANOTHER_BS_POWER_OFF, 28); - - - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk0_current, 33);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk1_current, 34);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk2_current, 35);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk3_current, 36);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk4_current, 37);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk5_current, 38);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk6_current, 39);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk7_current, 40);// - - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk0_current, 41);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk1_current, 42);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk2_current, 43);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk3_current, 44);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk4_current, 45);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk5_current, 46);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk6_current, 47);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk7_current, 48);// - - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk0_current, 49);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk1_current, 50);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk2_current, 51);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk3_current, 52);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk4_current, 53);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk5_current, 54);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk6_current, 55);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk7_current, 56);// - - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk0_current, 57);// - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk1_current, 58);// - ////////////////////////// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 65);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 66);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 67);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 68);// - - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 69);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 70);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 71);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 72);// - - - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 73);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 74);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 75);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 76);// - - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 77);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 78);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 79);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 80);// - - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 81);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 82);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 83);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 84);// - - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 85);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 86);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 87);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654, 88);// - - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 89);// - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.lock_status_error.bit.mintime_err_keys_3210, 90);// - - ///////////////// - - - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_TK_0, 97);// - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_TK_1, 98);// - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_TK_2, 99);// - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_TK_3, 100);// - - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_IN_0, 101);// - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_IN_1, 102);// - - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_OUT_0, 103);// - // setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_OUT_1, 105);// - - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_ADC_0, 104);// - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_HWP_0, 105);// - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_ADC_1, 106);// - - setRegisterDiscreteOutput(edrk.errors.e3.bits.NOT_READY_CONTR, 107);// - - - /////////////////// - - - setRegisterDiscreteOutput(edrk.errors.e5.bits.KEY_AVARIA, 113);// - - setRegisterDiscreteOutput(edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER, 114); - setRegisterDiscreteOutput(edrk.errors.e7.bits.SVU_BLOCK_ON_QTV - || control_station.active_array_cmd[CONTROL_STATION_CMD_BLOCK_BS], 115); - setRegisterDiscreteOutput(edrk.errors.e7.bits.UMP_NOT_ANSWER, 116); - setRegisterDiscreteOutput(edrk.errors.e7.bits.UMP_NOT_READY, 117); - setRegisterDiscreteOutput(edrk.errors.e6.bits.RASCEPITEL_ERROR_NOT_ANSWER, 118); - setRegisterDiscreteOutput(edrk.errors.e7.bits.ANOTHER_RASCEPITEL_ON, 119); - - setRegisterDiscreteOutput(edrk.errors.e7.bits.AUTO_SET_MASTER, 120); - setRegisterDiscreteOutput(edrk.errors.e7.bits.ANOTHER_PCH_NOT_ANSWER, 121); - setRegisterDiscreteOutput(edrk.errors.e8.bits.WDOG_OPTICAL_BUS, 122); - setRegisterDiscreteOutput(edrk.errors.e2.bits.ERROR_RAZBOR_SHEMA, 123); - - setRegisterDiscreteOutput(edrk.errors.e1.bits.NO_CONFIRM_ON_RASCEPITEL, 124); - - setRegisterDiscreteOutput(edrk.errors.e1.bits.ANOTHER_BS_NOT_ON_RASCEPITEL, 125); - setRegisterDiscreteOutput(edrk.errors.e1.bits.ANOTHER_BS_VERY_LONG_WAIT, 126); - setRegisterDiscreteOutput(edrk.errors.e1.bits.VERY_LONG_BOTH_READY2, 127); - setRegisterDiscreteOutput(edrk.errors.e1.bits.BOTH_KEYS_CHARGE_DISCHARGE, 128); - - // setRegisterDiscreteOutput(edrk.errors.e5.bits.OP_PIT, 115);// - // setRegisterDiscreteOutput(edrk.errors.e5.bits.POWER_UPC, 116);// - /////////////////// - - setRegisterDiscreteOutput(!control_station.alive_control_station[CONTROL_STATION_ZADATCHIK_CAN], 129); - setRegisterDiscreteOutput(!control_station.alive_control_station[CONTROL_STATION_MPU_SVU_CAN], 130); - // setRegisterDiscreteOutput(CAN_timeout[get_real_in_mbox(UNITS_TYPE_BOX,UMU_CAN_DEVICE)], 131); - real_box = get_real_in_mbox(UNITS_TYPE_BOX,BKSSD_CAN_DEVICE); - if (real_box != -1) - setRegisterDiscreteOutput(CAN_timeout[real_box], 132); - - real_box = get_real_in_mbox(UNITS_TYPE_BOX,VPU_CAN); - if (real_box != -1) - setRegisterDiscreteOutput(CAN_timeout[real_box], 133); - real_box = get_real_in_mbox(UNITS_TYPE_BOX,ANOTHER_BSU1_CAN_DEVICE); - if (real_box != -1) - setRegisterDiscreteOutput(CAN_timeout[real_box], 134); - setRegisterDiscreteOutput(edrk.errors.e7.bits.CAN2CAN_BS, 135); - - - if (edrk.flag_second_PCH==0) - setRegisterDiscreteOutput(edrk.warnings.e4.bits.ANOTHER_BS_POWER_OFF, 137); - else - setRegisterDiscreteOutput(edrk.warnings.e4.bits.ANOTHER_BS_POWER_OFF, 136); - - setRegisterDiscreteOutput(edrk.errors.e7.bits.ANOTHER_BS_ALARM, 138); // ; edrk.errors.e4.bits.FAST_OPTICAL_ALARM - setRegisterDiscreteOutput(edrk.warnings.e7.bits.ANOTHER_BS_ALARM, 139);// ; edrk.warnings.e4.bits.FAST_OPTICAL_ALARM - - setRegisterDiscreteOutput(edrk.warnings.e7.bits.UMP_NOT_READY, 140); - - setRegisterDiscreteOutput(edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK, 141); - setRegisterDiscreteOutput(edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK, 142); - - setRegisterDiscreteOutput(edrk.errors.e9.bits.SENSOR_ROTOR_1_2_BREAK, 143); - - - - /// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk0_ack, 145);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk1_ack, 146);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk2_ack, 147);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk3_ack, 148);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk4_ack, 149);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk5_ack, 150);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk6_ack, 151);// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk7_ack, 152);// - - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk0_ack, 153);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk1_ack, 154);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk2_ack, 155);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk3_ack, 156);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk4_ack, 157);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk5_ack, 158);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk6_ack, 159);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk7_ack, 160);// - - - - - // setRegisterDiscreteOutput(edrk.errors.e5.bits.KEY_AVARIA, 243);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.OP_PIT, 161);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.UTE4KA_WATER, 162);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.BLOCK_DOOR, 163);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.ERROR_PRE_CHARGE_ON, 164);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.FAN, 165);// - - setRegisterDiscreteOutput(edrk.errors.e5.bits.PUMP_1, 166);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.PRE_READY_PUMP, 167);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.ERROR_HEAT, 168);// - - setRegisterDiscreteOutput(edrk.errors.e5.bits.ERROR_PRED_VIPR, 170);// - - setRegisterDiscreteOutput(edrk.errors.e5.bits.ERROR_ISOLATE, 171);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.POWER_UPC, 172);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.ERROR_GROUND_NET, 173);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.PUMP_2, 174);// - setRegisterDiscreteOutput(edrk.warnings.e5.bits.ERROR_ISOLATE, 175);// - setRegisterDiscreteOutput(edrk.warnings.e5.bits.PRE_READY_PUMP, 176);// - - - /////////////////// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_1_MAX, 177);// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_2_MAX, 178);// - - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_1_MIN, 179);// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_2_MIN, 180);// - - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_A1B1_MAX, 181);// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_A2B2_MAX, 182);// - - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_B1C1_MAX, 183);// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_B2C2_MAX, 184);// - - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_A1B1_MIN, 185);// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_A2B2_MIN, 186);// - - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_B1C1_MIN, 187);// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_B2C2_MIN, 188);// - - - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_IN_MAX, 189);// - setRegisterDiscreteOutput(edrk.errors.e0.bits.U_IN_MIN, 190);// - - // setRegisterDiscreteOutput(edrk.errors.e0.bits.I_1_MAX, 191);// - // setRegisterDiscreteOutput(edrk.errors.e0.bits.I_2_MAX, 192);// - - - ////////////// - - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_UO2_MAX, 193);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_UO3_MAX, 194);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_UO4_MAX, 195);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_UO5_MAX, 196);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_UO6_MAX, 197);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_UO7_MAX, 198);// - - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_BREAK_1_MAX, 199);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.I_BREAK_2_MAX, 200);// - - // setRegisterDiscreteOutput(edrk.errors.e1.bits.HWP_ERROR, 201);// - //////////////////// - - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_AIR0_MAX, 203);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_AIR1_MAX, 204);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_AIR2_MAX, 205);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_AIR3_MAX, 206);// - - setRegisterDiscreteOutput(edrk.power_limit.bits.limit_by_temper, 207);// - setRegisterDiscreteOutput(edrk.power_limit.bits.limit_from_freq, 211);// - setRegisterDiscreteOutput(edrk.power_limit.bits.limit_from_uom_fast, 212);// - setRegisterDiscreteOutput(edrk.power_limit.bits.limit_from_SVU, 213);// - setRegisterDiscreteOutput(edrk.power_limit.bits.limit_moment, 214);// - setRegisterDiscreteOutput(edrk.power_limit.bits.limit_Iout, 216);// - - //////////////////// - setRegisterDiscreteOutput(edrk.errors.e5.bits.ERROR_PRE_CHARGE_ON, 209);// - setRegisterDiscreteOutput(edrk.errors.e7.bits.ERROR_SBOR_SHEMA, 210);// - - ///////////////////// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch0, 225);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch1, 226);// - - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch2, 227);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch3, 228);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch4, 229);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch5, 230);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch6, 231);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch7, 232);// - - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch8, 234);// - - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch10, 235);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch11, 236);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch12, 237);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch13, 238);// - - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch14, 239);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.plus.bit.ch15, 240);// - - - //////////////////// - setRegisterDiscreteOutput(edrk.errors.e5.bits.LINE_ERR0, 241);// - setRegisterDiscreteOutput(edrk.errors.e5.bits.LINE_HWP, 242);// - //////////////////// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch2, 243);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch3, 244);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch4, 245);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch5, 246);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch6, 247);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch7, 248);// - - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch8, 250);// - - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch10, 251);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch11, 252);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch12, 253);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch13, 254);// - - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch14, 255);// - setRegisterDiscreteOutput(project.hwp[0].read.comp_s.minus.bit.ch15, 256);// - - - - //////////////////// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_INPUT_A1B1, 257);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_INPUT_B1C1, 258);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_INPUT_A2B2, 259);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_INPUT_B2C2, 260);// - - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOW_FREQ_50HZ, 261);// - setRegisterDiscreteOutput(edrk.warnings.e8.bits.LOW_FREQ_50HZ, 262);// - - setRegisterDiscreteOutput(edrk.errors.e7.bits.READ_OPTBUS || edrk.errors.e7.bits.WRITE_OPTBUS, 263);// - setRegisterDiscreteOutput(edrk.errors.e7.bits.MASTER_SLAVE_SYNC, 264); // - - setRegisterDiscreteOutput(edrk.errors.e6.bits.ERR_SBUS, 265);// - setRegisterDiscreteOutput(edrk.errors.e6.bits.ERR_PBUS, 266);// - - setRegisterDiscreteOutput(edrk.errors.e6.bits.ER_DISBAL_BATT, 267);// - - setRegisterDiscreteOutput(edrk.errors.e6.bits.QTV_ERROR_NOT_U, 268);// - setRegisterDiscreteOutput(edrk.errors.e6.bits.ERROR_PRE_CHARGE_U, 269);// - - setRegisterDiscreteOutput(edrk.errors.e8.bits.U_IN_20_PROCENTS_HIGH, 270);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.U_IN_10_PROCENTS_LOW, 271);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.U_IN_20_PROCENTS_LOW, 272);// - - - //////////////// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.err_power, 273);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.err_power, 274);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.err_power, 275);// - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.lock_status_error.bit.err_power, 276);// - setRegisterDiscreteOutput(project.cds_in[0].read.sbus.lock_status_error.bit.err_power, 277);// - setRegisterDiscreteOutput(project.cds_in[1].read.sbus.lock_status_error.bit.err_power, 278);// - setRegisterDiscreteOutput(project.cds_out[0].read.sbus.lock_status_error.bit.err_power, 279);// - - - setRegisterDiscreteOutput(edrk.errors.e7.bits.NOT_VALID_CONTROL_STATION, 280);// - - //////////////// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_OUTPUT_U1, 281);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_OUTPUT_V1, 282);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_OUTPUT_W1, 283);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_OUTPUT_U2, 284);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_OUTPUT_V2, 285);// - setRegisterDiscreteOutput(edrk.errors.e8.bits.LOSS_OUTPUT_W2, 286);// - - setRegisterDiscreteOutput(edrk.errors.e8.bits.DISBALANCE_IM1_IM2, 287);// - setRegisterDiscreteOutput(edrk.errors.e7.bits.VERY_FAST_GO_0to1, 288);// - - //////////////// - setRegisterDiscreteOutput(project.cds_tk[0].read.sbus.lock_status_error.bit.err_switch, 289);// - setRegisterDiscreteOutput(project.cds_tk[1].read.sbus.lock_status_error.bit.err_switch, 290);// - setRegisterDiscreteOutput(project.cds_tk[2].read.sbus.lock_status_error.bit.err_switch, 291);// - setRegisterDiscreteOutput(project.cds_tk[3].read.sbus.lock_status_error.bit.err_switch, 292);// - setRegisterDiscreteOutput(project.cds_in[0].read.sbus.lock_status_error.bit.err_switch, 293);// - setRegisterDiscreteOutput(project.cds_in[1].read.sbus.lock_status_error.bit.err_switch, 294);// - setRegisterDiscreteOutput(project.cds_out[0].read.sbus.lock_status_error.bit.err_switch, 295);// - - setRegisterDiscreteOutput(project.adc[0].read.sbus.lock_status_error.bit.err_switch, 296);// - setRegisterDiscreteOutput(project.adc[1].read.sbus.lock_status_error.bit.err_switch, 298);// - - //////////////// - - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_UO1_MAX, 305);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_UO2_MAX, 306);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_UO3_MAX, 307);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_UO4_MAX, 308);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_UO5_MAX, 309);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_UO6_MAX, 310);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_UO7_MAX, 311);// - - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_UO1_MAX, 312);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_UO2_MAX, 313);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_UO3_MAX, 314);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_UO4_MAX, 315);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_UO5_MAX, 316);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_UO6_MAX, 317);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_UO7_MAX, 318);// - - - - - ///////////////////// - - setRegisterDiscreteOutput(edrk.warnings.e7.bits.READ_OPTBUS || edrk.warnings.e7.bits.WRITE_OPTBUS, 321);// - setRegisterDiscreteOutput(edrk.warnings.e7.bits.MASTER_SLAVE_SYNC, 322);// - - setRegisterDiscreteOutput(edrk.errors.e6.bits.ERROR_PRE_CHARGE_ANSWER, 323);// - - setRegisterDiscreteOutput(edrk.warnings.e1.bits.NO_INPUT_SYNC_SIGNAL, 324);// - setRegisterDiscreteOutput(edrk.errors.e1.bits.NO_INPUT_SYNC_SIGNAL, 325);// - setRegisterDiscreteOutput(edrk.errors.e3.bits.ERR_INT_PWM_LONG - || edrk.errors.e9.bits.ERR_PWM_WDOG - || edrk.errors.e9.bits.ERR_INT_PWM_VERY_LONG, 326);// - - setRegisterDiscreteOutput(edrk.errors.e5.bits.T_VIPR_MAX, 336); - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_AIR0_MAX, 337); - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_AIR1_MAX, 338); - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_AIR2_MAX, 339); - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_AIR3_MAX, 340); - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_BSU_Sensor_BK1, 341); - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_BSU_Sensor_BK1, 342); - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_BSU_Sensor_BK2, 343); - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_BSU_Sensor_BK2, 344); - ////// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_WATER_EXT_MAX, 345);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.T_WATER_INT_MAX, 346);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_WATER_EXT_MAX, 347);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.T_WATER_INT_MAX, 348);// - - setRegisterDiscreteOutput(edrk.errors.e7.bits.T_ACDRIVE_BEAR_MAX_DNE, 349);// - setRegisterDiscreteOutput(edrk.errors.e9.bits.T_ACDRIVE_BEAR_MAX_NE, 350);// - setRegisterDiscreteOutput(edrk.warnings.e7.bits.T_ACDRIVE_BEAR_MAX_DNE, 351);// - setRegisterDiscreteOutput(edrk.warnings.e9.bits.T_ACDRIVE_BEAR_MAX_NE, 352);// - - ////////////// - - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_ACDRIVE_WINDING_U1, 353);// - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_ACDRIVE_WINDING_V1, 354);// - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_ACDRIVE_WINDING_W1, 355);// - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_ACDRIVE_WINDING_U2, 356);// - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_ACDRIVE_WINDING_V2, 357);// - setRegisterDiscreteOutput(edrk.errors.e10.bits.T_ACDRIVE_WINDING_W2, 358);// - - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U1, 359);// - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V1, 360);// - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W1, 361);// - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U2, 362);// - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V2, 363);// - setRegisterDiscreteOutput(edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W2, 364);// - - //////////////////// - - setRegisterDiscreteOutput(edrk.errors.e2.bits.P_WATER_INT_MAX, 369);// - setRegisterDiscreteOutput(edrk.errors.e2.bits.P_WATER_INT_MIN, 370);// - - setRegisterDiscreteOutput(edrk.warnings.e2.bits.P_WATER_INT_MAX, 371);// - setRegisterDiscreteOutput(edrk.warnings.e2.bits.P_WATER_INT_MIN, 372);// - setRegisterDiscreteOutput(edrk.warnings.e5.bits.PUMP_1, 373);// - setRegisterDiscreteOutput(edrk.warnings.e5.bits.PUMP_2, 374);// - - - - //// - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_PUMP_ON_SBOR, 385); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_RESTART_PUMP_1_ON_SBOR, 386); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_RESTART_PUMP_2_ON_SBOR, 387); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_RESTART_PUMP_ALL_ON_SBOR, 388); - - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_PRED_ZARYAD, 389); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_PRED_ZARYAD_AFTER, 390); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_READY_UMP_BEFORE_QTV, 391); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_STATUS_QTV, 392); - - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_UMP_ON_AFTER, 393); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_UMP_NOT_ON, 394); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_UMP_NOT_OFF, 395); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_RASCEPITEL_WAIT_CMD, 396); - - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_RASCEPITEL_ON_AFTER, 397); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_DISABLE_SBOR, 398); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_VERY_LONG_SBOR, 399); - setRegisterDiscreteOutput(edrk.errors.e11.bits.ERROR_CONTROLLER_BUS, 400); - - setRegisterDiscreteOutput(edrk.warnings.e9.bits.BREAK_TEMPER_WARNING, 417);// - setRegisterDiscreteOutput(edrk.warnings.e9.bits.BREAK_TEMPER_ALARM, 418);// - setRegisterDiscreteOutput(edrk.warnings.e9.bits.BREAKER_GED_ON, 419);// - - ////////////////// - - - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER || - edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_POWER) { - setRegisterDiscreteOutput(1, 520); - } else { - setRegisterDiscreteOutput(0, 520); - } - // setRegisterDiscreteOutput(TODO , 546);// - setRegisterDiscreteOutput(!edrk.from_ing1.bits.UPC_24V_NORMA, 546);// - setRegisterDiscreteOutput(edrk.from_ing2.bits.SOST_ZAMKA, 547);// - setRegisterDiscreteOutput(edrk.from_shema_filter.bits.READY_UMP, 548);// - - - //////////////// - - -} - - - -void update_tables_HMI_analog(void) -{ - int power_kw_full; - int power_kw; - int oborots; - - Inverter_state state; - static int nn=0, ss=0, pl = 0; -// static int prev_edrk_KVITIR=0; - int i,status; -// static int check = 0; - - hmi_watch_dog = !hmi_watch_dog; //was transmitted, need to change - - //log_to_HMI.send_log = modbus_table_analog_in[7].all; - //setRegisterDiscreteOutput(log_to_HMI.flag_log_array_ready_sent, 310); - -// setRegisterDiscreteOutput(ss, nn); - - // - modbus_table_analog_out[4].all++;// = ++check; -// test -// setRegisterDiscreteOutput(1, 293); -// setRegisterDiscreteOutput(1, 294); - - - - - - - -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - - if (edrk.summ_errors) - { - modbus_table_analog_out[1].all = 6; // - modbus_table_analog_out[2].all = 3; // red - } - else - { - - if (edrk.SumSbor || edrk.Status_Ready.bits.ImitationReady2) - { - if (edrk.Status_Ready.bits.ready_final) - { - //modbus_table_analog_out[2].all = 1; // green - if (edrk.Go) - { - modbus_table_analog_out[1].all = 3; // - if (edrk.Provorot) - modbus_table_analog_out[1].all = 12; // = 11 - - } - else - modbus_table_analog_out[1].all = 2; // ready2 - } - else - modbus_table_analog_out[1].all = 4; // building - } - else - { - if (edrk.Status_Ready.bits.ready1) - modbus_table_analog_out[1].all = 1; // ready1 - else - modbus_table_analog_out[1].all = 0; // waiting - } - - if (edrk.RazborNotFinish) - modbus_table_analog_out[1].all = 11; // - - if (edrk.Status_Perehod_Rascepitel==1 && edrk.cmd_to_rascepitel==1) - modbus_table_analog_out[1].all = 7; // - - if (edrk.Status_Perehod_Rascepitel==1 && edrk.cmd_to_rascepitel==0) - modbus_table_analog_out[1].all = 8; // - - if (edrk.RunZahvatRascepitel) - modbus_table_analog_out[1].all = 9; // = 9 - if (edrk.RunUnZahvatRascepitel) - modbus_table_analog_out[1].all = 10; // = 10 - - // - //modbus_table_analog_out[1].all = 5; // - - if (modbus_table_analog_out[1].all == 1) - modbus_table_analog_out[2].all = 0; // gray - else - if (modbus_table_analog_out[1].all == 3 || - modbus_table_analog_out[1].all == 12 || - modbus_table_analog_out[1].all == 2) - modbus_table_analog_out[2].all = 1; // green - else - { - if (modbus_table_analog_out[2].all==0) - modbus_table_analog_out[2].all = 1; // green - else - modbus_table_analog_out[2].all = 0; // gray - } - - } - - - - - if (edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER || edrk.errors.e6.bits.QTV_ERROR_NOT_U) - { - modbus_table_analog_out[10].all = 3; - modbus_table_analog_out[11].all = 3; - } - else - { - if (edrk.from_shema_filter.bits.QTV_ON_OFF) - { - modbus_table_analog_out[10].all = 1; - modbus_table_analog_out[11].all = 1; - } - else - { - modbus_table_analog_out[10].all = 0; - modbus_table_analog_out[11].all = 0; - } - } - - - if (edrk.from_ing1.bits.VIPR_PREDOHR_NORMA==1) - { - if (edrk.from_shema_filter.bits.QTV_ON_OFF==1) - modbus_table_analog_out[12].all = 1; - else - modbus_table_analog_out[12].all = 0; - } - else - modbus_table_analog_out[12].all = 3; - - - if (edrk.errors.e6.bits.UO1_KEYS || edrk.errors.e1.bits.I_BREAK_1_MAX || edrk.errors.e1.bits.I_BREAK_2_MAX || edrk.errors.e2.bits.T_UO1_MAX) - modbus_table_analog_out[13].all = 3; - else - if (edrk.warnings.e2.bits.T_UO1_MAX) - modbus_table_analog_out[13].all = 2; - else - modbus_table_analog_out[13].all = 1; - - if (edrk.errors.e6.bits.UO2_KEYS || edrk.errors.e1.bits.I_UO2_MAX || edrk.errors.e2.bits.T_UO2_MAX) - modbus_table_analog_out[14].all = 3; - else - if (edrk.warnings.e2.bits.T_UO2_MAX) - modbus_table_analog_out[14].all = 2; - else - modbus_table_analog_out[14].all = 1; - - if (edrk.errors.e6.bits.UO3_KEYS || edrk.errors.e1.bits.I_UO3_MAX || edrk.errors.e2.bits.T_UO3_MAX) - modbus_table_analog_out[15].all = 3; - else - if (edrk.warnings.e2.bits.T_UO3_MAX) - modbus_table_analog_out[15].all = 2; - else - modbus_table_analog_out[15].all = 1; - - if (edrk.errors.e6.bits.UO4_KEYS || edrk.errors.e1.bits.I_UO4_MAX || edrk.errors.e2.bits.T_UO4_MAX) - modbus_table_analog_out[16].all = 3; - else - if (edrk.warnings.e2.bits.T_UO4_MAX) - modbus_table_analog_out[16].all = 2; - else - modbus_table_analog_out[16].all = 1; - - if (edrk.errors.e6.bits.UO5_KEYS || edrk.errors.e1.bits.I_UO5_MAX || edrk.errors.e2.bits.T_UO5_MAX) - modbus_table_analog_out[17].all = 3; - else - if (edrk.warnings.e2.bits.T_UO5_MAX) - modbus_table_analog_out[17].all = 2; - else - modbus_table_analog_out[17].all = 1; - - if (edrk.errors.e6.bits.UO6_KEYS || edrk.errors.e1.bits.I_UO6_MAX || edrk.errors.e2.bits.T_UO6_MAX) - modbus_table_analog_out[18].all = 3; - else - if (edrk.warnings.e2.bits.T_UO6_MAX) - modbus_table_analog_out[18].all = 2; - else - modbus_table_analog_out[18].all = 1; - - if (edrk.errors.e6.bits.UO7_KEYS || edrk.errors.e1.bits.I_UO7_MAX || edrk.errors.e2.bits.T_UO7_MAX) - modbus_table_analog_out[19].all = 3; - else - if (edrk.warnings.e2.bits.T_UO7_MAX) - modbus_table_analog_out[19].all = 2; - else - modbus_table_analog_out[19].all = 1; - - - // motor_state - if (edrk.errors.e10.bits.T_ACDRIVE_WINDING_U1 || edrk.errors.e10.bits.T_ACDRIVE_WINDING_V1 || - edrk.errors.e10.bits.T_ACDRIVE_WINDING_W1 || edrk.errors.e10.bits.T_ACDRIVE_WINDING_U2 || - edrk.errors.e10.bits.T_ACDRIVE_WINDING_V2 || edrk.errors.e10.bits.T_ACDRIVE_WINDING_W2 || - edrk.errors.e7.bits.T_ACDRIVE_BEAR_MAX_DNE || edrk.errors.e9.bits.T_ACDRIVE_BEAR_MAX_NE) { - modbus_table_analog_out[20].all = 3; - } else if (edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U1 || edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V1 || - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W1 || edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U2 || - edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V2 || edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W2 || - edrk.warnings.e7.bits.T_ACDRIVE_BEAR_MAX_DNE || edrk.warnings.e9.bits.T_ACDRIVE_BEAR_MAX_NE - || edrk.power_limit.all - ) { - modbus_table_analog_out[20].all = 2; - } else { - modbus_table_analog_out[20].all = 1; - } - - - // ump state - if (edrk.from_ing1.bits.ZARYAD_ON || edrk.from_shema_filter.bits.UMP_ON_OFF) - { - modbus_table_analog_out[21].all = 1; //green - } - else - { - if (edrk.errors.e7.bits.ERROR_SBOR_SHEMA || edrk.errors.e5.bits.ERROR_PRE_CHARGE_ON || - edrk.errors.e6.bits.ERROR_PRE_CHARGE_ANSWER || edrk.errors.e6.bits.ERROR_PRE_CHARGE_U || edrk.errors.e7.bits.UMP_NOT_READY) - modbus_table_analog_out[21].all = 3; // alarm - else - if (edrk.warnings.e7.bits.UMP_NOT_READY) - modbus_table_analog_out[21].all = 2; //fault - else - { - if (edrk.Stage_Sbor == STAGE_SBOR_STATUS_UMP_ON && edrk.SumSbor) - { - if (modbus_table_analog_out[21].all==0) - modbus_table_analog_out[21].all = 1; //green - else - modbus_table_analog_out[21].all = 0; //gray - } - else - modbus_table_analog_out[21].all = 0; - } - } - - - modbus_table_analog_out[30].all = fast_round_with_limiter(_IQtoF(filter.iqUin_m1)*NORMA_ACP/1.41, LIMITER_U_I_PULT); - modbus_table_analog_out[31].all = fast_round_with_limiter(_IQtoF(filter.iqUin_m2)*NORMA_ACP/1.41, LIMITER_U_I_PULT); - -// if (edrk.Status_Ready.bits.ready_final==0) -// { -// modbus_table_analog_out[32].all = edrk.Stage_Sbor; -// modbus_table_analog_out[33].all = edrk.Sbor_Mode;//_IQtoF(analog.iqIin_1)*NORMA_ACP; -// } -// else -// { - modbus_table_analog_out[32].all = fast_round_with_limiter(_IQtoF(analog.iqIin_1)*NORMA_ACP, LIMITER_U_I_PULT); - modbus_table_analog_out[33].all = fast_round_with_limiter(_IQtoF(analog.iqIin_2)*NORMA_ACP, LIMITER_U_I_PULT); -// } - - -// modbus_table_analog_out[34].all = _IQtoF(filter.iqU_1_long)*NORMA_ACP; - modbus_table_analog_out[35].all = fast_round_with_limiter(_IQtoF(filter.iqU_2_long)*NORMA_ACP, LIMITER_U_I_PULT); - modbus_table_analog_out[34].all = fast_round_with_limiter(_IQtoF(filter.iqU_1_long)*NORMA_ACP, LIMITER_U_I_PULT); - - modbus_table_analog_out[36].all = fast_round_with_limiter(_IQtoF(analog.iqIbreak_1+analog.iqIbreak_2)*NORMA_ACP, LIMITER_U_I_PULT);//Ibreak - - - -// modbus_table_analog_out[37].all = fast_round(_IQtoF(analog.iqIu_1_rms)*NORMA_ACP); -// modbus_table_analog_out[38].all = fast_round(_IQtoF(analog.iqIv_1_rms)*NORMA_ACP); -// modbus_table_analog_out[39].all = fast_round(_IQtoF(analog.iqIw_1_rms)*NORMA_ACP); -// -// modbus_table_analog_out[40].all = fast_round(_IQtoF(analog.iqIu_2_rms)*NORMA_ACP); -// modbus_table_analog_out[41].all = fast_round(_IQtoF(analog.iqIv_2_rms)*NORMA_ACP); -// modbus_table_analog_out[42].all = fast_round(_IQtoF(analog.iqIw_2_rms)*NORMA_ACP); - - modbus_table_analog_out[37].all = //fast_round(_IQtoF(filter.iqIm_1)*NORMA_ACP_RMS); - modbus_table_analog_out[38].all = //fast_round(_IQtoF(filter.iqIm_1)*NORMA_ACP_RMS); - modbus_table_analog_out[39].all = fast_round_with_limiter(_IQtoF(filter.iqIm_1)*NORMA_ACP_RMS, LIMITER_U_I_PULT); - - modbus_table_analog_out[40].all = //fast_round(_IQtoF(filter.iqIm_2)*NORMA_ACP_RMS); - modbus_table_analog_out[41].all = //fast_round(_IQtoF(filter.iqIm_2)*NORMA_ACP_RMS); - modbus_table_analog_out[42].all = fast_round_with_limiter(_IQtoF(filter.iqIm_2)*NORMA_ACP_RMS, LIMITER_U_I_PULT); - -// if (edrk.flag_second_PCH == 0) { -// modbus_table_analog_out[43].all = modbus_table_analog_out[44].all = fast_round(_IQtoF(filter.iqIm_1 + filter.iqIm_2)*NORMA_ACP_RMS); -// //modbus_table_analog_out[44].all = fast_round(_IQtoF(filter.iqIm_1 + filter.iqIm_2)*NORMA_ACP_RMS); -// } else { -// modbus_table_analog_out[43].all = modbus_table_analog_out[44].all = fast_round(_IQtoF(filter.iqIm_1 + filter.iqIm_2)*NORMA_ACP_RMS); -// //modbus_table_analog_out[44].all = fast_round(_IQtoF(filter.iqIm_1 + filter.iqIm_2)*NORMA_ACP_RMS); -// } - modbus_table_analog_out[43].all = modbus_table_analog_out[44].all = fast_round_with_limiter(_IQtoF(filter.iqIm_1 + filter.iqIm_2)*NORMA_ACP_RMS, LIMITER_U_I_PULT); - - modbus_table_analog_out[45].all = 0; //edrk.I_zad_vozbud_exp; - -// modbus_table_analog_out[4].all = control_station.active_array_cmd[CONTROL_STATION_CMD_SET_ROTOR]; -// modbus_table_analog_out[5].all = control_station.active_array_cmd[CONTROL_STATION_CMD_SET_POWER]; - - -//#if (_FLOOR6__) -// power_kw_full = edrk.power_kw_full; -// power_kw = edrk.power_kw; -// oborots = edrk.oborots; -// -// if (edrk.oborots) -// oborots = edrk.oborots; -// else -// oborots = edrk.zadanie.oborots_zad; -// -// -// if (edrk.Mode_ScalarVectorUFConst==ALG_MODE_SCALAR_OBOROTS -// || (edrk.Mode_ScalarVectorUFConst==ALG_MODE_FOC_OBOROTS) -// || (edrk.Mode_ScalarVectorUFConst==ALG_MODE_UF_CONST) -// ) // oborots -// { -// power_kw_full = (edrk.zadanie.oborots_zad)*50; -// power_kw = (edrk.zadanie.oborots_zad)*25; -// } -// else -// { -// oborots = edrk.zadanie.power_zad/25; -//// modbus_table_analog_out[48].all = abs(edrk.zadanie.power_zad); -// } -// -// -// -// -// -//#else - power_kw_full = fast_round_with_limiter(edrk.power_kw_full, LIMITER_U_I_PULT); - power_kw = edrk.power_kw; - oborots = edrk.oborots; -//#endif - - - if (edrk.Mode_ScalarVectorUFConst==ALG_MODE_UF_CONST) // UFCONST - { - modbus_table_analog_out[47].all = 0;//fast_round(edrk.zadanie.fzad*100.0); // - modbus_table_analog_out[46].all = 0; // - } - else - if (edrk.Mode_ScalarVectorUFConst==ALG_MODE_SCALAR_OBOROTS) // scalar oborots - { - modbus_table_analog_out[47].all = edrk.zadanie.oborots_zad; // - - if (oborots>=0) - modbus_table_analog_out[46].all = power_kw_full; // - else - modbus_table_analog_out[46].all = -power_kw_full; // - - } - else - if (edrk.Mode_ScalarVectorUFConst==ALG_MODE_SCALAR_POWER) // scalar power - { - modbus_table_analog_out[47].all = oborots; // - modbus_table_analog_out[46].all = edrk.zadanie.power_zad; // - - } - else - if (edrk.Mode_ScalarVectorUFConst==ALG_MODE_FOC_OBOROTS) // foc oborots - { - modbus_table_analog_out[47].all = edrk.zadanie.oborots_zad; // - - if (oborots>=0) - modbus_table_analog_out[46].all = power_kw_full; // - else - modbus_table_analog_out[46].all = -power_kw_full; // - -// modbus_table_analog_out[46].all = 0; // - - } - else - if (edrk.Mode_ScalarVectorUFConst==ALG_MODE_FOC_POWER) // foc power - { - modbus_table_analog_out[47].all = oborots; // - modbus_table_analog_out[46].all = edrk.zadanie.power_zad; // - - } - else - { - modbus_table_analog_out[46].all = 0;//-1; // - modbus_table_analog_out[47].all = 0;//-1; // - } - - - - - -//#if (_FLOOR6___) -// if (edrk.Mode_ScalarVectorUFConst==ALG_MODE_SCALAR_OBOROTS -// || (edrk.Mode_ScalarVectorUFConst==ALG_MODE_FOC_OBOROTS) -// || (edrk.Mode_ScalarVectorUFConst==ALG_MODE_UF_CONST) -// ) // oborots -// { -// // -// if (edrk.oborots == 0) -// { -// if (edrk.zadanie.oborots_zad>0) -// modbus_table_analog_out[49].all = edrk.zadanie.oborots_zad - 1; -// else if (edrk.zadanie.oborots_zad<0) -// modbus_table_analog_out[49].all = edrk.zadanie.oborots_zad + 1; -// else -// modbus_table_analog_out[49].all = 0; -// -// } -// else -// { -// modbus_table_analog_out[49].all = edrk.oborots; // -// } -// -// if (edrk.zadanie.oborots_zad<0) -// modbus_table_analog_out[48].all = -(edrk.zadanie.oborots_zad)*25; // -// else -// modbus_table_analog_out[48].all = (edrk.zadanie.oborots_zad)*25; // -// -// } -// else -// { -// modbus_table_analog_out[49].all = edrk.zadanie.power_zad/25; -// modbus_table_analog_out[48].all = abs(edrk.zadanie.power_zad); -// } -// -// modbus_table_analog_out[5].all = abs(power_kw*2);//power_kw_full; // -//#else - - modbus_table_analog_out[48].all = abs(power_kw); // - modbus_table_analog_out[49].all = oborots; // - modbus_table_analog_out[5].all = abs(power_kw_full); // - modbus_table_analog_out[6].all = fast_round(_IQtoF(edrk.zadanie.iq_limit_power_zad_rmp)*NORMA_ACP*NORMA_ACP/1000.0);//abs(power_kw_full); // -//#endif - - - // modbus_table_analog_out[48].all = fast_round(_IQtoF((filter.Power) * NORMA_ACP * NORMA_ACP) / 1000.0); // - - for (i=0;i<2;i++) - modbus_table_analog_out[50+i].all = fast_round_with_delta(modbus_table_analog_out[50+i].all, edrk.temper_edrk.real_int_temper_water[i]/10.0, 1); - - - modbus_table_analog_out[52].all = fast_round_with_delta(modbus_table_analog_out[52].all, edrk.p_water_edrk.filter_real_int_p_water[0]/10.0, 1); - - for (i=0;i<6;i++) - modbus_table_analog_out[53+i].all = fast_round_with_delta(modbus_table_analog_out[53+i].all, edrk.temper_edrk.real_int_temper_u[1+i]/10.0, 1); - - modbus_table_analog_out[59].all = fast_round_with_delta(modbus_table_analog_out[59].all, edrk.temper_edrk.real_int_temper_u[0]/10.0, 1); - - for (i=0;i<4;i++) - modbus_table_analog_out[60+i].all = fast_round_with_delta(modbus_table_analog_out[60+i].all, edrk.temper_edrk.real_int_temper_air[i]/10.0, 1); - - modbus_table_analog_out[8].all = fast_round_with_delta(modbus_table_analog_out[8].all, edrk.temper_edrk.max_real_int_temper_u/10.0, 1); - - if (edrk.errors.e2.bits.T_AIR0_MAX) - modbus_table_analog_out[64].all = 3; - else - if (edrk.warnings.e2.bits.T_AIR0_MAX) - modbus_table_analog_out[64].all = 2; - else - modbus_table_analog_out[64].all = 1; - - if (edrk.errors.e2.bits.T_AIR1_MAX) - modbus_table_analog_out[65].all = 3; - else - if (edrk.warnings.e2.bits.T_AIR1_MAX) - modbus_table_analog_out[65].all = 2; - else - modbus_table_analog_out[65].all = 1; - - if (edrk.errors.e2.bits.T_AIR2_MAX) - modbus_table_analog_out[66].all = 3; - else - if (edrk.warnings.e2.bits.T_AIR2_MAX) - modbus_table_analog_out[66].all = 2; - else - modbus_table_analog_out[66].all = 1; - - if (edrk.errors.e2.bits.T_AIR3_MAX) - modbus_table_analog_out[67].all = 3; - else - if (edrk.warnings.e2.bits.T_AIR3_MAX) - modbus_table_analog_out[67].all = 2; - else - modbus_table_analog_out[67].all = 1; - - - - if (edrk.auto_master_slave.local.bits.master) - modbus_table_analog_out[68].all = 0; // master salve - else - if (edrk.auto_master_slave.local.bits.slave) - modbus_table_analog_out[68].all = 1; // master salve - else - if (edrk.auto_master_slave.local.bits.try_master) - modbus_table_analog_out[68].all = 3; // master salve - else - if (edrk.errors.e7.bits.AUTO_SET_MASTER) - modbus_table_analog_out[68].all = 4; // master salve - else - modbus_table_analog_out[68].all = 2; // master salve - - for (i=0;i<6;i++) - modbus_table_analog_out[69+i].all = fast_round(edrk.temper_acdrive.winding.filter_real_temper[0+i]); - - modbus_table_analog_out[9].all = fast_round(edrk.temper_acdrive.winding.max_real_int_temper/10.0); - - modbus_table_analog_out[75].all = fast_round(edrk.temper_acdrive.bear.filter_real_temper[0]); - modbus_table_analog_out[76].all = fast_round(edrk.temper_acdrive.bear.filter_real_temper[1]); - - modbus_table_analog_out[23].all = fast_round(edrk.temper_acdrive.bear.max_real_int_temper/10.0); - - for (i=0;i<6;i++) - { - status = get_status_temper_acdrive_winding(i); - if (status==4) - modbus_table_analog_out[77+i].all = 3; - if (status==2) - modbus_table_analog_out[77+i].all = 2; - if (status==1) - modbus_table_analog_out[77+i].all = 1; - } - - for (i=0;i<2;i++) - { - status = get_status_temper_acdrive_bear(i); - if (status==4) - modbus_table_analog_out[83+i].all = 3; - if (status==2) - modbus_table_analog_out[83+i].all = 2; - if (status==1) - modbus_table_analog_out[83+i].all = 1; - } - - -//UOM - modbus_table_analog_out[85].all = edrk.from_uom.level_value; - - if (edrk.from_uom.ready==1) - { - if (edrk.from_uom.error) - modbus_table_analog_out[86].all = 1; // - else - { - if (edrk.from_uom.level_value==0) - modbus_table_analog_out[86].all = 2; // - else - { - if (edrk.power_limit.bits.limit_UOM || edrk.power_limit.bits.limit_from_uom_fast) - { - // - - if (modbus_table_analog_out[86].all==0) - modbus_table_analog_out[86].all = 3; // - else - modbus_table_analog_out[86].all = 0; //gray - } - else - modbus_table_analog_out[86].all = 3; - - //modbus_table_analog_out[86].all = 3; // - } - } - } - else - modbus_table_analog_out[86].all = 0; // - - -// active control station -// CONTROL_STATION_TERMINAL_RS232 = 0, - -// CONTROL_STATION_TERMINAL_CAN, - -// -// CONTROL_STATION_INGETEAM_PULT_RS485, - -// CONTROL_STATION_MPU_SVU_CAN, -// CONTROL_STATION_MPU_KEY_CAN, -// CONTROL_STATION_MPU_SVU_RS485, -// CONTROL_STATION_MPU_KEY_RS485, -// CONTROL_STATION_ZADATCHIK_CAN, -// CONTROL_STATION_VPU_CAN, - - modbus_table_analog_out[87].all = edrk.active_post_upravl; - - - // load procents - modbus_table_analog_out[88].all = 0; //error - - // 0- , , , - . [87]. - if (modbus_table_analog_out[87].all == 10) - modbus_table_analog_out[89].all = 3; //red - else - modbus_table_analog_out[89].all = 1; //no error - - ///////////////////////////// - ///////////////////////////// - if (edrk.warnings.e7.bits.READ_OPTBUS - && edrk.warnings.e7.bits.WRITE_OPTBUS - && edrk.warnings.e7.bits.MASTER_SLAVE_SYNC) - modbus_table_analog_out[90].all = 2; //warning - else - if (edrk.errors.e7.bits.READ_OPTBUS - || edrk.errors.e7.bits.WRITE_OPTBUS - || edrk.errors.e7.bits.MASTER_SLAVE_SYNC - || edrk.errors.e1.bits.NO_INPUT_SYNC_SIGNAL) - modbus_table_analog_out[90].all = 3; //error - else - if (edrk.warnings.e7.bits.READ_OPTBUS - || edrk.warnings.e7.bits.WRITE_OPTBUS - || edrk.warnings.e7.bits.MASTER_SLAVE_SYNC - || edrk.warnings.e1.bits.NO_INPUT_SYNC_SIGNAL) - modbus_table_analog_out[90].all = 5; //warning - else - if (edrk.ms.ready1==0) - modbus_table_analog_out[90].all = 1; //find - else - modbus_table_analog_out[90].all = 0; //ok - - modbus_table_analog_out[91].all = protect_levels.abnormal_temper_acdrive_winding_U1 / 10; - modbus_table_analog_out[92].all = protect_levels.abnormal_temper_acdrive_winding_V1 / 10; - modbus_table_analog_out[93].all = protect_levels.abnormal_temper_acdrive_winding_W1 / 10; - modbus_table_analog_out[94].all = protect_levels.abnormal_temper_acdrive_winding_U2 / 10; - modbus_table_analog_out[95].all = protect_levels.abnormal_temper_acdrive_winding_V2 / 10; - modbus_table_analog_out[96].all = protect_levels.abnormal_temper_acdrive_winding_W2 / 10; - modbus_table_analog_out[97].all = protect_levels.abnormal_temper_acdrive_bear_DNE / 10; - modbus_table_analog_out[98].all = protect_levels.abnormal_temper_acdrive_bear_NE / 10; - - modbus_table_analog_out[99].all = protect_levels.alarm_temper_acdrive_winding_U1 / 10; - modbus_table_analog_out[100].all = protect_levels.alarm_temper_acdrive_winding_V1 / 10; - modbus_table_analog_out[101].all = protect_levels.alarm_temper_acdrive_winding_W1 / 10; - modbus_table_analog_out[102].all = protect_levels.alarm_temper_acdrive_winding_U2 / 10; - modbus_table_analog_out[103].all = protect_levels.alarm_temper_acdrive_winding_V2 / 10; - modbus_table_analog_out[104].all = protect_levels.alarm_temper_acdrive_winding_W2 / 10; - modbus_table_analog_out[105].all = protect_levels.alarm_temper_acdrive_bear_DNE / 10; - modbus_table_analog_out[106].all = protect_levels.alarm_temper_acdrive_bear_NE / 10; - - modbus_table_analog_out[107].all = protect_levels.abnormal_temper_u_01 / 10; - modbus_table_analog_out[108].all = protect_levels.abnormal_temper_u_02 / 10; - modbus_table_analog_out[109].all = protect_levels.abnormal_temper_u_03 / 10; - modbus_table_analog_out[110].all = protect_levels.abnormal_temper_u_04 / 10; - modbus_table_analog_out[111].all = protect_levels.abnormal_temper_u_05 / 10; - modbus_table_analog_out[112].all = protect_levels.abnormal_temper_u_06 / 10; - modbus_table_analog_out[113].all = protect_levels.abnormal_temper_u_07 / 10; - modbus_table_analog_out[114].all = protect_levels.alarm_temper_u_01 / 10; - modbus_table_analog_out[115].all = protect_levels.alarm_temper_u_02 / 10; - modbus_table_analog_out[116].all = protect_levels.alarm_temper_u_03 / 10; - modbus_table_analog_out[117].all = protect_levels.alarm_temper_u_04 / 10; - modbus_table_analog_out[118].all = protect_levels.alarm_temper_u_05 / 10; - modbus_table_analog_out[119].all = protect_levels.alarm_temper_u_06 / 10; - modbus_table_analog_out[120].all = protect_levels.alarm_temper_u_07 / 10; - - modbus_table_analog_out[123].all = protect_levels.abnormal_temper_water_int / 10; - modbus_table_analog_out[124].all = protect_levels.abnormal_temper_water_ext / 10; - modbus_table_analog_out[125].all = protect_levels.alarm_p_water_min_int / 10; - modbus_table_analog_out[126].all = protect_levels.alarm_temper_water_int / 10; - modbus_table_analog_out[127].all = protect_levels.alarm_temper_water_ext / 10; - modbus_table_analog_out[128].all = protect_levels.alarm_p_water_max_int / 10; - - modbus_table_analog_out[129].all = protect_levels.abnormal_temper_air_int_01 / 10; - modbus_table_analog_out[130].all = protect_levels.abnormal_temper_air_int_02 / 10; - modbus_table_analog_out[131].all = protect_levels.abnormal_temper_air_int_03 / 10; - modbus_table_analog_out[132].all = protect_levels.abnormal_temper_air_int_04 / 10; - modbus_table_analog_out[133].all = protect_levels.alarm_temper_air_int_01 / 10; - modbus_table_analog_out[134].all = protect_levels.alarm_temper_air_int_02 / 10; - modbus_table_analog_out[135].all = protect_levels.alarm_temper_air_int_03 / 10; - modbus_table_analog_out[136].all = protect_levels.alarm_temper_air_int_04 / 10; - -// toControllerAlarmMinUlineA1B1C1 30137 -// toControllerAlarmMinUlineA2B2C2 30138 -// toControllerAlarmMinUdcUP 30139 -// toControllerAlarmMinUdcDOWN 30140 -// toControllerAlarmMaxUlineA1B1C1 30142 -// toControllerAlarmMaxUlineA2B2C2 30143 -// toControllerAlarmMaxUdcUP 30144 -// toControllerAlarmMaxUdcDOWN 30145 - - modbus_table_analog_out[137].all = _IQtoF(analog_protect.in_voltage[0].setup.levels.iqNominal_minus20) * NORMA_ACP;//_IQtoF(edrk.iqMIN_U_IN) * NORMA_ACP; - modbus_table_analog_out[138].all = _IQtoF(analog_protect.in_voltage[1].setup.levels.iqNominal_minus20) * NORMA_ACP; - - modbus_table_analog_out[139].all = _IQtoF(edrk.iqMIN_U_ZPT) * NORMA_ACP; - modbus_table_analog_out[140].all = _IQtoF(edrk.iqMIN_U_ZPT) * NORMA_ACP; - - modbus_table_analog_out[142].all = _IQtoF(analog_protect.in_voltage[0].setup.levels.iqNominal_plus20) * NORMA_ACP; - modbus_table_analog_out[143].all = _IQtoF(analog_protect.in_voltage[1].setup.levels.iqNominal_plus20) * NORMA_ACP; - -// modbus_table_analog_out[141].all = //_IQtoF(edrk.iqMAX_U_IN) * NORMA_ACP; -// modbus_table_analog_out[140].all = _IQtoF(edrk.iqMAX_U_IN) * NORMA_ACP; - - modbus_table_analog_out[144].all = _IQtoF(edrk.iqMAX_U_ZPT) * NORMA_ACP; - modbus_table_analog_out[145].all = _IQtoF(edrk.iqMAX_U_ZPT) * NORMA_ACP; - - modbus_table_analog_out[146].all = protect_levels.alarm_Izpt_max; - - modbus_table_analog_out[155].all = protect_levels.alarm_Imax_U01; - modbus_table_analog_out[156].all = protect_levels.alarm_Imax_U02; - modbus_table_analog_out[157].all = protect_levels.alarm_Imax_U03; - modbus_table_analog_out[158].all = protect_levels.alarm_Imax_U04; - modbus_table_analog_out[159].all = protect_levels.alarm_Imax_U05; - modbus_table_analog_out[160].all = protect_levels.alarm_Imax_U06; - modbus_table_analog_out[161].all = protect_levels.alarm_Imax_U07; - modbus_table_analog_out[162].all = protect_levels.alarm_Iged_max; - - - // save nPCH TimeToChangePump - modbus_table_analog_out[163].all = edrk.pult_data.data_to_pult.nPCH; - modbus_table_analog_out[154].all = edrk.pult_data.data_to_pult.TimeToChangePump; - - modbus_table_analog_out[222].all = edrk.pult_data.data_to_pult.count_build; - modbus_table_analog_out[223].all = edrk.pult_data.data_to_pult.count_revers; - - - - modbus_table_analog_out[197].all = edrk.pult_data.flagSaveDataPCH; - - - - - // build version - modbus_table_analog_out[219].all = edrk.buildYear; - modbus_table_analog_out[220].all = edrk.buildMonth; - modbus_table_analog_out[221].all = edrk.buildDay; - - //moto - for (i=0;i32760) - edrk.pult_data.data.count_build = 1; - - // edrk.pult_data.data_to_pult.count_build = edrk.pult_data.data.count_build; - // edrk.pult_data.data_to_pult.moto[21] = edrk.pult_data.data_to_pult.count_build; -} - - - -void inc_count_revers(void) -{ -// edrk.pult_data.data.count_revers = edrk.pult_data.data_from_pult.moto[22]; - - edrk.pult_data.data.count_revers++; - if (edrk.pult_data.data.count_revers>32760) - edrk.pult_data.data.count_revers = 1; - -// edrk.pult_data.data_to_pult.count_revers = edrk.pult_data.data.count_revers; - // edrk.pult_data.data_to_pult.moto[22] = edrk.pult_data.data_to_pult.count_revers - -} - - -void update_nPCH(void) -{ - - static int pause_w = 5, first_run = 1; // 10 - int flag_1 = 0, flag_2 = 0, i; - static int prev_active = 0; - - if (control_station.alive_control_station[CONTROL_STATION_INGETEAM_PULT_RS485]==0) - { - pause_w = 5; - } - - if (pause_w > 1) - { - pause_w--; -// if (edrk.pult_data.nPCH_from_pult) -// if (pause_w > 1) -// pause_w = 1; - prev_active = control_station.alive_control_station[CONTROL_STATION_INGETEAM_PULT_RS485]; - return; - } - - // -1 - // -1 , - // - if (pause_w==1 && first_run) - { - // - if (edrk.pult_data.data_from_pult.nPCH==-1) // - { - edrk.pult_data.data_to_pult.nPCH = edrk.pult_data.data.nPCH = 0; - } - else - edrk.pult_data.data_to_pult.nPCH = edrk.pult_data.data.nPCH = edrk.pult_data.data_from_pult.nPCH; - - // - if (edrk.pult_data.data_from_pult.TimeToChangePump == -1) // - edrk.pult_data.data_to_pult.TimeToChangePump = edrk.pult_data.data.TimeToChangePump = 0; - else - edrk.pult_data.data_to_pult.TimeToChangePump = edrk.pult_data.data.TimeToChangePump = edrk.pult_data.data_from_pult.TimeToChangePump; - - // + + - - for (i=0;i=0) - { - edrk.pult_data.data_to_pult.nPCH = edrk.pult_data.data_from_pult.nPCH; - flag_1 = 1; - edrk.pult_data.data.nPCH = edrk.pult_data.data_from_pult.nPCH; - } - - // -1 - if (edrk.pult_data.data_from_pult.nPCH != edrk.pult_data.data.nPCH && edrk.pult_data.data_from_pult.nPCH == -1) - { - edrk.pult_data.data_to_pult.nPCH = edrk.pult_data.data.nPCH; - flag_1 = 1; - } - - - // - // - if (edrk.pult_data.data_from_pult.TimeToChangePump != edrk.pult_data.data.TimeToChangePump - && edrk.pult_data.data_from_pult.TimeToChangePump >= 0) - { - edrk.pult_data.data_to_pult.TimeToChangePump = edrk.pult_data.data_from_pult.TimeToChangePump; - flag_1 = 1; - edrk.pult_data.data.TimeToChangePump = edrk.pult_data.data_from_pult.TimeToChangePump; - } - - // -1 - if (edrk.pult_data.data_from_pult.TimeToChangePump != edrk.pult_data.data.TimeToChangePump - && edrk.pult_data.data_from_pult.TimeToChangePump == -1) - { - edrk.pult_data.data_to_pult.TimeToChangePump = edrk.pult_data.data.TimeToChangePump; - flag_1 = 1; - } - - // build - // -1 - if (edrk.pult_data.data_from_pult.count_build != edrk.pult_data.data.count_build - && edrk.pult_data.data_from_pult.count_build == -1) - { - edrk.pult_data.data_to_pult.count_build = edrk.pult_data.data.count_build; - flag_1 = 1; - } - - if (edrk.pult_data.data_from_pult.count_build != edrk.pult_data.data.count_build) - { - edrk.pult_data.data_to_pult.count_build = edrk.pult_data.data.count_build; - flag_1 = 1; - } - - // revers - // -1 - if (edrk.pult_data.data_from_pult.count_revers != edrk.pult_data.data.count_revers - && edrk.pult_data.data_from_pult.count_revers == -1) - { - edrk.pult_data.data_to_pult.count_revers = edrk.pult_data.data.count_revers; - flag_1 = 1; - } - - if (edrk.pult_data.data_from_pult.count_revers != edrk.pult_data.data.count_revers ) - { - edrk.pult_data.data_to_pult.count_revers = edrk.pult_data.data.count_revers; - flag_1 = 1; - } - // - edrk.pult_data.flagSaveDataPCH = flag_1; - - - // moto - for (i=0;i= 2) - to_store = 2; - else - if (edrk.pult_cmd.log_what_memory >= 1) - to_store = 1; - else - to_store = 0; - - //40198 - ( ) (2), (1), +usb (3) - if (prev_cmd == 0 && cmd && to_store && flag_wait == 0) - { - edrk.pult_data.flagSaveSlowLogs = to_store; - flag_wait = 1; - time_wait_save_log = global_time.miliseconds; - } - - - if (flag_wait) - { - if (detect_pause_milisec(COUNT_WAIT_SAVE_LOG, &time_wait_save_log)) - { - flag_wait = 0; - flag_clear = 1; // - edrk.pult_data.flagSaveSlowLogs = 0; - time_wait_clear_log = global_time.miliseconds; - } - - } - else - { - - } - - - - - if (flag_clear) // - { - edrk.pult_data.flagSaveSlowLogs = 100; - if (detect_pause_milisec(COUNT_WAIT_SAVE_LOG, &time_wait_clear_log)) - { - flag_wait = 0; - flag_clear = 0; - edrk.pult_data.flagSaveSlowLogs = 0; - } - - } - - // 30033: - -// 0 - , ; -// 1 - , ; -// 2 - , ; -// 3 - ; -} - diff --git a/Inu/Src2/551/main/modbus_hmi_update.h b/Inu/Src2/551/main/modbus_hmi_update.h deleted file mode 100644 index 3ed0c05..0000000 --- a/Inu/Src2/551/main/modbus_hmi_update.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef _HMI_UPDATE -#define _HMI_UPDATE - - -#define LIMITER_U_I_PULT 5.0 //10.0 - - -typedef enum { - state_not_init = 0, state_ready1 = 1, state_ready2, state_go, state_assemble, state_fault, state_accident -} Inverter_state; - -void update_tables_HMI(void); -void update_logs_cmd_HMI(void); -void update_tables_HMI_on_inited(int perc_load); - -void update_tables_HMI_analog(void); -void update_tables_HMI_discrete(void); - -int update_progress_load_hmi(int proc_load); - -void setStateHMI(Inverter_state state); -void setElementsColorsHMI(Inverter_state state); - -void get_command_HMI(void); -void func_unpack_answer_from_Ingeteam(unsigned int cc); - -extern int hmi_watch_dog; - -void update_nPCH(void); - -void inc_count_build(void); -void inc_count_revers(void); - -void set_write_slow_logs(int cmd); - -void update_LoggerParams(void); - - -#endif //_HMI_UPDATE diff --git a/Inu/Src2/551/main/modbus_svu_update.c b/Inu/Src2/551/main/modbus_svu_update.c deleted file mode 100644 index d9e6fe2..0000000 --- a/Inu/Src2/551/main/modbus_svu_update.c +++ /dev/null @@ -1,710 +0,0 @@ -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "math.h" -#include "control_station.h" -#include "CAN_Setup.h" -#include "modbus_table_v2.h" -#include "mathlib.h" - -void update_errors_to_svu(void); -void update_protect_levels_to_MPU(void); - -void update_svu_modbus_table(void) -{ - int current_active_control; - - modbus_table_can_out[0].all = edrk.Stop ? 7 : -// edrk.Provorot ? 6 : - edrk.Go ? 5 : - edrk.Status_Ready.bits.ready_final ? 4 : - edrk.to_ing.bits.RASCEPITEL_ON ? 3 : - edrk.SumSbor ? 2 : - edrk.Status_Ready.bits.ready1 ? 1 : 0; - modbus_table_can_out[1].all = edrk.warning; - modbus_table_can_out[2].all = edrk.overheat; -// modbus_table_can_out[3].all = ; -// modbus_table_can_out[4].all = ; - modbus_table_can_out[5].all = edrk.Status_Ready.bits.Batt; - modbus_table_can_out[6].all = edrk.from_ing1.bits.UPC_24V_NORMA | edrk.from_ing1.bits.OP_PIT_NORMA ? 0 : 1; - modbus_table_can_out[7].all = WRotor.RotorDirectionSlow >= 0 ? 0 : 1; - modbus_table_can_out[8].all = edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER || - edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_POWER ? 1 : 0; - current_active_control = get_current_station_control(); - modbus_table_can_out[9].all = current_active_control == CONTROL_STATION_INGETEAM_PULT_RS485 ? 1 : - current_active_control == CONTROL_STATION_MPU_KEY_CAN || current_active_control == CONTROL_STATION_MPU_KEY_RS485 ? 2 : - current_active_control == CONTROL_STATION_ZADATCHIK_CAN ? 3 : - current_active_control == CONTROL_STATION_VPU_CAN ? 4 : - current_active_control == CONTROL_STATION_MPU_SVU_CAN || current_active_control == CONTROL_STATION_MPU_SVU_RS485 ? 5 : - 0; - modbus_table_can_out[10].all = edrk.errors.e2.bits.T_UO1_MAX || edrk.errors.e6.bits.UO1_KEYS ? 3 : - edrk.warnings.e2.bits.T_UO1_MAX ? 2 : 1; - modbus_table_can_out[11].all = edrk.errors.e2.bits.T_UO2_MAX || edrk.errors.e6.bits.UO2_KEYS ? 3 : - edrk.warnings.e2.bits.T_UO2_MAX ? 2 : 1; - modbus_table_can_out[12].all = edrk.errors.e2.bits.T_UO3_MAX || edrk.errors.e6.bits.UO3_KEYS ? 3 : - edrk.warnings.e2.bits.T_UO3_MAX ? 2 : 1; - modbus_table_can_out[13].all = edrk.errors.e2.bits.T_UO4_MAX || edrk.errors.e6.bits.UO4_KEYS ? 3 : - edrk.warnings.e2.bits.T_UO4_MAX ? 2 : 1; - modbus_table_can_out[14].all = edrk.errors.e2.bits.T_UO5_MAX || edrk.errors.e6.bits.UO5_KEYS ? 3 : - edrk.warnings.e2.bits.T_UO5_MAX ? 2 : 1; - modbus_table_can_out[15].all = edrk.errors.e2.bits.T_UO6_MAX || edrk.errors.e6.bits.UO6_KEYS ? 3 : - edrk.warnings.e2.bits.T_UO6_MAX ? 2 : 1; - modbus_table_can_out[16].all = edrk.errors.e2.bits.T_UO7_MAX || edrk.errors.e6.bits.UO7_KEYS ? 3 : - edrk.warnings.e2.bits.T_UO7_MAX ? 2 : 1; - modbus_table_can_out[17].all = edrk.Status_QTV_Ok;// edrk.from_shema.bits.QTV_ON_OFF; - modbus_table_can_out[18].all = edrk.from_svu.bits.BLOCKED; - modbus_table_can_out[19].all = edrk.from_shema_filter.bits.UMP_ON_OFF; - modbus_table_can_out[20].all = edrk.from_shema_filter.bits.READY_UMP; - modbus_table_can_out[21].all = edrk.from_ing1.bits.RASCEPITEL_ON; - modbus_table_can_out[22].all = edrk.from_ing1.bits.UPC_24V_NORMA; - modbus_table_can_out[23].all = edrk.from_ing1.bits.OHLAD_UTE4KA_WATER; - modbus_table_can_out[24].all = edrk.from_ing2.bits.SOST_ZAMKA; - modbus_table_can_out[25].all = edrk.from_ing1.bits.ZARYAD_ON | edrk.from_shema_filter.bits.UMP_ON_OFF; //, , - modbus_table_can_out[26].all = edrk.from_ing1.bits.VENTIL_ON; - modbus_table_can_out[27].all = edrk.to_ing.bits.NASOS_1_ON == 1 && edrk.from_ing1.bits.NASOS_ON == 1 ? 1 : 0; - modbus_table_can_out[28].all = edrk.to_ing.bits.NASOS_2_ON == 1 && edrk.from_ing1.bits.NASOS_ON == 1 ? 1 : 0; - modbus_table_can_out[29].all = edrk.from_ing1.bits.NASOS_NORMA; - modbus_table_can_out[30].all = edrk.from_ing1.bits.ZAZEML_ON; - modbus_table_can_out[31].all = edrk.from_ing1.bits.NAGREV_ON; - modbus_table_can_out[32].all = edrk.from_ing1.bits.BLOCK_IZOL_NORMA == 1 ? 1 : 0; - modbus_table_can_out[33].all = edrk.errors.e5.bits.ERROR_ISOLATE == 0 && edrk.from_ing1.bits.BLOCK_IZOL_NORMA == 1 ? 0 : 1; - modbus_table_can_out[34].all = edrk.from_ing1.bits.ALL_KNOPKA_AVARIA; - - if (edrk.MasterSlave == MODE_MASTER) - modbus_table_can_out[35].all = 1; - else - if (edrk.MasterSlave == MODE_SLAVE) - modbus_table_can_out[35].all = 0; - else - modbus_table_can_out[35].all = 2; // MODE_DONTKNOW - -// modbus_table_can_out[35].all = edrk.MasterSlave == MODE_MASTER ? 1 : 0; - modbus_table_can_out[36].all = edrk.from_ing1.bits.OP_PIT_NORMA & edrk.from_ing1.bits.UPC_24V_NORMA; - modbus_table_can_out[37].all = optical_read_data.status == 1 && optical_write_data.status == 1 ? 1 : 0; - modbus_table_can_out[38].all = edrk.warnings.e7.bits.MASTER_SLAVE_SYNC == 0 ? 1 : 0; - modbus_table_can_out[39].all = fast_round(_IQtoF(filter.iqIm) * NORMA_ACP); - modbus_table_can_out[40].all = fast_round(_IQtoF(filter.iqIin_sum) * NORMA_ACP); - modbus_table_can_out[41].all = fast_round(_IQtoF(filter.iqU_1_long + filter.iqU_2_long) * NORMA_ACP); - - if (filter.iqUin_m1>=filter.iqUin_m2) - modbus_table_can_out[42].all = fast_round(_IQtoF(filter.iqUin_m1) * NORMA_ACP / 1.41); - else - modbus_table_can_out[42].all = fast_round(_IQtoF(filter.iqUin_m2) * NORMA_ACP / 1.41); - - modbus_table_can_out[43].all = fast_round(_IQtoF(filter.iqU_1_long) * NORMA_ACP); - modbus_table_can_out[44].all = fast_round(_IQtoF(filter.iqU_2_long) * NORMA_ACP); - modbus_table_can_out[45].all = fast_round(_IQtoF(filter.iqIm_1) * NORMA_ACP / 1.41); - modbus_table_can_out[46].all = fast_round(_IQtoF(filter.iqIm_1) * NORMA_ACP / 1.41); - modbus_table_can_out[47].all = fast_round(_IQtoF(filter.iqIm_1) * NORMA_ACP / 1.41); - modbus_table_can_out[48].all = fast_round(_IQtoF(filter.iqIm_2) * NORMA_ACP / 1.41); - modbus_table_can_out[49].all = fast_round(_IQtoF(filter.iqIm_2) * NORMA_ACP / 1.41); - modbus_table_can_out[50].all = fast_round(_IQtoF(filter.iqIm_2) * NORMA_ACP / 1.41); - modbus_table_can_out[51].all = fast_round(_IQtoF(filter.iqIin_sum) * NORMA_ACP); -// modbus_table_can_out[52].all = Uvh rms -// modbus_table_can_out[53].all = -// modbus_table_can_out[54].all = -// modbus_table_can_out[55].all = - modbus_table_can_out[56].all = _IQtoF(analog.iqIbreak_1) * NORMA_ACP; - modbus_table_can_out[57].all = _IQtoF(analog.iqIbreak_2) * NORMA_ACP; - - //Temperatures - modbus_table_can_out[58].all = fast_round(edrk.temper_edrk.real_temper_u[0]); - modbus_table_can_out[59].all = fast_round(edrk.temper_edrk.real_temper_u[1]); - modbus_table_can_out[60].all = fast_round(edrk.temper_edrk.real_temper_u[2]); - modbus_table_can_out[61].all = fast_round(edrk.temper_edrk.real_temper_u[3]); - modbus_table_can_out[62].all = fast_round(edrk.temper_edrk.real_temper_u[4]); - modbus_table_can_out[63].all = fast_round(edrk.temper_edrk.real_temper_u[5]); - modbus_table_can_out[64].all = fast_round(edrk.temper_edrk.real_temper_u[6]); - modbus_table_can_out[65].all = fast_round(edrk.temper_edrk.real_temper_water[1]); - modbus_table_can_out[66].all = fast_round(edrk.temper_edrk.real_temper_water[0]); - modbus_table_can_out[67].all = fast_round(edrk.temper_edrk.real_temper_air[0]); - modbus_table_can_out[68].all = fast_round(edrk.temper_edrk.real_temper_air[1]); - modbus_table_can_out[69].all = fast_round(edrk.temper_edrk.real_temper_air[2]); - modbus_table_can_out[70].all = fast_round(edrk.temper_edrk.real_temper_air[3]); - - modbus_table_can_out[71].all = fast_round(edrk.p_water_edrk.real_p_water[0]); - - modbus_table_can_out[72].all = fast_round(_IQtoF(edrk.zadanie.iq_oborots_zad_hz_rmp) * NORMA_FROTOR * 60); - modbus_table_can_out[73].all = edrk.oborots;// fast_round(_IQtoF(WRotor.iqWRotorSumFilter3) * NORMA_FROTOR * 60); - modbus_table_can_out[74].all = edrk.oborots;//fast_round(_IQtoF(WRotor.iqWRotorSumFilter3) * NORMA_FROTOR * 60); //Sensor 1 - modbus_table_can_out[75].all = edrk.oborots;//fast_round(_IQtoF(WRotor.iqWRotorSumFilter3) * NORMA_FROTOR * 60); //Sensor 1 - - modbus_table_can_out[76].all = fast_round(_IQtoF(edrk.zadanie.iq_power_zad_rmp) * NORMA_ACP * NORMA_ACP / 1000.0); - modbus_table_can_out[77].all = fabs(edrk.power_kw);// fast_round(_IQtoF(filter.PowerScalar) * NORMA_ACP* NORMA_ACP / 1000.0); - - modbus_table_can_out[78].all = fast_round(edrk.temper_acdrive.winding.filter_real_temper[0]); - modbus_table_can_out[79].all = fast_round(edrk.temper_acdrive.winding.filter_real_temper[1]); - modbus_table_can_out[80].all = fast_round(edrk.temper_acdrive.winding.filter_real_temper[2]); - modbus_table_can_out[81].all = fast_round(edrk.temper_acdrive.winding.filter_real_temper[3]); - modbus_table_can_out[82].all = fast_round(edrk.temper_acdrive.winding.filter_real_temper[4]); - modbus_table_can_out[83].all = fast_round(edrk.temper_acdrive.winding.filter_real_temper[5]); - modbus_table_can_out[84].all = fast_round(edrk.temper_acdrive.bear.filter_real_temper[0]); // TODO: - modbus_table_can_out[85].all = fast_round(edrk.temper_acdrive.bear.filter_real_temper[1]); // - - modbus_table_can_out[86].all = Unites[UMU_CAN_DEVICE][24]; - modbus_table_can_out[87].all = Unites[UMU_CAN_DEVICE][25]; - modbus_table_can_out[88].all = Unites[UMU_CAN_DEVICE][28]; - modbus_table_can_out[89].all = Unites[UMU_CAN_DEVICE][29]; - modbus_table_can_out[90].all = Unites[UMU_CAN_DEVICE][34]; - modbus_table_can_out[91].all = Unites[UMU_CAN_DEVICE][35]; - modbus_table_can_out[92].all = Unites[UMU_CAN_DEVICE][34]; //, - modbus_table_can_out[93].all = Unites[UMU_CAN_DEVICE][35]; //, - - modbus_table_can_out[94].all = edrk.warnings.e2.bits.T_UO1_MAX | edrk.warnings.e2.bits.T_UO2_MAX | - edrk.warnings.e2.bits.T_UO3_MAX | edrk.warnings.e2.bits.T_UO4_MAX | - edrk.warnings.e2.bits.T_UO5_MAX | edrk.warnings.e2.bits.T_UO6_MAX | - edrk.warnings.e2.bits.T_UO7_MAX; - modbus_table_can_out[95].all = edrk.errors.e2.bits.T_UO1_MAX | edrk.errors.e2.bits.T_UO2_MAX | - edrk.errors.e2.bits.T_UO3_MAX | edrk.errors.e2.bits.T_UO4_MAX | - edrk.errors.e2.bits.T_UO5_MAX | edrk.errors.e2.bits.T_UO6_MAX | - edrk.errors.e2.bits.T_UO7_MAX; - modbus_table_can_out[96].all = edrk.warnings.e2.bits.T_AIR0_MAX | edrk.warnings.e2.bits.T_AIR1_MAX | - edrk.warnings.e2.bits.T_AIR2_MAX | edrk.warnings.e2.bits.T_AIR3_MAX; - modbus_table_can_out[97].all = edrk.errors.e2.bits.T_AIR0_MAX | edrk.errors.e2.bits.T_AIR1_MAX | - edrk.errors.e2.bits.T_AIR2_MAX | edrk.errors.e2.bits.T_AIR3_MAX; - modbus_table_can_out[98].all = edrk.warnings.e2.bits.T_WATER_EXT_MAX; - modbus_table_can_out[99].all = edrk.errors.e2.bits.T_WATER_EXT_MAX; - modbus_table_can_out[100].all = edrk.warnings.e2.bits.T_WATER_INT_MAX; - modbus_table_can_out[101].all = edrk.errors.e2.bits.T_WATER_INT_MAX; - - modbus_table_can_out[102].all = edrk.warnings.e2.bits.P_WATER_INT_MAX; - modbus_table_can_out[103].all = edrk.errors.e2.bits.P_WATER_INT_MAX; - modbus_table_can_out[104].all = edrk.warnings.e2.bits.P_WATER_INT_MIN; - modbus_table_can_out[105].all = edrk.errors.e2.bits.P_WATER_INT_MIN; - - modbus_table_can_out[106].all = edrk.warnings.e7.bits.T_ACDRIVE_WINDING_MAX; - modbus_table_can_out[107].all = edrk.errors.e7.bits.T_ACDRIVE_WINDING_MAX; - modbus_table_can_out[108].all = edrk.warnings.e7.bits.T_ACDRIVE_BEAR_MAX_DNE; // - modbus_table_can_out[109].all = edrk.errors.e7.bits.T_ACDRIVE_BEAR_MAX_DNE; - modbus_table_can_out[110].all = edrk.warnings.e9.bits.T_ACDRIVE_BEAR_MAX_NE; // - modbus_table_can_out[111].all = edrk.errors.e9.bits.T_ACDRIVE_BEAR_MAX_NE; - - modbus_table_can_out[112].all = edrk.warnings.e9.bits.I_GED_MAX; - modbus_table_can_out[113].all = edrk.errors.e1.bits.I_UO2_MAX | edrk.errors.e1.bits.I_UO3_MAX | - edrk.errors.e1.bits.I_UO4_MAX | edrk.errors.e1.bits.I_UO5_MAX | - edrk.errors.e1.bits.I_UO6_MAX | edrk.errors.e1.bits.I_UO7_MAX; //TODO add adc errors - modbus_table_can_out[114].all = edrk.errors.e0.bits.I_1_MAX | edrk.errors.e0.bits.I_2_MAX; - modbus_table_can_out[115].all = edrk.errors.e0.bits.U_1_MAX | edrk.errors.e0.bits.U_2_MAX; - modbus_table_can_out[116].all = edrk.warnings.e0.bits.U_IN_MIN; - modbus_table_can_out[117].all = edrk.errors.e0.bits.U_IN_MIN; - modbus_table_can_out[118].all = edrk.warnings.e0.bits.U_IN_MAX; - modbus_table_can_out[119].all = edrk.errors.e0.bits.U_IN_MAX; - modbus_table_can_out[120].all = edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK; //TODO - modbus_table_can_out[121].all = edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK; //TODO - modbus_table_can_out[122].all = edrk.Kvitir; - - - modbus_table_can_out[137].all = edrk.pult_data.data_from_pult.moto[15]; - modbus_table_can_out[138].all = edrk.pult_data.data_from_pult.moto[6]; - - update_errors_to_svu(); - update_protect_levels_to_MPU(); - - copy_from_can_out_to_rs_out(); - -} - -#define MPU_ADRESS_CMD_START 122 // -1 14, 122 123 14 -#define MPU_ADRESS_CMD_END 144 //138 // - -#if (MPU_ADRESS_CMD_END>=SIZE_MODBUS_TABLE) -#define MPU_ADRESS_CMD_END (SIZE_MODBUS_TABLE-1) -#endif - - -#if (MPU_ADRESS_CMD_END - MPU_ADRESS_CMD_START +1 )>CONTROL_STATION_MAX_RAW_DATA -#define MPU_LENGTH_CMD CONTROL_STATION_MAX_RAW_DATA -#else -#define MPU_LENGTH_CMD (MPU_ADRESS_CMD_END - MPU_ADRESS_CMD_START + 1) -#endif -///////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////// - -void unpack_answer_from_MPU_SVU_CAN_filter(unsigned int cc) -{ - unsigned int i = 0, j = 0, k, max_data; - - for (i = 0; i < MPU_LENGTH_CMD; i++) - { - max_data = 0;//control_station.raw_array_data_temp[cc][i][0].all; - //min_data = 0;//control_station.raw_array_data_temp[cc][i][0].all; - - for (j=0; j max_data) - max_data = control_station.raw_array_data_temp[cc][i][j].all; - } - - control_station.raw_array_data[cc][i].all = max_data; - - } - -} - -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// - - -void unpack_answer_from_MPU_SVU_CAN(unsigned int cc) { - int i = 0, j = 0, k; -// static unsigned int prev_CAN_count_cycle_input_units = 0; - - if (control_station.prev_CAN_count_cycle_input_units[cc] != mpu_can_setup.CAN_count_cycle_input_units[0]) - { - k = control_station.count_raw_array_data_temp[cc]; - for (i = 0, j = 0; i < MPU_LENGTH_CMD && j < CONTROL_STATION_MAX_RAW_DATA; i++, j++) - { - control_station.raw_array_data_temp[cc][j][k].all = modbus_table_can_in[MPU_ADRESS_CMD_START+i].all; - } - - control_station.count_raw_array_data_temp[cc]++; - if (control_station.count_raw_array_data_temp[cc]>=CONTROL_STATION_MAX_RAW_DATA_TEMP) - control_station.count_raw_array_data_temp[cc] = 0; - - control_station.prev_CAN_count_cycle_input_units[cc] = mpu_can_setup.CAN_count_cycle_input_units[0]; - } - - -// for (i = ADRESS_CMD_START, j = 0; i < SIZE_MODBUS_TABLE && j < CONTROL_STATION_MAX_RAW_DATA; i++, j++) -// { -// control_station.raw_array_data[cc][j].all = modbus_table_can_in[i].all; -// } - - unpack_answer_from_MPU_SVU_CAN_filter(cc); - -} - -void unpack_answer_from_MPU_SVU_RS(unsigned int cc) { - int i = 0, j = 0; - for (i = MPU_ADRESS_CMD_START, j = 0; i < SIZE_MODBUS_TABLE && j < CONTROL_STATION_MAX_RAW_DATA; i++, j++) - control_station.raw_array_data[cc][j].all = modbus_table_rs_in[i].all; -} - -void update_errors_to_svu() { - modbus_table_can_out[208].bit.bit0 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk0_ack; - modbus_table_can_out[208].bit.bit1 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk1_ack; - modbus_table_can_out[208].bit.bit2 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk2_ack; - modbus_table_can_out[208].bit.bit3 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk3_ack; - modbus_table_can_out[208].bit.bit4 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk4_ack; - modbus_table_can_out[208].bit.bit5 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk5_ack; - modbus_table_can_out[208].bit.bit6 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk6_ack; - modbus_table_can_out[208].bit.bit7 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk7_ack; - - modbus_table_can_out[208].bit.bit8 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk0_ack; - modbus_table_can_out[208].bit.bit9 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk1_ack; - modbus_table_can_out[208].bit.bit10 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk2_ack; - modbus_table_can_out[208].bit.bit11 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk3_ack; - modbus_table_can_out[208].bit.bit12 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk4_ack; - modbus_table_can_out[208].bit.bit13 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk5_ack; - modbus_table_can_out[208].bit.bit14 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk6_ack; - modbus_table_can_out[208].bit.bit15 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk7_ack; - - modbus_table_can_out[200].bit.bit0 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk0_ack; - modbus_table_can_out[200].bit.bit1 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk1_ack; - modbus_table_can_out[200].bit.bit2 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk2_ack; - modbus_table_can_out[200].bit.bit3 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk3_ack; - modbus_table_can_out[200].bit.bit4 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk4_ack; - modbus_table_can_out[200].bit.bit5 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk5_ack; - modbus_table_can_out[200].bit.bit6 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk6_ack; - modbus_table_can_out[200].bit.bit7 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk7_ack; - modbus_table_can_out[200].bit.bit8 = project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk0_ack; - modbus_table_can_out[200].bit.bit9 = project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk1_ack; - - if (edrk.flag_second_PCH == 1) { - modbus_table_can_out[200].bit.bit10 = edrk.errors.e4.bits.ANOTHER_BS_POWER_OFF; - } else { - modbus_table_can_out[200].bit.bit11 = edrk.errors.e4.bits.ANOTHER_BS_POWER_OFF; - } - - modbus_table_can_out[201].bit.bit0 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk0_current; - modbus_table_can_out[201].bit.bit1 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk1_current; - modbus_table_can_out[201].bit.bit2 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk2_current; - modbus_table_can_out[201].bit.bit3 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk3_current; - modbus_table_can_out[201].bit.bit4 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk4_current; - modbus_table_can_out[201].bit.bit5 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk5_current; - modbus_table_can_out[201].bit.bit6 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk6_current; - modbus_table_can_out[201].bit.bit7 = project.cds_tk[0].read.sbus.status_protect_current_ack.bit.tk7_current; - modbus_table_can_out[201].bit.bit8 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk0_current; - modbus_table_can_out[201].bit.bit9 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk1_current; - modbus_table_can_out[201].bit.bit10 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk2_current; - modbus_table_can_out[201].bit.bit11 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk3_current; - modbus_table_can_out[201].bit.bit12 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk4_current; - modbus_table_can_out[201].bit.bit13 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk5_current; - modbus_table_can_out[201].bit.bit14 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk6_current; - modbus_table_can_out[201].bit.bit15 = project.cds_tk[1].read.sbus.status_protect_current_ack.bit.tk7_current; - - modbus_table_can_out[202].bit.bit0 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk0_current; - modbus_table_can_out[202].bit.bit1 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk1_current; - modbus_table_can_out[202].bit.bit2 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk2_current; - modbus_table_can_out[202].bit.bit3 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk3_current; - modbus_table_can_out[202].bit.bit4 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk4_current; - modbus_table_can_out[202].bit.bit5 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk5_current; - modbus_table_can_out[202].bit.bit6 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk6_current; - modbus_table_can_out[202].bit.bit7 = project.cds_tk[2].read.sbus.status_protect_current_ack.bit.tk7_current; - modbus_table_can_out[202].bit.bit8 = project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk0_current; - modbus_table_can_out[202].bit.bit9 = project.cds_tk[3].read.sbus.status_protect_current_ack.bit.tk1_current; - - modbus_table_can_out[203].bit.bit0 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit1 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit2 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit3 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit4 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[203].bit.bit5 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[203].bit.bit6 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[203].bit.bit7 = project.cds_tk[0].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[203].bit.bit8 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit9 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit10 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit11 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[203].bit.bit12 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[203].bit.bit13 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[203].bit.bit14 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[203].bit.bit15 = project.cds_tk[1].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - - modbus_table_can_out[204].bit.bit0 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[204].bit.bit1 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[204].bit.bit2 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[204].bit.bit3 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[204].bit.bit4 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[204].bit.bit5 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[204].bit.bit6 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[204].bit.bit7 = project.cds_tk[2].read.sbus.lock_status_error.bit.mintime_err_keys_7654; - modbus_table_can_out[204].bit.bit8 = project.cds_tk[3].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - modbus_table_can_out[204].bit.bit9 = project.cds_tk[3].read.sbus.lock_status_error.bit.mintime_err_keys_3210; - - modbus_table_can_out[205].bit.bit0 = edrk.errors.e3.bits.NOT_READY_TK_0; - modbus_table_can_out[205].bit.bit1 = edrk.errors.e3.bits.NOT_READY_TK_1; - modbus_table_can_out[205].bit.bit2 = edrk.errors.e3.bits.NOT_READY_TK_2; - modbus_table_can_out[205].bit.bit3 = edrk.errors.e3.bits.NOT_READY_TK_3; - modbus_table_can_out[205].bit.bit4 = edrk.errors.e3.bits.NOT_READY_IN_0; - modbus_table_can_out[205].bit.bit5 = edrk.errors.e3.bits.NOT_READY_IN_1; - modbus_table_can_out[205].bit.bit6 = edrk.errors.e3.bits.NOT_READY_OUT_0; - modbus_table_can_out[205].bit.bit7 = edrk.errors.e3.bits.NOT_READY_ADC_0; - modbus_table_can_out[205].bit.bit8 = edrk.errors.e3.bits.NOT_READY_HWP_0; - modbus_table_can_out[205].bit.bit9 = edrk.errors.e3.bits.NOT_READY_ADC_1; - modbus_table_can_out[205].bit.bit10 = edrk.errors.e3.bits.NOT_READY_CONTR; - - modbus_table_can_out[206].bit.bit0 = edrk.errors.e5.bits.KEY_AVARIA; - modbus_table_can_out[206].bit.bit1 = edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER; - modbus_table_can_out[206].bit.bit2 = edrk.errors.e7.bits.SVU_BLOCK_ON_QTV; - modbus_table_can_out[206].bit.bit3 = edrk.errors.e7.bits.UMP_NOT_ANSWER; - modbus_table_can_out[206].bit.bit4 = edrk.errors.e7.bits.UMP_NOT_READY; - modbus_table_can_out[206].bit.bit5 = edrk.errors.e6.bits.RASCEPITEL_ERROR_NOT_ANSWER; - modbus_table_can_out[206].bit.bit6 = edrk.errors.e7.bits.ANOTHER_RASCEPITEL_ON; - modbus_table_can_out[206].bit.bit7 = edrk.warnings.e7.bits.AUTO_SET_MASTER; - modbus_table_can_out[206].bit.bit8 = edrk.errors.e7.bits.ANOTHER_PCH_NOT_ANSWER; - modbus_table_can_out[206].bit.bit9 = edrk.errors.e8.bits.WDOG_OPTICAL_BUS; - modbus_table_can_out[206].bit.bit10 = edrk.errors.e6.bits.QTV_ERROR_NOT_ANSWER; // - modbus_table_can_out[206].bit.bit11 = edrk.errors.e1.bits.NO_CONFIRM_ON_RASCEPITEL; // - modbus_table_can_out[206].bit.bit12 = edrk.errors.e6.bits.RASCEPITEL_ERROR_NOT_ANSWER; - modbus_table_can_out[206].bit.bit13 = edrk.errors.e1.bits.ANOTHER_BS_VERY_LONG_WAIT; - modbus_table_can_out[206].bit.bit14 = edrk.errors.e1.bits.VERY_LONG_BOTH_READY2; - modbus_table_can_out[206].bit.bit15 = edrk.errors.e1.bits.BOTH_KEYS_CHARGE_DISCHARGE; - - - modbus_table_can_out[207].bit.bit0 = !control_station.alive_control_station[CONTROL_STATION_ZADATCHIK_CAN]; - modbus_table_can_out[207].bit.bit1 = !control_station.alive_control_station[CONTROL_STATION_MPU_SVU_CAN]; - modbus_table_can_out[207].bit.bit2 = 0;// CAN_timeout[get_real_in_mbox(UNITS_TYPE_BOX,UMU_CAN_DEVICE)]; - modbus_table_can_out[207].bit.bit3 = CAN_timeout[get_real_in_mbox(UNITS_TYPE_BOX,BKSSD_CAN_DEVICE)]; - modbus_table_can_out[207].bit.bit4 = CAN_timeout[get_real_in_mbox(UNITS_TYPE_BOX,VPU_CAN)]; - modbus_table_can_out[207].bit.bit5 = edrk.warnings.e7.bits.CAN2CAN_BS; - modbus_table_can_out[207].bit.bit6 = edrk.errors.e7.bits.CAN2CAN_BS; - - if (edrk.flag_second_PCH == 1) { - modbus_table_can_out[207].bit.bit7 = edrk.warnings.e4.bits.ANOTHER_BS_POWER_OFF; - } else { - modbus_table_can_out[207].bit.bit8 = edrk.warnings.e4.bits.ANOTHER_BS_POWER_OFF; - } - modbus_table_can_out[207].bit.bit9 = edrk.errors.e7.bits.ANOTHER_BS_ALARM; // ( ) - modbus_table_can_out[207].bit.bit10 = edrk.warnings.e7.bits.READ_OPTBUS; // - - modbus_table_can_out[209].bit.bit0 = edrk.errors.e5.bits.OP_PIT; - modbus_table_can_out[209].bit.bit1 = edrk.errors.e5.bits.UTE4KA_WATER; - modbus_table_can_out[209].bit.bit2 = edrk.errors.e1.bits.BLOCK_DOOR; - modbus_table_can_out[209].bit.bit3 = edrk.errors.e7.bits.UMP_NOT_READY; - modbus_table_can_out[209].bit.bit4 = edrk.errors.e5.bits.FAN; - modbus_table_can_out[209].bit.bit5 = edrk.errors.e5.bits.PUMP_1; - modbus_table_can_out[209].bit.bit6 = edrk.errors.e5.bits.PRE_READY_PUMP; - modbus_table_can_out[209].bit.bit7 = edrk.errors.e5.bits.ERROR_HEAT; - modbus_table_can_out[209].bit.bit8 = edrk.warnings.e5.bits.ERROR_ISOLATE; - modbus_table_can_out[209].bit.bit9 = edrk.errors.e5.bits.ERROR_PRED_VIPR; - modbus_table_can_out[209].bit.bit10 = edrk.errors.e5.bits.ERROR_ISOLATE; - modbus_table_can_out[209].bit.bit11 = edrk.errors.e5.bits.POWER_UPC; - modbus_table_can_out[209].bit.bit12 = edrk.errors.e5.bits.ERROR_GROUND_NET; - modbus_table_can_out[209].bit.bit13 = edrk.errors.e5.bits.PUMP_2; - modbus_table_can_out[209].bit.bit14 = edrk.from_ing1.bits.BLOCK_IZOL_NORMA ? 0 : 1; - - modbus_table_can_out[210].bit.bit0 = edrk.errors.e0.bits.U_1_MAX; - modbus_table_can_out[210].bit.bit1 = edrk.errors.e0.bits.U_2_MAX; - modbus_table_can_out[210].bit.bit2 = edrk.errors.e0.bits.U_1_MIN; - modbus_table_can_out[210].bit.bit3 = edrk.errors.e0.bits.U_2_MIN; - modbus_table_can_out[210].bit.bit4 = edrk.errors.e0.bits.U_A1B1_MAX; - modbus_table_can_out[210].bit.bit5 = edrk.errors.e0.bits.U_A2B2_MAX; - modbus_table_can_out[210].bit.bit6 = edrk.errors.e0.bits.U_B1C1_MAX; - modbus_table_can_out[210].bit.bit7 = edrk.errors.e0.bits.U_B2C2_MAX; - modbus_table_can_out[210].bit.bit8 = edrk.errors.e0.bits.U_A1B1_MIN; - modbus_table_can_out[210].bit.bit9 = edrk.errors.e0.bits.U_A2B2_MIN; - modbus_table_can_out[210].bit.bit10 = edrk.errors.e0.bits.U_B1C1_MIN; - modbus_table_can_out[210].bit.bit11 = edrk.errors.e0.bits.U_B2C2_MIN; - modbus_table_can_out[210].bit.bit12 = edrk.errors.e0.bits.U_IN_MAX; - modbus_table_can_out[210].bit.bit13 = edrk.errors.e0.bits.U_IN_MIN; - - modbus_table_can_out[211].bit.bit0 = edrk.errors.e1.bits.I_UO2_MAX; - modbus_table_can_out[211].bit.bit1 = edrk.errors.e1.bits.I_UO3_MAX; - modbus_table_can_out[211].bit.bit2 = edrk.errors.e1.bits.I_UO4_MAX; - modbus_table_can_out[211].bit.bit3 = edrk.errors.e1.bits.I_UO5_MAX; - modbus_table_can_out[211].bit.bit4 = edrk.errors.e1.bits.I_UO6_MAX; - modbus_table_can_out[211].bit.bit5 = edrk.errors.e1.bits.I_UO7_MAX; - modbus_table_can_out[211].bit.bit6 = edrk.errors.e1.bits.I_BREAK_1_MAX; - modbus_table_can_out[211].bit.bit7 = edrk.errors.e1.bits.I_BREAK_2_MAX; - modbus_table_can_out[211].bit.bit8 = edrk.errors.e0.bits.I_1_MAX; - modbus_table_can_out[211].bit.bit9 = edrk.errors.e0.bits.I_1_MAX; - - modbus_table_can_out[211].bit.bit10 = edrk.warnings.e2.bits.T_AIR0_MAX; - modbus_table_can_out[211].bit.bit11 = edrk.warnings.e2.bits.T_AIR1_MAX; - modbus_table_can_out[211].bit.bit12 = edrk.warnings.e2.bits.T_AIR2_MAX; - modbus_table_can_out[211].bit.bit13 = edrk.warnings.e2.bits.T_AIR3_MAX; - - if (edrk.power_limit.all) - modbus_table_can_out[211].bit.bit14 = 1; - else - modbus_table_can_out[211].bit.bit14 = 0; - - modbus_table_can_out[211].bit.bit15 = edrk.warnings.e10.bits.WARNING_I_OUT_OVER_1_6_NOMINAL; - - modbus_table_can_out[212].bit.bit0 = edrk.errors.e5.bits.ERROR_PRE_CHARGE_ON; - modbus_table_can_out[212].bit.bit1 = edrk.errors.e7.bits.ERROR_SBOR_SHEMA; - - - - - modbus_table_can_out[213].bit.bit0 = project.hwp[0].read.comp_s.plus.bit.ch0; - modbus_table_can_out[213].bit.bit1 = project.hwp[0].read.comp_s.plus.bit.ch1; - modbus_table_can_out[213].bit.bit2 = project.hwp[0].read.comp_s.plus.bit.ch2; - modbus_table_can_out[213].bit.bit3 = project.hwp[0].read.comp_s.plus.bit.ch3; - modbus_table_can_out[213].bit.bit4 = project.hwp[0].read.comp_s.plus.bit.ch4; - modbus_table_can_out[213].bit.bit5 = project.hwp[0].read.comp_s.plus.bit.ch5; - modbus_table_can_out[213].bit.bit6 = project.hwp[0].read.comp_s.plus.bit.ch6; - modbus_table_can_out[213].bit.bit7 = project.hwp[0].read.comp_s.plus.bit.ch7; - modbus_table_can_out[213].bit.bit8 = 0; - modbus_table_can_out[213].bit.bit9 = project.hwp[0].read.comp_s.plus.bit.ch9; - modbus_table_can_out[213].bit.bit10 = project.hwp[0].read.comp_s.plus.bit.ch10; - modbus_table_can_out[213].bit.bit11 = project.hwp[0].read.comp_s.plus.bit.ch11; - modbus_table_can_out[213].bit.bit12 = project.hwp[0].read.comp_s.plus.bit.ch12; - modbus_table_can_out[213].bit.bit13 = project.hwp[0].read.comp_s.plus.bit.ch13; - modbus_table_can_out[213].bit.bit14 = project.hwp[0].read.comp_s.plus.bit.ch14; - modbus_table_can_out[213].bit.bit15 = project.hwp[0].read.comp_s.plus.bit.ch15; - - modbus_table_can_out[214].bit.bit0 = edrk.errors.e5.bits.LINE_ERR0; - modbus_table_can_out[214].bit.bit1 = edrk.errors.e5.bits.LINE_HWP; - modbus_table_can_out[214].bit.bit2 = project.hwp[0].read.comp_s.minus.bit.ch2; - modbus_table_can_out[214].bit.bit3 = project.hwp[0].read.comp_s.minus.bit.ch3; - modbus_table_can_out[214].bit.bit4 = project.hwp[0].read.comp_s.minus.bit.ch4; - modbus_table_can_out[214].bit.bit5 = project.hwp[0].read.comp_s.minus.bit.ch5; - modbus_table_can_out[214].bit.bit6 = project.hwp[0].read.comp_s.minus.bit.ch6; - modbus_table_can_out[214].bit.bit7 = project.hwp[0].read.comp_s.minus.bit.ch7; - modbus_table_can_out[214].bit.bit8 = 0; - modbus_table_can_out[214].bit.bit9 = project.hwp[0].read.comp_s.minus.bit.ch9; - modbus_table_can_out[214].bit.bit10 = project.hwp[0].read.comp_s.minus.bit.ch10; - modbus_table_can_out[214].bit.bit11 = project.hwp[0].read.comp_s.minus.bit.ch11; - modbus_table_can_out[214].bit.bit12 = project.hwp[0].read.comp_s.minus.bit.ch12; - modbus_table_can_out[214].bit.bit13 = project.hwp[0].read.comp_s.minus.bit.ch13; - modbus_table_can_out[214].bit.bit14 = project.hwp[0].read.comp_s.minus.bit.ch14; - modbus_table_can_out[214].bit.bit15 = project.hwp[0].read.comp_s.minus.bit.ch15; - - modbus_table_can_out[215].bit.bit0 = edrk.errors.e8.bits.LOSS_INPUT_A1B1 | edrk.errors.e9.bits.DISBALANCE_Uin_1; //TODO: - modbus_table_can_out[215].bit.bit1 = edrk.errors.e8.bits.LOSS_INPUT_B1C1 | edrk.errors.e9.bits.DISBALANCE_Uin_1; - modbus_table_can_out[215].bit.bit2 = edrk.errors.e8.bits.LOSS_INPUT_A2B2 | edrk.errors.e9.bits.DISBALANCE_Uin_2; - modbus_table_can_out[215].bit.bit3 = edrk.errors.e8.bits.LOSS_INPUT_B2C2 | edrk.errors.e9.bits.DISBALANCE_Uin_2; - modbus_table_can_out[215].bit.bit4 = edrk.errors.e9.bits.U_IN_FREQ_NOT_NORMA; - modbus_table_can_out[215].bit.bit5 = edrk.errors.e9.bits.U_IN_FREQ_NOT_STABLE; - modbus_table_can_out[215].bit.bit6 = edrk.errors.e7.bits.READ_OPTBUS | edrk.errors.e7.bits.WRITE_OPTBUS; - modbus_table_can_out[215].bit.bit7 = edrk.errors.e7.bits.MASTER_SLAVE_SYNC; - modbus_table_can_out[215].bit.bit8 = project.controller.read.errors_buses.bit.err_sbus; - modbus_table_can_out[215].bit.bit9 = project.controller.read.errors.bit.error_pbus || project.controller.read.errors_buses.bit.slave_addr_error - || project.controller.read.errors_buses.bit.count_error_pbus; - modbus_table_can_out[215].bit.bit10 = edrk.errors.e6.bits.ER_DISBAL_BATT; - modbus_table_can_out[215].bit.bit11 = edrk.errors.e6.bits.QTV_ERROR_NOT_U; - modbus_table_can_out[215].bit.bit12 = edrk.errors.e6.bits.ERROR_PRE_CHARGE_U; - modbus_table_can_out[215].bit.bit13 = edrk.errors.e8.bits.U_IN_20_PROCENTS_HIGH; - modbus_table_can_out[215].bit.bit14 = edrk.errors.e8.bits.U_IN_10_PROCENTS_LOW; - modbus_table_can_out[215].bit.bit15 = edrk.errors.e8.bits.U_IN_20_PROCENTS_LOW; - - modbus_table_can_out[216].bit.bit0 = project.cds_tk[0].read.sbus.lock_status_error.bit.err_power; - modbus_table_can_out[216].bit.bit1 = project.cds_tk[1].read.sbus.lock_status_error.bit.err_power; - modbus_table_can_out[216].bit.bit2 = project.cds_tk[2].read.sbus.lock_status_error.bit.err_power; - modbus_table_can_out[216].bit.bit3 = project.cds_tk[3].read.sbus.lock_status_error.bit.err_power; - modbus_table_can_out[216].bit.bit4 = project.cds_in[0].read.sbus.lock_status_error.bit.err_power; - modbus_table_can_out[216].bit.bit5 = project.cds_in[1].read.sbus.lock_status_error.bit.err_power; - modbus_table_can_out[216].bit.bit6 = project.cds_out[0].read.sbus.lock_status_error.bit.err_power; - modbus_table_can_out[216].bit.bit7 = edrk.errors.e7.bits.NOT_VALID_CONTROL_STATION; - modbus_table_can_out[216].bit.bit8 = edrk.errors.e8.bits.LOSS_OUTPUT_U1; - modbus_table_can_out[216].bit.bit9 = edrk.errors.e8.bits.LOSS_OUTPUT_V1; - modbus_table_can_out[216].bit.bit10 = edrk.errors.e8.bits.LOSS_OUTPUT_W1; - modbus_table_can_out[216].bit.bit11 = edrk.errors.e8.bits.LOSS_OUTPUT_U2; - modbus_table_can_out[216].bit.bit12 = edrk.errors.e8.bits.LOSS_OUTPUT_V2; - modbus_table_can_out[216].bit.bit13 = edrk.errors.e8.bits.LOSS_OUTPUT_W2; - modbus_table_can_out[216].bit.bit14 = edrk.errors.e8.bits.DISBALANCE_IM1_IM2; - modbus_table_can_out[216].bit.bit15 = edrk.errors.e7.bits.VERY_FAST_GO_0to1; - - modbus_table_can_out[217].bit.bit0 = project.cds_tk[0].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit1 = project.cds_tk[1].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit2 = project.cds_tk[2].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit3 = project.cds_tk[3].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit4 = project.cds_in[0].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit5 = project.cds_in[1].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit6 = project.cds_out[0].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit7 = project.adc[0].read.sbus.lock_status_error.bit.err_switch; - modbus_table_can_out[217].bit.bit8 = 0; - modbus_table_can_out[217].bit.bit9 = project.adc[1].read.sbus.lock_status_error.bit.err_switch; - - modbus_table_can_out[218].bit.bit0 = edrk.warnings.e2.bits.T_UO1_MAX; - modbus_table_can_out[218].bit.bit1 = edrk.warnings.e2.bits.T_UO2_MAX; - modbus_table_can_out[218].bit.bit2 = edrk.warnings.e2.bits.T_UO3_MAX; - modbus_table_can_out[218].bit.bit3 = edrk.warnings.e2.bits.T_UO4_MAX; - modbus_table_can_out[218].bit.bit4 = edrk.warnings.e2.bits.T_UO5_MAX; - modbus_table_can_out[218].bit.bit5 = edrk.warnings.e2.bits.T_UO6_MAX; - modbus_table_can_out[218].bit.bit6 = edrk.warnings.e2.bits.T_UO7_MAX; - modbus_table_can_out[218].bit.bit7 = edrk.errors.e2.bits.T_UO1_MAX; - modbus_table_can_out[218].bit.bit8 = edrk.errors.e2.bits.T_UO2_MAX; - modbus_table_can_out[218].bit.bit9 = edrk.errors.e2.bits.T_UO3_MAX; - modbus_table_can_out[218].bit.bit10 = edrk.errors.e2.bits.T_UO4_MAX; - modbus_table_can_out[218].bit.bit11 = edrk.errors.e2.bits.T_UO5_MAX; - modbus_table_can_out[218].bit.bit12 = edrk.errors.e2.bits.T_UO6_MAX; - modbus_table_can_out[218].bit.bit13 = edrk.errors.e2.bits.T_UO7_MAX; - - modbus_table_can_out[219].bit.bit0 = edrk.warnings.e7.bits.READ_OPTBUS | edrk.warnings.e7.bits.WRITE_OPTBUS; - modbus_table_can_out[219].bit.bit1 = edrk.warnings.e7.bits.MASTER_SLAVE_SYNC; - modbus_table_can_out[219].bit.bit2 = edrk.errors.e6.bits.ERROR_PRE_CHARGE_ANSWER; - modbus_table_can_out[219].bit.bit3 = edrk.warnings.e1.bits.NO_INPUT_SYNC_SIGNAL; - modbus_table_can_out[219].bit.bit4 = edrk.errors.e1.bits.NO_INPUT_SYNC_SIGNAL; - modbus_table_can_out[219].bit.bit5 = edrk.errors.e3.bits.ERR_INT_PWM_LONG - || edrk.errors.e9.bits.ERR_PWM_WDOG - || edrk.errors.e9.bits.ERR_INT_PWM_VERY_LONG; - modbus_table_can_out[219].bit.bit15 = edrk.errors.e5.bits.T_VIPR_MAX; - - modbus_table_can_out[220].bit.bit0 = edrk.errors.e2.bits.T_AIR0_MAX; - modbus_table_can_out[220].bit.bit1 = edrk.errors.e2.bits.T_AIR1_MAX; - modbus_table_can_out[220].bit.bit2 = edrk.errors.e2.bits.T_AIR2_MAX; - modbus_table_can_out[220].bit.bit3 = edrk.errors.e2.bits.T_AIR3_MAX; - modbus_table_can_out[220].bit.bit4 = edrk.warnings.e10.bits.T_BSU_Sensor_BK1; - modbus_table_can_out[220].bit.bit5 = edrk.errors.e10.bits.T_BSU_Sensor_BK1; - modbus_table_can_out[220].bit.bit6 = edrk.warnings.e10.bits.T_BSU_Sensor_BK2; - modbus_table_can_out[220].bit.bit7 = edrk.errors.e10.bits.T_BSU_Sensor_BK2; - - modbus_table_can_out[220].bit.bit8 = edrk.errors.e2.bits.T_WATER_EXT_MAX; - modbus_table_can_out[220].bit.bit9 = edrk.errors.e2.bits.T_WATER_INT_MAX; - modbus_table_can_out[220].bit.bit10 = edrk.warnings.e2.bits.T_WATER_EXT_MAX; - modbus_table_can_out[220].bit.bit11 = edrk.warnings.e2.bits.T_WATER_INT_MAX; - - modbus_table_can_out[220].bit.bit12 = edrk.errors.e7.bits.T_ACDRIVE_BEAR_MAX_DNE; - modbus_table_can_out[220].bit.bit13 = edrk.errors.e9.bits.T_ACDRIVE_BEAR_MAX_NE; - modbus_table_can_out[220].bit.bit14 = edrk.warnings.e7.bits.T_ACDRIVE_BEAR_MAX_DNE; - modbus_table_can_out[220].bit.bit15 = edrk.warnings.e9.bits.T_ACDRIVE_BEAR_MAX_NE; - - modbus_table_can_out[221].bit.bit0 = edrk.errors.e10.bits.T_ACDRIVE_WINDING_U1; - modbus_table_can_out[221].bit.bit1 = edrk.errors.e10.bits.T_ACDRIVE_WINDING_V1; - modbus_table_can_out[221].bit.bit2 = edrk.errors.e10.bits.T_ACDRIVE_WINDING_W1; - modbus_table_can_out[221].bit.bit3 = edrk.errors.e10.bits.T_ACDRIVE_WINDING_U2; - modbus_table_can_out[221].bit.bit4 = edrk.errors.e10.bits.T_ACDRIVE_WINDING_V2; - modbus_table_can_out[221].bit.bit5 = edrk.errors.e10.bits.T_ACDRIVE_WINDING_W2; - modbus_table_can_out[221].bit.bit6 = edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U1; - modbus_table_can_out[221].bit.bit7 = edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V1; - modbus_table_can_out[221].bit.bit8 = edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W1; - modbus_table_can_out[221].bit.bit9 = edrk.warnings.e10.bits.T_ACDRIVE_WINDING_U2; - modbus_table_can_out[221].bit.bit10 = edrk.warnings.e10.bits.T_ACDRIVE_WINDING_V2; - modbus_table_can_out[221].bit.bit11 = edrk.warnings.e10.bits.T_ACDRIVE_WINDING_W2; - - modbus_table_can_out[222].bit.bit0 = edrk.errors.e2.bits.P_WATER_INT_MAX; - modbus_table_can_out[222].bit.bit1 = edrk.errors.e2.bits.P_WATER_INT_MIN; - modbus_table_can_out[222].bit.bit2 = edrk.warnings.e2.bits.P_WATER_INT_MAX; - modbus_table_can_out[222].bit.bit3 = edrk.warnings.e2.bits.P_WATER_INT_MIN; - - - modbus_table_can_out[223].all = edrk.errors.e11.all; - - - -} - -void update_protect_levels_to_MPU() { - modbus_table_can_out[139].all = protect_levels.abnormal_temper_acdrive_winding_U1 / 10; - modbus_table_can_out[140].all = protect_levels.abnormal_temper_acdrive_winding_V1 / 10; - modbus_table_can_out[141].all = protect_levels.abnormal_temper_acdrive_winding_W1 / 10; - modbus_table_can_out[142].all = protect_levels.abnormal_temper_acdrive_winding_U2 / 10; - modbus_table_can_out[143].all = protect_levels.abnormal_temper_acdrive_winding_V2 / 10; - modbus_table_can_out[144].all = protect_levels.abnormal_temper_acdrive_winding_W2 / 10; - modbus_table_can_out[145].all = protect_levels.abnormal_temper_acdrive_bear_DNE / 10; - modbus_table_can_out[146].all = protect_levels.abnormal_temper_acdrive_bear_NE / 10; - - modbus_table_can_out[147].all = protect_levels.alarm_temper_acdrive_winding_U1 / 10; - modbus_table_can_out[148].all = protect_levels.alarm_temper_acdrive_winding_V1 / 10; - modbus_table_can_out[149].all = protect_levels.alarm_temper_acdrive_winding_W1 / 10; - modbus_table_can_out[150].all = protect_levels.alarm_temper_acdrive_winding_U2 / 10; - modbus_table_can_out[151].all = protect_levels.alarm_temper_acdrive_winding_V2 / 10; - modbus_table_can_out[152].all = protect_levels.alarm_temper_acdrive_winding_W2 / 10; - modbus_table_can_out[153].all = protect_levels.alarm_temper_acdrive_bear_DNE / 10; - modbus_table_can_out[154].all = protect_levels.alarm_temper_acdrive_bear_NE / 10; - - modbus_table_can_out[155].all = protect_levels.abnormal_temper_u_01 / 10; - modbus_table_can_out[156].all = protect_levels.abnormal_temper_u_02 / 10; - modbus_table_can_out[157].all = protect_levels.abnormal_temper_u_03 / 10; - modbus_table_can_out[158].all = protect_levels.abnormal_temper_u_04 / 10; - modbus_table_can_out[159].all = protect_levels.abnormal_temper_u_05 / 10; - modbus_table_can_out[160].all = protect_levels.abnormal_temper_u_06 / 10; - modbus_table_can_out[161].all = protect_levels.abnormal_temper_u_07 / 10; - modbus_table_can_out[162].all = protect_levels.alarm_temper_u_01 / 10; - modbus_table_can_out[163].all = protect_levels.alarm_temper_u_02 / 10; - modbus_table_can_out[164].all = protect_levels.alarm_temper_u_03 / 10; - modbus_table_can_out[165].all = protect_levels.alarm_temper_u_04 / 10; - modbus_table_can_out[166].all = protect_levels.alarm_temper_u_05 / 10; - modbus_table_can_out[167].all = protect_levels.alarm_temper_u_06 / 10; - modbus_table_can_out[168].all = protect_levels.alarm_temper_u_07 / 10; - - modbus_table_can_out[169].all = protect_levels.abnormal_temper_water_ext / 10; - modbus_table_can_out[170].all = protect_levels.abnormal_temper_water_int / 10; - modbus_table_can_out[171].all = protect_levels.alarm_p_water_min_int / 100; - modbus_table_can_out[172].all = protect_levels.alarm_temper_water_int / 10; - modbus_table_can_out[173].all = protect_levels.alarm_temper_water_ext / 10; - modbus_table_can_out[174].all = protect_levels.alarm_p_water_max_int / 100; - - modbus_table_can_out[175].all = protect_levels.abnormal_temper_air_int_01 / 10; - modbus_table_can_out[176].all = protect_levels.abnormal_temper_air_int_02 / 10; - modbus_table_can_out[177].all = protect_levels.abnormal_temper_air_int_03 / 10; - modbus_table_can_out[178].all = protect_levels.abnormal_temper_air_int_04 / 10; - modbus_table_can_out[179].all = protect_levels.alarm_temper_air_int_01 / 10; - modbus_table_can_out[180].all = protect_levels.alarm_temper_air_int_02 / 10; - modbus_table_can_out[181].all = protect_levels.alarm_temper_air_int_03 / 10; - modbus_table_can_out[182].all = protect_levels.alarm_temper_air_int_04 / 10; - - modbus_table_can_out[183].all = _IQtoF(analog_protect.in_voltage[0].setup.levels.iqNominal_minus20) * NORMA_ACP;//_IQtoF(edrk.iqMIN_U_IN) * NORMA_ACP; - modbus_table_can_out[184].all = _IQtoF(analog_protect.in_voltage[1].setup.levels.iqNominal_minus20) * NORMA_ACP; - modbus_table_can_out[185].all = _IQtoF(analog_protect.in_voltage[0].setup.levels.iqNominal_plus20) * NORMA_ACP;//_IQtoF(edrk.iqMIN_U_ZPT) * NORMA_ACP; - modbus_table_can_out[186].all = _IQtoF(analog_protect.in_voltage[1].setup.levels.iqNominal_plus20) * NORMA_ACP; - modbus_table_can_out[187].all = //_IQtoF(edrk.iqMAX_U_IN) * NORMA_ACP; - modbus_table_can_out[188].all = _IQtoF(edrk.iqMAX_U_IN) * NORMA_ACP; - modbus_table_can_out[189].all = //_IQtoF(edrk.iqMAX_U_ZPT) * NORMA_ACP; - modbus_table_can_out[190].all = _IQtoF(edrk.iqMAX_U_ZPT) * NORMA_ACP; - - modbus_table_can_out[191].all = protect_levels.alarm_Izpt_max; - - modbus_table_can_out[192].all = protect_levels.alarm_Imax_U01; - modbus_table_can_out[193].all = protect_levels.alarm_Imax_U02; - modbus_table_can_out[194].all = protect_levels.alarm_Imax_U03; - modbus_table_can_out[195].all = protect_levels.alarm_Imax_U04; - modbus_table_can_out[196].all = protect_levels.alarm_Imax_U05; - modbus_table_can_out[197].all = protect_levels.alarm_Imax_U06; - modbus_table_can_out[198].all = protect_levels.alarm_Imax_U07; - modbus_table_can_out[199].all = protect_levels.alarm_Iged_max; -} diff --git a/Inu/Src2/551/main/modbus_svu_update.h b/Inu/Src2/551/main/modbus_svu_update.h deleted file mode 100644 index d58d103..0000000 --- a/Inu/Src2/551/main/modbus_svu_update.h +++ /dev/null @@ -1,17 +0,0 @@ -/* - * modbus_update_table.h - * - * Created on: 4 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_MODBUS_SVU_UPDATE_H_ -#define SRC_MAIN_MODBUS_SVU_UPDATE_H_ - - - -void update_svu_modbus_table(void); -void unpack_answer_from_MPU_SVU_CAN(unsigned int cc); -void unpack_answer_from_MPU_SVU_RS(unsigned int cc); - -#endif /* SRC_MAIN_MODBUS_SVU_UPDATE_H_ */ diff --git a/Inu/Src2/551/main/not_use/log_to_mem.c b/Inu/Src2/551/main/not_use/log_to_mem.c deleted file mode 100644 index 8a5c223..0000000 --- a/Inu/Src2/551/main/not_use/log_to_mem.c +++ /dev/null @@ -1,249 +0,0 @@ -/****************************************************************/ -/* TMS320C32 */ -/* ====== BIOS, , ====== */ -/* () 1998-2001. */ -/****************************************************************/ -/* log_to_mem.c - **************************************************************** - * y * - ****************************************************************/ - -#include - -#include "MemoryFunctions.h" - - -// -// / logs_data(), write_to_mem clear_mem -// y y (. / write_to_mem) -//#pragma DATA_SECTION(count_mem,".fast_vars"); -//static long count_mem = START_ADDRESS_LOG; - -#pragma DATA_SECTION(count_mem_slow,".fast_vars"); -static long count_mem_slow = START_ADDRESS_LOG_SLOW; - - -// y y -// y -int hb_logs_data = 0; -#pragma DATA_SECTION(stop_log,".fast_vars"); -int stop_log = 0; -int stop_log_slow = 0; - - -#pragma DATA_SECTION(logpar,".fast_vars"); -LOGSPARAMS logpar = LOGSPARAMS_DEFAULTS; - -#pragma DATA_SECTION(no_write,".fast_vars"); -int no_write = 0; // , ( ) -#pragma DATA_SECTION(no_write_slow,".fast_vars"); -int no_write_slow = 0; // , ( ) - -#pragma CODE_SECTION(clear_logpar,".fast_run"); -void clear_logpar() -{ - int i; - for(i=0;i= END_ADDRESS_LOG) logpar.addres_mem = END_ADDRESS_LOG; - - i_WriteMemory(logpar.addres_mem,DataM); - // *(int *)logpar.count_mem = ((DataM & 0xFFFF) ); - logpar.addres_mem++; - } - - if (tlog==SLOW_LOG) - { - if (no_write_slow) return; - - if (logpar.stop_log_slow_level_1) return; - - if (count_mem_slow >= END_ADDRESS_LOG_SLOW) count_mem_slow = END_ADDRESS_LOG_SLOW; - - i_WriteMemory(count_mem_slow,DataM); - // *(int *)logpar.count_mem = ((DataM & 0xFFFF) ); - count_mem_slow++; - } - - - - -} - - -#pragma CODE_SECTION(test_mem_limit,".fast_run"); -void test_mem_limit(int tlog,int ciclelog) -{ - if (tlog==FAST_LOG) - { - if( logpar.addres_mem >= (END_ADDRESS_LOG - LENGTH_HAZARD)) - { - logpar.real_finish_addres_mem = logpar.addres_mem; - - if (ciclelog==1) - { - stop_log = 0; - logpar.stop_log_level_1=0; - logpar.addres_mem = START_ADDRESS_LOG; - } - else - { - stop_log = 1; - logpar.stop_log_level_1=1; - } - } - - if( logpar.addres_mem >= (END_ADDRESS_LOG_LEVEL_2)) - { - logpar.stop_log_level_2=1; - } - else - { - logpar.stop_log_level_2=0; - } - - if( logpar.addres_mem >= (END_ADDRESS_LOG_LEVEL_3)) - { - logpar.stop_log_level_3=1; - } - else - { - logpar.stop_log_level_3=0; - } - } - else - { - if (tlog==SLOW_LOG) - { - if (ciclelog==1) - { - logpar.stop_log_slow_level_1=0; - } - - if( count_mem_slow >= (END_ADDRESS_LOG_SLOW - LENGTH_HAZARD)) - { - if (ciclelog==1) - { - stop_log_slow = 0; - logpar.stop_log_slow_level_1=0; - count_mem_slow = START_ADDRESS_LOG_SLOW; - } - else - { - stop_log_slow = 1; - logpar.stop_log_slow_level_1=1; - } - } - - if( count_mem_slow >= (END_ADDRESS_LOG_SLOW_LEVEL_2)) - { - logpar.stop_log_slow_level_2=1; - } - else - { - logpar.stop_log_slow_level_2=0; - } - - if( count_mem_slow >= (END_ADDRESS_LOG_SLOW_LEVEL_3)) - { - logpar.stop_log_slow_level_3=1; - } - else - { - logpar.stop_log_slow_level_3=0; - } - } - } -} - - - -// y, -void clear_mem(int tlog) -{ - if (tlog==FAST_LOG) - { - logpar.real_finish_addres_mem = 0; - - for (logpar.addres_mem=START_ADDRESS_LOG; logpar.addres_mem -#include -#include -#include -#include -#include - -#include "IQmathLib.h" -#include "xp_project.h" - - -OPTICAL_BUS_DATA optical_write_data = OPTICAL_BUS_DATA_DEFAULT; -OPTICAL_BUS_DATA optical_read_data = OPTICAL_BUS_DATA_DEFAULT; - -void parse_task_from_optical_bus(void); - - -#pragma CODE_SECTION(optical_bus_update_data_write,".fast_run"); -void optical_bus_update_data_write(void) -{ - - optical_write_data.data.cmd.bit.alarm = edrk.summ_errors; - - optical_write_data.data.cmd.bit.master = edrk.auto_master_slave.local.bits.master; - optical_write_data.data.cmd.bit.slave = edrk.auto_master_slave.local.bits.slave; - optical_write_data.data.cmd.bit.maybe_master = edrk.auto_master_slave.local.bits.try_master; - - -// optical_write_data.cmd.bit.controlMode = ; - -// optical_write_data.cmd.bit.err_optbus = -// optical_write_data.cmd.bit.master = -// optical_write_data.cmd.bit.maybe_master = -// optical_write_data.cmd.bit.pwm_status = -// - - if (edrk.Status_Rascepitel_Ok==0) - { - if (edrk.RunZahvatRascepitel) - optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF;// 10 - , - else - optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_OFF;//00 - , - } - else - { - - if (edrk.Status_Ready.bits.ready_final==0) // - { - if (edrk.RunUnZahvatRascepitel) - optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF;// 10 - , - else - optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON;// 01 - , - } - else - { - if (edrk.you_can_on_rascepitel==0) - optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_DISABLE_THIS_ON; // 11 - , - else - optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON; // 01 - , - } - } - - if (edrk.Status_Ready.bits.ready7 || (edrk.Status_Ready.bits.ready5 && edrk.Status_Ready.bits.ImitationReady2) ) - optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY2; - else - if (edrk.SumSbor) - optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY1TO2; - else - optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY1; - -// optical_write_data.cmd.bit.ready_to_go = -// optical_write_data.cmd.bit.slave = -// -// optical_write_data.cmd.bit.start_pwm = - optical_write_data.data.cmd.bit.statusQTV = edrk.from_shema_filter.bits.QTV_ON_OFF; -// optical_write_data.cmd.bit.stop_pwm = - - optical_write_data.data.cmd.bit.sync_1_2 = sync_data.local_flag_sync_1_2; - optical_write_data.data.cmd.bit.sync_line_detect = !sync_data.timeout_sync_signal; - -// optical_write_data.data1 = f.Mode == 1 ? f.fzad * 60.0 + 0.5 : -// f.Mode == 2 ? f.p_zad / 1000.0 : -// 0; -// optical_write_data.data2 = rotor.direct_rotor == -1 ? -_IQtoIQ15(rotor.iqFout) : -// _IQtoIQ15(rotor.iqFout); -// optical_write_data.data3 = _IQtoIQ15(analog.iqIq_zadan); -// optical_write_data.data4.bit.controlMode = f.Mode == 2 ? 1 : 0; -// optical_write_data.data4.bit.leading_ready = f.Ready2 && (f.Mode == 1 || f.Mode == 2) ? 1 : 0; -// optical_write_data.data4.bit.leading_Go = edrk.Go; - - - -} - - - -#pragma CODE_SECTION(optical_bus_write,".fast_run"); -void optical_bus_write(void) -{ -#if(USE_TK_3) - // check error write PBUS OPTICAL BUS - project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); - - // write data to OPTICAL BUS - project.cds_tk[3].optical_data_out.buf[0] = optical_write_data.data.pzad_or_wzad; - project.cds_tk[3].optical_data_out.buf[1] = optical_write_data.data.angle_pwm; - project.cds_tk[3].optical_data_out.buf[2] = optical_write_data.data.iq_zad_i_zad; - project.cds_tk[3].optical_data_out.buf[3] = optical_write_data.data.cmd.all; - - optical_write_data.status = 1; - - - project.cds_tk[3].optical_bus_write_data(&project.cds_tk[3]); -#endif -} - - - - - -#pragma CODE_SECTION(optical_bus_read_old,".fast_run"); -void optical_bus_read_old(void) -{ - -// // check error write PBUS OPTICAL BUS -// project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); -//// read data from OPTICAL BUS -// project.cds_tk[3].read_pbus(&project.cds_tk[3]); -// // check error read PBUS OPTICAL BUS -// project.cds_tk[3].optical_bus_check_error_read(&project.cds_tk[3]); -// -// -// if (project.cds_tk[3].optical_data_in.new_data_ready) -// { -// if (project.cds_tk[3].optical_data_in.overfull_new_data) -// optical_read_data.overfull_data++; -// -// optical_read_data.raw.pzad_or_wzad = (int)project.cds_tk[3].optical_data_in.buf[0]; -// optical_read_data.raw.angle_pwm = (int)project.cds_tk[3].optical_data_in.buf[1]; -// optical_read_data.raw.iq_zad = (int)project.cds_tk[3].optical_data_in.buf[2]; -// optical_read_data.raw.cmd.all = project.cds_tk[3].optical_data_in.buf[3]; -// -// -// optical_read_data.data.pzad_or_wzad = optical_read_data.raw.pzad_or_wzad; -// optical_read_data.data.angle_pwm = optical_read_data.raw.angle_pwm ; -// optical_read_data.data.iq_zad = optical_read_data.raw.iq_zad; -// optical_read_data.data.cmd.all = optical_read_data.raw.cmd.all; -// -// optical_read_data.code_status.bit.ready = 1; -// optical_read_data.code_status.bit.recive_error = 0; -// optical_read_data.code_status.bit.overfull = 0; -// optical_read_data.code_status.bit.timeout = 0; -// optical_read_data.code_status.bit.wait = 0; -// -// project.cds_tk[3].optical_data_in.new_data_ready = 0; -// -// return (optical_read_data.code_status); -// -// } -// else -// { -// -// if (project.cds_tk[3].optical_data_in.ready == 1) -// { -// optical_read_data.code_status.bit.ready = 0; -// -// if (project.cds_tk[3].optical_data_in.raw_local_error) -// optical_read_data.code_status.bit.recive_error = 1; -// else -// optical_read_data.code_status.bit.recive_error = 0; -// -// optical_read_data.code_status.bit.overfull = 0; -// optical_read_data.code_status.bit.timeout = 0; -// optical_read_data.code_status.bit.wait = 1; -// -// return (optical_read_data.code_status); -// } -// -// } -// -// -// if (project.cds_tk[3].optical_data_out.ready == 0) { -// -// project.cds_tk[3].optical_data_out.error_not_ready_count += 1; -// -//// optical_read_data.code_status.bit.ready = 0; -// optical_read_data.code_status.bit.send_error = 1; -//// optical_read_data.code_status.bit.overfull = 0; -//// optical_read_data.code_status.bit.timeout = 0; -//// optical_read_data.code_status.bit.wait = 0; -// -//// optical_write_data.status = 2; -//// return (optical_read_data.code_status); -// } -// else -// optical_read_data.code_status.bit.send_error = 0; -// -// -// if (project.cds_tk[3].optical_data_in.ready == 0) { -// -//// f.read_task_from_optical_bus = 0; -// project.cds_tk[3].optical_data_in.error_not_ready_count += 1; -// -// optical_read_data.code_status.bit.ready = 0; -// // optical_read_data.code_status.bit.send_error = 0; -// //optical_read_data.code_status.bit.recive_error = 1; -// optical_read_data.code_status.bit.overfull = 0; -// optical_read_data.code_status.bit.timeout = 1; -// optical_read_data.code_status.bit.wait = 0; -// -// optical_read_data.data.pzad_or_wzad = 0; -// optical_read_data.data.angle_pwm = 0; -// optical_read_data.data.iq_zad = 0; -// optical_read_data.data.cmd.all = 0; -// -// // optical_read_data.status = 2; -// -// // return (project.cds_tk[3].optical_data_in.raw_local_error); -// } -// else -// { -// -// -// -// } -// -// return (optical_read_data.code_status); -// -// -//// optical_read_data.status = 1; -// -// // return (project.cds_tk[3].optical_data_in.raw_local_error); -// -//// if (f.flag_leading == 0 && f.flag_distance && f.Ready1){ -//// parse_task_from_optical_bus(); -//// } else { -//// f.read_task_from_optical_bus = 0; -//// } -//// rotor.iqFrotFromOptica = _IQ15toIQ(optical_read_data.data2); //Frot -//// analog.iqIq_zad_from_optica = _IQ15toIQ(optical_read_data.data3); - -} - - -#pragma CODE_SECTION(optical_bus_read,".fast_run"); -void optical_bus_read(void) -{ - - // check error write PBUS OPTICAL BUS -// project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); -// read data from OPTICAL BUS -#if(USE_TK_3) - project.cds_tk[3].read_pbus(&project.cds_tk[3]); -#endif - - return; -} - -#pragma CODE_SECTION(optical_bus_read_clear_count_error,".fast_run"); -void optical_bus_read_clear_count_error(void) -{ -#if(USE_TK_3) - project.cds_tk[3].optical_data_in.local_count_error = 0; -#endif - - return; -} - - - -#pragma CODE_SECTION(optical_bus_get_status_and_read,".fast_run"); -STATUS_DATA_READ_OPT_BUS optical_bus_get_status_and_read(void) -{ - STATUS_DATA_READ_OPT_BUS status_read; - // check error write PBUS OPTICAL BUS - // project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); -// read data from OPTICAL BUS -// project.cds_tk[3].read_pbus(&project.cds_tk[3]); - -#if(USE_TK_3) - // check error read PBUS OPTICAL BUS - project.cds_tk[3].optical_bus_check_error_read(&project.cds_tk[3]); - - status_read.all = project.cds_tk[3].optical_data_in.status_read.all; - - project.cds_tk[3].optical_data_in.prev_status_read.all = status_read.all; - project.cds_tk[3].optical_data_in.status_read.all = 0; - - optical_read_data.status = project.cds_tk[3].optical_data_in.ready; - - - if (status_read.bit.new_data_ready) - { - if (status_read.bit.overfull_new_data) - optical_read_data.overfull_data++; - - optical_read_data.raw.pzad_or_wzad = (int)project.cds_tk[3].optical_data_in.buf[0]; - optical_read_data.raw.angle_pwm = (int)project.cds_tk[3].optical_data_in.buf[1]; - optical_read_data.raw.iq_zad_i_zad = (int)project.cds_tk[3].optical_data_in.buf[2]; - optical_read_data.raw.cmd.all = project.cds_tk[3].optical_data_in.buf[3]; - - - optical_read_data.data.pzad_or_wzad = optical_read_data.raw.pzad_or_wzad; - optical_read_data.data.angle_pwm = optical_read_data.raw.angle_pwm ; - optical_read_data.data.iq_zad_i_zad = optical_read_data.raw.iq_zad_i_zad; - optical_read_data.data.cmd.all = optical_read_data.raw.cmd.all; - - - return (status_read); // - - } - else - { - // ? - // optical_bus, ! - -// optical_read_data.data.pzad_or_wzad = 0; -// optical_read_data.data.angle_pwm = 0; -// optical_read_data.data.iq_zad_i_zad = 0; -// optical_read_data.data.cmd.all = 0; - - - - } -#else - status_read.all = 0; -#endif - return status_read; // - -} - - - - -void parse_task_from_optical_bus(void) { - - // if (optical_read_data.data4.bit.leading_ready == 1) { -// f.read_task_from_optical_bus = 1; -// f.Mode = optical_read_data.data4.bit.controlMode == 0 ? 1 : 2; -// if (f.Mode == 1) { -// f.fzad = (float)optical_read_data.data1 / 60.0; -// f.iq_fzad = _IQ(f.fzad / NORMA_FROTOR); -// limit_mzz_zad_turns(); -// } -// if (f.Mode == 2) { -// f.p_zad = (float)optical_read_data.data1 * 1000.0; -// f.iq_p_zad = _IQ(f.p_zad/NORMA_ACP/NORMA_ACP); -// // -// if (f.iq_p_zad > f.iq_p_limit_zad) { f.iq_p_zad = f.iq_p_limit_zad;} -// if (f.iq_p_zad < -f.iq_p_limit_zad) { f.iq_p_zad = -f.iq_p_limit_zad;} -// -// limit_mzz_zad_power(); -// } - -// if (f.Ready1 && f.Ready2) { -// if (((f.Mode == 1 && f.fzad != 0) || (f.Mode == 2 && f.p_zad != 0)) -// && (!f.Is_Blocked || (f.Is_Blocked && edrk.Go)) -// && (f.rotor_stopped == 0) -//// && (faults.faults5.bit.rotor_stopped == 0) -// //&& (optical_read_data.data4.bit.leading_Go == 1) -// ){ -// edrk.Go = 1; -// } else { -// if (edrk.Go) { -// f.p_zad = 0; -// f.fzad = 0; -// f.iq_fzad = 0; -// f.iq_p_zad = 0; -// if(f.Mode == 1) -// { -// if(a.iqk < 1677722 || rotor.iqFout < 48210 //1677722 ~ 0.1 503316 = 3% 838860 = 5% -// || (analog.iqIm_1 < 1677721 && analog.iqIm_2 < 1677721)) //1677721 ~ 300A -// { -// edrk.Go = 0; -// f.iq_fzad = 0; -// } -// -// } -// if(edrk.Go && f.Mode == 2) -// { -// if(analog.iqW < 186413) -// { -// if(a.iqk < 1677722 || //1677722 ~ 0.1 -// (analog.iqIm_1 < 1677721 && analog.iqIm_2 < 1677721)) //1677721 ~ 300A -// { -// edrk.Go = 0; -// } -// } -// } -// -// } else { -// f.iq_mzz_zad = _IQ(500.0/NORMA_MZZ); -// } -// } -// } else { -// edrk.Go = 0; -// } - - - // _IQ15toIQ(optical_read_data.data3); //Iq_zad -// } else { - // f.read_task_from_optical_bus = 0; -// } - -} - diff --git a/Inu/Src2/551/main/optical_bus.h b/Inu/Src2/551/main/optical_bus.h deleted file mode 100644 index ce30d7c..0000000 --- a/Inu/Src2/551/main/optical_bus.h +++ /dev/null @@ -1,109 +0,0 @@ -/* - * optical_bus.h - * - * Created on: 18 . 2020 . - * Author: stud - */ - -#ifndef SRC_MAIN_OPTICAL_BUS_H_ -#define SRC_MAIN_OPTICAL_BUS_H_ - -#include "xp_cds_tk_23550.h" - -enum -{ -CODE_READY_CMD_NOT_READY=0, //// 0 - not ready -CODE_READY_CMD_READY1, //1-ready1 -CODE_READY_CMD_READY1TO2, //2-ready1to2 -CODE_READY_CMD_READY2 //3 -ready2 -}; - -enum -{ - CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_OFF=0, //0 - , - CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON, // 1 - , - CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF, // 2 - , - CODE_RASCEPITEL_CMD_DISABLE_THIS_ON // 3 - , -}; - - - typedef union { - struct { - unsigned int wdog_tick :1; // 0_1_0 .. - unsigned int statusQTV :1; //1-QTV On, QTV - off - unsigned int master :1; // 1 -Master, 0 - not Master - unsigned int slave :1; // 1 -Slave, 0 - not Slave - - unsigned int sync_line_detect :1; // 1 - yes, 0 - no - unsigned int sync_1_2 :1; // 1 - yes, 0 - no - unsigned int alarm :1; // 1 - yes, 0 - no - unsigned int ready_cmd :2; // 00 - not ready,01-ready1,10-ready1to2, 11 -ready2 - - unsigned int prepare_stop_PWM :1; // 0 - regul turns; 1 - power -// unsigned int ready_to_go :1; // 1 - yes, 0 - no , - unsigned int start_pwm :1; // 1 - yes, 0 - no - unsigned int stop_pwm :1; // 1 - yes, 0 - no - - unsigned int pwm_status :1; // 1 -On, 0 - Off -// unsigned int can_on_rascepitel :1; // 1 - yes, 0 - no - unsigned int maybe_master :1; // 1 - yes, 0 - no Master - unsigned int rascepitel_cmd :2; // - // 00 - , - // 01 - , - // 10 - , - // 11 - , - } bit; - unsigned int all; - } OPTICAL_BUS_DATA_LOW_CMD; - - -// typedef union { -// struct { -// unsigned int ready :1; // -// unsigned int overfull :1; // -// unsigned int recive_error :1; // -// unsigned int send_error :1; // -// unsigned int wait :1; // -// unsigned int timeout :1; // -// } bit; -// unsigned int all; -// } OPTICAL_BUS_CODE_STATUS; - - -typedef struct { - int pzad_or_wzad; //given turns or power, depends on controlMode; - int angle_pwm; //current rotor turns - int iq_zad_i_zad; // Iq_zadan or Izad - OPTICAL_BUS_DATA_LOW_CMD cmd; -} OPTICAL_BUS_DATA_LOW; - -typedef struct { - OPTICAL_BUS_DATA_LOW raw; - OPTICAL_BUS_DATA_LOW data; - unsigned int status; - unsigned int overfull_data; - unsigned int timer; - unsigned int flag_clear; - unsigned int data_was_update_between_pwm_int; - unsigned int error_wdog; - unsigned int count_error_wdog; - unsigned int count_read_optical_bus_old_data; // - - -// OPTICAL_BUS_CODE_STATUS code_status; -} OPTICAL_BUS_DATA; - - -#define OPTICAL_BUS_DATA_DEFAULT {{0,0,0,0},{0,0,0,0},0,0,0,0,0,0,0,0} - -extern OPTICAL_BUS_DATA optical_write_data; -extern OPTICAL_BUS_DATA optical_read_data; - - -void optical_bus_read(void); -STATUS_DATA_READ_OPT_BUS optical_bus_get_status_and_read(void); -void optical_bus_write(void); -void optical_bus_read_clear_count_error(void); -void optical_bus_update_data_write(void); - -#endif /* SRC_MAIN_OPTICAL_BUS_H_ */ diff --git a/Inu/Src2/551/main/optical_bus_tools.c b/Inu/Src2/551/main/optical_bus_tools.c deleted file mode 100644 index 431ade9..0000000 --- a/Inu/Src2/551/main/optical_bus_tools.c +++ /dev/null @@ -1,791 +0,0 @@ -/* - * optical_bus_tools.c - * - * Created on: 19 . 2024 . - * Author: user - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "global_time.h" -#include "IQmathLib.h" -#include "oscil_can.h" -#include "uf_alg_ing.h" -#include "MemoryFunctions.h" -#include "RS_Functions.h" -#include "v_rotor_22220.h" -#include "log_to_memory.h" -#include "log_params.h" - - - - - -/////////////////////////////////////////////////////////////////// - - -#pragma CODE_SECTION(optical_bus_read_write_interrupt,".fast_run2"); -void optical_bus_read_write_interrupt(void) -{ - - - static unsigned int prev_error_read = 0, count_read_optical_bus_error = 0, count_read_optical_bus_old_data = 0; - static unsigned int max_count_read_old_data_optical_bus = 15; - static unsigned int flag_disable_resend = 0; - static unsigned int count_resend = 0, cc=0; -// static STATUS_DATA_READ_OPT_BUS buf_status[10]; - static STATUS_DATA_READ_OPT_BUS optbus_status; - static unsigned int tt=0; - static unsigned int cmd_wdog_sbus = 0, count_wait_wdog_sbus = 0, wdog_sbus = 0; - static int prepare_time = 0; - - - static unsigned int t_finish_optbus = 14, t_read_optbus = 6, t_write_optbus = 10, max_count_read_error_optical_bus = 15, max_count_read_wdog_optical_bus = 48; - static int cmd_optbus = 0; - - static int flag_enable_read=0, flag_finish_read = 0, flag_enable_write = 0, flag_finish_write = 0, count_wait_write = 0; - - if (prepare_time==0) - { - if (edrk.flag_second_PCH==0) - { - t_read_optbus = 6; - t_write_optbus = 8; - t_finish_optbus = 20; - } - - if (edrk.flag_second_PCH==1) - { - t_read_optbus = 12; - t_write_optbus = 14; - t_finish_optbus = 10; - } - prepare_time = 1; - } - - if (flag_special_mode_rs==1) - return; - - if (edrk.KvitirProcess) - return; - - if (edrk.disable_interrupt_timer2) - return; - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_17_ON; -#endif - -//i_led2_on_off(1); - - -//#if (ENABLE_LOG_INTERRUPTS) -// add_log_interrupts(2); -//#endif - - -// pause_1000(100); - if (optical_read_data.flag_clear) - { - // stage_1 = 0; - optical_read_data.timer = 0; - optical_read_data.flag_clear = 0; - optical_read_data.error_wdog = 0; -// count_wait_wdog_sbus = 0; - -// if (optical_read_data.data_was_update_between_pwm_int==0) -// sum_count_err_read_opt_bus++; -// -// optical_read_data.data_was_update_between_pwm_int = 0; - - // optical_read_data.data_was_update_between_pwm_int = 0; - cc = 0; - prev_error_read = 0; - } - - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - if (optical_read_data.timer==0) - { - PWM_LINES_TK_16_ON; - } - else - { - PWM_LINES_TK_16_OFF; - } -#endif - - - -// else - optical_read_data.timer++; - -// if (edrk.into_pwm_interrupt==1) -// { -// if (optical_read_data.timer>=2) -// { -// optical_read_data.timer--; -// optical_read_data.timer--; -// } -// flag_disable_resend = 0; -// count_resend = 0; -// -// } - -// -// -// -// if (stage_1==0) -// tt = t1; -// -// if (stage_1==1) -// tt = t2; - - if (edrk.ms.another_bs_maybe_on==1 && edrk.flag_second_PCH==0 /*edrk.auto_master_slave.local.bits.master*/ ) - { - - if (optical_read_data.data.cmd.bit.wdog_tick) - { - // i_led1_on(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_21_ON; -#endif - } - else - { - // i_led1_off(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_21_OFF; -#endif - - } - - optical_write_data.data.cmd.bit.wdog_tick = optical_read_data.data.cmd.bit.wdog_tick; - - if (optical_write_data.data.cmd.bit.wdog_tick) - { - // i_led2_on(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_22_ON; -#endif - - } - else - { - // i_led2_off(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_22_OFF; -#endif - } - - - - } - - - if (edrk.ms.another_bs_maybe_on==1 && edrk.flag_second_PCH==1 /*edrk.auto_master_slave.local.bits.slave*/ ) - { - - if (optical_write_data.data.cmd.bit.wdog_tick) - { - // i_led2_on(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_22_ON; -#endif - - } - else - { - // i_led2_off(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_22_OFF; -#endif - } - - - - if (optical_read_data.data.cmd.bit.wdog_tick) - { - // i_led1_on(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_21_ON; -#endif - } - else - { - // i_led1_off(); -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_21_OFF; -#endif - } - - - - - // - optical_write_data.data.cmd.bit.wdog_tick = wdog_sbus; - if (cmd_wdog_sbus==0) - { -// optical_write_data.data.cmd.bit.wdog_tick = wdog_sbus; - count_wait_wdog_sbus = 0; - cmd_wdog_sbus++; - } - else - // - if (cmd_wdog_sbus==1) - { - if (optical_read_data.data.cmd.bit.wdog_tick == wdog_sbus) //&& prev_error_read==0 - { - // result_code_wdog_sbus = 1; - optical_read_data.count_error_wdog = count_wait_wdog_sbus; - count_wait_wdog_sbus = 0; - wdog_sbus = !wdog_sbus; - cmd_wdog_sbus = 0; - } - else - { - if (count_wait_wdog_sbus=t_finish_optbus) - cmd_optbus = 1; - break; - case 5: - break; - case 6: - break; - case 7: - break; - case 8: - break; - case 9: - break; - default: - break; - } -} - - - -if (edrk.flag_second_PCH==0) -{ - switch (cmd_optbus) - { - case 0 : if (optical_read_data.timer==t_write_optbus) - cmd_optbus = 1; - break; - - case 1: flag_enable_read = 0; - flag_finish_read = 0; - flag_enable_write = 1; - flag_finish_write = 0; - count_wait_write = TIME_WAIT_CMD_WRITE; - cmd_optbus = 2; - break; - - case 2: if (flag_enable_write==0) - { - if (count_wait_write) - { - count_wait_write--; - } - else - cmd_optbus = 3; - } - break; - case 3: flag_enable_read = 1; - flag_finish_read = 0; - flag_enable_write = 0; - flag_finish_write = 0; - count_wait_write = 0; - cmd_optbus = 4; - break; - - case 4: if (flag_finish_read) - { - //flag_enable_write = 1; - //count_wait_write = 2; - //cmd_optbus = 2; - flag_enable_read = 0; - flag_finish_read = 0; - flag_enable_write = 1; - flag_finish_write = 0; - count_wait_write = TIME_WAIT_CMD_WRITE; - cmd_optbus = 2; // case 5: case 1: - } - break; - - case 5: if (optical_read_data.timer>=t_finish_optbus) - cmd_optbus = 1; - break; - case 6: - break; - case 7: - break; - case 8: - break; - case 9: - break; - case 10: - break; - default: - break; - } -} -// if (optical_read_data.timer==t_read_optbus) -// flag_run_cycle = 1; -// -// if (flag_run_cycle==t_read_optbus) -// { -// flag_enable_read = 1; -// flag_finish_read = 0; -// flag_enable_write = 0; -// flag_finish_write = 0; -// flag_run_cycle = 0; -// } -// -// -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_18_OFF; -#endif - - if (flag_enable_read) - { - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_19_ON; -#endif - -#if(USE_TK_3) - project.cds_tk[3].read_pbus(&project.cds_tk[3]); -#endif - optbus_status = optical_bus_get_status_and_read(); - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_19_OFF; -#endif - cc++; - - if (optbus_status.bit.new_data_ready) - { - - prev_error_read = 0; - optical_read_data.count_read_optical_bus_old_data = 0; - - if (optical_read_data.data_was_update_between_pwm_int<10000) - optical_read_data.data_was_update_between_pwm_int += 1; - - count_read_optical_bus_error = 0; - flag_finish_read = 1; - flag_enable_read = 0; - } - - if (optbus_status.bit.receiver_error || optbus_status.bit.bad_status12 ) - { - - prev_error_read = 1; - - if (count_read_optical_bus_error<=max_count_read_error_optical_bus) - count_read_optical_bus_error++; - else - { - optical_read_data.data.pzad_or_wzad = 0; - optical_read_data.data.angle_pwm = 0; - optical_read_data.data.iq_zad_i_zad = 0; - optical_read_data.data.cmd.all = 0; - flag_finish_read = 1; - } - } - - - if (optbus_status.bit.old_data) - { - -// prev_error_read = 1; - - flag_finish_read = 1; - - if (optical_read_data.count_read_optical_bus_old_data<=max_count_read_old_data_optical_bus) - optical_read_data.count_read_optical_bus_old_data++; - else - { - optical_read_data.data.pzad_or_wzad = 0; - optical_read_data.data.angle_pwm = 0; - optical_read_data.data.iq_zad_i_zad = 0; - optical_read_data.data.cmd.all = 0; - } - } - - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - if (optbus_status.bit.new_data_ready) - { - PWM_LINES_TK_18_ON; - } -#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12) - { - // PWM_LINES_TK_16_ON; - } -#endif - - } - -// -// -// if (flag_finish_read) -// { -// flag_enable_write = 1; -// count_wait_write = 10; -// } - - - if (flag_enable_write) - { -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_20_ON; -#endif -#if(_ENABLE_PWM_LINES_FOR_TESTS) - static unsigned int ccc = 0; - ccc++; - optical_write_data.data.angle_pwm = ccc; -#endif - optical_bus_write(); - - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_20_OFF; -#endif - flag_enable_write = 0; - } -// -// if (count_wait_write) -// { -// count_wait_write--; -// } -// else -// flag_finish_write = 1; -// -// -// -//// read -// -// if (optical_read_data.timer==(t_read_optbus-1)) -// prev_error_read = 1; -// else -// if (optical_read_data.timer==t_read_optbus || prev_error_read==1) -// { -// -//#if(_ENABLE_PWM_LINES_FOR_TESTS) -// PWM_LINES_TK_18_OFF; -//#endif -// -//#if(_ENABLE_PWM_LINES_FOR_TESTS) -// PWM_LINES_TK_19_ON; -//#endif -// -// project.cds_tk[3].read_pbus(&project.cds_tk[3]); -// -// optbus_status = optical_bus_get_status_and_read(); -// -//#if(_ENABLE_PWM_LINES_FOR_TESTS) -// PWM_LINES_TK_19_OFF; -//#endif -// cc++; -// -// if (optbus_status.bit.new_data_ready) -// { -// -// prev_error_read = 0; -// -// if (optical_read_data.data_was_update_between_pwm_int<10000) -// optical_read_data.data_was_update_between_pwm_int += 1; -// -// count_read_optical_bus_error = 0; -// } -// -// if (optbus_status.bit.receiver_error || optbus_status.bit.bad_status12 ) -// { -// -// prev_error_read = 1; -// -// if (count_read_optical_bus_error<=max_count_read_error_optical_bus) -// count_read_optical_bus_error++; -// else -// { -// optical_read_data.data.pzad_or_wzad = 0; -// optical_read_data.data.angle_pwm = 0; -// optical_read_data.data.iq_zad_i_zad = 0; -//// optical_read_data.data.cmd.all = 0x40; // ! alarm = 1 -// optical_read_data.data.cmd.all = 0; -// } -// } -// -//#if(_ENABLE_PWM_LINES_FOR_TESTS) -// if (optbus_status.bit.new_data_ready) -// { -// PWM_LINES_TK_18_ON; -// } -//#endif -// -//#if(_ENABLE_PWM_LINES_FOR_TESTS) -// if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12) -// { -// // PWM_LINES_TK_16_ON; -// } -//#endif -// -// } -// -// -// -// -//// write -// -// if (optical_read_data.timer==t_write_optbus) -// { -//#if(_ENABLE_PWM_LINES_FOR_TESTS) -// PWM_LINES_TK_20_ON; -//#endif -// -// optical_bus_write(); -// -// -//#if(_ENABLE_PWM_LINES_FOR_TESTS) -// PWM_LINES_TK_20_OFF; -//#endif -// } - - - -// finish - -// if (optical_read_data.timer>=t_finish_optbus) -// { -// optical_read_data.timer = 0; -// } - -// if (prev_error_read==0) -// i_led2_off(); - - -// if (optical_read_data.timer==t2) -// { -// -// // if (edrk.flag_second_PCH==0) -// stage_2 = 1; -// // else -// // stage_1 = 1; -// -// optical_read_data.timer = 0; -// } - -// if (optical_read_data.timer>=t3) -// { -// optical_read_data.timer = 0; -// } - - - - - -// -// if (stage_1==2 && prev_stage1!=stage_1) -// { -// // i_led2_on(); -//// if (flag_disable_resend==0) -//// { -// // if (edrk.ms.another_bs_maybe_on==1 && (edrk.auto_master_slave.local.bits.master ) ) -// -// // if (edrk.flag_second_PCH==0) -// { -// i_led2_on(); -// i_led2_off(); -// i_led2_on(); -// -// optical_bus_write(); -// } -//// else -//// { -//// i_led2_on(); -//// -//// optical_bus_read(); -//// optbus_status = optical_bus_get_status_and_read(); -//// buf_status[cc] = optbus_status; -//// cc++; -//// -//// if (optbus_status.bit.new_data_ready) -//// optical_read_data.data_was_update_between_pwm_int = 1; -//// -//// if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12 -//// ) -//// { -//// i_led1_on(); -//// i_led1_off(); -//// -//// } -//// -//// } -// stage_1 = 0; -// } - -// if (stage_1==1 && prev_stage1!=stage_1) -// { -// -//// if (edrk.flag_second_PCH==1) -//// { -//// i_led2_on(); -//// i_led2_off(); -//// i_led2_on(); -//// -//// optical_bus_write(); -//// } -//// else -// // { -// i_led2_on(); -// -// optical_bus_read(); -// optbus_status = optical_bus_get_status_and_read(); -// buf_status[cc] = optbus_status; -// cc++; -// -// if (optbus_status.bit.new_data_ready) -// optical_read_data.data_was_update_between_pwm_int = 1; -// -// if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12 -// ) -// { -// i_led1_on(); -// i_led1_off(); -// -// } -// -// // } -// -// -// // stage_1 = 0; -// } - - // prev_stage1 = stage_1; - // } -// if (edrk.flag_second_PCH==1) -// { -// i_led2_off(); -// } - -//i_led2_on_off(0); - -//#if (ENABLE_LOG_INTERRUPTS) -// add_log_interrupts(102); -//#endif - -#if(_ENABLE_PWM_LINES_FOR_TESTS) - PWM_LINES_TK_17_OFF; -#endif - - -} -/////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////// - - diff --git a/Inu/Src2/551/main/optical_bus_tools.h b/Inu/Src2/551/main/optical_bus_tools.h deleted file mode 100644 index 89c6bc6..0000000 --- a/Inu/Src2/551/main/optical_bus_tools.h +++ /dev/null @@ -1,14 +0,0 @@ -/* - * optical_bus_tools.h - * - * Created on: 19 . 2024 . - * Author: user - */ - -#ifndef SRC_MAIN_OPTICAL_BUS_TOOLS_H_ -#define SRC_MAIN_OPTICAL_BUS_TOOLS_H_ - -void optical_bus_read_write_interrupt(void); - - -#endif /* SRC_MAIN_OPTICAL_BUS_TOOLS_H_ */ diff --git a/Inu/Src2/551/main/overheat_limit.c b/Inu/Src2/551/main/overheat_limit.c deleted file mode 100644 index e2c2720..0000000 --- a/Inu/Src2/551/main/overheat_limit.c +++ /dev/null @@ -1,80 +0,0 @@ -/* - * overheat_limit.c - * - * Created on: 17 . 2020 . - * Author: star - */ - -#include -#include -#include -#include - -#include "IQmathLib.h" -#include "math_pi.h" -#include "limit_lib.h" - - -TEMPERATURE_LIMIT_KOEFFS temper_limit_koeffs = TEMPERATURE_LIMIT_KOEFFS_DEFAULTS; - -void calc_limit_overheat() -{ - int *p_alarm, *p_abnormal; - _iq sum_limit = CONST_IQ_1; - _iq val; - int i = 0; - - p_alarm = &protect_levels.alarm_temper_u_01; - p_abnormal = &protect_levels.abnormal_temper_u_01; - edrk.temper_limit_koeffs.power_units = CONST_IQ_1; - for (i = 0; i < 7; i++) { - val = linear_decrease(edrk.temper_edrk.real_int_temper_u[i], - *(p_alarm + i), *(p_abnormal + i)); - edrk.temper_limit_koeffs.power_units = _IQmpy(val, edrk.temper_limit_koeffs.power_units); - } - sum_limit = _IQmpy(sum_limit,edrk.temper_limit_koeffs.power_units); - - p_alarm = &protect_levels.alarm_temper_air_int_01; - p_abnormal = &protect_levels.abnormal_temper_air_int_01; - edrk.temper_limit_koeffs.area = CONST_IQ_1; - for (i = 0; i < 4; i++) { - val = linear_decrease(edrk.temper_edrk.real_int_temper_air[i], - *(p_alarm + i), *(p_abnormal + i)); - edrk.temper_limit_koeffs.area = _IQmpy(val, edrk.temper_limit_koeffs.area); - } - sum_limit = _IQmpy(sum_limit,edrk.temper_limit_koeffs.area); - - edrk.temper_limit_koeffs.water_int = linear_decrease(edrk.temper_edrk.real_temper_water[0] * 10.0, - protect_levels.alarm_temper_water_int, protect_levels.abnormal_temper_water_int); - sum_limit = _IQmpy(sum_limit,edrk.temper_limit_koeffs.water_int); - - edrk.temper_limit_koeffs.water_ext = linear_decrease(edrk.temper_edrk.real_temper_water[1] * 10.0, - protect_levels.alarm_temper_water_ext, protect_levels.abnormal_temper_water_ext); - sum_limit = _IQmpy(sum_limit,edrk.temper_limit_koeffs.water_ext); - - - p_alarm = &protect_levels.alarm_temper_acdrive_winding_U1; - p_abnormal = &protect_levels.abnormal_temper_acdrive_winding_U1; - edrk.temper_limit_koeffs.acdrive_windings = CONST_IQ_1; - for (i = 0; i < 6; i++) { - val = linear_decrease(edrk.temper_acdrive.winding.real_int_temper[i], - *(p_alarm + i), *(p_abnormal + i)); - edrk.temper_limit_koeffs.acdrive_windings = _IQmpy(val, edrk.temper_limit_koeffs.acdrive_windings); - } - sum_limit = _IQmpy(sum_limit,edrk.temper_limit_koeffs.acdrive_windings); - - edrk.temper_limit_koeffs.acdrive_bears = linear_decrease(edrk.temper_acdrive.bear.real_int_temper[0], - protect_levels.alarm_temper_acdrive_bear_DNE, protect_levels.abnormal_temper_acdrive_bear_DNE); - sum_limit = _IQmpy(sum_limit,edrk.temper_limit_koeffs.acdrive_bears); - edrk.temper_limit_koeffs.acdrive_bears = linear_decrease(edrk.temper_acdrive.bear.real_int_temper[1], - protect_levels.alarm_temper_acdrive_bear_NE, protect_levels.abnormal_temper_acdrive_bear_NE); - sum_limit = _IQmpy(sum_limit,edrk.temper_limit_koeffs.acdrive_bears); - - edrk.temper_limit_koeffs.sum_limit = sum_limit; - - if (edrk.temper_limit_koeffs.sum_limit < (CONST_IQ_1 - 1000)) - edrk.temper_limit_koeffs.code_status = 1; - else - edrk.temper_limit_koeffs.code_status = 0; - -} diff --git a/Inu/Src2/551/main/overheat_limit.h b/Inu/Src2/551/main/overheat_limit.h deleted file mode 100644 index dc1199d..0000000 --- a/Inu/Src2/551/main/overheat_limit.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * overheat_limit.h - * - * Created on: 17 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_OVERHEAT_LIMIT_H_ -#define SRC_MAIN_OVERHEAT_LIMIT_H_ - -void calc_limit_overheat(); - -#endif /* SRC_MAIN_OVERHEAT_LIMIT_H_ */ diff --git a/Inu/Src2/551/main/params.h b/Inu/Src2/551/main/params.h deleted file mode 100644 index 7abc592..0000000 --- a/Inu/Src2/551/main/params.h +++ /dev/null @@ -1,182 +0,0 @@ -#ifndef _PARAMS -#define _PARAMS - - -#if(_FLOOR6) - - -#define _ENABLE_PWM_LINES_FOR_TESTS 0//1 -#define _ENABLE_PWM_LINES_FOR_TESTS_ROTOR 0//1 -#define _ENABLE_PWM_LINES_FOR_TESTS_PWM 0//1 -#define _ENABLE_PWM_LINES_FOR_TESTS_RS 0//1 -#define _ENABLE_PWM_LINES_FOR_TESTS_SYNC 0//1 -#define _ENABLE_PWM_LINES_FOR_TESTS_GO 0//1 - -#else - -#define _ENABLE_PWM_LINES_FOR_TESTS 0 -#define _ENABLE_PWM_LINES_FOR_TESTS_ROTOR 0 -#define _ENABLE_PWM_LINES_FOR_TESTS_PWM 0 -#define _ENABLE_PWM_LINES_FOR_TESTS_RS 0 -#define _ENABLE_PWM_LINES_FOR_TESTS_SYNC 0//1 -#define _ENABLE_PWM_LINES_FOR_TESTS_GO 0//1 -#endif - - -#if(_FLOOR6) -#define MODE_DISABLE_ENABLE_WDOG 1 // 0 - wdog , 1 - -#else -#define MODE_DISABLE_ENABLE_WDOG 0 // 0 - wdog , 1 - -#endif - - - - - - -#define CHECK_IN_OUT_TERMINAL 1 - -#define WORK_ON_STEND_D 0//1 - - - -//////////////////////////////////////////////////////////////////// -#ifndef MODE_DISABLE_ENABLE_WDOG -#define MODE_DISABLE_ENABLE_WDOG 0 -#endif - - -#ifndef CHECK_IN_OUT_TERMINAL -#define CHECK_IN_OUT_TERMINAL 0 -#endif - -#ifndef WORK_ON_STEND_D -#define WORK_ON_STEND_D 0 -#endif - - - -/*************************************************************************************/ -//#define BAN_ROTOR_REVERS_DIRECT 1 - -//#define TIME_PAUSE_ZADATCHIK 750//500 - -//#define TIME_SET_LINE_RELAY_FAN 3000 // -//#define LEVEL_FAN_ON_TEMPER_ACDRIVE 1400 // -//#define LEVEL_FAN_OFF_TEMPER_ACDRIVE 1200 // -//( LEVEL_FAN_ON_TEMPER_ACDRIVE ~20 ) -//#define TIME_SET_LINE_RELAY_FAN 3000 // - -/* -#define MAX_TIME_DIRECT_ROTOR 5000 // . -#define MIN_TIME_DIRECT_ROTOR -5000 // - -#define LEVEL_FORWARD_TIME_DIRECT_ROTOR 4000 // -#define LEVEL_BACK_TIME_DIRECT_ROTOR -4000 // - -#define MAX_TIME_ERROR_ROTOR 5000 // . -#define MIN_TIME_ERROR_ROTOR 0 // . - - -#define LEVEL_ON_ERROR_ROTOR 4000 // -#define LEVEL_OFF_ERROR_ROTOR 1000 // -*/ - - -/* -#define PID_KP_IM 0.018 //0.036 //0.018 //0.18 //0.095 // PID Kp -#define PID_KI_IM 0.08 // 0.008 // PID Ki -#define PID_KD_IM 0.0000 //100 // PID Kd -#define PID_KC_IM 0.09 // PID Kc - - -#define PID_KP_F 12//26//12 //40 //20 //12 //20 //60.0 //20.0 //0.095 // PID Kp -#define PID_KI_F 0.00010 // 0.008 // PID Ki -#define PID_KD_F 0.000 //100 PID Kd -#define PID_KC_F 0.005 // PID Kc -//#define PID_KC_F 0.000 // PID Kc - -#define ADD_KP_DF (1000.0/NORMA_MZZ)//(500.0/NORMA_MZZ)//(50.0/NORMA_MZZ) -#define ADD_KI_DF (2000.0/NORMA_MZZ)//(1000.0/NORMA_MZZ)//(100.0/NORMA_MZZ) -#define MAX_DELTA_pidF 2.0 -#define MIN_MZZ_FOR_DF 1761607 //(210/NORMA_MZZ) -*/ - -/* -#define Im_PREDEL 600 // -#define I_OUT_PREDEL -20 // . y -#define U_IN_PREDEL 500 // y - -#define IQ_NORMAL_CHARGE_UD_MAX 12163481 // 1450 V //13002342 // 1550 //_IQtoF(filter.iqU_1_long)*NORMA_ACP -#define IQ_NORMAL_CHARGE_UD_MIN 10066329 // 1200 V - - -#define U_D_MAX_ERROR_GLOBAL 17616076 // 2100 V //17196646 //2050V // 16777216 //2000V/2000*2^24 -#define U_D_MAX_ERROR 16777216 // 2000V //16357785 //1950V //15938355 //1900V/2000*2^24 - -//#define U_D_NORMA_MIN 3774873 // 450 V // 13421772 // 450 V 22.05.08 //1600V/2000*2^24 -//#define U_D_NORMA_MAX 15518924 // //15099494 //1850V/2000*2^24 - -#define U_D_MIN_ERROR 10905190 // 1300V/2000*2^24 - -#define I_IN_MAX_ERROR_GLOBAL 18454937 // 2200 A //16777216 //2000 A // 13421772 //1600 A //10905190 //1300 // 900A - -#define KOEFF_WROTOR_FILTER_SPARTAN 7//8 -#define MAX_DELTA_WROTOR_S_1_2 1 - -#define ENABLE_I_HDW_PROTECT_ON_GLOBAL 1 // - -#define TIME_WAIT_CHARGE 2000 //5000 // 10000 -#define TIME_WAIT_CHARGE_OUT 15000 //15000 -#define TIME_SET_LINE_RELAY 10000 -#define TIME_SET_LINE_RELAY5 3000 -#define TIME_WAIT_LEVEL_QPU2 3000 -*/ - - -/* -///--------------------------- 22220 paremetrs -------------------///////// - -//////////////////////////////////////////////////////////////// -// Loaded capasitors level -#define V_CAPASITORS_LOADED_IQ 11184810 //13421772 ~ 2400V // 11184810 ~ 2000V -#define V_NOMINAL 15099494 //15099494 ~ 2700V - -// Level of nominal currents -#define I_OUT_NOMINAL_IQ 10066329 //8388608 ~ 1500A //5592405 ~ 1000A // 10066329 ~ 1800A - //11184811 ~ 2000A // 12482249 ~ 2232A // 6710886 ~ 1200A -#define I_ZPT_NOMINAL_IQ 6123683 //1095A - - - -#define NORMA_MZZ 3000 //5000 -//#define NORMA_ACP 3000 -#define DISABLE_TEST_TKAK_ON_START 1 -//#define MOTOR_STEND 1 - - -//#define FREQ_PWM 350 //401 //379 - -#ifdef MOTOR_STEND -#define POLUS 4 // - -#define BPSI_NORMAL 0.9//0.7 //Hz -#define MAX_FZAD_FROM_SU 16.7 // -#define MAX_FZAD_FROM_SU_OBOROT 1100 - -#else -#define POLUS 6 // -#define BPSI_NORMAL 0.9 //Hz -#define MAX_FZAD_FROM_SU 16.7 // -#define MAX_FZAD_FROM_SU_OBOROT 1650 -#define COS_FI 0.83 - -#endif -*/ - -#define KOEF_TEMPER_DECR_MZZ 2.0 - -#endif - - - diff --git a/Inu/Src2/551/main/params_alg.h b/Inu/Src2/551/main/params_alg.h deleted file mode 100644 index 8529a80..0000000 --- a/Inu/Src2/551/main/params_alg.h +++ /dev/null @@ -1,218 +0,0 @@ -/* - * params_alg.h - * - * Created on: 26 . 2020 . - * Author: Yura - */ - -#ifndef SRC_MAIN_PARAMS_ALG_H_ -#define SRC_MAIN_PARAMS_ALG_H_ - -// , -#define DISABLE_CALC_KM_ON_SLAVE 0//1 - -#define DISABLE_WORK_BREAK 0 // - -#define SENSOR_ALG_22220 1 -#define SENSOR_ALG_23550 2 - - -#define SENSOR_ALG SENSOR_ALG_22220 -//#define SENSOR_ALG SENSOR_ALG_23550 - - -#define NOMINAL_U_ZARYAD 2520 // -#define NOMINAL_U_BREAK_LEVEL 2580 // + IQ_DELTA_U_START_RECUP=100V -#define NOMINAL_SET_IZAD 910 // -#define NOMINAL_SET_K_U_DISBALANCE 40//20 // . , >0 -#define NOMINAL_SET_LIMIT_POWER 6300 // - - - - - -/////////////////////////////////////////////////////////////// -#define U_D_MAX_ERROR_GLOBAL IQ_I_U_VALUE_PLUS_2850 //U_D_MAX_ERROR_GLOBAL_2850 - -#define MAX_U_PROC_SMALL 2.5 //1.4 -#define MAX_U_PROC 1.1 //1.11 //1.4 -#define MIN_U_PROC 0.8 //0.7 - -#define ADD_U_MAX_GLOBAL 200.0 //V GLOBAL ZadanieU_Charge -#define ADD_U_MAX_GLOBAL_SMALL 500.0 //V GLOBAL ZadanieU_Charge -#define LEVEL_DETECT_U_SMALL 1000.0 //V GLOBAL ZadanieU_Charge - -#define KOEF_IM_ON_TORMOG 0.65// 0.75 // -#define KOEF_IM_ON_TORMOG_WITH_MAX_TEMPER_BREAK 0.1// - -/////////////////////////////////////////////////////////////// - - -#define MZZ_ADD_1 0.5 // 0.25 //0.5 1 -#define MZZ_ADD_2 0.15 ///0.1 //0.05 //0.1 1 -#define MZZ_ADD_3 0.25 //0.05 ///0.1 //0.05 //0.1 1 - -#define FZAD_ADD_MAX 0.08 //0.005 //0.08 fzad 1 -#define FZAD_DEC 0.0004 // fzad 1 - -#define POWERZAD_ADD_MAX 0.08 //0.005 //0.08 fzad 1 -#define POWERZAD_DEC 0.0004 // fzad 1 - -#define POLUS 6 //6 // -#define BPSI_NORMAL 0.22 //0.3 // -#define BPSI_MAXIMAL 0.35 //0.3 // -#define BPSI_MINIMAL 0.05 //0.3 // -#define PROVOROT_F_HZ 0.2 // -#define PROVOROT_OBOROTS 10 // - - -#define ADD_KP_DF (1000.0/NORMA_MZZ)//(500.0/NORMA_MZZ)//(50.0/NORMA_MZZ) -#define ADD_KI_DF (2000.0/NORMA_MZZ)//(1000.0/NORMA_MZZ)//(100.0/NORMA_MZZ) - -#define ADD_KP_DPOWER (1000.0/NORMA_MZZ)//(500.0/NORMA_MZZ)//(50.0/NORMA_MZZ) -#define ADD_KI_DPOWER (2000.0/NORMA_MZZ)//(1000.0/NORMA_MZZ)//(100.0/NORMA_MZZ) - -#define MIN_MZZ_FOR_DF 210 -#define MIN_MZZ_FOR_DPOWER 210 - - -//////////////////// - - -#define PID_KP_IM 0.036 //0.018 //0.0013// 0.018 //0.036 //0.018 //0.18 //0.095 // PID Kp -#define PID_KI_IM 0.32 // 0.16 //0.32 //0.16 //0.08//0.8//0.025 //0.08 // PID Ki -#define PID_KD_IM 0.0000 //*100 // PID Kd -#define PID_KC_IM 0.09 // PID Kc - - -#define PID_KP_F 12.0//6.0//12.0 //6.0 //18 //12//6//26//12 //40 //20 //12 //20 //60.0 //20.0 //0.095 // PID Kp -#define PID_KI_F 0.00020 //0.00010 // 0.008 // PID Ki -//#define PID_KI_F 0.00030 //0.00010 // 0.008 // PID Ki -#define PID_KD_F 0.000 //*100 PID Kd -#define PID_KC_F 0.00005//0.005 // PID Kc -//#define PID_KC_F 0.000 // PID Kc - -#define PID_KP_POWER 9//3//26//12 //40 //20 //12 //20 //60.0 //20.0 //0.095 // PID Kp -//#define PID_KI_F 0.00020 //0.00010 // 0.008 // PID Ki -#define PID_KI_POWER 0.00030 //0.00010 // 0.008 // PID Ki -#define PID_KD_POWER 0.000 //*100 PID Kd -#define PID_KC_POWER 0.0001 // PID Kc - - - -/////////////////// -// . k - -#define K_STATOR_MAX 0.93 // 0.91 // DEF_PERIOD_MIN_MKS = 60 -#define K_STATOR_MIN 0.020 // 0.91 // DEF_PERIOD_MIN_MKS = 60 - -//#define K_STATOR_MAX 0.89 // DEF_PERIOD_MIN_MKS = 80 - - - - -#define MAX_ZADANIE_I_VOZBUD 200.0 // A - -#define MAX_ZADANIE_U_CHARGE 2800.0//1500.0 //V -//#define MAX_ZADANIE_F_ROTOR 70 - -#define MAX_ZADANIE_OBOROTS_ROTOR 230.0 //340 //240 1000 //260.0 // +/- ob/min -#define MIN_ZADANIE_OBOROTS_ROTOR -230.0 //-180.0 //-230.0 // 1000 //260.0 // +/- ob/min - -#define MAX_1_ZADANIE_OBOROTS_ROTOR 120.0 //340 //240 1000 //260.0 // +/- ob/min -#define MIN_1_ZADANIE_OBOROTS_ROTOR -90.0 //-230.0 // 1000 //260.0 // +/- ob/min - - -#define DEAD_ZONE_ZADANIE_OBOROTS_ROTOR 10.0 - -#define MAX_ZADANIE_I_M 950.0// 1000.0 //750.0 // A - -#define MAX_ZADANIE_POWER 6300.0 // kWt -#define MIN_ZADANIE_POWER -6300.0 // kWt - -#define MAX_1_ZADANIE_POWER 3000.0 // kWt -#define MIN_1_ZADANIE_POWER -3000.0 // kWt - - -#define SUPER_MAX_ZADANIE_LIMIT_POWER 6500.0 // kWt - -#define MAX_ZADANIE_LIMIT_POWER 6300.0 // kWt -#define MAX_1_ZADANIE_LIMIT_POWER 2000.0 // kWt - -#define MIN_ZADANIE_LIMIT_POWER 100.0 // kWt -#define MIN_ZADANIE_LIMIT_POWER_FROM_SVU 50.0 // kWt -#define POWER_ZAPAS_FOR_UOM 5 //50 // - -#define DEAD_ZONE_ZADANIE_POWER 50.0 // kWt -#define DEAD_ZONE_ZADANIE_LIMIT_POWER 50.0 // kWt - - - -#define MAX_ZADANIE_K_M K_STATOR_MAX // A -#define MAX_ZADANIE_F 30.0 // Hz -#define MIN_ZADANIE_F -30.0 //60.0 // Hz - - -#define MAX_ZADANIE_K_U_DISBALANCE 2.0 //1.0 // k -#define MAX_ZADANIE_KPLUS_U_DISBALANCE 1.0 // k - - - -#define T_NARAST_ZADANIE_F 5.0 // sec -#define T_NARAST_ZADANIE_OBOROTS_ROTOR 80.0 //20.0 //30.0 //15.0 // sec - -#define T1_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS 80.0 //20.0 //30.0 //15.0 // sec -#define T1_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS 40.0 //20.0 //30.0 //15.0 // sec - -#define T2_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS 80.0 //160.0 //20.0 //30.0 //15.0 // sec -#define T2_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS 40.0 //20.0 //30.0 //15.0 // sec - -#define T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS 600.0 //160.0 //20.0 //30.0 //15.0 // sec -#define T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS 600.0 //20.0 //30.0 //15.0 // sec - - - -#define T_NARAST_ZADANIE_K_M 15.0 // sec -#define T_NARAST_ZADANIE_I_M 15.0 // sec - -#define T1_NARAST_ZADANIE_POWER_PLUS 80.0 //30.0 // sec -#define T1_NARAST_ZADANIE_POWER_MINUS 30.0 //30.0 // sec -#define T2_NARAST_ZADANIE_POWER_PLUS 80.0 //30.0 // sec -#define T2_NARAST_ZADANIE_POWER_MINUS 30.0 //30.0 // sec - -#define T_NARAST_ZADANIE_LIMIT_POWER 5.0 //30.0 // sec - -#define T1_NARAST_ZADANIE_LIMIT_POWER_PLUS 30.0 //30.0 // sec -#define T1_NARAST_ZADANIE_LIMIT_POWER_MINUS 5.0 //30.0 // sec -#define T2_NARAST_ZADANIE_LIMIT_POWER_PLUS 80.0 //30.0 // sec -#define T2_NARAST_ZADANIE_LIMIT_POWER_MINUS 5.0 //30.0 // sec - - -#define T_NARAST_ZADANIE_U_CHARGE 2.0 // sec -#define T_NARAST_ZADANIE_K_U_DISBALANCE 15.0 // sec -#define T_NARAST_ZADANIE_KPLUS_U_DISBALANCE 15.0 // sec - -#define T_NARAST_ZADANIE_IMITATION_OBOROTS_ROTOR 30.0 // sec - - - - - - -#define ENABLE_DECR_MZZ_POWER_IZAD 1 -// -#define POWER_AIN_100KW 186413 - -#define DELTA_LEVEL_POWER_AIN_DECR_MZZ_DEF (3*POWER_AIN_100KW) // 300 kW // 559240 //300 //186413 //100kW // iqP = P (W) /3000/3000 * 2^24 // -#define MIN_DELTA_LEVEL_POWER_AIN_DECR_MZZ_DEF (5*POWER_AIN_100KW) // 500 kW -#define SMEWENIE_LEVEL_POWER_AIN_DECR_MZZ_DEF 0 //(1*POWER_AIN_100KW) // 100 kW - -// 1 - 0 - -#define MAX_KOEF_OGRAN_POWER_LIMIT CONST_IQ_05 // 0.5 -#define EXP_FILTER_KOEF_OGRAN_POWER_LIMIT 4.40//2.22 // - - - - - -#endif /* SRC_MAIN_PARAMS_ALG_H_ */ diff --git a/Inu/Src2/551/main/params_bsu.h b/Inu/Src2/551/main/params_bsu.h deleted file mode 100644 index a58cc2c..0000000 --- a/Inu/Src2/551/main/params_bsu.h +++ /dev/null @@ -1,123 +0,0 @@ -/* - * params_bsu.h - * - * Created on: 14 . 2020 . - * Author: yura - */ - -#ifndef SRC_MAIN_PARAMS_BSU_H_ -#define SRC_MAIN_PARAMS_BSU_H_ - -#include "iq_values_norma_f.h" -#include "iq_values_norma_iu.h" -#include "iq_values_norma_oborot.h" - - - -#define TKAK_23550 1 -#define TKAK_EDRK 2 - - -#define TKAK_VERSION TKAK_23550 -//#define TKAK_VERSION TKAK_EDRK - - -// 23550_sp2 -#define TK_DEAD_TIME_NS 25000 //25.0 //40.0 -#define TK_ACKN_TIME_NS 2200 -#define TK_MIN_TIME_NS 25000 //80.0 //35.0 // 15.0 //40.0 -#define TK_SOFT_OFF_TIME_NS 20000 //25.0 //40.0 - -#define DIV_TIME_TK_SOFT_OFF 20 // 20 nsec -> 1 bit -#define DIV_TIME_TK_DEAD 640 //0.16 // 160 nsec -> 1 bit -#define DIV_TIME_TK_MIN 640 //0.16 // 160 nsec -> 1 bit - -#define DIV_TIME_TK_ACKN 20 // 20 nsec -> 1 bit - - -// sp2 - -#define TK_DEAD_TIME_MKS 25.0 //40.0 -#define TK_ACKN_TIME_MKS 2.2 -#define TK_MIN_TIME_MKS 25.0 // 15.0 //40.0 -#define DIV_TIME_TK 0.4 // 0.16 - - -#define MAX_READ_SBUS 1 //10 - - -#if (_FLOOR6==1) -// tkak -#define TKAK0_OFF_PROTECT 1 -#define TKAK1_OFF_PROTECT 1 -#define TKAK2_OFF_PROTECT 1 -#define TKAK3_OFF_PROTECT 1 -#define IN0_OFF_PROTECT 1 -#define IN1_OFF_PROTECT 1 -#define OUT0_OFF_PROTECT 1 - -#else -// tkak -#define TKAK0_OFF_PROTECT 0 -#define TKAK1_OFF_PROTECT 0 -#define TKAK2_OFF_PROTECT 0 -#define TKAK3_OFF_PROTECT 0 -#define IN0_OFF_PROTECT 0 -#define IN1_OFF_PROTECT 0 -#define OUT0_OFF_PROTECT 0 -#endif - -// -#define TK_DISABLE_OUTPUT_A1 0 -#define TK_DISABLE_OUTPUT_B1 0 -#define TK_DISABLE_OUTPUT_C1 0 - -#define TK_DISABLE_OUTPUT_A2 0 -#define TK_DISABLE_OUTPUT_B2 0 -#define TK_DISABLE_OUTPUT_C2 0 - -////////////////////// - -#define ENABLE_ROTOR_SENSOR_ZERO_SIGNAL 0 -#define ENABLE_ROTOR_SENSOR_1_PM67 1 -#define ENABLE_ROTOR_SENSOR_2_PM67 0 - -#define ENABLE_ROTOR_SENSOR_1_PBUS 0//1 -#define ENABLE_ROTOR_SENSOR_2_PBUS 0//1 - - -#if (ENABLE_ROTOR_SENSOR_1_PM67==1 && ENABLE_ROTOR_SENSOR_2_PM67==0) -// 1- 2- -#define ENABLE_COMBO_SENSOR_1_TO_2 1 -#define ENABLE_COMBO_SENSOR_2_TO_1 0 -#endif - -#if (ENABLE_ROTOR_SENSOR_1_PM67==0 && ENABLE_ROTOR_SENSOR_2_PM67==1) -// 2- 1- -#define ENABLE_COMBO_SENSOR_2_TO_1 1 -#define ENABLE_COMBO_SENSOR_1_TO_2 0 -#endif - - - -#if (ENABLE_ROTOR_SENSOR_1_PM67==1 && ENABLE_ROTOR_SENSOR_2_PM67==1) - -#define ENABLE_COMBO_SENSOR_1 0 -#define ENABLE_COMBO_SENSOR_1 0 - -#endif - - -#define ROTOR_SENSOR_IMPULSES_PER_ROTATE 4096 // 1024 // - 1 - - - - -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// - -///////////////////////////////////////////////////////// - - - -#endif /* SRC_MAIN_PARAMS_BSU_H_ */ diff --git a/Inu/Src2/551/main/params_hwp.h b/Inu/Src2/551/main/params_hwp.h deleted file mode 100644 index d3ebed4..0000000 --- a/Inu/Src2/551/main/params_hwp.h +++ /dev/null @@ -1,9 +0,0 @@ - - -#define LEVEL_HWP_U_ZPT 2900 //2800 // V 0-2900 V -#define LEVEL_HWP_I_AF 800 //700 // A 0-450A -#define LEVEL_HWP_I_ZPT 1600 // A 0-2000?? -#define LEVEL_HWP_U_ABC 3100 //2900 //2800 // V 0-2000V -#define LEVEL_HWP_I_BREAK 900 //750 // A 0-2000A - -#define convert_real_to_mv_hwp(nc,value) ((float)value*(float)R_ADC[0][nc]/(float)K_LEM_ADC[0][nc]*10.0) diff --git a/Inu/Src2/551/main/params_motor.h b/Inu/Src2/551/main/params_motor.h deleted file mode 100644 index 1e8c030..0000000 --- a/Inu/Src2/551/main/params_motor.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * params_motor.h - * - * Created on: 14 . 2020 . - * Author: yura - */ - -#ifndef SRC_MAIN_PARAMS_MOTOR_H_ -#define SRC_MAIN_PARAMS_MOTOR_H_ - - - -#define SDVIG_OBMOTKI_ZERO 1 -#define SDVIG_OBMOTKI_30_PLUS 2 -#define SDVIG_OBMOTKI_30_MINUS 3 - - -#define SETUP_SDVIG_OBMOTKI SDVIG_OBMOTKI_ZERO //SDVIG_OBMOTKI_ZERO -//#define SETUP_SDVIG_OBMOTKI SDVIG_OBMOTKI_30_PLUS //SDVIG_OBMOTKI_ZERO - -#define COS_FI 0.87 -// Level of nominal currents -#define I_OUT_NOMINAL_IQ 5033164// 900 A //8388608 ~ 1500A //5592405 ~ 1000A // 10066329 ~ 1800A - //11184811 ~ 2000A // 12482249 ~ 2232A // 6710886 ~ 1200A -#define I_OUT_NOMINAL 900 - -#define MOTOR_CURRENT_NOMINAL 650.0 //930.0 -#define MOTOR_CURRENT_MAX 900.00 //1489.0 - - - - -#define P_NOMINAL 6300 //KWt -#define WROT_NOMINAL 180.0 -#define WROT_MAX 530.0 -#define FROT_NOMINAL (WROT_NOMINAL / 60.0) -#define FROT_MAX (WROT_MAX / 60.0) - -//#define REVERS_ON_CLOCK 1 // 0 // 1- .. 0 - - - - -//#define WORK_TWICE 0 /* y */ - - -//#define MAX_ZAD_OBOROTS 200 - -#define L_SIGMA_S 0.0001467 -#define L_SIGMA_R 0.00000923 -#define L_M 0.00421 -#define R_STATOR 0.002 -#define R_ROTOR_SHTRIH 0.0021 -#define SLIP_NOM 0.006 -#define R_ROTOR (R_ROTOR_SHTRIH / SLIP_NOM) -#define F_STATOR_NOM 50.0 - - - -#endif /* SRC_MAIN_PARAMS_MOTOR_H_ */ diff --git a/Inu/Src2/551/main/params_norma.h b/Inu/Src2/551/main/params_norma.h deleted file mode 100644 index 9c3e790..0000000 --- a/Inu/Src2/551/main/params_norma.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * params_norma.h - * - * Created on: 14 . 2020 . - * Author: yura - */ - -#ifndef _PARAMS_NORMA_H_ -#define _PARAMS_NORMA_H_ - - -//////////////////////////////////////////////////// -#define NORMA_FROTOR_INT 20 -#define NORMA_ACP_INT 3000 -#define NORMA_ANGLE 360 -//////////////////////////////////////////////////// -//////////////////////////////////////////////////// -//#define NORMA_FROTOR 20.0 -//#define NORMA_ACP 3000.0 - -#define NORMA_FROTOR ((float)NORMA_FROTOR_INT) -#define NORMA_ACP ((float)NORMA_ACP_INT) - - -#define NORMA_MZZ_INT NORMA_ACP_INT -#define NORMA_MZZ ((float)NORMA_MZZ_INT) - -#define NORMA_I_U_INT NORMA_MZZ_INT - -//////////////////////////////////////////////////// -//////////////////////////////////////////////////// -//////////////////////////////////////////////////// - -//#define F_STATOR_MAX NORMA_FROTOR // . - - - - -#define NORMA_ACP_P 100.0 - -#define NORMA_ACP_RMS 2127.66 - -#define NORMA_ACP_TEMPER_MILL_AMP 100.0 // - -#ifndef PROJECT_SHIP -#error PROJECT_SHIP predifine Name -#else - - -#if (PROJECT_SHIP == 1) -#define NORMA_ACP_TEMPER 100.0 // 23550.1 -#endif - - -#if (PROJECT_SHIP == 2) -#define NORMA_ACP_TEMPER 200.0 // 23550.3 -#endif - -#if (PROJECT_SHIP== 3) -#define NORMA_ACP_TEMPER 200.0 // 23550.3 - -#endif - - -#endif - - - - -#endif /* _PARAMS_NORMA_H_ */ diff --git a/Inu/Src2/551/main/params_protect_adc.h b/Inu/Src2/551/main/params_protect_adc.h deleted file mode 100644 index b5772bd..0000000 --- a/Inu/Src2/551/main/params_protect_adc.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * params_protect_adc.h - * - * Created on: 8 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_PARAMS_PROTECT_ADC_H_ -#define SRC_MAIN_PARAMS_PROTECT_ADC_H_ -#include -#include - - -#define LEVEL_ADC_I_AF LEVEL_HWP_I_AF // A 0-450A -#define LEVEL_ADC_I_ZPT LEVEL_HWP_I_ZPT // A 0-2000?? -//#define LEVEL_ADC_U_ABC 1000 // V 0-2000V -#define LEVEL_ADC_I_BREAK LEVEL_HWP_I_BREAK // A 0-2000A -#define LEVEL_ADC_U_ZPT_MAX LEVEL_HWP_U_ZPT -#define LEVEL_ADC_U_ZPT_MIN 1800 -#define LEVEL_ADC_U_IN_MAX LEVEL_HWP_U_ABC -#define LEVEL_ADC_U_IN_MIN 1380 -#define LEVEL_ADC_I_OUT_MAX I_OUT_NOMINAL //(I_OUT_NOMINAL * 1.9) - - -#endif /* SRC_MAIN_PARAMS_PROTECT_ADC_H_ */ diff --git a/Inu/Src2/551/main/params_pwm24.h b/Inu/Src2/551/main/params_pwm24.h deleted file mode 100644 index a2571a6..0000000 --- a/Inu/Src2/551/main/params_pwm24.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * params_pwm24.h - * - * Created on: 14 . 2020 . - * Author: yura - */ - -#ifndef SRC_MAIN_PARAMS_PWM24_H_ -#define SRC_MAIN_PARAMS_PWM24_H_ - -/////////////////////////////////////////////////////// -#if (_SIMULATE_AC==1) -#define FREQ_PWM 100 //450 //800 /* */ //3138 // 2360//2477 // -#else -#define FREQ_PWM 450 //800 /* */ //3138 // 2360//2477 // -#endif - -#define DEF_PERIOD_MIN_MKS 60 // 80 //60 // = 2*TK_MIN_TIME_MKS = 30 - // + TK_DEAD_TIME_MKS + 5mks = 60 -#define DEF_PERIOD_MIN_BR_XTICS 165 - -///////////////// - - - -#define PWM_ONE_INTERRUPT_RUN 1 -#define PWM_TWICE_INTERRUPT_RUN 0 - - - -//#define PWN_COUNT_RUN_PER_INTERRUPT PWM_ONE_INTERRUPT_RUN // -#define PWN_COUNT_RUN_PER_INTERRUPT PWM_TWICE_INTERRUPT_RUN // - - - - - -/////////////////////////// -#define FREQ_INTERNAL_GENERATOR_XILINX_TMS 1875000 // 67 - - -////////////////////// - - - - - -#endif /* SRC_MAIN_PARAMS_PWM24_H_ */ diff --git a/Inu/Src2/551/main/params_temper_p.h b/Inu/Src2/551/main/params_temper_p.h deleted file mode 100644 index 2864649..0000000 --- a/Inu/Src2/551/main/params_temper_p.h +++ /dev/null @@ -1,58 +0,0 @@ - -#define INDEX_T_WATER_EXT 0 -#define INDEX_T_WATER_INT 1 - - -#define ALARM_TEMPER_BREAK_INT 1100 -#define ABNORMAL_TEMPER_BREAK_INT 900 -#define DELTA_TEMPER_BREAK_INT 20 - - - - -// koef to svu -#define K_TEMPER_TO_SVU 10.0 -#define K_P_WATER_TO_SVU 100.0 - - - -// T UO1_7 -#define ALARM_TEMPER_AF 400 -#define ABNORMAL_TEMPER_AF 350 - - -// T water INT EXT -#define ALARM_TEMPER_WATER_INT 400 -#define ABNORMAL_TEMPER_WATER_INT 350 - - -#define ALARM_TEMPER_WATER_EXT 400 -#define ABNORMAL_TEMPER_WATER_EXT 350 - - -// P water max -#define ALARM_P_WATER_MAX_INT 600 -#define ABNORMAL_P_WATER_MAX_INT 500 - -// P water min -#define ALARM_P_WATER_MIN_INT 150//300 -#define ABNORMAL_P_WATER_MIN_INT 180//320 - -#define ALARM_P_WATER_MIN_INT_ON_OFF_PUMP 60// 110 -#define ABNORMAL_P_WATER_MIN_INT_ON_OFF_PUMP 110// 130 - - -// air -#define ALARM_TEMPER_AIR_INT 450 -#define ABNORMAL_TEMPER_AIR_INT 400 - -//ac drive -#define ALARM_TEMPER_ACDRIVE_WINDING 1300 -#define ABNORMAL_TEMPER_ACDRIVE_WINDING 1200 - -#define ALARM_TEMPER_ACDRIVE_BEAR 900 -#define ABNORMAL_TEMPER_ACDRIVE_BEAR 800 - -#define ALARM_TEMPER_BSU 600 -#define ABNORMAL_TEMPER_BSU 500 - diff --git a/Inu/Src2/551/main/pll_tools.c b/Inu/Src2/551/main/pll_tools.c deleted file mode 100644 index decc342..0000000 --- a/Inu/Src2/551/main/pll_tools.c +++ /dev/null @@ -1,105 +0,0 @@ -/* - * pll_tools.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - - -#include - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include "adc_tools.h" -#include "limit_power.h" -#include "pll_tools.h" - - -//#pragma DATA_SECTION(pll1, ".slow_vars") -PLL_REC pll1 = PLL_REC_DEFAULT; - - - -void init_50hz_input_net50hz(void) -{ - - //1. - - pll1.setup.freq_run_pll = (FREQ_RUN_PLL); // . - pll1.setup.rotation_u_cba = 0;//0;//1; // : 0 - A-B-C, 1 - A-C-B - - pll1.init(&pll1); // - - // - -} - -void calc_pll_50hz(void) -{ - - // . - pll1.input.Input_U_AB = analog.iqUin_A1B1; - pll1.input.Input_U_BC = analog.iqUin_B1C1; - pll1.input.Input_U_CA = analog.iqUin_C1A1; - - // , setup.freq_run_pll - pll1.calc_pll(&pll1); -// - - -} - - -void get_freq_50hz_float(void) -{ - float int_delta_freq_test; - // 3. . - - // *100. - // - . - pll1.get_freq_float(&pll1); - - if (edrk.Status_Ready.bits.preImitationReady2) - edrk.freq_50hz_1 = 5001; - else - edrk.freq_50hz_1 = pll1.output.int_freq_net; - - if (delta_freq_test>0) - { - int_delta_freq_test = _IQtoF( delta_freq_test) * pll1.setup.freq_run_pll / PI * 50.00; // freq*100 - edrk.freq_50hz_1 -= int_delta_freq_test; - } - - // - - -} - -void get_freq_50hz_iq(void) -{ - - // 3. . - - // *100. - // - . - pll1.get_freq_iq(&pll1); - - if (edrk.Status_Ready.bits.preImitationReady2) - edrk.iq_freq_50hz = level_50hz; - else - edrk.iq_freq_50hz = pll1.output.iq_freq_net; - - if (delta_freq_test>0) - edrk.iq_freq_50hz -= delta_freq_test; - - // - - -} - diff --git a/Inu/Src2/551/main/pll_tools.h b/Inu/Src2/551/main/pll_tools.h deleted file mode 100644 index 1659e90..0000000 --- a/Inu/Src2/551/main/pll_tools.h +++ /dev/null @@ -1,22 +0,0 @@ -/* - * pll_tools.h - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#ifndef SRC_MAIN_PLL_TOOLS_H_ -#define SRC_MAIN_PLL_TOOLS_H_ - - -#define FREQ_RUN_PLL (2*FREQ_PWM) - - -void get_freq_50hz_float(void); -void get_freq_50hz_iq(void); -void calc_pll_50hz(void); -void init_50hz_input_net50hz(void); - -extern PLL_REC pll1; - -#endif /* SRC_MAIN_PLL_TOOLS_H_ */ diff --git a/Inu/Src2/551/main/project.c b/Inu/Src2/551/main/project.c deleted file mode 100644 index a8fcec9..0000000 --- a/Inu/Src2/551/main/project.c +++ /dev/null @@ -1,1049 +0,0 @@ -#include -#include -#include - -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" -#include "xp_cds_in.h" -#include "xp_cds_out.h" -#include "xp_cds_rs.h" -#include "xp_cds_tk.h" -#include "xp_cds_tk_balzam.h" -#include "xp_project.h" - - -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// - - -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// - - - - - - - -void tkak_init_plate(int k, int tkak0_off_protect, int tk_disable_output_a, int tk_disable_output_b) -{ - unsigned int t_ticks; - //tkak 0 - project.cds_tk[k].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup - - - if (k==3) - { -// project.cds_tk[k].setup_pbus.use_reg_in_pbus.all = 0x0; // PBUS all off - // - if (tkak0_off_protect==1) - project.cds_tk[k].write.sbus.mask_protect_tk.all = 0x0000; // only break ack+cur - else - project.cds_tk[k].write.sbus.mask_protect_tk.all = 0x0303; // only break ack+cur - -// project.cds_tk[k].write.sbus.mask_tk_out_40pin.all = 0x0003; // optical bus+break - project.cds_tk[k].write.sbus.mask_tk_out_40pin.all = 0x00cf; // optical bus+break - -#if (TKAK_VERSION==TKAK_EDRK) - project.cds_tk[k].write.sbus.ack_time.bit.time = (int)(TK_ACKN_TIME_MKS / 0.02); - project.cds_tk[k].write.sbus.dead_min_time.bit.mintime = (int)(TK_MIN_TIME_MKS / DIV_TIME_TK); - project.cds_tk[k].write.sbus.dead_min_time.bit.deadtime = (int)(TK_DEAD_TIME_MKS / DIV_TIME_TK); -#endif - -#if (TKAK_VERSION==TKAK_23550) - project.cds_tk[k].write.sbus.ack_time.bit.time = (int)(TK_ACKN_TIME_NS/1000.0 / 0.02); - project.cds_tk[k].write.sbus.dead_min_time.bit.mintime = (int)(TK_MIN_TIME_NS/1000.0 / DIV_TIME_TK); - project.cds_tk[k].write.sbus.dead_min_time.bit.deadtime = (int)(TK_DEAD_TIME_NS/1000.0 / DIV_TIME_TK); -#endif - } - else - { - project.cds_tk[k].setup_pbus.use_reg_in_pbus.all = 0x0; // PBUS all off - - if (tk_disable_output_a==1 && tk_disable_output_b==1) - { - project.cds_tk[k].write.sbus.mask_tk_out_40pin.all = 0x0000; //mask key 1-use key,0-not use key - project.cds_tk[k].write.sbus.mask_protect_tk.all = 0xff00; // cur+ack 1-on protect. - } - else - if (tk_disable_output_a==0 && tk_disable_output_b==1) - { - project.cds_tk[k].write.sbus.mask_tk_out_40pin.all = 0x000f; //mask key 1-use key,0-not use key - project.cds_tk[k].write.sbus.mask_protect_tk.all = 0x0f0f; // cur+ack 1-on protect. - } - else - if (tk_disable_output_a==1 && tk_disable_output_b==0) - { - project.cds_tk[k].write.sbus.mask_tk_out_40pin.all = 0x00f0; //mask key 1-use key,0-not use key - project.cds_tk[k].write.sbus.mask_protect_tk.all = 0xf0f0; // cur+ack 1-on protect. - } - else - if (tk_disable_output_a==0 && tk_disable_output_b==0) - { - project.cds_tk[k].write.sbus.mask_tk_out_40pin.all = 0x00ff; //mask key 1-use key,0-not use key - project.cds_tk[k].write.sbus.mask_protect_tk.all = 0xffff; // cur+ack 1-on protect. - } - - if (tkak0_off_protect==1) - project.cds_tk[k].write.sbus.mask_protect_tk.all = 0x0000; // cur+ack 1-on protect. - - -#if (TKAK_VERSION==TKAK_EDRK) - project.cds_tk[k].write.sbus.ack_time.bit.time = (int)(TK_ACKN_TIME_MKS / 0.02); - project.cds_tk[k].write.sbus.dead_min_time.bit.mintime = (int)(TK_MIN_TIME_MKS / DIV_TIME_TK); - project.cds_tk[k].write.sbus.dead_min_time.bit.deadtime = (int)(TK_DEAD_TIME_MKS / DIV_TIME_TK); -#endif - - -#if (TKAK_VERSION==TKAK_23550) - - -// TK_ACKN_TIME_NS - - t_ticks = (unsigned int)(TK_ACKN_TIME_NS / DIV_TIME_TK_ACKN); - -#if (TK_ACKN_TIME_NS>(DIV_TIME_TK_ACKN*255)) -#error "TK_ACKN_TIME_NS !" -#endif - project.cds_tk[k].write.sbus.ack_time.bit.time = (unsigned int)t_ticks; - - -// TK_MIN_TIME_NS - - t_ticks = (unsigned int)(TK_MIN_TIME_NS / DIV_TIME_TK_MIN); - -#if (TK_MIN_TIME_NS>(DIV_TIME_TK_MIN*255)) -#error "TK_MIN_TIME_NS !" -#endif - - project.cds_tk[k].write.sbus.dead_min_time.bit.mintime = (unsigned int)t_ticks; - - -//TK_DEAD_TIME_NS - - // dead_time = 5 , - 5 - t_ticks = (unsigned int)(TK_DEAD_TIME_NS / DIV_TIME_TK_DEAD); - -#if (TK_DEAD_TIME_NS>(DIV_TIME_TK_DEAD*255)) -#error "TK_DEAD_TIME_MKS !" -#endif - project.cds_tk[k].write.sbus.dead_min_time.bit.deadtime = (unsigned int)(t_ticks); - - -// TK_SOFT_OFF_TIME_NS - - t_ticks = (unsigned int)(TK_SOFT_OFF_TIME_NS / DIV_TIME_TK_SOFT_OFF); - -#if (TK_SOFT_OFF_TIME_NS>(DIV_TIME_TK_SOFT_OFF*65535)) -#error "TK_SOFT_OFF_TIME_MKS !" -#endif - project.cds_tk[k].write.sbus.time_after_err = (unsigned int)t_ticks; - -#endif - - - - - project.cds_tk[k].write.sbus.protect_error.bit.enable_soft_disconnect = 1; - project.cds_tk[k].write.sbus.protect_error.bit.detect_soft_disconnect = 0;//1; - - } - - if (tkak0_off_protect==1) - { - project.cds_tk[k].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_tk[k].write.sbus.protect_error.bit.enable_err_switch = 0; - project.cds_tk[k].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_tk[k].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_tk[k].write.sbus.protect_error.bit.disable_err_mintime = 0; - project.cds_tk[k].write.sbus.protect_error.bit.enable_line_err = 0; - project.cds_tk[k].write.sbus.protect_error.bit.enable_soft_disconnect = 0; - project.cds_tk[k].write.sbus.protect_error.bit.detect_soft_disconnect = 0; - } - else - { - - project.cds_tk[k].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_tk[k].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_tk[k].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_tk[k].write.sbus.protect_error.bit.disable_err_mintime = 1; - project.cds_tk[k].write.sbus.protect_error.bit.enable_line_err = 1;//1;//0;//1; - - // ! ! - if (project.cds_tk[k].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_tk[k].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_tk[k].write.sbus.protect_error.bit.enable_err_switch = 1; - } - - -} - - -//////////////////////////////////////////////////////////////// -// . HWP -//////////////////////////////////////////////////////////////// -void project_prepare_config(void) -{ - int k = 0; -// write here setup for all plates -// -// -// -// ... -// project.cds_tk[0].write.sbus.ack_time.bit.delay = ...; - - - - -////////////////////////////////////////////////////////////////// -/// -// PBUS -///////////////////////////////////////////////////////////////// - -#if (USE_IN_0) - project.cds_in[0].type_plate = cds_in_type_in_1; -////////////////////////////////////////////////////////////////// -// PBUS IN0 sensors -// - project.cds_in[0].setup_pbus.use_reg_in_pbus.all = 0; -//DataFromIn - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg0 = 1; // use -//Gotov - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg1 = 1; // use -//Direction - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg2 = 1; // use - -//#if (ENABLE_ROTOR_SENSOR_1_PBUS==1) -// sensor1 -//SpeedS1_cnt - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg3 = 1; // use -//SpeedS1_cnt90 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg4 = 1; // use -//#endif - -//#if (ENABLE_ROTOR_SENSOR_2_PBUS==1) -// sensor2 -//SpeedS2_cnt - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg5 = 1; // use -//SpeedS2_cnt90 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg6 = 1; // use -//#endif - -//#if (TYPE_CDS_XILINX_IN_0==TYPE_CDS_XILINX_SP2) -// if (project.cds_in[0].type_cds_xilinx == TYPE_CDS_XILINX_SP2) -//is Channel Alive -// project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg13 = 1; // use -//#endif - - - if (project.cds_in[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - { - - #if (ENABLE_ROTOR_SENSOR_1_PBUS==1) - //Time since zero point S1 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg7 = 1; // use - // Impulses since zero point Rising S1 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg8 = 1; // use - //Impulses since zero point Falling S1 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg9 = 1; // use - #endif - - - #if (ENABLE_ROTOR_SENSOR_2_PBUS==1) - //Time since zero point S2 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg10 = 1; // use - // Impulses since zero point Rising S2 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg11 = 1; // use - //Impulses since zero point Falling S2 - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg12 = 1; // use - #endif - - //is Channel Alive - project.cds_in[0].setup_pbus.use_reg_in_pbus.bit.reg13 = 1; // use - - } - project.cds_in[0].status_serial_bus.max_read_error = MAX_READ_SBUS; - - -#endif - - -#if (USE_IN_1) - project.cds_in[1].type_plate = cds_in_type_in_2; -// IN1 - project.cds_in[1].setup_pbus.use_reg_in_pbus.all = 0; - project.cds_in[1].setup_pbus.use_reg_in_pbus.bit.reg0 = 1; // use - project.cds_in[1].setup_pbus.use_reg_in_pbus.bit.reg1 = 0; // not use - project.cds_in[1].setup_pbus.use_reg_in_pbus.bit.reg2 = 0; // not use - - project.cds_in[1].status_serial_bus.max_read_error = MAX_READ_SBUS; - -#endif - -#if (USE_IN_2) - project.cds_in[2].type_plate = cds_in_type_in_2; -// IN2 - project.cds_in[2].setup_pbus.use_reg_in_pbus.all = 0; - project.cds_in[2].setup_pbus.use_reg_in_pbus.bit.reg0 = 1; // use - project.cds_in[2].setup_pbus.use_reg_in_pbus.bit.reg1 = 0; // not use - project.cds_in[2].setup_pbus.use_reg_in_pbus.bit.reg2 = 0; // not use - project.cds_in[2].status_serial_bus.max_read_error = MAX_READ_SBUS; - -#endif - -#if (USE_ROT_1) -// CDS_RS - project.cds_rs[0].setup_pbus.use_reg_in_pbus.all = 0xffff; // use all 16 -#endif - -#if (USE_ADC_0) -//ADC0 - project.adc[0].setup_pbus.use_reg_in_pbus.all = 0xffff; // use all 16 - ///////////////////////////////////////////////////////////////////////////// - // SERIAL_BUS Timing setup - ///////////////////////////////////////////////////////////////////////////// - project.adc[0].status_serial_bus.max_read_error = 2;//MAX_READ_SBUS; - project.adc[0].status_serial_bus.max_write_error = 2;//MAX_READ_SBUS; -#endif - -#if (USE_ADC_1) -//ADC1 - project.adc[1].setup_pbus.use_reg_in_pbus.all = 0xffff; // use all 16 - ///////////////////////////////////////////////////////////////////////////// - // SERIAL_BUS Timing setup - ///////////////////////////////////////////////////////////////////////////// - project.adc[1].status_serial_bus.max_read_error = 2;//MAX_READ_SBUS; - project.adc[1].status_serial_bus.max_write_error = 2;//MAX_READ_SBUS; - -#endif - -#if (USE_ADC_2) -//ADC1 - project.adc[1].setup_pbus.use_reg_in_pbus.all = 0xffff; // use all 16 - ///////////////////////////////////////////////////////////////////////////// - // SERIAL_BUS Timing setup - ///////////////////////////////////////////////////////////////////////////// - - project.adc[1].status_serial_bus.max_read_error = 2;//MAX_READ_SBUS; - project.adc[1].status_serial_bus.max_write_error = 2;//MAX_READ_SBUS; - -#endif - - - -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// - -#if (USE_TK_0) - tkak_init_plate(0, TKAK0_OFF_PROTECT, TK_DISABLE_OUTPUT_A1, TK_DISABLE_OUTPUT_B1); -#endif -#if (USE_TK_1) - tkak_init_plate(1, TKAK1_OFF_PROTECT, TK_DISABLE_OUTPUT_C1, TK_DISABLE_OUTPUT_A2); -#endif -#if (USE_TK_2) - tkak_init_plate(2, TKAK2_OFF_PROTECT, TK_DISABLE_OUTPUT_B2, TK_DISABLE_OUTPUT_C2); -#endif - -#if (USE_TK_3) - tkak_init_plate(3, TKAK3_OFF_PROTECT, 0, 0); -#endif - -/* -#if (USE_TK_0) -//tkak 0 - project.cds_tk[0].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup - project.cds_tk[0].setup_pbus.use_reg_in_pbus.all = 0x0; // PBUS all off - -#if (TK_DISABLE_OUTPUT_A1==1 && TK_DISABLE_OUTPUT_B1==1) - project.cds_tk[0].write.sbus.mask_tk_out_40pin.all = 0x0000; //mask key 1-use key,0-not use key - project.cds_tk[0].write.sbus.mask_protect_tk.all = 0xff00; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_A1==0 && TK_DISABLE_OUTPUT_B1==1) - project.cds_tk[0].write.sbus.mask_tk_out_40pin.all = 0x000f; //mask key 1-use key,0-not use key - project.cds_tk[0].write.sbus.mask_protect_tk.all = 0xff0f; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_A1==1 && TK_DISABLE_OUTPUT_B1==0) - project.cds_tk[0].write.sbus.mask_tk_out_40pin.all = 0x00f0; //mask key 1-use key,0-not use key - project.cds_tk[0].write.sbus.mask_protect_tk.all = 0xfff0; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_A1==0 && TK_DISABLE_OUTPUT_B1==0) - project.cds_tk[0].write.sbus.mask_tk_out_40pin.all = 0x00ff; //mask key 1-use key,0-not use key - project.cds_tk[0].write.sbus.mask_protect_tk.all = 0xffff; // cur+ack 1-on protect. -#endif -#endif -#endif -#endif - -#if (TKAK0_OFF_PROTECT==1) - project.cds_tk[0].write.sbus.mask_protect_tk.all = 0x0000; // cur+ack 1-on protect. -#endif - - - project.cds_tk[0].write.sbus.ack_time.bit.time = (int)(TK_ACKN_TIME_MKS / 0.02); - project.cds_tk[0].write.sbus.dead_min_time.bit.mintime = (int)(TK_MIN_TIME_MKS / DIV_TIME_TK); - project.cds_tk[0].write.sbus.dead_min_time.bit.deadtime = (int)(TK_DEAD_TIME_MKS / DIV_TIME_TK); - - - -#if (TKAK0_OFF_PROTECT==1) - project.cds_tk[0].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_tk[0].write.sbus.protect_error.bit.enable_err_switch = 0; - project.cds_tk[0].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_tk[0].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_tk[0].write.sbus.protect_error.bit.disable_err_mintime = 0; - project.cds_tk[0].write.sbus.protect_error.bit.enable_line_err = 0; -#else - - - project.cds_tk[0].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_tk[0].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_tk[0].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_tk[0].write.sbus.protect_error.bit.disable_err_mintime = 1; - project.cds_tk[0].write.sbus.protect_error.bit.enable_line_err = 1; - - // ! ! - if (project.cds_tk[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_tk[0].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_tk[0].write.sbus.protect_error.bit.enable_err_switch = 1; - - -#endif - - -#endif - - -#if (USE_TK_1) -////////////////////////////////////////////////////////////// -// tkak1 - project.cds_tk[1].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup - project.cds_tk[1].setup_pbus.use_reg_in_pbus.all = 0x0; // PBUS all off - - -#if (TK_DISABLE_OUTPUT_C1==1 && TK_DISABLE_OUTPUT_A2==1) - project.cds_tk[1].write.sbus.mask_tk_out_40pin.all = 0x0000; //mask key 1-use key,0-not use key - project.cds_tk[1].write.sbus.mask_protect_tk.all = 0xff00; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_C1==0 && TK_DISABLE_OUTPUT_A2==1) - project.cds_tk[1].write.sbus.mask_tk_out_40pin.all = 0x000f; //mask key 1-use key,0-not use key - project.cds_tk[1].write.sbus.mask_protect_tk.all = 0xff0f; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_C1==1 && TK_DISABLE_OUTPUT_A2==0) - project.cds_tk[1].write.sbus.mask_tk_out_40pin.all = 0x00f0; //mask key 1-use key,0-not use key - project.cds_tk[1].write.sbus.mask_protect_tk.all = 0xfff0; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_C1==0 && TK_DISABLE_OUTPUT_A2==0) - project.cds_tk[1].write.sbus.mask_tk_out_40pin.all = 0x00ff; //mask key 1-use key,0-not use key - project.cds_tk[1].write.sbus.mask_protect_tk.all = 0xffff; // cur+ack 1-on protect. -#endif -#endif -#endif -#endif - - -#if (TKAK1_OFF_PROTECT==1) - project.cds_tk[1].write.sbus.mask_protect_tk.all = 0x0000; // cur+ack 1-on protect. -#endif - - project.cds_tk[1].write.sbus.ack_time.bit.time = (int)(TK_ACKN_TIME_MKS / 0.02); - project.cds_tk[1].write.sbus.dead_min_time.bit.mintime = (int)(TK_MIN_TIME_MKS / DIV_TIME_TK); - project.cds_tk[1].write.sbus.dead_min_time.bit.deadtime = (int)(TK_DEAD_TIME_MKS / DIV_TIME_TK); - -#if (TKAK1_OFF_PROTECT==1) - project.cds_tk[1].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_tk[1].write.sbus.protect_error.bit.enable_err_switch = 0; - project.cds_tk[1].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_tk[1].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_tk[1].write.sbus.protect_error.bit.disable_err_mintime = 0; - project.cds_tk[1].write.sbus.protect_error.bit.enable_line_err = 0; -#else - - project.cds_tk[1].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_tk[1].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_tk[1].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_tk[1].write.sbus.protect_error.bit.disable_err_mintime = 1; - project.cds_tk[1].write.sbus.protect_error.bit.enable_line_err = 1; - - // ! ! - if (project.cds_tk[1].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_tk[1].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_tk[1].write.sbus.protect_error.bit.enable_err_switch = 1; - -#endif - -#endif - - -#if (USE_TK_2) -////////////////////////////////////////////////////////////// -// tkak2 - project.cds_tk[2].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup - project.cds_tk[2].setup_pbus.use_reg_in_pbus.all = 0x0; // PBUS all off - -#if (TK_DISABLE_OUTPUT_B2==1 && TK_DISABLE_OUTPUT_C2==1) - project.cds_tk[2].write.sbus.mask_tk_out_40pin.all = 0x0000; //mask key 1-use key,0-not use key - project.cds_tk[2].write.sbus.mask_protect_tk.all = 0xff00; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_B2==0 && TK_DISABLE_OUTPUT_C2==1) - project.cds_tk[2].write.sbus.mask_tk_out_40pin.all = 0x000f; //mask key 1-use key,0-not use key - project.cds_tk[2].write.sbus.mask_protect_tk.all = 0xff0f; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_B2==1 && TK_DISABLE_OUTPUT_C2==0) - project.cds_tk[2].write.sbus.mask_tk_out_40pin.all = 0x00f0; //mask key 1-use key,0-not use key - project.cds_tk[2].write.sbus.mask_protect_tk.all = 0xfff0; // cur+ack 1-on protect. -#else -#if (TK_DISABLE_OUTPUT_B2==0 && TK_DISABLE_OUTPUT_C2==0) - project.cds_tk[2].write.sbus.mask_tk_out_40pin.all = 0x00ff; //mask key 1-use key,0-not use key - project.cds_tk[2].write.sbus.mask_protect_tk.all = 0xffff; // cur+ack 1-on protect. -#endif -#endif -#endif -#endif - -#if (TKAK1_OFF_PROTECT==1) - project.cds_tk[2].write.sbus.mask_protect_tk.all = 0x0000; // cur+ack 1-on protect. -#endif - - - project.cds_tk[2].write.sbus.ack_time.bit.time = (int)(TK_ACKN_TIME_MKS / 0.02); - project.cds_tk[2].write.sbus.dead_min_time.bit.mintime = (int)(TK_MIN_TIME_MKS / DIV_TIME_TK); - project.cds_tk[2].write.sbus.dead_min_time.bit.deadtime = (int)(TK_DEAD_TIME_MKS / DIV_TIME_TK); - -#if (TKAK2_OFF_PROTECT==1) - - project.cds_tk[2].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_tk[2].write.sbus.protect_error.bit.enable_err_switch = 0; - project.cds_tk[2].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_tk[2].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_tk[2].write.sbus.protect_error.bit.disable_err_mintime = 0; - project.cds_tk[2].write.sbus.protect_error.bit.enable_line_err = 0; - -#else - - project.cds_tk[2].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_tk[2].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_tk[2].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_tk[2].write.sbus.protect_error.bit.disable_err_mintime = 1; - project.cds_tk[2].write.sbus.protect_error.bit.enable_line_err = 1; - - // ! ! - if (project.cds_tk[2].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_tk[2].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_tk[2].write.sbus.protect_error.bit.enable_err_switch = 1; - -#endif - -#endif - - - -#if (USE_TK_3) -////////////////////////////////////////////////////////////// - -// - project.cds_tk[3].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup -// project.cds_tk[3].setup_pbus.use_reg_in_pbus.all = 0x0; // PBUS all off - -#if (TKAK3_OFF_PROTECT==1) - project.cds_tk[3].write.sbus.mask_protect_tk.all = 0x0000; // only break ack+cur -#else - project.cds_tk[3].write.sbus.mask_protect_tk.all = 0x0303; // only break ack+cur -#endif - project.cds_tk[3].write.sbus.mask_tk_out_40pin.all = 0x00cf; // optical bus+break - - project.cds_tk[3].write.sbus.ack_time.bit.time = (int)(TK_ACKN_TIME_MKS / 0.02); - project.cds_tk[3].write.sbus.dead_min_time.bit.mintime = (int)(TK_MIN_TIME_MKS / DIV_TIME_TK); - project.cds_tk[3].write.sbus.dead_min_time.bit.deadtime = (int)(TK_DEAD_TIME_MKS / DIV_TIME_TK); - - -#if (TKAK3_OFF_PROTECT==1) - - project.cds_tk[3].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_tk[3].write.sbus.protect_error.bit.enable_err_switch = 0; - project.cds_tk[3].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_tk[3].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_tk[3].write.sbus.protect_error.bit.disable_err_mintime = 0; - project.cds_tk[3].write.sbus.protect_error.bit.enable_line_err = 0; - -#else - - project.cds_tk[3].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_tk[3].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_tk[3].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_tk[3].write.sbus.protect_error.bit.disable_err_mintime = 1; - project.cds_tk[3].write.sbus.protect_error.bit.enable_line_err = 1; - - // ! ! - if (project.cds_tk[3].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_tk[3].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_tk[3].write.sbus.protect_error.bit.enable_err_switch = 1; - - -#endif - -#endif -*/ -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// - -// Out plane setup - - -#if (USE_OUT_0) -////////////////////////////////////////////////////////////// -// out0 - - project.cds_out[0].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup - - - -#if (OUT0_OFF_PROTECT==1) - - project.cds_out[0].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_out[0].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_out[0].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_out[0].write.sbus.protect_error.bit.enable_err_switch = 0; - -#else - - project.cds_out[0].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_out[0].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_out[0].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_out[0].write.sbus.protect_error.bit.enable_err_switch = 1; - // ! ! - if (project.cds_out[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_out[0].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_out[0].write.sbus.protect_error.bit.enable_err_switch = 1; - -#endif - - project.cds_out[0].write.sbus.enable_protect_out.all = 0x0000; - - -// OUT ERR - project.cds_out[0].write.sbus.enable_protect_out.bit.dout0 = 1; // . - project.cds_out[0].write.sbus.enable_protect_out.bit.dout6 = 1; // 6 - QTV -// project.cds_out[0].write.sbus.enable_protect_out.bit.dout7 = 1; // QTV OFF - project.cds_out[0].write.sbus.enable_protect_out.bit.dout8 = 1; // 8 - QTV - project.cds_out[0].write.sbus.enable_protect_out.bit.dout13 = 1; // 13 - - -#endif - -#if (USE_OUT_1) - -// out1 - project.cds_out[1].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup - -#if (OUT1_OFF_PROTECT==1) - - project.cds_out[1].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_out[1].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_out[1].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_out[1].write.sbus.protect_error.bit.enable_err_switch = 0; - -#else - project.cds_out[1].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_out[1].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_out[1].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_out[1].write.sbus.protect_error.bit.enable_err_switch = 1; - - - project.cds_out[1].write.sbus.protect_error.bit.enable_err_power = 1; - // ! ! - if (project.cds_out[1].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_out[1].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_out[1].write.sbus.protect_error.bit.enable_err_switch = 1; -#endif - - project.cds_out[1].write.sbus.enable_protect_out.all = 0x0000; - -#endif - -#if (USE_OUT_2) - -//out2 - project.cds_out[2].status_serial_bus.max_read_error = MAX_READ_SBUS;// SERIAL_BUS Timing setup - - - project.cds_out[2].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_out[2].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_out[2].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_out[2].write.sbus.protect_error.bit.enable_err_switch = 1; - - - project.cds_out[2].write.sbus.protect_error.bit.enable_err_power = 1; - - // ! ! - if (project.cds_out[2].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_out[2].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_out[2].write.sbus.protect_error.bit.enable_err_switch = 1; - - project.cds_out[2].write.sbus.enable_protect_out.all = 0x0000; - -#endif - -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -///////////// - -#if (USE_IN_0) - -///////////////////////////////////////////////////////////////////// -// setup incremental sensor rotor -///////////////////////////////////////////////////////////////////// -// -// 1 - 20ns 0 - 2us. - project.cds_in[0].write.sbus.enabled_channels.bit.discret = 0; - - -// In0- , In1-, In2-90. -// , 67 , -// In1-, In2-90. - - project.cds_in[0].write.sbus.enabled_channels.bit.sens_1_direct_ch = 1; //1 - project.cds_in[0].write.sbus.enabled_channels.bit.sens_1_direct_ch_90deg = 1; //0 - project.cds_in[0].write.sbus.enabled_channels.bit.sens_1_inv_ch = 0; - project.cds_in[0].write.sbus.enabled_channels.bit.sens_1_inv_ch_90deg = 0; - - project.cds_in[0].write.sbus.enabled_channels.bit.sens_2_direct_ch = 1; - project.cds_in[0].write.sbus.enabled_channels.bit.sens_2_direct_ch_90deg = 1; - project.cds_in[0].write.sbus.enabled_channels.bit.sens_2_inv_ch = 0; - project.cds_in[0].write.sbus.enabled_channels.bit.sens_2_inv_ch_90deg = 0; - -// In1-, In2-90. -// 0xf - project.cds_in[0].write.sbus.first_sensor.bit.direct_ch = 0x0; // in2 - project.cds_in[0].write.sbus.first_sensor.bit.direct_ch_90deg = 0x1; // in1 -// - project.cds_in[0].write.sbus.first_sensor.bit.inv_ch = 0x0f; // in0 - project.cds_in[0].write.sbus.first_sensor.bit.inv_ch_90deg = 0x0f; // in0 - -// , In2-, In1-90. - project.cds_in[0].write.sbus.second_sensor.bit.direct_ch = 0x01; // in0 - project.cds_in[0].write.sbus.second_sensor.bit.direct_ch_90deg = 0x00; // in1 -// - project.cds_in[0].write.sbus.second_sensor.bit.inv_ch = 0x0f; // in0 - project.cds_in[0].write.sbus.second_sensor.bit.inv_ch_90deg = 0x0f; // in0 -// - project.cds_in[0].write.sbus.zero_sensors.bit.for_sensor1 = 0x02; // - project.cds_in[0].write.sbus.zero_sensors.bit.for_sensor2 = 0x0f; // -// - project.cds_in[0].write.sbus.zero_sensors.bit.enable_sensor1 = 1; - project.cds_in[0].write.sbus.zero_sensors.bit.enable_sensor2 = 0; - -///////////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////////// - -// In plane setup -//in0 - -#if (IN0_OFF_PROTECT==1) - - project.cds_in[0].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_in[0].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_in[0].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_in[0].write.sbus.protect_error.bit.enable_err_switch = 0; - -#else - - project.cds_in[0].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_in[0].write.sbus.protect_error.bit.disable_err_hwp = 1; - - // ! ! - if (project.cds_in[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - { - project.cds_in[0].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_in[0].write.sbus.protect_error.bit.enable_err_switch = 0; - } - else - { - project.cds_in[0].write.sbus.protect_error.bit.enable_err_power = 1; - project.cds_in[0].write.sbus.protect_error.bit.enable_err_switch = 1; - } - -#endif - - -#endif - - -#if (USE_IN_1) - -// in1 - -#if (IN1_OFF_PROTECT==1) - - project.cds_in[1].write.sbus.protect_error.bit.disable_err0_in = 0; - project.cds_in[1].write.sbus.protect_error.bit.disable_err_hwp = 0; - project.cds_in[1].write.sbus.protect_error.bit.enable_err_power = 0; - project.cds_in[1].write.sbus.protect_error.bit.enable_err_switch = 0; - -#else - - - project.cds_in[1].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_in[1].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_in[1].write.sbus.protect_error.bit.enable_err_power = 1; - - // ! ! - if (project.cds_in[1].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_in[1].write.sbus.protect_error.bit.enable_err_switch = 0; - else - project.cds_in[1].write.sbus.protect_error.bit.enable_err_switch = 1; - -#endif - -#endif - - - -#if (USE_IN_2) - -// in2 - project.cds_in[2].write.sbus.protect_error.bit.disable_err0_in = 1; - project.cds_in[2].write.sbus.protect_error.bit.disable_err_hwp = 1; - project.cds_in[2].write.sbus.protect_error.bit.enable_err_power = 1; - - // ! ! - if (project.cds_in[2].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - project.cds_in[2].write.sbus.protect_error.bit.enable_err_switch = 0; - - else - project.cds_in[2].write.sbus.protect_error.bit.enable_err_switch = 1; - - -#endif - -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -#if (USE_HWP_0) - - // HWP setup -// project.hwp[0].write.HWP_Speed = MODE_HWP_SPEED_AUTO;//MODE_HWP_SPEED_NORMAL;// MODE_HWP_SPEED_AUTO; // MODE_HWP_SPEED_SLOW;//MODE_HWP_SPEED_NORMAL;// MODE_HWP_SPEED_SLOW; - - project.hwp[0].write.test_all_channel = 1; - - project.hwp[0].write.use_channel.minus.all = 0xfffc; - project.hwp[0].write.use_channel.plus.all = 0xffff; - -/* - project.hwp[0].write.use_channel.plus.bit.ch0 = 1; - - project.hwp[0].write.use_channel.minus.bit.ch5 = 1; - project.hwp[0].write.use_channel.plus.bit.ch5 = 1; - - project.hwp[0].write.use_channel.minus.bit.ch11 = 1; - project.hwp[0].write.use_channel.plus.bit.ch11 = 1; - -*/ - - project.hwp[0].write.values[0].plus = convert_real_to_mv_hwp(0,LEVEL_HWP_U_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[0].minus = convert_real_to_mv_hwp(0,LEVEL_HWP_U_ZPT); - - project.hwp[0].write.values[1].plus = convert_real_to_mv_hwp(1,LEVEL_HWP_U_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[1].minus = convert_real_to_mv_hwp(1,LEVEL_HWP_U_ZPT); - - project.hwp[0].write.values[2].plus = convert_real_to_mv_hwp(2,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[2].minus = convert_real_to_mv_hwp(2,LEVEL_HWP_I_AF); - project.hwp[0].write.values[3].plus = convert_real_to_mv_hwp(3,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[3].minus = convert_real_to_mv_hwp(3,LEVEL_HWP_I_AF); - project.hwp[0].write.values[4].plus = convert_real_to_mv_hwp(4,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[4].minus = convert_real_to_mv_hwp(4,LEVEL_HWP_I_AF); - project.hwp[0].write.values[5].plus = convert_real_to_mv_hwp(5,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[5].minus = convert_real_to_mv_hwp(5,LEVEL_HWP_I_AF); - project.hwp[0].write.values[6].plus = convert_real_to_mv_hwp(6,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[6].minus = convert_real_to_mv_hwp(6,LEVEL_HWP_I_AF); - project.hwp[0].write.values[7].plus = convert_real_to_mv_hwp(7,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[7].minus = convert_real_to_mv_hwp(7,LEVEL_HWP_I_AF); - - project.hwp[0].write.values[8].plus = convert_real_to_mv_hwp(8,LEVEL_HWP_I_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[8].minus = convert_real_to_mv_hwp(8,LEVEL_HWP_I_ZPT); - project.hwp[0].write.values[9].plus = convert_real_to_mv_hwp(9,LEVEL_HWP_I_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[9].minus = convert_real_to_mv_hwp(9,LEVEL_HWP_I_ZPT); - - project.hwp[0].write.use_channel.minus.bit.ch9 = 0; - project.hwp[0].write.use_channel.plus.bit.ch9 = 0; - - project.hwp[0].write.values[10].plus = convert_real_to_mv_hwp(10,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[10].minus = convert_real_to_mv_hwp(10,LEVEL_HWP_U_ABC); - project.hwp[0].write.values[11].plus = convert_real_to_mv_hwp(11,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[11].minus = convert_real_to_mv_hwp(11,LEVEL_HWP_U_ABC); - project.hwp[0].write.values[12].plus = convert_real_to_mv_hwp(12,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[12].minus = convert_real_to_mv_hwp(12,LEVEL_HWP_U_ABC); - project.hwp[0].write.values[13].plus = convert_real_to_mv_hwp(13,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[13].minus = convert_real_to_mv_hwp(13,LEVEL_HWP_U_ABC); - - - project.hwp[0].write.values[14].plus = convert_real_to_mv_hwp(14,LEVEL_HWP_I_BREAK); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[14].minus = convert_real_to_mv_hwp(14,LEVEL_HWP_I_BREAK); - project.hwp[0].write.values[15].plus = convert_real_to_mv_hwp(15,LEVEL_HWP_I_BREAK); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[0].write.values[15].minus = convert_real_to_mv_hwp(15,LEVEL_HWP_I_BREAK); -#endif -////////////////////////////////////////////////////////////// -#if (USE_HWP_1) - - // HWP setup - // project.hwp[1].write.HWP_Speed = MODE_HWP_SPEED_AUTO;//MODE_HWP_SPEED_NORMAL;// MODE_HWP_SPEED_SLOW; - project.hwp[1].write.test_all_channel = 1; - - project.hwp[1].write.use_channel.minus.all = 0xfffc; - project.hwp[1].write.use_channel.plus.all = 0xffff; - - - project.hwp[1].write.values[0].plus = convert_real_to_mv_hwp(0,LEVEL_HWP_U_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[0].minus = convert_real_to_mv_hwp(0,LEVEL_HWP_U_ZPT); - - project.hwp[1].write.values[1].plus = convert_real_to_mv_hwp(1,LEVEL_HWP_U_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[1].minus = convert_real_to_mv_hwp(1,LEVEL_HWP_U_ZPT); - - project.hwp[1].write.values[2].plus = convert_real_to_mv_hwp(2,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[2].minus = convert_real_to_mv_hwp(2,LEVEL_HWP_I_AF); - project.hwp[1].write.values[3].plus = convert_real_to_mv_hwp(3,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[3].minus = convert_real_to_mv_hwp(3,LEVEL_HWP_I_AF); - project.hwp[1].write.values[4].plus = convert_real_to_mv_hwp(4,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[4].minus = convert_real_to_mv_hwp(4,LEVEL_HWP_I_AF); - project.hwp[1].write.values[5].plus = convert_real_to_mv_hwp(5,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[5].minus = convert_real_to_mv_hwp(5,LEVEL_HWP_I_AF); - project.hwp[1].write.values[6].plus = convert_real_to_mv_hwp(6,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[6].minus = convert_real_to_mv_hwp(6,LEVEL_HWP_I_AF); - project.hwp[1].write.values[7].plus = convert_real_to_mv_hwp(7,LEVEL_HWP_I_AF); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[7].minus = convert_real_to_mv_hwp(7,LEVEL_HWP_I_AF); - - project.hwp[1].write.values[8].plus = convert_real_to_mv_hwp(8,LEVEL_HWP_I_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[8].minus = convert_real_to_mv_hwp(8,LEVEL_HWP_I_ZPT); - project.hwp[1].write.values[9].plus = convert_real_to_mv_hwp(9,LEVEL_HWP_I_ZPT); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[9].minus = convert_real_to_mv_hwp(9,LEVEL_HWP_I_ZPT); - - - project.hwp[1].write.values[10].plus = convert_real_to_mv_hwp(10,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[10].minus = convert_real_to_mv_hwp(10,LEVEL_HWP_U_ABC); - project.hwp[1].write.values[11].plus = convert_real_to_mv_hwp(11,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[11].minus = convert_real_to_mv_hwp(11,LEVEL_HWP_U_ABC); - project.hwp[1].write.values[12].plus = convert_real_to_mv_hwp(12,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[12].minus = convert_real_to_mv_hwp(12,LEVEL_HWP_U_ABC); - project.hwp[1].write.values[13].plus = convert_real_to_mv_hwp(13,LEVEL_HWP_U_ABC); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[13].minus = convert_real_to_mv_hwp(13,LEVEL_HWP_U_ABC); - - - project.hwp[1].write.values[14].plus = convert_real_to_mv_hwp(14,LEVEL_HWP_I_BREAK); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[14].minus = convert_real_to_mv_hwp(14,LEVEL_HWP_I_BREAK); - project.hwp[1].write.values[15].plus = convert_real_to_mv_hwp(15,LEVEL_HWP_I_BREAK); //Uzpt1 //2 3000V - 845; 3977V - 1120; 2800V - 789; 2600V - 732 - project.hwp[1].write.values[15].minus = convert_real_to_mv_hwp(15,LEVEL_HWP_I_BREAK); - #endif - - -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// - //Incremental sensor_init -#if (ENABLE_ROTOR_SENSOR_1_PM67==1) - inc_sensor.use_sensor1 = 1; -#else - inc_sensor.use_sensor1 = 0; -#endif - -#if (ENABLE_ROTOR_SENSOR_2_PM67==1) - inc_sensor.use_sensor2 = 1; -#else - inc_sensor.use_sensor2 = 0; -#endif - - -#if (ENABLE_COMBO_SENSOR_1_TO_2==1) - inc_sensor.use_sensor2 = 1; -#endif -#if (ENABLE_COMBO_SENSOR_2_TO_1==1) - inc_sensor.use_sensor1 = 1; -#endif - - - inc_sensor.pm67regs.write_comand_reg.bit.set_sampling_time = SAMPLING_TIME_MS; -// 23550 -// inc_sensor.pm67regs.write_comand_reg.bit.filter_sensitivity = 5; //0x0; // 170 . -// 22220 - inc_sensor.pm67regs.write_comand_reg.bit.filter_sensitivity = 300;//5; //0x0; // 170 . - - - - inc_sensor.set(&inc_sensor); - - //Rotation sensor_init -/* - rotation_sensor.in_plane.cds_in = &project.cds_in[0]; - rotation_sensor.use_sensor1 = 1; - rotation_sensor.use_sensor2 = 1; - rotation_sensor.use_angle_plane = 0; - - - - - rotation_sensor.in_plane.write.sbus.enabled_channels.bit.sens_1_direct_ch =1; - rotation_sensor.in_plane.write.sbus.enabled_channels.bit.sens_2_direct_ch_90deg =1; - -// rotation_sensor.in_plane.write.sbus.enabled_channels.bit.sens_1_direct_ch_90deg = 1; - -// rotation_sensor.in_plane.write.sbus.enabled_channels.bit.sens_2_direct_ch = 1; -// rotation_sensor.in_plane.write.sbus.enabled_channels.bit.sens_2_direct_ch_90deg = 1; - - rotation_sensor.in_plane.write.sbus.first_sensor_inputs.bit.direct_ch = 0; -// rotation_sensor.in_plane.write.sbus.first_sensor_inputs.bit.direct_ch_90deg = 1; - -// rotation_sensor.in_plane.write.sbus.second_sensor_inputs.bit.direct_ch = 3; - rotation_sensor.in_plane.write.sbus.second_sensor_inputs.bit.direct_ch_90deg = 1; - - - - - - - rotation_sensor.in_plane.write.regs.comand_reg.bit.set_sampling_time = SAMPLING_TIME_MS; - rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0xF; // 170 . - -*/ - - - - - //filter 1 ~ 20nsec - /* rotation_sensor.rotation_plane.cds_rs = &project.cds_rs[0]; - rotation_sensor.rotation_plane.write.sbus.config.all = 0; - rotation_sensor.rotation_plane.write.sbus.config.bit.channel1_enable = 1; - rotation_sensor.rotation_plane.write.sbus.config.bit.plane_is_master = 1; - rotation_sensor.rotation_plane.write.sbus.config.bit.survey_time = 49; - rotation_sensor.rotation_plane.write.sbus.config.bit.transmition_speed = TS250; - */ -// rotation_sensor.set(&rotation_sensor); - - if (project.controller.status != component_Ready) - return; - -// project.load_cfg_to_plates(); - -} - - - diff --git a/Inu/Src2/551/main/project.h b/Inu/Src2/551/main/project.h deleted file mode 100644 index 32dba9a..0000000 --- a/Inu/Src2/551/main/project.h +++ /dev/null @@ -1,74 +0,0 @@ -#ifndef PROJECT_H -#define PROJECT_H - - -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_GlobalPrototypes.h" -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" -#include "DSP281x_Ev.h" // DSP281x Examples Include File -#include "MemoryFunctions.h" -#include "Spartan2E_Adr.h" -#include "Spartan2E_Functions.h" -#include "TuneUpPlane.h" -#include "x_parallel_bus.h" -#include "x_serial_bus.h" -#include "xerror.h" -#include "xp_adc.h" -#include "xp_cds_in.h" -#include "xp_cds_out.h" -#include "xp_cds_tk.h" -#include "xp_controller.h" -#include "xp_hwp.h" -#include "xp_project.h" -#include "xPeriphSP6_loader.h" -//#include "xp_omega.h" -//#include "xp_dispatcher.h" - -//#include "x_error_buffer.h" - -//#include "x_project_plane.h" - -/* Define this macros to add to project files, contaning code */ -/* communicating with test terminal by RS232 */ -#define USE_TEST_TERMINAL 1 - -/* Define this macros to add to project files, contaning code */ -/* communicating with SVO and MPU by RS, using modbus protokol */ -#define USE_MODBUS_TABLE_SVU 1 - -/* Define this macros to add to project files, contaning code */ -/* communicating with Ingeteam Pult by RS, using modbus protocol */ -#define USE_MODBUS_TABLE_PULT 1 - - -// control_station.c control_station.h -#define USE_CONTROL_STATION 1 - - - - - -///////////////////////////////////////////////////////////////// -#ifndef USE_CONTROL_STATION -#define USE_CONTROL_STATION 0 -#endif - -#ifndef USE_TEST_TERMINAL -#define USE_TEST_TERMINAL 0 -#endif - -#ifndef USE_MODBUS_TABLE_SVU -#define USE_MODBUS_TABLE_SVU 0 -#endif - -#ifndef USE_MODBUS_TABLE_PULT -#define USE_MODBUS_TABLE_PULT 0 -#endif -//////////////////////////////////////////////////////////////// -// . HWP -//////////////////////////////////////////////////////////////// -void project_prepare_config(void); - - -#endif // end PROJECT_H diff --git a/Inu/Src2/551/main/project_setup.h b/Inu/Src2/551/main/project_setup.h deleted file mode 100644 index 4f3ec93..0000000 --- a/Inu/Src2/551/main/project_setup.h +++ /dev/null @@ -1,414 +0,0 @@ -#ifndef PROJECT_SETUP_H -#define PROJECT_SETUP_H - -/* - - myXilinx - C_PROJECT_TYPE - - project_setup.h /main/ - .. /myXilinx/ - .. ! -*/ - -/*------------------------------------------------------------------------------ - Project type -------------------------------------------------------------------------------*/ -// -///////////////////////////////////////////////////////////////////////////// -#define PROJECT_22220 10 -#define PROJECT_21300 11 -#define PROJECT_21180 12 -#define PROJECT_BALZAM 13 -#define PROJECT_23470 14 -#define PROJECT_23550 15 -#define PROJECT_10510 16 - - -#define PROJECT_STEND_D PROJECT_BALZAM - -/////////////////////////////////////////////// -// -/////////////////////////////////////////////// - -//#define C_PROJECT_TYPE PROJECT_21180 -//#define C_PROJECT_TYPE PROJECT_21300 -//#define C_PROJECT_TYPE PROJECT_22220 -//#define C_PROJECT_TYPE PROJECT_BALZAM -//#define C_PROJECT_TYPE PROJECT_23470 -//#define C_PROJECT_TYPE PROJECT_STEND_D - -#define C_PROJECT_TYPE PROJECT_23550 -//#define C_PROJECT_TYPE PROJECT_10510 - -// RS232 -#define RS232_SPEED_A 57600//115200//57600 -#define RS232_SPEED_B 57600//115200//57600// - -/////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////// -// -/////////////////////////////////////////////////////////////////////////////////// - -#if (C_PROJECT_TYPE==PROJECT_23550 || C_PROJECT_TYPE==PROJECT_BALZAM) -#define XPWMGEN 1 // xilinx 24 -#define TMSPWMGEN 0 // tms -#else -#define XPWMGEN 0 // xilinx 24 -#define TMSPWMGEN 1 // tms -#endif - - -/////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////// - -#if (TMSPWMGEN==1) && (XPWMGEN==1) - #error " . !!!" - -#endif - - -#if (TMSPWMGEN==1) - -#else //TMSPWMGEN - -#if (XPWMGEN==1) -#else - - #error " . !!!" - -#endif //XPWMGEN -#endif //TMSPWMGEN - -//////////////////////////////////////////////// -//////////////////////////////////////////////// -///////////////////////////////////////////////// -// -//////////////////////////////////////////////// -///////////////////////////////////////////////// -#if (_FLOOR6==0) -#define USE_ADC_0 1 -#define USE_ADC_1 1 -////#define USE_ADC_2 1 -// -#define USE_IN_0 1 - -#define USE_IN_1 1 -//////#define USE_IN_2 1 -//// -#define USE_OUT_0 1 -//////#define USE_OUT_1 1 -//////#define USE_OUT_2 1 -//// -//// -#define USE_TK_0 1 -#define USE_TK_1 1 -//// -#define USE_TK_2 1 -#define USE_TK_3 1 -//// -//////#define USE_TK_4 1 -//////#define USE_TK_5 1 -//////#define USE_TK_6 1 -//////#define USE_TK_7 1 -//// -#define USE_HWP_0 1 -////#define USE_HWP_1 1 -//////#define USE_HWP_2 1 -//// -//////#define USE_ROT_1 1 - -#else - -#if (_FLOOR6_ADD==1) - -#define USE_ADC_0 1 -#define USE_ADC_1 1 -////// -#define USE_IN_0 1 -//// -//#define USE_IN_1 1 -//////// -//#define USE_OUT_0 1 -//////// -//////// -#define USE_TK_0 1 -#define USE_TK_1 1 -////// -#define USE_TK_2 1 -#define USE_TK_3 1 - -//#define USE_HWP_0 1 - - -#else - -#define USE_ADC_0 1 -#define USE_ADC_1 1 -//// -#define USE_IN_0 1 -// -#define USE_IN_1 1 -////// -#define USE_OUT_0 1 -////// -////// -#define USE_TK_0 1 -#define USE_TK_1 1 -//// -#define USE_TK_2 1 -#define USE_TK_3 1 - -#define USE_HWP_0 1 -#define USE_HWP_1 1 - -#endif - -#endif - - -//////////////////////////////////////////////// -// SP2 SP6 -//////////////////////////////////////////////// -//////////////////////////////////////////////// -//////////////////////////////////////////////// - -#define TYPE_CDS_XILINX_IN_0 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_IN_1 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_IN_2 TYPE_CDS_XILINX_SP2 - -#define TYPE_CDS_XILINX_OUT_0 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_OUT_1 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_OUT_2 TYPE_CDS_XILINX_SP2 - -#define TYPE_CDS_XILINX_TK_0 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_TK_1 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_TK_2 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_TK_3 TYPE_CDS_XILINX_SP6 -#define TYPE_CDS_XILINX_TK_4 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_TK_5 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_TK_6 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_TK_7 TYPE_CDS_XILINX_SP2 - -#define TYPE_CDS_XILINX_ADC_0 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_ADC_1 TYPE_CDS_XILINX_SP2 -#define TYPE_CDS_XILINX_ADC_2 TYPE_CDS_XILINX_SP2 - -#define TYPE_CDS_XILINX_RS_0 TYPE_CDS_XILINX_SP2 - -//////////////////////////////////////////////// -//////////////////////////////////////////////// -//////////////////////////////////////////////// - -//#define TYPE_CDS_XILINX_HWP_0_1 MODE_HWP_SPEED_SLOW - -#ifndef USE_ADC_0 -#define USE_ADC_0 0 -#endif - -#ifndef USE_ADC_1 -#define USE_ADC_1 0 -#endif - -#ifndef USE_ADC_2 -#define USE_ADC_2 0 -#endif - - -#ifndef USE_IN_0 -#define USE_IN_0 0 -#endif - -#ifndef USE_IN_1 -#define USE_IN_1 0 -#endif - -#ifndef USE_IN_2 -#define USE_IN_2 0 -#endif - - -#ifndef USE_OUT_0 -#define USE_OUT_0 0 -#endif - -#ifndef USE_OUT_1 -#define USE_OUT_1 0 -#endif - -#ifndef USE_OUT_2 -#define USE_OUT_2 0 -#endif - - - -#ifndef USE_TK_0 -#define USE_TK_0 0 -#endif - -#ifndef USE_TK_1 -#define USE_TK_1 0 -#endif - - -// -#ifndef USE_TK_2 -#define USE_TK_2 0 -#endif - -#ifndef USE_TK_3 -#define USE_TK_3 0 -#endif - - -#ifndef USE_TK_4 -#define USE_TK_4 0 -#endif - -#ifndef USE_TK_5 -#define USE_TK_5 0 -#endif - -#ifndef USE_TK_6 -#define USE_TK_6 0 -#endif - -#ifndef USE_TK_7 -#define USE_TK_7 0 -#endif - - -#ifndef USE_HWP_0 -#define USE_HWP_0 0 -#endif - -#ifndef USE_HWP_1 -#define USE_HWP_1 0 -#endif - -#ifndef USE_HWP_2 -#define USE_HWP_2 0 -#endif - - -#ifndef USE_ROT_1 -#define USE_ROT_1 0 -#endif - - - -//////////////////////////////////////////////// -// -//////////////////////////////////////////////// -#if (USE_HWP_2==1) -#define MAX_COUNT_PLATES_HWP 3 -#else -#if (USE_HWP_1==1) -#define MAX_COUNT_PLATES_HWP 2 -#else -#define MAX_COUNT_PLATES_HWP 1 -#endif -#endif -//////////////////////////////////////////////// - -#if (USE_TK_7==1) -#define MAX_COUNT_PLATES_CDS_TK 8 -#else -#if (USE_TK_6==1) -#define MAX_COUNT_PLATES_CDS_TK 7 -#else -#if (USE_TK_5==1) -#define MAX_COUNT_PLATES_CDS_TK 6 -#else -#if (USE_TK_4==1) -#define MAX_COUNT_PLATES_CDS_TK 5 -#else -#if (USE_TK_3==1) -#define MAX_COUNT_PLATES_CDS_TK 4 -#else -#if (USE_TK_2==1) -#define MAX_COUNT_PLATES_CDS_TK 3 -#else -#if (USE_TK_1==1) -#define MAX_COUNT_PLATES_CDS_TK 2 -#else -#if (USE_TK_0==1) -#define MAX_COUNT_PLATES_CDS_TK 1 -#else -#define MAX_COUNT_PLATES_CDS_TK 1 - -#endif -#endif -#endif -#endif -#endif -#endif -#endif -#endif -//////////////////////////////////////////////// - -#if (USE_ADC_2==1) -#define MAX_COUNT_PLATES_ADC 3 -#else -#if (USE_ADC_1==1) -#define MAX_COUNT_PLATES_ADC 2 -#else -#if (USE_ADC_0==1) -#define MAX_COUNT_PLATES_ADC 1 -#else -#define MAX_COUNT_PLATES_ADC 1 -#endif -#endif -#endif - - -//////////////////////////////////////////////// -#if (USE_OUT_2==1) -#define MAX_COUNT_PLATES_OUT 3 -#else -#if (USE_OUT_1==1) -#define MAX_COUNT_PLATES_OUT 2 -#else -#if (USE_OUT_0==1) -#define MAX_COUNT_PLATES_OUT 1 -#else -#define MAX_COUNT_PLATES_OUT 1 -#endif -#endif -#endif - - -//////////////////////////////////////////////// -//////////////////////////////////////////////// -#if (USE_IN_2==1) -#define MAX_COUNT_PLATES_IN 3 -#else -#if (USE_IN_1==1) -#define MAX_COUNT_PLATES_IN 2 -#else -#if (USE_IN_0==1) -#define MAX_COUNT_PLATES_IN 1 -#else -#define MAX_COUNT_PLATES_IN 1 -#endif -#endif -#endif - - -//////////////////////////////////////////////// - - -#if (USE_ROT_1==1) -#define MAX_COUNT_PLATES_CDS_RS 1 -#else -#define MAX_COUNT_PLATES_CDS_RS 0 -#endif - -//////////////////////////////////////////////// - - -#include - -#endif // end PROJECT_SETUP_H - diff --git a/Inu/Src2/551/main/protect_levels.h b/Inu/Src2/551/main/protect_levels.h deleted file mode 100644 index b9bf78f..0000000 --- a/Inu/Src2/551/main/protect_levels.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * params_protect.h - * - * Created on: 17 . 2020 . - * Author: star - */ - -#ifndef SRC_MAIN_PROTECT_LEVELS_H_ -#define SRC_MAIN_PROTECT_LEVELS_H_ - -#include - -typedef struct { - - int alarm_temper_u_01; - int alarm_temper_u_02; - int alarm_temper_u_03; - int alarm_temper_u_04; - int alarm_temper_u_05; - int alarm_temper_u_06; - int alarm_temper_u_07; - - int abnormal_temper_u_01; - int abnormal_temper_u_02; - int abnormal_temper_u_03; - int abnormal_temper_u_04; - int abnormal_temper_u_05; - int abnormal_temper_u_06; - int abnormal_temper_u_07; - - int alarm_temper_water_int; - int abnormal_temper_water_int; - - int alarm_temper_water_ext; - int abnormal_temper_water_ext; - - int alarm_p_water_max_int; - int abnormal_p_water_max_int; - - int alarm_p_water_min_int; - int abnormal_p_water_min_int; - - int alarm_temper_air_int_01; - int alarm_temper_air_int_02; - int alarm_temper_air_int_03; - int alarm_temper_air_int_04; - - int abnormal_temper_air_int_01; - int abnormal_temper_air_int_02; - int abnormal_temper_air_int_03; - int abnormal_temper_air_int_04; - - int alarm_temper_acdrive_winding_U1; - int alarm_temper_acdrive_winding_V1; - int alarm_temper_acdrive_winding_W1; - int alarm_temper_acdrive_winding_U2; - int alarm_temper_acdrive_winding_V2; - int alarm_temper_acdrive_winding_W2; - - int abnormal_temper_acdrive_winding_U1; - int abnormal_temper_acdrive_winding_V1; - int abnormal_temper_acdrive_winding_W1; - int abnormal_temper_acdrive_winding_U2; - int abnormal_temper_acdrive_winding_V2; - int abnormal_temper_acdrive_winding_W2; - - int alarm_temper_acdrive_bear_DNE; - int alarm_temper_acdrive_bear_NE; - int abnormal_temper_acdrive_bear_DNE; - int abnormal_temper_acdrive_bear_NE; - - int alarm_Uin_max_Up; - int alarm_Uin_max_Down; - int alarm_Uin_min_Up; - int alarm_Uin_min_Down; - - int alarm_Udc_max_Up; - int alarm_Udc_max_Down; - int alarm_Udc_min_Up; - int alarm_Udc_min_Down; - int alarm_Izpt_max; - - int alarm_Imax_U01; - int alarm_Imax_U02; - int alarm_Imax_U03; - int alarm_Imax_U04; - int alarm_Imax_U05; - int alarm_Imax_U06; - int alarm_Imax_U07; - - int alarm_Iged_max; - -} PROTECT_LEVELS; - -#define PROTECT_LEVELS_DEFAULTS {ALARM_TEMPER_AF,ALARM_TEMPER_AF,ALARM_TEMPER_AF,\ - ALARM_TEMPER_AF,ALARM_TEMPER_AF,ALARM_TEMPER_AF,ALARM_TEMPER_AF,\ - ABNORMAL_TEMPER_AF,ABNORMAL_TEMPER_AF,ABNORMAL_TEMPER_AF,ABNORMAL_TEMPER_AF,\ - ABNORMAL_TEMPER_AF,ABNORMAL_TEMPER_AF,ABNORMAL_TEMPER_AF,\ - ALARM_TEMPER_WATER_INT,ABNORMAL_TEMPER_WATER_INT,\ - ALARM_TEMPER_WATER_EXT,ABNORMAL_TEMPER_WATER_EXT,\ - ALARM_P_WATER_MAX_INT,ABNORMAL_P_WATER_MAX_INT,\ - ALARM_P_WATER_MIN_INT,ABNORMAL_P_WATER_MIN_INT,\ - ALARM_TEMPER_AIR_INT,ALARM_TEMPER_AIR_INT,ALARM_TEMPER_AIR_INT,ALARM_TEMPER_AIR_INT,\ - ABNORMAL_TEMPER_AIR_INT,ABNORMAL_TEMPER_AIR_INT,ABNORMAL_TEMPER_AIR_INT,ABNORMAL_TEMPER_AIR_INT,\ - ALARM_TEMPER_ACDRIVE_WINDING,ALARM_TEMPER_ACDRIVE_WINDING,ALARM_TEMPER_ACDRIVE_WINDING,\ - ALARM_TEMPER_ACDRIVE_WINDING,ALARM_TEMPER_ACDRIVE_WINDING,ALARM_TEMPER_ACDRIVE_WINDING,\ - ABNORMAL_TEMPER_ACDRIVE_WINDING,ABNORMAL_TEMPER_ACDRIVE_WINDING,ABNORMAL_TEMPER_ACDRIVE_WINDING,\ - ABNORMAL_TEMPER_ACDRIVE_WINDING,ABNORMAL_TEMPER_ACDRIVE_WINDING,ABNORMAL_TEMPER_ACDRIVE_WINDING,\ - ALARM_TEMPER_ACDRIVE_BEAR,ALARM_TEMPER_ACDRIVE_BEAR,\ - ABNORMAL_TEMPER_ACDRIVE_BEAR,ABNORMAL_TEMPER_ACDRIVE_BEAR,\ - LEVEL_ADC_U_IN_MAX,LEVEL_ADC_U_IN_MAX, LEVEL_ADC_U_IN_MIN,LEVEL_ADC_U_IN_MIN,\ - LEVEL_ADC_U_ZPT_MAX,LEVEL_ADC_U_ZPT_MAX,LEVEL_ADC_U_ZPT_MIN,LEVEL_ADC_U_ZPT_MIN,\ - LEVEL_ADC_I_ZPT,\ - LEVEL_ADC_I_BREAK, LEVEL_ADC_I_AF,LEVEL_ADC_I_AF,LEVEL_ADC_I_AF,LEVEL_ADC_I_AF,LEVEL_ADC_I_AF,LEVEL_ADC_I_AF,\ - LEVEL_ADC_I_OUT_MAX} - -extern PROTECT_LEVELS protect_levels; - -#endif /* SRC_MAIN_PROTECT_LEVELS_H_ */ diff --git a/Inu/Src2/551/main/pump_control.c b/Inu/Src2/551/main/pump_control.c deleted file mode 100644 index 212473c..0000000 --- a/Inu/Src2/551/main/pump_control.c +++ /dev/null @@ -1,255 +0,0 @@ -/* - * pump_management.c - * - * Created on: 28 . 2020 . - * Author: stud - */ - -#include -#include - -#include "control_station.h" -#include "digital_filters.h" -#include "sbor_shema.h" - -#pragma DATA_SECTION(pumps, ".slow_vars") -PUMP_CONTROL pumps = PUMP_CONTROL_DEFAULTS; - -void turn_on_nasos_1_2(unsigned int without_time_wait); -int select_pump(void); - -void pump_control(void) -{ - unsigned int pump_off_without_time_wait; - - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MODE_PUMP] == 0) - { -// edrk.SelectPump1_2 = 0; - pumps.SelectedPump1_2 = 0; - edrk.ManualStartPump = 0; - // edrk.AutoStartPump = 1; - pump_off_without_time_wait = 0; - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MODE_PUMP] == 1) - { -// edrk.SelectPump1_2 = 1; - pumps.SelectedPump1_2 = 1; - edrk.ManualStartPump = 0; - // edrk.AutoStartPump = 1; - pump_off_without_time_wait = 0; - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MODE_PUMP] == 2) - { -// edrk.SelectPump1_2 = 2; - pumps.SelectedPump1_2 = 2; - edrk.ManualStartPump = 0; - // edrk.AutoStartPump = 1; - pump_off_without_time_wait = 0; - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MODE_PUMP] == 3) - { -// edrk.SelectPump1_2 = 1; - pumps.SelectedPump1_2 = 1; - edrk.ManualStartPump = 1; - edrk.AutoStartPump = 0; - pump_off_without_time_wait = 1; - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MODE_PUMP] == 4) - { -// edrk.SelectPump1_2 = 2; - pumps.SelectedPump1_2 = 2; - edrk.ManualStartPump = 1; - edrk.AutoStartPump = 0; - pump_off_without_time_wait = 1; - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MODE_PUMP] == 5) - { -// edrk.SelectPump1_2 = 0; - pumps.SelectedPump1_2 = 0; - edrk.ManualStartPump = 0; - edrk.AutoStartPump = 0; - pump_off_without_time_wait = 1; - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MODE_PUMP] == 6) - { -// edrk.SelectPump1_2 = 1; - pumps.SelectedPump1_2 = 0; - edrk.ManualStartPump = 1; - edrk.AutoStartPump = 0; - pump_off_without_time_wait = 1; - } - - edrk.SumStartPump = edrk.AutoStartPump || edrk.ManualStartPump; - - turn_on_nasos_1_2(pump_off_without_time_wait); - - set_status_pump_fan(); - -} - - -#define TIME_WAIT_PUMP_ON 30 //3 sec -#define TIME_WAIT_ALL_PUMP_OFF 600 //30 sec - -/////////////////////////////////////////////// -void turn_on_nasos_1_2(unsigned int without_time_wait) -{ -static unsigned int time_wait_on_pump1=0; -static unsigned int time_wait_on_pump2=0; - -static unsigned int time_wait_off_all_pump1=0; -static unsigned int time_wait_off_all_pump2=0; - -static unsigned int prev_without_time_wait = 0; - - - -int cmd_p1 = 0, - cmd_p2 = 0, - add_cmd_without_time_wait1 = 0, - add_cmd_without_time_wait2 = 0; - -int fast_stop_pump=0; - - -// if () -//(prev_start_nasos!=edrk.StartPump) && - - - cmd_p1 = 0; - cmd_p2 = 0; - - fast_stop_pump = ( edrk.errors.e5.bits.ERROR_ISOLATE || edrk.errors.e5.bits.FAN || - edrk.errors.e5.bits.PRE_READY_PUMP || edrk.errors.e5.bits.UTE4KA_WATER || edrk.errors.e2.bits.P_WATER_INT_MIN || - edrk.errors.e2.bits.P_WATER_INT_MAX || edrk.errors.e5.bits.PUMP_1 || edrk.errors.e5.bits.PUMP_2); - - if ( edrk.SumStartPump==1 && edrk.errors.e5.bits.PRE_READY_PUMP == 0 && fast_stop_pump==0) - { - if (pumps.SelectedPump1_2 == 0 /*&& edrk.ManualStartPump == 0*/) - { - // - switch (select_pump()) { - case 1: - cmd_p1 = 1; - cmd_p2 = 0; - break; - case 2: - cmd_p1 = 0; - cmd_p2 = 1; - break; - default: - cmd_p1 = 0; - cmd_p2 = 0; - } - pumps.lock_pump = 1; - } else - if (pumps.SelectedPump1_2==1 && edrk.errors.e5.bits.PUMP_1==0) - cmd_p1 = 1; - else if (pumps.SelectedPump1_2==2 && edrk.errors.e5.bits.PUMP_2==0) - cmd_p2 = 1; - } - else - { - cmd_p1 = 0; - cmd_p2 = 0; - } - - - if (cmd_p1) - { - edrk.SelectPump1_2 = 1; - if (pause_detect_error(&time_wait_on_pump1,TIME_WAIT_PUMP_ON,1)) - { - edrk.to_ing.bits.NASOS_1_ON = 1; - time_wait_off_all_pump1 = 0; - } - } - else - { - time_wait_on_pump1 = 0; - - if (without_time_wait==0 && prev_without_time_wait != without_time_wait) - add_cmd_without_time_wait1 = 1; - - if (cmd_p1!=cmd_p2 || fast_stop_pump || without_time_wait || add_cmd_without_time_wait1) { - edrk.to_ing.bits.NASOS_1_ON = 0; - } - else - { - if (pause_detect_error(&time_wait_off_all_pump1,TIME_WAIT_ALL_PUMP_OFF,1)) { - edrk.to_ing.bits.NASOS_1_ON = 0; - } - } - - } - - - if (cmd_p2) - { - edrk.SelectPump1_2 = 2; - if (pause_detect_error(&time_wait_on_pump2,TIME_WAIT_PUMP_ON,1)) - { - edrk.to_ing.bits.NASOS_2_ON = 1; - time_wait_off_all_pump2 = 0; - } - } - else - { - time_wait_on_pump2 = 0; - - if (without_time_wait==0 && prev_without_time_wait != without_time_wait) - add_cmd_without_time_wait2 = 1; - - if (cmd_p1!=cmd_p2 || fast_stop_pump || without_time_wait || add_cmd_without_time_wait2) { - edrk.to_ing.bits.NASOS_2_ON = 0; - } - else - { - if (pause_detect_error(&time_wait_off_all_pump2,TIME_WAIT_ALL_PUMP_OFF,1)) { - edrk.to_ing.bits.NASOS_2_ON = 0; - } - } - } - - if (edrk.to_ing.bits.NASOS_1_ON == 0 && edrk.to_ing.bits.NASOS_2_ON == 0) { - pumps.lock_pump = 0; - } - - prev_without_time_wait = without_time_wait; - -} - -int select_pump() { - int p_n = 0; - if (pumps.lock_pump == 0) { - - p_n = calc_auto_moto_pump(); - if (p_n == 0) - p_n = 1; -// if (pumps.pump1_engine_minutes > pumps.pump2_engine_minutes) { -// p_n = 2; -// } else { -// p_n = 1; -// } - - } else { - p_n = edrk.SelectPump1_2; - } - if ((edrk.errors.e5.bits.PUMP_1 == 1) && (edrk.errors.e5.bits.PUMP_2 == 1)) { - p_n = 0; - } - if (p_n == 1 && edrk.warnings.e5.bits.PUMP_1 == 1) { - p_n = 2; - } - if (p_n == 2 && edrk.warnings.e5.bits.PUMP_2 == 1) { - p_n = 1; - } - return p_n; -} diff --git a/Inu/Src2/551/main/pump_control.h b/Inu/Src2/551/main/pump_control.h deleted file mode 100644 index 92a5931..0000000 --- a/Inu/Src2/551/main/pump_control.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - * pump_management.h - * - * Created on: 28 . 2020 . - * Author: stud - */ - -#ifndef SRC_MAIN_PUMP_CONTROL_H_ -#define SRC_MAIN_PUMP_CONTROL_H_ - -typedef struct { - int pump1_engine_minutes; - int pump2_engine_minutes; - int time_switch_minuts; - int SelectedPump1_2; - int lock_pump; -} PUMP_CONTROL; - -#define PUMP_CONTROL_DEFAULTS {0,0,5,0,0} - -extern PUMP_CONTROL pumps; - -void pump_control(void); - - - -#endif /* SRC_MAIN_PUMP_CONTROL_H_ */ diff --git a/Inu/Src2/551/main/pwm_logs.c b/Inu/Src2/551/main/pwm_logs.c deleted file mode 100644 index 42ca3e7..0000000 --- a/Inu/Src2/551/main/pwm_logs.c +++ /dev/null @@ -1,476 +0,0 @@ -/* - * pwm_logs.c - * - * Created on: 19 . 2024 . - * Author: user - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "global_time.h" -#include "IQmathLib.h" -#include "oscil_can.h" -#include "uf_alg_ing.h" -#include "MemoryFunctions.h" -#include "RS_Functions.h" -#include "v_rotor_22220.h" -#include "log_to_memory.h" -#include "log_params.h" -#include "logs_hmi.h" -#include "vector_control.h" - - -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -#pragma CODE_SECTION(prepare_data_to_logs,".fast_run"); -unsigned int prepare_data_to_logs(void) -{ - logsdata.logs[0] = (int16) global_time.total_seconds10full;// = 0;//(int16)(_IQtoIQ15(Izad)); - logsdata.logs[1] = (int16) _IQtoIQ15(analog.iqIu_1); - logsdata.logs[2] = (int16) _IQtoIQ15(analog.iqIv_1); - logsdata.logs[3] = (int16) _IQtoIQ15(analog.iqIw_1); - - logsdata.logs[4] = (int16) _IQtoIQ15(analog.iqIu_2); - logsdata.logs[5] = (int16) _IQtoIQ15(analog.iqIv_2); - logsdata.logs[6] = (int16) _IQtoIQ15(analog.iqIw_2); - - logsdata.logs[7] = (int16) _IQtoIQ15(analog.iqUin_A1B1);// (int16) _IQtoIQ15(analog.iqUin_m1); - logsdata.logs[8] = (int16) _IQtoIQ15(analog.iqUin_A2B2);// (int16) _IQtoIQ15(analog.iqUin_m2); - - - logsdata.logs[9] = (int16) _IQtoIQ15(analog.iqU_1); - logsdata.logs[10] = (int16) _IQtoIQ15(analog.iqU_2);//11 - - logsdata.logs[11] = (int16) _IQtoIQ15(analog.iqIin_1); - logsdata.logs[12] = (int16) _IQtoIQ15(analog.iqIin_2); - - logsdata.logs[13] = (int16) _IQtoIQ15(analog.iqIm_1); - logsdata.logs[14] = (int16) _IQtoIQ15(analog.iqIm_2); - - logsdata.logs[15] = (int16) _IQtoIQ15(analog.iqIbreak_1);//(int16) _IQtoIQ15(filter.iqU_1_long); - logsdata.logs[16] = (int16) _IQtoIQ15(analog.iqIbreak_2);//(int16) _IQtoIQ15(filter.iqU_2_long); - - - logsdata.logs[17] = (int16)svgen_pwm24_1.Ta_0; - logsdata.logs[18] = (int16)svgen_pwm24_1.Ta_1; - logsdata.logs[19] = (int16)break_result_1; - logsdata.logs[20] = (int16)break_result_2; -// logpar.log21 = (int16)svgen_pwm24_1.Tb_1; -// logpar.log22 = (int16)svgen_pwm24_1.Tc_0; -// logpar.log23 = (int16)svgen_pwm24_1.Tc_1; //23 - - logsdata.logs[21] = (int16)(_IQtoIQ14(edrk.iq_power_kw_full_znak));// edrk.from_uom.iq_level_value_kwt - //(int16) _IQtoIQ15(rotor_22220.iqFdirty); - // - - logsdata.logs[22] = (int16) _IQtoIQ15(WRotor.iqWRotorSumFilter2); - logsdata.logs[23] = (int16) _IQtoIQ15(WRotor.iqWRotorSumFilter);//WRotor.iqWRotorSumFilter WRotor.iqWRotorSum - logsdata.logs[24] = (int16) _IQtoIQ15(simple_scalar1.iq_decr_mzz_power);//WRotor.iqWRotorSumFilter2 WRotor.iqWRotorSumFilter3 - - logsdata.logs[25] = (int16) edrk.from_uom.level_value;// (int16) _IQtoIQ15(WRotor.iqWRotorSumFilter); -// logpar.log25 = (int16) _IQtoIQ15(WRotor.iqWRotorSumFilter); -// logsdata.logs[25] = (int16) _IQtoIQ15(WRotor.iqWRotorSum); - - logsdata.logs[26] = (int16) edrk.power_limit.all;//_IQtoIQ15(pll1.vars.pll_Uq); - logsdata.logs[27] = (int16)(_IQtoIQ14(edrk.zadanie.iq_limit_power_zad));//(int16) _IQtoIQ15(pll1.vars.pll_Ud);//28 - - logsdata.logs[28] = (int16) _IQtoIQ12(edrk.master_theta);//29 - logsdata.logs[29] = (int16) _IQtoIQ15(edrk.master_Uzad);//30 - logsdata.logs[30] = (int16) _IQtoIQ14(edrk.f_stator); - logsdata.logs[31] = (int16) _IQtoIQ12(edrk.k_stator1); - - logsdata.logs[32] = optical_read_data.data.cmd.all; - logsdata.logs[33] = optical_read_data.data.pzad_or_wzad; - logsdata.logs[34] = optical_read_data.data.angle_pwm; - logsdata.logs[35] = optical_read_data.data.iq_zad_i_zad; - - logsdata.logs[36] = optical_write_data.data.cmd.all; - logsdata.logs[37] = optical_write_data.data.angle_pwm; - logsdata.logs[38] = optical_write_data.data.pzad_or_wzad; - logsdata.logs[39] = optical_write_data.data.iq_zad_i_zad; - - ///////////// - logsdata.logs[40] = (int16)(_IQtoIQ15(simple_scalar1.Izad)); - logsdata.logs[41] = (int16)(_IQtoIQ15(simple_scalar1.mzz_zad_in2));//(int16)(_IQtoIQ15(Uze_t1)); - logsdata.logs[42] = (int16)(_IQtoIQ15(simple_scalar1.pidF.OutMax)); - logsdata.logs[43] = (int16)(_IQtoIQ15(simple_scalar1.mzz_zad_int)); - logsdata.logs[44] = (int16)(_IQtoIQ15(simple_scalar1.Izad_from_master)); - - logsdata.logs[45] = (int16)(_IQtoIQ14(simple_scalar1.pidF.Fdb)); - logsdata.logs[46] = (int16)(_IQtoIQ14(simple_scalar1.pidF.Ref)); - logsdata.logs[47] = (int16)(_IQtoIQ14(simple_scalar1.pidF.Ui)); - logsdata.logs[48] = (int16)(_IQtoIQ14(simple_scalar1.pidF.Up)); - logsdata.logs[49] = (int16)(_IQtoIQ14(simple_scalar1.pidF.SatErr)); - logsdata.logs[50] = (int16)(_IQtoIQ14(simple_scalar1.pidF.Out)); - - logsdata.logs[51] = (int16)(_IQtoIQ14(simple_scalar1.pidPower.Fdb)); - logsdata.logs[52] = (int16)(_IQtoIQ14(simple_scalar1.pidPower.Ref)); - logsdata.logs[53] = (int16)(_IQtoIQ14(simple_scalar1.pidPower.Ui)); - logsdata.logs[54] = (int16)(_IQtoIQ14(simple_scalar1.pidPower.Up)); - logsdata.logs[55] = (int16)(_IQtoIQ14(simple_scalar1.pidPower.SatErr)); - logsdata.logs[56] = (int16)(_IQtoIQ14(simple_scalar1.pidPower.Out)); - - logsdata.logs[57] = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Fdb)); - logsdata.logs[58] = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Ref)); - logsdata.logs[59] = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Ui)); - logsdata.logs[60] = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Up)); - logsdata.logs[61] = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.SatErr)); - logsdata.logs[62] = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Out)); - logsdata.logs[63] = (int16)(_IQtoIQ12(simple_scalar1.Uze_t1)); - - logsdata.logs[64] = (int16)(_IQtoIQ15(simple_scalar1.bpsi_curent)); - - logsdata.logs[65] = (int16)(_IQtoIQ14(simple_scalar1.iqKoefOgran)); - - logsdata.logs[66] = (int16)(_IQtoIQ14(simple_scalar1.Fz)); - logsdata.logs[67] = (int16) (_IQtoIQ15(simple_scalar1.iq_decr_mzz_power_filter));//rotor_22220.direct_rotor - - logsdata.logs[68] = (int16)(_IQtoIQ14(simple_scalar1.pidF.OutMin));//(int16)edrk.Status_Sbor; - logsdata.logs[69] = (int16)(_IQtoIQ14(simple_scalar1.pidPower.OutMax));//(int16)edrk.Stage_Sbor; - logsdata.logs[70] = (int16)(_IQtoIQ14(filter.iqUin_m1));//(int16)edrk.Sbor_Mode; - - logsdata.logs[71] = (int16)edrk.from_shema.all; - logsdata.logs[72] = (int16)(_IQtoIQ15(simple_scalar1.iqKoefOgranIzad));//(int16)edrk.from_shema_filter.all; -// - logsdata.logs[73] = (int16)(_IQtoIQ14(filter.iqUin_m2));//simple_scalar1.direction; - logsdata.logs[74] = (int16)(_IQtoIQ14(edrk.iq_power_kw_full_filter_znak));// - logsdata.logs[75] = (int16)(_IQtoIQ15(edrk.iq_freq_50hz));//edrk.iq_freq_50hz - logsdata.logs[76] = (int16)(_IQtoIQ15(simple_scalar1.mzz_zad_in1)); - - - - // can regs -// logsdata.logs[77] = (int16)(((unsigned long)edrk.canes_reg >> 16) & 0x1ff); -// logsdata.logs[78] = (int16)((unsigned long)edrk.canes_reg & 0x3f); -// -// logsdata.logs[79] = (int16)(edrk.cantec_reg & 0xff); -// logsdata.logs[80] = (int16)(edrk.canrec_reg & 0xff); - - logsdata.logs[77] = (int16)(_IQtoIQ14(uf_alg.Ud)); - logsdata.logs[78] = (int16)(_IQtoIQ14(uf_alg.Uq)); - logsdata.logs[79] = (int16)(_IQtoIQ14(edrk.all_limit_koeffs.uom_limit));//edrk.MasterSlave; - - logsdata.logs[80] = (int16)(_IQtoIQ14(edrk.Kplus)); - -// logsdata.logs[65] = (int16)(_IQtoIQ14(edrk.all_limit_koeffs.temper_limit)); - logsdata.logs[81] = (int16)(_IQtoIQ14(edrk.all_limit_koeffs.uin_freq_limit)); - - logsdata.logs[82] = (int16)(_IQtoIQ14(uf_alg.svgen_dq_Ta)); - logsdata.logs[83] = (int16)(_IQtoIQ14(uf_alg.svgen_dq_Tb)); - logsdata.logs[84] = (int16)(_IQtoIQ14(uf_alg.svgen_dq_Tc)); - - // uf_alg.tetta_bs - - // logsdata.logs[64] = (int16)(_IQtoIQ15()); - -// logsdata.logs[64] = (int16)(_IQtoIQ15()); - - logsdata.logs[85] = edrk.from_uom.digital_line.all; -// logsdata.logs[75] = edrk.errors.e2.all; -// logsdata.logs[76] = edrk.errors.e3.all; -// logsdata.logs[77] = edrk.errors.e4.all; -// logsdata.logs[78] = edrk.errors.e5.all; -// logsdata.logs[79] = edrk.errors.e6.all; -// logsdata.logs[80] = edrk.errors.e7.all; -// - - - - -// logsdata.logs[67] = (int16) _IQtoIQ15(WRotor.iqWRotorCalcBeforeRegul1); -// logsdata.logs[68] = (int16) _IQtoIQ15(WRotor.iqWRotorCalcBeforeRegul2); -// logsdata.logs[69] = (int16) _IQtoIQ15(WRotor.iqWRotorCalc1); - -// logsdata.logs[24] = (int16) _IQtoIQ15(WRotor.iqWRotorCalc2); - -// logsdata.logs[70] = (int16) _IQtoIQ15(WRotor.iqWRotorSumRamp); - -// logsdata.logs[72] = (int16) (WRotorPBus.RotorDirectionSlow); - // logsdata.logs[73] = (int16) (WRotorPBus.RotorDirectionSlow2); - // logsdata.logs[74] = (int16) (WRotorPBus.RotorDirectionInstant); //(int16) (WRotorPBus.); - -// logsdata.logs[75] = (int16) (WRotor.iqTimeSensor1); -// logsdata.logs[76] = (int16) (WRotor.iqTimeSensor2); -// -// logsdata.logs[77] = (int16) _IQtoIQ15(WRotor.iqWRotorCalc1Ramp); -// logsdata.logs[78] = (int16) _IQtoIQ15(WRotor.iqWRotorCalc2Ramp); -// -// logsdata.logs[79] = (int16) _IQtoIQ15(WRotor.iqPrevWRotorCalc1); -// logsdata.logs[80] = (int16) _IQtoIQ15(WRotor.iqPrevWRotorCalc2); - -// logsdata.logs[81] = (int16) modbus_table_can_in[124].all;// (); -// logsdata.logs[82] = (int16) modbus_table_can_in[134].all;// -// logsdata.logs[83] = (int16) modbus_table_can_in[125].all;// () -// -// logsdata.logs[84] = (int16) project.cds_in[0].read.pbus.data_in.all; -// logsdata.logs[85] = (int16) project.cds_in[1].read.pbus.data_in.all; - - logsdata.logs[86] = (int16) _IQtoIQ15(vect_control.iqId1); - logsdata.logs[87] = (int16) _IQtoIQ15(vect_control.iqIq1); - - logsdata.logs[88] = (int16) _IQtoIQ15(vect_control.iqId2); - logsdata.logs[89] = (int16) _IQtoIQ15(vect_control.iqIq2); - logsdata.logs[90] = 0; - - return 90; -} - - -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////// - -#define PAUSE_SLOW_LOG 20 - -void save_slow_logs(int run, int t_slow) -{ - static int c_step=0; -// static int c_all = (FREQ_PWM>>2); // 0,125 - static int c_all = (FREQ_PWM>>1); // 0,250 - - static int prev_run=0; - static unsigned int c_pause=0; - - if (rs_a.RS_PrevCmd==CMD_RS232_UPLOAD) - return; - - if (run==0 && c_pause>=PAUSE_SLOW_LOG) - return; - - c_all = (FREQ_PWM>>(1+t_slow)); - - if (c_all<2) - c_all = 2; - - - if (c_step>=c_all) - { - test_mem_limit(SLOW_LOG, 1); - - // prepare_data_to_logs(); - - getSlowLogs(1); - c_step = 0; - - if (run==1) - c_pause = 0; - else - { - if (c_pause= 1)//1 - { - count_step_run++; - // i_led1_on_off(1); -// write_to_mem(FAST_LOG, (int16)count_step_run); - - getFastLogs(!f.Ciclelog); -// for (i_log=0;i_log<72;i_log++) -// write_to_mem(FAST_LOG, (int16) logsdata.logs[i_log]); - -// write_to_mem(FAST_LOG, (int16) logpar.log85); - - - -// if (logpar.start_write_fast_log) { -// get_log_params_count(); -// logpar.start_write_fast_log = 0; -// } - count_step = 0; - } - } - else { - if (f.Stop && log_params.log_saved_to_const_mem == 0) { - log_params.copy_log_to_const_memory = 1; - log_params.log_saved_to_const_mem = 1; - f.flag_send_alarm_log_to_MPU = 1; - } - } - - -#if(_ENABLE_PWM_LINES_FOR_TESTS_PWM) - PWM_LINES_TK_23_OFF; -#endif - - if (filter.iqU_1_long>edrk.iqMIN_U_ZPT || filter.iqU_2_long>edrk.iqMIN_U_ZPT) - local_enable_slow_log = 1; - else - local_enable_slow_log = 0; - - if (edrk.stop_logs_rs232 == 0 && edrk.stop_slow_log ==0 && log_to_HMI.send_log == 0) - save_slow_logs(edrk.Go || (local_enable_slow_log && edrk.Status_Ready.bits.ready_final==0), edrk.t_slow_log); -} -////////////////////////////////////////////////////////////////// - - - - diff --git a/Inu/Src2/551/main/pwm_logs.h b/Inu/Src2/551/main/pwm_logs.h deleted file mode 100644 index 8eb5f07..0000000 --- a/Inu/Src2/551/main/pwm_logs.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * pwm_logs.h - * - * Created on: 19 . 2024 . - * Author: user - */ - -#ifndef SRC_MAIN_PWM_LOGS_H_ -#define SRC_MAIN_PWM_LOGS_H_ - -unsigned int prepare_data_to_logs(void); -void save_slow_logs(int run, int t_slow); -void run_write_logs(void); - - -#endif /* SRC_MAIN_PWM_LOGS_H_ */ diff --git a/Inu/Src2/551/main/pwm_test_lines.c b/Inu/Src2/551/main/pwm_test_lines.c deleted file mode 100644 index ef24f64..0000000 --- a/Inu/Src2/551/main/pwm_test_lines.c +++ /dev/null @@ -1,36 +0,0 @@ - -#include <281xEvTimersInit.h> -#include -#include - -#include "DSP281x_Device.h" -#include "MemoryFunctions.h" -#include "Spartan2E_Adr.h" -#include "x_wdog.h" - -#include - -unsigned int cmd_pwm_test_lines = 0xffff; - -void pwm_test_lines_start(void) -{ - cmd_pwm_test_lines = 0xffff; - -// i_WriteMemory(ADR_TK_MASK_1, 0); - i_WriteMemory(ADR_PWM_DIRECT2,0xffff); -// i_WriteMemory(ADR_PWM_DRIVE_MODE, 3); // on direct tk lines -// i_WriteMemory(ADR_PWM_DIRECT2,0xffff); - - i_WriteMemory(ADR_TK_MASK_1, 0x0); //Turn on additional 16 tk lines -} - -void pwm_test_lines_stop(void) -{ - cmd_pwm_test_lines = 0xffff; - - i_WriteMemory(ADR_TK_MASK_1, 0xffff); //Turn off additional 16 tk lines - i_WriteMemory(ADR_PWM_DIRECT2,0xffff); - -// i_WriteMemory(ADR_PWM_DRIVE_MODE, 0); // off direct tk lines -} - diff --git a/Inu/Src2/551/main/pwm_test_lines.h b/Inu/Src2/551/main/pwm_test_lines.h deleted file mode 100644 index d411550..0000000 --- a/Inu/Src2/551/main/pwm_test_lines.h +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - -extern unsigned int cmd_pwm_test_lines; - -#define PWM_LINES_TK_16_OFF {cmd_pwm_test_lines &= 0xfffe; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_16_ON {cmd_pwm_test_lines |= 0x0001; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} - -#define PWM_LINES_TK_17_OFF {cmd_pwm_test_lines &= 0xfffd; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_17_ON {cmd_pwm_test_lines |= 0x0002; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} - -#define PWM_LINES_TK_18_OFF {cmd_pwm_test_lines &= 0xfffb; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_18_ON {cmd_pwm_test_lines |= 0x0004; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} - -#define PWM_LINES_TK_19_OFF {cmd_pwm_test_lines &= 0xfff7; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_19_ON {cmd_pwm_test_lines |= 0x0008; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//// -#define PWM_LINES_TK_20_OFF {cmd_pwm_test_lines &= 0xffef; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_20_ON {cmd_pwm_test_lines |= 0x0010; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} - -#define PWM_LINES_TK_21_OFF {cmd_pwm_test_lines &= 0xffdf; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_21_ON {cmd_pwm_test_lines |= 0x0020; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} - -#define PWM_LINES_TK_22_OFF {cmd_pwm_test_lines &= 0xffbf; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_22_ON {cmd_pwm_test_lines |= 0x0040; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} - -#define PWM_LINES_TK_23_OFF {cmd_pwm_test_lines &= 0xff7f; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -#define PWM_LINES_TK_23_ON {cmd_pwm_test_lines |= 0x0080; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -////// -//#define PWM_LINES_TK_24_ON {cmd_pwm_test_lines &= 0xfeff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_24_OFF {cmd_pwm_test_lines |= 0x0100; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -// -//#define PWM_LINES_TK_25_ON {cmd_pwm_test_lines &= 0xfdff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_25_OFF {cmd_pwm_test_lines |= 0x0200; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -// -//#define PWM_LINES_TK_26_ON {cmd_pwm_test_lines &= 0xfbff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_26_OFF {cmd_pwm_test_lines |= 0x0400; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -// -//#define PWM_LINES_TK_27_ON {cmd_pwm_test_lines &= 0xf7ff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_27_OFF {cmd_pwm_test_lines |= 0x0800; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -////// -//#define PWM_LINES_TK_28_ON {cmd_pwm_test_lines &= 0xefff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_28_OFF {cmd_pwm_test_lines |= 0x1000; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -// -//#define PWM_LINES_TK_29_ON {cmd_pwm_test_lines &= 0xdfff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_29_OFF {cmd_pwm_test_lines |= 0x2000; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -// -//#define PWM_LINES_TK_30_ON {cmd_pwm_test_lines &= 0xbfff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_30_OFF {cmd_pwm_test_lines |= 0x4000; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -// -//#define PWM_LINES_TK_31_ON {cmd_pwm_test_lines &= 0x7fff; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//#define PWM_LINES_TK_31_OFF {cmd_pwm_test_lines |= 0x8000; i_WriteMemory(ADR_PWM_DIRECT2, cmd_pwm_test_lines);} -//// - - - - -///////////////////////////////////////// -void pwm_test_lines_start(void); -void pwm_test_lines_stop(void); diff --git a/Inu/Src2/551/main/ramp_zadanie_tools.c b/Inu/Src2/551/main/ramp_zadanie_tools.c deleted file mode 100644 index 61a6689..0000000 --- a/Inu/Src2/551/main/ramp_zadanie_tools.c +++ /dev/null @@ -1,417 +0,0 @@ -/* - * ramp_zadanie_tools.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - - - -#include - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include "ramp_zadanie_tools.h" -#include "v_rotor.h" - - - -void change_ramp_zadanie(void) -{ - _iq rampafloat, rampafloat_plus, rampafloat_minus; - - if (edrk.cmd_very_slow_start) - { - rampafloat_plus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS)); - rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS)); - edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus1 = rampafloat_plus; - edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus1 = -rampafloat_minus; - - edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus1 = rampafloat_minus; - edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus1 = -rampafloat_plus; - -// rampafloat_plus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS)); -// rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS)); - edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus2 = rampafloat_plus; - edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus2 = -rampafloat_minus; - - edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus2 = rampafloat_minus; - edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus2 = -rampafloat_plus; - } - else - { - rampafloat_plus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS)); - rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS)); - edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus1 = rampafloat_plus; - edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus1 = -rampafloat_minus; - - edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus1 = rampafloat_minus; - edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus1 = -rampafloat_plus; - - rampafloat_plus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS)); - rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS)); - edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus2 = rampafloat_plus; - edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus2 = -rampafloat_minus; - - edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus2 = rampafloat_minus; - edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus2 = -rampafloat_plus; - - } - -} - -void init_ramp_all_zadanie(void) -{ - _iq rampafloat, rampafloat_plus, rampafloat_minus; - -//rmp_oborots_imitation - edrk.zadanie.rmp_oborots_imitation.RampLowLimit = _IQ(MIN_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); - edrk.zadanie.rmp_oborots_imitation.RampHighLimit = _IQ(MAX_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); - rampafloat = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_IMITATION_OBOROTS_ROTOR)); - edrk.zadanie.rmp_oborots_imitation.RampPlus = rampafloat; - edrk.zadanie.rmp_oborots_imitation.RampMinus = -rampafloat; - - edrk.zadanie.rmp_oborots_imitation.Out = 0; - edrk.zadanie.rmp_oborots_imitation.DesiredInput = 0; - -// rmp_fzad - edrk.zadanie.rmp_fzad.RampLowLimit = _IQ(-MAX_ZADANIE_F/NORMA_FROTOR); //0 - edrk.zadanie.rmp_fzad.RampHighLimit = _IQ(MAX_ZADANIE_F/NORMA_FROTOR); - -// rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_F)); - rampafloat = _IQ((MAX_ZADANIE_F/NORMA_FROTOR)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_F)); - edrk.zadanie.rmp_fzad.RampPlus = rampafloat; - edrk.zadanie.rmp_fzad.RampMinus = -rampafloat; - - edrk.zadanie.rmp_fzad.DesiredInput = 0; - edrk.zadanie.rmp_fzad.Out = 0; - -// rmp_oborots_hz - edrk.zadanie.rmp_oborots_zad_hz.RampLowLimit = _IQ(MIN_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); //0 - edrk.zadanie.rmp_oborots_zad_hz.RampHighLimit = _IQ(MAX_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); - - edrk.zadanie.rmp_oborots_zad_hz.RampLowLimit1 = _IQ(MIN_1_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); //0 - edrk.zadanie.rmp_oborots_zad_hz.RampHighLimit1 = _IQ(MAX_1_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); - -// rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_OBOROTS_ROTOR)); -// rampafloat = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_OBOROTS_ROTOR)); -// edrk.zadanie.rmp_oborots_zad_hz.RampPlus = rampafloat; -// edrk.zadanie.rmp_oborots_zad_hz.RampMinus = -rampafloat; - - change_ramp_zadanie(); - - edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0; - edrk.zadanie.rmp_oborots_zad_hz.Out = 0; - - -// - edrk.zadanie.rmp_Izad.RampLowLimit = _IQ(0); //0 - edrk.zadanie.rmp_Izad.RampHighLimit = _IQ(MAX_ZADANIE_I_M/NORMA_ACP); - -// rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_I_M)); - rampafloat = _IQ((MAX_ZADANIE_I_M/NORMA_ACP)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_I_M)); - edrk.zadanie.rmp_Izad.RampPlus = rampafloat; - edrk.zadanie.rmp_Izad.RampMinus = -rampafloat; - - edrk.zadanie.rmp_Izad.DesiredInput = 0; - edrk.zadanie.rmp_Izad.Out = 0; - -// - edrk.zadanie.rmp_ZadanieU_Charge.RampLowLimit = _IQ(0); //0 - edrk.zadanie.rmp_ZadanieU_Charge.RampHighLimit = _IQ(MAX_ZADANIE_U_CHARGE/NORMA_ACP); - -// rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_U_CHARGE)); - rampafloat = _IQ((MAX_ZADANIE_U_CHARGE/NORMA_ACP)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_U_CHARGE)); - edrk.zadanie.rmp_ZadanieU_Charge.RampPlus = rampafloat; - edrk.zadanie.rmp_ZadanieU_Charge.RampMinus = -rampafloat; - - edrk.zadanie.rmp_ZadanieU_Charge.DesiredInput = _IQ(NOMINAL_U_ZARYAD/NORMA_ACP); - edrk.zadanie.rmp_ZadanieU_Charge.Out = _IQ(NOMINAL_U_ZARYAD/NORMA_ACP); - - - -// - edrk.zadanie.rmp_k_u_disbalance.RampLowLimit = _IQ(0); //0 - edrk.zadanie.rmp_k_u_disbalance.RampHighLimit = _IQ(MAX_ZADANIE_K_U_DISBALANCE); - -// rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_K_U_DISBALANCE)); - rampafloat = _IQ((MAX_ZADANIE_K_U_DISBALANCE)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_K_U_DISBALANCE)); - edrk.zadanie.rmp_k_u_disbalance.RampPlus = rampafloat; - edrk.zadanie.rmp_k_u_disbalance.RampMinus = -rampafloat; - - edrk.zadanie.rmp_k_u_disbalance.DesiredInput = 0; - edrk.zadanie.rmp_k_u_disbalance.Out = 0; - - -// - edrk.zadanie.rmp_kplus_u_disbalance.RampLowLimit = _IQ(0); //0 - edrk.zadanie.rmp_kplus_u_disbalance.RampHighLimit = _IQ(MAX_ZADANIE_KPLUS_U_DISBALANCE); - -// rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_KPLUS_U_DISBALANCE)); - rampafloat = _IQ((MAX_ZADANIE_KPLUS_U_DISBALANCE)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_KPLUS_U_DISBALANCE)); - edrk.zadanie.rmp_kplus_u_disbalance.RampPlus = rampafloat; - edrk.zadanie.rmp_kplus_u_disbalance.RampMinus = -rampafloat; - - edrk.zadanie.rmp_kplus_u_disbalance.DesiredInput = 0; - edrk.zadanie.rmp_kplus_u_disbalance.Out = 0; - - - -// - edrk.zadanie.rmp_kzad.RampLowLimit = _IQ(0); //0 - edrk.zadanie.rmp_kzad.RampHighLimit = _IQ(MAX_ZADANIE_K_M); - -// rampafloat = _IQ(1.0/(2.0*FREQ_PWM*T_NARAST_ZADANIE_K_M)); - rampafloat = _IQ((MAX_ZADANIE_K_M)/(2.0*FREQ_PWM*T_NARAST_ZADANIE_K_M)); - edrk.zadanie.rmp_kzad.RampPlus = rampafloat; - edrk.zadanie.rmp_kzad.RampMinus = -rampafloat; - - edrk.zadanie.rmp_kzad.DesiredInput = 0; - edrk.zadanie.rmp_kzad.Out = 0; - - -// - edrk.zadanie.rmp_powers_zad.RampLowLimit = _IQ(MIN_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); //0 - edrk.zadanie.rmp_powers_zad.RampHighLimit = _IQ(MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); - - edrk.zadanie.rmp_powers_zad.RampLowLimit1 = _IQ(MIN_1_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); - edrk.zadanie.rmp_powers_zad.RampHighLimit1 = _IQ(MAX_1_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); - -// rampafloat = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_POWER)); -// edrk.zadanie.rmp_powers_zad.RampPlus = rampafloat; -// edrk.zadanie.rmp_powers_zad.RampMinus = -rampafloat; - - -//// - rampafloat_plus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_POWER_PLUS)); - rampafloat_minus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_POWER_MINUS)); - - edrk.zadanie.rmp_powers_zad.PosRampPlus1 = rampafloat_plus; - edrk.zadanie.rmp_powers_zad.PosRampMinus1 = -rampafloat_minus; - - edrk.zadanie.rmp_powers_zad.NegRampPlus1 = rampafloat_minus; - edrk.zadanie.rmp_powers_zad.NegRampMinus1 = -rampafloat_plus; - - rampafloat_plus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_PLUS)); - rampafloat_minus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_MINUS)); - - edrk.zadanie.rmp_powers_zad.PosRampPlus2 = rampafloat_plus; - edrk.zadanie.rmp_powers_zad.PosRampMinus2 = -rampafloat_minus; - - edrk.zadanie.rmp_powers_zad.NegRampPlus2 = rampafloat_minus; - edrk.zadanie.rmp_powers_zad.NegRampMinus2 = -rampafloat_plus; - -//// - - edrk.zadanie.rmp_powers_zad.DesiredInput = 0; - edrk.zadanie.rmp_powers_zad.Out = 0; - -// - - edrk.zadanie.rmp_limit_powers_zad.RampLowLimit = 0;//_IQ(0); //0 - edrk.zadanie.rmp_limit_powers_zad.RampHighLimit = _IQ(SUPER_MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); - - edrk.zadanie.rmp_limit_powers_zad.RampLowLimit1 = 0; - edrk.zadanie.rmp_limit_powers_zad.RampHighLimit1 = _IQ(MAX_1_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); - - rampafloat_plus = _IQ((SUPER_MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_LIMIT_POWER_PLUS)); - rampafloat_minus = _IQ((SUPER_MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_LIMIT_POWER_MINUS)); - - edrk.zadanie.rmp_limit_powers_zad.PosRampPlus1 = rampafloat_plus; - edrk.zadanie.rmp_limit_powers_zad.PosRampMinus1 = -rampafloat_minus; - - edrk.zadanie.rmp_limit_powers_zad.NegRampPlus1 = rampafloat_minus; - edrk.zadanie.rmp_limit_powers_zad.NegRampMinus1 = -rampafloat_plus; - - rampafloat_plus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_PLUS)); - rampafloat_minus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_MINUS)); - - edrk.zadanie.rmp_limit_powers_zad.PosRampPlus2 = rampafloat_plus; - edrk.zadanie.rmp_limit_powers_zad.PosRampMinus2 = -rampafloat_minus; - - edrk.zadanie.rmp_limit_powers_zad.NegRampPlus2 = rampafloat_minus; - edrk.zadanie.rmp_limit_powers_zad.NegRampMinus2 = -rampafloat_plus; - -// rampafloat = _IQ((MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_LIMIT_POWER)); - -// edrk.zadanie.rmp_limit_powers_zad.RampPlus = rampafloat; -// edrk.zadanie.rmp_limit_powers_zad.RampMinus = -rampafloat; - - edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0; - edrk.zadanie.rmp_limit_powers_zad.Out = 0; - -// - - -} - -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -#pragma CODE_SECTION(ramp_all_zadanie,".fast_run"); -void load_current_ramp_oborots_power(void) -{ - int mode=0; - static int prev_mode = 0; - - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS || - edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_OBOROTS) - mode = 1; - - if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER || - edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_POWER) - mode = 2; - - if (mode==1 && prev_mode==2) - { - // , - edrk.zadanie.rmp_oborots_zad_hz.Out = WRotor.iqWRotorSumFilter3; - //edrk.zadanie.iq_oborots_zad_hz = WRotor.iqWRotorSumFilter3; - } - - - if (mode==2 && prev_mode==1) - { - // , - edrk.zadanie.rmp_powers_zad.Out = edrk.iq_power_kw_one_filter_znak;//filter.PowerScalarFilter2; - } - - prev_mode = mode; - -} -////////////////////////////////////////////////////////// -#pragma CODE_SECTION(ramp_all_zadanie,".fast_run"); -void ramp_all_zadanie(int flag_set_zero) -{ - -// if (edrk.Status_Ready.bits.ImitationReady2) - { - edrk.zadanie.rmp_oborots_imitation.DesiredInput = edrk.zadanie.iq_oborots_zad_hz; - edrk.zadanie.rmp_oborots_imitation.calc(&edrk.zadanie.rmp_oborots_imitation); - edrk.zadanie.rmp_oborots_imitation_rmp = edrk.zadanie.rmp_oborots_imitation.Out; - } - ////////////////////////////////////////////// - if (flag_set_zero==0) - edrk.zadanie.rmp_Izad.DesiredInput = edrk.zadanie.iq_Izad; - else - if (flag_set_zero==2) - { - edrk.zadanie.rmp_Izad.DesiredInput = 0; - edrk.zadanie.rmp_Izad.Out = 0; - } - else - edrk.zadanie.rmp_Izad.DesiredInput = 0; - - edrk.zadanie.rmp_Izad.calc(&edrk.zadanie.rmp_Izad); - edrk.zadanie.iq_Izad_rmp = edrk.zadanie.rmp_Izad.Out; - ////////////////////////////////////////////// - edrk.zadanie.rmp_ZadanieU_Charge.DesiredInput = edrk.zadanie.iq_ZadanieU_Charge; - edrk.zadanie.rmp_ZadanieU_Charge.calc(&edrk.zadanie.rmp_ZadanieU_Charge); - edrk.zadanie.iq_ZadanieU_Charge_rmp = edrk.zadanie.rmp_ZadanieU_Charge.Out; - ////////////////////////////////////////////// - if (flag_set_zero==0) - edrk.zadanie.rmp_fzad.DesiredInput = edrk.zadanie.iq_fzad; - else - if (flag_set_zero==2) - { - edrk.zadanie.rmp_fzad.DesiredInput = 0; - edrk.zadanie.rmp_fzad.Out = 0; - } - else - edrk.zadanie.rmp_fzad.DesiredInput = 0; - - edrk.zadanie.rmp_fzad.calc(&edrk.zadanie.rmp_fzad); - edrk.zadanie.iq_fzad_rmp = edrk.zadanie.rmp_fzad.Out; - ////////////////////////////////////////////// - edrk.zadanie.rmp_k_u_disbalance.DesiredInput = edrk.zadanie.iq_k_u_disbalance; - edrk.zadanie.rmp_k_u_disbalance.calc(&edrk.zadanie.rmp_k_u_disbalance); - edrk.zadanie.iq_k_u_disbalance_rmp = edrk.zadanie.rmp_k_u_disbalance.Out; - ////////////////////////////////////////////// - edrk.zadanie.rmp_kplus_u_disbalance.DesiredInput = edrk.zadanie.iq_kplus_u_disbalance; - edrk.zadanie.rmp_kplus_u_disbalance.calc(&edrk.zadanie.rmp_kplus_u_disbalance); - edrk.zadanie.iq_kplus_u_disbalance_rmp = edrk.zadanie.rmp_kplus_u_disbalance.Out; - ////////////////////////////////////////////// - if (flag_set_zero==0) - edrk.zadanie.rmp_kzad.DesiredInput = edrk.zadanie.iq_kzad; - else - if (flag_set_zero==2) - { - edrk.zadanie.rmp_kzad.DesiredInput = 0; - edrk.zadanie.rmp_kzad.Out = 0; - } - else - edrk.zadanie.rmp_kzad.DesiredInput = 0; - edrk.zadanie.rmp_kzad.calc(&edrk.zadanie.rmp_kzad); - edrk.zadanie.iq_kzad_rmp = edrk.zadanie.rmp_kzad.Out; - ////////////////////////////////////////////// - if (flag_set_zero==0) - edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = edrk.zadanie.iq_oborots_zad_hz; - else - if (flag_set_zero==2) - { - edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0; - edrk.zadanie.rmp_oborots_zad_hz.Out = 0; - } - else - edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0; - - edrk.zadanie.rmp_oborots_zad_hz.calc(&edrk.zadanie.rmp_oborots_zad_hz); - edrk.zadanie.iq_oborots_zad_hz_rmp = edrk.zadanie.rmp_oborots_zad_hz.Out; - - ////////////////////////////////////////////// -// if (flag_set_zero==0) -// edrk.zadanie.rmp_limit_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad; -// else -// if (flag_set_zero==2) -// { -// edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0; -// edrk.zadanie.rmp_limit_powers_zad.Out = 0; -// } -// else -// edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0; - - edrk.zadanie.rmp_limit_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad; - edrk.zadanie.rmp_limit_powers_zad.calc(&edrk.zadanie.rmp_limit_powers_zad); - edrk.zadanie.iq_limit_power_zad_rmp = edrk.zadanie.rmp_limit_powers_zad.Out; - - - - ////////////////////////////////////////////// - if (flag_set_zero==0) - { - if (edrk.zadanie.iq_power_zad>=0) - { - if (edrk.zadanie.iq_power_zad>edrk.zadanie.iq_limit_power_zad_rmp) - edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad_rmp; - else - edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_power_zad; - } - else - { - if (edrk.zadanie.iq_power_zad<-edrk.zadanie.iq_limit_power_zad_rmp) - edrk.zadanie.rmp_powers_zad.DesiredInput = -edrk.zadanie.iq_limit_power_zad_rmp; - else - edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_power_zad; - } - - } - else - if (flag_set_zero==2) - { - edrk.zadanie.rmp_powers_zad.DesiredInput = 0; - edrk.zadanie.rmp_powers_zad.Out = 0; - } - else - edrk.zadanie.rmp_powers_zad.DesiredInput = 0; - - edrk.zadanie.rmp_powers_zad.calc(&edrk.zadanie.rmp_powers_zad); - edrk.zadanie.iq_power_zad_rmp = edrk.zadanie.rmp_powers_zad.Out; - -} - -////////////////////////////////////////////////////////// diff --git a/Inu/Src2/551/main/ramp_zadanie_tools.h b/Inu/Src2/551/main/ramp_zadanie_tools.h deleted file mode 100644 index c90acb6..0000000 --- a/Inu/Src2/551/main/ramp_zadanie_tools.h +++ /dev/null @@ -1,20 +0,0 @@ -/* - * ramp_zadanie_tools.h - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#ifndef SRC_MAIN_RAMP_ZADANIE_TOOLS_H_ -#define SRC_MAIN_RAMP_ZADANIE_TOOLS_H_ - -#define FREQ_RUN_RAMP FREQ_PWM // (2.0*FREQ_PWM) - - -void ramp_all_zadanie(int flag_set_zero); -void change_ramp_zadanie(void); -void init_ramp_all_zadanie(void); -void load_current_ramp_oborots_power(void); - - -#endif /* SRC_MAIN_RAMP_ZADANIE_TOOLS_H_ */ diff --git a/Inu/Src2/551/main/sbor_shema.c b/Inu/Src2/551/main/sbor_shema.c deleted file mode 100644 index 7157fd1..0000000 --- a/Inu/Src2/551/main/sbor_shema.c +++ /dev/null @@ -1,1764 +0,0 @@ -/* - * sbor_shema.c - * - * Created on: 18 . 2021 . - * Author: stud - */ -#include "sbor_shema.h" -#include "IQmathLib.h" -#include "edrk_main.h" -#include "optical_bus.h" -#include "adc_tools.h" -#include "control_station.h" -#include "control_station_project.h" -#include "digital_filters.h" -#include "detect_errors.h" - -#define RASCEPITEL_MANUAL_ALWAYS_ON 0//1 -/////////////////////////////////////////////// -/////////////////////////////////////////////// -//#define IQ_MINIMAL_DELTA_RUN_CHARGE_1 559240 //100V -//#define IQ_MINIMAL_DELTA_RUN_CHARGE_2 1118480 //200V - -//#define IQ_MINIMAL_DELTA_RUN_CHARGE 279620 // 50V -#define IQ_MINIMAL_DELTA_RUN_CHARGE_1 1118480// 200 V ///279620 // 50V -#define IQ_MINIMAL_DELTA_RUN_CHARGE_2 1118480// 200 V //279620 // 50V - -#define IQ_MINIMAL_DELTA_RUN_WORK 2796202 // 500V // 2236960 // 400V // 1677720 // 300 V // 559240 // 100V - -#define IQ_MINIMAL_ZAD_U_CHARGE 55924 // 10V -#define IQ_MAXIMAL_ZAD_U_CHARGE 14596177 // 2610V -#define IQ_MINIMAL_DELTA_RUN_CHARGE2 139810 //25 V -#define TIME_WAIT_CHARGE_ON 300 //30 sec -#define TIME_PAUSE_U_RISE 30 // 1 sec - -#define IQ_MINIMAL_RISE_U 55924 // 10V - -unsigned int zaryad_on_off(unsigned int flag) -{ - static int restart_charge=0, batt_ok = 0; - static unsigned int time_wait_on_charge=0; - static unsigned int time_pause_detect_u_rise=0; - - static _iq prev_U1=0, prev_U2 = 0; - - batt_ok = 0; -// - !!! - if (_IQabs(filter.iqU_1_long-filter.iqU_2_long)>IQ_MINIMAL_DELTA_RUN_WORK) - { -// edrk.errors.e6.bits.ERROR_PRE_CHARGE_U |= 1; - edrk.errors.e6.bits.ER_DISBAL_BATT |= 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - batt_ok = 0; - } - - if (flag && edrk.summ_errors==0 && edrk.errors.e6.bits.ERROR_PRE_CHARGE_U==0 && edrk.errors.e6.bits.ER_DISBAL_BATT==0 ) - { -// !!! - if ((edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_ZAD_U_CHARGE) <= 0) - { - edrk.errors.e5.bits.ERROR_PRE_CHARGE_ON |= 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - return 0; - } - // !!! - - if ((IQ_MAXIMAL_ZAD_U_CHARGE - edrk.zadanie.iq_ZadanieU_Charge) < 0) - { - edrk.errors.e5.bits.ERROR_PRE_CHARGE_ON |= 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - return 0; - } - -// - if (_IQabs(filter.iqU_1_long-filter.iqU_2_long)>IQ_MINIMAL_DELTA_RUN_WORK) //IQ_MINIMAL_DELTA_RUN_CHARGE_1 - { - edrk.errors.e6.bits.ER_DISBAL_BATT |= 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - return 0; - } - - if (restart_charge == 0) - { - - // , - if ( edrk.from_ing1.bits.ZARYAD_ON && - (filter.iqU_1_long>=(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_1) - || filter.iqU_2_long>=(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_1)) - ) - { - restart_charge = 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - } - else - { - //TODO !!! if. ? - if ( edrk.from_ing1.bits.ZARYAD_ON==0 && - (filter.iqU_1_long>=(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2) - || filter.iqU_2_long>=(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2)) ) - { - restart_charge = 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - } - else - { - // , . - if ( (filter.iqU_1_long<(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2)) - && (filter.iqU_2_long<(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2)) ) - edrk.to_ing.bits.ZARYAD_ON = 1; - - } - - } - - if (pause_detect_error(&time_pause_detect_u_rise,TIME_PAUSE_U_RISE,1)) - { - time_pause_detect_u_rise = 0; - - if (((filter.iqU_1_long-prev_U1)>=IQ_MINIMAL_RISE_U) || - ((filter.iqU_2_long-prev_U2)>=IQ_MINIMAL_RISE_U) ) - time_wait_on_charge = 0; - - - prev_U1 = filter.iqU_1_long; - prev_U2 = filter.iqU_2_long; - } - - - // !!! - if (pause_detect_error(&time_wait_on_charge,TIME_WAIT_CHARGE_ON,1)) - edrk.errors.e6.bits.ERROR_PRE_CHARGE_U |= 1; -/* - - if (filter.iqU_1_long>=(edrk.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE2) - || filter.iqU_2_long>=(edrk.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE2)) - { - restart_charge = 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - } - else - { - // - if (_IQabs(filter.iqU_1_long-filter.iqU_2_long)>IQ_MINIMAL_DELTA_RUN_CHARGE) - edrk.errors.e6.bits.ER_DISBAL_BATT |= 1; - - // , . - if ( (filter.iqU_1_long<(edrk.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE)) - || (filter.iqU_2_long<(edrk.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE)) ) - edrk.to_ing.bits.ZARYAD_ON = 1; - - if ( (filter.iqU_1_long>=(edrk.iq_ZadanieU_Charge)) - && (filter.iqU_2_long>=(edrk.iq_ZadanieU_Charge)) ) - { - restart_charge = 1; - edrk.to_ing.bits.ZARYAD_ON = 0; - } - } -*/ - - } - else//restart_charge==0 - { - // - !!! - if ( (filter.iqU_1_long<(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2)) - || (filter.iqU_2_long<(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2)) ) - edrk.errors.e6.bits.ERROR_PRE_CHARGE_U |= 1; - - } //restart_charge==0 - } - else // flag==1 - { - restart_charge = 0; - edrk.to_ing.bits.ZARYAD_ON = 0; - time_wait_on_charge = 0; - prev_U1 = filter.iqU_1_long; - prev_U2 = filter.iqU_2_long; - } - - if ( (filter.iqU_1_long>=(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2)) - && (filter.iqU_2_long>=(edrk.zadanie.iq_ZadanieU_Charge-IQ_MINIMAL_DELTA_RUN_CHARGE_2)) - && (_IQabs(filter.iqU_1_long-filter.iqU_2_long)=edrk.iqMIN_U_IN) - && (filter.iqUin_m2>=edrk.iqMIN_U_IN) - && (filter.iqUin_m1<=edrk.iqMAX_U_IN) - && (filter.iqUin_m2<=edrk.iqMAX_U_IN) ) - return 1; - else - return 0; - - } - else - return 0; -} -/////////////////////////////////////////////// - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// - -#define TIME_WAIT_SBOR 8500 -#define TIME_WAIT_ANSWER_NASOS 500 -#define TIME_WAIT_OK_NASOS 50 - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -void sbor_shema_pusk_nasos(unsigned int t_start, unsigned int t_finish) -{ - static unsigned int time_error_nasos = 0; - static unsigned int time_ok_nasos = 0; - - int status_pump, status_pump_long; - - // - if (edrk.Sbor_Mode == t_start) - { - edrk.enter_to_pump_stage = 1; - // , ! - edrk.warnings.e5.bits.PUMP_1 = edrk.warnings.e5.bits.PUMP_2 = 0; - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_PUMP]==0) - edrk.AutoStartPump = 1; - - time_error_nasos = 0; - time_ok_nasos = 0; - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_PUMP]==1) - edrk.Sbor_Mode = t_finish; // - } - - // - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Mode>1) )) - { - - - } - - // , - if (edrk.Sbor_Mode==(t_finish-1)) - { - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_PUMP_ON_SBOR |= 1; - edrk.Status_Sbor = 2; - edrk.AutoStartPump = 0; - } - - } -// - if (edrk.Sbor_Mode>t_finish) - { - if (edrk.StatusPumpFanAll==0) - { - // - if (edrk.SelectPump1_2==1) - { - // 1 - // - if (pause_detect_error(&time_error_nasos,TIME_WAIT_ANSWER_NASOS,1)) - { - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_RESTART_PUMP_1_ON_SBOR |= 1; - edrk.Status_Sbor = 102; - edrk.AutoStartPump = 0; - } - } - else - if (edrk.SelectPump1_2==2) - { - // 2 - // - if (pause_detect_error(&time_error_nasos,TIME_WAIT_ANSWER_NASOS,1)) - { - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_RESTART_PUMP_1_ON_SBOR |= 1; - edrk.Status_Sbor = 102; - edrk.AutoStartPump = 0; - } - } - else - { - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_RESTART_PUMP_ALL_ON_SBOR |= 1; - edrk.Status_Sbor = 102; - edrk.AutoStartPump = 0; - } - } - else - time_error_nasos = 0; - - } -} - -void sbor_shema_pusk_zaryad(unsigned int t_start, unsigned int t_finish) -{ - ///////////////////////////////////// - // - if (edrk.Sbor_Mode == t_start) - { - if (control_station.active_array_cmd[CONTROL_STATION_CMD_ENABLE_ON_CHARGE]==1) - edrk.Run_Pred_Zaryad = 1; // ! - } - - // - // - - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_finish) - { - if (edrk.Zaryad_OK==0 || edrk.from_ing1.bits.ZARYAD_ON==1 ) - { - edrk.Run_Pred_Zaryad = 0; - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_PRED_ZARYAD_AFTER |= 1; - edrk.Status_Sbor = 104; - } - } - -} -void sbor_shema_pusk_ump(unsigned int t_start, unsigned int t_finish) -{ - static int enable_run_ump=0; - // UMP - if (edrk.Sbor_Mode==t_start && edrk.Zaryad_OK == 1) - { - // edrk.Run_UMP = 1; - enable_run_ump = 0; - } - - - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_finish && (edrk.Zaryad_OK == 0 || edrk.Status_UMP_Ok==0)) -// { -// -// edrk.Run_UMP = 0; -// edrk.Run_Pred_Zaryad = 0; -// edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; -// edrk.errors.e7.bits.UMP_NOT_ANSWER |= 1; -// edrk.Run_QTV = 0; -// edrk.Status_Sbor = 105; -// } - - -} - -void sbor_shema_pusk_qtv(unsigned int t_start, unsigned int t_finish) -{ - if (edrk.Sbor_Mode==t_start && edrk.Zaryad_OK == 1 && edrk.Status_UMP_Ok) - { - edrk.Run_QTV = 1; - } - - - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_finish) - { - if (edrk.Zaryad_OK == 0 || edrk.Status_QTV_Ok==0) - { - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_STATUS_QTV |= 1; - edrk.Run_QTV = 0; - edrk.Status_Sbor = 106; - } - - } - -} - -void sbor_shema_stop_ump(unsigned int t_start, unsigned int t_finish) -{ - // UMP - if (edrk.Sbor_Mode==t_start && edrk.Status_QTV_Ok == 1) - { - edrk.Run_UMP = 0; - } - - - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_finish && edrk.Status_UMP_Ok==1) - { - - edrk.Run_UMP = 0; - edrk.Run_Pred_Zaryad = 0; - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_UMP_ON_AFTER |= 1; - edrk.Run_QTV = 0; - edrk.Status_Sbor = 107; - } - - -} - - -void sbor_shema_rascepitel_level_1(unsigned int t_start, unsigned int t_finish) -{ - -#if(RASCEPITEL_MANUAL_ALWAYS_ON==1) - if (edrk.Sbor_Mode==t_start && (edrk.Zaryad_OK == 1 && edrk.Status_QTV_Ok==1 )) - { - edrk.Stage_Sbor = STAGE_SBOR_STATUS_RASCEPITEL_1; - // , - if (optical_read_data.data.cmd.bit.ready_cmd != CODE_READY_CMD_READY2 ) - { - edrk.Run_Rascepitel = 1; - edrk.Sbor_Mode = t_finish; // - } - else - edrk.RunZahvatRascepitel = 1; // - } - - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_start && edrk.Sbor_Modet_finish) - { - if (edrk.Run_Rascepitel==0) - { - // - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_RASCEPITEL_WAIT_CMD |= 1; - edrk.Run_QTV = 0; - - edrk.Status_Sbor = 108; - // - if (edrk.RunZahvatRascepitel) - edrk.errors.e1.bits.NO_CONFIRM_ON_RASCEPITEL |= 1; - - edrk.RunZahvatRascepitel = 0; - edrk.Run_Rascepitel = 0; - } - } - -} - -void sbor_shema_rascepitel_level_2(unsigned int t_start, unsigned int t_finish) -{ - - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_finish && edrk.Status_Rascepitel_Ok==0) - { - // , - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.Run_QTV = 0; - edrk.Status_Sbor = 109; -// edrk.errors.e6.bits.RASCEPITEL_ERROR_NOT_ANSWER |= 1; - edrk.errors.e11.bits.ERROR_RASCEPITEL_ON_AFTER |= 1; - edrk.RunZahvatRascepitel = 0; - edrk.Run_Rascepitel = 0; - - - } - - - // - // edrk.RunZahvatRascepitel - - -// -// if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_start && edrk.Sbor_Modet_start && edrk.Sbor_Modet_finish) -// { -// if (edrk.Status_Rascepitel_Ok==0) -// { -// edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; -// edrk.Run_QTV = 0; -// edrk.RunZahvatRascepitel = 0; -// edrk.Status_Sbor = 9; -// edrk.Run_Rascepitel = 0; -// -// } -// -// } - -} - -void sbor_shema_wait_ready_another(unsigned int t_start, unsigned int t_finish) -{ - - if (edrk.Sbor_Mode>t_start && edrk.Sbor_Modet_start && (edrk.Zaryad_OK == 1 && edrk.Status_QTV_Ok==1 && edrk.Status_Rascepitel_Ok)) - { - edrk.Stage_Sbor = STAGE_SBOR_STATUS_FINISH; - edrk.SborFinishOk = 1; - // allow_discharge = 1; - } - - - if (edrk.Sbor_Mode>t_finish && (edrk.SborFinishOk) ) - { - edrk.time_wait_sbor = 0; - } - else - edrk.Sbor_Mode++; - -} - - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -#define TIME_WAIT_RELE_UMP_ON 20 //2 sec -#define TIME_WAIT_RELE_UMP_OFF 20 //2 sec - -#define TIME_WAIT_ANSWER_UMP_ON 150 //15 sec -#define TIME_WAIT_ANSWER_UMP_OFF 40 //4 sec - -#define TIME_PAUSE_AFTER_GET_READY_UMP 50 // 5 sec - -/////////////////////////////////////////////// -int ump_on_off(unsigned int flag) -{ -static unsigned int time_wait_rele_on_ump=0; -static unsigned int time_wait_rele_off_ump=0; -static unsigned int time_wait_answer_on_ump=0; -static unsigned int time_wait_answer_off_ump=0; - - -int cmd_ump=0;//,cmd_p2=0; -static int UMP_Ok = 0; -static int prev_error = 0, count_ready_upm = 0; //, flag_enable_on_ump = 0; - - - cmd_ump = 0; -// cmd_p2 = 0; - - if ( flag==1 && edrk.summ_errors==0) - { - cmd_ump = 1; - } - else - { - cmd_ump = 0; - } - - - - edrk.cmd_to_ump = cmd_ump; - - - if (cmd_ump) - { - -// if ((pause_detect_error(&time_wait_rele_on_qtv,TIME_WAIT_RELE_UMP_ON,1)==0) && edrk.from_shema.bits.UMP_ON_OFF==0) -// { -// edrk.to_shema.bits.QTV_ON_OFF = 1; -// } -// else - - // ! - if (edrk.from_shema_filter.bits.READY_UMP == 1) - { - // TIME_PAUSE_AFTER_GET_READY_UMP - if (count_ready_upm10) && edrk.Status_Ready.bits.ready1; - - if (mode && edrk.summ_errors==0 && enable_sbor==0) - { - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_DISABLE_SBOR |= 1; - // - edrk.AutoStartPump = 0; - edrk.Sbor_Mode = 0; - edrk.Razbor_Mode = 0; - edrk.time_wait_sbor = 0; - time_wait_razbor = 0; - - edrk.Run_Pred_Zaryad = 0; - edrk.Zaryad_OK = 0; - edrk.Run_QTV = 0; - edrk.Run_UMP = 0; - edrk.SborFinishOk = 0; - edrk.RunZahvatRascepitel = 0; - -// edrk.Run_Rascepitel = 0; - - edrk.Status_Sbor = 1; - first_run = 1; - edrk.enter_to_pump_stage = 0; - return (edrk.Sbor_Mode); - } - -// - if (mode && edrk.summ_errors==0 && enable_sbor) - { - - if (pause_detect_error(&edrk.time_wait_sbor,TIME_WAIT_SBOR,1)) - { -// if (edrk.SborFinishOk==0) - edrk.errors.e7.bits.ERROR_SBOR_SHEMA |= 1; - edrk.errors.e11.bits.ERROR_VERY_LONG_SBOR |= 1; - } - - if (first_run) - { - if (edrk.flag_another_bs_first_ready12==1 && edrk.flag_this_bs_first_ready12==0) - { - // ? - add_t1 = 80; // 12 - } - else - if (edrk.flag_another_bs_first_ready12==0 && edrk.flag_this_bs_first_ready12==1) - { - // ? - add_t1 = 5; // 1 - } - else - if (edrk.flag_this_bs_first_ready12==0 && edrk.flag_another_bs_first_ready12==0) - { - // - if (edrk.flag_second_PCH == 1) - add_t1 = 120; // 18 - else - add_t1 = 80; // 7 - } - - - - -// if (optical_read_data.data.cmd.bit.ready_cmd==CODE_READY_CMD_READY1TO2 && edrk.flag_second_PCH == 1) -// { -// // ? -// add_t1 = 150; // 15 -// } -// else -// { -// if (edrk.flag_second_PCH == 0) -// add_t1 = 0; -// else -// add_t1 = 70; // 7 -// } - - first_run = 0; - } - - // - t1 = 10 + add_t1; - delta_t = 300;//200; - t2 = t1 + delta_t; - sbor_shema_pusk_nasos(t1,t2);//350 - - t1 = t2+30;//380 - delta_t = 700; - t2 = t1 + delta_t; - sbor_shema_pusk_zaryad(t1,t2);//1080 - - t1 = t2+10;//1090 - delta_t = 750+750+300+600;//2400 - t2 = t1 + delta_t; - sbor_shema_pusk_ump(t1,t2);//3490 - - t1 = t2+30; //3520 3 - delta_t = 200; - t2 = t1 + delta_t; - sbor_shema_pusk_qtv(t1,t2);//3720 - - t1 = t2; - delta_t = 150; - t2 = t1 + delta_t; - sbor_shema_stop_ump(t1,t2);//3870 - - // , tfinish - // tstart tfinish - // ! - t1 = t2; - delta_t = 250; - t2 = t1 + delta_t; - sbor_shema_rascepitel_level_1(t1,t2);//4120 - - // tfinish - // tfinish - t1 = t2; - delta_t = 300; - t2 = t1 + delta_t; - sbor_shema_rascepitel_level_2(t1,t2);//4420 - - t1 = t2; - delta_t = 200; - t2 = t1 + delta_t; - sbor_shema_rascepitel_level_3(t1,t2);//4620 - - // , , - // tfinish - t1 = t2; - delta_t = 300; - t2 = t1 + delta_t; - sbor_shema_rascepitel_level_4(t1,t2);//4920 - - // tfinish - // , - t1 = t2; - delta_t = 1800; - t2 = t1 + delta_t; - sbor_shema_wait_ready_another(t1,t2);//6720 - - t1 = t2; - delta_t = 50; - t2 = t1 + delta_t; - sbor_shema_wait_finish(t1,t2);//6770 - - edrk.Razbor_Mode = 0; - edrk.RazborNotFinish = 0; - edrk.RunUnZahvatRascepitel = 0; - - if (edrk.Zaryad_OK) - may_be_discharge = 1; - - } - ///////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////// - // - ///////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////// - else - { - first_run = 1; - edrk.enter_to_pump_stage = 0; - // - if (edrk.Razbor_Mode==0) - edrk.RazborNotFinish = 1; - - if (edrk.Status_QTV_Ok==0 && edrk.Status_UMP_Ok==0 && may_be_discharge && edrk.Razbor_Mode>100) - { - allow_discharge = 1; - may_be_discharge = 0; - } - - edrk.AutoStartPump = 0; - edrk.Sbor_Mode = 0; - edrk.time_wait_sbor = 0; - edrk.Zaryad_OK = 0; - edrk.Run_QTV = 0; - edrk.Run_UMP = 0; - edrk.SborFinishOk = 0; - edrk.Run_Pred_Zaryad = 0; - edrk.RunZahvatRascepitel = 0; - - if (edrk.Razbor_Mode==10 && edrk.Status_QTV_Ok==1) - { - // - edrk.errors.e2.bits.ERROR_RAZBOR_SHEMA |= 1; - - } - - if (edrk.Run_Rascepitel && edrk.Razbor_Mode==20 && edrk.Status_QTV_Ok==0 && edrk.Status_Rascepitel_Ok==0) - { - // , -, - edrk.Run_Rascepitel = 0; - edrk.Razbor_Mode=1000; // - } - - - - if (edrk.Run_Rascepitel && edrk.Razbor_Mode==30 && edrk.Status_QTV_Ok==0 && edrk.Status_Rascepitel_Ok==1) - { - // , - if (optical_read_data.data.cmd.bit.ready_cmd != CODE_READY_CMD_READY2 ) - edrk.Run_Rascepitel = 0; - else - edrk.RunUnZahvatRascepitel = 1; // - } - - - - if (edrk.Razbor_Mode>40 && edrk.Razbor_Mode<390 && (edrk.Status_QTV_Ok==0) - && edrk.RunUnZahvatRascepitel && edrk.Status_Rascepitel_Ok==1) - { - // , - // - if (optical_read_data.data.cmd.bit.rascepitel_cmd == CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON ) // 01 - , - { - edrk.Run_Rascepitel = 0; - edrk.RunUnZahvatRascepitel = 0; - edrk.Razbor_Mode = 390; - } - } - - - - // , - if (edrk.Razbor_Mode>40 && edrk.Razbor_Mode<600 && (edrk.Status_QTV_Ok==0) - && edrk.RunUnZahvatRascepitel==0 && edrk.Run_Rascepitel==0) - { - if (edrk.Status_Rascepitel_Ok == 0 ) - { - edrk.Razbor_Mode = 600; - } - } - - - - if (edrk.Razbor_Mode>390 && (edrk.Status_QTV_Ok==0) - && edrk.RunUnZahvatRascepitel && edrk.Status_Rascepitel_Ok==1 && edrk.Run_Rascepitel) - { - if (optical_read_data.data.cmd.bit.rascepitel_cmd != CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON ) // - { - edrk.RunUnZahvatRascepitel = 0; - edrk.errors.e1.bits.NO_CONFIRM_ON_RASCEPITEL |= 1; - edrk.errors.e2.bits.ERROR_RAZBOR_SHEMA |= 1; - } - - } - - - - - -// -// if (edrk.Razbor_Mode==400 && (edrk.Status_QTV_Ok==0) -// && edrk.RunUnZahvatRascepitel && edrk.Status_Rascepitel_Ok==1) -// { -// // , -// // -// if (optical_read_data.data.cmd.bit.rascepitel_cmd != CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON ) // 01 - , -// { -//// edrk.Run_Rascepitel = 0; -// edrk.RunUnZahvatRascepitel = 0; -// edrk.errors.e1.bits.NO_CONFIRM_ON_RASCEPITEL |= 1; -// edrk.errors.e2.bits.ERROR_RAZBOR_SHEMA |= 1; -// -// } -// } -// - - - - - if (edrk.Razbor_Mode==600 && edrk.Status_QTV_Ok==0 - && edrk.Run_Rascepitel == 0 - && edrk.Status_Rascepitel_Ok==1 ) - { -#if(RASCEPITEL_MANUAL_ALWAYS_ON==1) - -#else - // ! - if (edrk.Run_Rascepitel_from_RS==0) // rs232? - { - edrk.errors.e6.bits.RASCEPITEL_ERROR_NOT_ANSWER |= 1; - edrk.errors.e2.bits.ERROR_RAZBOR_SHEMA |= 1; - } -#endif - } - -#if(RASCEPITEL_MANUAL_ALWAYS_ON==1) - - edrk.RazborNotFinish = 0; - edrk.RunUnZahvatRascepitel = 0; - edrk.Razbor_Mode=650; // - - -#else - - // , , - if (edrk.Run_Rascepitel==0 && edrk.Razbor_Mode>20 && edrk.Status_QTV_Ok==0 && (edrk.Status_Rascepitel_Ok==0 || edrk.Run_Rascepitel_from_RS==1) ) - { - edrk.RazborNotFinish = 0; - edrk.RunUnZahvatRascepitel = 0; - edrk.Razbor_Mode=650; // - } -#endif - -// edrk.Run_Rascepitel = 0; - - if (edrk.Razbor_Mode>650) - { - time_wait_razbor = 0; - } - else - edrk.Razbor_Mode++; - - } - - - if (edrk.errors.e7.bits.ERROR_SBOR_SHEMA) - { - // - edrk.AutoStartPump = 0; - edrk.Sbor_Mode = 0; - edrk.Run_Pred_Zaryad = 0; - edrk.time_wait_sbor = 0; - edrk.Zaryad_OK = 0; - edrk.Run_QTV = 0; - edrk.Run_UMP = 0; - edrk.SborFinishOk = 0; - edrk.RunZahvatRascepitel = 0; -// edrk.Run_Rascepitel = 0; // , ???? - - } - - - ////////////////////////////////////// - ////////////////////////////////////// - - edrk.Status_Charge = zaryad_on_off(edrk.Run_Pred_Zaryad); - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_UMP]==1 || edrk.Status_Ready.bits.ImitationReady2) - { - edrk.Status_UMP_Ok = edrk.Run_UMP; - edrk.Zaryad_UMP_Ok = 1; - edrk.to_shema.bits.UMP_ON_OFF = 0; - } - else - { - edrk.Status_UMP_Ok = ump_on_off(edrk.Run_UMP); - edrk.Zaryad_UMP_Ok = detect_zaryad_ump(); - } - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_DISABLE_ON_QTV]==1 || edrk.Status_Ready.bits.ImitationReady2) - { - edrk.Status_QTV_Ok = edrk.Run_QTV; - edrk.to_shema.bits.QTV_ON_OFF = 0; - edrk.to_shema.bits.QTV_ON = 0; - } - else - edrk.Status_QTV_Ok = qtv_on_off(edrk.Run_QTV); - - rascepitel_on_off ( edrk.Run_Rascepitel || edrk.Run_Rascepitel_from_RS, - &edrk.Status_Perehod_Rascepitel, - &edrk.Status_Rascepitel_Ok, - &edrk.Final_Status_Rascepitel - ); - - - ////////////////////////////////////// - ////////////////////////////////////// - - - - - - - ////////////////////////////////////// - ////////////////////////////////////// - - - if (control_station.active_array_cmd[CONTROL_STATION_CMD_MANUAL_DISCHARGE]==1 && edrk.SborFinishOk==0) - edrk.ManualDischarge = 1; - else - edrk.ManualDischarge = 0; - - if (allow_discharge && edrk.SborFinishOk == 0) - { - edrk.Discharge = 1; - allow_discharge = 0; - } - - - if ( edrk.Zaryad_OK == 1 && edrk.Status_QTV_Ok==1 && edrk.Status_Rascepitel_Ok) - edrk.Status_Ready.bits.ready7 = 1; - else - edrk.Status_Ready.bits.ready7 = 0; - -// if (edrk.StatusPumpFanAll) -// edrk.Status_Ready.bits.ready1 = 1; -// else -// edrk.Status_Ready.bits.ready1 = 0; - - if (edrk.Run_Pred_Zaryad) - edrk.Status_Ready.bits.ready2 = 1; - else - edrk.Status_Ready.bits.ready2 = 0; - - if (edrk.Zaryad_OK) - edrk.Status_Ready.bits.ready3 = 1; - else - edrk.Status_Ready.bits.ready3 = 0; - - if (edrk.Status_QTV_Ok) - edrk.Status_Ready.bits.ready4 = 1; - else - edrk.Status_Ready.bits.ready4 = 0; - - if (edrk.SborFinishOk || edrk.Status_Ready.bits.ImitationReady2==1) - edrk.Status_Ready.bits.ready5 = 1; - else - edrk.Status_Ready.bits.ready5 = 0; - - if (edrk.ms.ready3 || edrk.ms.another_bs_maybe_on==0) - edrk.Status_Ready.bits.ready6 = 1; - else - edrk.Status_Ready.bits.ready6 = 0; - - - if (edrk.Status_Ready.bits.ready5==1 && edrk.Status_Ready.bits.ready6==1 && edrk.Status_Ready.bits.MasterSlaveActive) - { - if (edrk.Status_Ready.bits.ImitationReady2) - edrk.Status_Ready.bits.preImitationReady2 = 1; - edrk.Status_Ready.bits.ready_final = 1; - } - else - { - edrk.Status_Ready.bits.ready_final = 0; - edrk.Status_Ready.bits.preImitationReady2 = 0; - } - - if (edrk.Status_Ready.bits.ready_final && prev_ready_final==0) - edrk.count_sbor++; - - prev_ready_final = edrk.Status_Ready.bits.ready_final; - - return (edrk.Sbor_Mode); -} - - - - - -unsigned int imit_signals_rascepitel(unsigned int *counter, unsigned int max_pause, unsigned int s, unsigned int cmd_clear) -{ - if (cmd_clear==1) - { - (*counter) = 0; - return 0; - } - - if (s) - { - if ((*counter)>=max_pause) - return 1; - else - (*counter)++; - - return 0; - } - - if (s==0) - { - if ((*counter)==0) - return 0; - else - (*counter)--; - - return 1; - } - return 0; -} - - -#define TIME_WAIT_OFF_BLOCK_KEY 100 -void auto_block_key_on_off(void) -{ - static unsigned int count_err = TIME_WAIT_OFF_BLOCK_KEY; - - if (edrk.SumSbor && edrk.enter_to_pump_stage) - { - edrk.Status_Ready.bits.Batt = 1; - edrk.to_ing.bits.BLOCK_KEY_OFF = 0; - count_err = 0; - } - - if (filter.iqU_1_long >= U_LEVEL_ON_BLOCK_KEY || filter.iqU_2_long >= U_LEVEL_ON_BLOCK_KEY) - { - edrk.Status_Ready.bits.Batt = 1; - edrk.to_ing.bits.BLOCK_KEY_OFF = 0; - count_err = 0; - } - - if (filter.iqU_1_long <= U_LEVEL_OFF_BLOCK_KEY && filter.iqU_2_long <= U_LEVEL_OFF_BLOCK_KEY && edrk.SumSbor==0) - { - if (pause_detect_error(&count_err,TIME_WAIT_OFF_BLOCK_KEY,1)) - { - edrk.to_ing.bits.BLOCK_KEY_OFF = 1; - edrk.Status_Ready.bits.Batt = 0; - } - } - else - count_err = 0; - - -} - - -/////////////////////////////////////////////// - - diff --git a/Inu/Src2/551/main/sbor_shema.h b/Inu/Src2/551/main/sbor_shema.h deleted file mode 100644 index fdde9ff..0000000 --- a/Inu/Src2/551/main/sbor_shema.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * sbor_shema.h - * - * Created on: 18 . 2021 . - * Author: stud - */ - -#ifndef SRC_MAIN_SBOR_SHEMA_H_ -#define SRC_MAIN_SBOR_SHEMA_H_ - - -#define U_LEVEL_ON_BLOCK_KEY 559240 // 100V -#define U_LEVEL_OFF_BLOCK_KEY 279620 // 50V - -unsigned int sbor_shema(int mode); -void rascepitel_on_off(unsigned int flag, int *status_perehod, int *status_on_off, int *final_status_on_off); -void auto_block_key_on_off(void); - -unsigned int imit_signals_rascepitel(unsigned int *counter, unsigned int max_pause, unsigned int s, unsigned int cmd_clear); -void set_status_pump_fan(void); - - - -#endif /* SRC_MAIN_SBOR_SHEMA_H_ */ diff --git a/Inu/Src2/551/main/sim_model.c b/Inu/Src2/551/main/sim_model.c deleted file mode 100644 index a84fa72..0000000 --- a/Inu/Src2/551/main/sim_model.c +++ /dev/null @@ -1,222 +0,0 @@ -/* - * sim_model.c - * - * Created on: 30 . 2024 . - * Author: yura - */ - -#if (_SIMULATE_AC==1) - -#ifdef __TI_COMPILER_VERSION__ -#pragma SET_DATA_SECTION(".slow_vars") -#endif - -//#include "math.h" - - -#include "V_MotorModel.h" -//#include "V_MotorParams.h" -//#include -#include "IQmathLib.h" -//#include "main.h" - -#include "v_pwm24_v2.h" -#include "edrk_main.h" -#include "params_norma.h" -#include "adc_tools.h" -#include "filter_v1.h" -#include "mathlib.h" -#include "v_rotor_22220.h" - -// -//#pragma DATA_SECTION(sim_model, ".slow_vars") -TMotorModel sim_model = MOTOR_MODEL_DEFAULTS; - - - -void sim_model_init(void) -{ - - //model.motorInternals.udc = 540; // - - sim_model.motorInternals.udc = 540; // - sim_model.tpr = svgen_pwm24_1.Tclosed_high;//450; // - sim_model.cmpr0 = svgen_pwm24_1.Tclosed_high/2; - sim_model.cmpr1 = svgen_pwm24_1.Tclosed_high/2; - sim_model.cmpr2 = svgen_pwm24_1.Tclosed_high/2; - sim_model.cmpr3 = svgen_pwm24_1.Tclosed_high/2; - - - sim_model.MotorParametersNum = 10;// - sim_model.dt = 0;//_IQ4mpy(_IQ4(150 / 4), pwm.DeadBand >> 20) >> 4; // - - // - sim_model.Init(&sim_model); // - - - - sim_model.Init(&sim_model); - -} - -void sim_model_execute(void) -{ - // - sim_model.cmpr0 = svgen_pwm24_1.Ta_imp/2 + svgen_pwm24_1.Tclosed_high/2;//PWM0->CMPA_bit.CMPA; - sim_model.cmpr1 = svgen_pwm24_1.Tb_imp/2 + svgen_pwm24_1.Tclosed_high/2;//PWM1->CMPA_bit.CMPA; - sim_model.cmpr2 = svgen_pwm24_1.Tc_imp/2 + svgen_pwm24_1.Tclosed_high/2;;//PWM2->CMPA_bit.CMPA; - - sim_model.InvertorEna = edrk.Go;// - - // - sim_model.Execute(&sim_model); - -} - - -void calc_norm_ADC_0_sim(int run_norma) -{ - _iq a1,a2,a3; - -#if (1) - -#if (_FLOOR6) - analog.iqU_1 = _IQ(sim_model.motorInternals.udc/NORMA_ACP/2.0);// iq_norm_ADC[0][0] - analog_zero.iqU_1 + analog.iqU_1_imit; - analog.iqU_2 = analog.iqU_1;//iq_norm_ADC[0][1] - analog_zero.iqU_2 + analog.iqU_1_imit; -#else - analog.iqU_1 = iq_norm_ADC[0][0] - analog_zero.iqU_1; - analog.iqU_2 = iq_norm_ADC[0][1] - analog_zero.iqU_2; -#endif - - analog.iqIu_1 = _IQ(sim_model.motorInternals.isPhaseA/NORMA_ACP/2.0); - analog.iqIv_1 = _IQ(sim_model.motorInternals.isPhaseB/NORMA_ACP/2.0); - analog.iqIw_1 = _IQ(sim_model.motorInternals.isPhaseC/NORMA_ACP/2.0); - - analog.iqIu_2 = analog.iqIu_1; - analog.iqIv_2 = analog.iqIv_1; - analog.iqIw_2 = analog.iqIw_1; - - analog.iqIin_1 = 0;//_IQ(sim_model.motorInternals.power/sim_model.motorInternals.udc/NORMA_ACP/2.0); //-iq_norm_ADC[0][9]; // - analog.iqIin_2 = analog.iqIin_1;//-iq_norm_ADC[0][9]; // - - analog.iqUin_A1B1 = 0;//iq_norm_ADC[0][10]; - -// 23550.1 - -// 23550.1 - - analog.iqUin_B1C1 = 0;//iq_norm_ADC[0][11]; // 23550.1 - analog.iqUin_A2B2 = 0;//iq_norm_ADC[0][12]; // 23550.1 - -// 23550.3 bs1 bs2 - -// analog.iqUin_B1C1 = iq_norm_ADC[0][12]; // 23550.3 -// analog.iqUin_A2B2 = iq_norm_ADC[0][11]; // 23550.3 -// - analog.iqUin_B2C2 = 0;//iq_norm_ADC[0][13]; - - analog.iqIbreak_1 = 0;//iq_norm_ADC[0][14]; - analog.iqIbreak_2 = 0;//iq_norm_ADC[0][15]; - -#else - analog.iqU_1 = analog.iqIu_1 = analog.iqIu_2 = analog.iqIv_1 = analog.iqIv_2 = - analog.iqIw_1 = analog.iqIw_2 = analog.iqIin_1 = analog.iqIin_2 = analog.iqUin_A1B1 = - analog.iqUin_B1C1 = analog.iqUin_A2B2 = analog.iqUin_B2C2 = analog.iqIbreak_1 = analog.iqIbreak_2 - = 0; -#endif - - analog.iqUin_C1A1 = -(analog.iqUin_A1B1 + analog.iqUin_B1C1); - analog.iqUin_C2A2 = -(analog.iqUin_A2B2 + analog.iqUin_B2C2); - - - - filter.iqU_1_long = exp_regul_iq(koef_Uzpt_long_filter, filter.iqU_1_long, analog.iqU_1); - filter.iqU_2_long = exp_regul_iq(koef_Uzpt_long_filter, filter.iqU_2_long, analog.iqU_2); - - -// analog.iqU_1_fast = filter_U1_3point(analog.iqU_1_fast); - filter.iqU_1_fast = exp_regul_iq(koef_Uzpt_fast_filter, filter.iqU_1_fast, analog.iqU_1); - filter.iqU_2_fast = exp_regul_iq(koef_Uzpt_fast_filter, filter.iqU_2_fast, analog.iqU_2); - - -// filter.iqUzpt_2_2_fast = exp_regul_iq(koef_Uzpt_fast_filter, filter.iqUzpt_2_2_fast, analog.iqUzpt_2_2); - - - -//15 - - - analog.iqIm_1 = im_calc(analog.iqIu_1, analog.iqIv_1, analog.iqIw_1); - analog.iqIm_2 = im_calc(analog.iqIu_2, analog.iqIv_2, analog.iqIw_2); - - analog.iqIu = analog.iqIu_1+analog.iqIu_2; - analog.iqIv = analog.iqIv_1+analog.iqIv_2; - analog.iqIw = analog.iqIw_1+analog.iqIw_2; - - analog.iqIm = im_calc(analog.iqIu, analog.iqIv, analog.iqIw); - - - analog.iqIin_sum = analog.iqIin_1+analog.iqIin_2; - -// analog.iqIm_3 = im_calc(analog.iqIa1_1_fir_n+analog.iqIa2_1_fir_n, analog.iqIb1_1_fir_n+analog.iqIb2_1_fir_n, analog.iqIc1_1_fir_n+analog.iqIc2_1_fir_n); - - analog.iqUin_m1 = im_calc(analog.iqUin_A1B1, analog.iqUin_B1C1, analog.iqUin_C1A1); - analog.iqUin_m2 = im_calc(analog.iqUin_A2B2, analog.iqUin_B2C2, analog.iqUin_C2A2); - -// analog.iqUin_m2 = im_calc(analog.UinA2, analog.UinB2, analog.UinC2); - - filter.iqUin_m1 = exp_regul_iq(koef_Uin_filter, filter.iqUin_m1, analog.iqUin_m1); - filter.iqUin_m2 = exp_regul_iq(koef_Uin_filter, filter.iqUin_m2, analog.iqUin_m2); - - - -// i_led1_on_off(0); -// i_led1_on_off(1); - -//1 - - filter.iqIm_1 = exp_regul_iq(koef_Im_filter, filter.iqIm_1, analog.iqIm_1); - filter.iqIm_2 = exp_regul_iq(koef_Im_filter, filter.iqIm_2, analog.iqIm_2); - filter.iqIm = exp_regul_iq(koef_Im_filter, filter.iqIm, analog.iqIm); - - filter.iqIin_sum = exp_regul_iq(koef_Im_filter, filter.iqIin_sum, analog.iqIin_sum); - -//3 -// filter_batter2_Iin.InpVarCurr = (analog.iqIin_1)-ZERO_I_IN; - // filter_batter2_Iin.calc(&filter_batter2_Iin); - -// filter.iqIin = _IQmpy(filter_batter2_Iin.Out,_IQ_09); - - - filter.iqIin_1 = exp_regul_iq(koef_Im_filter, filter.iqIin_1, analog.iqIin_1); - filter.iqIin_2 = exp_regul_iq(koef_Im_filter, filter.iqIin_2, analog.iqIin_2); - - a1 = analog.iqU_1+analog.iqU_2; - a2 = analog.iqIin_1; - a3 = _IQmpy(a1,a2); - analog.PowerScalar = a3; -// filter.Power = analog.iqU_1+analog.iqU_2; - filter.PowerScalar = exp_regul_iq(koef_Power_filter, filter.PowerScalar, analog.PowerScalar); - filter.PowerScalarFilter2 = exp_regul_iq(koef_Power_filter2, filter.PowerScalarFilter2, filter.PowerScalar); - -} - - - -void calc_rotors_sim(void) -{ - rotor_22220.direct_rotor = 1; - rotor_22220.iqF = _IQ(sim_model.motorInternals.omega_rpm/60.0/NORMA_FROTOR); - - - rotor_22220.iqFout = exp_regul_iq(koef_Wout_filter, rotor_22220.iqFout, rotor_22220.iqF); - rotor_22220.iqFlong = exp_regul_iq(koef_Wout_filter_long, rotor_22220.iqFlong, rotor_22220.iqF); - -} - - - -//#ifdef __TI_COMPILER_VERSION__ -//#pragma RESET_DATA_SECTION -//#endif - -#endif diff --git a/Inu/Src2/551/main/sim_model.h b/Inu/Src2/551/main/sim_model.h deleted file mode 100644 index 9a27c91..0000000 --- a/Inu/Src2/551/main/sim_model.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * sim_model.h - * - * Created on: 30 . 2024 . - * Author: yura - */ - -#ifndef SRC_MAIN_SIM_MODEL_H_ -#define SRC_MAIN_SIM_MODEL_H_ - -#include "V_MotorModel.h" - -void sim_model_init(void); -void sim_model_execute(void); -void calc_norm_ADC_0_sim(int run_norma); -void calc_rotors_sim(void); - - -extern TMotorModel sim_model; - -#endif /* SRC_MAIN_SIM_MODEL_H_ */ diff --git a/Inu/Src2/551/main/sync_tools.c b/Inu/Src2/551/main/sync_tools.c deleted file mode 100644 index ee9ee9d..0000000 --- a/Inu/Src2/551/main/sync_tools.c +++ /dev/null @@ -1,520 +0,0 @@ -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File - -#include -#include -#include - -#include "big_dsp_module.h" -#include "MemoryFunctions.h" -#include "Spartan2E_Adr.h" -#include "Spartan2E_Functions.h" -#include "TuneUpPlane.h" -#include "pwm_test_lines.h" -#include "xp_write_xpwm_time.h" -#include "profile_interrupt.h" - -#include "edrk_main.h" -#define SIZE_SYNC_BUF 20 - -//#pragma DATA_SECTION(logbuf_sync1,".fa"); -unsigned int logbuf_sync1[SIZE_SYNC_BUF]; -unsigned int c_logbuf_sync1=0; - -//unsigned int capnum0; -//unsigned int capnum1; -//unsigned int capnum2; -//unsigned int capnum3; - -int delta_capnum = 0; -int delta_error = 0; -//int level_find_sync_zero = LEVEL_FIND_SYNC_ZERO; -unsigned int temp; - -unsigned int count_error_sync = 0; - -unsigned int count_timeout_sync = 0; -//unsigned int enable_profile_led1_sync = 1; -//unsigned int enable_profile_led2_sync = 0; - -SYNC_TOOLS_DATA sync_data=SYNC_TOOLS_DATA_DEFAULT; - - -#pragma CODE_SECTION(calculate_sync_detected,".fast_run2"); -void calculate_sync_detected(void) -{ - - - -// if (capnum0 > 1000) { -// return; -// } -// sync_data.level_find_sync_zero = LEVEL_FIND_SYNC_ZERO; - - delta_capnum = sync_data.capnum0 - sync_data.capnum1; - - if (delta_capnum > 0) // - { - sync_data.pwm_freq_plus_minus_zero = -1;//1; - - if (count_error_sync < MAX_COUNT_ERROR_SYNC) { - count_error_sync++; - count_error_sync++; - count_error_sync++; - } else - sync_data.local_flag_sync_1_2 = 0; - } - else - if (delta_capnum < 0) // - { - - if (sync_data.capnum0 > sync_data.level_find_sync_zero) - { - delta_error = sync_data.capnum0 - sync_data.level_find_sync_zero; - - if (delta_error > 50) { - if (count_error_sync < MAX_COUNT_ERROR_SYNC) { - count_error_sync++; - count_error_sync++; - count_error_sync++; - } else - sync_data.local_flag_sync_1_2 = 0; - } else { - if (count_error_sync > 0) { - count_error_sync--; - } - if (count_error_sync == 0) - sync_data.local_flag_sync_1_2 = 1; - } - sync_data.pwm_freq_plus_minus_zero = 1; - } - else - if (sync_data.capnum0 < sync_data.level_find_sync_zero) - { - - delta_error = sync_data.level_find_sync_zero - sync_data.capnum0; - - if (delta_error > 50) { - if (count_error_sync < MAX_COUNT_ERROR_SYNC) { - count_error_sync++; - count_error_sync++; - count_error_sync++; - } else - sync_data.local_flag_sync_1_2 = 0; - } else { - if (count_error_sync > 0) { - count_error_sync--; - } - if (count_error_sync == 0) - sync_data.local_flag_sync_1_2 = 1; - } - - sync_data.pwm_freq_plus_minus_zero = -1; - - } - else - { - sync_data.pwm_freq_plus_minus_zero = 0; - sync_data.local_flag_sync_1_2 = 1; - count_error_sync = 0; - } - } else - sync_data.pwm_freq_plus_minus_zero = 0; - - sync_data.delta_error_sync = delta_error; - sync_data.delta_capnum = sync_data.capnum0 - sync_data.level_find_sync_zero; //delta_capnum; - sync_data.count_error_sync = count_error_sync; - - -} - - -#pragma CODE_SECTION(sync_detected,".fast_run2"); -void sync_detected(void) -{ - -#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) - PWM_LINES_TK_18_ON; -#endif - - sync_data.latch_interrupt = 1; - // stop_sync_interrupt(); - - - // i_led2_on_off(1); - - // //Enable more interrupts from this capture - // EvbRegs.EVBIMRC.bit.CAP6INT = 0; - - // if (edrk.disable_interrupt_sync==0) - - -// WriteMemory(ADR_SAW_REQUEST, 0x8000); -// sync_data.capnum0 = ReadMemory(ADR_SAW_VALUE); - -// WriteMemory(ADR_SAW_REQUEST, 0x8000); -// sync_data.capnum0 = ReadMemory(ADR_SAW_VALUE); - - // pause_1000(1); - - WriteMemory(ADR_SAW_REQUEST, 0x8000); - sync_data.capnum1 = ReadMemory(ADR_SAW_VALUE); - - WriteMemory(ADR_SAW_REQUEST, 0x8000); - sync_data.capnum1 = ReadMemory(ADR_SAW_VALUE); - - sync_data.count_timeout_sync = 0; - sync_data.timeout_sync_signal = 0; - - logbuf_sync1[c_logbuf_sync1++] = sync_data.capnum0; -// logbuf_sync1[c_logbuf_sync1++] = sync_data.capnum1; - - if (c_logbuf_sync1==SIZE_SYNC_BUF) - c_logbuf_sync1=0; - - - if (sync_data.count_pause_ready < MAX_COUNT_PAUSE_READY) { - sync_data.count_pause_ready++; - sync_data.count_pause_ready++; - } else - sync_data.sync_ready = 1; - -//////////////////////////////////// - - // calculate_sync_detected(); - - - -// sync_data.capnum0 = capnum0; - - - - // - // stop_sync_interrupt(); - // EvbRegs.EVBIFRC.all = BIT2; - // - - - // // Acknowledge interrupt to receive more interrupts from PIE group 5 - // PieCtrlRegs.PIEACK.all = PIEACK_GROUP5; - - // if (edrk.disable_interrupt_sync==0) - // { - //// //Enable more interrupts from this capture - //// EvbRegs.EVBIMRC.bit.CAP6INT = 1; - //// - //// //use mask to clear EVAIFRA register - //// EvbRegs.EVBIFRC.bit.CAP6INT = 1; - // } - - //Enable more interrupts from this capture -// EvbRegs.EVBIMRC.bit.CAP6INT = 1; - - //use mask to clear EVAIFRA register -// EvbRegs.EVBIFRC.bit.CAP6INT = 1; - - - - - // DINT; - // PieCtrlRegs.PIEIER5.all = TempPIEIER; - - // return; - - - - // IER &= ~(M_INT5); - - //Enable more interrupts from this capture - // EvbRegs.EVBIMRC.bit.CAP6INT = 1; - - // Note: To be safe, use a mask value to write to the entire - // EVAIFRA register. Writing to one bit will cause a read-modify-write - // operation that may have the result of writing 1's to clear - // bits other then those intended. - //use mask to clear EVAIFRA register - // EvbRegs.EVBIFRC.bit.CAP6INT = 1; - // EvbRegs.EVBIFRC.all = BIT2; - - // IER &= ~(M_INT5); - - - // asm(" NOP;"); - - // i_led2_on_off(0); - - - // start_sync_interrupt(); - // EvbRegs.EVBIMRC.bit.CAP6INT = 1; - // Clear CAPINT6 interrupt flag - - // Acknowledge interrupt to receive more interrupts from PIE group 5 - // PieCtrlRegs.PIEACK.all = PIEACK_GROUP5; - - sync_data.count_interrupts++; - -#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC) - PWM_LINES_TK_18_OFF; -#endif - -} - - -//static long k_3=50; - -#pragma CODE_SECTION(Sync_handler,".fast_run2"); -interrupt void Sync_handler(void) { - - // Set interrupt priority: - volatile Uint16 TempPIEIER = PieCtrlRegs.PIEIER5.all; - IER |= M_INT5; - IER &= MINT5; // Set "global" priority - PieCtrlRegs.PIEIER5.all &= MG57; // Set "group" priority - PieCtrlRegs.PIEACK.all = 0xFFFF; // Enable PIE interrupts - - WriteMemory(ADR_SAW_REQUEST, 0x8000); - sync_data.capnum0 = ReadMemory(ADR_SAW_VALUE); - - stop_sync_interrupt_local(); // , , . - - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.sync) - i_led1_on_off_special(1); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.sync) - i_led2_on_off_special(1); -#endif - - EINT; - -// i_led2_on_off(1); - - // Insert ISR Code here....... - sync_detected(); -// pause_1000(k_3); - // Next line for debug only (remove after inserting ISR Code): - //ESTOP0; - -// i_led2_on_off(0); - - // Enable more interrupts from this timer -// EvbRegs.EVBIMRC.bit.CAP6INT = 1; - // Note: To be safe, use a mask value to write to the entire - // EVBIFRA register. Writing to one bit will cause a read-modify-write - // operation that may have the result of writing 1's to clear - // bits other then those intended. - EvbRegs.EVBIFRC.all = BIT2; - // Acknowledge interrupt to recieve more interrupts from PIE group 5 -// PieCtrlRegs.PIEACK.all |= PIEACK_GROUP5; - - // Restore registers saved: - DINT; - PieCtrlRegs.PIEIER5.all = TempPIEIER; - -#if (_ENABLE_INTERRUPT_PROFILE_LED1) - if (profile_interrupt.for_led1.bits.sync) - i_led1_on_off_special(0); -#endif -#if (_ENABLE_INTERRUPT_PROFILE_LED2) - if (profile_interrupt.for_led2.bits.sync) - i_led2_on_off_special(0); -#endif - - -} - -void setup_sync_int(void) { - -// return; - -// EALLOW; - - if (edrk.flag_second_PCH==1) - sync_data.level_find_sync_zero = xpwm_time.pwm_tics+5; - else - sync_data.level_find_sync_zero = LEVEL_FIND_SYNC_ZERO; - - sync_data.timeout_sync_signal = 0; - sync_data.count_timeout_sync = 0; - -// sync_data.what_main_pch = 1; // -// sync_data.what_main_pch = 2; // - sync_data.what_main_pch = 0; // - - - -///////////////////////////////////////////// - -// EvbRegs.EVBIFRC.bit.CAP6INT = 1; //Resets flag EVB Interrupt Flag Register - EvbRegs.EVBIFRC.all = BIT2; //Resets flag EVB Interrupt Flag Register - EvbRegs.EVBIMRC.bit.CAP6INT = 0; //1 //SET flag EVB Interrupt Mask Register C - // CAP6INT ENABLE - //0 Disable - //1 Enable - - -///////////////////////////////////////////// - EvbRegs.T4PR = 0xFFFF; //Set timer period - EvbRegs.T4CNT = 0; // Clear timer counter - - EvbRegs.T4CON.all = 0; // Disable timer - EvbRegs.T4CON.bit.FREE = 1; // FREE/SOFT, 00 = stop immediately on emulator suspend - EvbRegs.T4CON.bit.SOFT = 0; - EvbRegs.T4CON.bit.TMODE = 2; //TMODEx, 10 = continuous-up count mode - EvbRegs.T4CON.bit.TPS = 0; //TPSx, 111 = x/1 prescaler - EvbRegs.T4CON.bit.TENABLE = 0; // TENABLE, 1 = enable timer - EvbRegs.T4CON.bit.TCLKS10 = 0; //TCLKS, 00 = HSPCLK is clock source - EvbRegs.T4CON.bit.TCLD10 = 0; //Timer compare register reload condition, 00 When counter is 0 - EvbRegs.T4CON.bit.TECMPR = 1; // TECMPR, 1 = Enable timer compare operation - EvbRegs.T4CON.bit.SET3PR = 0; // SELT3PR: 0 - Use own period register - - -//////////////////////////////////////////////////// - EvbRegs.CAPCONB.all = 0; // Clear - - EvbRegs.CAPCONB.bit.CAP6EDGE = 2; //3:2 Edge Detection for Unit 6 - //Edge detection control for Capture Unit 6. -// 00 No detection -// 01 Detects rising edge -// 10 Detects falling edge -// 11 Detects both edges - - EvbRegs.CAPCONB.bit.CAP6TSEL = 0; // GP Timer selection for Unit 6 -// GP timer selection for Capture Units 4 and 5 -// 0 Selects GP timer 4 -// 1 Selects GP timer 3 - EvbRegs.CAPFIFOB.bit.CAP6FIFO = 0; //CAP6 FIFO status - EvbRegs.CAPCONB.bit.CAP6EN = 1; //Enables Capture Unit 6 -///////////////////////////////////////// - - - EALLOW; - PieVectTable.CAPINT6 = &Sync_handler; //?CAP????????????????? - EDIS; - - - PieCtrlRegs.PIEIER5.bit.INTx7 = 1; - - - -} - -void start_sync_interrupt(void) -{ - EvbRegs.EVBIFRC.all = BIT2; //Resets flag EVB Interrupt Flag Register - - IER |= M_INT5; // @suppress("Symbol is not resolved") -// PieCtrlRegs.PIEIER5.bit.INTx7 = 1; - - EvbRegs.EVBIMRC.bit.CAP6INT = 1; //SET flag EVB Interrupt Mask Register C - // //use mask to clear EVAIFRA register -// PieCtrlRegs.PIEACK.all |= PIEACK_GROUP5; - sync_data.latch_interrupt = 0; - sync_data.enabled_interrupt = 1; - -} - -void stop_sync_interrupt(void) -{ - sync_data.latch_interrupt = 0; - sync_data.enabled_interrupt = 0; - - IER &= ~(M_INT5); // @suppress("Symbol is not resolved") -// PieCtrlRegs.PIEIER5.bit.INTx7 = 0; - EvbRegs.EVBIMRC.bit.CAP6INT = 0; //SET flag EVB Interrupt Mask Register C - EvbRegs.EVBIFRC.all = BIT2; //Resets flag EVB Interrupt Flag Register -// PieCtrlRegs.PIEACK.all |= PIEACK_GROUP5; -} - -void stop_sync_interrupt_local(void) -{ - sync_data.latch_interrupt = 0; - - IER &= ~(M_INT5); // @suppress("Symbol is not resolved") - EvbRegs.EVBIMRC.bit.CAP6INT = 0; //SET flag EVB Interrupt Mask Register C - EvbRegs.EVBIFRC.all = BIT2; //Resets flag EVB Interrupt Flag Register -} - - -void setup_sync_line(void) { - - // output - EALLOW; - GpioMuxRegs.GPBMUX.bit.TCLKINB_GPIOB12 = 0; - GpioMuxRegs.GPBDIR.bit.GPIOB12 = 1; - EDIS; - - //input - EALLOW; - GpioMuxRegs.GPBMUX.bit.CAP6QI2_GPIOB10 = 1;// Configure as CAP6 -// GpioMuxRegs.GPBDIR.bit.GPIOB10 = 1; - EDIS; - -} - -#pragma CODE_SECTION(sync_inc_error,".fast_run"); -void sync_inc_error(void) -{ - - - if (sync_data.count_pause_ready > 0) { - sync_data.count_pause_ready--; - } else - sync_data.sync_ready = 0; - - - if (sync_data.count_timeout_sync < MAX_COUNT_TIMEOUT_SYNC) - { - sync_data.count_timeout_sync++; - } - else - { - sync_data.timeout_sync_signal = 1; - sync_data.count_pause_ready = 0; - sync_data.local_flag_sync_1_2 = 0; - } - - - if (count_error_sync < MAX_COUNT_ERROR_SYNC) { - count_error_sync++; - } else - sync_data.local_flag_sync_1_2 = 0; -} - -void clear_sync_error(void) -{ - sync_data.count_timeout_sync = 0; - sync_data.timeout_sync_signal = 0; -} - - -int get_status_sync_line(void) -{ - return !GpioDataRegs.GPBDAT.bit.GPIOB10; -} - -//int index_sync_ar = 0; -// -// -//void write_sync_logs(void) -//{ -// static int c=0; -// return; -// -//// logbuf1[index_filter_ar]=active_rect1.Id;//EvaRegs.CMPR1;//(active_rect1.pll_Ud);//svgenDQ.Wt; -//// logbuf2[index_filter_ar]=active_rect1.Iq;//EvaRegs.CMPR2;//filter.iqU_1_long;// (active_rect1.pll_Uq);//Iq; -//// logbuf3[index_filter_ar]=EvaRegs.CMPR1;//active_rect1.SetUzpt;////(active_rect1.Tetta);//abc_to_dq.Ud; -// -// index_sync_ar++; -// if (index_sync_ar>=SIZE_SYNC_BUF) -// { -// index_sync_ar=0; -// c++; -// if (c>=10) -// c=0; -// } -// -//} diff --git a/Inu/Src2/551/main/sync_tools.h b/Inu/Src2/551/main/sync_tools.h deleted file mode 100644 index e41ee87..0000000 --- a/Inu/Src2/551/main/sync_tools.h +++ /dev/null @@ -1,113 +0,0 @@ - - -#define LEVEL_FIND_SYNC_ZERO 10 //74 //24 -#define MAX_COUNT_ERROR_SYNC 100 -#define MAX_COUNT_TIMEOUT_SYNC 100 -#define MAX_COUNT_PAUSE_READY 100 - - - - - - - -typedef struct { - //Sync vals - int pwm_freq_plus_minus_zero; - int disable_sync; - int sync_ready; - unsigned int level_find_sync_zero; - - int delta_error_sync; - int delta_capnum; - int count_error_sync; - unsigned int capnum0; - unsigned int capnum1; - - int PWMcounterVal; - int local_flag_sync_1_2; - int global_flag_sync_1_2; - int timeout_sync_signal; - - unsigned int count_timeout_sync; - unsigned int count_pause_ready; - int enable_do_sync; - int latch_interrupt; - int enabled_interrupt; - unsigned int count_interrupts; - unsigned int what_main_pch; - - - -// int pzad_or_wzad; //given turns or power, depends on controlMode; -// int angle_pwm; //current rotor turns -// int iq_zad; // Iq_zadan -// union { -// struct { -// unsigned int wdog_tick :1; // 0_1_0 .. -// unsigned int statusQTV :1; //1-QTV On, QTV - off -// unsigned int master :1; // 1 -Master, 0 - not Master -// unsigned int slave :1; // 1 -Slave, 0 - not Slave -// -// unsigned int sync_1_2 :1; // 1 - yes, 0 - no -// unsigned int alarm :1; // 1 - yes, 0 - no -// unsigned int ready_cmd :2; // 00 - not ready,01-ready1,10-ready1to2, 11 -ready2 -// -// unsigned int controlMode :1; // 0 - regul turns; 1 - power -// unsigned int ready_to_go :1; // 1 - yes, 0 - no , -// unsigned int start_pwm :1; // 1 - yes, 0 - no -// unsigned int stop_pwm :1; // 1 - yes, 0 - no -// -// unsigned int pwm_status :1; // 1 -On, 0 - Off -// unsigned int err_optbus :1; // 1 - yes, 0 - no optbus -// unsigned int maybe_master :1; // 1 - yes, 0 - no Master -// unsigned int rascepitel :1; // 1 - yes, 0 - no -// -//// unsigned int leading_ready :1; //1 - second inverter ready to work or in work -//// unsigned int leading_Go :1; -// } bit; -// unsigned int all; -// } cmd; -} SYNC_TOOLS_DATA; - -#define SYNC_TOOLS_DATA_DEFAULT {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} - -extern SYNC_TOOLS_DATA sync_data; - - - - -void setup_sync_line(void); -void sync_inc_error(void); -void setup_sync_int(void); -void start_sync_interrupt(void); -void stop_sync_interrupt(void); -void stop_sync_interrupt_local(void); - - -int get_status_sync_line(void); - -void clear_sync_error(void); - -void Sync_alg(void); -void calculate_sync_detected(void); - - - - -inline void i_sync_pin_on() -{ - EALLOW; - GpioDataRegs.GPBSET.bit.GPIOB12 = 1; - EDIS; -} - - -inline void i_sync_pin_off() -{ - EALLOW; - GpioDataRegs.GPBCLEAR.bit.GPIOB12 = 1; - EDIS; -} - - diff --git a/Inu/Src2/551/main/synhro_tools.c b/Inu/Src2/551/main/synhro_tools.c deleted file mode 100644 index 47bbc5e..0000000 --- a/Inu/Src2/551/main/synhro_tools.c +++ /dev/null @@ -1,144 +0,0 @@ -/* - * synhro_tools.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - - - -#include - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include "optical_bus.h" -#include "sync_tools.h" - -////////////////////////////////////////////////////////// -unsigned int wait_synhro_optical_bus(void) -{ - static unsigned int cmd = 0; - static unsigned int count_wait_synhro = 0; - - -// -// switch(cmd) -// { -// 0 : if (optical_read_data.data.cmd.bit.wdog_tick == 0) -// cmd = 1; -// -// break; -// -// 1 : optical_write_data.data.cmd.bit.wdog_tick = 1; -// break; -// -// -// default: break -// } - - - - return 0; -} - -////////////////////////////////////////////////////////// -void clear_wait_synhro_optical_bus(void) -{ - - // optical_read_data.data.cmd.bit.wdog_tick = 0; - // optical_write_data.data.cmd.bit.wdog_tick = 0; - -} -////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////// -void who_select_sync_signal(void) -{ - - if (sync_data.what_main_pch) - { - if (sync_data.what_main_pch==2) - { - // !!! - if (edrk.flag_second_PCH) - sync_data.enable_do_sync = 1; - else - sync_data.enable_do_sync = 0; - return; - } - - if (sync_data.what_main_pch==1) - { - // !!! - if (edrk.flag_second_PCH) - sync_data.enable_do_sync = 0; - else - sync_data.enable_do_sync = 1; - return; - } - - } - -// if (optical_read_data.status == 1) - sync_data.global_flag_sync_1_2 = (sync_data.local_flag_sync_1_2 || optical_read_data.data.cmd.bit.sync_1_2); -// else - // sync_data.global_flag_sync_1_2 = (sync_data.local_flag_sync_1_2); - - - if (sync_data.timeout_sync_signal && optical_read_data.data.cmd.bit.sync_line_detect) - { - // , , - // . - - sync_data.enable_do_sync = 0; - - return; - } - - if (sync_data.timeout_sync_signal==0 && optical_read_data.data.cmd.bit.sync_line_detect==0) - { - // , , - // . - - sync_data.enable_do_sync = 1; - - return; - } - - if (sync_data.sync_ready && sync_data.timeout_sync_signal==0 && optical_read_data.data.cmd.bit.sync_line_detect) - { - - - if (optical_read_data.data.cmd.bit.sync_1_2 && sync_data.enable_do_sync==0) - { - // , - - } - else - if (optical_read_data.data.cmd.bit.sync_1_2 && sync_data.enable_do_sync==1) - { - // , , - - } - else - { - // , - // , - // . - if (edrk.flag_second_PCH==0) - sync_data.enable_do_sync = 1; - else - sync_data.enable_do_sync = 0; - } - return; - } - -} - -////////////////////////////////////////////////////////// - diff --git a/Inu/Src2/551/main/synhro_tools.h b/Inu/Src2/551/main/synhro_tools.h deleted file mode 100644 index e6de9f1..0000000 --- a/Inu/Src2/551/main/synhro_tools.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * synhro_tools.h - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#ifndef SRC_MAIN_SYNHRO_TOOLS_H_ -#define SRC_MAIN_SYNHRO_TOOLS_H_ - -void who_select_sync_signal(void); - -void clear_wait_synhro_optical_bus(void); - -unsigned int wait_synhro_optical_bus(void); -void clear_wait_synhro_optical_bus(void); - - -#endif /* SRC_MAIN_SYNHRO_TOOLS_H_ */ diff --git a/Inu/Src2/551/main/temper_p_tools.c b/Inu/Src2/551/main/temper_p_tools.c deleted file mode 100644 index c6a773a..0000000 --- a/Inu/Src2/551/main/temper_p_tools.c +++ /dev/null @@ -1,77 +0,0 @@ -/* - * temper_p_tools.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - - - -#include - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include "adc_tools.h" - -#include "temper_p_tools.h" -/////////////////////////////////////////////// -void nagrev_auto_on_off(void) -{ - //min_real_int_temper_air - -// if (edrk.temper_edrk.max_real_int_temper_waterTEMPER_NAGREF_OFF) -// edrk.to_ing.bits.NAGREV_OFF = 1; - - if (edrk.temper_edrk.min_real_int_temper_airTEMPER_NAGREF_OFF_1 || edrk.temper_edrk.max_real_int_temper_air>TEMPER_NAGREF_OFF_2) - edrk.to_ing.bits.NAGREV_OFF = 1; - -} -/////////////////////////////////////////////// - - - -//#define koef_P_Water_filter 1600000 //0.095367431640625 -void calc_p_water_edrk(void) -{ - _iq iqtain,iq_temp; - static _iq koef_P_Water_filter = _IQ (0.1/2.0); // 5 - - edrk.p_water_edrk.adc_p_water[0] = ADC_f[1][14]; - - edrk.p_water_edrk.real_p_water[0] = (_IQtoF(analog.P_Water_internal) * NORMA_ACP_P - 4.0) / 1.6; - edrk.p_water_edrk.real_int_p_water[0] = edrk.p_water_edrk.real_p_water[0] * K_P_WATER_TO_SVU; - - - iqtain = _IQ(edrk.p_water_edrk.real_p_water[0]/100.0); - iq_temp = _IQ(edrk.p_water_edrk.filter_real_p_water[0]/100.0); - - if (edrk.p_water_edrk.flag_init_filter_temp[0]==0) - { - iq_temp = iqtain; - edrk.p_water_edrk.flag_init_filter_temp[0]=1; - } - -// iq_temp_engine[i] = exp_regul_iq(koef_Temper_ENGINE_filter, iq_temp_engine[i], iqtain); - - iq_temp += _IQmpy( (iqtain-iq_temp), koef_P_Water_filter); - edrk.p_water_edrk.filter_real_p_water[0] = _IQtoF(iq_temp)*100.0; - edrk.p_water_edrk.filter_real_int_p_water[0] = edrk.p_water_edrk.filter_real_p_water[0]*K_P_WATER_TO_SVU; - - - -} - -////////////////////////////////////////////////////////// - diff --git a/Inu/Src2/551/main/temper_p_tools.h b/Inu/Src2/551/main/temper_p_tools.h deleted file mode 100644 index d99cb89..0000000 --- a/Inu/Src2/551/main/temper_p_tools.h +++ /dev/null @@ -1,22 +0,0 @@ -/* - * temper_p_tools.h - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#ifndef SRC_MAIN_TEMPER_P_TOOLS_H_ -#define SRC_MAIN_TEMPER_P_TOOLS_H_ - - -#define TEMPER_NAGREF_ON_1 120 -#define TEMPER_NAGREF_ON_2 200 - -#define TEMPER_NAGREF_OFF_1 170 -#define TEMPER_NAGREF_OFF_2 250 - -void nagrev_auto_on_off(void); -void calc_p_water_edrk(void); - - -#endif /* SRC_MAIN_TEMPER_P_TOOLS_H_ */ diff --git a/Inu/Src2/551/main/tk_Test.c b/Inu/Src2/551/main/tk_Test.c deleted file mode 100644 index fd5cc61..0000000 --- a/Inu/Src2/551/main/tk_Test.c +++ /dev/null @@ -1,369 +0,0 @@ - -#include <281xEvTimersInit.h> -#include -#include - -#include "DSP281x_Device.h" -#include "MemoryFunctions.h" -#include "Spartan2E_Adr.h" -#include "x_wdog.h" -#include "project.h" - -#pragma CODE_SECTION(pause_10,".fast_run"); -void pause_10(unsigned long t) -{ - unsigned long i; - - for (i = 0; i < t; i++) - { - asm(" NOP"); - } -} - - -void test_impulse(unsigned int impulse_channel,long impulse_time) -{ - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel); - pause_10(impulse_time); - i_WriteMemory(ADR_PWM_DIRECT,0xffff); -} - -#pragma CODE_SECTION(test_double_impulse,".fast_run"); -void test_double_impulse(unsigned int impulse_channel_1,unsigned int impulse_channel_2,long impulse_time,long middle_impulse_time,long last_impulse_time, int soft_off_enable, int soft_on_enable) -{ - project.disable_all_interrupt(); -// i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_2); -// pause_10(middle_impulse_time); - - if (soft_on_enable) - { - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_2); - pause_10(last_impulse_time); - } - - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_1); -// pause_10(impulse_time); - pause_10(impulse_time); - - - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_2); - pause_10(middle_impulse_time); - - i_WriteMemory(ADR_PWM_DIRECT, impulse_channel_1); - pause_10(last_impulse_time); - - if (soft_off_enable) - { - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_2); - pause_10(last_impulse_time); - } - - i_WriteMemory(ADR_PWM_DIRECT,0xffff); - - project.enable_all_interrupt(); -} - -void test_sin_impulse(unsigned int impulse_channel_1,unsigned int impulse_channel_2, unsigned int impulse_channel_3, long impulse_time,long middle_impulse_time) -{ - project.disable_all_interrupt(); - - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_2); - pause_10(middle_impulse_time); - - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_1); - pause_10(impulse_time); - - - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_2); - pause_10(middle_impulse_time); - - i_WriteMemory(ADR_PWM_DIRECT,impulse_channel_3); - pause_10(impulse_time); - i_WriteMemory(ADR_PWM_DIRECT,0xffff); - - project.enable_all_interrupt(); -} - - -void test_tk_ak_one_impulse(int tk0, int tk1, int tk2, int tk3, int period, int periodMiddle, int periodLast, int doubleImpulse, int sinImpulse, int soft_off_enable, int soft_on_enable) -{ - long p2 = 0, pM = 0, pL = 0; - float pf; - unsigned int tk0_0 = 0, tk0_1 = 0, tk0_2 = 0, tk0_3 = 0, tk0_4 = 0, tk0_5 = 0, tk0_6 = 0, tk0_7 = 0; - unsigned int tk1_0 = 0, tk1_1 = 0, tk1_2 = 0, tk1_3 = 0, tk1_4 = 0, tk1_5 = 0, tk1_6 = 0, tk1_7 = 0; - unsigned int tk2_0 = 0, tk2_1 = 0, tk2_2 = 0, tk2_3 = 0, tk2_4 = 0, tk2_5 = 0, tk2_6 = 0, tk2_7 = 0; - unsigned int tk3_0 = 0, tk3_1 = 0, tk3_2 = 0, tk3_3 = 0, tk3_4 = 0, tk3_5 = 0, tk3_6 = 0, tk3_7 = 0; - unsigned int break1 = 0, break2 = 0, break3 = 0, break4 = 0,key0 = 0, key1 = 0, key2 = 0, key3 = 0, key4 = 0, key5 = 0, key6 = 0, key7 = 0, key8 = 0,key9 = 0,key10 = 0,key11 = 0; - unsigned int Dkey0 = 0xffff, Dkey1 = 0xffff, Dkey2 = 0xffff; - unsigned int currentPWMMode1, currentPWMMode0, currPWMPeriod; - - - //// ////////// -#if (XPWMGEN==1) - i_WriteMemory(ADR_PWM_DRIVE_MODE, 3); -// pause_1000(100000L); - i_WriteMemory(ADR_PWM_DIRECT,0xffff); - i_WriteMemory(ADR_TK_MASK_0, 0); - -#endif - stop_eva_timer2(); - IER &= ~M_INT9; //stop CAN - //////////////////////////////////// - - if (period<=1) - period=1; - if (period>=1000) - period=1000; - pf = (float)(period) *11.724;/// 2.8328173374613003095975232198142;//(periodMiddle)*12; - p2 = pf; -// p2=(period) * 19 / 10;//(period)*12; - - if (periodMiddle<=1) - periodMiddle=1; - if (periodMiddle>=1000) - periodMiddle=1000; -// pM=(periodMiddle) * 19 / 10;//(periodMiddle)*12; - pf = (float)(periodMiddle)*11.724;// / 2.8328173374613003095975232198142;//(periodMiddle)*12; - pM = pf; - - if (periodLast<=1) - periodLast=1; - if (periodLast>=1000) - periodLast=1000; -// pM=(periodMiddle) * 19 / 10;//(periodMiddle)*12; - pf = (float)(periodLast)*11.724;// / 2.8328173374613003095975232198142;//(periodMiddle)*12; - pL = pf; - - - - tk0_0 = (tk0 >> 0) & 0x1; - tk0_1 = (tk0 >> 1) & 0x1; - tk0_2 = (tk0 >> 2) & 0x1; - tk0_3 = (tk0 >> 3) & 0x1; - tk0_4 = (tk0 >> 4) & 0x1; - tk0_5 = (tk0 >> 5) & 0x1; - tk0_6 = (tk0 >> 6) & 0x1; - tk0_7 = (tk0 >> 7) & 0x1; - - tk1_0 = (tk1 >> 0) & 0x1; - tk1_1 = (tk1 >> 1) & 0x1; - tk1_2 = (tk1 >> 2) & 0x1; - tk1_3 = (tk1 >> 3) & 0x1; - tk1_4 = (tk1 >> 4) & 0x1; - tk1_5 = (tk1 >> 5) & 0x1; - tk1_6 = (tk1 >> 6) & 0x1; - tk1_7 = (tk1 >> 7) & 0x1; - - tk2_0 = (tk2 >> 0) & 0x1; - tk2_1 = (tk2 >> 1) & 0x1; - tk2_2 = (tk2 >> 2) & 0x1; - tk2_3 = (tk2 >> 3) & 0x1; - tk2_4 = (tk2 >> 4) & 0x1; - tk2_5 = (tk2 >> 5) & 0x1; - tk2_6 = (tk2 >> 6) & 0x1; - tk2_7 = (tk2 >> 7) & 0x1; - - tk3_0 = (tk3 >> 0) & 0x1; - tk3_1 = (tk3 >> 1) & 0x1; - tk3_2 = (tk3 >> 2) & 0x1; - tk3_3 = (tk3 >> 3) & 0x1; - tk3_4 = (tk3 >> 4) & 0x1; - tk3_5 = (tk3 >> 5) & 0x1; - tk3_6 = (tk3 >> 6) & 0x1; - tk3_7 = (tk3 >> 7) & 0x1; - - if(doubleImpulse) - { - if(tk0_0 && tk0_7){ - Dkey0 = 0xfff6; - Dkey1 = 0xfff0; - } - else if(tk0_4 && tk0_3){ - Dkey0 = 0xfff9; - Dkey1 = 0xfff0; - } - else if(tk1_3 && tk0_0){ - Dkey0 = 0xffde; - Dkey1 = 0xffcc; - } - else if(tk1_0 && tk0_3){ - Dkey0 = 0xffed; - Dkey1 = 0xffcc; - } - else if(tk0_4 && tk1_3){ - Dkey0 = 0xffdb; - Dkey1 = 0xffc3; - } - else if(tk0_7 && tk1_0){ - Dkey0 = 0xffe7; - Dkey1 = 0xffc3; - }///// - else if(tk1_4 && tk2_3){ - Dkey0 = 0xFDBF; - Dkey1 = 0xFC3F; - } - else if(tk1_7 && tk2_0){ - Dkey0 = 0xFE7F; - Dkey1 = 0xFC3F; - } - else if(tk1_4 && tk2_7){ - Dkey0 = 0xF7BF; - Dkey1 = 0xF33F; - } - else if(tk1_7 && tk2_4){ - Dkey0 = 0xFB7F; - Dkey1 = 0xF33F; - } - else if(tk2_0 && tk2_7){ - Dkey0 = 0xF6FF; - Dkey1 = 0xF0FF; - } - else if(tk2_3 && tk2_4){ - Dkey0 = 0xF9FF; - Dkey1 = 0xF0FF; - } - else if (tk0_0){ - Dkey0 = 0xfffe; - Dkey1 = 0xfffc; - } - else if (tk0_3){ - Dkey0 = 0xfffd; - Dkey1 = 0xfffc; - } - else if (tk0_4){ - Dkey0 = 0xfffb; - Dkey1 = 0xfff3; - } - else if (tk0_7){ - Dkey0 = 0xfff7; - Dkey1 = 0xfff3; - } - else if (tk1_0){ - Dkey0 = 0xffef; - Dkey1 = 0xffcf; - } - else if (tk1_3){ - Dkey0 = 0xffdf; - Dkey1 = 0xffcf; - } - else if (tk1_4){ - Dkey0 = 0xffbf; - Dkey1 = 0xff3f; - } - else if (tk1_7){ - Dkey0 = 0xff7f; - Dkey1 = 0xff3f; - } - else if (tk2_0){ - Dkey0 = 0xfeff; - Dkey1 = 0xfcff; - } - else if (tk2_3){ - Dkey0 = 0xfdff; - Dkey1 = 0xfcff; - } - else if (tk2_4){ - Dkey0 = 0xfbff; - Dkey1 = 0xf3ff; - } - else if (tk2_7){ - Dkey0 = 0xf7ff; - Dkey1 = 0xf3ff; - } - - } - else if(sinImpulse) - { - if(tk0_0){ - Dkey0 = 0xfff6; - Dkey1 = 0xfff0; - Dkey2 = 0xfff9; - } - else if(tk0_7){ - Dkey0 = 0xfff9; - Dkey1 = 0xfff0; - Dkey2 = 0xfff6; - } - else if(tk1_0){ - Dkey0 = 0xffde; - Dkey1 = 0xffcc; - Dkey2 = 0xffed; - } - else if(tk0_4){ - Dkey0 = 0xffed; - Dkey1 = 0xffcc; - Dkey2 = 0xffde; - } - else if(tk1_4){ - Dkey0 = 0xffdb; - Dkey1 = 0xffc3; - Dkey2 = 0xffe7; - } - else if(tk1_7){ - Dkey0 = 0xffe7; - Dkey1 = 0xffc3; - Dkey2 = 0xffdb; - } - } - else - { - key0 = !(((tk0_0 == 1) && (tk0_1 == 1) && (tk0_2 == 0) && (tk0_3 == 0)) || - ((tk0_0 == 0) && (tk0_1 == 1) && (tk0_2 == 1) && (tk0_3 == 0))); - - key1 = !(((tk0_0 == 0) && (tk0_1 == 1) && (tk0_2 == 1) && (tk0_3 == 0)) || - ((tk0_0 == 0) && (tk0_1 == 0) && (tk0_2 == 1) && (tk0_3 == 1))); - - key2 = !(((tk0_4 == 1) && (tk0_5 == 1) && (tk0_6 == 0) && (tk0_7 == 0)) || - ((tk0_4 == 0) && (tk0_5 == 1) && (tk0_6 == 1) && (tk0_7 == 0))); - - key3 = !(((tk0_4 == 0) && (tk0_5 == 1) && (tk0_6 == 1) && (tk0_7 == 0)) || - ((tk0_4 == 0) && (tk0_5 == 0) && (tk0_6 == 1) && (tk0_7 == 1))); - - key4 = !(((tk1_0 == 1) && (tk1_1 == 1) && (tk1_2 == 0) && (tk1_3 == 0)) || - ((tk1_0 == 0) && (tk1_1 == 1) && (tk1_2 == 1) && (tk1_3 == 0))); - - key5 = !(((tk1_0 == 0) && (tk1_1 == 1) && (tk1_2 == 1) && (tk1_3 == 0)) || - ((tk1_0 == 0) && (tk1_1 == 0) && (tk1_2 == 1) && (tk1_3 == 1))); - - key6 = !(((tk1_4 == 1) && (tk1_5 == 1) && (tk1_6 == 0) && (tk1_7 == 0)) || - ((tk1_4 == 0) && (tk1_5 == 1) && (tk1_6 == 1) && (tk1_7 == 0))); - - key7 = !(((tk1_4 == 0) && (tk1_5 == 1) && (tk1_6 == 1) && (tk1_7 == 0)) || - ((tk1_4 == 0) && (tk1_5 == 0) && (tk1_6 == 1) && (tk1_7 == 1))); - - key8 = !(((tk2_0 == 1) && (tk2_1 == 1) && (tk2_2 == 0) && (tk2_3 == 0)) || - ((tk2_0 == 0) && (tk2_1 == 1) && (tk2_2 == 1) && (tk2_3 == 0))); - - key9 =!(((tk2_0 == 0) && (tk2_1 == 1) && (tk2_2 == 1) && (tk2_3 == 0)) || - ((tk2_0 == 0) && (tk2_1 == 0) && (tk2_2 == 1) && (tk2_3 == 1))); - - key10 = !(((tk2_4 == 1) && (tk2_5 == 1) && (tk2_6 == 0) && (tk2_7 == 0)) || - ((tk2_4 == 0) && (tk2_5 == 1) && (tk2_6 == 1) && (tk2_7 == 0))); - - key11 = !(((tk2_4 == 0) && (tk2_5 == 1) && (tk2_6 == 1) && (tk2_7 == 0)) || - ((tk2_4 == 0) && (tk2_5 == 0) && (tk2_6 == 1) && (tk2_7 == 1))); - - break1 = !tk3_1; - break2 = !tk3_2; - break3 = !tk3_3; - break4 = !tk3_4; - - Dkey0 &= ((break4 << 15)|(break3 << 14)|(break2 << 13)|(break1 << 12)| (key11 << 11) | (key10 << 10) | (key9 << 9) | (key8 << 8)| (key7 << 7)| (key6 << 6)| (key5 << 5)| (key4 << 4)| (key3 << 3)| (key2 << 2)| (key1 << 1)| (key0 << 0)); - - } - if(doubleImpulse) - test_double_impulse(Dkey0, Dkey1, p2, pM, pL, soft_off_enable, soft_on_enable); - else if(sinImpulse) - test_sin_impulse(Dkey0, Dkey1, Dkey2, p2, pM); - else - test_impulse(Dkey0,p2); - - // - start_eva_timer2(); - IER |= M_INT9; //start CAN -#if (XPWMGEN==1) - i_WriteMemory(ADR_PWM_DIRECT,0xffff); - i_WriteMemory(ADR_PWM_DRIVE_MODE, 0); -#endif - return; -} diff --git a/Inu/Src2/551/main/tk_Test.h b/Inu/Src2/551/main/tk_Test.h deleted file mode 100644 index 2a44453..0000000 --- a/Inu/Src2/551/main/tk_Test.h +++ /dev/null @@ -1,12 +0,0 @@ - -#ifndef TK_TEST_H -#define TK_TEST_H - - - -void test_tk_ak_one_impulse(int tk0, int tk1, int tk2, int tk3, int period, int periodMiddle, int periodLast, int doubleImpulse, int sinImpulse, int soft_off_enable, int soft_on_enable); -void test_double_impulse(unsigned int impulse_channel_1,unsigned int impulse_channel_2,long impulse_time,long middle_impulse_time, long last_impulse_time, int soft_off_enable, int soft_on_enable); - - -#endif - diff --git a/Inu/Src2/551/main/ukss_tools.c b/Inu/Src2/551/main/ukss_tools.c deleted file mode 100644 index 097fc1e..0000000 --- a/Inu/Src2/551/main/ukss_tools.c +++ /dev/null @@ -1,605 +0,0 @@ -/* - * ukss_tools.c - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - - - -#include - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include -#include "adc_tools.h" -#include "CAN_project.h" -#include "CAN_Setup.h" -#include "global_time.h" -#include "v_rotor.h" -#include "ukss_tools.h" -#include "control_station_project.h" -#include "control_station.h" -#include "sync_tools.h" - - -#pragma DATA_SECTION(Unites2VPU, ".slow_vars") -int Unites2VPU[SIZE_UNITS_OUT]={0}; - -#pragma DATA_SECTION(Unites2Zadat4ik, ".slow_vars") -int Unites2Zadat4ik[SIZE_UNITS_OUT]={0}; - -#pragma DATA_SECTION(Unites2BKSSD, ".slow_vars") -int Unites2BKSSD[SIZE_UNITS_OUT]={0}; - -#pragma DATA_SECTION(Unites2UMU, ".slow_vars") -int Unites2UMU[SIZE_UNITS_OUT]={0}; - - - -void edrk_clear_cmd_ukss(void) -{ - int i; - - for (i=0;i - -#include -#include -#include -#include -#include -#include -#include "IQmathLib.h" -#include "mathlib.h" -#include "CAN_Setup.h" -#include "uom_tools.h" - - -#pragma DATA_SECTION(uom_levels, ".slow_vars") -int uom_levels[9] = {0, 0, 15, 30, 45, 60, 75, 90, 100}; -#pragma DATA_SECTION(iq_uom_levels, ".slow_vars") -_iq iq_uom_levels[9] = {_IQ(1.0), _IQ(1.0), _IQ(0.85), _IQ(0.7), _IQ(0.55), _IQ(0.4), _IQ(0.25), _IQ(0.1), _IQ(0.016)}; -void update_uom(void) -{ -// int index; - static _iq max_nominal_power = _IQ(MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); - static _iq super_max_nominal_power = _IQ(SUPER_MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); - - static unsigned int prev_CAN_count_cycle_input_units = 0, c_data = 0; - unsigned int cur_can_cycle; - - static FROM_ZADAT4IK zad = {0}, zad_w = {0}; - static unsigned int temp_code = 0 , temp_code1 = 0, temp_code2 = 0, temp_code3 = 0; - - - - cur_can_cycle = unites_can_setup.CAN_count_cycle_input_units[0]; - - if (prev_CAN_count_cycle_input_units != cur_can_cycle) - { - zad = edrk.from_zadat4ik; - - temp_code = (zad.bits.UOM_READY_ACTIVE & 0x1) + - (zad.bits.UOM_LIMIT_1 & 0x1) << 1 + - (zad.bits.UOM_LIMIT_2 & 0x1) << 2 + - (zad.bits.UOM_LIMIT_3 & 0x1) << 3 ; - - if (c_data == 0) - { - temp_code1 = temp_code; - c_data = 1; - } - else - if (c_data == 1) - { - temp_code2 = temp_code; - c_data = 2; - } - else - if (c_data == 2) - { - temp_code3 = temp_code; - c_data = 0; - } - - } - - prev_CAN_count_cycle_input_units = cur_can_cycle; - - if ((temp_code1 == temp_code2) && (temp_code2 == temp_code3)) - zad_w = zad; - - - edrk.from_uom.digital_line.bits.ready = zad_w.bits.UOM_READY_ACTIVE; - edrk.from_uom.digital_line.bits.level0 = zad_w.bits.UOM_LIMIT_1; - edrk.from_uom.digital_line.bits.level1 = zad_w.bits.UOM_LIMIT_2; - edrk.from_uom.digital_line.bits.level2 = zad_w.bits.UOM_LIMIT_3; - - - if (edrk.from_uom.digital_line.bits.ready && edrk.disable_uom==0) - { - edrk.from_uom.ready = 1; -//000 - 100 - if (edrk.from_uom.digital_line.bits.level0 == 0 && - edrk.from_uom.digital_line.bits.level1 == 0 && - edrk.from_uom.digital_line.bits.level2 == 0 ) - edrk.from_uom.code = 1; -//001 - 85 - if (edrk.from_uom.digital_line.bits.level0 == 1 && - edrk.from_uom.digital_line.bits.level1 == 0 && - edrk.from_uom.digital_line.bits.level2 == 0 ) - edrk.from_uom.code = 2; -//011 - 70 - if (edrk.from_uom.digital_line.bits.level0 == 1 && - edrk.from_uom.digital_line.bits.level1 == 1 && - edrk.from_uom.digital_line.bits.level2 == 0 ) - edrk.from_uom.code = 3; -//010 - 55 - if (edrk.from_uom.digital_line.bits.level0 == 0 && - edrk.from_uom.digital_line.bits.level1 == 1 && - edrk.from_uom.digital_line.bits.level2 == 0 ) - edrk.from_uom.code = 4; -//110 - 40 - if (edrk.from_uom.digital_line.bits.level0 == 0 && - edrk.from_uom.digital_line.bits.level1 == 1 && - edrk.from_uom.digital_line.bits.level2 == 1 ) - edrk.from_uom.code = 5; -//111 - 25 - if (edrk.from_uom.digital_line.bits.level0 == 1 && - edrk.from_uom.digital_line.bits.level1 == 1 && - edrk.from_uom.digital_line.bits.level2 == 1 ) - edrk.from_uom.code = 6; -//101 - 10 - if (edrk.from_uom.digital_line.bits.level0 == 1 && - edrk.from_uom.digital_line.bits.level1 == 0 && - edrk.from_uom.digital_line.bits.level2 == 1 ) - edrk.from_uom.code = 7; -//100 - 0 - if (edrk.from_uom.digital_line.bits.level0 == 0 && - edrk.from_uom.digital_line.bits.level1 == 0 && - edrk.from_uom.digital_line.bits.level2 == 1 ) - edrk.from_uom.code = 8; - } - else - { - edrk.from_uom.ready = 0; - edrk.from_uom.code = 0; - } - - edrk.from_uom.iq_level_value = iq_uom_levels[edrk.from_uom.code]; - - if (edrk.from_uom.code<=1) - edrk.from_uom.iq_level_value_kwt = super_max_nominal_power; - else - edrk.from_uom.iq_level_value_kwt = _IQmpy(max_nominal_power, - edrk.from_uom.iq_level_value); - - edrk.from_uom.level_value = uom_levels[edrk.from_uom.code]; - - - -} - -////////////////////////////////////////////////////////// diff --git a/Inu/Src2/551/main/uom_tools.h b/Inu/Src2/551/main/uom_tools.h deleted file mode 100644 index 134648f..0000000 --- a/Inu/Src2/551/main/uom_tools.h +++ /dev/null @@ -1,15 +0,0 @@ -/* - * uom_tools.h - * - * Created on: 13 . 2024 . - * Author: Evgeniy_Sokolov - */ - -#ifndef SRC_MAIN_UOM_TOOLS_H_ -#define SRC_MAIN_UOM_TOOLS_H_ - - -void update_uom(void); - - -#endif /* SRC_MAIN_UOM_TOOLS_H_ */ diff --git a/Inu/Src2/551/main/v_pwm24_v2.c b/Inu/Src2/551/main/v_pwm24_v2.c deleted file mode 100644 index 147c2c9..0000000 --- a/Inu/Src2/551/main/v_pwm24_v2.c +++ /dev/null @@ -1,948 +0,0 @@ -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "IQmathLib.h" - -#include -#include -#include -#include -#include -#include - -#include "rmp_cntl_v1.h" -#include "svgen_mf.h" -#include "uf_alg_ing.h" -#include "vhzprof.h" -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" -#include "MemoryFunctions.h" -#include "Spartan2E_Adr.h" -#include "TuneUpPlane.h" -#include "x_wdog.h" -#include "xp_write_xpwm_time.h" - - - - - - - - -// xilinx (60000000 / 16 / FREQ_PWM = 3750000 / FREQ_PWM) -//#pragma DATA_SECTION(var_freq_pwm_xtics,".fast_vars1"); -//int var_freq_pwm_xtics = DEF_FREQ_PWM_XTICS; - -// xilinx -//#pragma DATA_SECTION(var_period_max_xtics,".fast_vars1"); -//int var_period_max_xtics = DEF_FREQ_PWM_XTICS - DEF_PERIOD_MIN_XTICS; - -// xilinx (mintime+deadtime) (F * T.. = (60 / 16 / 2) * T = (60 * T / 16 / 2)) -//#pragma DATA_SECTION(var_period_min_xtics,".fast_vars1"); -//int var_period_min_xtics = DEF_PERIOD_MIN_XTICS;// - -// xilinx (mintime) (F * T.. = (60 / 16 / 2) * T = (60 * T / 16 / 2)) -#pragma DATA_SECTION(var_period_min_br_xtics,".fast_vars1"); -int var_period_min_br_xtics = DEF_PERIOD_MIN_BR_XTICS;// - - - - -#define IQ_ALFA_SATURATION1 15099494//16441671//15099494 -#define IQ_ALFA_SATURATION2 1677721//16441671//15099494 - - -#define PI 3.1415926535897932384626433832795 - -//#pragma DATA_SECTION(iq_alfa_coef,".fast_vars"); -//_iq iq_alfa_coef = 16777216; - - -//#pragma DATA_SECTION(pidCur_Ki,".fast_vars"); -//_iq pidCur_Ki = 0; - -//#pragma DATA_SECTION(ar_tph,".fast_vars"); -//_iq ar_tph[7]; - -//#pragma DATA_SECTION(winding_displacement,".fast_vars"); -//_iq winding_displacement = CONST_IQ_PI6; - - - -#pragma DATA_SECTION(svgen_pwm24_1,".v_24pwm_vars"); -SVGEN_PWM24 svgen_pwm24_1 = SVGEN_PWM24_DEFAULTS; -#pragma DATA_SECTION(svgen_pwm24_2,".v_24pwm_vars"); -SVGEN_PWM24 svgen_pwm24_2 = SVGEN_PWM24_DEFAULTS; - - - - -ALG_PWM24 alg_pwm24 = ALG_PWM24_DEFAULTS; - - - - - - - -#pragma CODE_SECTION(start_PWM24,".fast_run2") -void start_PWM24(int O1, int O2) -{ - if ((O1 == 1) && (O2 == 1)) - { - soft_start_x24_pwm_1_2(); - } - else - { - if ((O1 == 0) && (O2 == 1)) - { - soft_start_x24_pwm_2(); - } - if ((O1 == 1) && (O2 == 0)) - { - soft_start_x24_pwm_1(); - } - } -} - - - - -void InitPWM_Variables(int n_pch) -{ - -// init_DQ_pid(); -// break_resistor_managment_init(); - - -///////////// - - -////////////// -// a.k = 0; -// a.k1 = 0; -// a.k2 = 0; - - alg_pwm24.k1 = 0; - alg_pwm24.k2 = 0; - - alg_pwm24.freq1 = 0; - -/////////////////// - - -/////////////////// - svgen_pwm24_1.prev_level = V_PWM24_PREV_PWM_CLOSE; - svgen_pwm24_1.saw_direct.all = xpwm_time.saw_direct.all & 0x3f; - svgen_pwm24_1.Tclosed_saw_direct_0 = xpwm_time.Tclosed_saw_direct_0;// xpwm_time.Tclosed_high;//var_freq_pwm_xtics + 1; - svgen_pwm24_1.Tclosed_saw_direct_1 = xpwm_time.Tclosed_saw_direct_1; - svgen_pwm24_1.Tclosed_high = xpwm_time.Tclosed_high; - - svgen_pwm24_2.prev_level = V_PWM24_PREV_PWM_CLOSE; - svgen_pwm24_2.saw_direct.all = (xpwm_time.saw_direct.all >> 6) & 0x3f; - svgen_pwm24_2.Tclosed_saw_direct_0 = xpwm_time.Tclosed_saw_direct_0;// xpwm_time.Tclosed_high;//var_freq_pwm_xtics + 1; - svgen_pwm24_2.Tclosed_saw_direct_1 = xpwm_time.Tclosed_saw_direct_1; - svgen_pwm24_2.Tclosed_high = xpwm_time.Tclosed_high; - - - svgen_pwm24_1.XilinxFreq = CONST_IQ_1 / xpwm_time.Tclosed_high;//(var_freq_pwm_xtics + 1); - svgen_pwm24_2.XilinxFreq = svgen_pwm24_1.XilinxFreq; - - svgen_pwm24_1.number_svgen = 1; - svgen_pwm24_2.number_svgen = 2; - - // pwm_minimal_impuls_zero = DEF_PERIOD_MIN_XTICS_80; - - svgen_pwm24_1.pwm_minimal_impuls_zero_minus = (float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0;// DEF_PERIOD_MIN_XTICS_100;//DEF_PERIOD_MIN_XTICS_80; - svgen_pwm24_1.pwm_minimal_impuls_zero_plus = (float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0;// DEF_PERIOD_MIN_XTICS_80; - - svgen_pwm24_2.pwm_minimal_impuls_zero_minus = svgen_pwm24_1.pwm_minimal_impuls_zero_minus; - svgen_pwm24_2.pwm_minimal_impuls_zero_plus = svgen_pwm24_1.pwm_minimal_impuls_zero_plus; - - - if (n_pch==0) - { - svgen_pwm24_1.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_CBA; - svgen_pwm24_2.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_CBA; - } - else - { -// svgen_pwm24_1.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_ACB; // -// svgen_pwm24_2.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_ACB; - svgen_pwm24_1.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_BAC; - svgen_pwm24_2.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_BAC; - } - - - InitVariablesSvgen_Ing(xpwm_time.freq_pwm); -} - - - - - - - - - - -void InitXPWM(unsigned int freq_pwm) -{ - int i; - unsigned int pwm_t;//, freq_pwm_xtics; - - - - - pwm_t = (FREQ_INTERNAL_GENERATOR_XILINX_TMS / freq_pwm ); - // freq_pwm_xtics = (FREQ_INTERNAL_GENERATOR_XILINX_TMS / freq_pwm ); - -// write init pwm -// , , - xpwm_time.Tclosed_saw_direct_1 = pwm_t + 2;//1; // =1 , - xpwm_time.Tclosed_saw_direct_0 = 0; // =0 , - - xpwm_time.Tclosed_high = pwm_t + 2;//1; - - // - // " =0x0 - // SAW_DIRECTbit = 0 > =0 - // - // SAW_DIRECTbit = 1 <= =0 - // - xpwm_time.saw_direct.all = 0x0555; - - -// - xpwm_time.pwm_tics = pwm_t; - xpwm_time.freq_pwm = freq_pwm; - xpwm_time.half_pwm_tics = xpwm_time.pwm_tics >> 1; - - xpwm_time.one_or_two_interrupts_run = PWN_COUNT_RUN_PER_INTERRUPT; - xpwm_time.init(&xpwm_time); - -// write to xilinx regs - xpwm_time.write_zero_winding_break_times(&xpwm_time); - - -// - i_WriteMemory(ADR_PWM_DIRECT, 0xffff); - i_WriteMemory(ADR_PWM_DRIVE_MODE, 0); //Choose PWM sourse PWMGenerator on Spartan 200e - // DeadTime - i_WriteMemory(ADR_PWM_DEAD_TIME, 360); //Dead time in tics. - stop_wdog(); - - i_WriteMemory(ADR_PWM_PERIOD, pwm_t); // Saw period in tics. 1 tic = FREQ_INTERNAL_GENERATOR_XILINX_TMS - // - i_WriteMemory(ADR_PWM_SAW_DIRECT, xpwm_time.saw_direct.all); - //" (15 0). 0 - , 1 - . - // Xilinx, 0x2006(15) = 0, - // 0x2006(15) = 1, " - i_WriteMemory(ADR_TK_MASK_0, 0); - // " (31 16) Xilinx, 0x2006(15) = 0, - // 0x2006(15) = 1, " - i_WriteMemory(ADR_TK_MASK_1, 0xffff); //Turn off additional 16 tk lines - - - i_WriteMemory(ADR_PWM_IT_TYPE, PWN_COUNT_RUN_PER_INTERRUPT); //1 or 2 interrupt per PWM period - -// -//#if (C_PROJECT_TYPE == PROJECT_BALZAM) || (C_PROJECT_TYPE == PROJECT_23550) -// i_WriteMemory(ADR_PWM_IT_TYPE, 1); //1 interrupt per PWM period -//#else -// i_WriteMemory(ADR_PWM_IT_TYPE, 0); //interrupt on each counter twist -//#endif - -/* End f PWM Gen init */ -} - - - -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// - -/* -void init_freq_array(void) -{ - unsigned int i = 0; - //unsigned int j = 0; - int var1 = 0; - - var1 = 32767 / (FREQ_PWM_MAX - FREQ_PWM_MIN); - - for (i = 0; i < COUNT_VAR_FREQ; i++) - { - //j = rand() / 1023; - //freq_array[i] = array_optim_freq[j]; - //do - freq_array[i] = FREQ_PWM_MIN + (rand() / var1); - //while ((freq_array[i] < 945) && (freq_array[i] > 930)); - } - - //freq_array[0] = 991; - //freq_array[1] = 1430; -} -*/ - - -//#pragma CODE_SECTION(calc_freq_pwm,".v_24pwm_run"); -//#pragma CODE_SECTION(calc_freq_pwm,".fast_run"); -/*void calc_freq_pwm() -{ - static int prev_freq_pwm = 0; - static float pwm_period = 0; - static float var0 = 0; - //static int line = 0; - //static int i = 0; - static unsigned int proc_ticks = 1; - int var1 = 0; - //static int i = 0; - - if ((f.flag_change_pwm_freq == 1) && (f.flag_random_freq == 1)) - { - if (proc_ticks >= 1) - { - proc_ticks = 0; - - - if (line == 0) - { - VAR_FREQ_PWM_HZ = VAR_FREQ_PWM_HZ + 1; - if (VAR_FREQ_PWM_HZ > FREQ_PWM_MAX) - { - VAR_FREQ_PWM_HZ = FREQ_PWM_MAX; - line = 1; - } - } - else - { - VAR_FREQ_PWM_HZ = VAR_FREQ_PWM_HZ - 1; - if (VAR_FREQ_PWM_HZ < FREQ_PWM) - { - VAR_FREQ_PWM_HZ = FREQ_PWM; - line = 0; - } - } - - - - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - //VAR_FREQ_PWM_HZ = freq_array[i]; - //i_led2_on_off(1); - - var1 = 32767 / (freq_pwm_max_hz - freq_pwm_min_hz); - VAR_FREQ_PWM_HZ = freq_pwm_min_hz + (rand() / var1); - - //i_led2_on_off(0); - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - if (VAR_FREQ_PWM_HZ > freq_pwm_max_hz) - { - VAR_FREQ_PWM_HZ = freq_pwm_max_hz; - } - else - { - if (VAR_FREQ_PWM_HZ < freq_pwm_min_hz) - { - VAR_FREQ_PWM_HZ = freq_pwm_min_hz; - } - } - //i++; - - //if (i >= COUNT_VAR_FREQ) - //{ - //i = 0; - //} - - } - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - //if (VAR_FREQ_PWM_HZ == FREQ_PWM_MIN) - //{ - //VAR_FREQ_PWM_HZ = FREQ_PWM_MAX; - //} - //else - //{ - //VAR_FREQ_PWM_HZ = FREQ_PWM_MIN; - //} - - //if (f.Rele1 == 1) - //{ - //if (i == 0) - //{ - //VAR_FREQ_PWM_HZ = 1192;; - //i = 1; - //} - //else - //{ - //VAR_FREQ_PWM_HZ = 792; - //} - //} - //else - //{ - //i = 0; - //VAR_FREQ_PWM_HZ = 1192; - //} - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - } - //else - //{ - //VAR_FREQ_PWM_HZ = FREQ_PWM; - //} - - - if (prev_freq_pwm != VAR_FREQ_PWM_HZ) - { - prev_freq_pwm = VAR_FREQ_PWM_HZ; - FREQ_MAX = _IQ(2.0*PI*F_STATOR_MAX/VAR_FREQ_PWM_HZ); - - var0 = (float)VAR_FREQ_PWM_HZ; - //pwm_period = ((float64)HSPCLK) / ((float64)VAR_FREQ_PWM_HZ); - - pwm_period = HSPCLK / var0; - - pwm_period = pwm_period / 2.0; - - FREQ_PWM_XTICS = ((int) pwm_period) >> 3; - - XILINX_FREQ = 16777216/(FREQ_PWM_XTICS + 1); - - FLAG_CHANGE_FREQ_PWM = 1; - } - - proc_ticks++; -} -*/ -/* -#pragma CODE_SECTION(test_calc_pwm24_dq,".v_24pwm_run"); -void test_calc_pwm24_dq(_iq U_zad1, _iq U_zad2,_iq teta) -{ - svgen_pwm24_1.Freq = 0; - svgen_pwm24_2.Freq = 0; - - svgen_pwm24_1.Gain = U_zad1; - svgen_pwm24_2.Gain = U_zad2; - - svgen_pwm24_1.Alpha = teta; - svgen_pwm24_2.Alpha = teta; - - svgen_pwm24_1.delta_U = filter.iqU_1_fast - filter.iqU_2_fast; - svgen_pwm24_2.delta_U = filter.iqU_1_fast - filter.iqU_2_fast; -// svgen_pwm24_2.delta_U = filter.iqU_3_fast - filter.iqU_4_fast; - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - svgen_pwm24_1.delta_U = 0; - svgen_pwm24_2.delta_U = 0; - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - svgen_pwm24_1.Ia = analog.iqIu_1; - svgen_pwm24_1.Ib = analog.iqIv_1; - svgen_pwm24_1.Ic = analog.iqIw_1;; - - svgen_pwm24_2.Ia = analog.iqIu_2; - svgen_pwm24_2.Ib = analog.iqIv_2; - svgen_pwm24_2.Ic = analog.iqIw_2; - - svgen_pwm24_1.calc_dq(&svgen_pwm24_1); - svgen_pwm24_2.calc_dq(&svgen_pwm24_2); - - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - set_predel_dshim24(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - -} -*/ - -/* -#pragma CODE_SECTION(svgen_pwm24_calc,".v_24pwm_run"); -void svgen_pwm24_calc_dq(SVGEN_PWM24 *vt) -{ - - if (vt->Alpha > CONST_IQ_2PI) - { - vt->Alpha -= CONST_IQ_2PI; - } - - if (vt->Alpha < 0) - { - vt->Alpha += CONST_IQ_2PI; - } - - - calc_time_one_tk(vt->Gain, vt->Alpha, vt->delta_U, vt->Ia, vt->Ib, vt->Ic, - vt->number_svgen,vt->direct_rotor, &vt->Ta_0, &vt->Ta_1,&vt->Tb_0, &vt->Tb_1,&vt->Tc_0, &vt->Tc_1); - - - vt->Ta_0.Ti = vt->Ta_0.Ti/vt->XilinxFreq; - vt->Ta_1.Ti = vt->Ta_1.Ti/vt->XilinxFreq; - - vt->Tb_0.Ti = vt->Tb_0.Ti/vt->XilinxFreq; - vt->Tb_1.Ti = vt->Tb_1.Ti/vt->XilinxFreq; - - vt->Tc_0.Ti = vt->Tc_0.Ti/vt->XilinxFreq; - vt->Tc_1.Ti = vt->Tc_1.Ti/vt->XilinxFreq; - -} -*/ - - - - -void svgen_set_time_keys_closed(SVGEN_PWM24 *vt) -{ -//a - if (vt->saw_direct.bits.bit0) - vt->Ta_0 = vt->Tclosed_saw_direct_1; - else - vt->Ta_0 = vt->Tclosed_saw_direct_0; - - if (vt->saw_direct.bits.bit1) - vt->Ta_1 = vt->Tclosed_saw_direct_1; - else - vt->Ta_1 = vt->Tclosed_saw_direct_0; -//b - if (vt->saw_direct.bits.bit2) - vt->Tb_0 = vt->Tclosed_saw_direct_1; - else - vt->Tb_0 = vt->Tclosed_saw_direct_0; - - if (vt->saw_direct.bits.bit3) - vt->Tb_1 = vt->Tclosed_saw_direct_1; - else - vt->Tb_1 = vt->Tclosed_saw_direct_0; -//c - if (vt->saw_direct.bits.bit4) - vt->Tc_0 = vt->Tclosed_saw_direct_1; - else - vt->Tc_0 = vt->Tclosed_saw_direct_0; - - if (vt->saw_direct.bits.bit5) - vt->Tc_1 = vt->Tclosed_saw_direct_1; - else - vt->Tc_1 = vt->Tclosed_saw_direct_0; - - vt->prev_level = V_PWM24_PREV_PWM_CLOSE; - -} - - - - - -#pragma CODE_SECTION(correct_balance_uzpt_pwm24,".fast_run2"); -_iq correct_balance_uzpt_pwm24(_iq Tinput, _iq Kplus) -{ -//_iq pwm_t, timpuls_corr; - volatile _iq timpuls_corr; - - if (Tinput >= (-Kplus)) - timpuls_corr = CONST_IQ_1 - _IQdiv(CONST_IQ_1-Tinput, CONST_IQ_1+Kplus); - else - timpuls_corr = -CONST_IQ_1 + _IQdiv(CONST_IQ_1+Tinput, CONST_IQ_1-Kplus); - - - return timpuls_corr; -} - - - - - -#pragma CODE_SECTION(recalc_time_pwm_minimal_2_xilinx_pwm24,".fast_run2"); -void recalc_time_pwm_minimal_2_xilinx_pwm24(SVGEN_PWM24 *pwm24, - unsigned int *T0, unsigned int *T1, - int *T_imp, - _iq timpuls_corr ) -{ -//_iq pwm_t, timpuls_corr; - - volatile unsigned long pwm_t; - volatile unsigned int minimal_plus, minimal_minus; - - - - minimal_plus = pwm24->pwm_minimal_impuls_zero_plus; - minimal_minus = pwm24->pwm_minimal_impuls_zero_minus; - - // if (pwm24->prev_level == V_PWM24_PREV_PWM_CLOSE || pwm24->prev_level == V_PWM24_PREV_PWM_MIDDLE || pwm24->prev_level == V_PWM24_PREV_PWM_WORK_KM0) - // { - // minimal_plus *= 2; -// minimal_minus *= 2; -// } - - pwm_t = timpuls_corr / pwm24->XilinxFreq; - - *T_imp = pwm_t; - -// if (pwm_t>(pwm24->Tclosed_high-4*minimal_minus)) -// pwm_t=(pwm24->Tclosed_high-4*minimal_minus); - - - if (timpuls_corr >= 0) - { - *T0 = pwm_t + minimal_plus; - *T1 = pwm24->Tclosed_high - minimal_minus; - } - else - { - *T0 = minimal_plus; - *T1 = pwm24->Tclosed_high + pwm_t - minimal_minus; - } - - - if (*T0 < minimal_plus) - *T0 = minimal_plus; - - if (*T0 > (pwm24->Tclosed_high - 2 * minimal_plus)) - *T0 = (pwm24->Tclosed_high - 2 * minimal_plus); - - if (*T1 < (2 * minimal_minus)) - *T1 = 2 * minimal_minus; - - if (*T1 > (pwm24->Tclosed_high - minimal_minus)) - *T1 = (pwm24->Tclosed_high - minimal_minus); - -} - -#define WRITE_SWGEN_PWM_TIMES_VER 2//1 - -#if (WRITE_SWGEN_PWM_TIMES_VER==1) -#pragma CODE_SECTION(write_swgen_pwm_times,".fast_run2"); -void write_swgen_pwm_times(unsigned int mode_reload) -{ - - if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_ABC) - { - xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Ta_0; - xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Ta_1; - xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tb_0; - xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tb_1; - xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tc_0; - xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tc_1; - } - - if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_ABC) - { - xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Ta_0; - xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Ta_1; - xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tb_0; - xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tb_1; - xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tc_0; - xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tc_1; - } - - if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_BCA) - { - xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tb_0; - xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tb_1; - xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tc_0; - xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tc_1; - xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Ta_0; - xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Ta_1; - } - - if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_BCA) - { - xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tb_0; - xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tb_1; - xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tc_0; - xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tc_1; - xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Ta_0; - xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Ta_1; - } - - if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_CAB) - { - xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tc_0; - xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tc_1; - xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Ta_0; - xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Ta_1; - xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tb_0; - xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tb_1; - } - if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_CAB) - { - xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tc_0; - xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tc_1; - xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Ta_0; - xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Ta_1; - xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tb_0; - xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tb_1; - } - - // fix revers - if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_BAC) - { - xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tb_0; - xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tb_1; - xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Ta_0; - xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Ta_1; - xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tc_0; - xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tc_1; - } - if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_BAC) - { - xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tb_0; - xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tb_1; - xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Ta_0; - xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Ta_1; - xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tc_0; - xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tc_1; - } - - if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_ACB) - { - xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Ta_0; - xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Ta_1; - xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tc_0; - xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tc_1; - xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tb_0; - xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tb_1; - } - if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_ACB) - { - xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Ta_0; - xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Ta_1; - xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tc_0; - xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tc_1; - xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tb_0; - xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tb_1; - } - - if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_CBA) - { - xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tc_0; - xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tc_1; - xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tb_0; - xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tb_1; - xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Ta_0; - xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Ta_1; - } - if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_CBA) - { - xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tc_0; - xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tc_1; - xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tb_0; - xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tb_1; - xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Ta_0; - xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Ta_1; - } - - xpwm_time.Tbr0_0 = break_result_1; - xpwm_time.Tbr0_1 = break_result_2; - xpwm_time.Tbr1_0 = 0;//break_result_3; - xpwm_time.Tbr1_1 = 0;//break_result_4; - xpwm_time.mode_reload = mode_reload; - - xpwm_time.write_1_2_winding_break_times(&xpwm_time); -} -#endif -/////////////////////////////////////////////////////// -// ver 2 -/////////////////////////////////////////////////////// -#if (WRITE_SWGEN_PWM_TIMES_VER==2) - -#pragma CODE_SECTION(set_pwm_times,".fast_run2"); -void set_pwm_times(unsigned int Ta0, unsigned int Ta1, unsigned int Tb0, unsigned int Tb1, unsigned int Tc0, unsigned int Tc1, unsigned int winding_num) -{ - if (winding_num == 0) - { - xpwm_time.Ta0_0 = Ta0; - xpwm_time.Ta0_1 = Ta1; - xpwm_time.Tb0_0 = Tb0; - xpwm_time.Tb0_1 = Tb1; - xpwm_time.Tc0_0 = Tc0; - xpwm_time.Tc0_1 = Tc1; - } - else - { - xpwm_time.Ta1_0 = Ta0; - xpwm_time.Ta1_1 = Ta1; - xpwm_time.Tb1_0 = Tb0; - xpwm_time.Tb1_1 = Tb1; - xpwm_time.Tc1_0 = Tc0; - xpwm_time.Tc1_1 = Tc1; - } -} - -#pragma CODE_SECTION(process_phase_sequence,".fast_run2"); -void process_phase_sequence(SVGEN_PWM24 svgen_pwm, unsigned int winding_num) -{ - switch (svgen_pwm.phase_sequence) - { - case V_PWM24_PHASE_SEQ_NORMAL_ABC: - set_pwm_times(svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, winding_num); - break; - case V_PWM24_PHASE_SEQ_NORMAL_BCA: - set_pwm_times(svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, winding_num); - break; - case V_PWM24_PHASE_SEQ_NORMAL_CAB: - set_pwm_times(svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, winding_num); - break; - case V_PWM24_PHASE_SEQ_REVERS_BAC: - set_pwm_times(svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, winding_num); - break; - case V_PWM24_PHASE_SEQ_REVERS_ACB: - set_pwm_times(svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, winding_num); - break; - case V_PWM24_PHASE_SEQ_REVERS_CBA: - set_pwm_times(svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, winding_num); - break; - } -} -#pragma CODE_SECTION(write_swgen_pwm_times,".fast_run2"); -void write_swgen_pwm_times(unsigned int mode_reload) -{ - process_phase_sequence(svgen_pwm24_1, 0); - process_phase_sequence(svgen_pwm24_2, 1); - - // fix breaks - xpwm_time.Tbr0_0 = break_result_1; - xpwm_time.Tbr0_1 = break_result_2; - xpwm_time.Tbr1_0 = 0; // break_result_3; - xpwm_time.Tbr1_1 = 0; // break_result_4; - xpwm_time.mode_reload = mode_reload; - - xpwm_time.write_1_2_winding_break_times(&xpwm_time); -} -#endif - - - - - - - - -/////////////////////////////////////////////////////// - -void svgen_set_time_middle_keys_open(SVGEN_PWM24 *vt) -{ - //a - if (vt->saw_direct.bits.bit0) - vt->Ta_0 = vt->Tclosed_saw_direct_0; - else - vt->Ta_0 = vt->Tclosed_saw_direct_1; - - if (vt->saw_direct.bits.bit1) - vt->Ta_1 = vt->Tclosed_saw_direct_0; - else - vt->Ta_1 = vt->Tclosed_saw_direct_1; - //b - if (vt->saw_direct.bits.bit2) - vt->Tb_0 = vt->Tclosed_saw_direct_0; - else - vt->Tb_0 = vt->Tclosed_saw_direct_1; - - if (vt->saw_direct.bits.bit3) - vt->Tb_1 = vt->Tclosed_saw_direct_0; - else - vt->Tb_1 = vt->Tclosed_saw_direct_1; - //c - if (vt->saw_direct.bits.bit4) - vt->Tc_0 = vt->Tclosed_saw_direct_0; - else - vt->Tc_0 = vt->Tclosed_saw_direct_1; - - if (vt->saw_direct.bits.bit5) - vt->Tc_1 = vt->Tclosed_saw_direct_0; - else - vt->Tc_1 = vt->Tclosed_saw_direct_1; - - - vt->prev_level = V_PWM24_PREV_PWM_MIDDLE; - -/* - - vt->Ta_0 = 0; - vt->Ta_1 = vt->Tclosed;//var_freq_pwm_xtics + 1; - - vt->Tb_0 = 0; - vt->Tb_1 = vt->Tclosed;// var_freq_pwm_xtics + 1; - - vt->Tc_0 = 0; - vt->Tc_1 = vt->Tclosed;// var_freq_pwm_xtics + 1; - */ -} - -/////////////////////////////////////////////////////// -/////////////////////////////////////////////////////// -/////////////////////////////////////////////////////// - - -#pragma CODE_SECTION(detect_level_interrupt,".fast_run"); -unsigned int detect_level_interrupt(int flag_second_PCH) -{ - unsigned int curr_period1, curr_period2, curr_period0; - static unsigned int count_err_read_pwm_xilinx = 0; - - - WriteMemory(ADR_SAW_REQUEST, 0x8000); - curr_period0 = ReadMemory(ADR_SAW_VALUE); - WriteMemory(ADR_SAW_REQUEST, 0x8000); - curr_period1 = ReadMemory(ADR_SAW_VALUE); - WriteMemory(ADR_SAW_REQUEST, 0x8000); - curr_period2 = ReadMemory(ADR_SAW_VALUE); - - xpwm_time.current_period = curr_period2; - - - // ? - if (xpwm_time.current_periodcurr_period1) // - { - if ((curr_period2-curr_period1)>xpwm_time.half_pwm_tics)// - { -// xpwm_time.what_next_interrupt = 1; - // - return 1; - } - } - else// - { - if ((curr_period1-curr_period2)>xpwm_time.half_pwm_tics)// - { - // - return 1; - } - } - - // , ! - return 0; - - -} - diff --git a/Inu/Src2/551/main/v_pwm24_v2.h b/Inu/Src2/551/main/v_pwm24_v2.h deleted file mode 100644 index e0e1baa..0000000 --- a/Inu/Src2/551/main/v_pwm24_v2.h +++ /dev/null @@ -1,161 +0,0 @@ -#ifndef _V_PWM24_H -#define _V_PWM24_H - -#ifdef __cplusplus - extern "C" { -#endif - -#include "IQmathLib.h" -#include "DSP281x_Device.h" -#include "word_structurs.h" - - -#include "vhzprof.h" -#include "rmp_cntl_v1.h" - -enum { V_PWM24_PREV_PWM_CLOSE = 1, - V_PWM24_PREV_PWM_MIDDLE, - V_PWM24_PREV_PWM_WORK_KM0, - V_PWM24_PREV_PWM_WORK -}; - -enum { V_PWM24_PHASE_SEQ_NORMAL_ABC = 1, - V_PWM24_PHASE_SEQ_NORMAL_BCA, - V_PWM24_PHASE_SEQ_NORMAL_CAB, - V_PWM24_PHASE_SEQ_REVERS_ACB, - V_PWM24_PHASE_SEQ_REVERS_CBA, - V_PWM24_PHASE_SEQ_REVERS_BAC -}; - - - - -typedef struct { _iq freq1; - _iq k1; - _iq k2; - } ALG_PWM24; - -typedef ALG_PWM24 *ALG_PWM24_handle; - - -#define ALG_PWM24_DEFAULTS {0,0,0} -//////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////// - - -typedef struct { // _iq Gain; // Input: reference gain voltage (pu) - //_iq Offset; // Input: reference offset voltage (pu) -// _iq Freq; // Input: reference frequency (pu) -// _iq FreqMax; // Parameter: Maximum step angle = 6*base_freq*T (pu) -// _iq Alpha; // History: Sector angle (pu) - //_iq Full_Alpha; - //_iq NewEntry; // History: Sine (angular) look-up pointer (pu) -// _iq delta_U; -// _iq delta_t; - //int16 Periodmax; - //int16 PeriodMin; - unsigned int XilinxFreq; // Xilinx freq in TIC - - unsigned int pwm_minimal_impuls_zero_plus; - unsigned int pwm_minimal_impuls_zero; - unsigned int pwm_minimal_impuls_zero_minus; - - WORD_UINT2BITS_STRUCT saw_direct; - - //int region; - //Uint32 SectorPointer; // History: Sector number (Q0) - independently with global Q - //PIDREG3 delta_t; -// _iq Ia; -// _iq Ib; -// _iq Ic; - unsigned int number_svgen; - unsigned int phase_sequence; // - int prev_level; // , middle close - unsigned int Tclosed_high; - unsigned int Tclosed_saw_direct_0; - unsigned int Tclosed_saw_direct_1; - - - unsigned int Ta_0; - unsigned int Ta_1; - - int Ta_imp; - - unsigned int Tb_0; - unsigned int Tb_1; - - int Tb_imp; - - unsigned int Tc_0; - unsigned int Tc_1; - - int Tc_imp; - -// void (*calc)(); // Pointer to calculation function -// void (*calc_dq)(); // Pointer to calculation function which don`t calculate angle from freq - } SVGEN_PWM24; - -typedef SVGEN_PWM24 *SVGEN_PWM24_handle; - - -//#define SVGEN_PWM24_TIME_DEFAULTS { 0,0,0,0 } - - -#define SVGEN_PWM24_DEFAULTS { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} -// (void (*)(unsigned int))svgen_pwm24_calc } - -//extern int ar_sa_a[3][4][7]; - -extern SVGEN_PWM24 svgen_pwm24_1; -extern SVGEN_PWM24 svgen_pwm24_2; - -extern _iq pidCur_Kp; -extern _iq pidCur_Ki; - -extern _iq iq_alfa_coef; - -extern _iq iq_koef_mod_korrect_1; -extern _iq iq_koef_mod_korrect_2; - - - -void write_swgen_pwm_times(unsigned int mode_reload); - -//void change_freq_pwm(_iq FreqMax, int freq_pwm_xtics, _iq XilinxFreq); - -unsigned int detect_level_interrupt(int flag_second_PCH); - - -void svgen_set_time_keys_closed(SVGEN_PWM24 *vt); -void svgen_set_time_middle_keys_open(SVGEN_PWM24 *vt); - - -_iq correct_balance_uzpt_pwm24(_iq Tinput, _iq Kplus); - -void recalc_time_pwm_minimal_2_xilinx_pwm24(SVGEN_PWM24 *pwm24, - unsigned int *T0, unsigned int *T1, - int *T_imp, - _iq timpuls_corr ); - - - -//////////////////////////////////////////////////////////// - -void InitXPWM(unsigned int freq_pwm); - -void start_PWM24(int O1, int O2); - -void InitPWM_Variables(int n_pch); - -////////////////////////////////////////////// - -extern ALG_PWM24 alg_pwm24; -extern RMP_V1 rmp_freq; -extern VHZPROF vhz1; - - -#ifdef __cplusplus - } -#endif - -#endif /* _V_PWM24_H */ diff --git a/Inu/Src2/551/main/v_rotor.c b/Inu/Src2/551/main/v_rotor.c deleted file mode 100644 index 1da2a2f..0000000 --- a/Inu/Src2/551/main/v_rotor.c +++ /dev/null @@ -1,1095 +0,0 @@ -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "IQmathLib.h" - -#include -#include - -#include "filter_v1.h" -#include "xp_cds_in.h" -#include "xp_inc_sensor.h" -#include "xp_project.h" -#include "params.h" -#include "pwm_test_lines.h" -#include "params_norma.h" -#include "mathlib.h" #include "params_alg.h" - - -#pragma DATA_SECTION(WRotor,".fast_vars"); -WRotorValues WRotor = WRotorValues_DEFAULTS; - -#if (SENSOR_ALG==SENSOR_ALG_23550) - -#pragma DATA_SECTION(WRotorPBus,".slow_vars"); -WRotorValuesAngle WRotorPBus = WRotorValuesAngle_DEFAULTS; - -#pragma DATA_SECTION(rotor_error_update_count,".fast_vars"); -unsigned int rotor_error_update_count = 0; - - -#define SIZE_BUF_SENSOR_LOGS 32 -#pragma DATA_SECTION(sensor_1_zero,".slow_vars"); -unsigned int sensor_1_zero[6+4+8][SIZE_BUF_SENSOR_LOGS], count_sensor_1_zero=0; - -#endif - -_iq koefW = _IQ(0.05); //0.05 -_iq koefW2 = _IQ(0.01); //0.05 -_iq koefW3 = _IQ(0.002); //0.05 - - - - - - - -#if (SENSOR_ALG==SENSOR_ALG_23550) -/////////////////////////////////////////////////////////////// -void rotorInit(void) -{ - WRotorPBus.ModeAutoDiscret = 1; -} - - - -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -#define MAX_COUNT_OVERFULL_DISCRET 2250 -#define MAX_DIRECTION 4000 -#define MAX_DIRECTION_2 2000 -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -void RotorDirectionFilter(int RotorDirectionIn, int *RotorDirectionOut, int *RotorDirectionOut2, int *count_direction) -{ - -// static int count_direction = 0; -// static int count_direction_minus = 0; - - - if (RotorDirectionIn==0) - { - if (*count_direction>0) (*count_direction)--; - if (*count_direction<0) (*count_direction)++; -// if (count_direction_minus>0) count_direction_minus--; - } - else - if (RotorDirectionIn>0) - { - if (*count_direction0) count_direction_minus--; - } - else - { - if (*count_direction>-MAX_DIRECTION) (*count_direction)--; -// if (count_direction_plus>0) count_direction_plus--; - } - - - if (RotorDirectionIn==0) - *RotorDirectionOut = 0; - else - if (RotorDirectionIn>0) - *RotorDirectionOut = 1; - else - *RotorDirectionOut = -1; - - - if (*count_direction>MAX_DIRECTION_2) - *RotorDirectionOut2 = 1; - else - if (*count_direction<-MAX_DIRECTION_2) - *RotorDirectionOut2 = -1; - else - *RotorDirectionOut2 = 0; - - - -} -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -#define LEVEL_VALUE_SENSOR_OVERFULL 65535 -#define MAX_COUNT_ERROR_ANALISATOR_SENSOR_PBUS 4000 -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(AnalisatorRotorSensorPBus,".fast_run"); -int AnalisatorRotorSensorPBus(_iq d1, _iq d2, unsigned int *count_overfull_discret, unsigned int *count_zero_discret, _iq *prev_iqTimeRotor, - unsigned int *discret_out, unsigned int discret_in, _iq *iqWRotorCalcBeforeRegul, _iq *iqWRotorCalc, - int modeS1, int modeS2, - int valid_sensor_direct, int valid_sensor_90, - unsigned int *error_count ) -{ - int flag_not_ready_rotor, flag_overfull_rotor; - _iq iqTimeRotor; - // discret0 = 2 mks -// static long long KoefNorm_discret0 = 409600000LL;//((500 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - static long long KoefNorm_discret0 = 102400000LL;//((500 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - // discret1 = 20 ns -// static long long KoefNorm_discret1 = 40960000000LL;//((50 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - static long long KoefNorm_discret1 = 10240000000LL;//((50 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - -// _iq iqWRotorSumm;//,iqWRotorCalc; - - static _iq time_level_discret_1to0 = 60000 ;//682666; // KoefNorm_discret1/60000 = 0.813801288604736328125 . - static _iq time_level_discret_0to1 = 400;//204800; // KoefNorm_discret0/2000 = 0.244140625 . - static unsigned int discret; - - - if (valid_sensor_direct == 0) - d1 = 0; - if (valid_sensor_90 == 0) - d2 = 0; - - -// - , . - if (valid_sensor_direct == 0 && valid_sensor_90 == 0) - { - if (*error_count>1; - - - -// max OVERFULL - if (flag_overfull_rotor) - { - if (*count_overfull_discret0) - (*count_overfull_discret)--; - } - -// zero? - if (flag_not_ready_rotor) - { - if (*count_zero_discret0) - (*count_zero_discret)--; - } - -// real zero? - if (*count_zero_discret==MAX_COUNT_OVERFULL_DISCRET) - { - // , ! - iqWRotorCalc = 0; - *prev_iqTimeRotor = 0; - iqTimeRotor = 0; - } - else - { - // , prev_iqTimeRotor - if (iqTimeRotor==0) - iqTimeRotor = *prev_iqTimeRotor; - } - *prev_iqTimeRotor = iqTimeRotor; - - - -// - if (WRotorPBus.ModeAutoDiscret==1) - { - if ( (*count_overfull_discret==MAX_COUNT_OVERFULL_DISCRET) || (iqTimeRotor==0) ) - { - // , =0 - // discret_out = 0 - if (discret_in == 1) // discret? - { - // discret =1, 0. - *discret_out = 0; - *count_overfull_discret = 0; // ! - } - - } - else - { - // . discret==0 ... - if (discret==0 && iqTimeRotortime_level_discret_1to0 && iqTimeRotor!=65535) - *discret_out = 0; - } - } - - if (WRotorPBus.ModeAutoDiscret==2) - { - *discret_out = 0; - } - - if (WRotorPBus.ModeAutoDiscret==3) - { - *discret_out = 1; - } - - if ( (*count_overfull_discret==MAX_COUNT_OVERFULL_DISCRET) ) - { - // 0, .. ! - *prev_iqTimeRotor = iqTimeRotor = 0; - } - - - - - if ((iqTimeRotor != 0)) // && (WRotorPBus.iqTimeRotor<65535) - { - if (discret==0) - *iqWRotorCalcBeforeRegul = KoefNorm_discret0 / iqTimeRotor; - if (discret==1) - *iqWRotorCalcBeforeRegul = KoefNorm_discret1 / iqTimeRotor; - - *iqWRotorCalc = exp_regul_iq(koefW, *iqWRotorCalc, *iqWRotorCalcBeforeRegul); - } - else - { - *iqWRotorCalc = 0; - *iqWRotorCalcBeforeRegul = 0; - } - - -// if (*iqWRotorCalc == 0) -// *RotorDirection = 0; - - - return 0; - -} -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////// - - -#pragma CODE_SECTION(RotorMeasurePBus,".fast_run"); -void RotorMeasurePBus(void) -{ - // discret0 = 2 mks -// static long long KoefNorm_discret0 = 409600000LL;//((500 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - static long long KoefNorm_discret0 = 102400000LL;//((500 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - // discret1 = 20 ns -// static long long KoefNorm_discret1 = 40960000000LL;//((50 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - static long long KoefNorm_discret1 = 10240000000LL;//((50 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - - static _iq time_level_discret_1to0 = 60000 ;//682666; // KoefNorm_discret1/60000 = 0.813801288604736328125 . - static _iq time_level_discret_0to1 = 400;//204800; // KoefNorm_discret0/2000 = 0.244140625 . - - static long long KoefNorm_angle = 16384LL; //2^24/1024 -// volatile float MyVar0 = 0; - - unsigned int MyVar3 = 0; -// int direction1 = 0, direction2 = 0; - volatile unsigned int discret; - - static unsigned int discret_out1, discret_out2; - - static int count_full_oborots = 0; - static unsigned int count_overfull_discret1 = 0; - static unsigned int count_zero_discret1 = 0; - static unsigned int count_overfull_discret2 = 0; - static unsigned int count_zero_discret2 = 0; - - static unsigned int count_discret_to_1 = 0; - static unsigned int count_discret_to_0 = 0; - - static unsigned int c_error_pbus_1 = 0; - static unsigned int c_error_pbus_2 = 0; - - - static _iq prev_iqTimeRotor1 = 0, prev_iqTimeRotor2 = 0; - - _iq iqWRotorSumm = 0; - - int flag_not_ready_rotor1, flag_overfull_rotor1; - int flag_not_ready_rotor2, flag_overfull_rotor2; - - //i_led1_on_off(1); - - - - flag_not_ready_rotor1 = 0; - flag_overfull_rotor1 = 0; - flag_not_ready_rotor2 = 0; - flag_overfull_rotor2 = 0; - - - - discret = project.cds_in[0].read.sbus.enabled_channels.bit.discret; - if (project.cds_in[0].read.sbus.enabled_channels.bit.discret != project.cds_in[0].write.sbus.enabled_channels.bit.discret) - discret = 2; - - if (project.cds_in[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - { - sensor_1_zero[0][count_sensor_1_zero] = project.cds_in[0].read.pbus.Time_since_zero_point_S1; - sensor_1_zero[1][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S1; - sensor_1_zero[2][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S1; - sensor_1_zero[3][count_sensor_1_zero] = project.cds_in[0].read.pbus.Time_since_zero_point_S2; - sensor_1_zero[4][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S2; - sensor_1_zero[5][count_sensor_1_zero] = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S2; - } - sensor_1_zero[6][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS1_cnt; - sensor_1_zero[7][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS1_cnt90; - sensor_1_zero[8][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS2_cnt; - sensor_1_zero[9][count_sensor_1_zero] = project.cds_in[0].read.pbus.SpeedS2_cnt90; - - sensor_1_zero[10][count_sensor_1_zero] = inc_sensor.data.Time1; - sensor_1_zero[11][count_sensor_1_zero] = inc_sensor.data.Impulses1; - sensor_1_zero[12][count_sensor_1_zero] = inc_sensor.data.CountZero1; - sensor_1_zero[13][count_sensor_1_zero] = inc_sensor.data.CountOne1; - - sensor_1_zero[14][count_sensor_1_zero] = inc_sensor.data.Time2; - sensor_1_zero[15][count_sensor_1_zero] = inc_sensor.data.Impulses2; - sensor_1_zero[16][count_sensor_1_zero] = inc_sensor.data.CountZero2; - sensor_1_zero[17][count_sensor_1_zero] = inc_sensor.data.CountOne2; - - count_sensor_1_zero++; - if (count_sensor_1_zero>=SIZE_BUF_SENSOR_LOGS) - { - count_sensor_1_zero = 0; - count_full_oborots++; - if (count_full_oborots>3) - count_full_oborots = 0; - } -/* - if (count_sensor_1_zero==904) - { - discret = 3; - } -*/ - -#if (ENABLE_ROTOR_SENSOR_ZERO_SIGNAL==1) - if (project.cds_in[0].type_cds_xilinx == TYPE_CDS_XILINX_SP6) - { - -#if (ENABLE_ROTOR_SENSOR_1_PBUS==1) - WRotorPBus.iqWRotorRawAngle1F = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S1-32768; - WRotorPBus.iqWRotorRawAngle1R = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S1-32768; - WRotorPBus.iqAngle1F = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle1F; - WRotorPBus.iqAngle1R = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle1R; -#else - WRotorPBus.iqWRotorRawAngle1F = 0; - WRotorPBus.iqWRotorRawAngle1R = 0; - WRotorPBus.iqAngle1F = 0; - WRotorPBus.iqAngle1R = 0; -#endif - -#if (ENABLE_ROTOR_SENSOR_2_PBUS==1) - WRotorPBus.iqWRotorRawAngle2F = project.cds_in[0].read.pbus.Impulses_since_zero_point_Falling_S2-32768; - WRotorPBus.iqWRotorRawAngle2R = project.cds_in[0].read.pbus.Impulses_since_zero_point_Rising_S2-32768; - WRotorPBus.iqAngle2F = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle2F; - WRotorPBus.iqAngle2R = KoefNorm_angle * WRotorPBus.iqWRotorRawAngle2R; -#else - WRotorPBus.iqWRotorRawAngle2F = 0; - WRotorPBus.iqWRotorRawAngle2R = 0; - WRotorPBus.iqAngle2F = 0; - WRotorPBus.iqAngle2R = 0; -#endif - } - else - { - WRotorPBus.iqWRotorRawAngle1F = 0; - WRotorPBus.iqWRotorRawAngle1R = 0; - WRotorPBus.iqAngle1F = 0; - WRotorPBus.iqAngle1R = 0; - - WRotorPBus.iqWRotorRawAngle2F = 0; - WRotorPBus.iqWRotorRawAngle2R = 0; - WRotorPBus.iqAngle2F = 0; - WRotorPBus.iqAngle2R = 0; - - } -#endif - - -#if (ENABLE_ROTOR_SENSOR_1_PBUS==1) - //************************************************************************************************** - MyVar3 = project.cds_in[0].read.pbus.SpeedS1_cnt; - - if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { - WRotorPBus.iqWRotorRaw0 = MyVar3; - } - else - { - WRotorPBus.iqWRotorRaw0 = 0; - } - - MyVar3 = project.cds_in[0].read.pbus.SpeedS1_cnt90; - - if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { - WRotorPBus.iqWRotorRaw1 = MyVar3; - } - else - { - WRotorPBus.iqWRotorRaw1 = 0; - } -#else - WRotorPBus.iqWRotorRaw0 = 0; - WRotorPBus.iqWRotorRaw1 = 0; -#endif - - -#if (ENABLE_ROTOR_SENSOR_2_PBUS==1) - //*************************************************************************************************** - MyVar3 = project.cds_in[0].read.pbus.SpeedS2_cnt; - - if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { - WRotorPBus.iqWRotorRaw2 = MyVar3; - } - else - { - WRotorPBus.iqWRotorRaw2 = 0; - } - - MyVar3 = project.cds_in[0].read.pbus.SpeedS2_cnt90; - - if ((MyVar3 <= COUNT_DECODER_ZERO_WROTORPBus) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { - WRotorPBus.iqWRotorRaw3 = MyVar3; - } - else - { - WRotorPBus.iqWRotorRaw3 = 0; - } -#else - WRotorPBus.iqWRotorRaw2 = 0; - WRotorPBus.iqWRotorRaw3 = 0; -#endif - - -#if (ENABLE_ROTOR_SENSOR_1_PBUS==1) -// if (project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_direct && project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_90 ) - AnalisatorRotorSensorPBus(WRotorPBus.iqWRotorRaw0, WRotorPBus.iqWRotorRaw1, &count_overfull_discret1, &count_zero_discret1, - &prev_iqTimeRotor1, &discret_out1, project.cds_in[0].read.sbus.enabled_channels.bit.discret, - &WRotorPBus.iqWRotorCalcBeforeRegul1, &WRotorPBus.iqWRotorCalc1, - project.cds_in[0].read.pbus.direction_in.bit.mode_sensor1_direct, project.cds_in[0].read.pbus.direction_in.bit.mode_sensor1_90, - project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_direct, project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor1_90, - &c_error_pbus_1 ); -#endif - -#if (ENABLE_ROTOR_SENSOR_2_PBUS==1) -// if (project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_direct && project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_90 ) - AnalisatorRotorSensorPBus(WRotorPBus.iqWRotorRaw2, WRotorPBus.iqWRotorRaw3, &count_overfull_discret2, &count_zero_discret2, - &prev_iqTimeRotor2, &discret_out2, project.cds_in[0].read.sbus.enabled_channels.bit.discret, - &WRotorPBus.iqWRotorCalcBeforeRegul2, &WRotorPBus.iqWRotorCalc2, - project.cds_in[0].read.pbus.direction_in.bit.mode_sensor2_direct, project.cds_in[0].read.pbus.direction_in.bit.mode_sensor2_90, - project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_direct, project.cds_in[0].read.pbus.direction_in.bit.value_vaild_sensor2_90, - &c_error_pbus_2); -#endif - - - // RotorDirectionFilter(WRotorPBus.RotorDirectionInstant, &WRotorPBus.RotorDirectionSlow); - - - - if (discret_out1==1 || discret_out2==1) - { - project.cds_in[0].write.sbus.enabled_channels.bit.discret = 1; - count_discret_to_1++; - } - else - { - project.cds_in[0].write.sbus.enabled_channels.bit.discret = 0; - count_discret_to_0++; - } - - -} - - - -#define MAX_COUNT_OVERFULL_DISCRET_2 150 -#pragma CODE_SECTION(RotorMeasure,".fast_run"); -void RotorMeasure(void) -{ - - // 600 Khz clock on every edge -// static long long KoefNorm = 53635601LL;//((600 000/6256/NORMA_WROTOR/2) * ((long)2 << 24)); //15 - NormaWRotor 782*8 = 6256 -// static long long KoefNormMS = 491520000LL;//((600 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 -// static long long KoefNormNS = 49152000000LL;//((60 000 000/1024/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - static long long KoefNormMS = 122880000LL;//((600 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - static long long KoefNormNS = 12288000000LL;//((60 000 000/4096/NORMA_WROTOR) * ((long)2 << 24)); //20 - NORMA_FROTOR 1024*8 = 8129 - static long long KoefNormImpulses = 838860800000000LL;// (2^24 * 1000000000 / (Impulses(ns)) / NORMA_WROTOR - - static _iq max_value_rotor = _IQ(500.0/60.0/NORMA_FROTOR); - static _iq wrotor_add_ramp = _IQ(0.001/NORMA_FROTOR); - -// volatile float MyVar0 = 0; -// volatile unsigned int MyVar1 = 0; -// volatile unsigned int MyVar2 = 0; - unsigned int MyVar3; - - - inc_sensor.read_sensors(&inc_sensor); - - // flag_not_ready_rotor = 0; - -//************************************************************************************************** -// sensor 1 - - if (inc_sensor.use_sensor1) - { - MyVar3 = inc_sensor.data.CountOne1; -// MyVar3 = (unsigned long) rotation_sensor.in_plane.out.CountOne1; - - if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_21_ON; -#endif - - WRotor.iqWRotorRaw0 = MyVar3; - } - else - { - -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_21_OFF; -#endif - - WRotor.iqWRotorRaw0 = 0; - } - MyVar3 = inc_sensor.data.CountZero1; - - if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_22_ON; -#endif - WRotor.iqWRotorRaw1 = MyVar3; - } - else - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_22_OFF; -#endif - WRotor.iqWRotorRaw1 = 0; - } - } - else - { - WRotor.iqWRotorRaw0 = 0; - WRotor.iqWRotorRaw1 = 0; - } - //logpar.uns_log0 = (Uint16)(my_var1); - //logpar.uns_log1 = (Uint16)(my_var2); - - // sensor 2 - if (inc_sensor.use_sensor2) - { - MyVar3 = inc_sensor.data.CountOne2; - - if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_18_ON; -#endif - WRotor.iqWRotorRaw2 = MyVar3; - } - else - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_18_OFF; -#endif - WRotor.iqWRotorRaw2 = 0; - } - - MyVar3 = inc_sensor.data.CountZero2; - - if ((MyVar3 < COUNT_DECODER_ZERO_WROTOR) - && (MyVar3 > COUNT_DECODER_MAX_WROTOR)) - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_23_ON; -#endif - WRotor.iqWRotorRaw3 = MyVar3; - } - else - { -#if(_ENABLE_PWM_LINES_FOR_TESTS_ROTOR) - PWM_LINES_TK_23_OFF; -#endif - WRotor.iqWRotorRaw3 = 0; - } - } - else - { - WRotor.iqWRotorRaw2 = 0; - WRotor.iqWRotorRaw3 = 0; - } - -// if (WRotor.iqWRotorRaw0==0 && WRotor.iqWRotorRaw1==0 && WRotor.iqWRotorRaw2==0 && WRotor.iqWRotorRaw3==0) -// flag_not_ready_rotor = 1; - - if (WRotor.iqWRotorRaw0==0) - { - if (WRotor.count_zero_discret0==MAX_COUNT_OVERFULL_DISCRET_2) - { - WRotor.prev_iqWRotorRaw0 = WRotor.iqWRotorRaw0 = 0; - } - else - { - WRotor.iqWRotorRaw0 = WRotor.prev_iqWRotorRaw0; - WRotor.count_zero_discret0++; - } - } - else - { - WRotor.count_zero_discret0 = 0; - WRotor.prev_iqWRotorRaw0 = WRotor.iqWRotorRaw0; - } - - if (WRotor.iqWRotorRaw1==0) - { - if (WRotor.count_zero_discret1==MAX_COUNT_OVERFULL_DISCRET_2) - { - WRotor.prev_iqWRotorRaw1 = WRotor.iqWRotorRaw1 = 0; - } - else - { - WRotor.iqWRotorRaw1 = WRotor.prev_iqWRotorRaw1; - WRotor.count_zero_discret1++; - } - } - else - { - WRotor.count_zero_discret1 = 0; - WRotor.prev_iqWRotorRaw1 = WRotor.iqWRotorRaw1; - } - - if (WRotor.iqWRotorRaw2==0) - { - if (WRotor.count_zero_discret2==MAX_COUNT_OVERFULL_DISCRET_2) - { - WRotor.prev_iqWRotorRaw2 = WRotor.iqWRotorRaw2 = 0; - } - else - { - WRotor.iqWRotorRaw2 = WRotor.prev_iqWRotorRaw2; - WRotor.count_zero_discret2++; - } - } - else - { - WRotor.count_zero_discret2 = 0; - WRotor.prev_iqWRotorRaw2 = WRotor.iqWRotorRaw2; - } - - if (WRotor.iqWRotorRaw3==0) - { - if (WRotor.count_zero_discret3==MAX_COUNT_OVERFULL_DISCRET_2) - { - WRotor.prev_iqWRotorRaw3 = WRotor.iqWRotorRaw3 = 0; - } - else - { - WRotor.iqWRotorRaw3 = WRotor.prev_iqWRotorRaw3; - WRotor.count_zero_discret3++; - } - } - else - { - WRotor.count_zero_discret3 = 0; - WRotor.prev_iqWRotorRaw3 = WRotor.iqWRotorRaw3; - } - - - WRotor.iqTimeSensor1 = WRotor.iqWRotorRaw0 + WRotor.iqWRotorRaw1; - WRotor.iqTimeSensor2 = WRotor.iqWRotorRaw2 + WRotor.iqWRotorRaw3; - - // -// // zero? -// if (flag_not_ready_rotor) -// { -// if (*count_zero_discret0) -// (*count_zero_discret)--; -// } -// -// // real zero? -// if (count_zero_discret==MAX_COUNT_OVERFULL_DISCRET) -// { -// // , ! -// WRotor.iqTimeSensor1 = 0; -// WRotor.prev_iqTimeSensor1 = 0; -// } -// else -// { -// // , prev_iqTimeRotor -// if (WRotor.iqTimeSensor1==0) -// WRotor.iqTimeSensor1 = WRotor.prev_iqTimeSensor1; -// } -// WRotor.prev_iqTimeSensor1 = WRotor.iqTimeSensor1; -// -// -// // max OVERFULL -// if (flag_overfull_rotor) -// { -// if (*count_overfull_discret0) -// (*count_overfull_discret)--; -// } -// -// // zero? -// if (flag_not_ready_rotor) -// { -// if (*count_zero_discret0) -// (*count_zero_discret)--; -// } -// -// // real zero? -// if (*count_zero_discret==MAX_COUNT_OVERFULL_DISCRET) -// { -// // , ! -// iqWRotorCalc = 0; -// *prev_iqTimeRotor = 0; -// iqTimeRotor = 0; -// } -// else -// { -// // , prev_iqTimeRotor -// if (iqTimeRotor==0) -// iqTimeRotor = *prev_iqTimeRotor; -// } -// *prev_iqTimeRotor = iqTimeRotor; -// -// -// - -/// - if (WRotor.iqTimeSensor1 != 0 && inc_sensor.use_sensor1) - { - if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time1==0) - WRotor.iqWRotorCalcBeforeRegul1 = KoefNormMS / WRotor.iqTimeSensor1; - if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time1==1) - WRotor.iqWRotorCalcBeforeRegul1 = KoefNormNS / WRotor.iqTimeSensor1; - - if (WRotor.iqWRotorCalcBeforeRegul1 > max_value_rotor) - { - WRotor.iqWRotorCalc1 = 0; - WRotor.iqWRotorCalcBeforeRegul1 = 0; - } - else - WRotor.iqWRotorCalc1 = exp_regul_iq(koefW, WRotor.iqWRotorCalc1, WRotor.iqWRotorCalcBeforeRegul1); - - ///// - if (WRotor.iqWRotorCalc1) - { - if (WRotor.iqPrevWRotorCalc1 != WRotor.iqWRotorCalc1) - { - WRotor.iqWRotorCalc1Ramp = zad_intensiv_q(wrotor_add_ramp, wrotor_add_ramp, WRotor.iqWRotorCalc1Ramp, WRotor.iqWRotorCalc1); - WRotor.iqPrevWRotorCalc1 = WRotor.iqWRotorCalc1; - } - } - else - { - WRotor.iqPrevWRotorCalc1 = 0; - WRotor.iqWRotorCalc1Ramp = 0; - } - //// - } - else - { - WRotor.iqWRotorCalc1 = 0; - WRotor.iqWRotorCalcBeforeRegul1 = 0; - } -/// - if (WRotor.iqTimeSensor2 != 0 && inc_sensor.use_sensor2) - { - if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time2==0) - WRotor.iqWRotorCalcBeforeRegul2 = KoefNormMS / WRotor.iqTimeSensor2; - if (inc_sensor.pm67regs.read_comand_reg.bit.sampling_time2==1) - WRotor.iqWRotorCalcBeforeRegul2 = KoefNormNS / WRotor.iqTimeSensor2; - - if (WRotor.iqWRotorCalcBeforeRegul2 > max_value_rotor) - { - WRotor.iqWRotorCalc2 = 0; - WRotor.iqWRotorCalcBeforeRegul2 = 0; - } - else - WRotor.iqWRotorCalc2 = exp_regul_iq(koefW, WRotor.iqWRotorCalc2, WRotor.iqWRotorCalcBeforeRegul2); - - - - ///// - if (WRotor.iqWRotorCalc2) - { - if (WRotor.iqPrevWRotorCalc2 != WRotor.iqWRotorCalc2) - { - WRotor.iqWRotorCalc2Ramp = zad_intensiv_q(wrotor_add_ramp, wrotor_add_ramp, WRotor.iqWRotorCalc2Ramp, WRotor.iqWRotorCalc2); - WRotor.iqPrevWRotorCalc2 = WRotor.iqWRotorCalc2; - } - } - else - { - WRotor.iqPrevWRotorCalc2 = 0; - WRotor.iqWRotorCalc2Ramp = 0; - } - //// - } - else - { - WRotor.iqWRotorCalc2 = 0; - WRotor.iqWRotorCalcBeforeRegul2 = 0; - } -/// - if (inc_sensor.data.TimeCalcFromImpulses1 && inc_sensor.use_sensor1) - WRotor.iqWRotorImpulsesBeforeRegul1 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses1 * ROTOR_SENSOR_IMPULSES_PER_ROTATE); - else - WRotor.iqWRotorImpulsesBeforeRegul1 = 0; - - WRotor.iqWRotorImpulses1 = exp_regul_iq(koefW, WRotor.iqWRotorImpulses1, WRotor.iqWRotorImpulsesBeforeRegul1); - - if (inc_sensor.data.TimeCalcFromImpulses2 && inc_sensor.use_sensor2) - WRotor.iqWRotorImpulsesBeforeRegul2 = (long long) KoefNormImpulses / (inc_sensor.data.TimeCalcFromImpulses2 * ROTOR_SENSOR_IMPULSES_PER_ROTATE); - else - WRotor.iqWRotorImpulsesBeforeRegul2 = 0; - - WRotor.iqWRotorImpulses2 = exp_regul_iq(koefW, WRotor.iqWRotorImpulses2, WRotor.iqWRotorImpulsesBeforeRegul2); - - - // WRotor.iqWRotorCalcBeforeRegul = _IQdiv(WRotor.iqWRotorCalcBeforeRegul,IQ_CONST_3); -} -#define LEVEL_SWITCH_TO_GET_IMPULSES_OBOROTS 50 // Oborot -void select_values_wrotor(void) -{ - static _iq level_switch_to_get_impulses_hz = _IQ(LEVEL_SWITCH_TO_GET_IMPULSES_OBOROTS/60.0/NORMA_FROTOR); - static unsigned int prev_RotorDirectionInstant = 0; - static unsigned int status_RotorRotation = 0; // ? - static _iq wrotor_add = _IQ(0.002/NORMA_FROTOR); - - - - - if (WRotor.iqWRotorCalc1>level_switch_to_get_impulses_hz - || WRotor.iqWRotorCalc2>level_switch_to_get_impulses_hz) - { - // - if (WRotor.iqWRotorImpulses1 || WRotor.iqWRotorImpulses2) - { - if(WRotor.iqWRotorImpulses1>WRotor.iqWRotorImpulses2) - WRotor.iqWRotorSum = WRotor.iqWRotorImpulsesBeforeRegul1; - else - WRotor.iqWRotorSum = WRotor.iqWRotorImpulsesBeforeRegul2; - } - else - { - if(WRotor.iqWRotorCalc1>WRotor.iqWRotorCalc2) - WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul1; - else - WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul2; - } - - - } - else - { - if(WRotor.iqWRotorCalc1>WRotor.iqWRotorCalc2) - WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul1; - else - WRotor.iqWRotorSum = WRotor.iqWRotorCalcBeforeRegul2; - - } - - - // -// if (prev_prev_RotorDirectionInstant && WRotorPBus.RotorDirectionSlow) -// if (WRotor.iqWRotorSum) -// { -// inc_sensor.break_direction = 1; -// } -// prev_prev_RotorDirectionInstant = WRotorPBus.RotorDirectionSlow; - - - -//// !!! -// if (WRotorPBus.RotorDirectionSlow==0) -// { -// if (WRotor.iqWRotorSum) -// inc_sensor.break_direction = 1; -// } -// else -// inc_sensor.break_direction = 0; - - -// if (WRotorPBus.RotorDirectionSlow==0) -// { -// // 0 !!! !!! -// WRotor.iqWRotorSumFilter = exp_regul_iq(koefW, WRotor.iqWRotorSumFilter, 0); -// } -// else - - - WRotor.iqWRotorSumFilter = exp_regul_iq(koefW, WRotor.iqWRotorSumFilter, WRotor.iqWRotorSum*WRotorPBus.RotorDirectionSlow); - - WRotor.iqWRotorSumRamp = zad_intensiv_q(wrotor_add, wrotor_add, WRotor.iqWRotorSumRamp, WRotor.iqWRotorSumFilter); - - - WRotor.iqWRotorSumFilter2 = exp_regul_iq(koefW2, WRotor.iqWRotorSumFilter2, WRotor.iqWRotorSumFilter); - WRotor.iqWRotorSumFilter3 = exp_regul_iq(koefW3, WRotor.iqWRotorSumFilter3, WRotor.iqWRotorSumFilter); - -} - - -#pragma CODE_SECTION(RotorMeasure,".fast_run"); -void RotorMeasureDetectDirection(void) -{ - int direction1, direction2, sum_direct; - - direction1 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == ROTOR_SENSOR_CODE_CLOCKWISE ? 1 : - project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1 == ROTOR_SENSOR_CODE_COUNTERCLOCKWISE ? -1 : - 0; - - direction2 = project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == ROTOR_SENSOR_CODE_COUNTERCLOCKWISE ? 1 : - project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2 == ROTOR_SENSOR_CODE_CLOCKWISE ? -1 : - 0; - - sum_direct = (direction1 + direction2) > 0 ? 1 : - (direction1 + direction2) < 0 ? -1 : - 0; - - WRotorPBus.RotorDirectionInstant = sum_direct; - -} - - -/////////////////////////////////////////////////////////////// - -#endif - - - -/////////////////////////////////////////////////////////////// - -#pragma CODE_SECTION(update_rot_sensors,".fast_run"); -void update_rot_sensors(void) -{ - inc_sensor.update_sensors(&inc_sensor); -} -/////////////////////////////////////////////////////////////// diff --git a/Inu/Src2/551/main/v_rotor.h b/Inu/Src2/551/main/v_rotor.h deleted file mode 100644 index aad2216..0000000 --- a/Inu/Src2/551/main/v_rotor.h +++ /dev/null @@ -1,185 +0,0 @@ -#ifndef V_ROTOR_H -#define V_ROTOR_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "params_bsu.h" - - -// -#define ROTOR_SENSOR_CODE_CLOCKWISE 2 // -#define ROTOR_SENSOR_CODE_COUNTERCLOCKWISE 1 // - -// -//#define ROTOR_SENSOR_CODE_CLOCKWISE 1 // -//#define ROTOR_SENSOR_CODE_COUNTERCLOCKWISE 2 // - - -void update_rot_sensors(void); -void RotorMeasure(void); -void RotorMeasurePBus(void); -void rotorInit(void); -void select_values_wrotor(void); - - -void RotorDirectionFilter(int RotorDirectionIn, int *RotorDirectionOut, int *RotorDirectionOut2, int *count_direction); - -int AnalisatorRotorSensorPBus(_iq d1, _iq d2, unsigned int *count_overfull_discret, unsigned int *count_zero_discret, _iq *prev_iqTimeRotor, - unsigned int *discret_out, unsigned int discret_in, _iq *iqWRotorCalcBeforeRegul, _iq *iqWRotorCalc, - int modeS1, int modeS2, - int valid_sensor_direct, int valid_sensor_90, - unsigned int *error_count); - -void RotorMeasureDetectDirection(void); - - -typedef struct -{ - _iq iqWRotorRaw0; - _iq iqWRotorRaw1; - _iq iqWRotorRaw2; - _iq iqWRotorRaw3; - - _iq iqWRotorFilter0; - _iq iqWRotorFilter1; - _iq iqWRotorFilter2; - _iq iqWRotorFilter3; - - _iq iqWRotorDelta; - - _iq iqTimeSensor1; - _iq iqTimeSensor2; - - _iq prev_iqWRotorRaw0; - _iq prev_iqWRotorRaw1; - _iq prev_iqWRotorRaw2; - _iq prev_iqWRotorRaw3; - - unsigned int count_zero_discret0; - unsigned int count_zero_discret1; - unsigned int count_zero_discret2; - unsigned int count_zero_discret3; - - - _iq iqWRotorCalc1; - _iq iqWRotorCalcBeforeRegul1; - _iq iqWRotorCalc2; - _iq iqWRotorCalcBeforeRegul2; - -// int RotorDirectionInstant; -// int RotorDirectionSlow; - - _iq iqWRotorImpulses1; - _iq iqWRotorImpulsesBeforeRegul1; - _iq iqWRotorImpulses2; - _iq iqWRotorImpulsesBeforeRegul2; - - _iq iqWRotorSum; - _iq iqWRotorSumFilter; - _iq iqWRotorSumFilter2; - _iq iqWRotorSumFilter3; - - _iq iqWRotorSumRamp; - - _iq iqWRotorCalc1Ramp; - _iq iqPrevWRotorCalc1; - _iq iqWRotorCalc2Ramp; - _iq iqPrevWRotorCalc2; - - int RotorDirectionSlow; - -} WRotorValues; - -#define WRotorValues_DEFAULTS {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0, 0,0,0,0, 0} - -typedef struct -{ - _iq iqWRotorRaw0; - _iq iqWRotorRaw1; - _iq iqWRotorRaw2; - _iq iqWRotorRaw3; - - - _iq iqWRotorFilter0; - _iq iqWRotorFilter1; - _iq iqWRotorFilter2; - _iq iqWRotorFilter3; - - _iq iqWRotorDelta; -// _iq iqWRotorCalcBeforeRegul; - - _iq iqWRotorCalcBeforeRegul1; - _iq iqWRotorCalcBeforeRegul2; - - _iq iqTimeRotor1; - _iq iqTimeRotor2; - - _iq iqTimeRotor; - - - _iq iqWRotorCalc1; - _iq iqWRotorCalc2; -// _iq iqWRotorCalc; - - int ModeAutoDiscret; - - int RotorDirectionInstant; - int RotorDirectionSlow; - int RotorDirectionCount; - int RotorDirectionSlow2; - - - -#if (ENABLE_ROTOR_SENSOR_ZERO_SIGNAL==1) - _iq iqWRotorRawAngle1F; - _iq iqWRotorRawAngle1R; - _iq iqWRotorRawAngle2F; - _iq iqWRotorRawAngle2R; - - _iq iqAngle1F; - _iq iqAngle1R; - - _iq iqAngle2F; - _iq iqAngle2R; - - _iq iqAngleCalc; - -#endif - -} WRotorValuesAngle; - -#if (ENABLE_ROTOR_SENSOR_ZERO_SIGNAL==1) -#define WRotorValuesAngle_DEFAULTS {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} -#else -#define WRotorValuesAngle_DEFAULTS {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} -#endif - -extern WRotorValues WRotor; -extern WRotorValuesAngle WRotorPBus; - - - - - - -//#define NORMA_WROTOR 15 - -#define IQ_WROTOR_MAX_MIN_DELTA 3744914286 //17920 - - -#define WRMP_COEF 0.001 // 0.24 Hz per sec - -#define IQ_CONST_3 50331648 - -#define COUNT_DECODER_ZERO_WROTORPBus 65535 //0x00fe5000//0x01fca000 -#define COUNT_DECODER_ZERO_WROTOR 65500 //0x00fe5000//0x01fca000 -#define COUNT_DECODER_MAX_WROTOR 10 - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/Inu/Src2/551/main/v_rotor_22220.c b/Inu/Src2/551/main/v_rotor_22220.c deleted file mode 100644 index c31de31..0000000 --- a/Inu/Src2/551/main/v_rotor_22220.c +++ /dev/null @@ -1,666 +0,0 @@ - -#include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "DSP281x_SWPrioritizedIsrLevels.h" // DSP281x Examples Include File -#include "DSP281x_Device.h" // DSP281x Headerfile Include File -#include "IQmathLib.h" - -#include "params_bsu.h" -#include "v_rotor.h" - -#include "filter_v1.h" -#include "xp_cds_in.h" - -#include "xp_inc_sensor.h" -#include "xp_project.h" -#include "params.h" - -//#include "pwm_test_lines.h" -#include "params_norma.h" -#include "edrk_main.h" -#include "params_pwm24.h" -#include "v_rotor_22220.h" - -#include "rmp_cntl_v1.h" -#include "mathlib.h" - - -//#define _ENABLE_PWM_LED2_PROFILE 1 - -#if (_ENABLE_PWM_LED2_PROFILE) -extern unsigned int profile_pwm[30]; -extern unsigned int pos_profile_pwm; -#endif - -// -//#pragma DATA_SECTION(buf1,".logs2") -//int buf1[SIZE_BUF_W]; -//#pragma DATA_SECTION(buf2,".logs2") -//int buf2[SIZE_BUF_W]; -//#pragma DATA_SECTION(buf3,".logs2") -//int buf3[SIZE_BUF_W]; -//#pragma DATA_SECTION(buf4,".logs2") -//int buf4[SIZE_BUF_W]; -//#pragma DATA_SECTION(buf5,".logs2") -//int buf5[SIZE_BUF_W]; -//#pragma DATA_SECTION(buf6,".logs2") -//int buf6[SIZE_BUF_W]; -//#pragma DATA_SECTION(buf7,".logs2") -//int buf7[SIZE_BUF_W]; -//#pragma DATA_SECTION(buf8,".logs2") -//int buf8[SIZE_BUF_W]; -// - -/////////////////////////////////////////////// -/////////////////////////////////////////////// -/////////////////////////////////////////////// -RMP_V1 rmp_wrot = RMP_V1_DEFAULTS; - -#pragma DATA_SECTION(rotor_22220,".fast_vars"); -ROTOR_VALUE_22220 rotor_22220 = ROTOR_VALUE_22220_DEFAULTS; - - -void rotorInit_22220(void) -{ - unsigned int i = 0, size = 0, *pint = 0; - - rmp_wrot.RampLowLimit = 0; - rmp_wrot.RampHighLimit = _IQ(1.0); - rmp_wrot.RampPlus = _IQ(0.0015/NORMA_FROTOR); - rmp_wrot.RampMinus = _IQ(-0.0015/NORMA_FROTOR); - rmp_wrot.DesiredInput = 0; - rmp_wrot.Out = 0; - - -// pint = (unsigned int*)&rotor; -// size = sizeof(rotor) / sizeof(unsigned int); -// for(i = 0; i < size; i++) -// { -// *(pint + i) = 0; -// } - -} - -_iq koef_Wout_filter = _IQ(0.2); //_IQ(0.15); -_iq koef_Wout_filter_long = _IQ(0.001);//_IQ(0.03); - -#define SIZE_BUF_F1 10 -#pragma DATA_SECTION(f1,".slow_vars") -static _iq f1[SIZE_BUF_F1]={0,0,0,0,0,0,0,0,0,0}; -#pragma DATA_SECTION(f1_int,".slow_vars") -static long f1_int[SIZE_BUF_F1]={0,0,0,0,0,0,0,0,0,0}; - -#pragma CODE_SECTION(clear_iqFsensors,".fast_run"); -void clear_iqFsensors(void) -{ - int i; - - for (i=0;i=(SIZE_BUF_W-1)) -// { -// flag_buf = 0; -// c_s = 0; -// } -// else -// c_s++; -// -// -// buf1[c_s] = inc_sensor.data.CountOne1;// rotation_sensor.in_plane.out.CountOne1; -// buf2[c_s] = inc_sensor.data.CountZero1;//rotation_sensor.in_plane.out.CountZero1; -// buf3[c_s] = inc_sensor.data.CountOne2;//rotation_sensor.in_plane.out.CountOne2; -// buf4[c_s] = inc_sensor.data.CountZero2;//rotation_sensor.in_plane.out.CountZero2; -// buf5[c_s] = direction1;//inc_sensor.data.;//(rotation_sensor.in_plane.out.direction1); -// buf6[c_s] = direction2;//(rotation_sensor.in_plane.out.direction2); -// buf7[c_s] = (project.cds_in[0].read.pbus.direction_in.bit.dir_sens_1); -// buf8[c_s] = (project.cds_in[0].read.pbus.direction_in.bit.dir_sens_2); -// -// } -// prev_flag_buf = flag_buf; - - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - - - if(inc_sensor.use_sensor1) - { - if((inc_sensor.data.CountOne1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1) - || inc_sensor.data.CountOne1 == 65535) - { inc_sensor.data.CountOne1 = 0; } - if((inc_sensor.data.CountZero1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1) - || inc_sensor.data.CountZero1 == 65535) - { inc_sensor.data.CountZero1 = 0; } - - // - if (inc_sensor.data.Impulses1 < 5) { - - if(inc_sensor.data.CountOne1 && inc_sensor.data.CountZero1 ) - { - sum_count = (long)inc_sensor.data.CountOne1 + (long)inc_sensor.data.CountZero1; - - if (s_number3 2) - { - if (s_number 139810L)//10 rpm - { - if (inc_sensor.data.CountOne1 == 0 && inc_sensor.data.CountZero1 == 0 - && inc_sensor.data.Impulses1 == 0) { - sens1_err_count += 1; - } else { - sens1_err_count = 0; - } - if (sens1_err_count > 50) { - sens1_err_count = 50; - edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 1; -// faults.faults4.bit.Speed_Datchik_1_Off |= 1; - } - } else { - sens1_err_count = 0; - edrk.warnings.e9.bits.SENSOR_ROTOR_1_BREAK = 0; - } - } - - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - - -// logpar.log4 = rotation_sensor.in_plane.out.CountOne2; -// logpar.log20 = rotation_sensor.in_plane.out.CountZero2; - if(inc_sensor.use_sensor2) - { - if((inc_sensor.data.CountOne2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) - || inc_sensor.data.CountOne2 == 65535) - { inc_sensor.data.CountOne2 = 0; } - if((inc_sensor.data.CountZero2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) - || inc_sensor.data.CountZero2 == 65535) - { inc_sensor.data.CountZero2 = 0; } - - // , - if (inc_sensor.data.Impulses2 < 5) { - - if(inc_sensor.data.CountOne2 && inc_sensor.data.CountZero2 ) - { - sum_count = (long)inc_sensor.data.CountOne2+(long)inc_sensor.data.CountZero2; - if (s_number3 2) - { - if (s_number 139810L)//10 rpm - { - if (inc_sensor.data.CountOne2 == 0 && inc_sensor.data.CountZero2 == 0 - && inc_sensor.data.Impulses2 == 0) { - sens2_err_count += 1; - } else { - sens2_err_count = 0; - } - if (sens2_err_count > 50) { - sens2_err_count = 50; -// faults.faults4.bit.Speed_Datchik_2_Off |= 1; - edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 1; - } - } else { - sens2_err_count = 0; - edrk.warnings.e9.bits.SENSOR_ROTOR_2_BREAK = 0; - } - - } - - if(s_number > SENSORS_NUMBER_ONLY_IN) {s_number = SENSORS_NUMBER_ONLY_IN;} //TODO set SENSORS_NUMBER when tune angle measure - if(s_number > 3) - { - sort_F_array(rotor_22220.iqFsensors, s_number); - deltaF = rotor_22220.iqFout >> 2; - if(deltaF < 43000) // ~3 ob/min - { - deltaF = 43000; - } - i = 0; - begin_data = 0; - end_data = s_number; //TODO test, as usial - while(i < s_number) - { - if(_IQabs(rotor_22220.iqFout - rotor_22220.iqFsensors[i]) >= deltaF) - { - i++; - } - else - { - break; - } - } - if(i < s_number) { begin_data = i; } - else {begin_data = 0;} - while((i < s_number) && (_IQabs(rotor_22220.iqFout - rotor_22220.iqFsensors[i]) < deltaF)) - { - i++; - } - if(i <= SENSORS_NUMBER) - { - end_data = i; - } - else - { - end_data = SENSORS_NUMBER; - } - } - else - { - begin_data = 0; - end_data = s_number; - } - if (begin_data >= end_data) { //This part to prevent freeze of speed on some level if signal lost - begin_data = 0; - end_data = s_number; - } - // - for(i = begin_data; i < end_data; i++) - { - accumulator += rotor_22220.iqFsensors[i]; - } - // - if(end_data != begin_data) - { - rotor_22220.iqFdirty = accumulator / (end_data - begin_data); - prev_wrot_count = 0; - } - else - { - rotor_22220.iqFdirty = prev_wrot; - prev_wrot_count += 1; - } - -// logpar.log19 = (int16)(_IQtoIQ15(rotor.iqF)); -// rotor_22220.iqFdirty = rotor_22220.iqF; - - if (prev_wrot != rotor_22220.iqFdirty || rotor_22220.iqFdirty==0 ) - { - rmp_wrot.DesiredInput = rotor_22220.iqFdirty; - rmp_wrot.calc(&rmp_wrot); - rotor_22220.iqF = rmp_wrot.Out; - } - else - { - rotor_22220.iqF = rotor_22220.iqFdirty; - } - prev_wrot=rotor_22220.iqF; - - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - - - // . - if (prev_wrot_count > 10) { - prev_wrot = 0; - prev_wrot_count = 10; - } - - rotor_22220.iqFout = exp_regul_iq(koef_Wout_filter, rotor_22220.iqFout, rotor_22220.iqF); - rotor_22220.iqFlong = exp_regul_iq(koef_Wout_filter_long, rotor_22220.iqFlong, rotor_22220.iqF); - -#if (_ENABLE_PWM_LED2_PROFILE) - if (profile_pwm[pos_profile_pwm++]) - i_led2_on_off(1); - else - i_led2_on_off(0); -#endif - - - rotor_22220.direct_rotor_in1 = direction1;//rotation_sensor.in_plane.out.direction1; - -#if (_STEND_40MWT==1) - // , .. , !!! - - rotor_22220.direct_rotor_in2 = -rotation_sensor.in_plane.out.direction2; - -#else - - rotor_22220.direct_rotor_in2 = direction2;//rotation_sensor.in_plane.out.direction2; - -#endif - // rotor.direct_rotor_angle = rotation_sensor.rotation_plane.out.direction; - - rotor_22220.error.sens_err1 = sens_err1; - rotor_22220.error.sens_err2 = sens_err2; - -// rotor.direct_rotor = (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0 ? 1 : // + rotor.direct_rotor_angle -// (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) < 0 ? -1 : // + rotor.direct_rotor_angle -// 0; - if(rotor_22220.iqFout >139810L) //10ob/min - { - if((rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) > 0) - { - direct_accum++; - } - else if((rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) < 0) - { - direct_accum--; - } - else - { - if(direct_accum > 0) {direct_accum--;} - if(direct_accum < 0) {direct_accum++;} - } - if(direct_accum > 60) { direct_accum = 60; } - if(direct_accum < -60) { direct_accum = -60; } - rotor_22220.direct_rotor = direct_accum > 0 ? 1 : - direct_accum < 0 ? -1 : - 0; -// if (f.flag_second_PCH) { -// rotor.direct_rotor = - rotor.direct_rotor; -// } - } - else - { - rotor_22220.direct_rotor = (rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) > 0 ? 1 : // + rotor.direct_rotor_angle - (rotor_22220.direct_rotor_in1 + rotor_22220.direct_rotor_in2) < 0 ? -1 : // + rotor.direct_rotor_angle - 0; -// if (f.flag_second_PCH) { -// rotor.direct_rotor = - rotor.direct_rotor; -// } - direct_accum = rotor_22220.direct_rotor; - } - -// if(rotation_sensor.in_plane.write.regs.comand_reg.bit.set_sampling_time) // -// { -// rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0x5C; -// } -// else // , -// { -// rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0xA8; -// } - if (s_number2 0; i--) - { - for(j = 1; j < size; j++) - { - if(array[j - 1] > array[j]) - { - tmp = array[j]; - array[j] = array[j - 1]; - array[j - 1] = tmp; - } - } - } -} - - -void select_values_wrotor_22220(void) -{ - // static _iq level_switch_to_get_impulses_hz = _IQ(LEVEL_SWITCH_TO_GET_IMPULSES_OBOROTS/60.0/NORMA_FROTOR); - static unsigned int prev_RotorDirectionInstant = 0; - static unsigned int status_RotorRotation = 0; // ? - static _iq wrotor_add = _IQ(0.002/NORMA_FROTOR); - - WRotor.RotorDirectionSlow = rotor_22220.direct_rotor; - WRotor.iqWRotorSum = rotor_22220.iqFout;// * rotor_22220.direct_rotor; - WRotor.iqWRotorSumFilter = rotor_22220.iqFout * rotor_22220.direct_rotor; - WRotor.iqWRotorSumFilter2 = rotor_22220.iqFlong * rotor_22220.direct_rotor; - WRotor.iqWRotorSumFilter3 = rotor_22220.iqFlong * rotor_22220.direct_rotor; - WRotor.iqWRotorSumRamp = zad_intensiv_q(wrotor_add, wrotor_add, WRotor.iqWRotorSumRamp, WRotor.iqWRotorSumFilter); - - -} diff --git a/Inu/Src2/551/main/v_rotor_22220.h b/Inu/Src2/551/main/v_rotor_22220.h deleted file mode 100644 index aec28c5..0000000 --- a/Inu/Src2/551/main/v_rotor_22220.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef V_ROTOR_22220_H -#define V_ROTOR_22220_H - -#define SIZE_BUF_W 100 //2000 - -/////// 22220.5 -#define SENSORS_NUMBER 10 -#define SENSORS_NUMBER_ONLY_IN 6 -//#define IMPULSES_PER_TURN (1LL << 13) //Old sensor -#define IMPULSES_PER_TURN 4096 //Lira sensor -//#define ANGLE_RESOLUTION (1LL << 18) //2^18 - -typedef struct -{ - int direct_rotor; - int direct_rotor_in1; - int direct_rotor_in2; - int direct_rotor_angle; - union { - unsigned int sens_err1:1; - unsigned int sens_err2:1; - unsigned int reserved:14; - } error; - - _iq iqFsensors[SENSORS_NUMBER]; - - - _iq iqFdirty; - _iq iqF; - _iq iqFout; - _iq iqFlong; - - _iq iqFrotFromOptica; - - unsigned int error_update_count; -} ROTOR_VALUE_22220; - -#define ROTOR_VALUE_22220_DEFAULTS {0,0,0,0,0, {0,0,0,0,0,0,0,0},0,0,0,0,0,0} - - -_iq counter_To_iqF2(long count, unsigned int freq_mode); -void sort_F_array(_iq *array, unsigned int size); -_iq impulses_To_iqF(unsigned int time, unsigned int impulses); //time mks. impulses count -void Rotor_measure_22220(void); -void rotorInit_22220(void); -void select_values_wrotor_22220(void); - -extern ROTOR_VALUE_22220 rotor_22220; - -extern _iq koef_Wout_filter, koef_Wout_filter_long; - - -#endif - diff --git a/Inu/Src2/551/main/vector.h b/Inu/Src2/551/main/vector.h deleted file mode 100644 index 25bc4f1..0000000 --- a/Inu/Src2/551/main/vector.h +++ /dev/null @@ -1,254 +0,0 @@ -/* - ???? ??? (?) 2006 ?. - - Processor: TMS320C32 - - Filename: vector_troll.h - - ??????? ?????????? ?????????y - - Edit date: 04-12-02 - - Function: - - Revisions: -*/ - - -#ifndef _VECTOR_SEV -#define _VECTOR_SEV - - -#ifdef __cplusplus - extern "C" { -#endif - - -#include "IQmathLib.h" -#include "x_basic_types.h" - -typedef struct -{ - float W; /* */ - float Angle; /* */ - float Phi; /* */ - float k; /* . */ - float k1; /* . */ - float k2; /* . */ - float f; /* */ - - _iq iqk; - _iq iqk1; - _iq iqk2; - _iq iqf; - - - -} WINDING; - -#define WINDING_DEFAULT {0,0,0,0,0,0,0,0,0,0,0} - - -typedef struct -{ - unsigned int Prepare; - unsigned int terminal_prepare; - unsigned int Test_Lamps; - unsigned int fault; - - - - unsigned int Stop; - unsigned int Mode; - unsigned int Revers; - unsigned int Is_Blocked; - - unsigned int Ready1; - unsigned int Ready2; - unsigned int Discharge; - unsigned int Assemble; - - unsigned int ErrorChannel1; - unsigned int ErrorChannel2; - unsigned int FaultChannel1; - unsigned int FaultChannel2; - - unsigned int Set_power; - - unsigned int Impuls; - - unsigned int Obmotka1; - unsigned int Obmotka2; -// unsigned int Down50; - - unsigned int I_over_nominal; //????????? ?????? ?????? ? ??????????? ???????????? ???? - unsigned int Moment_over_1_6_noninal; //????????? ?????? ?????? ? ??????????? ???????????? ??????? ? 1.6 ??? - unsigned int Moment_over_1_8_nominal; //????????? ?????? ?????? ? ??????????? ???????????? ??????? ? 1.8 ??? - unsigned int DownToNominal; - unsigned int DownToNominalMoment; - unsigned int Down50Temperature; - unsigned int nominal_I_exceeded_counter; //??????? ??? ????????? ??????????? ??? - unsigned int nominal_M_exceeded_counter; //??????? ??? ????????? ??????????? ?????? - - unsigned int Up50; - unsigned int Ciclelog; - unsigned int Provorot; - unsigned int Bpsi; - unsigned int Piregul1; - unsigned int Piregul2; - unsigned int Startstoplog; - unsigned int Setspeed; - - unsigned int BWC_is_ON; - - unsigned int Setsdvigfaza; - unsigned int Off_piregul; - - unsigned int Restart; - unsigned int Log1_Log2; - - unsigned int Work_net; - unsigned int Mask_impuls; - unsigned int Impuls_width; - - - unsigned int Work; - - unsigned int Auto; - - unsigned int Uzad; - unsigned int Umin; - -// unsigned int RScount; - unsigned int vector_run; - unsigned int test_all_run; - - unsigned int decr_mzz_temp; -// unsigned int flag_decr_mzz_temp; - - unsigned int flag_Break_Resistor_Error; - unsigned int flag_local_control; //1 - local - unsigned int flag_leading; //??????? ?? ?????? - unsigned int flag_second_leading; //?????? ?? ?????? - unsigned int flag_distance; - unsigned int flag_kvitirovanie; - unsigned int flag_batary_loaded; - unsigned int flag_Pump_Is_On; - unsigned int power_units_doors_closed; - unsigned int power_units_doors_locked; - - unsigned int flag_decr_mzz_power; - - real decr_mzz_power; - _iq iq_decr_mzz_power; - - _iq iq_decr_mzz_voltage; - - real fzad; - real kzad; - real kzad_plus; -// real fzad_provorot; - real Sdvigfaza; -// real k_3garonica; -// _iq iq_k_3garonica; -// real bpsi_zad; - -// real Piregul1_p; -// real Piregul1_i; - -// real Piregul2_p; -// real Piregul2_i; - - - real mzz_zad; - real fr_zad; - real Power; - real p_zad; - - -// _iq iq_bpsi_zad; - _iq iq_mzz_zad; -// _iq iq_fzad_provorot; - _iq iq_fzad; - - _iq iq_p_zad; - - unsigned int flag_Enable_Prepare; - - - unsigned int status_MODE_WORK_SVU; - unsigned int status_MODE_WORK_MPU; - unsigned int status_MODE_WORK_VPU; -// unsigned int status_MODE_WORK_EVPU; - -// unsigned int filter_READY_UKSS_BV1; -// unsigned int filter_READY_UKSS_BV2; -// unsigned int filter_READY_UKSS_BI1; -// unsigned int filter_READY_UKSS_BI2; - unsigned int filter_READY_UMU; -// unsigned int filter_READY_UKSS_UKSI; - - unsigned int On_Power_QTV; - - unsigned int RS_MPU_ERROR; -// unsigned int CAN_MPU_ERROR; - -// unsigned int enable_fast_prepare; - -// unsigned int tau_break1; -// unsigned int tau_break2; - - unsigned int flag_tormog; - -// unsigned int GoPWM; - - int special_test_from_mpu; - - int MessageToCan1; - int MessageToCan2; - int flag_change_pwm_freq; - int flag_random_freq; - long tmp; - - - - - - -//optical bus data - unsigned int read_task_from_optical_bus; - - - - unsigned int count_wait_after_kvitir; - unsigned int flag_record_log; - unsigned int count_step_ram_off; - unsigned int count_start_impuls; - - int flag_send_alarm_log_to_MPU; - - -} FLAG; - -#define FLAG_DEFAULTS {\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0,0,0,0,0,\ - 0,0,0,0,0,0\ - } -extern FLAG f; -//extern WINDING a; - -#ifdef __cplusplus - } -#endif - -#endif /* _VECTOR_SEV */ - - diff --git a/Inu/Src2/551/main/xPlatesAddress.h b/Inu/Src2/551/main/xPlatesAddress.h deleted file mode 100644 index fa0a62c..0000000 --- a/Inu/Src2/551/main/xPlatesAddress.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef _XPLATESADDRESS_H -#define _XPLATESADDRESS_H - - #define ADC_0_addr 7 - #define ADC_1_addr 8 - - #define TK_0_addr 5 - #define TK_1_addr 6 - #define TK_2_addr 9 - #define TK_3_addr 10 - - #define IN_0_addr 2 - #define IN_1_addr 3 - #define IN_2_addr 4 - - #define OUT_0_addr 11 - #define OUT_1_addr 12 - - #define RotPlane_addr 14 - -#endif diff --git a/Inu/Src2/VectorControl/abc_to_alphabeta.c b/Inu/Src2/VectorControl/abc_to_alphabeta.c deleted file mode 100644 index 7f37475..0000000 --- a/Inu/Src2/VectorControl/abc_to_alphabeta.c +++ /dev/null @@ -1,23 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library -#include "abc_to_alphabeta.h" - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(abc_to_alphabeta_calc,".fast_run"); -void abc_to_alphabeta_calc(ABC_TO_ALPHABETA *v) -{ - static _iq iq_1_sqrt3 = _IQ(0.57735026918962576450914878050196); // = 1/sqrt(3) - static _iq iq_2_sqrt3 = _IQ(1.1547005383792515290182975610039); // =2/sqrt(3) - - v->Ualpha = v->Ua; - v->Ubeta = _IQmpy(iq_1_sqrt3,v->Ua) + _IQmpy(iq_2_sqrt3,v->Ub); - -} - - - -///////////////////////////////////////////////// diff --git a/Inu/Src2/VectorControl/abc_to_alphabeta.h b/Inu/Src2/VectorControl/abc_to_alphabeta.h deleted file mode 100644 index eb16b58..0000000 --- a/Inu/Src2/VectorControl/abc_to_alphabeta.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef __ABC_ALPHABETA_H__ -#define __ABC_ALPHABETA_H__ - -#include "IQmathLib.h" - -typedef struct { _iq Ua; //phase A voltage, input - _iq Ub; //phase B voltage, input - _iq Uc; //phase C voltage, input -// _iq Tetta; //phase angle, input - _iq Ualpha; // axis d voltage, output - _iq Ubeta; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - }ABC_TO_ALPHABETA; - - - - - -typedef ABC_TO_ALPHABETA *ABC_TO_ALPHABETA_handle; - -#define ABC_TO_ALPHABETA_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(Uint32))abc_to_alphabeta_calc\ - } - - -void abc_to_alphabeta_calc(ABC_TO_ALPHABETA_handle); - - - - - - - - -#endif // end __ABC_ALPHABETA_H diff --git a/Inu/Src2/VectorControl/abc_to_dq.c b/Inu/Src2/VectorControl/abc_to_dq.c deleted file mode 100644 index 39c414d..0000000 --- a/Inu/Src2/VectorControl/abc_to_dq.c +++ /dev/null @@ -1,38 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library -#include "abc_to_dq.h" - - - - - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(abc_to_dq_calc,".fast_run"); -void abc_to_dq_calc(ABC_TO_DQ *v) -{ - static _iq iq_two_third_pi = _IQ(6.283185307179586476925286766559/3.0); - static _iq iq_two_third = _IQ(2.0/3.0); - - v->Id = _IQmpy(iq_two_third,_IQmpy(v->Ia, _IQsin(v->Tetta)) + _IQmpy(v->Ib, _IQsin(v->Tetta - iq_two_third_pi)) + _IQmpy(v->Ic, _IQsin(v->Tetta + iq_two_third_pi))); - v->Iq = _IQmpy(iq_two_third,_IQmpy(v->Ia, _IQcos(v->Tetta)) + _IQmpy(v->Ib, _IQcos(v->Tetta - iq_two_third_pi)) + _IQmpy(v->Ic, _IQcos(v->Tetta + iq_two_third_pi))); -} - - -#pragma CODE_SECTION(abc_to_dq_calc_v2,".fast_run"); -void abc_to_dq_calc_v2(ABC_TO_DQ *v) -{ - static _iq iq_two_third_pi = _IQ(6.283185307179586476925286766559/3.0); - static _iq iq_two_third = _IQ(2.0/3.0); - - v->Id = _IQmpy(iq_two_third,_IQmpy(v->Ia, _IQcos(v->Tetta)) + _IQmpy(v->Ib, _IQcos(v->Tetta - iq_two_third_pi)) + _IQmpy(v->Ic, _IQcos(v->Tetta + iq_two_third_pi))); - v->Iq = _IQmpy(iq_two_third,_IQmpy(-v->Ia, _IQsin(v->Tetta)) - _IQmpy(v->Ib, _IQcos(v->Tetta - iq_two_third_pi)) - _IQmpy(v->Ic, _IQcos(v->Tetta + iq_two_third_pi))); -} - - -///////////////////////////////////////////////// - - diff --git a/Inu/Src2/VectorControl/abc_to_dq.h b/Inu/Src2/VectorControl/abc_to_dq.h deleted file mode 100644 index 1afd618..0000000 --- a/Inu/Src2/VectorControl/abc_to_dq.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef __ABC_DQ_H__ -#define __ABC_DQ_H__ - - - -typedef struct { _iq Ia; //phase A voltage, input - _iq Ib; //phase B voltage, input - _iq Ic; //phase C voltage, input - _iq Tetta; //phase angle, input - _iq Id; // axis d voltage, output - _iq Iq; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - void (*calc_v2)(); // Pointer to calculation function - }ABC_TO_DQ; - - - - - -typedef ABC_TO_DQ *ABC_TO_DQ_handle; - -#define ABC_TO_DQ_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(_iq))abc_to_dq_calc, \ - (void (*)(_iq))abc_to_dq_calc_v2 } - - -void abc_to_dq_calc(ABC_TO_DQ_handle); -void abc_to_dq_calc_v2(ABC_TO_DQ_handle); - - - - - - - - -#endif // end __ABC_DQ_H diff --git a/Inu/Src2/VectorControl/alphabeta_to_abc.c b/Inu/Src2/VectorControl/alphabeta_to_abc.c deleted file mode 100644 index a161888..0000000 --- a/Inu/Src2/VectorControl/alphabeta_to_abc.c +++ /dev/null @@ -1,24 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library -#include "alphabeta_to_abc.h" - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(alphabeta_to_abc_calc,".fast_run"); -void alphabeta_to_abc_calc(ALPHABETA_TO_ABC *v) -{ - static _iq iq_0_5 = _IQ(0.5); // = 1/2 - static _iq iq_sqrt3_2 = _IQ(0.86602540378443864676372317075294); // =sqrt(3)/2 - - v->Ua = v->Ualpha; - v->Ub = -_IQmpy(iq_0_5,v->Ualpha) + _IQmpy(iq_sqrt3_2,v->Ubeta); - v->Uc = -_IQmpy(iq_0_5,v->Ualpha) - _IQmpy(iq_sqrt3_2,v->Ubeta); - -} - - - -///////////////////////////////////////////////// diff --git a/Inu/Src2/VectorControl/alphabeta_to_abc.h b/Inu/Src2/VectorControl/alphabeta_to_abc.h deleted file mode 100644 index c5dca95..0000000 --- a/Inu/Src2/VectorControl/alphabeta_to_abc.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef __ALPHABETA_ABC_H__ -#define __ALPHABETA_ABC_H__ - -#include "IQmathLib.h" - -typedef struct { _iq Ualpha; // axis d voltage, output - _iq Ubeta; // axis q voltage, output - _iq Ua; //phase A voltage, input - _iq Ub; //phase B voltage, input - _iq Uc; //phase C voltage, input - void (*calc)(); // Pointer to calculation function - }ALPHABETA_TO_ABC; - - - - - -typedef ALPHABETA_TO_ABC *ALPHABETA_TO_ABC_handle; - -#define ALPHABETA_TO_ABC_DEFAULTS{ 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(Uint32))alphabeta_to_abc_calc\ - } - - -void alphabeta_to_abc_calc(ALPHABETA_TO_ABC_handle); - - - - - - - - -#endif // end __ALPHABETA_ABC_H__ diff --git a/Inu/Src2/VectorControl/alphabeta_to_dq.c b/Inu/Src2/VectorControl/alphabeta_to_dq.c deleted file mode 100644 index 026a212..0000000 --- a/Inu/Src2/VectorControl/alphabeta_to_dq.c +++ /dev/null @@ -1,22 +0,0 @@ -#include "IQmathLib.h" // Include header for IQmath library -#include "alphabeta_to_dq.h" - - - - - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(alphabeta_to_dq_calc,".fast_run"); -void alphabeta_to_dq_calc(ALPHABETA_TO_DQ *v) -{ - - v->Ud = _IQmpy(v->Ualpha, _IQcos(v->Tetta)) + _IQmpy(v->Ubeta, _IQsin(v->Tetta)); - v->Uq = -_IQmpy(v->Ualpha, _IQsin(v->Tetta)) + _IQmpy(v->Ubeta, _IQcos(v->Tetta)); - -} -///////////////////////////////////////////////// diff --git a/Inu/Src2/VectorControl/alphabeta_to_dq.h b/Inu/Src2/VectorControl/alphabeta_to_dq.h deleted file mode 100644 index cd7ac27..0000000 --- a/Inu/Src2/VectorControl/alphabeta_to_dq.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef __ALPHABETA_DQ_H__ -#define __ALPHABETA_DQ_H__ - -#include "IQmathLib.h" - -typedef struct { _iq Ualpha; //phase A voltage, input - _iq Ubeta; //phase B voltage, input - _iq Tetta; //phase angle, input - _iq Ud; // axis d voltage, output - _iq Uq; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - }ALPHABETA_TO_DQ; - - - - - -typedef ALPHABETA_TO_DQ *ALPHABETA_TO_DQ_handle; - -#define ALPHABETA_TO_DQ_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(Uint32))alphabeta_to_dq_calc \ - } - - -void alphabeta_to_dq_calc(ALPHABETA_TO_DQ_handle); - - -#endif // end __ALPHABETA_DQ_H diff --git a/Inu/Src2/VectorControl/dq_to_alphabeta_cos.c b/Inu/Src2/VectorControl/dq_to_alphabeta_cos.c deleted file mode 100644 index e0dd2f8..0000000 --- a/Inu/Src2/VectorControl/dq_to_alphabeta_cos.c +++ /dev/null @@ -1,39 +0,0 @@ -#include "dq_to_alphabeta_cos.h" - -#include "IQmathLib.h" // Include header for IQmath library - - - - - -///////////////////////////////////////////////// - - -#pragma CODE_SECTION(dq_to_alphabeta_calc,".fast_run2"); -void dq_to_alphabeta_calc(DQ_TO_ALPHABETA_handle v) -{ - - v->Ualpha = _IQmpy(v->Ud, _IQcos(v->Tetta)) + _IQmpy(v->Uq, _IQsin(v->Tetta)); - v->Ubeta = -_IQmpy(v->Ud, _IQsin(v->Tetta)) + _IQmpy(v->Uq, _IQcos(v->Tetta)); - -} - - -#pragma CODE_SECTION(dq_to_alphabeta_calc2,".fast_run2"); -void dq_to_alphabeta_calc2(DQ_TO_ALPHABETA_handle v) -{ - - v->Ualpha = _IQmpy(v->Ud, _IQsin(v->Tetta)) + _IQmpy(v->Uq, _IQcos(v->Tetta)); - v->Ubeta = -_IQmpy(v->Ud, _IQcos(v->Tetta)) + _IQmpy(v->Uq, _IQsin(v->Tetta)); - -} - -#pragma CODE_SECTION(dq_to_alphabeta_calc_cos,".fast_run2"); -void dq_to_alphabeta_calc_cos(DQ_TO_ALPHABETA_handle v) -{ - - v->Ualpha = _IQmpy(v->Ud, _IQcos(v->Tetta)) - _IQmpy(v->Uq, _IQsin(v->Tetta)); - v->Ubeta = _IQmpy(v->Ud, _IQsin(v->Tetta)) + _IQmpy(v->Uq, _IQcos(v->Tetta)); - -} -///////////////////////////////////////////////// diff --git a/Inu/Src2/VectorControl/dq_to_alphabeta_cos.h b/Inu/Src2/VectorControl/dq_to_alphabeta_cos.h deleted file mode 100644 index b261ced..0000000 --- a/Inu/Src2/VectorControl/dq_to_alphabeta_cos.h +++ /dev/null @@ -1,40 +0,0 @@ - - - -#ifndef __DQ_ALPHABETA_H__ -#define __DQ_ALPHABETA_H__ - -#include "IQmathLib.h" - -typedef struct { _iq Ualpha; //phase A voltage, input - _iq Ubeta; //phase B voltage, input - _iq Tetta; //phase angle, input - _iq Ud; // axis d voltage, output - _iq Uq; // axis q voltage, output - void (*calc)(); // Pointer to calculation function - void (*calc2)(); // Pointer to calculation function. Like in MATLAB - void (*calc_cos)(); // Pointer to calculation function, Ualpha = Uq*Cos(Tetta) - }DQ_TO_ALPHABETA; - - - - - -typedef DQ_TO_ALPHABETA *DQ_TO_ALPHABETA_handle; - -#define DQ_TO_ALPHABETA_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(Uint32))dq_to_alphabeta_calc, \ - (void (*)(Uint32))dq_to_alphabeta_calc2, \ - (void (*)(Uint32))dq_to_alphabeta_calc_cos \ - } - - -void dq_to_alphabeta_calc(DQ_TO_ALPHABETA_handle); -void dq_to_alphabeta_calc2(DQ_TO_ALPHABETA_handle); -void dq_to_alphabeta_calc_cos(DQ_TO_ALPHABETA_handle); - -#endif // end __DQ_ALPHABETA_H__ diff --git a/Inu/Src2/VectorControl/efficiency_compensation.c b/Inu/Src2/VectorControl/efficiency_compensation.c deleted file mode 100644 index 16ab749..0000000 --- a/Inu/Src2/VectorControl/efficiency_compensation.c +++ /dev/null @@ -1,107 +0,0 @@ -#include "IQmathLib.h" -#include "efficiency_compensation.h" - - -static int DetectCommandRegion(_iq *power); - -/***********************************************************/ -/** -* -*Increases the power demand depending on the machine efficiency -* -*@param p_zad - pointer to demanded power -*@param i_q - pointer to value of active current -* -*@return return value throw the pointer -* -*************************************************************/ -void EfficiencyCompensation(_iq *p_zad, _iq *i_q) -{ - static _iq efficiency_nominal = 16139681L; //96.2% - static _iq i_q_nom_levels[] = {3635063L, 4395630L, 4977240L, 5536481L, 6011835L, 6487190L, - 6934582L, 7381975L, 7751073L, 8220835L}; - - /** - * Coefficients of a square trinomial calculating the correction - */ - static _iq a0[10] = {17585441L, 17401328L, 17206712L, 17060750L, 17074172L, 16904722L, - 17129537L, 17698285L, 17188257L, 17740228L}; - static _iq a1[10] = {-2030043L, -1358954L, -905969L, -587202L, -553648L, -318767L, - -536870L, -1224736L, -570425L, -1224736L}; - static _iq a2[10] = {-385875L, -385875L, -385875L, -402653L, -335544L, - -348966L, -234881L, 0L, -167772L, 50331L}; - _iq efficiency, p_local; - _iq i_q_abs; - int region = 0; - region = DetectCommandRegion(p_zad); - efficiency = efficiency_nominal; - i_q_abs = _IQabs(*i_q); - p_local = *p_zad; - - if (i_q_abs <= i_q_nom_levels[region]) { - *p_zad = _IQdiv(*p_zad, efficiency_nominal); - return; - } - /** - * This is because of all electric values are normalized by 3000, - * but in a model in this place the current is normalized by 1000. - */ - i_q_abs = i_q_abs * 3; - efficiency = _IQmpy(i_q_abs, i_q_abs); - efficiency = a0[region] + _IQmpy(a1[region], i_q_abs) + _IQmpy(a2[region], efficiency); - p_local = _IQdiv(p_local, efficiency); - *p_zad = p_local; -} - - - -#define BOUND_1p5_MWT 2796202L -#define BOUND_2p5_MWT 4660337L -#define BOUND_3p5_MWT 6524472L -#define BOUND_4p5_MWT 8388608L -#define BOUND_6p5_MWT 12116878L -#define BOUND_7p5_MWT 13981013L -#define BOUND_8p5_MWT 15845148L -#define BOUND_5p5_MWT 10252743L -#define BOUND_9p5_MWT 17709283L - -/**************************************************************/ -/** - * Detects command mode: - * 0 corresponds to 1 MWt - * 1 - 2 MWt - * ... - * 9 - 10 MWt - * - * @param power - pointer to demanded power in watts - * - * @return corresponding mode - */ -int DetectCommandRegion(_iq *power) -{ - int region = 0; - if (*power < BOUND_1p5_MWT) - { - region = 0; - } else if (*power < BOUND_2p5_MWT) { - region = 1; - } else if (*power < BOUND_3p5_MWT) { - region = 2; - } else if (*power < BOUND_4p5_MWT) { - region = 3; - } else if (*power < BOUND_5p5_MWT) { - region = 4; - } else if (*power < BOUND_6p5_MWT) { - region = 5; - } else if (*power < BOUND_7p5_MWT) { - region = 6; - } else if (*power < BOUND_8p5_MWT) { - region = 7; - } else if (*power < BOUND_9p5_MWT) { - region = 8; - } else { - region = 9; - } - - return region; -} diff --git a/Inu/Src2/VectorControl/efficiency_compensation.h b/Inu/Src2/VectorControl/efficiency_compensation.h deleted file mode 100644 index f1d744b..0000000 --- a/Inu/Src2/VectorControl/efficiency_compensation.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * efficiency_compensation.h - * - * Created on: 26 . 2018 . - * Author: stud - */ - -#ifndef SRC_VECTORCONTROL_EFFICIENCY_COMPENSATION_H_ -#define SRC_VECTORCONTROL_EFFICIENCY_COMPENSATION_H_ - -void EfficiencyCompensation(_iq *p_zad, _iq *i_q); - -#endif /* SRC_VECTORCONTROL_EFFICIENCY_COMPENSATION_H_ */ diff --git a/Inu/Src2/VectorControl/params_motor.h b/Inu/Src2/VectorControl/params_motor.h deleted file mode 100644 index 5f050ca..0000000 --- a/Inu/Src2/VectorControl/params_motor.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * params_motor.h - * - * Created on: 14 . 2020 . - * Author: yura - */ - -#ifndef SRC_MAIN_PARAMS_MOTOR_H_ -#define SRC_MAIN_PARAMS_MOTOR_H_ - - -#define L_SIGMA_S 0.000964 -#define L_SIGMA_R 0.001134 -#define L_M 0.03889 -#define R_STATOR 0.0118 -#define R_ROTOR_SHTRIH 0.0111 -#define SLIP_NOM 0.0106 -#define R_ROTOR (R_ROTOR_SHTRIH / SLIP_NOM) -#define F_STATOR_NOM 12.0 - - - -#endif /* SRC_MAIN_PARAMS_MOTOR_H_ */ diff --git a/Inu/Src2/VectorControl/pwm_vector_regul.c b/Inu/Src2/VectorControl/pwm_vector_regul.c deleted file mode 100644 index 3a45fc4..0000000 --- a/Inu/Src2/VectorControl/pwm_vector_regul.c +++ /dev/null @@ -1,666 +0,0 @@ -#include "IQmathLib.h" -#include "math.h" -#include "my_filter.h" -#include "params.h" -#include "adc_tools.h" -#include "abc_to_dq.h" -#include "regul_power.h" -#include "regul_turns.h" -#include "teta_calc.h" -#include "pwm_vector_regul.h" -#include "pid_reg3.h" -#include "vector.h" -#include "params_motor.h" -#include "v_pwm24.h" -//#include "power_distribution.h" -#include "rotation_speed.h" -//#include "calc_3_power.h" - -// #pragma DATA_SECTION(pidD,".fast_vars1"); -// PIDREG3 pidD = PIDREG3_UNDEPENDENT_DEFAULTS; -PIDREG3 pidD = PIDREG3_DEFAULTS; -// #pragma DATA_SECTION(pidQ,".fast_vars1"); -// PIDREG3 pidQ = PIDREG3_UNDEPENDENT_DEFAULTS; -PIDREG3 pidQ = PIDREG3_DEFAULTS; - -// #pragma DATA_SECTION(pidD2,".fast_vars1"); -// PIDREG3 pidD2 = PIDREG3_UNDEPENDENT_DEFAULTS; -PIDREG3 pidD2 = PIDREG3_DEFAULTS; -// #pragma DATA_SECTION(pidQ2,".fast_vars1"); -// PIDREG3 pidQ2 = PIDREG3_UNDEPENDENT_DEFAULTS; -PIDREG3 pidQ2 = PIDREG3_DEFAULTS; - -// #pragma DATA_SECTION(cos_fi,".fast_vars1"); -COS_FI_STRUCT cos_fi = {0,0}; - -// #pragma DATA_SECTION(vect_control,".fast_vars1"); -VECTOR_CONTROL vect_control = VECTOR_CONTROL_DEFAULTS; - -// #pragma DATA_SECTION(koeff_Fs_filter,".fast_vars"); -long koeff_Fs_filter = _IQ(0.01); - -// _iq winding_displacement = FIXED_PIna6; - -WINDING a; -FLAG f; - - -ROTOR_VALUE rotor = ROTOR_VALUE_DEFAULTS; - -#define IQ_ONE 16777216 - -#define CONST_IQ_2PI 105414357 -#define CONST_IQ_2PI3 35138119 // 120 grad -#define CONST_IQ_PI2 26340229 - -#define K_MODUL_MIN 167772 //83886 //0.5% - - -#define K_MODUL_MAX 15435038LL //13421772LL //80% 16106127LL ~ 96% //15435038LL ~ 0.92% - //15770583LL ~ 0.94% -#define K_MODUL_MAX_PART 15099494LL // 15099494LL ~ 0.9 - -#define I_ZERO_LEVEL_IQ 111848 //20A //279620 ~ 50A //55924 ~ 10A -#define K_MODUL_DEAD_TIME 503316 //3% - -//#define I_ZERO_LEVEL_IQ 27962 //5A //55924 ~ 10A - -#define Id_MIN 1118481 //200A //111848 ~ 20A //55924 ~ 10A - -// #pragma DATA_SECTION(zadan_Id_min,".fast_vars"); -_iq zadan_Id_min = Id_MIN; - -#define START_TETTA_IQ 4392264 //15 grad - -void analog_dq_calc(void); -void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2); -void calcUdUqCompensation(_iq Frot); - -float kI_D = 0.2; -float kI_Q = 0.2; -float kP_D = 0.1;//.1; -float kP_Q = 0.3;//.3; - -// float kI_D_rev = 0.02; -// float kI_Q_rev = 0.03; -// float kP_D_rev = 0.1;//.3; -// float kP_Q_rev = 0.15;//.3; - -void init_DQ_pid() -{ - unsigned int i = 0; - int *pStr = (int*)&f; - for (i = 0; i < sizeof(f) / sizeof(int); i++) { - *(pStr + i) = 0; - } - - *pStr = (int*)&a; - for (i = 0; i < sizeof(a) / sizeof(int); i++) { - *(pStr + i) = 0; - } - - pidD.Ref = 0; - pidD.Kp = _IQ(kP_D);//_IQ(0.2); //_IQ(0.1); - pidD.Ki = _IQ(kI_D); //_IQ(0.2);// // - pidD.Kc = _IQ(0.3); - pidD.Kd = 0; - pidD.OutMax = K_MODUL_MAX_PART; - pidD.OutMin = -K_MODUL_MAX_PART; //0; - pidD.Up = 0; - pidD.Ui = 0; - pidD.Ud = 0; - pidD.Out = 0; - - pidQ.Ref = 0; - pidQ.Kp = _IQ(kP_Q);//_IQ(0.3);//_IQ(0.3); - pidQ.Ki = _IQ(kI_Q); //_IQ(0.2); // - pidQ.Kc = _IQ(0.3); - pidQ.Kd = 0; - pidQ.OutMax = K_MODUL_MAX_PART; //_IQ(0.9); - pidQ.OutMin = -K_MODUL_MAX_PART; //-_IQ(0.9); //0; - pidQ.Up = 0; - pidQ.Ui = 0; - pidQ.Ud = 0; - pidQ.Out = 0; - - pidD2.Ref = 0; - pidD2.Kp = _IQ(kP_D);//_IQ(0.1); //_IQ(0.1); - pidD2.Ki = _IQ(kI_D); //_IQ(0.2);// // - pidD2.Kc = _IQ(0.3); - pidD2.Kd = 0; - pidD2.OutMax = K_MODUL_MAX_PART; - pidD2.OutMin = -K_MODUL_MAX_PART; //0; - pidD2.Up = 0; - pidD2.Ui = 0; - pidD2.Ud = 0; - pidD2.Out = 0; - - pidQ2.Ref = 0; - pidQ2.Kp = _IQ(kP_Q);// _IQ(0.3);//_IQ(0.3); - pidQ2.Ki = _IQ(kI_Q); //_IQ(0.2); // - pidQ2.Kc = _IQ(0.3); - pidQ2.Kd = 0; - pidQ2.OutMax = K_MODUL_MAX_PART; //_IQ(0.9); - pidQ2.OutMin = -K_MODUL_MAX_PART; //-_IQ(0.9); //0; - pidQ2.Up = 0; - pidQ2.Ui = 0; - pidQ2.Ud = 0; - pidQ2.Out = 0; - - init_tetta_pid(); - init_Fvect_pid(); - init_Pvect_pid(); - init_tetta_calc_struct(); - - init_Adc_Variables(); - - cos_fi.cos_fi_nom = _IQ(0.87); - cos_fi.cos_fi_nom_squared = _IQ(0.87 * 0.87); - // cos_fi.cos_fi_nom = _IQ(COS_FI); - // cos_fi.cos_fi_nom_squared = _IQ(COS_FI * COS_FI); - - vect_control.koef_Ud_comp = _IQ((L_SIGMA_S + L_M * L_SIGMA_R / (L_M + L_SIGMA_R)) * 2 * 3.14 * NORMA_FROTOR); //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r) - vect_control.koef_Uq_comp = _IQ((L_M + L_SIGMA_S) * 2 * 3.14 * NORMA_FROTOR); - vect_control.k_modul_max = K_MODUL_MAX; - vect_control.k_modul_max_square = _IQmpy(K_MODUL_MAX, K_MODUL_MAX); - vect_control.iq_Id_out_max = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP); - - vect_control.koeff_correct_Id = IQ_ONE; - - vect_control.equial_Iq_Proportional = _IQ(0.05); - vect_control.flag_reverse = 0; -} - -void reset_DQ_pid() -{ - pidD.Up = 0; - pidD.Up1 = 0; - pidD.Ui = 0; - pidD.Ud = 0; - pidD.Out = 0; - pidQ.Up = 0; - pidQ.Up1 = 0; - pidQ.Ui = 0; - pidQ.Ud = 0; - pidQ.Out = 0; - - pidD2.Up = 0; - pidD2.Up1 = 0; - pidD2.Ui = 0; - pidD2.Ud = 0; - pidD2.Out = 0; - pidQ2.Up = 0; - pidQ2.Up1 = 0; - pidQ2.Ui = 0; - pidQ2.Ud = 0; - pidQ2.Out = 0; -} - -// #pragma CODE_SECTION(Idq_to_Udq,".fast_run2"); -void Idq_to_Udq(_iq Id_zad, _iq Iq_zad, _iq Id_measured, _iq Iq_measured, _iq* Ud_zad, _iq* Uq_zad) -{ - pidD.Ref = Id_zad; - pidD.Fdb = Id_measured; - pidD.calc(&pidD); - *Ud_zad = pidD.Out; - - pidQ.Ref = Iq_zad; - pidQ.Fdb = Iq_measured; - pidQ.calc(&pidQ); - *Uq_zad = pidQ.Out; - -} - -// inline void set_pid_coeffs() { -// if (vect_control.flag_reverse == 0) { -// pidD.Kp = _IQ(kP_D);//_IQ(0.2); //_IQ(0.1); -// pidD.Ki = _IQ(kI_D); //_IQ(0.2);// // -// pidQ.Kp = _IQ(kP_Q);//_IQ(0.3);//_IQ(0.3); -// pidQ.Ki = _IQ(kI_Q); //_IQ(0.2); // -// pidD2.Kp = _IQ(kP_D);//_IQ(0.1); //_IQ(0.1); -// pidD2.Ki = _IQ(kI_D); //_IQ(0.2);// // -// pidQ2.Kp = _IQ(kP_Q);// _IQ(0.3);//_IQ(0.3); -// pidQ2.Ki = _IQ(kI_Q); //_IQ(0.2); // -// } else { -// pidD.Kp = _IQ(kP_D_rev);//_IQ(0.2); //_IQ(0.1); -// pidD.Ki = _IQ(kI_D_rev); //_IQ(0.2);// // -// pidQ.Kp = _IQ(kP_Q_rev);//_IQ(0.3);//_IQ(0.3); -// pidQ.Ki = _IQ(kI_Q_rev); //_IQ(0.2); // -// pidD2.Kp = _IQ(kP_D_rev);//_IQ(0.1); //_IQ(0.1); -// pidD2.Ki = _IQ(kI_D_rev); //_IQ(0.2);// // -// pidQ2.Kp = _IQ(kP_Q_rev);// _IQ(0.3);//_IQ(0.3); -// pidQ2.Ki = _IQ(kI_Q_rev); //_IQ(0.2); // -// } -// } - -void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad, _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1 - , _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2) -{ - pidD.Ref = Id_zad; - pidD.Fdb = Id_measured1; - pidD.calc(&pidD); - *Ud_zad1 = pidD.Out; - - pidQ.Ref = Iq_zad; - pidQ.Fdb = Iq_measured1; - pidQ.calc(&pidQ); - *Uq_zad1 = pidQ.Out; - - pidD2.Ref = Id_zad; - pidD2.Fdb = Id_measured2; - pidD2.calc(&pidD2); - *Ud_zad2 = pidD2.Out; - - pidQ2.Ref = Iq_zad; - pidQ2.Fdb = Iq_measured2; - pidQ2.calc(&pidQ2); - *Uq_zad2 = pidQ2.Out; -} - -// #pragma CODE_SECTION(pwm_vector_model_titov,".fast_run"); -void pwm_vector_model_titov(_iq Pzad, _iq Fzad, int direction, _iq Frot, int mode, - int reset, int calcPWMtimes) -{ - int calc_channel = 1; - _iq U_zad1 = 0, U_zad2 = 0; - - - // if (f.flag_leading == 0 && f.read_task_from_optical_bus == 1 && f.sync_rotor_from_optical_bus == 1) { - // Frot = rotor.iqFrotFromOptica; - // } - - // Id - if (_IQabs(Frot) > IQ_F_ROTOR_NOM) { - vect_control.koeff_correct_Id = _IQdiv(IQ_F_ROTOR_NOM, _IQabs(Frot)); - } else { - vect_control.koeff_correct_Id = IQ_ONE; - } - - - if(direction < 0) { Frot = -Frot; } -// prev_dir = direction; - if(reset == 1) // - - - { - reset_DQ_pid(); - reset_tetta_pid(); - analog.iqIq1_filter = 0; - analog.iqIq2_filter = 0; - analog.iqUq2_filter = 0; - U_zad1 = K_MODUL_MIN; - U_zad2 = K_MODUL_MIN; - vect_control.iq_Id_out_max = Id_out_max_low_speed; - vect_control.flag_reverse = 0; - } - - set_cos_tetta_calc_params(); - analog_dq_calc(); - - - vector_turns(Fzad, Frot, analog.iqIq1, mode, &vect_control.iqIq_zad, &vect_control.iqId_zad); - vector_power(Pzad, analog.iqPvsi1 + analog.iqPvsi2, analog.iqIq1, mode, Frot, &vect_control.iqIq_zad, &vect_control.iqId_zad); - - vect_control.iqPzad = pidPvect.Ref; - vect_control.iqPizm = pidPvect.Fdb; - vect_control.iqFrot = Frot; - - // Id - if (vect_control.iqId_zad < Id_MIN) { vect_control.iqId_zad = zadan_Id_min; } - - // Id - // if (_IQabs(Frot) > IQ_F_ROTOR_NOM) { - // vect_control.iqId_zad = _IQmpy(vect_control.iqId_zad, vect_control.koeff_correct_Id); - // } - - -// Frot =_IQ(15.0 / 6 / NORMA_FROTOR); -// vect_control.iqIq_zad = _IQ(IQ_OUT_NOM / NORMA_ACP); -// vect_control.iqId_zad = 0;// _IQ(789.0 / NORMA_ACP); - - // set_pid_coeffs(); - Idq_to_Udq_2_windings(vect_control.iqId_zad, vect_control.iqIq_zad, analog.iqId1, analog.iqIq1, &vect_control.iqUdKm1, &vect_control.iqUqKm1, - analog.iqId2, analog.iqIq2, &vect_control.iqUdKm2, &vect_control.iqUqKm2); - - - calc_tetta_Id(Frot, vect_control.iqId_zad, vect_control.iqIq_zad, &analog.tetta, &analog.Fsl, &analog.iqFstator, reset); - filter.Fsl = exp_regul_iq(koeff_Fs_filter, filter.Fsl, analog.Fsl); - - // analog.tetta = _IQ(vect_control.theta); - analog.iqIq_zadan = vect_control.iqIq_zad; - - // calcUdUqCompensation(Frot); //TODO: test on power after testing rotating - vect_control.iqUdKm1Out = vect_control.iqUdKm1; - vect_control.iqUqKm1Out = vect_control.iqUqKm1; - vect_control.iqUdKm2Out = vect_control.iqUdKm2; - vect_control.iqUqKm2Out = vect_control.iqUqKm2; - - if(vect_control.iqUdKm1Out > K_MODUL_MAX_PART) { vect_control.iqUdKm1Out = K_MODUL_MAX_PART;} - if(vect_control.iqUdKm1Out < -K_MODUL_MAX_PART) { vect_control.iqUdKm1Out = -K_MODUL_MAX_PART;} - if(vect_control.iqUqKm1Out > K_MODUL_MAX_PART) { vect_control.iqUqKm1Out = K_MODUL_MAX_PART;} - if(vect_control.iqUqKm1Out < -K_MODUL_MAX_PART) { vect_control.iqUqKm1Out = -K_MODUL_MAX_PART;} - - if(vect_control.iqUdKm2Out > K_MODUL_MAX_PART) { vect_control.iqUdKm2Out = K_MODUL_MAX_PART;} - if(vect_control.iqUdKm2Out < -K_MODUL_MAX_PART) { vect_control.iqUdKm2Out = -K_MODUL_MAX_PART;} - if(vect_control.iqUqKm2Out > K_MODUL_MAX_PART) { vect_control.iqUqKm2Out = K_MODUL_MAX_PART;} - if(vect_control.iqUqKm2Out < -K_MODUL_MAX_PART) { vect_control.iqUqKm2Out = -K_MODUL_MAX_PART;} - - U_zad1 = _IQsqrt(_IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out) + _IQmpy(vect_control.iqUqKm1Out, vect_control.iqUqKm1Out)); - U_zad2 = _IQsqrt(_IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out) + _IQmpy(vect_control.iqUqKm2Out, vect_control.iqUqKm2Out)); - vect_control.iqUzad1 = U_zad1; - vect_control.iqUzad2 = U_zad2; - - if (U_zad1 > vect_control.k_modul_max) { - if (vect_control.iqUqKm1Out > 0) { - vect_control.iqUqKm1Out = _IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out)); - } else { - vect_control.iqUqKm1Out = -_IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out)); - } - } - if (U_zad2 > vect_control.k_modul_max) { - if (vect_control.iqUqKm2Out > 0) { - vect_control.iqUqKm2Out = _IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out)); - } else { - vect_control.iqUqKm2Out = -_IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out)); - } - } - - // Iq - // vect_control.equial_Iq_Delta = analog.iqIq1 - analog.iqIq2; - // vect_control.equial_Iq_Out = _IQmpy(vect_control.equial_Iq_Delta, - // vect_control.equial_Iq_Proportional); - // vect_control.iqUqKm1Out -= vect_control.equial_Iq_Out; - // vect_control.iqUqKm2Out += vect_control.equial_Iq_Out; - - // if (U_zad1 < K_MODUL_DEAD_TIME) { - // vect_control.iqUdKm1 = 0; - // vect_control.iqUqKm1 = 0; - // } - // if (U_zad2 < K_MODUL_DEAD_TIME) { - // vect_control.iqUdKm2 = 0; - // vect_control.iqUqKm2 = 0; - // } - - vect_control.iqUzad1 = _IQsqrt(_IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out) + _IQmpy(vect_control.iqUqKm1Out, vect_control.iqUqKm1Out)); - vect_control.iqUzad2 = _IQsqrt(_IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out) + _IQmpy(vect_control.iqUqKm2Out, vect_control.iqUqKm2Out)); - - analog_Udq_calc(vect_control.iqUdKm1, vect_control.iqUqKm1, vect_control.iqUdKm2, vect_control.iqUqKm2); - - - if(mode == 0) // - - { - U_zad1 = K_MODUL_MIN; - U_zad2 = K_MODUL_MIN; - vect_control.iqUdKm1Out = 0; - vect_control.iqUqKm1Out = 0; - vect_control.iqUdKm2Out = 0; - vect_control.iqUqKm2Out = 0; -// teta_st = analog.tetta; - } - - a.iqk1 = vect_control.iqUzad1; -// analog.iqUdKm = Ud_zad; -// analog.iqUqKm = Uq_zad; -// analog.iqId_zad = vect_control.iqId_zad; - - - if(mode && f.Go && calcPWMtimes) - { - test_calc_dq_pwm24(vect_control.iqUdKm1Out, vect_control.iqUqKm1Out, vect_control.iqUdKm2Out, vect_control.iqUqKm2Out, analog.tetta,K_MODUL_MAX); - } - - -} - - - -// #pragma DATA_SECTION(koeff_Iq_filter_slow,".fast_vars"); -long koeff_Iq_filter_slow = _IQ(0.5); //_IQ(0.3); -// #pragma DATA_SECTION(koeff_Idq_filter,".fast_vars"); -long koeff_Idq_filter = _IQ(0.15); -// #pragma DATA_SECTION(koeff_Uq_filter,".fast_vars"); -long koeff_Uq_filter = 500; -// #pragma DATA_SECTION(koeff_Ud_filter,".fast_vars"); -long koeff_Ud_filter = _IQ(0.5); - - -// #pragma CODE_SECTION(analog_dq_calc,".fast_run"); -void analog_dq_calc() -{ - ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS; - - abc_dq_converter.Ia = analog.iqIa1_1; - abc_dq_converter.Ib = analog.iqIb1_1; - abc_dq_converter.Ic = analog.iqIc1_1; - abc_dq_converter.Tetta = analog.tetta; - abc_dq_converter.calc(&abc_dq_converter); - analog.iqId1 = abc_dq_converter.Id; - filter.iqId1 = exp_regul_iq(koeff_Idq_filter, filter.iqId1, analog.iqId1); - analog.iqIq1 = abc_dq_converter.Iq; - filter.iqIq1 = exp_regul_iq(koeff_Idq_filter, filter.iqIq1, analog.iqIq1); -// analog.iqIq1_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow, -// analog.iqIq1_filter, analog.iqIq1); - analog.iqIq1_filter = exp_regul_iq(koeff_Iq_filter_slow, analog.iqIq1_filter, analog.iqIq1); - - abc_dq_converter.Ia = analog.iqIa2_1; - abc_dq_converter.Ib = analog.iqIb2_1; - abc_dq_converter.Ic = analog.iqIc2_1; - abc_dq_converter.Tetta = analog.tetta - winding_displacement; - abc_dq_converter.calc(&abc_dq_converter); - analog.iqId2 = abc_dq_converter.Id; - filter.iqId2 = exp_regul_iq(koeff_Idq_filter, filter.iqId2, analog.iqId2); - analog.iqIq2 = abc_dq_converter.Iq; - filter.iqIq2 = exp_regul_iq(koeff_Idq_filter, filter.iqIq2, analog.iqIq2); -// analog.iqIq2_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow, -// analog.iqIq2_filter, analog.iqIq2); - analog.iqIq2_filter = exp_regul_iq(koeff_Iq_filter_slow, analog.iqIq2_filter, analog.iqIq2); - - if (_IQabs(analog.iqId1) < I_ZERO_LEVEL_IQ) { analog.iqId1 = 0; } - if (_IQabs(analog.iqIq1) < I_ZERO_LEVEL_IQ) { analog.iqIq1 = 0; } - if (_IQabs(analog.iqId2) < I_ZERO_LEVEL_IQ) { analog.iqId2 = 0; } - if (_IQabs(analog.iqIq2) < I_ZERO_LEVEL_IQ) { analog.iqIq2 = 0; } - -// analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, analog.iqUq1), 25165824L); -// analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, analog.iqUq2), 25165824L); - - analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, _IQabs(analog.iqUq1)), 25165824L); - analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, _IQabs(analog.iqUq2)), 25165824L); - -// analog.iqM_1 = _IQdiv(analog.iqPvsi1, _IQmpy(rotor.iqW, CONST_IQ_2PI)); -// analog.iqM_2 = _IQdiv(analog.iqPvsi2, _IQmpy(rotor.iqW, CONST_IQ_2PI)); -// logpar.log11 = (int16)_IQtoIQ15(analog.iqIq1_filter); -} - -// #pragma CODE_SECTION(analog_Udq_calc,".fast_run"); -void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2) -{ - analog.iqUd1 = _IQmpy(Ud1, _IQmpy((filter.iqU_1_long + filter.iqU_2_long), 8388608L)); // 8388608 = _IQ(0.5) - analog.iqUq1 = _IQmpy(Uq1, _IQmpy((filter.iqU_1_long + filter.iqU_2_long), 8388608L)); - analog.iqUd2 = _IQmpy(Ud2, _IQmpy((filter.iqU_3_long + filter.iqU_4_long), 8388608L)); - analog.iqUq2 = _IQmpy(Uq2, _IQmpy((filter.iqU_3_long + filter.iqU_4_long), 8388608L)); - - filter.iqUd1 = exp_regul_iq(koeff_Ud_filter, filter.iqUd1, analog.iqUd1); - -} - - -//#pragma CODE_SECTION(analog_dq_calc_const,".fast_run"); -void analog_dq_calc_const() -{ - ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS; - - abc_dq_converter.Ia = analog.iqIa1_1; - abc_dq_converter.Ib = analog.iqIb1_1; - abc_dq_converter.Ic = analog.iqIc1_1; - abc_dq_converter.Tetta = analog.tetta; // svgen_pwm24_1.Alpha; - abc_dq_converter.calc(&abc_dq_converter); - analog.iqId1 = abc_dq_converter.Id; - analog.iqIq1 = abc_dq_converter.Iq; - analog.iqIq1_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow, - analog.iqIq1_filter, analog.iqIq1); - - abc_dq_converter.Ia = analog.iqIa2_1; - abc_dq_converter.Ib = analog.iqIb2_1; - abc_dq_converter.Ic = analog.iqIc2_1; - abc_dq_converter.Tetta = analog.tetta; // svgen_pwm24_2.Alpha; - abc_dq_converter.calc(&abc_dq_converter); - analog.iqId2 = abc_dq_converter.Id; - analog.iqIq2 = abc_dq_converter.Iq; - analog.iqIq2_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow, - analog.iqIq2_filter, analog.iqIq2); - - analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, analog.iqUq1), 25165824L); - analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, analog.iqUq2), 25165824L); -// analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1, analog.iqUq1), 25165824L); -// analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2, analog.iqUq2), 25165824L); - -// analog.iqM_1 = _IQdiv(analog.iqPvsi1, _IQmpy(rotor.iqW, CONST_IQ_2PI)); -// analog.iqM_2 = _IQdiv(analog.iqPvsi2, _IQmpy(rotor.iqW, CONST_IQ_2PI)); -} - -void calcUdUqCompensation(_iq Frot) { - _iq Uzpt = (filter.iqU_1_long + filter.iqU_2_long) >> 1; - _iq UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), analog.iqIq1); - _iq UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), analog.iqId1); - if (Uzpt != 0) { - vect_control.iqUdCompensation1 = -_IQdiv(UdVolt, Uzpt); - vect_control.iqUqCompensation1 = _IQdiv(UqVolt, Uzpt); - } else { - vect_control.iqUdCompensation1 = 0; - vect_control.iqUqCompensation1 = 0; - } - - vect_control.iqUdKm1Out = vect_control.iqUdKm1 + vect_control.iqUdCompensation1; - vect_control.iqUqKm1Out = vect_control.iqUqKm1 + vect_control.iqUqCompensation1; - - Uzpt = (filter.iqU_3_long + filter.iqU_4_long) >> 1; - UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), analog.iqIq2); - UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), analog.iqId2); - if (Uzpt != 0) { - vect_control.iqUdCompensation2 = -_IQdiv(UdVolt, Uzpt); - vect_control.iqUqCompensation2 = _IQdiv(UqVolt, Uzpt); - } else { - vect_control.iqUdCompensation2 = 0; - vect_control.iqUqCompensation2 = 0; - } - - - vect_control.iqUdKm2Out = vect_control.iqUdKm2 + vect_control.iqUdCompensation2; - vect_control.iqUqKm2Out = vect_control.iqUqKm2 + vect_control.iqUqCompensation2; -} - -#define IQ_150_RPM 2097152 //150/ -#define IQ_140_RPM 1957341 //140/ -//#define IQ_135_RPM 1887436 //135/ -#define IQ_125_RPM 1747626 //125/ -#define IQ_120_RPM 1677721 //120/ -#define IQ_115_RPM 1607816 //115/ -#define IQ_110_RPM 1537911 //110/ - -#define K_MODUL_SWITCH_COS_FI 13925089 //83% -#define K_MODUL_SWITCH_COS_FI_2 14260633 //85% -#define K_MODUL_SWITCH_OFF 10905190 //65% //11744051 ~ 70% //12582912 ~ 75% - -#define U200V 1118481 - - -// #pragma CODE_SECTION(set_cos_tetta_calc_params,".fast_run1"); -void set_cos_tetta_calc_params() { - - _iq currentCos = _IQ(COS_FI); - _iq currentCosSq = _IQ(COS_FI * COS_FI); - static _iq cosFi_low_speed = _IQ(0.85); - static _iq cosFi_Sq_low_speed = _IQ(0.85 * 0.85); - static _iq cosFi_mid_speed = _IQ(COS_FI); - static _iq cosFi_Sq_mid_speed = _IQ(COS_FI * COS_FI); - static _iq cosFi_high_speed = _IQ(0.89); - static _iq cosFi_Sq_high_speed = _IQ(0.89 * 0.89); - static _iq stepChangeCos = _IQ(0.001); - static _iq kt_low_speed = _IQ(0.0045); - static _iq kt_over_140rpm = _IQ(0.0048); - static _iq kt_over_150rpm = _IQ(0.0049); - static _iq kt_single_inv = _IQ(0.0039); - - _iq current_kt = _IQ(0.0045); - static _iq stepChangeKt =_IQ(0.00001); - - static _iq addCompensateUd = _IQ(0.0004); - - static unsigned int flag_high_Km = 0; - - if (a.iqk1 < K_MODUL_SWITCH_OFF) { - flag_high_Km = 0; - } - -// if (_IQabs(rotor.iqFout) > IQ_150_RPM) { -// currentCos = _IQ(0.9); -// currentCosSq = _IQ(0.9 * 0.9); -//// tetta_calc.k_t = _IQ(0.0049); -// current_kt = kt_over_150rpm; //_IQ(0.0049); -// } else - if (_IQabs(rotor.iqFout) > IQ_140_RPM || (a.iqk1 > K_MODUL_SWITCH_COS_FI_2) - || flag_high_Km == 1) { - if (a.iqk1 > K_MODUL_SWITCH_COS_FI_2) { - flag_high_Km = 1; - } - currentCos = cosFi_high_speed; - currentCosSq = cosFi_Sq_high_speed; -// tetta_calc.k_t = _IQ(0.0048); - current_kt = kt_over_140rpm; //_IQ(0.0048); - } else if ((_IQabs(rotor.iqFout) > IQ_115_RPM && cos_fi.cos_fi_nom < _IQ(0.889)) - || (_IQabs(rotor.iqFout) < IQ_135_RPM && cos_fi.cos_fi_nom > _IQ(0.889)) - || (a.iqk1 > K_MODUL_SWITCH_COS_FI)) { - currentCos = cosFi_mid_speed; - currentCosSq = cosFi_Sq_mid_speed; -// tetta_calc.k_t = _IQ(0.0045); - current_kt = kt_low_speed; //_IQ(0.0045); - } else if (_IQabs(rotor.iqFout) < IQ_110_RPM) { - currentCos = cosFi_low_speed; - currentCosSq = cosFi_Sq_low_speed; - //(f.secondPChState != 4) - if(((analog.iqIm_1 > I_OUT_NOMINAL_IQ) || (analog.iqIm_2 > I_OUT_NOMINAL_IQ)) - && (f.secondPChState != 4)) { - current_kt = kt_single_inv; //_IQ(0.0039); - } else if ((analog.iqIm_1 < 9507089) || (analog.iqIm_2 < 9507089)) { - current_kt = kt_low_speed; //_IQ(0.0045); - } - } - - if (analog.iqUd1 < -U200V || (a.iqk1 > K_MODUL_SWITCH_COS_FI_2)) { - current_kt += addCompensateUd; // _IQ(0.0004); - } - cos_fi.cos_fi_nom = zad_intensiv_q(stepChangeCos, stepChangeCos, cos_fi.cos_fi_nom, currentCos); - cos_fi.cos_fi_nom_squared = zad_intensiv_q(stepChangeCos, stepChangeCos, cos_fi.cos_fi_nom_squared, currentCosSq); - - tetta_calc.k_t = zad_intensiv_q(stepChangeKt, stepChangeKt, tetta_calc.k_t, current_kt); -} - - -void limit_mzz_zad_power(_iq Frot) -{ - Frot = labs(Frot); - - if (vect_control.flag_reverse) { -// f.iq_mzz_zad = _IQ(1300.0/NORMA_MZZ); - f.iq_mzz_zad = _IQ(1100.0/NORMA_MZZ); - } else - if (Frot < 279620) //20 ob/min - { -// f.iq_mzz_zad = _IQ(1200.0/NORMA_MZZ); - f.iq_mzz_zad = _IQ(1400.0/NORMA_MZZ); - } - else if (Frot < 419430) //30 ob/min - { - f.iq_mzz_zad = _IQ(1400.0/NORMA_MZZ); - } -// else if(rotor.iqFout < 699050) //50 ob/min - else if (Frot < 838860) //60 ob/min -// else if (rotor.iqFout < 978670) //70 ob/min - { -// f.iq_mzz_zad = _IQ(1800.0/NORMA_MZZ); - f.iq_mzz_zad = _IQ(2000.0/NORMA_MZZ); - } - else - { - f.iq_mzz_zad = _IQ(2000.0/NORMA_MZZ); -// f.iq_mzz_zad = _IQ(1500.0/NORMA_MZZ); - } -} - - diff --git a/Inu/Src2/VectorControl/pwm_vector_regul.h b/Inu/Src2/VectorControl/pwm_vector_regul.h deleted file mode 100644 index 9b7c079..0000000 --- a/Inu/Src2/VectorControl/pwm_vector_regul.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef PWM_VECTOR_REGUL -#define PWM_VECTOR_REGUL - -#include "pid_reg3.h" - -void pwm_vector_model_titov(_iq Pzad, _iq Fzad, int direction, _iq Frot, int mode, - int reset, int calcPWMtimes); -void init_DQ_pid(void); -//void detect_I_M_overload(void); -void analog_dq_calc_const(void); - -void set_cos_tetta_calc_params(); -void limit_mzz_zad_power(_iq Frot); - -extern _iq zadan_Id_min; -extern PIDREG3 pidD; -extern PIDREG3 pidQ; -extern PIDREG3 pidD2; -extern PIDREG3 pidQ2; - -extern long koeff_Ud_filter; - -typedef struct { - _iq cos_fi_nom; - _iq cos_fi_nom_squared; -} COS_FI_STRUCT; - -extern COS_FI_STRUCT cos_fi; - -#define ONE_IQ24 16777216 - -typedef struct { - _iq iqId_zad; - _iq iqIq_zad; - _iq iqUdKm1; - _iq iqUqKm1; - _iq iqUdKm2; - _iq iqUqKm2; - _iq iqUdCompensation1; - _iq iqUqCompensation1; - _iq iqUdCompensation2; - _iq iqUqCompensation2; - _iq iqUdKm1Out; - _iq iqUqKm1Out; - _iq iqUdKm2Out; - _iq iqUqKm2Out; - - _iq iqUzad1; - _iq iqUzad2; - - _iq koef_Ud_comp; - _iq koef_Uq_comp; - _iq koeff_correct_Id; - - _iq equial_Iq_Proportional; //Пропорциональный коэффициент регулятора поддержания Iq одинаковым на обоих обмотках - _iq equial_Iq_Delta; //Разница в Iq двух обмоток - _iq equial_Iq_Out; - - _iq k_modul_max; - _iq k_modul_max_square; - _iq iq_Id_out_max; - - _iq iqPzad; - _iq iqPizm; - _iq iqFrot; - - int flag_reverse; - - float theta; - -} VECTOR_CONTROL; - -#define VECTOR_CONTROL_DEFAULTS {0,0,0,0,0,0,0,0,0,0, 0,0, 0,0,0, 0,0,0, 0,0,0,0,0, 0,0} - -extern VECTOR_CONTROL vect_control; - -extern PIDREG3 pidFvect; - -#endif //PWM_VECTOR_REGUL - - diff --git a/Inu/Src2/VectorControl/regul_power.c b/Inu/Src2/VectorControl/regul_power.c deleted file mode 100644 index 8ffcb42..0000000 --- a/Inu/Src2/VectorControl/regul_power.c +++ /dev/null @@ -1,269 +0,0 @@ -#include "vector.h" -#include "IQmathLib.h" -#include "math.h" -#include "pid_reg3.h" -#include "adc_tools.h" -#include "params.h" -#include "regul_power.h" -#include "pwm_vector_regul.h" -#include "my_filter.h" - -// #pragma DATA_SECTION(pidPvect,".fast_vars1"); -PIDREG3 pidPvect = PIDREG3_DEFAULTS; - - -#define IQ_OUT_SAT_POWER (2880.0 * COS_FI) -#define MAX_PID_CURRENT_P 2200.0 // 2200.0 ~ 1.6 Inom - -#define IM_CURRENT_STOP_RMP 11184810 //2000 - -#define IQ_1500A 8388608 -#define IQ_1800A 10066329 - -#define K_RMP_100A_PER_TIC 559240 -#define K_RMP_400A_PER_SEC 2663 - -#define K_MODUL_MAX 15770583 - -// #define P_DELTA_ZAD_IZM 5592405 //3 MWt - -#define LIMIT_Iq_ON_POWER_REVERSE - -_iq Id_out_max_low_speed = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP); -_iq Id_out_max_full = _IQ(ID_OUT_SAT_POWER / NORMA_ACP); - - -void init_Pvect_pid() -{ - pidPvect.Ref = 0; - pidPvect.Kp = _IQ(1.0);//_IQ(1.0); - pidPvect.Ki = _IQ(0.1);//_IQ(0.05);//_IQ(0.06); - pidPvect.Kc = _IQ(0.5); - // pidPvect.Kp = _IQ(1); //_IQ(2.5);//_IQ(5.0);// - // pidPvect.Ki = _IQ(0.1);//_IQ(0.05);//_IQ(0.06); - // pidPvect.Kc = _IQ(0.5); - pidPvect.OutMax = _IQ(MAX_PID_CURRENT_P / NORMA_ACP); - pidPvect.OutMin = -_IQ(MAX_PID_CURRENT_P / NORMA_ACP); -} - -void reset_Pvect_pid() -{ - pidPvect.Up = 0; - pidPvect.Up1 = 0; - pidPvect.Ui = 0; - pidPvect.Ud = 0; - pidPvect.Out = 0; -} - -_iq koeff_Iq_filter = _IQ(0.5); - -#define LEVEL_LIMIT_KM 14596177 //87% //15099494 ~ 90% //15435038 ~ 92% - -#define POWER_7_MWt 13048945 -#define POWER_4_MWt 7456540 -#define POWER_200_kWt 372827 - -// #pragma CODE_SECTION(vector_power,".fast_run2"); -void vector_power(_iq Pzad, _iq P_measured, _iq Iq_measured, int mode, _iq Frot, _iq* Iq_zad, _iq* Id_zad) -{ - static _iq Pzad_rmp = 0; - static _iq koef_slow = _IQ(0.000075); //_IQ(0.000065);15sec //_IQ(0.000085); 13sec // //_IQ(0.000065); normal rampa 13 seconds - static _iq koef_slow_low_task = _IQ(0.000040); //Ðàìïà äëÿ çàäàíèé ìîùíîñòè ìåíüøå ïîëîâèíû íîìèíàëà, ÷òîáû íåáûëî ïåðåðåãóëÿöèè - static _iq koef_slow_2 = _IQ(0.000045); //_IQ(0.000039); // _IQ(0.000015); //_IQ(0.000011) ~ 10 sec 0-10MWt //_IQ(0.00002) ~ 7 ñåê 0-10ÌÂò - static _iq koef_fast = _IQ(0.00013); //_IQ(0.00008); //_IQ(0.000025); ~3.1 sev 10-0MWt -// static _iq prev_Pzad = 0; -// static _iq cos_fi_nom_alg = _IQ(COS_FI), cos_fi_nom_squared_alg = _IQ(COS_FI * COS_FI); -// static _iq cos_fi_nom = _IQ(COS_FI), cos_fi_nom_squared = _IQ(COS_FI * COS_FI); - static _iq Iq_out_nom = _IQ(IQ_OUT_NOM / NORMA_ACP); //, Id_out_nom = _IQ(ID_OUT_NOM / NORMA_ACP); -// static _iq Iq_out_max = _IQ(IQ_OUT_SAT_POWER / NORMA_ACP); - static _iq Id_out_max = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP); - static _iq pid_Out_max = _IQ(MAX_PID_CURRENT_P / NORMA_ACP); - _iq koef_rmp, koef_spad; //koef_spad - äëþ çàùèòû îò ïðåâûøåíèþ âèíòîì ðàçðåø¸ííûõ îáîðîòîâ (170îá/ìèí) - _iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat; -// _iq Iq_out_limited = 0; - static _iq Iq_out_filter = 0; - static _iq mzz_zad_int=0; - static int counterStopRampa = 0; -#ifdef LIMIT_Iq_ON_POWER_REVERSE - static int flag_reverse = 0; -#endif //LIMIT_Iq_ON_POWER_REVERSE - - // if (f.DownTemperature) { - // mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, _IQmpy(f.iq_mzz_zad, temperature_limit_koeff)); - // } else -// if(f.DownToNominal && (f.iq_mzz_zad > Iq_out_nom)) { -// mzz_zad_int = zad_intensiv_q(29480, 29480, mzz_zad_int, Iq_out_nom); -// } else if (a.iqk1 > LEVEL_LIMIT_KM || a.iqk2 > LEVEL_LIMIT_KM) { -// Pzad = 0; -// Pzad_rmp = zad_intensiv_q((koef_fast << 3), (koef_fast << 3), Pzad_rmp, Pzad); -// // f.DownToNominalVoltage = 1; -// } -// else - { - if (f.iq_mzz_zad != 0) { - mzz_zad_int = zad_intensiv_q(28480, 28480, mzz_zad_int, f.iq_mzz_zad); - } else { - mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, pid_Out_max); - } -// f.DownToNominalVoltage = 0; - } - - if((mode != 2) || (!f.Go) -// || (f.flag_leading == 0 && f.read_task_from_optical_bus == 1 && f.sync_Iq_from_optical_bus == 1) - ) - { - Pzad_rmp = P_measured; -// prev_Pzad = P_measured; - pidPvect.Ui = Iq_measured; - Id_out_max = Id_out_max_low_speed; - if(!f.Go) - { - *Iq_zad = *Id_zad = 0; - } - #ifdef LIMIT_Iq_ON_POWER_REVERSE - flag_reverse = 0; - #endif - return; - } - -#ifdef LIMIT_Iq_ON_POWER_REVERSE - // Ïðè òîðîæåíèè îãðàíè÷èâàåì Iq íóë¸ì ñî ñòîðîíû íå òîðìîæåíèÿ - // ò.ê. ïðè òîðìîæåíèè èç-çà êîëåáàíèé çàäàíèå íà òîê ìîæåò ïîìåíÿòü çíàê - if ((Frot > 0 && Pzad_rmp < 0 && pidPvect.Out < 0) || - (Frot < 0 && Pzad_rmp > 0 && pidPvect.Out > 0)) { - flag_reverse = 1; - vect_control.flag_reverse = 1; - } else - if ((Frot > 0 && Pzad_rmp > 0) || (Frot < 0 && Pzad_rmp < 0)) - { - flag_reverse = 0; - vect_control.flag_reverse = 0; - } - if (flag_reverse == 0) { - pidPvect.OutMax = mzz_zad_int; - pidPvect.OutMin = -mzz_zad_int; - } else { - if (Pzad_rmp < 0) { - pidPvect.OutMax = 0; - pidPvect.OutMin = -mzz_zad_int; - } else { - pidPvect.OutMax = mzz_zad_int; - pidPvect.OutMin = 0; - } - } -#else - pidPvect.OutMax = mzz_zad_int; - pidPvect.OutMin = -mzz_zad_int; -#endif //LIMIT_Iq_ON_POWER_REVERSE - -// if (f.setCosTerminal) { -// cos_fi.cos_fi_nom = f.cosinusTerminal; -// cos_fi.cos_fi_nom_squared = f.cosinusTerminalSquared; -// } - - - //if(Pzad != prev_Pzad) - //{ - if((_IQabs(Pzad_rmp) <= _IQabs(Pzad)) && - (((Pzad >= 0) && (Pzad_rmp >= 0)) || ((Pzad < 0) && (Pzad_rmp < 0)))) - { - // if (_IQabs(Pzad) < POWER_4_MWt) { - // koef_rmp = koef_slow_low_task; - // } else if (_IQabs(Pzad_rmp) < POWER_7_MWt) { // 7MWt - // koef_rmp = koef_slow; - // } else { - // koef_rmp = koef_slow_2; - // } - koef_rmp = koef_slow; - //Ïðè íàáðîñå ìîùíîñòè ýëåêòðè÷åñêàÿ ìîùíîñòü ìîæåò ïðåâûñèòü çàäàííóþ - //ïîýòîìó åñëè ïðèáëèçèëèñü ê çàäàíèþ, òî çàìåäëÿåì ðàìïó âåêòîðîé ìîùíîñòè - // if (_IQabs(f.iq_p_zad_electric) - _IQabs(analog.iqW) < POWER_200_kWt) { - // koef_rmp = koef_rmp / 4; - // } - } - else - { - koef_rmp = koef_fast; - } - //} - -// EfficiencyCompensation(&Pzad, &Iq_measured); - - // if (analog.iqIm_1 > IM_CURRENT_STOP_RMP || analog.iqIm_2 > IM_CURRENT_STOP_RMP) { - // counterStopRampa = 400; - // } else { - // if (counterStopRampa > 0) { - // counterStopRampa -= 1; - // } - // } - // if (counterStopRampa <= 0) { - // Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Pzad_rmp, Pzad); - // counterStopRampa = 0; - // } else { - // Pzad_rmp = zad_intensiv_q(koef_slow_2, koef_slow_2, Pzad_rmp, Pzad); - // } - - Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Pzad_rmp, Pzad); - -#ifdef P_DELTA_ZAD_IZM - //Åñëè èçìåðåííàÿ ìîùíîñòü íà ìíîãî ìåíüøå çàäàííîé ðàìïû, - //òî ðàìïà îãðàíè÷èâàåòñÿ çíà÷åíèåì èçìåðåííîé ïëþñ íåêîòîðàÿ äåëüòà - if ((Pzad_rmp > 0 && P_measured > 0) && - ((Pzad_rmp - P_measured) > P_DELTA_ZAD_IZM)) { - Pzad_rmp = P_measured + P_DELTA_ZAD_IZM; - } - if ((Pzad_rmp < 0 && P_measured < 0) && - ((Pzad_rmp - P_measured) < -P_DELTA_ZAD_IZM)) { - Pzad_rmp = P_measured - P_DELTA_ZAD_IZM; - } -#endif - - f.iq_p_rampa = Pzad_rmp; - - - // - if(_IQabs(Frot) > IQ_170_RPM) - { - koef_spad = _IQdiv((IQ_170_RPM + IQ_10_RPM - _IQabs(Frot)), IQ_10_RPM); - if(koef_spad < 0) { - koef_spad = 0; - } - pidPvect.OutMax = _IQmpy(pidPvect.OutMax, koef_spad); - pidPvect.OutMin = _IQmpy(pidPvect.OutMin, koef_spad); - Pzad_rmp = _IQmpy(Pzad_rmp, koef_spad); - } - pidPvect.Ref = Pzad_rmp; - pidPvect.Fdb = P_measured; - - - pidPvect.calc(&pidPvect); - - Iq_out_unsat = pidPvect.Out; - - Iq_out_filter = Iq_out_unsat; // exp_regul_iq(koeff_Iq_filter, Iq_out_filter, Iq_out_unsat); - - // ìîäåëè çäåñü íå áûëî îãðàíè÷åíèé - Iq_out_sat = _IQsat(Iq_out_filter, mzz_zad_int, -mzz_zad_int); //Test - - if(Iq_out_filter < 0) - { - Id_out_unsat = _IQdiv(_IQmpy((-Iq_out_filter), _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom); - } else { - Id_out_unsat = _IQdiv(_IQmpy(Iq_out_filter, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom); - } - - if (_IQabs(Frot) < IQ_50_RPM) { - Id_out_max = zad_intensiv_q(5592, 5592, Id_out_max, Id_out_max_low_speed); //???? - } else { - Id_out_max = zad_intensiv_q(5592, 5592, Id_out_max, Id_out_max_full); - } - Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0); - -// prev_Iq_zad = Iq_out_sat; - - *Iq_zad = Iq_out_sat; - *Id_zad = Id_out_sat; - -} - - diff --git a/Inu/Src2/VectorControl/regul_power.h b/Inu/Src2/VectorControl/regul_power.h deleted file mode 100644 index 2f1b127..0000000 --- a/Inu/Src2/VectorControl/regul_power.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef REGUL_POWER -#define REGUL_POWER -#include "IQmathLib.h" -#include "pid_reg3.h" - -void init_Pvect_pid(void); -void vector_power(_iq Pzad, _iq P_measured, _iq Iq, int mode, _iq Frot, _iq* Iq_zad, _iq* Id_zad); - -extern PIDREG3 pidPvect; - -extern _iq Id_out_max_low_speed; -extern _iq Id_out_max_full; - - -#define IQ_5_RPM 69905L //5/ -#define IQ_10_RPM 139810L -#define IQ_20_RPM 279620L -#define IQ_50_RPM 699050L -#define IQ_135_RPM 1887436L -#define IQ_165_RPM 2306867L //165/ -#define IQ_170_RPM 2376772L //170/ -#define IQ_175_RPM 2446677L -#define IQ_190_RPM 2656392L -#define IQ_250_RPM 3495253L -#define IQ_255_RPM 3565158L - - -#define ID_OUT_SAT_POWER 800 //0.456 ~ sin(Fi) -#define ID_OUT_SAT_POWER_LOW_SPEED (500.0) //0.52 ~ sin(Fi) - - -#endif //REGUL_POWER diff --git a/Inu/Src2/VectorControl/regul_turns.c b/Inu/Src2/VectorControl/regul_turns.c deleted file mode 100644 index 444547f..0000000 --- a/Inu/Src2/VectorControl/regul_turns.c +++ /dev/null @@ -1,185 +0,0 @@ -#include "IQmathLib.h" -#include "math.h" -#include "params.h" -#include "adc_tools.h" -#include "regul_turns.h" -#include "pid_reg3.h" -#include "vector.h" -#include "teta_calc.h" -#include "pwm_vector_regul.h" -#include "my_filter.h" - -// #pragma DATA_SECTION(pidFvect,".fast_vars1"); -PIDREG3 pidFvect = PIDREG3_DEFAULTS; - -#define MAX_PID_CURRENT IQ_OUT_NOM - -#define IQ_165_RPM 2306867 //165/ -#define IQ_170_RPM 2376772 //170/ -#define IQ_5_RPM 69905 //5/ - -#define TIME_RMP_FAST 10.0 //sec -#define TIME_RMP_SLOW 30.0 //sec -#define F_DEST 3.0 //Hz - -void init_Fvect_pid() -{ - pidFvect.Ref = 0; - pidFvect.Kp = _IQ(10.0);//_IQ(30.0);// // - pidFvect.Ki = _IQ(0.03); //_IQ(0.015); - pidFvect.Kc = _IQ(0.5); - pidFvect.OutMax = _IQ(MAX_PID_CURRENT / NORMA_ACP); - pidFvect.OutMin = -_IQ(MAX_PID_CURRENT / NORMA_ACP); -} - -void reset_F_pid() -{ - pidFvect.Up = 0; - pidFvect.Up1 = 0; - pidFvect.Ui = 0; - pidFvect.Ud = 0; - pidFvect.Out = 0; -} - -void vector_turns(_iq Fzad, _iq Frot, _iq Iq, int mode, _iq* Iq_zad, _iq* Id_zad) -{ - static _iq Fzad_rmp = 0, koef_fast = _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 2.5); - static _iq koef_slow = _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 5.0); -// static _iq cos_fi_nom = _IQ(COS_FI), cos_fi_nom_squared = _IQ(COS_FI * COS_FI); //prev_Fzad = 0, - static _iq Iq_out_max = _IQ(IQ_OUT_NOM / NORMA_ACP); - static _iq Id_out_max = _IQ(500.0 / NORMA_ACP); //_IQ(ID_OUT_NOM / NORMA_ACP); - static _iq mzz_zad_int=0; - static int Iq_rmp_to_optica = 0; -// static int flag_Iq_synced_with_optica = 0; -// static _iq step_Iq_rmp = _IQ(20.0 / NORMA_ACP); - _iq koef_rmp; //, koef_spad; - _iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat; - _iq deltaVar; - -// if (f.DownTemperature) { -// // mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, _IQmpy(Iq_out_max, temperature_limit_koeff)); -// mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, _IQmpy(f.iq_mzz_zad, temperature_limit_koeff)); -// } else - { - mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, f.iq_mzz_zad); -// mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, pid_Out_max); - } - - pidFvect.OutMax = Iq_out_max; - pidFvect.OutMin = -Iq_out_max; - - if (Fzad >= 0 && Frot > 0) { - pidFvect.OutMin = 0; - } - - if (Fzad <= 0 && Frot < 0) { - pidFvect.OutMax = 0; - } - - - - if((mode != 1) || (!f.Go)) // -, - { // - Fzad_rmp = Frot; -// prev_Fzad = Frot; - reset_F_pid(); // , - - pidFvect.Ui = Iq; - pidFvect.Out = Iq; - *Iq_zad = Iq; - Iq_rmp_to_optica = Iq; -// *Id_zad = _IQdiv(_IQmpy(Iq, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom); - if(!f.Go) - { - *Iq_zad = *Id_zad = 0; - } -// flag_Iq_synced_with_optica = 0; - return; - } - - // - // Iq -// if ((f.flag_leading == 0) && (f.read_task_from_optical_bus == 1) && (f.sync_Iq_from_optical_bus == 1)) { -// // if (flag_Iq_synced_with_optica == 0) { -// // if (_IQabs(analog.iqIq_zad_from_optica - Iq_rmp_to_optica) > 1118481) { //200A -// // Iq_rmp_to_optica = zad_intensiv_q(step_Iq_rmp, step_Iq_rmp, Iq_rmp_to_optica, analog.iqIq_zad_from_optica); -// // } else { -// // Iq_rmp_to_optica = analog.iqIq_zad_from_optica; -// // flag_Iq_synced_with_optica = 1; -// // } -// // } else { -// // Iq_rmp_to_optica = analog.iqIq_zad_from_optica; -// // } -// Iq_rmp_to_optica = analog.iqIq_zad_from_optica; -// Iq_out_unsat = Iq_rmp_to_optica; -// Iq_out_sat = _IQsat(Iq_out_unsat, mzz_zad_int, -mzz_zad_int); -// Id_out_unsat = _IQdiv(_IQmpy(_IQabs(Iq_out_sat), _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom); -// Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0); -// *Iq_zad = Iq_out_sat; -// *Id_zad = Id_out_sat; - -// reset_F_pid(); -// pidFvect.Ui = Iq_out_sat; -// pidFvect.Out = Iq_out_sat; -// Fzad_rmp = Frot; -// return; -// } - - //if(Fzad != prev_Fzad) // . , . - //{ // , . - if(_IQabs(Fzad_rmp) <= _IQabs(Fzad) && - (((Fzad >= 0) && (Fzad_rmp >= 0)) || ((Fzad < 0 ) && (Fzad_rmp < 0)))) - { - koef_rmp = koef_slow; - } - else - { - koef_rmp = koef_fast; - } -// prev_Fzad = Fzad; - //} - - // Fzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Fzad_rmp, Fzad); -// logpar.log2 = (int16)(_IQtoIQ15(Fzad)); -// logpar.log19 = (int16)(_IQtoIQ15(Fzad_rmp)); - - // - zad_intensiv_q - // . - deltaVar = Fzad-Fzad_rmp; - - if ((deltaVar>koef_rmp) || (deltaVar<-koef_rmp)) - { - if (deltaVar>0) Fzad_rmp += koef_rmp; - else Fzad_rmp -= koef_rmp; - } - else - Fzad_rmp = Fzad; - - pidFvect.Ref = Fzad_rmp; - pidFvect.Fdb = Frot; - - pidFvect.calc(&pidFvect); - Iq_out_unsat = pidFvect.Out; - - //TODO: ATTENTION!!! worked better on stend - if (_IQabs(Iq_out_unsat) < 27962) //5A - { - pidTetta.Ui = 0; - pidTetta.SatErr = 0; - } - Iq_out_sat = _IQsat(Iq_out_unsat, Iq_out_max, -Iq_out_max); -// if(f.DownToNominal) //In this mode max I equial Inom -// { -// Iq_out_sat = _IQsat(Iq_out_unsat, I_OUT_NOMINAL_IQ, -I_OUT_NOMINAL_IQ); -// } - // Iq_out_sat = _IQsat(Iq_out_unsat, mzz_zad_int, -mzz_zad_int); //Test - // d - - Iq_out_unsat = _IQabs(Iq_out_unsat); - Id_out_unsat = _IQdiv(_IQmpy(Iq_out_unsat, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom); - Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0); - - - - *Iq_zad = Iq_out_sat; - *Id_zad = Id_out_sat; - -} diff --git a/Inu/Src2/VectorControl/regul_turns.h b/Inu/Src2/VectorControl/regul_turns.h deleted file mode 100644 index 57529b5..0000000 --- a/Inu/Src2/VectorControl/regul_turns.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef REGUL_TURNS -#define REGUL_TURNS -#include "IQmathLib.h" -#include "pid_reg3.h" - -void init_Fvect_pid(void); -void vector_turns(_iq Fzad, _iq Frot, _iq Iq, int mode, _iq* Iq_zad, _iq* Id_zad); - - -#endif //REGUL_TURNS - - diff --git a/Inu/Src2/VectorControl/teta_calc.c b/Inu/Src2/VectorControl/teta_calc.c deleted file mode 100644 index d76bfe5..0000000 --- a/Inu/Src2/VectorControl/teta_calc.c +++ /dev/null @@ -1,262 +0,0 @@ -#include "teta_calc.h" -#include "params.h" -#include "pid_reg3.h" -#include "IQmathLib.h" -#include "vector.h" -#include "my_filter.h" - -#define CONST_IQ_2PI 105414357 -#define PI 3.1415926535897932384626433832795 - -PIDREG3 pidTetta = PIDREG3_DEFAULTS; - -#define MAX_Ud_Pid_Out (167772 * 100) //419430 ~ 0.5 //251658 ~ 0.3 //209715 ~ 0.25 //167772 ~ 0.2Hz //100663 //0.12Hz -// 184549 ~ 2.2 - -#define MAX_Ud_Pid_Out_Id 176160 //0.2 ~ 167772 //0.21 ~ 176160 - //0.15 ~ 125829 -#define BPSI_START 0.17 //0.15 - -void init_tetta_pid() -{ - pidTetta.Ref = 0; - pidTetta.Fdb = 0; - pidTetta.Kp = _IQ(0.1); //_IQ(0.15); - pidTetta.Ki = _IQ(0.0003); //_IQ(0.00003) - pidTetta.Kc = _IQ(0.5); - pidTetta.OutMax = MAX_Ud_Pid_Out; - pidTetta.OutMin = -MAX_Ud_Pid_Out; - pidTetta.Up = 0; - pidTetta.Ui = 0; -} - -void reset_tetta_pid() -{ - pidTetta.Up = 0; - pidTetta.Up1 = 0; - pidTetta.Ui = 0; - pidTetta.Out = 0; -} - - -//#pragma CODE_SECTION(calc_tetta,".fast_run"); -void calc_tetta(_iq Frot, int direction, _iq Ud, int direct_zadan, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int mode, int reset) // -{ - static _iq tetta = 0, Fsl_start = 0, bpsi_start = _IQ(BPSI_START / NORMA_FROTOR);// , bpsi_start_on_go = _IQ(0.1 / NORMA_FROTOR); - static _iq hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2.0); - static int flag_start = 0; - static int koeff_zad_int = 30; //1000; - static long stop_frot = 27962; //2 rpm // 10000; -// static _iq prev_Fsl = 0; - static int flag_reverse = 0; -// static int flag_out_of_reverse = 0; - static int prev_direction = 0; - static _iq koeffKpRmp = _IQ(0.0003750); - static _iq koeffKiRmp = _IQ(0.00005); - static _iq lowSpeedKp = _IQ(0.1); - static _iq lowSpeedKi = _IQ(0.0003); - static _iq highSpeedKp = _IQ(0.01); - static _iq highSpeedKi = _IQ(0.011); //_IQ(0.02); - - _iq Fstat, Fsl; - int reverse_Ud = 1; - - if(reset == 1) - { - flag_start = 1; //Won`t work, if machine will stop without changing mode - Fsl_start = 0; - flag_reverse = 0; -// flag_out_of_reverse = 0; - } - if (_IQabs(Frot) < 768955) { //55 rpm - pidTetta.Kp = zad_intensiv_q(koeffKpRmp, koeffKpRmp, pidTetta.Kp, lowSpeedKp); - pidTetta.Ki = zad_intensiv_q(koeffKiRmp, koeffKiRmp, pidTetta.Ki, lowSpeedKi); - } - else if ((_IQabs(Frot) < 1118481)) { //80 rpm - pidTetta.Kp = zad_intensiv_q(koeffKpRmp, koeffKpRmp, pidTetta.Kp, highSpeedKp); - pidTetta.Ki = zad_intensiv_q(koeffKiRmp, koeffKiRmp, pidTetta.Ki, highSpeedKi); - } - // direction == 1 - //direction == 0 - -// if(((Frot >= 0) && (direct_zadan == 1) && (direction >= 0)) || -// ((direct_zadan == -1) && (direction >= 0) && (Frot >= stop_frot))) - //27962 ~ 2 rpm -// if(((direct_zadan == 1) && (direction == 0)) || (direction > 0)) -// { -// Ud = -Ud; -// } - - - if(Frot >= 27962) { - reverse_Ud = -1; - flag_reverse = (Frot >= 27962 && direct_zadan == -1) ? 1 : 0; - prev_direction = 1; - } - else if (Frot <= -27962) { - reverse_Ud = 1; - flag_reverse = (Frot <= -27962 && direct_zadan == 1) ? 1 : 0; - prev_direction = -1; - } - if(_IQabs(Frot) < 27962) { - if (flag_reverse == 1) { - if (prev_direction == 1) { - reverse_Ud = -1; - } else { - reverse_Ud = 1; - } - } else -// if (flag_start) - { - if (direct_zadan == 1) { - reverse_Ud = -1; - } else { - reverse_Ud = 1; - } - } - } - Ud = Ud * reverse_Ud; - - pidTetta.Ref = Ud * 100; //Ref - - pidTetta.calc(&pidTetta); - Fsl = pidTetta.Out / 100; - - if(flag_start) // - { - if(_IQabs(Frot) < stop_frot) - { - Fsl_start = bpsi_start * direct_zadan; // , - . - } - else - { - if(Fsl_start == 0) - { - Fsl_start = bpsi_start * direct_zadan; //direction; - } - flag_start = 0; - } - - } - else - { - // , , .. . - if(_IQabs(Frot) < (stop_frot)) - { -// if((direct_zadan == 1) && (Fsl + Fsl_start < bpsi_start)) -// { -// Fsl_start += (koeff_zad_int << 2); -//// Fsl_start += koeff_zad_int; -// } -// if((direct_zadan == -1) && (Fsl + Fsl_start > -bpsi_start)) -// { -// Fsl_start -= (koeff_zad_int << 2); -//// Fsl_start -= koeff_zad_int; -// } - } - else - { - if(_IQabs(Fsl_start) > koeff_zad_int) - { - if(Fsl_start > 0) - { - Fsl_start -= koeff_zad_int; - } - if(Fsl_start < 0) - { - Fsl_start += koeff_zad_int; - } - } - else - { - Fsl_start = 0; - } - } - - } - Fsl += Fsl_start; - Fstat = Frot * POLUS + Fsl; - tetta += _IQmpy(Fstat, hz_to_angle); - - - if (tetta > CONST_IQ_2PI) - { - tetta -= CONST_IQ_2PI; - } - - if (tetta < 0) - { - tetta += CONST_IQ_2PI; - } - *tetta_out = tetta; - *Fsl_out = Fsl; -// prev_Fsl = Fsl; - *Fstator_out = Fstat; - - -} - -TETTA_CALC tetta_calc = TETTA_CALC_DEF; - -void init_tetta_calc_struct() -{ - tetta_calc.Imds = 0; - tetta_calc.tetta = 0; - tetta_calc.k_r = _IQ(0.015); - tetta_calc.k_t = _IQ(0.0045); //_IQ(0.0045); - tetta_calc.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2.); -} - -void calc_tetta_Id(_iq Frot, _iq Id, _iq Iq, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int reset) { - - _iq Fsl, Fst; - if (reset) { - tetta_calc.Imds = 0; - } - - tetta_calc.Imds = tetta_calc.Imds + _IQmpy(tetta_calc.k_r, (Id - tetta_calc.Imds)); - Fsl = _IQmpy(tetta_calc.k_t, Iq); - if (tetta_calc.Imds != 0) { - Fsl = _IQdiv(Fsl, tetta_calc.Imds); - } else { - Fsl = 0; - } - // Fsl = _IQ(0.2 / NORMA_FROTOR); - if (Fsl > MAX_Ud_Pid_Out_Id) { Fsl = MAX_Ud_Pid_Out_Id;} - if (Fsl < -MAX_Ud_Pid_Out_Id) { Fsl = -MAX_Ud_Pid_Out_Id;} - - Fst = Frot * POLUS + Fsl; - tetta_calc.tetta += _IQmpy(Fst, tetta_calc.hz_to_angle); - - if (tetta_calc.tetta > CONST_IQ_2PI) - { - tetta_calc.tetta -= CONST_IQ_2PI; - } - - if (tetta_calc.tetta < 0) - { - tetta_calc.tetta += CONST_IQ_2PI; - } - *Fsl_out = Fsl; - *tetta_out = tetta_calc.tetta; - *Fstator_out = Fst; - - -} - -#define LEVEL_REDUCE_UD 838860 //150V - -void correct_tetta_calc_Kt(void) { - static _iq coeff_add_Kt = _IQ(0.00005); - static _iq max_Kt = _IQ(0.0069); - static _iq min_Kt = _IQ(0.0029); - - if (tetta_calc.Imds > LEVEL_REDUCE_UD) { - tetta_calc.k_t -= coeff_add_Kt; - } - if (tetta_calc.Imds < -LEVEL_REDUCE_UD) { - tetta_calc.k_t += coeff_add_Kt; - } - if (tetta_calc.k_t > max_Kt) {tetta_calc.k_t = max_Kt; } - if (tetta_calc.k_t < min_Kt) {tetta_calc.k_t = min_Kt; } -} - - diff --git a/Inu/Src2/VectorControl/teta_calc.h b/Inu/Src2/VectorControl/teta_calc.h deleted file mode 100644 index b04dda5..0000000 --- a/Inu/Src2/VectorControl/teta_calc.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef TETA_CALC -#define TETA_CALC - -#include "IQmathLib.h" -#include "pid_reg3.h" - -void init_tetta_pid(void); -void reset_tetta_pid(void); -void calc_tetta(_iq Frot, int direction, _iq Ud, int direct_zadan, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int mode, int reset); -void calc_tetta_Id(_iq Frot, _iq Id, _iq Iq, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int reset); -void init_tetta_calc_struct(void); - -// k_r = Ts / Tr_cm -// Tr_cm = Lr / Rr -// Lr - -// Rr - -// -// k_t = 1 / (Tr_cm * 2 * Pi * f_b) -// -// K = Ts * f_b -// f_b - (12 ) -// Ts - (840 ) - -typedef struct { - _iq Imds; - _iq tetta; - - _iq hz_to_angle; - _iq k_r; - _iq k_t; -} TETTA_CALC; - -#define TETTA_CALC_DEF {0,0,0,0,0} - -extern TETTA_CALC tetta_calc; - -extern PIDREG3 pidTetta; -#endif //TETA_CALC - diff --git a/Inu/Src2/main/IQmathLib.c b/Inu/Src2/main/IQmathLib.c deleted file mode 100644 index 5720d7e..0000000 --- a/Inu/Src2/main/IQmathLib.c +++ /dev/null @@ -1,182 +0,0 @@ -#include "IQmathLib.h" - - -// Преобразование числа с плавающей точкой в число с фиксированной точкой -#define float_to_fixed(A) (long)((A)*(1 << (GLOBAL_Q)) + (A > 0 ? 0.5: -0.5)) -// Преобразование числа с плавающей точкой в число с фиксированной точкой с выбором числа бит, отдаваемых под дробную часть -#define float_to_fixed_base_select(A, F_BITS) (long)((A)*(1 << (F_BITS)) + (A > 0 ? 0.5: -0.5)) -// Преобразование целого числа в число с фиксированной точкой -#define int_to_fixed(A) (long)((A) << (GLOBAL_Q)) -// Преобразование целого числа в число с фиксированной точкой с выбором числа бит, отдаваемых под дробную часть -#define int_to_fixed_base_select(A, F_BITS) (long)((A) << (F_BITS)) -//Преобразование числа с фиксированной точкой в число с плавающей точкой -#define fixed_to_float(A) ((double)A / (1 << GLOBAL_Q)) -//Перобразование числа с фиксированной точкой в целое число -#define fixed_to_int(A) ((int)(A >> GLOBAL_Q) ) - - -long multiply(long x, long y) -{ - long long z = (long long)x * (long long)y; - return (long)(z >> GLOBAL_Q); -} -//служебная функция. Умножает числа с 27 битами, отданными под дробную часть -static inline long multiply_27(long x, long y) -{ - long long z = (long long)x * (long long)y; - return z & 0x4000000 ? (long)(z >> 27) + 1 : (long)(z >> 27); -} - -long long multiply_fixed_base_select(long long x, long long y, int base) -{ - long long z = (long long)x * (long long)y; - return z & (1 << base) ? (z >> base) + 1 : (z >> base); -} - -long divide(long num, long den) -{ - long long numLong = (long long)num; - long long quotient = (numLong << GLOBAL_Q) / den; - return (long)quotient; -} -// -static inline long long divide_fixed_base_select(long long num, long long den, int base) -{ - long long quotient = ((long long)num << base) / den; - return quotient; -} - -#define div_def(A,B) (long)(((long long)(A) << 24)/(B)) -#define div_mod(A,B) (A)%(B) -#define mult_def(A,B) (long)((((long long)(A))*((long long)(B))) >> 24) -#define abs_def(A) ((A) > 0 ? (A): -(A)) - -long sin_fixed(long x) -{ - //Константы сделал ститическими, что бы они вычислялись во время запуска программы, а не исполнения - static long FIXED_2PI = float_to_fixed(TWO_PI); - static long FIXED_PI = float_to_fixed(PI); - static long FIXED_PIna2 = float_to_fixed(PI_2); - //Здесть так же что бы не производить операции деления посчитал констаны ряда Тейлора - static long one_110 = float_to_fixed_base_select(1./110, 27); - static long one_72 = float_to_fixed_base_select(1./72, 27); - static long one_42 = float_to_fixed_base_select(1./42, 27); - static long one_20= float_to_fixed_base_select(1./20, 27); - static long one_6 = float_to_fixed_base_select(1./6, 27); - - long long xx, tmp ; - while(x >= FIXED_2PI) { x -= FIXED_2PI;} //Помещаю аргумент в диапазон 2 ПИ - while(x <= -FIXED_2PI) { x += FIXED_2PI;} - //Так как ряды быстрее сходнятся при малых значениях, помещаю значение аргумента - //в ближайшие к нулю области - if(x > FIXED_PI) - { - x -= FIXED_2PI; - } - else if(x < -FIXED_PI) - { - x += FIXED_2PI; - } - if(x < -FIXED_PIna2) - { - x = -FIXED_PI - x; - } - else if(x > FIXED_PIna2) - { - x = FIXED_PI - x; - } - //проверяю угол на значения, при которых синус раве 0 или 1 - if(x == 0) return 0; - if(x == FIXED_PIna2) return int_to_fixed(1); - if(x == -FIXED_PIna2) return int_to_fixed(-1); - //Перевожу в формат с максимальной точностью для возможного дипазано значений - x <<= (27 - GLOBAL_Q); - //Считаю ряд фурье - xx = multiply_27(x, x); - tmp = ONE_27 - multiply_27(one_110, xx); - tmp = multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(tmp, one_72); - tmp = multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(tmp, one_42); - tmp = multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(tmp, one_20); - tmp = multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(tmp, one_6); - tmp = multiply_27(x, tmp); - return tmp >> (27 - GLOBAL_Q); //Перед возвращением из функции преобразую в первоначальный формат -} - -long cos_fixed(long x) -{ - //Константы сделал ститическими, что бы они вычислялись во время запуска программы, а не исполнения - static long FIXED_2PI = float_to_fixed(TWO_PI); - static long FIXED_PI = float_to_fixed(PI); - static long FIXED_PIna2 = float_to_fixed(PI_2); - //Здесть так же что бы не производить операции деления посчитал констаны ряда Тейлора - static long one_132 = float_to_fixed_base_select(1./132, 27); - static long one_90 = float_to_fixed_base_select(1./90, 27); - static long one_56 = float_to_fixed_base_select(1./56, 27); - static long one_30 = float_to_fixed_base_select(1./30, 27); - static long one_12 = float_to_fixed_base_select(1./12, 27); - - long xx, tmp, counter = 0; - while(x >= FIXED_2PI) { x -= FIXED_2PI;} //Помещаю аргумент в диапазон 2 ПИ - while(x < 0) { x += FIXED_2PI;} - x = _IQabs(x); //Так как косинус симметричен относительно нуля, нахожу его модуль - //проверяю угол на значения, при которых синус раве 0 или 1 - if(x == 0) return 1 << GLOBAL_Q; - if(x == FIXED_PI) return -(1 << GLOBAL_Q); - if(x == (FIXED_PIna2) || (x == FIXED_3PIna2))return 0; - //Так как ряды быстрее сходнятся при малых значениях, помещаю значение аргумента - //в ближайшие к нулю области - while(x > FIXED_PIna2) - { - x -= FIXED_PIna2; - counter++; - } - - if(counter == 1 || counter == 3) { x = FIXED_PIna2 - x;} - //Перевожу в формат с максимальной точностью для возможного дипазона значений - x <<= (27 - GLOBAL_Q); - //Считаю ряд фурье - xx = multiply_27(x, x); - tmp = ONE_27 - multiply_27(xx, one_132); - tmp= multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(xx, one_90); - tmp= multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(tmp, one_56); - tmp = multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(tmp, one_30); - tmp = multiply_27(xx, tmp); - tmp = ONE_27 - multiply_27(tmp, one_12); - tmp = multiply_27(xx, tmp); - tmp = ONE_27 - (tmp >> 1); - tmp >>= (27 - GLOBAL_Q); - return (counter == 0) || (counter == 3) ? tmp : -tmp; -} - -long sqrt_fixed(long x) -{ - int variable_size_bits = sizeof(x) << 3; - long average_val, prev_avg_val; - if(x <= 0) return 0; - while(!(x & (1 << --variable_size_bits))); //Нахожу старший значащий бит - //Нахожу приближение корня сдвгом на половину числа бит между старшим значащим битом - //и положением точки - if(variable_size_bits > GLOBAL_Q) - { - average_val = x >> ((variable_size_bits - GLOBAL_Q) >> 1); - } - else - { - average_val = x << ((GLOBAL_Q - variable_size_bits) >> 1); - } - prev_avg_val = divide(x, average_val); //Нахожу 1/А - //В цикле нахожу среднее арифметическое между А и 1/А, пока число не перестанет меняться - while(_IQabs(prev_avg_val - average_val) > 1) - { - prev_avg_val = average_val; - average_val = (average_val + divide(x, average_val)) >> 1; - } - return average_val; -} diff --git a/Inu/Src2/main/IQmathLib.h b/Inu/Src2/main/IQmathLib.h deleted file mode 100644 index 0e52bad..0000000 --- a/Inu/Src2/main/IQmathLib.h +++ /dev/null @@ -1,661 +0,0 @@ -/** -* IQmath MATLAB -* -*/ -#ifndef IQ_MATH_LIB -#define IQ_MATH_LIB - - -#ifndef GLOBAL_Q -#define GLOBAL_Q 24 -#endif - -typedef long _iq; -typedef long _iq30; -typedef long _iq29; -typedef long _iq28; -typedef long _iq27; -typedef long _iq26; -typedef long _iq25; -typedef long _iq24; -typedef long _iq23; -typedef long _iq22; -typedef long _iq21; -typedef long _iq20; -typedef long _iq19; -typedef long _iq18; -typedef long _iq17; -typedef long _iq16; -typedef long _iq15; -typedef long _iq14; -typedef long _iq13; -typedef long _iq12; -typedef long _iq11; -typedef long _iq10; -typedef long _iq9; -typedef long _iq8; -typedef long _iq7; -typedef long _iq6; -typedef long _iq5; -typedef long _iq4; -typedef long _iq3; -typedef long _iq2; -typedef long _iq1; - -//--------------------------------------------------------------------------- -#define _IQmpy2(A) ((A)<<1) -#define _IQmpy4(A) ((A)<<2) -#define _IQmpy8(A) ((A)<<3) -#define _IQmpy16(A) ((A)<<4) -#define _IQmpy32(A) ((A)<<5) -#define _IQmpy64(A) ((A)<<6) - -#define _IQdiv2(A) ((A)>>1) -#define _IQdiv4(A) ((A)>>2) -#define _IQdiv8(A) ((A)>>3) -#define _IQdiv16(A) ((A)>>4) -#define _IQdiv32(A) ((A)>>5) -#define _IQdiv64(A) ((A)>>6) -//--------------------------------------------------------------------------- -#define _IQ30(A) (long) ((A) * 1073741824.0L) -#define _IQ29(A) (long) ((A) * 536870912.0L) -#define _IQ28(A) (long) ((A) * 268435456.0L) -#define _IQ27(A) (long) ((A) * 134217728.0L) -#define _IQ26(A) (long) ((A) * 67108864.0L) -#define _IQ25(A) (long) ((A) * 33554432.0L) -#define _IQ24(A) (long) ((A) * 16777216.0L) -#define _IQ23(A) (long) ((A) * 8388608.0L) -#define _IQ22(A) (long) ((A) * 4194304.0L) -#define _IQ21(A) (long) ((A) * 2097152.0L) -#define _IQ20(A) (long) ((A) * 1048576.0L) -#define _IQ19(A) (long) ((A) * 524288.0L) -#define _IQ18(A) (long) ((A) * 262144.0L) -#define _IQ17(A) (long) ((A) * 131072.0L) -#define _IQ16(A) (long) ((A) * 65536.0L) -#define _IQ15(A) (long) ((A) * 32768.0L) -#define _IQ14(A) (long) ((A) * 16384.0L) -#define _IQ13(A) (long) ((A) * 8192.0L) -#define _IQ12(A) (long) ((A) * 4096.0L) -#define _IQ11(A) (long) ((A) * 2048.0L) -#define _IQ10(A) (long) ((A) * 1024.0L) -#define _IQ9(A) (long) ((A) * 512.0L) -#define _IQ8(A) (long) ((A) * 256.0L) -#define _IQ7(A) (long) ((A) * 128.0L) -#define _IQ6(A) (long) ((A) * 64.0L) -#define _IQ5(A) (long) ((A) * 32.0L) -#define _IQ4(A) (long) ((A) * 16.0L) -#define _IQ3(A) (long) ((A) * 8.0L) -#define _IQ2(A) (long) ((A) * 4.0L) -#define _IQ1(A) (long) ((A) * 2.0L) - -#if GLOBAL_Q == 30 -#define _IQ(A) _IQ30(A) -#endif -#if GLOBAL_Q == 29 -#define _IQ(A) _IQ29(A) -#endif -#if GLOBAL_Q == 28 -#define _IQ(A) _IQ28(A) -#endif -#if GLOBAL_Q == 27 -#define _IQ(A) _IQ27(A) -#endif -#if GLOBAL_Q == 26 -#define _IQ(A) _IQ26(A) -#endif -#if GLOBAL_Q == 25 -#define _IQ(A) _IQ25(A) -#endif -#if GLOBAL_Q == 24 -#define _IQ(A) _IQ24(A) -#endif -#if GLOBAL_Q == 23 -#define _IQ(A) _IQ23(A) -#endif -#if GLOBAL_Q == 22 -#define _IQ(A) _IQ22(A) -#endif -#if GLOBAL_Q == 21 -#define _IQ(A) _IQ21(A) -#endif -#if GLOBAL_Q == 20 -#define _IQ(A) _IQ20(A) -#endif -#if GLOBAL_Q == 19 -#define _IQ(A) _IQ19(A) -#endif -#if GLOBAL_Q == 18 -#define _IQ(A) _IQ18(A) -#endif -#if GLOBAL_Q == 17 -#define _IQ(A) _IQ17(A) -#endif -#if GLOBAL_Q == 16 -#define _IQ(A) _IQ16(A) -#endif -#if GLOBAL_Q == 15 -#define _IQ(A) _IQ15(A) -#endif -#if GLOBAL_Q == 14 -#define _IQ(A) _IQ14(A) -#endif -#if GLOBAL_Q == 13 -#define _IQ(A) _IQ13(A) -#endif -#if GLOBAL_Q == 12 -#define _IQ(A) _IQ12(A) -#endif -#if GLOBAL_Q == 11 -#define _IQ(A) _IQ11(A) -#endif -#if GLOBAL_Q == 10 -#define _IQ(A) _IQ10(A) -#endif -#if GLOBAL_Q == 9 -#define _IQ(A) _IQ9(A) -#endif -#if GLOBAL_Q == 8 -#define _IQ(A) _IQ8(A) -#endif -#if GLOBAL_Q == 7 -#define _IQ(A) _IQ7(A) -#endif -#if GLOBAL_Q == 6 -#define _IQ(A) _IQ6(A) -#endif -#if GLOBAL_Q == 5 -#define _IQ(A) _IQ5(A) -#endif -#if GLOBAL_Q == 4 -#define _IQ(A) _IQ4(A) -#endif -#if GLOBAL_Q == 3 -#define _IQ(A) _IQ3(A) -#endif -#if GLOBAL_Q == 2 -#define _IQ(A) _IQ2(A) -#endif -#if GLOBAL_Q == 1 -#define _IQ(A) _IQ1(A) -#endif - -//--------------------------------------------------------------------------- - -#define _IQ30toF(A) ((float) ((A) / 1073741824.0L)) -#define _IQ29toF(A) ((float) ((A) / 536870912.0L)) -#define _IQ28toF(A) ((float) ((A) / 268435456.0L)) -#define _IQ27toF(A) ((float) ((A) / 134217728.0L)) -#define _IQ26toF(A) ((float) ((A) / 67108864.0L)) -#define _IQ25toF(A) ((float) ((A) / 33554432.0L)) -#define _IQ24toF(A) ((float) ((A) / 16777216.0L)) -#define _IQ23toF(A) ((float) ((A) / 8388608.0L)) -#define _IQ22toF(A) ((float) ((A) / 4194304.0L)) -#define _IQ21toF(A) ((float) ((A) / 2097152.0L)) -#define _IQ20toF(A) ((float) ((A) / 1048576.0L)) -#define _IQ19toF(A) ((float) ((A) / 524288.0L)) -#define _IQ18toF(A) ((float) ((A) / 262144.0L)) -#define _IQ17toF(A) ((float) ((A) / 131072.0L)) -#define _IQ16toF(A) ((float) ((A) / 65536.0L)) -#define _IQ15toF(A) ((float) ((A) / 32768.0L)) -#define _IQ14toF(A) ((float) ((A) / 16384.0L)) -#define _IQ13toF(A) ((float) ((A) / 8192.0L)) -#define _IQ12toF(A) ((float) ((A) / 4096.0L)) -#define _IQ11toF(A) ((float) ((A) / 2048.0L)) -#define _IQ10toF(A) ((float) ((A) / 1024.0L)) -#define _IQ9toF(A) ((float) ((A) / 512.0L)) -#define _IQ8toF(A) ((float) ((A) / 256.0L)) -#define _IQ7toF(A) ((float) ((A) / 128.0L)) -#define _IQ6toF(A) ((float) ((A) / 64.0L)) -#define _IQ5toF(A) ((float) ((A) / 32.0L)) -#define _IQ4toF(A) ((float) ((A) / 16.0L)) -#define _IQ3toF(A) ((float) ((A) / 8.0L)) -#define _IQ2toF(A) ((float) ((A) / 4.0L)) -#define _IQ1toF(A) ((float) ((A) / 2.0L)) - -#if GLOBAL_Q == 30 -#define _IQtoF(A) _IQ30toF(A) -#endif -#if GLOBAL_Q == 29 -#define _IQtoF(A) _IQ29toF(A) -#endif -#if GLOBAL_Q == 28 -#define _IQtoF(A) _IQ28toF(A) -#endif -#if GLOBAL_Q == 27 -#define _IQtoF(A) _IQ27toF(A) -#endif -#if GLOBAL_Q == 26 -#define _IQtoF(A) _IQ26toF(A) -#endif -#if GLOBAL_Q == 25 -#define _IQtoF(A) _IQ25toF(A) -#endif -#if GLOBAL_Q == 24 -#define _IQtoF(A) _IQ24toF(A) -#endif -#if GLOBAL_Q == 23 -#define _IQtoF(A) _IQ23toF(A) -#endif -#if GLOBAL_Q == 22 -#define _IQtoF(A) _IQ22toF(A) -#endif -#if GLOBAL_Q == 21 -#define _IQtoF(A) _IQ21toF(A) -#endif -#if GLOBAL_Q == 20 -#define _IQtoF(A) _IQ20toF(A) -#endif -#if GLOBAL_Q == 19 -#define _IQtoF(A) _IQ19toF(A) -#endif -#if GLOBAL_Q == 18 -#define _IQtoF(A) _IQ18toF(A) -#endif -#if GLOBAL_Q == 17 -#define _IQtoF(A) _IQ17toF(A) -#endif -#if GLOBAL_Q == 16 -#define _IQtoF(A) _IQ16toF(A) -#endif -#if GLOBAL_Q == 15 -#define _IQtoF(A) _IQ15toF(A) -#endif -#if GLOBAL_Q == 14 -#define _IQtoF(A) _IQ14toF(A) -#endif -#if GLOBAL_Q == 13 -#define _IQtoF(A) _IQ13toF(A) -#endif -#if GLOBAL_Q == 12 -#define _IQtoF(A) _IQ12toF(A) -#endif -#if GLOBAL_Q == 11 -#define _IQtoF(A) _IQ11toF(A) -#endif -#if GLOBAL_Q == 10 -#define _IQtoF(A) _IQ10toF(A) -#endif -#if GLOBAL_Q == 9 -#define _IQtoF(A) _IQ9toF(A) -#endif -#if GLOBAL_Q == 8 -#define _IQtoF(A) _IQ8toF(A) -#endif -#if GLOBAL_Q == 7 -#define _IQtoF(A) _IQ7toF(A) -#endif -#if GLOBAL_Q == 6 -#define _IQtoF(A) _IQ6toF(A) -#endif -#if GLOBAL_Q == 5 -#define _IQtoF(A) _IQ5toF(A) -#endif -#if GLOBAL_Q == 4 -#define _IQtoF(A) _IQ4toF(A) -#endif -#if GLOBAL_Q == 3 -#define _IQtoF(A) _IQ3toF(A) -#endif -#if GLOBAL_Q == 2 -#define _IQtoF(A) _IQ2toF(A) -#endif -#if GLOBAL_Q == 1 -#define _IQtoF(A) _IQ1toF(A) -#endif - -#define _IQsat(A, Pos, Neg) ((A > Pos) ? Pos : (A < Neg) ? Neg : A) -//--------------------------------------------------------------------------- -#define _IQtoIQ30(A) ((long) (A) << (30 - GLOBAL_Q)) -#define _IQ30toIQ(A) ((long) (A) >> (30 - GLOBAL_Q)) - -#if (GLOBAL_Q >= 29) -#define _IQtoIQ29(A) ((long) (A) >> (GLOBAL_Q - 29)) -#define _IQ29toIQ(A) ((long) (A) << (GLOBAL_Q - 29)) -#else -#define _IQtoIQ29(A) ((long) (A) << (29 - GLOBAL_Q)) -#define _IQ29toIQ(A) ((long) (A) >> (29 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 28) -#define _IQtoIQ28(A) ((long) (A) >> (GLOBAL_Q - 28)) -#define _IQ28toIQ(A) ((long) (A) << (GLOBAL_Q - 28)) -#else -#define _IQtoIQ28(A) ((long) (A) << (28 - GLOBAL_Q)) -#define _IQ28toIQ(A) ((long) (A) >> (28 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 27) -#define _IQtoIQ27(A) ((long) (A) >> (GLOBAL_Q - 27)) -#define _IQ27toIQ(A) ((long) (A) << (GLOBAL_Q - 27)) -#else -#define _IQtoIQ27(A) ((long) (A) << (27 - GLOBAL_Q)) -#define _IQ27toIQ(A) ((long) (A) >> (27 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 26) -#define _IQtoIQ26(A) ((long) (A) >> (GLOBAL_Q - 26)) -#define _IQ26toIQ(A) ((long) (A) << (GLOBAL_Q - 26)) -#else -#define _IQtoIQ26(A) ((long) (A) << (26 - GLOBAL_Q)) -#define _IQ26toIQ(A) ((long) (A) >> (26 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 25) -#define _IQtoIQ25(A) ((long) (A) >> (GLOBAL_Q - 25)) -#define _IQ25toIQ(A) ((long) (A) << (GLOBAL_Q - 25)) -#else -#define _IQtoIQ25(A) ((long) (A) << (25 - GLOBAL_Q)) -#define _IQ25toIQ(A) ((long) (A) >> (25 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 24) -#define _IQtoIQ24(A) ((long) (A) >> (GLOBAL_Q - 24)) -#define _IQ24toIQ(A) ((long) (A) << (GLOBAL_Q - 24)) -#else -#define _IQtoIQ24(A) ((long) (A) << (24 - GLOBAL_Q)) -#define _IQ24toIQ(A) ((long) (A) >> (24 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 23) -#define _IQtoIQ23(A) ((long) (A) >> (GLOBAL_Q - 23)) -#define _IQ23toIQ(A) ((long) (A) << (GLOBAL_Q - 23)) -#else -#define _IQtoIQ23(A) ((long) (A) << (23 - GLOBAL_Q)) -#define _IQ23toIQ(A) ((long) (A) >> (23 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 22) -#define _IQtoIQ22(A) ((long) (A) >> (GLOBAL_Q - 22)) -#define _IQ22toIQ(A) ((long) (A) << (GLOBAL_Q - 22)) -#else -#define _IQtoIQ22(A) ((long) (A) << (22 - GLOBAL_Q)) -#define _IQ22toIQ(A) ((long) (A) >> (22 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 21) -#define _IQtoIQ21(A) ((long) (A) >> (GLOBAL_Q - 21)) -#define _IQ21toIQ(A) ((long) (A) << (GLOBAL_Q - 21)) -#else -#define _IQtoIQ21(A) ((long) (A) << (21 - GLOBAL_Q)) -#define _IQ21toIQ(A) ((long) (A) >> (21 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 20) -#define _IQtoIQ20(A) ((long) (A) >> (GLOBAL_Q - 20)) -#define _IQ20toIQ(A) ((long) (A) << (GLOBAL_Q - 20)) -#else -#define _IQtoIQ20(A) ((long) (A) << (20 - GLOBAL_Q)) -#define _IQ20toIQ(A) ((long) (A) >> (20 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 19) -#define _IQtoIQ19(A) ((long) (A) >> (GLOBAL_Q - 19)) -#define _IQ19toIQ(A) ((long) (A) << (GLOBAL_Q - 19)) -#else -#define _IQtoIQ19(A) ((long) (A) << (19 - GLOBAL_Q)) -#define _IQ19toIQ(A) ((long) (A) >> (19 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 18) -#define _IQtoIQ18(A) ((long) (A) >> (GLOBAL_Q - 18)) -#define _IQ18toIQ(A) ((long) (A) << (GLOBAL_Q - 18)) -#else -#define _IQtoIQ18(A) ((long) (A) << (18 - GLOBAL_Q)) -#define _IQ18toIQ(A) ((long) (A) >> (18 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 17) -#define _IQtoIQ17(A) ((long) (A) >> (GLOBAL_Q - 17)) -#define _IQ17toIQ(A) ((long) (A) << (GLOBAL_Q - 17)) -#else -#define _IQtoIQ17(A) ((long) (A) << (17 - GLOBAL_Q)) -#define _IQ17toIQ(A) ((long) (A) >> (17 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 16) -#define _IQtoIQ16(A) ((long) (A) >> (GLOBAL_Q - 16)) -#define _IQ16toIQ(A) ((long) (A) << (GLOBAL_Q - 16)) -#else -#define _IQtoIQ16(A) ((long) (A) << (16 - GLOBAL_Q)) -#define _IQ16toIQ(A) ((long) (A) >> (16 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 15) -#define _IQtoIQ15(A) ((long) (A) >> (GLOBAL_Q - 15)) -#define _IQ15toIQ(A) ((long) (A) << (GLOBAL_Q - 15)) -#define _IQtoQ15(A) ((long) (A) >> (GLOBAL_Q - 15)) -#define _Q15toIQ(A) ((long) (A) << (GLOBAL_Q - 15)) -#else -#define _IQtoIQ15(A) ((long) (A) << (15 - GLOBAL_Q)) -#define _IQ15toIQ(A) ((long) (A) >> (15 - GLOBAL_Q)) -#define _IQtoQ15(A) ((long) (A) << (15 - GLOBAL_Q)) -#define _Q15toIQ(A) ((long) (A) >> (15 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 14) -#define _IQtoIQ14(A) ((long) (A) >> (GLOBAL_Q - 14)) -#define _IQ14toIQ(A) ((long) (A) << (GLOBAL_Q - 14)) -#define _IQtoQ14(A) ((long) (A) >> (GLOBAL_Q - 14)) -#define _Q14toIQ(A) ((long) (A) << (GLOBAL_Q - 14)) -#else -#define _IQtoIQ14(A) ((long) (A) << (14 - GLOBAL_Q)) -#define _IQ14toIQ(A) ((long) (A) >> (14 - GLOBAL_Q)) -#define _IQtoQ14(A) ((long) (A) << (14 - GLOBAL_Q)) -#define _Q14toIQ(A) ((long) (A) >> (14 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 13) -#define _IQtoIQ13(A) ((long) (A) >> (GLOBAL_Q - 13)) -#define _IQ13toIQ(A) ((long) (A) << (GLOBAL_Q - 13)) -#define _IQtoQ13(A) ((long) (A) >> (GLOBAL_Q - 13)) -#define _Q13toIQ(A) ((long) (A) << (GLOBAL_Q - 13)) -#else -#define _IQtoIQ13(A) ((long) (A) << (13 - GLOBAL_Q)) -#define _IQ13toIQ(A) ((long) (A) >> (13 - GLOBAL_Q)) -#define _IQtoQ13(A) ((long) (A) << (13 - GLOBAL_Q)) -#define _Q13toIQ(A) ((long) (A) >> (13 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 12) -#define _IQtoIQ12(A) ((long) (A) >> (GLOBAL_Q - 12)) -#define _IQ12toIQ(A) ((long) (A) << (GLOBAL_Q - 12)) -#define _IQtoQ12(A) ((long) (A) >> (GLOBAL_Q - 12)) -#define _Q12toIQ(A) ((long) (A) << (GLOBAL_Q - 12)) -#else -#define _IQtoIQ12(A) ((long) (A) << (12 - GLOBAL_Q)) -#define _IQ12toIQ(A) ((long) (A) >> (12 - GLOBAL_Q)) -#define _IQtoQ12(A) ((long) (A) << (12 - GLOBAL_Q)) -#define _Q12toIQ(A) ((long) (A) >> (12 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 11) -#define _IQtoIQ11(A) ((long) (A) >> (GLOBAL_Q - 11)) -#define _IQ11toIQ(A) ((long) (A) << (GLOBAL_Q - 11)) -#define _IQtoQ11(A) ((long) (A) >> (GLOBAL_Q - 11)) -#define _Q11toIQ(A) ((long) (A) << (GLOBAL_Q - 11)) -#else -#define _IQtoIQ11(A) ((long) (A) << (11 - GLOBAL_Q)) -#define _IQ11toIQ(A) ((long) (A) >> (11 - GLOBAL_Q)) -#define _IQtoQ11(A) ((long) (A) << (11 - GLOBAL_Q)) -#define _Q11toIQ(A) ((long) (A) >> (11 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 10) -#define _IQtoIQ10(A) ((long) (A) >> (GLOBAL_Q - 10)) -#define _IQ10toIQ(A) ((long) (A) << (GLOBAL_Q - 10)) -#define _IQtoQ10(A) ((long) (A) >> (GLOBAL_Q - 10)) -#define _Q10toIQ(A) ((long) (A) << (GLOBAL_Q - 10)) -#else -#define _IQtoIQ10(A) ((long) (A) << (10 - GLOBAL_Q)) -#define _IQ10toIQ(A) ((long) (A) >> (10 - GLOBAL_Q)) -#define _IQtoQ10(A) ((long) (A) << (10 - GLOBAL_Q)) -#define _Q10toIQ(A) ((long) (A) >> (10 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 9) -#define _IQtoIQ9(A) ((long) (A) >> (GLOBAL_Q - 9)) -#define _IQ9toIQ(A) ((long) (A) << (GLOBAL_Q - 9)) -#define _IQtoQ9(A) ((long) (A) >> (GLOBAL_Q - 9)) -#define _Q9toIQ(A) ((long) (A) << (GLOBAL_Q - 9)) -#else -#define _IQtoIQ9(A) ((long) (A) << (9 - GLOBAL_Q)) -#define _IQ9toIQ(A) ((long) (A) >> (9 - GLOBAL_Q)) -#define _IQtoQ9(A) ((long) (A) << (9 - GLOBAL_Q)) -#define _Q9toIQ(A) ((long) (A) >> (9 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 8) -#define _IQtoIQ8(A) ((long) (A) >> (GLOBAL_Q - 8)) -#define _IQ8toIQ(A) ((long) (A) << (GLOBAL_Q - 8)) -#define _IQtoQ8(A) ((long) (A) >> (GLOBAL_Q - 8)) -#define _Q8toIQ(A) ((long) (A) << (GLOBAL_Q - 8)) -#else -#define _IQtoIQ8(A) ((long) (A) << (8 - GLOBAL_Q)) -#define _IQ8toIQ(A) ((long) (A) >> (8 - GLOBAL_Q)) -#define _IQtoQ8(A) ((long) (A) << (8 - GLOBAL_Q)) -#define _Q8toIQ(A) ((long) (A) >> (8 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 7) -#define _IQtoIQ7(A) ((long) (A) >> (GLOBAL_Q - 7)) -#define _IQ7toIQ(A) ((long) (A) << (GLOBAL_Q - 7)) -#define _IQtoQ7(A) ((long) (A) >> (GLOBAL_Q - 7)) -#define _Q7toIQ(A) ((long) (A) << (GLOBAL_Q - 7)) -#else -#define _IQtoIQ7(A) ((long) (A) << (7 - GLOBAL_Q)) -#define _IQ7toIQ(A) ((long) (A) >> (7 - GLOBAL_Q)) -#define _IQtoQ7(A) ((long) (A) << (7 - GLOBAL_Q)) -#define _Q7toIQ(A) ((long) (A) >> (7 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 6) -#define _IQtoIQ6(A) ((long) (A) >> (GLOBAL_Q - 6)) -#define _IQ6toIQ(A) ((long) (A) << (GLOBAL_Q - 6)) -#define _IQtoQ6(A) ((long) (A) >> (GLOBAL_Q - 6)) -#define _Q6toIQ(A) ((long) (A) << (GLOBAL_Q - 6)) -#else -#define _IQtoIQ6(A) ((long) (A) << (6 - GLOBAL_Q)) -#define _IQ6toIQ(A) ((long) (A) >> (6 - GLOBAL_Q)) -#define _IQtoQ6(A) ((long) (A) << (6 - GLOBAL_Q)) -#define _Q6toIQ(A) ((long) (A) >> (6 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 5) -#define _IQtoIQ5(A) ((long) (A) >> (GLOBAL_Q - 5)) -#define _IQ5toIQ(A) ((long) (A) << (GLOBAL_Q - 5)) -#define _IQtoQ5(A) ((long) (A) >> (GLOBAL_Q - 5)) -#define _Q5toIQ(A) ((long) (A) << (GLOBAL_Q - 5)) -#else -#define _IQtoIQ5(A) ((long) (A) << (5 - GLOBAL_Q)) -#define _IQ5toIQ(A) ((long) (A) >> (5 - GLOBAL_Q)) -#define _IQtoQ5(A) ((long) (A) << (5 - GLOBAL_Q)) -#define _Q5toIQ(A) ((long) (A) >> (5 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 4) -#define _IQtoIQ4(A) ((long) (A) >> (GLOBAL_Q - 4)) -#define _IQ4toIQ(A) ((long) (A) << (GLOBAL_Q - 4)) -#define _IQtoQ4(A) ((long) (A) >> (GLOBAL_Q - 4)) -#define _Q4toIQ(A) ((long) (A) << (GLOBAL_Q - 4)) -#else -#define _IQtoIQ4(A) ((long) (A) << (4 - GLOBAL_Q)) -#define _IQ4toIQ(A) ((long) (A) >> (4 - GLOBAL_Q)) -#define _IQtoQ4(A) ((long) (A) << (4 - GLOBAL_Q)) -#define _Q4toIQ(A) ((long) (A) >> (4 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 3) -#define _IQtoIQ3(A) ((long) (A) >> (GLOBAL_Q - 3)) -#define _IQ3toIQ(A) ((long) (A) << (GLOBAL_Q - 3)) -#define _IQtoQ3(A) ((long) (A) >> (GLOBAL_Q - 3)) -#define _Q3toIQ(A) ((long) (A) << (GLOBAL_Q - 3)) -#else -#define _IQtoIQ3(A) ((long) (A) << (3 - GLOBAL_Q)) -#define _IQ3toIQ(A) ((long) (A) >> (3 - GLOBAL_Q)) -#define _IQtoQ3(A) ((long) (A) << (3 - GLOBAL_Q)) -#define _Q3toIQ(A) ((long) (A) >> (3 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 2) -#define _IQtoIQ2(A) ((long) (A) >> (GLOBAL_Q - 2)) -#define _IQ2toIQ(A) ((long) (A) << (GLOBAL_Q - 2)) -#define _IQtoQ2(A) ((long) (A) >> (GLOBAL_Q - 2)) -#define _Q2toIQ(A) ((long) (A) << (GLOBAL_Q - 2)) -#else -#define _IQtoIQ2(A) ((long) (A) << (2 - GLOBAL_Q)) -#define _IQ2toIQ(A) ((long) (A) >> (2 - GLOBAL_Q)) -#define _IQtoQ2(A) ((long) (A) << (2 - GLOBAL_Q)) -#define _Q2toIQ(A) ((long) (A) >> (2 - GLOBAL_Q)) -#endif - -#if (GLOBAL_Q >= 1) -#define _IQtoQ1(A) ((long) (A) >> (GLOBAL_Q - 1)) -#define _Q1toIQ(A) ((long) (A) << (GLOBAL_Q - 1)) -#else -#define _IQtoQ1(A) ((long) (A) << (1 - GLOBAL_Q)) -#define _Q1toIQ(A) ((long) (A) >> (1 - GLOBAL_Q)) -#endif - -#define _IQtoIQ1(A) ((long) (A) >> (GLOBAL_Q - 1)) -#define _IQ1toIQ(A) ((long) (A) << (GLOBAL_Q - 1)) - -///////////////////////////////////////////////////////////// -long multiply(long x, long y); -long long multiply_fixed_base_select(long long x, long long y, int base); -long divide(long num, long den); -long sin_fixed(long x); -long cos_fixed(long x); -long sqrt_fixed(long x); - -#define _IQabs(A) ((A) > 0 ? (A): -(A)) -#define _IQmpy(A,B) multiply(A,B) -#define _IQ19mpy(A,B) multiply_fixed_base_select(A,B,19) -#define _IQdiv(A,B) divide(A,B) -#define _IQsin(A) sin_fixed(A) -#define _IQcos(A) cos_fixed(A) -#define _IQsqrt(A) sqrt_fixed(A) - -#define PI 3.1415926535897932384626433832795 -#define PI_2 1.5707963267948966192313216916398 -#define TWO_PI 6.283185307179586476925286766559 - -#ifndef ONE_24 -#define ONE_24 16777216 -#endif -#ifndef ONE_27 -#define ONE_27 134217728 -#endif -#ifndef ONE_28 -#define ONE_28 268435456 -#endif - -// #ifndef FIXED_PI -// #define FIXED_PI 52707179 -// #endif - -// #ifndef FIXED_2PI -// #define FIXED_2PI 105414357 -// #endif - -#ifndef FIXED_PI_30 -#define FIXED_PI_30 3373259426 -#endif - -#ifndef FIXED_2PI_30 -#define FIXED_2PI_30 6746518852 -#endif - -#ifndef FIXED_3PIna2 -#define FIXED_3PIna2 79060768 -#endif - -#ifndef FIXED_PIna3 -#define FIXED_PIna3 17569059 -#endif - -#ifndef FIXED_PIna6 -#define FIXED_PIna6 8784529 -#endif - - -#endif //IQ_MATH_LIB diff --git a/Inu/Src2/main/PWMTools.c b/Inu/Src2/main/PWMTools.c deleted file mode 100644 index 561b909..0000000 --- a/Inu/Src2/main/PWMTools.c +++ /dev/null @@ -1,719 +0,0 @@ -#include -#include -//#include "project.h" - -#include "IQmathLib.h" -//#include "f281xpwm.h" -// -////#include "SpaceVectorPWM.h" -//#include "MemoryFunctions.h" -//#include "PWMTools.h" -//#include "pwm_vector_regul.h" -//#include "PWMTMSHandle.h" -//#include "TuneUpPlane.h" -//#include "RS_Functions.h" -//#include "CAN_setup.h" -//#include "global_time.h" -#include "params.h" -#include "vector.h" -//#include "rmp_cntl_my1.h" -//#include "vhzprof.h" -//#include "adc_tools.h" -//#include "v_pwm24.h" -//#include "break_regul.h" -//#include "break_tools.h" -//#include "detect_phase.h" -//#include "mathlib.h" -//#include "project.h" -//#include "log_to_memory.h" -//#include "rotation_speed.h" -//#include "detect_overload.h" -//#include "xp_write_xpwm_time.h" -//#include "errors.h" -//#include "sync_tools.h" -//#include "optical_bus.h" -//#include "IQmathLib.h" - -#define DEF_FREQ_PWM_XTICS (3750000 / FREQ_PWM / 2) -#define DEF_PERIOD_MIN_XTICS 400 //375 ~ 100mks //315 ~ 84 mks //460//(3750000 * mks / 1000000) -#define DEF_PERIOD_MIN_BR_XTICS 165 - -#define DEF_FREQ_PWM_XTICS_MIN = 4261 -#define DEF_FREQ_PWM_XTICS_MAX = 4687 - -#define STOP_ROTOR_LIMIT 27962 //2 rpm -#define STOP_ROTOR_MIN_CURRENT 4194304 //750A //3355443 //600A - -#define K_MODUL_MAX 15770583 //13421772 //80% //10066329 //60% //5033164 //30% 15099494 ~ 90% //15435038 ~ 0.92% - //15770583 ~ 0.94% -#define MIN_Fsl_WHEN_STOPED 41943 //0.05 //67108 //0.08Hz - -#define PWM_ONE_INTERRUPT_RUN 1 -#define PWM_TWICE_INTERRUPT_RUN 0 - - -//4464 -// xilinx (60000000 / 16 / FREQ_PWM = 3750000 / FREQ_PWM) -#pragma DATA_SECTION(VAR_FREQ_PWM_XTICS,".fast_vars1"); -int VAR_FREQ_PWM_XTICS = DEF_FREQ_PWM_XTICS; - -// xilinx -#pragma DATA_SECTION(VAR_PERIOD_MAX_XTICS,".fast_vars1"); -int VAR_PERIOD_MAX_XTICS = DEF_FREQ_PWM_XTICS - DEF_PERIOD_MIN_XTICS; - -// xilinx (mintime+deadtime) (F * T.. = (60 / 16 / 2) * T = (60 * T / 16 / 2)) -#pragma DATA_SECTION(VAR_PERIOD_MIN_XTICS,".fast_vars1"); -int VAR_PERIOD_MIN_XTICS = DEF_PERIOD_MIN_XTICS;// - -// xilinx (mintime) (F * T.. = (60 / 16 / 2) * T = (60 * T / 16 / 2)) -#pragma DATA_SECTION(VAR_PERIOD_MIN_BR_XTICS,".fast_vars1"); -int VAR_PERIOD_MIN_BR_XTICS = DEF_PERIOD_MIN_BR_XTICS;// - -#pragma DATA_SECTION(freq1,".fast_vars1"); -_iq freq1; - -#pragma DATA_SECTION(k1,".fast_vars1"); -_iq k1 = 0; - -RMP_MY1 rmp_freq = RMP_MY1_DEFAULTS; -//VHZPROF vhz1 = VHZPROF_DEFAULTS; - -// WINDING a; -// FLAG f; - -#define COUNT_SAVE_LOG_OFF 100 // -#define COUNT_START_IMP 2 - -int i = 0; -/*void InitPWM() -{ - WriteMemory(ADR_PWM_MODE_0, 0x0000); // TMS -}*/ - -static void write_swgen_pwm_times(); -void write_swgen_pwm_times_split_eages(unsigned int mode_reload); -void fix_pwm_freq_synchro_ain(); -void detect_level_interrupt(void); -void detect_current_saw_val(void); - -void InitXPWM(void) -{ - - #ifdef XPWMGEN - /* Start of PWM Generator initialization*/ - for(i = 0; i < 16; i++) // - { - WriteMemory(ADR_PWM_KEY_NUMBER, i); - WriteMemory(ADR_PWM_TIMING, 0); - - } - WriteMemory(ADR_PWM_DIRECT, 0xffff); - WriteMemory(ADR_PWM_DRIVE_MODE, 0); //Choose PWM sourse PWMGenerator on Spartan 200e - WriteMemory(ADR_PWM_DEAD_TIME, 360); //Dead time in tics. 1 tic = 16.67 nsec -#ifndef _test_without_power - // OFF WDOG - WriteMemory(ADR_PWM_WDOG, 0x8005); //TODO turn on -#else - // ON WDOG - WriteMemory(ADR_PWM_WDOG, 0x0005); //TODO turn on -#endif - WriteMemory(ADR_PWM_PERIOD, VAR_FREQ_PWM_XTICS); // Saw period in tics. 1 tic = 16.67 nsec - WriteMemory(ADR_PWM_SAW_DIRECT, 0x0555); - WriteMemory(ADR_TK_MASK_0, 0); - WriteMemory(ADR_TK_MASK_1, 0xffff); //Turn off additional 16 tk lines -#if C_PROJECT_TYPE == PROJECT_BALZAM - WriteMemory(ADR_PWM_IT_TYPE, 1); //1 interrupt per PWM period -#else - WriteMemory(ADR_PWM_IT_TYPE, 0); //interrupt on each counter twist -#endif -// WriteMemory(ADR_PWM_WDOG, 0x8008);// PWM - #endif - - #ifdef TMSPWMGEN - - WriteMemory(ADR_PWM_MODE_0, 0x0000); // TMS - - #endif - -/* End f PWM Gen init */ -} - -void initPWM_Variables(void) -{ - // , , -// xpwm_time.Tclosed_0 = 0; -// xpwm_time.Tclosed_1 = VAR_FREQ_PWM_XTICS + 1; -// xpwm_time.pwm_tics = VAR_FREQ_PWM_XTICS; -// xpwm_time.saw_direct.all = 0;//0x0555; -// xpwm_time.one_or_two_interrupts_run = PWM_TWICE_INTERRUPT_RUN; - - - init_alpha_pwm24(VAR_FREQ_PWM_XTICS); - InitVariablesSvgen(VAR_FREQ_PWM_XTICS); - init_DQ_pid(); - break_resistor_managment_init(); - - rmp_freq.RampLowLimit = _IQ(-4); //0 - rmp_freq.RampHighLimit = _IQ(4); - - rmp_freq.RampPlus = _IQ(0.00005); //_IQ(0.0002);_IQ(0.000005); - - rmp_freq.RampMinus = _IQ(-0.00005); //_IQ(-0.000005); - rmp_freq.DesiredInput = 0; - rmp_freq.Out = 0; - - a.k = 0; - a.k1 = 0; - a.k2 = 0; - - k1 = 0; - freq1 = 0; -} - -#pragma CODE_SECTION(start_PWM24,".fast_run2") -void start_PWM24(int O1, int O2) -{ - if ((O1 == 1) && (O2 == 1)) - { - start_pwm(); - } - else - { - if ((O1 == 0) && (O2 == 1)) - { - start_pwm_b(); - } - if ((O1 == 1) && (O2 == 0)) - { - start_pwm_a(); - } - } -} - -inline void init_regulators() -{ - if(f.Mode != 0) - { - pwm_vector_model_titov(f.iq_p_zad, f.iq_fzad, rotor.direct_rotor, - rotor.iqFout, f.Mode, 1, 1); - } -} - -#define select_working_channels(go_a, go_b) go_a = !f.Obmotka1; \ - go_b = !f.Obmotka2; - -void PWM_interrupt() -{ - static unsigned int pwm_run = 0; - static _iq Uzad1, Fzad, Uzad2; - static int count_step=0; - static int count_step_ram_off = 0; - static int count_start_impuls = 0; - static int flag_record_log = 0; - static int log_saved_to_const_mem = 0; - static int prevGo = -1; - static volatile unsigned int go_a = 0; - static volatile unsigned int go_b = 0; - - static int stop_rotor_counter = 0; - - static int prev_go_a = 1; - static int prev_go_b = 1; - - int pwm_enable_calc_main = 0; - - int start_int_xtics = 0, end_int_xtics = 0; - -// i_led1_on_off(1); - if (pwm_run == 1) - { -// stop_pwm(); -// errors.slow_stop.bit.PWM_interrupt_to_long |= 1; - return; - } - pwm_run = 1; - - -// detect_level_interrupt(); -// start_int_xtics = xpwm_time.current_period; - if (xpwm_time.where_interrupt == PWM_LOW_LEVEL_INTERRUPT - || xpwm_time.one_or_two_interrupts_run == PWM_ONE_INTERRUPT_RUN) - { - pwm_enable_calc_main = 1; - if (f.flag_second_PCH) { - fix_pwm_freq_synchro_ain(); - } - } - else { -// i_sync_pin_on(); - pwm_enable_calc_main = 0; - } - - -// project.cds_in[0].read_pbus(&project.cds_in[0]); //read direction -// project.read_all_pbus(); -// optical_bus_read(); - - update_rot_sensors(); - global_time.calc(&global_time); -// - inc_RS_timeout_cicle(); - inc_CAN_timeout_cicle(); - detect_I_M_overload(); - DetectI_Out_BreakFase(); - Rotor_measure(); - - if ((f.Go == 1) && (f.Stop == 0) - && (f.rotor_stopped == 0) -// && (faults.faults5.bit.rotor_stopped == 0) - /* && (f.Ready2 == 1)*/) - { - if (f.Ready2) {f.flag_turn_On_Pump = 1;} // - - if (f.Go != prevGo) { - set_start_mem(FAST_LOG); -// clear_mem(FAST_LOG); -// count_start_impuls = 0; - count_step = 0; - count_step_ram_off = COUNT_SAVE_LOG_OFF; - - init_regulators(); - stop_rotor_counter = 0; - } else { - if (f.Mode == 0) { - rmp_freq.DesiredInput = freq1; - rmp_freq.calc(&rmp_freq); -// Fzad = rmp_freq.Out; - Fzad = freq1; -// if(k1 < 87772) -// { k1 = 87772;} - Uzad1 = k1; - Uzad2 = k1; - } - - select_working_channels(go_a, go_b); - - if (go_a == 0 && prev_go_a != go_a) { - stop_pwm_a(); - } - if (go_a == 1 && prev_go_a != go_a) { - start_pwm_a(); - } - if (go_b == 0 && prev_go_b != go_b) { - stop_pwm_b(); - } - if (go_b == 1 && prev_go_b != go_b) { - start_pwm_b(); - } - - prev_go_a = go_a; - prev_go_b = go_b; - - if (count_start_impuls < COUNT_START_IMP) { - count_start_impuls++; - - Fzad = 0; - rmp_freq.Out = 0; - -// set_start_mem(FAST_LOG); -// set_start_mem(SLOW_LOG); - } else { - - if (count_start_impuls==2) - { - if (go_a == 1 && go_b == 1) { - // middle pwm - // start_pwm(); f.Go - start_pwm(); - } else if (go_a == 1) { - write_swgen_pwm_times(); //TODO: Check with new PWM - start_pwm_a(); - } else if (go_b == 1) { - write_swgen_pwm_times(); - start_pwm_b(); - } - } // end if (count_start_impuls==1) - - count_start_impuls++; - if (count_start_impuls > 2 * COUNT_START_IMP) { - count_start_impuls = 2 * COUNT_START_IMP; - } - - - } - } - flag_record_log = 1; - log_saved_to_const_mem = 0; - } else { - if (f.Discharge - && (errors.slow_stop2.bit.Break_Resistor == 0) - && (errors.plains_and_others.bit.er0_setted == 0) - && (errors.umu_errors.bit.Voltage380_BSU_Off == 0)) -// && !f.Stop) - { - start_break_pwm(); - break_resistor_managment_calc(); - } else { - //Do not stop, when come for the first time, to write to xilinx times to close keys. - //Otherwise mintime error can occure. - //Also we do not stop pwm wile break_resistor keys working - if (!count_start_impuls) { - // - stop_pwm(); - } - break_resistor_set_closed(); - } - if (count_step_ram_off > 0) { - count_step_ram_off--; - flag_record_log = 1; - } else { - flag_record_log = 0; - } - - pwm_vector_model_titov(f.iq_p_zad, f.iq_fzad, rotor.direct_rotor, - rotor.iqFout, 0, 1, 1); - - if (count_start_impuls > 0) { - count_start_impuls -= 1; - } else { - count_start_impuls = 0; - } - - Uzad1 = 87772; //0.5% - Uzad2 = 87772; - k1 = Uzad1; - svgen_pwm24_1.Gain = Uzad1; - svgen_pwm24_2.Gain = Uzad1; - svgen_dq_1.Ualpha = 0; - svgen_dq_1.Ubeta = 0; - svgen_dq_2.Ualpha = 0; - svgen_dq_2.Ubeta = 0; - a.iqk = Uzad1; - } -// a.iqk1 = Uzad1; -// a.iqk2 = Uzad2; -// a.iqk = (a.iqk1 + a.iqk2) >> 1; -// a.iqf = Fzad; - prevGo = f.Go; - - break_resistor_recup_calc(); - break_resistor_managment_update(); - - if (count_start_impuls >= (2 * COUNT_START_IMP) ) { - - if (f.Mode == 0) { -// test_calc_pwm24(Uzad1, Uzad2, Fzad); - if (pwm_enable_calc_main) { - test_calc_simple_dq_pwm24(Uzad1, Uzad2, Fzad, Fzad, K_MODUL_MAX); - } - analog_dq_calc_const(); - } else { - if ((_IQabs(rotor.iqFout) < STOP_ROTOR_LIMIT) - && (_IQabs(analog.iqIq1) > STOP_ROTOR_MIN_CURRENT)) { - if ((stop_rotor_counter >= 2520)) { - stop_rotor_counter = 2520; -// faults.faults5.bit.rotor_stopped |= 1; - f.rotor_stopped |= 1; - } else { - stop_rotor_counter += 1; - } - if (_IQabs(analog.Fsl) < MIN_Fsl_WHEN_STOPED) { -// faults.faults5.bit.rotor_stopped |= 1; - f.rotor_stopped |= 1; - } - } else { - if (stop_rotor_counter > 0) { - stop_rotor_counter -= 1; - } else { - stop_rotor_counter = 0; - } - } - - pwm_vector_model_titov(f.iq_p_zad, f.iq_fzad, rotor.direct_rotor, - rotor.iqFout, f.Mode, 0, pwm_enable_calc_main); - } - - } else { - // middle - if (count_start_impuls) - { -// svgen_set_time_keys_closed(&svgen_pwm24_1); -// svgen_set_time_keys_closed(&svgen_pwm24_2); - svgen_set_time_middle_keys_open(&svgen_pwm24_1); - svgen_set_time_middle_keys_open(&svgen_pwm24_2); - } - else - // - { - svgen_set_time_keys_closed(&svgen_pwm24_1); - svgen_set_time_keys_closed(&svgen_pwm24_2); - // -// if (faults.faults5.bit.rotor_stopped == 1) { - if (f.rotor_stopped == 1) { - if (stop_rotor_counter > 0) { - stop_rotor_counter -= 1; - } else { -// faults.faults5.bit.rotor_stopped = 0; - f.rotor_stopped = 0; - stop_rotor_counter = 0; - } - } else { - stop_rotor_counter = 0; - } - } - } - - if (f.Mode) { - a.iqf = analog.iqFstator; - } else { - a.iqf = Fzad; - analog.iqFstator = Fzad; - a.iqk1 = _IQsqrt(_IQmpy(svgen_dq_1.Ualpha, svgen_dq_1.Ualpha) + _IQmpy(svgen_dq_1.Ubeta, svgen_dq_1.Ubeta)); //For output Kmodul to terminal - a.iqk2 = _IQsqrt(_IQmpy(svgen_dq_2.Ualpha, svgen_dq_2.Ualpha) + _IQmpy(svgen_dq_2.Ubeta, svgen_dq_2.Ubeta)); //end counting Uout - } - a.iqk = (a.iqk1 + a.iqk2) / 2; - -// write_swgen_pwm_times(); - - if (xpwm_time.one_or_two_interrupts_run == PWM_ONE_INTERRUPT_RUN) - write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_FORCE); - else - { - if (f.Go == 1) - { - if (count_start_impuls == (2 * COUNT_START_IMP)) - { - if (pwm_enable_calc_main) - write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_HIGH); - else - write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_LOW); - } - else -// if (pwm_enable_calc_main) - write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_FORCE); - } - else - { - if (count_start_impuls == (2 * COUNT_START_IMP) - 1) - { - if (pwm_enable_calc_main) - write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_HIGH); - else - write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_LOW); - - } - else - write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_FORCE); - } - - // if (pwm_enable_calc_main) - // prev_run_calc_uf = run_calc_uf; - - } - - - - - - // logs recording -// if ((flag_record_log && !f.stop_Log) || f.Startstoplog) -// //if (f.Log1_Log2 == 1)// && f.Go == 1 && (!f.Stop)) -// //if(f.Go == 1) -// { -// test_mem_limit(FAST_LOG, !f.Ciclelog); -// count_step++; -// -// if (count_step >= 0) { -// fillADClogs(); -//// logpar.log13 = flag_record_log; -//// logpar.log14 = count_start_impuls; -//// logpar.log10 = (int16)_IQtoIQ15(analog.iqIq1_filter); -//// logpar.log11 = (int16)_IQtoIQ15(rotor.iqFrotFromOptica); -// logpar.log15 = (int16) _IQtoIQ15(analog.iqIq2); -// logpar.log16 = (int16) _IQtoIQ15(a.iqk1); -// logpar.log17 = (int16) _IQtoIQ15(analog.iqId1); -// logpar.log18 = (int16) _IQtoIQ15(analog.iqIq1); -// -//// logpar.log24 = (int16) break_result_1; -//// logpar.log25 = (int16) break_result_2; -// logpar.log27 = (int16)(_IQtoIQ15(analog.iqIbtr1_1)); -// logpar.log28 = (int16)(_IQtoIQ15(analog.iqIbtr2_1)); -// -// getFastLogs(!f.Ciclelog); -// count_step = 0; -// } -// } else { -// if (f.Stop && log_saved_to_const_mem == 0) { -// logpar.copy_log_to_const_memory = 1; -// log_saved_to_const_mem = 1; -// } -// } - -// optical_bus_write(); - -// detect_current_saw_val(); - end_int_xtics = xpwm_time.current_period; - f.PWMcounterVal = labs(start_int_xtics - end_int_xtics); - - pwm_run = 0; - - i_sync_pin_off(); -// i_led1_on_off(0); - -} - -void slow_vector_update() -{ - _iq iqKzad = 0; - freq1 = _IQ (f.fzad/F_STATOR_MAX);//f.iqFRotorSetHz; - iqKzad = _IQ(f.kzad); - k1 = zad_intensiv_q(30000, 30000, k1, iqKzad); //20000 - - -} - -//#pragma CODE_SECTION(write_swgen_pwm_times,".fast_run"); -//void write_swgen_pwm_times() -//{ -// xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tc_0.Ti; -// xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tc_1.Ti; -// xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tb_0.Ti; -// xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tb_1.Ti; -// xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Ta_0.Ti; -// xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Ta_1.Ti; -// -// xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tc_0.Ti; -// xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tc_1.Ti; -// xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tb_0.Ti; -// xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tb_1.Ti; -// xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Ta_0.Ti; -// xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Ta_1.Ti; -// -// xpwm_time.Tbr0_0 = break_result_1; -// xpwm_time.Tbr0_1 = break_result_2; -// xpwm_time.Tbr1_0 = break_result_3; -// xpwm_time.Tbr1_1 = break_result_4; -// -// xpwm_time.write_1_2_winding_break_times(&xpwm_time); -// -// -//// logpar.log29 = xpwm_time.Ta0_0; -//// logpar.log30 = xpwm_time.Ta0_1; -//// logpar.log10 = xpwm_time.Tb0_0; -//// logpar.log11 = xpwm_time.Tb0_1; -//// logpar.log12 = xpwm_time.Tc0_0; -//// logpar.log13 = xpwm_time.Tc0_1; -//// logpar.log7 = _IQtoIQ12(svgen_pwm24_1.Alpha); -//// logpar.log8 = xpwm_time.Ta0_0 - xpwm_time.Tb0_0; -//// logpar.log9 = xpwm_time.Ta0_1 - xpwm_time.Tb0_1; -//// logpar.log10 = xpwm_time.Tb0_0 - xpwm_time.Tc0_0; -//// logpar.log11 = xpwm_time.Tb0_1 - xpwm_time.Tc0_1; -//// logpar.log12 = xpwm_time.Tc0_0 - xpwm_time.Ta0_0; -//// logpar.log13 = xpwm_time.Tc0_1 - xpwm_time.Ta0_1; -// -//} - -//#pragma CODE_SECTION(write_swgen_pwm_times_split_eages,".fast_run2"); -//void write_swgen_pwm_times_split_eages(unsigned int mode_reload) -//{ -// -// xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tc_0.Ti; -// xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tc_1.Ti; -// xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tb_0.Ti; -// xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tb_1.Ti; -// xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Ta_0.Ti; -// xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Ta_1.Ti; -// -// xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tc_0.Ti; -// xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tc_1.Ti; -// xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tb_0.Ti; -// xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tb_1.Ti; -// xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Ta_0.Ti; -// xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Ta_1.Ti; -// -// xpwm_time.Tbr0_0 = break_result_1; -// xpwm_time.Tbr0_1 = break_result_2; -// xpwm_time.Tbr1_0 = break_result_3; -// xpwm_time.Tbr1_1 = break_result_4; -// -// xpwm_time.mode_reload = mode_reload; -// -// xpwm_time.write_1_2_winding_break_times_split(&xpwm_time); -//} - -#define CONST_IQ_1 16777216 //1 - -//#pragma CODE_SECTION(fix_pwm_freq_synchro_ain,".fast_run"); -//void fix_pwm_freq_synchro_ain() -//{ -//// if (f.Sync_input_or_output == SYNC_INPUT) -// { -// sync_inc_error(); -// -// if (f.disable_sync || f.sync_ready == 0) -// { -// -// -// return; -// } -// -// if (f.pwm_freq_plus_minus_zero==1) -// { -// -// -// //Increment xtics -// VAR_FREQ_PWM_XTICS = DEF_FREQ_PWM_XTICS + 1; -// WriteMemory(ADR_PWM_PERIOD, VAR_FREQ_PWM_XTICS); // Saw period in tics. 1 tic = 16.67 nsec -// -// change_freq_pwm(VAR_FREQ_PWM_XTICS); -// -// -// } -// -// if (f.pwm_freq_plus_minus_zero==-1) -// { -// //4464 -// //Decrement xtics -// VAR_FREQ_PWM_XTICS = DEF_FREQ_PWM_XTICS - 1; -// WriteMemory(ADR_PWM_PERIOD, VAR_FREQ_PWM_XTICS); // Saw period in tics. 1 tic = 16.67 nsec -// -// change_freq_pwm(VAR_FREQ_PWM_XTICS); -// -// } -// -// if (f.pwm_freq_plus_minus_zero==0) -// { -// VAR_FREQ_PWM_XTICS = DEF_FREQ_PWM_XTICS - 1; -// WriteMemory(ADR_PWM_PERIOD, VAR_FREQ_PWM_XTICS); -// change_freq_pwm(VAR_FREQ_PWM_XTICS); -// } -// -// } -// -// -// -//} - - -//void detect_level_interrupt(void) -//{ -// -// WriteMemory(ADR_SAW_REQUEST, 0x8000); -// xpwm_time.current_period = ReadMemory(ADR_SAW_VALUE); -// -// if (xpwm_time.current_periodxpwm_time.pwm_tics/2) -// xpwm_time.where_interrupt = PWM_HIGH_LEVEL_INTERRUPT; -// -//} -// -//void detect_current_saw_val(void) -//{ -// WriteMemory(ADR_SAW_REQUEST, 0x8000); -// xpwm_time.current_period = ReadMemory(ADR_SAW_VALUE); -//} - - - - diff --git a/Inu/Src2/main/PWMTools.h b/Inu/Src2/main/PWMTools.h deleted file mode 100644 index 593f723..0000000 --- a/Inu/Src2/main/PWMTools.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef PWMTOOLS_H -#define PWMTOOLS_H -#include "f281xpwm.h" - -void InitXPWM(void); -void InitPWM(void); -void PWM_interrupt(void); -void initPWM_Variables(void); - -void slow_vector_update(void); - -extern PWMGEND pwmd; - -extern int VAR_FREQ_PWM_XTICS; -extern int VAR_PERIOD_MAX_XTICS; -extern int VAR_PERIOD_MIN_XTICS; -extern int VAR_PERIOD_MIN_BR_XTICS; -#endif //PWMTOOLS_H - diff --git a/Inu/Src2/main/adc_tools.c b/Inu/Src2/main/adc_tools.c deleted file mode 100644 index 4af68a7..0000000 --- a/Inu/Src2/main/adc_tools.c +++ /dev/null @@ -1,528 +0,0 @@ -// #include "project.h" -#include "adc_tools.h" -// #include "xp_project.h" -#include "IQmathLib.h" -#include "math.h" -#include "my_filter.h" -#include "params.h" -// #include "params_protect.h" -#include "vector.h" -// #include "xp_adc.h" -// #include "TuneUpPlane.h" //Ìîðãàíèå ñâåòîäèîäîì -// #include "log_to_memory.h" -// #include "errors.h" - -// #define COUNT_ARR_ADC_BUF 2 -#define Shift_Filter 1 //2 - -#define BTR_ENABLED - -#define R_ADC_DEFAULT { 1180 ,1180 , 256, 256, 256, 256, 256, 1180, 1180, 256, 256, 256, 256, 256, 256, 256, 256, 256 } -#define K_LEM_ADC_DEFAULT { 60000,60000,5000, 5000,5000,5000,5000,60000,60000,5000,5000,5000,5000,5000,5000,5000,5000,5000 } -//#define LOG_ACP_TO_BUF - -int ADC_f[COUNT_ARR_ADC_BUF][16]; - -int ADC_sf[COUNT_ARR_ADC_BUF][16]; - - -#define ZERO_ADC_DEFAULT {2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048} -_iq19 iq19_zero_ADC[18] = ZERO_ADC_DEFAULT; - -int zero_ADC[18] = ZERO_ADC_DEFAULT; -_iq iq_k_norm_ADC[18]; -_iq19 iq19_k_norm_ADC[18]; - -_iq iq_norm_ADC[18]; - -unsigned int const R_ADC[18] = R_ADC_DEFAULT; -unsigned int const K_LEM_ADC[18] = K_LEM_ADC_DEFAULT; - -//#pragma DATA_SECTION(analog,".fast_vars"); -ANALOG_VALUE analog; -//#pragma DATA_SECTION(filter,".fast_vars"); -ANALOG_VALUE filter; - -ANALOG_RAW_DATA rawData = ANALOG_RAW_DATA_DEFAULT; - -_iq koef_Ud_long_filter=0; -_iq koef_Ud_fast_filter=0; -_iq koef_Im_filter=0; - -_iq koef_Iabc_filter=0; - -_iq koef_Wlong=0; - - -void calc_norm_ADC(void); -void analog_values_calc(void); -_iq im_calc( _iq ia, _iq ib, _iq ic); - -void read_adc(ANALOG_RAW_DATA *data) -{ - ADC_f[0][0] = data->U1_1; - ADC_f[0][1] = data->U1_2; - ADC_f[0][2] = data->Izpt1_1; - ADC_f[0][3] = data->Izpt1_2; - ADC_f[0][4] = data->Ia1; - ADC_f[0][5] = data->Ib1; - ADC_f[0][6] = data->Ic1; - ADC_f[0][7] = 0; - ADC_f[0][8] = data->U2_1; - ADC_f[0][9] = data->U2_2; - ADC_f[0][10] = data->Izpt2_1; - ADC_f[0][11] = data->Izpt2_2; - ADC_f[0][12] = data->Ia2; - ADC_f[0][13] = data->Ib2; - ADC_f[0][14] = data->Ic2; - ADC_f[0][15] = 0; - - - ADC_sf[0][0] += (ADC_f[0][0] - ADC_sf[0][0]) >> Shift_Filter; - ADC_sf[0][1] += (ADC_f[0][1] - ADC_sf[0][1]) >> Shift_Filter; - ADC_sf[0][2] += (ADC_f[0][2] - ADC_sf[0][2]) >> Shift_Filter; - ADC_sf[0][3] += (ADC_f[0][3] - ADC_sf[0][3]) >> Shift_Filter; - ADC_sf[0][4] += (ADC_f[0][4] - ADC_sf[0][4]) >> Shift_Filter; - ADC_sf[0][5] += (ADC_f[0][5] - ADC_sf[0][5]) >> Shift_Filter; - ADC_sf[0][6] += (ADC_f[0][6] - ADC_sf[0][6]) >> Shift_Filter; - - ADC_sf[0][7] += (ADC_f[0][7] - ADC_sf[0][7]) >> Shift_Filter; - ADC_sf[0][8] += (ADC_f[0][8] - ADC_sf[0][8]) >> Shift_Filter; - ADC_sf[0][9] += (ADC_f[0][9] - ADC_sf[0][9]) >> Shift_Filter; - ADC_sf[0][10] += (ADC_f[0][10] - ADC_sf[0][10]) >> Shift_Filter; - ADC_sf[0][11] += (ADC_f[0][11] - ADC_sf[0][11]) >> Shift_Filter; - ADC_sf[0][12] += (ADC_f[0][12] - ADC_sf[0][12]) >> Shift_Filter; - ADC_sf[0][13] += (ADC_f[0][13] - ADC_sf[0][13]) >> Shift_Filter; - ADC_sf[0][14] += (ADC_f[0][14] - ADC_sf[0][14]) >> Shift_Filter; - ADC_sf[0][15] += (ADC_f[0][15] - ADC_sf[0][15]) >> Shift_Filter; - - - ADC_f[1][0] = 0; - ADC_f[1][1] = 0; - ADC_f[1][2] = 0; - ADC_f[1][3] = 0; - - ADC_sf[1][0] += (ADC_f[1][0] - ADC_sf[1][0]) >> Shift_Filter; - ADC_sf[1][1] += (ADC_f[1][1] - ADC_sf[1][1]) >> Shift_Filter; - ADC_sf[1][2] += (ADC_f[1][2] - ADC_sf[1][2]) >> Shift_Filter; - ADC_sf[1][3] += (ADC_f[1][3] - ADC_sf[1][3]) >> Shift_Filter; -} - -void acp_Handler(void) -{ - // read_adc(); - calc_norm_ADC(); - analog_values_calc(); - // Read_Fast_Errors(); -} - -#define COUNT_DETECT_ZERO 500 - -// void detect_zero_analog() -// { -// int i, k; -// double tmp_buff[2][16] = {0}; -// // _iq koef_zero_ADC_filter = 1; -// for(i = 0; i < 16; tmp_buff[0][i++] = 0); -// for(i = 0; i < 16; tmp_buff[1][i++] = 0); -// i = 0; -// for (i = 0; i err_level_adc_on_go ? 1 : 0; -// mask |= filter.iqU_2_long > err_level_adc_on_go ? 1 << 1: 0; -// } -// else -// { -// mask |= filter.iqU_1_long > err_level_adc ? 1 : 0; -// mask |= filter.iqU_2_long > err_level_adc ? 1 << 1: 0; -// } - - -// #ifdef NEW_I_ZPT_REZISTORS -// mask |= ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 2: 0; -// mask |= ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 3: 0; -// #else -// mask |= ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS? 1 << 2: 0; -// mask |= ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS? 1 << 3: 0; -// #endif - -// mask |= ADC_sf[0][4] < ERR_LEVEL_I_FAZA_PLUS? 1 << 4: 0; -// mask |= ADC_sf[0][5] < ERR_LEVEL_I_FAZA_PLUS? 1 << 5: 0; -// mask |= ADC_sf[0][6] < ERR_LEVEL_I_FAZA_PLUS? 1 << 6: 0; - -// if(f.Go) -// { -// mask |= filter.iqU_3_long > err_level_adc_on_go ? 1 << 8: 0; -// mask |= filter.iqU_4_long > err_level_adc_on_go ? 1 << 9: 0; -// } -// else -// { -// mask |= (filter.iqU_3_long > err_level_adc) ? 1 << 8: 0; -// mask |= (filter.iqU_4_long > err_level_adc) ? 1 << 9: 0; -// } - - -// #ifdef NEW_I_ZPT_REZISTORS -// mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 10: 0; -// mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 11: 0; -// #else -// mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS? 1 << 10: 0; -// mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS? 1 << 11: 0; -// #endif - -// mask |= ADC_sf[0][12] < ERR_LEVEL_I_FAZA_PLUS? 1 << 12: 0; -// mask |= ADC_sf[0][13] < ERR_LEVEL_I_FAZA_PLUS? 1 << 13: 0; -// mask |= ADC_sf[0][14] < ERR_LEVEL_I_FAZA_PLUS? 1 << 14: 0; - -// //Calculate values for BTR -// #ifdef BTR_ENABLED -// mask |= ADC_sf[1][0] < ERR_LEVEL_BREAK_REZ? 1 << 7: 0; //BTR1pos -// mask |= ADC_sf[1][2] < ERR_LEVEL_BREAK_REZ? 1 << 15: 0; //BTR1neg -// #endif //BTR_ENABLED -// /* */ - -// result = mask & prev_mask; -// prev_mask |= mask; // -// // -// if(mask == 0) { prev_mask = 0xFFFF;} -// return result; - -// } - - -// unsigned int detect_protect_ACP_minus() -// { -// unsigned int mask = 0, result = 0; -// static unsigned int prev_mask = 0xFCFC; // - -// /* */ -// if(f.Ready2) -// { -// // mask |= (ADC_sf[0] > ERR_LEVEL_ADC_MINUS_6) ? 1: 0; -// // mask |= (ADC_sf[1] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 1: 0; -// } -// #ifdef NEW_I_ZPT_REZISTORS -// mask |= (ADC_sf[0][2] > ERR_LEVEL_I_ZPT_MINUS_REZ_249)? 1 << 2: 0; -// mask |= (ADC_sf[0][3] > ERR_LEVEL_I_ZPT_MINUS_REZ_249)? 1 << 3: 0; -// #else -// mask |= (ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS)? 1 << 2: 0; -// mask |= (ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS)? 1 << 3: 0; -// #endif -// mask |= ADC_sf[0][4] > ERR_LEVEL_I_FAZA_MINUS? 1 << 4: 0; -// mask |= ADC_sf[0][5] > ERR_LEVEL_I_FAZA_MINUS? 1 << 5: 0; -// mask |= ADC_sf[0][6] > ERR_LEVEL_I_FAZA_MINUS? 1 << 6: 0; -// /* */ -// /* if(f.Ready2) -// { -// mask |= (ADC_sf[0][7] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 8: 0; -// mask |= (ADC_sf[0][8] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 9: 0; -// } */ -// #ifdef NEW_I_ZPT_REZISTORS -// mask |= ADC_sf[0][10] > ERR_LEVEL_I_ZPT_MINUS_REZ_249? 1 << 10: 0; -// mask |= ADC_sf[0][11] > ERR_LEVEL_I_ZPT_MINUS_REZ_249? 1 << 11: 0; -// #else -// mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS? 1 << 10: 0; -// mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS? 1 << 11: 0; -// #endif - -// mask |= ADC_sf[0][12] > ERR_LEVEL_I_FAZA_MINUS? 1 << 12: 0; -// mask |= ADC_sf[0][13] > ERR_LEVEL_I_FAZA_MINUS? 1 << 13: 0; -// mask |= ADC_sf[0][14] > ERR_LEVEL_I_FAZA_MINUS? 1 << 14: 0; - -// //Calculate values for BTR -// #ifdef BTR_ENABLED -// mask |= ADC_sf[1][1] < ERR_LEVEL_BREAK_REZ? 1 << 7: 0; //BTR2pos -// mask |= ADC_sf[1][3] < ERR_LEVEL_BREAK_REZ? 1 << 15: 0; //BTR2neg -// #endif //BTR_ENABLED -// /* */ - - -// result = mask & prev_mask; -// prev_mask |= mask; // -// // -// if(mask == 0) { prev_mask = 0xFFFF;} -// return result; - -// } - -// #pragma CODE_SECTION(fillADClogs,".fast_run"); -// void fillADClogs(void) -// { -// logpar.log1 = (int16)_IQtoIQ15(analog.iqU_1); -// logpar.log2 = (int16)_IQtoIQ15(analog.iqU_2); -// logpar.log3 = (int16)_IQtoIQ15(analog.iqIin_1); -// logpar.log4 = (int16)_IQtoIQ15(analog.iqIin_2); -// logpar.log5 = (int16)_IQtoIQ15(analog.iqIa1_1); -// logpar.log6 = (int16)_IQtoIQ15(analog.iqIb1_1); -// logpar.log7 = (int16)_IQtoIQ15(analog.iqIc1_1); -// logpar.log8 = (int16)_IQtoIQ15(analog.iqU_3); -// logpar.log9 = (int16)_IQtoIQ15(analog.iqU_4); -// logpar.log10 = (int16)_IQtoIQ15(analog.iqIin_3); -// logpar.log11 = (int16)_IQtoIQ15(analog.iqIin_4); -// logpar.log12 = (int16)_IQtoIQ15(analog.iqIa2_1); -// logpar.log13 = (int16)_IQtoIQ15(analog.iqIb2_1); -// logpar.log14 = (int16)_IQtoIQ15(analog.iqIc2_1); -// // logpar.log15 = (int16)_IQtoIQ15(filter.iqU_1_fast); -// // logpar.log16 = (int16)_IQtoIQ15(filter.iqU_1_long); -// } - -/********************************************************************/ -/* Расчет модулy тока из показаний трех фаз */ -/********************************************************************/ -_iq SQRT_32 = _IQ(0.8660254037844); -_iq CONST_23 = _IQ(2.0/3.0); -_iq CONST_15 = _IQ(1.5); - -_iq im_calc( _iq ia, _iq ib, _iq ic) -{ - _iq isa,isb, t; - - - isa = _IQmpy(CONST_15,ia); - isb = _IQmpy(SQRT_32,ib-ic ); - -// ( _IQ19mpy(SQRT_32, _IQ15toIQ19(ib)) - _IQ15mpy(SQRT_32, _IQ15toIQ19(ic)) ); - - // t = _IQmag(isa,isb); - t = _IQsqrt(_IQmpy(isa, isa) + _IQmpy(isb, isb)); - t = _IQmpy(CONST_23,t); - - return (t); - -} - - diff --git a/Inu/Src2/main/adc_tools.h b/Inu/Src2/main/adc_tools.h deleted file mode 100644 index 137324f..0000000 --- a/Inu/Src2/main/adc_tools.h +++ /dev/null @@ -1,137 +0,0 @@ -#ifndef ADC_TOOLS_H -#define ADC_TOOLS_H - -#include "IQmathLib.h" -// #include "isr.h" - -typedef struct -{ - - _iq iqIin_1; - _iq iqIin_2; - _iq iqIin_3; - _iq iqIin_4; -// _iq iqIin_1_rms; -// _iq iqIin_2_rms; -// _iq iqIin_3_rms; -// _iq iqIin_4_rms; - _iq iqIin; - - _iq iqIa1_1; - _iq iqIb1_1; - _iq iqIc1_1; - _iq iqIa2_1; - _iq iqIb2_1; - _iq iqIc2_1; - - _iq iqIa1_rms; - _iq iqIb1_rms; - _iq iqIc1_rms; - _iq iqIa2_rms; - _iq iqIb2_rms; - _iq iqIc2_rms; - - _iq iqIq_zadan; - _iq iqIq_zad_from_optica; - _iq iqId1; - _iq iqIq1; - _iq iqIq1_filter; - _iq iqId2; - _iq iqIq2; - _iq iqIq2_filter; - - _iq iqUd1; - _iq iqUq1; - _iq iqUd2; - _iq iqUq2; - _iq iqUq2_filter; - _iq tetta; - _iq Fsl; - - _iq iqFstator; - - _iq iqU_1; - _iq iqU_2; - _iq iqU_3; - _iq iqU_4; - - _iq iqW1; - _iq iqWexp; - _iq iqWfir; - _iq iqWout; - _iq iqPvsi1; - _iq iqPvsi2; - _iq iqM_1; - _iq iqM_2; - - _iq iqW2; - - _iq iqW; - - _iq iqU_1_long; - _iq iqU_2_long; - _iq iqU_3_long; - _iq iqU_4_long; - - _iq iqU_1_fast; - _iq iqU_2_fast; - _iq iqU_3_fast; - _iq iqU_4_fast; - - _iq iqIm_1; - _iq iqIm_2; - _iq iqIm_1_long; - _iq iqIm_2_long; - - _iq iqIm; - - _iq iqIbtr1_1; - _iq iqIbtr1_2; - _iq iqIbtr2_1; - _iq iqIbtr2_2; - -} ANALOG_VALUE; - -typedef struct { - float U1_1; - float U1_2; - float Izpt1_1; - float Izpt1_2; - float Ia1; - float Ib1; - float Ic1; - float U2_1; - float U2_2; - float Izpt2_1; - float Izpt2_2; - float Ia2; - float Ib2; - float Ic2; - - void (*read_adc)(); -} ANALOG_RAW_DATA; - - - -extern ANALOG_VALUE analog; -extern ANALOG_VALUE filter; -extern _iq iq_norm_ADC[18]; -extern ANALOG_RAW_DATA rawData; - -#define COUNT_ARR_ADC_BUF 2 -extern int ADC_f[COUNT_ARR_ADC_BUF][16]; - -void log_acp(void); -void acp_Handler(void); -void init_Adc_Variables(void); -void calc_Izpt_rms(void); -unsigned int detect_protect_ACP_plus(void); -unsigned int detect_protect_ACP_minus(void); -void fillADClogs(void); -void read_adc(ANALOG_RAW_DATA *data); - -#define ANALOG_RAW_DATA_DEFAULT {0,0,0,0,0,0,0,0,0,0,0,0,0,0,\ - read_adc} - -#endif // ADC_TOOLS_H - diff --git a/Inu/Src2/main/dmctype.h b/Inu/Src2/main/dmctype.h deleted file mode 100644 index 95f314f..0000000 --- a/Inu/Src2/main/dmctype.h +++ /dev/null @@ -1,31 +0,0 @@ -/* ================================================================================= -File name: DMCTYPE.H - -Originator: Digital Control Systems Group - Texas Instruments - -Description: DMC data type definition file. -===================================================================================== - History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20 -------------------------------------------------------------------------------*/ - -#ifndef DMCTYPE -#define DMCTYPE - -//--------------------------------------------------------------------------- -// For Portability, User Is Recommended To Use Following Data Type Size -// Definitions For 16-bit and 32-Bit Signed/Unsigned Integers: -// - -typedef int int16; -typedef long int32; -typedef unsigned int Uint16; -typedef unsigned long Uint32; -typedef float float32; -typedef long double float64; - - -#endif // DMCTYPE - diff --git a/Inu/Src2/main/my_filter.c b/Inu/Src2/main/my_filter.c deleted file mode 100644 index 3fc444f..0000000 --- a/Inu/Src2/main/my_filter.c +++ /dev/null @@ -1,122 +0,0 @@ -// #include "DSP281x_Device.h" // DSP281x Headerfile Include File -// #include "DSP281x_Examples.h" // DSP281x Examples Include File -#include "IQmathLib.h" -#include "my_filter.h" - - -// #pragma CODE_SECTION(exp_regul_iq19,".fast_run"); -_iq19 exp_regul_iq19(_iq19 k_exp_regul, _iq19 InpVarCurr, _iq19 InpVarInstant) -{ - _iq19 t1; - - t1 = (InpVarCurr + _IQ19mpy( (InpVarInstant-InpVarCurr), k_exp_regul)); - return t1; -} - - -// #pragma CODE_SECTION(exp_regul_iq,".fast_run"); -_iq exp_regul_iq(_iq k_exp_regul, _iq InpVarCurr, _iq InpVarInstant) -{ - _iq t1; - t1 = (InpVarCurr + _IQmpy( (InpVarInstant-InpVarCurr), k_exp_regul)); - return t1; -} - -// #pragma CODE_SECTION(exp_regul_iq,".fast_run"); -void exp_regul_iq_fast(_iq k_exp_regul, _iq *InpVarCurr, _iq InpVarInstant) -{ - _iq t1; - volatile _iq t2,t3,t4; - - - t2 = (InpVarInstant- *InpVarCurr); - t3 = _IQmpy( t2, k_exp_regul); - t4 = *InpVarCurr + t3; - t1 = t4; - *InpVarCurr = t1; -// t1 = (InpVarCurr + _IQmpy( (InpVarInstant-InpVarCurr), k_exp_regul)); -// return t1; -} - -_iq zad_intensiv_q(_iq StepP, _iq StepN, _iq InpVarCurr, _iq InpVarInstant) -{ - _iq deltaVar, VarOut; - - deltaVar = InpVarInstant-InpVarCurr; - - if ((deltaVar>StepP) || (deltaVar<-StepN)) - { - if (deltaVar>0) InpVarCurr += StepP; - else InpVarCurr -= StepN; - } - else - InpVarCurr=InpVarInstant; - - - VarOut = InpVarCurr; - return VarOut; - - -} - - - - - -/* - -_iq18 filter_1p(_iq18 predx,_iq18 predy, _iq18 inpx) -{ - _iq18 t1; - - t1 = _IQ18mpy(k1_filter_1p_fast,(predx+inpx))+_IQ18mpy(k2_filter_1p_fast,predy); - return t1; -} - - -*/ - -/* - -void init_filter_batter2() -{ - u_filter_batter2[0]=0; - u_filter_batter2[1]=0; - u_filter_batter2[2]=0; - - i_filter_batter2[0]=0; - i_filter_batter2[1]=0; - i_filter_batter2[2]=0; - - k_filter_batter2[0]=_IQ(K1_FILTER_BATTER2_3HZ); - k_filter_batter2[1]=_IQ(K2_FILTER_BATTER2_3HZ); - k_filter_batter2[2]=_IQ(K3_FILTER_BATTER2_3HZ ); - -} - - - - -//#pragma CODE_SECTION(filter_batter2,".fast_run"); -_iq filter_batter2(_iq InpVarCurr) -{ -_iq y; - - y = _IQmpy(k_filter_batter2[0],( InpVarCurr + _IQmpyI32(i_filter_batter2[0],2) + i_filter_batter2[1] ) ) + - _IQmpy(k_filter_batter2[1], u_filter_batter2[0]) + _IQmpy(k_filter_batter2[2], u_filter_batter2[1]); - - u_filter_batter2[1]=u_filter_batter2[0]; - u_filter_batter2[0]=y; - - i_filter_batter2[1]=i_filter_batter2[0]; - i_filter_batter2[0]=InpVarCurr; - return y; - -} - - -*/ - - - - diff --git a/Inu/Src2/main/my_filter.h b/Inu/Src2/main/my_filter.h deleted file mode 100644 index cb8b6d3..0000000 --- a/Inu/Src2/main/my_filter.h +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef _MY_FILTER -#define _MY_FILTER - - - -#ifdef __cplusplus - extern "C" { -#endif - - - - -#include "IQmathLib.h" -//#include "filter.h" -//#include "myfir16.h" - - - -#define k1_filter_1p_fast 62643 // 0.238965*262144 -#define k2_filter_1p_fast 136857 // 0.52207*262144 -#define k3_filter_1p_fast2 8552 - - -#define filter_1p_fast(predx, predy,inpx) predy=_IQ18mpy(k1_filter_1p_fast,(predx+inpx))+_IQ18mpy(k2_filter_1p_fast,predy);predx=inpx -#define filter_1p_fast2(predx, predy,inpx) predy=_IQ18mpy(k3_filter_1p_fast2,(predx+inpx));predx=inpx - - - - - -//extern FIR16 fir; -//extern FIR16 fir_wrotor; -//extern IIR5BIQ16 iir; - - -void init_my_filter_fir(); -int calc_my_filter_fir(int xn); - -void init_my_filter_fir_58(); -int calc_my_filter_fir_58(int xn); - - - -void calc_my_filter_fir_50_75hz(_iq xn_0,_iq xn_1,_iq xn_2,_iq xn_3,_iq xn_4,_iq xn_5, - _iq *yn_0,_iq *yn_1,_iq *yn_2,_iq *yn_3,_iq *yn_4,_iq *yn_5); - -void init_my_filter_fir_50_75hz(); - - - -void init_my_filter_iir(); -int calc_my_filter_iir(int xn); - - - -_iq19 exp_regul_iq19(_iq19 k_exp_regul, _iq19 InpVarCurr, _iq19 InpVarInstant); -_iq exp_regul_iq(_iq k_exp_regul, _iq InpVarCurr, _iq InpVarInstant); - - - - -_iq18 filter_1p(_iq18 predx,_iq18 predy, _iq18 inpx); - -void exp_regul_iq_fast(_iq k_exp_regul, _iq *InpVarCurr, _iq InpVarInstant); - - -_iq zad_intensiv_q(_iq StepP, _iq StepN, _iq InpVarCurr, _iq InpVarInstant); - - - -#ifdef __cplusplus - } -#endif - -#endif /* _MY_FILTER */ - diff --git a/Inu/Src2/main/params.h b/Inu/Src2/main/params.h deleted file mode 100644 index e34752d..0000000 --- a/Inu/Src2/main/params.h +++ /dev/null @@ -1,181 +0,0 @@ -#ifndef _PARAMS -#define _PARAMS - - -#define PROPUSK_TEST_TKAK_ON_GO 1 // Go=1 - - -/*************************************************************************************/ -//#define _test_without_power 1 // -//#define _test_without_skaf 1 // -//#define _test_fast_calc 1 // -//#define _test_fast_calc_level1 1 // - -#define _test_without_doors -//#define _test_without_BSO -//#define _test_without_CAN -//#define CHECK_IN_OUT_TERMINAL - -//#define OFF_ERROR_UKSS1 // 1 -//#define OFF_ERROR_UKSS2 //1 -//#define OFF_ERROR_UKSS3 //2 -//#define OFF_ERROR_UKSS4 //2 -//#define OFF_ERROR_UKSS5 // -//#define OFF_ERROR_UKSS6 //BTR1 -//#define OFF_ERROR_UKSS7 //BTR2 -//#define OFF_ERROR_UKSS8 // - -//#define DISABLE_RS_MPU 1 - -#define NEW_I_ZPT_REZISTORS - -#define ENABLE_RECUPER 1 - -#define FREQ_TIMER0 200 /* _0 */ -#define FREQ_TIMER1 3300 /* _1 */ - -//#define FREQ_PWM 1180 /* */ //3138 // 2360//2477 // -//#define DOUBLE_UPDATE_PWM 1 - -#define DMIN 750 // 15mks Dminimum -#define SECOND (FREQ_PWM*3) -#define MINUTE (SECOND*60) - - - -//#define REVERS_ON_CLOCK 1 // 0 // 1- .. 0 - - -//#define BAN_ROTOR_REVERS_DIRECT 1 - -//#define TIME_PAUSE_ZADATCHIK 750//500 -// -//#define TIME_SET_LINE_RELAY_FAN 3000 // -//#define LEVEL_FAN_ON_TEMPER_ACDRIVE 1400 // -//#define LEVEL_FAN_OFF_TEMPER_ACDRIVE 1200 // -////( LEVEL_FAN_ON_TEMPER_ACDRIVE ~20 ) -//#define TIME_SET_LINE_RELAY_FAN 3000 // -// -// -//#define MAX_TIME_DIRECT_ROTOR 5000 // . -//#define MIN_TIME_DIRECT_ROTOR -5000 // -// -//#define LEVEL_FORWARD_TIME_DIRECT_ROTOR 4000 // -//#define LEVEL_BACK_TIME_DIRECT_ROTOR -4000 // -// -//#define MAX_TIME_ERROR_ROTOR 5000 // . -//#define MIN_TIME_ERROR_ROTOR 0 // . -// -// -//#define LEVEL_ON_ERROR_ROTOR 4000 // -//#define LEVEL_OFF_ERROR_ROTOR 1000 // -// -// -// -//#define WORK_TWICE 0 /* y */ -// -// -//#define PID_KP_IM 0.018 //0.036 //0.018 //0.18 //0.095 // PID Kp -//#define PID_KI_IM 0.08 // 0.008 // PID Ki -//#define PID_KD_IM 0.0000 //*100 // PID Kd -//#define PID_KC_IM 0.09 // PID Kc -// -// -//#define PID_KP_F 12//26//12 //40 //20 //12 //20 //60.0 //20.0 //0.095 // PID Kp -//#define PID_KI_F 0.00010 // 0.008 // PID Ki -//#define PID_KD_F 0.000 //*100 PID Kd -//#define PID_KC_F 0.005 // PID Kc -////#define PID_KC_F 0.000 // PID Kc -// -//#define ADD_KP_DF (1000.0/NORMA_MZZ)//(500.0/NORMA_MZZ)//(50.0/NORMA_MZZ) -//#define ADD_KI_DF (2000.0/NORMA_MZZ)//(1000.0/NORMA_MZZ)//(100.0/NORMA_MZZ) -//#define MAX_DELTA_pidF 2.0 -//#define MIN_MZZ_FOR_DF 1761607 //(210/NORMA_MZZ) -// -// -//#define Im_PREDEL 600 /* */ -//#define I_OUT_PREDEL -20 /* . y */ -//#define U_IN_PREDEL 500 /* y */ -// -//#define IQ_NORMAL_CHARGE_UD_MAX 12163481 // 1450 V //13002342 // 1550 //_IQtoF(filter.iqU_1_long)*NORMA_ACP -//#define IQ_NORMAL_CHARGE_UD_MIN 10066329 // 1200 V -// -// -//#define U_D_MAX_ERROR_GLOBAL 17616076 // 2100 V //17196646 //2050V // 16777216 //2000V/2000*2^24 -//#define U_D_MAX_ERROR 16777216 // 2000V //16357785 //1950V //15938355 //1900V/2000*2^24 -// -////#define U_D_NORMA_MIN 3774873 // 450 V // 13421772 // 450 V 22.05.08 //1600V/2000*2^24 -////#define U_D_NORMA_MAX 15518924 // //15099494 //1850V/2000*2^24 -// -//#define U_D_MIN_ERROR 10905190 // 1300V/2000*2^24 -// -//#define I_IN_MAX_ERROR_GLOBAL 18454937 // 2200 A //16777216 //2000 A // 13421772 //1600 A //10905190 //1300 // 900A -// -//#define KOEFF_WROTOR_FILTER_SPARTAN 7//8 -//#define MAX_DELTA_WROTOR_S_1_2 1 -// -//#define ENABLE_I_HDW_PROTECT_ON_GLOBAL 1 // -// -//#define TIME_WAIT_CHARGE 2000 //5000 // 10000 -//#define TIME_WAIT_CHARGE_OUT 15000 //15000 -//#define TIME_SET_LINE_RELAY 10000 -//#define TIME_SET_LINE_RELAY5 3000 -//#define TIME_WAIT_LEVEL_QPU2 3000 -// - -///--------------------------- 22220 paremetrs -------------------///////// - -//////////////////////////////////////////////////////////////// -// Loaded capasitors level -#define V_CAPASITORS_LOADED_IQ 11184810 //13421772 ~ 2400V // 11184810 ~ 2000V -#define V_NOMINAL 14260633 //15099494 ~ 2700V - -// Level of nominal currents -#define I_OUT_NOMINAL 1585.0 //A -#define I_OUT_NOMINAL_IQ 12482249 //9777761 ~ 1748 //8388608 ~ 1500A // 10066329 ~ 1800A // 8863067 ~ 1585 - //11184811 ~ 2000A // 12482249 ~ 2232A -#define I_OUT_1_6_NOMINAL_IQ 14180908 -#define I_OUT_1_8_NOMINAL_IQ 15953520 -#define I_ZPT_NOMINAL_IQ 6123683 //1095A - -#define IQ_OUT_NOM 2000.0 //1350.0 -#define ID_OUT_NOM (I_OUT_NOMINAL * 0.52) - -#define NORMA_FROTOR 20.0 -#define NORMA_MZZ 3000.0 //5000 -#define NORMA_ACP 3000.0 -#define DISABLE_TEST_TKAK_ON_START 1 -//#define MOTOR_STEND 1 -#define F_STATOR_MAX 20.0 /* . */ -#define F_STATOR_NOM 12.0 //Hz -#define IQ_F_STATOR_NOM 10066329 - -#define F_ROTOR_NOM 2.0 //Hz -#define IQ_F_ROTOR_NOM 1677721 - -#define FREQ_PWM 450 //420 // 350 //401 //379 -#define FREQ_PWM_BASE 400 - -#ifdef MOTOR_STEND -#define POLUS 4 /* */ -#define BPSI_NORMAL 0.9//0.7 //Hz -#define MAX_FZAD_FROM_SU 16.7 // -#define MAX_FZAD_FROM_SU_OBOROT 1100 -#define COS_FI 0.98 -#else -#define POLUS 6 /* */ -#define BPSI_NORMAL 0.9 //Hz -#define MAX_FZAD_FROM_SU 16.7 // -#define MAX_FZAD_FROM_SU_OBOROT 1650 -#define COS_FI 0.87 -//PCH21 - 0.81 -//PCH32 - 0.82 -//PCH12 - 0.82 -#endif - - -#define KOEF_TEMPER_DECR_MZZ 2.0 - -#endif - - - diff --git a/Inu/Src2/main/pid_reg3.c b/Inu/Src2/main/pid_reg3.c deleted file mode 100644 index 4cc46c1..0000000 --- a/Inu/Src2/main/pid_reg3.c +++ /dev/null @@ -1,107 +0,0 @@ -/*===================================================================================== - File name: PID_REG3.C (IQ version) - - Originator: Digital Control Systems Group - Texas Instruments - - Description: The PID controller with anti-windup - -===================================================================================== - History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20 --------------------------------------------------------------------------------------*/ - -#include "IQmathLib.h" // Include header for IQmath library -// Don't forget to set a proper GLOBAL_Q in "IQmathLib.h" file -//#include "dmctype.h" -#include "pid_reg3.h" - -#define IQ_100 1677721600 - -#pragma CODE_SECTION(pid_reg3_calc,".fast_run"); -void pid_reg3_calc(PIDREG3 *v) -{ -_iq Ud2; - // Compute the error - v->Err = v->Ref - v->Fdb; - - // Compute the proportional output - v->Up = _IQmpy(v->Kp,v->Err); - - // Compute the integral output - v->Ui = v->Ui + _IQmpy(v->Ki,v->Up) + _IQmpy(v->Kc,v->SatErr); -/* - // Saturate the integral output - if (v->Ui > v->OutMax) - v->Ui = v->OutMax; - else if (v->Ui < v->OutMin) - v->Ui = v->OutMin; -*/ - // Compute the derivative output - Ud2 = v->Up - v->Up1;// _IQmpy(IQ_100,(v->Up - v->Up1)); - v->Ud = _IQmpy(v->Kd,Ud2); - - - // Compute the pre-saturated output - v->OutPreSat = v->Up + v->Ui + v->Ud; - - // Saturate the output - if (v->OutPreSat > v->OutMax) - v->Out = v->OutMax; - else if (v->OutPreSat < v->OutMin) - v->Out = v->OutMin; - else - v->Out = v->OutPreSat; - - // Compute the saturate difference - v->SatErr = v->Out - v->OutPreSat; - - // Update the previous proportional output - v->Up1 = v->Up; - - -} - -#pragma CODE_SECTION(pid_reg3_calc,".fast_run"); -void pid_reg3_undependent_calc(PIDREG3 *v) -{ -_iq Ud2; - // Compute the error - v->Err = v->Ref - v->Fdb; - - // Compute the proportional output - v->Up = _IQmpy(v->Kp,v->Err); - - // Compute the integral output - v->Ui = v->Ui + _IQmpy(v->Ki,v->Err) + _IQmpy(v->Kc,v->SatErr); -/* - // Saturate the integral output - if (v->Ui > v->OutMax) - v->Ui = v->OutMax; - else if (v->Ui < v->OutMin) - v->Ui = v->OutMin; -*/ - // Compute the derivative output - Ud2 = v->Up - v->Up1;// _IQmpy(IQ_100,(v->Up - v->Up1)); - v->Ud = _IQmpy(v->Kd,Ud2); - - - // Compute the pre-saturated output - v->OutPreSat = v->Up + v->Ui + v->Ud; - - // Saturate the output - if (v->OutPreSat > v->OutMax) - v->Out = v->OutMax; - else if (v->OutPreSat < v->OutMin) - v->Out = v->OutMin; - else - v->Out = v->OutPreSat; - - // Compute the saturate difference - v->SatErr = v->Out - v->OutPreSat; - - // Update the previous proportional output - v->Up1 = v->Up; - -} diff --git a/Inu/Src2/main/pid_reg3.h b/Inu/Src2/main/pid_reg3.h deleted file mode 100644 index 20b68c6..0000000 --- a/Inu/Src2/main/pid_reg3.h +++ /dev/null @@ -1,117 +0,0 @@ -/* ================================================================================= -File name: PID_REG3.H (IQ version) - -Originator: Digital Control Systems Group - Texas Instruments - -Description: -Header file containing constants, data type definitions, and -function prototypes for the PIDREG3. -===================================================================================== - History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20 -------------------------------------------------------------------------------*/ -#ifndef __PIDREG3_H__ -#define __PIDREG3_H__ - -#include "IQmathlib.h" -//#include "dmctype.h" - -typedef struct { _iq Ref; // Input: Reference input - _iq Fdb; // Input: Feedback input - _iq Err; // Variable: Error - _iq Kp; // Parameter: Proportional gain - _iq Up; // Variable: Proportional output - _iq Ui; // Variable: Integral output - _iq Ud; // Variable: Derivative output - _iq OutPreSat; // Variable: Pre-saturated output - _iq OutMax; // Parameter: Maximum output - _iq OutMin; // Parameter: Minimum output - _iq Out; // Output: PID output - _iq SatErr; // Variable: Saturated difference - _iq Ki; // Parameter: Integral gain - _iq Kc; // Parameter: Integral correction gain - _iq Kd; // Parameter: Derivative gain - _iq Up1; // History: Previous proportional output - void (*calc)(); // Pointer to calculation function - } PIDREG3; - -typedef PIDREG3 *PIDREG3_handle; -/*----------------------------------------------------------------------------- -Default initalizer for the PIDREG3 object. ------------------------------------------------------------------------------*/ -#define PIDREG3_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(PIDREG3_handle))pid_reg3_calc } - -#define PIDREG3_UNDEPENDENT_DEFAULTS { 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - 0, \ - (void (*)(PIDREG3_handle))pid_reg3_undependent_calc } - -/*------------------------------------------------------------------------------ -Prototypes for the functions in PIDREG3.C -------------------------------------------------------------------------------*/ -void pid_reg3_calc(PIDREG3_handle); -void pid_reg3_undependent_calc(PIDREG3 *v); - -#endif // __PIDREG3_H__ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Inu/Src2/main/rotation_speed.h b/Inu/Src2/main/rotation_speed.h deleted file mode 100644 index a28fa49..0000000 --- a/Inu/Src2/main/rotation_speed.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef _ROT_SPEED -#define _ROT_SPEED - -#include "IQmathLib.h" - -#define SENSORS_NUMBER 10 -#define SENSORS_NUMBER_ONLY_IN 6 -#define IMPULSES_PER_TURN (1LL << 13) //Old sensor -//#define IMPULSES_PER_TURN (1LL << 10) //New sensor -#define ANGLE_RESOLUTION (1LL << 18) //2^18 - -typedef struct -{ - int direct_rotor; - int direct_rotor_in1; - int direct_rotor_in2; - int direct_rotor_angle; - union { - unsigned int sens_err1:1; - unsigned int sens_err2:1; - unsigned int reserved:14; - } error; - - _iq iqFsensors[SENSORS_NUMBER]; - - _iq iqF; - _iq iqFout; - _iq iqFlong; - - _iq iqFrotFromOptica; - - unsigned int error_update_count; -} ROTOR_VALUE; - -#define ROTOR_VALUE_DEFAULTS {0,0,0,0,0, {0,0,0,0,0,0,0,0},0,0,0,0,0} - -extern ROTOR_VALUE rotor; - -void Rotor_measure(void); -void rotorInit(void); -void update_rot_sensors(); - -#endif //_ROT_SPEED - diff --git a/Inu/Src2/main/svgen_dq.h b/Inu/Src2/main/svgen_dq.h deleted file mode 100644 index 2949fd1..0000000 --- a/Inu/Src2/main/svgen_dq.h +++ /dev/null @@ -1,38 +0,0 @@ -/* ================================================================================= -File name: SVGEN_DQ.H (IQ version) - -Originator: Digital Control Systems Group - Texas Instruments - -Description: -Header file containing constants, data type definitions, and -function prototypes for the SVGEN_DQ. -===================================================================================== - History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20 -------------------------------------------------------------------------------*/ -#ifndef __SVGEN_DQ_H__ -#define __SVGEN_DQ_H__ - -typedef struct { _iq Ualpha; // Input: reference alpha-axis phase voltage - _iq Ubeta; // Input: reference beta-axis phase voltage - _iq Ta; // Output: reference phase-a switching function - _iq Tb; // Output: reference phase-b switching function - _iq Tc; // Output: reference phase-c switching function - void (*calc)(); // Pointer to calculation function - } SVGENDQ; - -typedef SVGENDQ *SVGENDQ_handle; -/*----------------------------------------------------------------------------- -Default initalizer for the SVGENDQ object. ------------------------------------------------------------------------------*/ -#define SVGENDQ_DEFAULTS { 0,0,0,0,0, \ - (void (*)(Uint32))svgendq_calc } - -/*------------------------------------------------------------------------------ -Prototypes for the functions in SVGEN_DQ.C -------------------------------------------------------------------------------*/ -void svgendq_calc(SVGENDQ_handle); - -#endif // __SVGEN_DQ_H__ diff --git a/Inu/Src2/main/svgen_dq_v2.c b/Inu/Src2/main/svgen_dq_v2.c deleted file mode 100644 index cb8b85c..0000000 --- a/Inu/Src2/main/svgen_dq_v2.c +++ /dev/null @@ -1,120 +0,0 @@ -/*===================================================================================== - File name: SVGEN_DQ.C (IQ version) - - Originator: Digital Control Systems Group - Texas Instruments - - Description: Space-vector PWM generation based on d-q components - -===================================================================================== - History: -------------------------------------------------------------------------------------- - 04-15-2005 Version 3.20 --------------------------------------------------------------------------------------*/ - -#include "IQmathLib.h" // Include header for IQmath library -// Don't forget to set a proper GLOBAL_Q in "IQmathLib.h" file -#include "dmctype.h" -#include "svgen_dq.h" -_iq iq_max = _IQ(1.0)-1; - - -// #pragma CODE_SECTION(svgendq_calc,".fast_run2"); -void svgendq_calc(SVGENDQ *v) -{ - _iq Va,Vb,Vc,t1,t2,temp_sv1,temp_sv2; - Uint16 Sector = 0; // Sector is treated as Q0 - independently with global Q - - Sector = 0; \ - temp_sv1=_IQdiv2(v->Ubeta); /*divide by 2*/ \ - temp_sv2=_IQmpy(_IQ(0.8660254),v->Ualpha); /* 0.8660254 = sqrt(3)/2*/ \ - -// Inverse clarke transformation - Va = v->Ubeta; \ - Vb = -temp_sv1 + temp_sv2; \ - Vc = -temp_sv1 - temp_sv2; \ - -// 60 degree Sector determination - if (Va>0) - Sector = 1; - if (Vb>0) - Sector = Sector + 2; - if (Vc>0) - Sector = Sector + 4; - -// X,Y,Z (Va,Vb,Vc) calculations X = Va, Y = Vb, Z = Vc \ Va = v.Ubeta; - Vb = temp_sv1 + temp_sv2; - Vc = temp_sv1 - temp_sv2; -// Sector 0: this is special case for (Ualpha,Ubeta) = (0,0) - - if (Sector==0) // Sector 0: this is special case for (Ualpha,Ubeta) = (0,0) - { - v->Ta = _IQ(0.5); - v->Tb = _IQ(0.5); - v->Tc = _IQ(0.5); - } - if (Sector==1) // Sector 1: t1=Z and t2=Y (abc ---> Tb,Ta,Tc) - { - t1 = Vc; - t2 = Vb; - v->Tb = _IQdiv2((_IQ(1)-t1-t2)); // tbon = (1-t1-t2)/2 - v->Ta = v->Tb+t1; // taon = tbon+t1 - v->Tc = v->Ta+t2; // tcon = taon+t2 - } - else if (Sector==2) // Sector 2: t1=Y and t2=-X (abc ---> Ta,Tc,Tb) - { - t1 = Vb; - t2 = -Va; - v->Ta = _IQdiv2((_IQ(1)-t1-t2)); // taon = (1-t1-t2)/2 - v->Tc = v->Ta+t1; // tcon = taon+t1 - v->Tb = v->Tc+t2; // tbon = tcon+t2 - } - else if (Sector==3) // Sector 3: t1=-Z and t2=X (abc ---> Ta,Tb,Tc) - { - t1 = -Vc; - t2 = Va; - v->Ta = _IQdiv2((_IQ(1)-t1-t2)); // taon = (1-t1-t2)/2 - v->Tb = v->Ta+t1; // tbon = taon+t1 - v->Tc = v->Tb+t2; // tcon = tbon+t2 - } - else if (Sector==4) // Sector 4: t1=-X and t2=Z (abc ---> Tc,Tb,Ta) - { - t1 = -Va; - t2 = Vc; - v->Tc = _IQdiv2((_IQ(1)-t1-t2)); // tcon = (1-t1-t2)/2 - v->Tb = v->Tc+t1; // tbon = tcon+t1 - v->Ta = v->Tb+t2; // taon = tbon+t2 - } - else if (Sector==5) // Sector 5: t1=X and t2=-Y (abc ---> Tb,Tc,Ta) - { - t1 = Va; - t2 = -Vb; - v->Tb = _IQdiv2((_IQ(1)-t1-t2)); // tbon = (1-t1-t2)/2 - v->Tc = v->Tb+t1; // tcon = tbon+t1 - v->Ta = v->Tc+t2; // taon = tcon+t2 - } - else if (Sector==6) // Sector 6: t1=-Y and t2=-Z (abc ---> Tc,Ta,Tb) - { - t1 = -Vb; - t2 = -Vc; - v->Tc = _IQdiv2((_IQ(1)-t1-t2)); // tcon = (1-t1-t2)/2 - v->Ta = v->Tc+t1; // taon = tcon+t1 - v->Tb = v->Ta+t2; // tbon = taon+t2 - } - -// Convert the unsigned GLOBAL_Q format (ranged (0,1)) -> signed GLOBAL_Q format (ranged (-1,1)) - v->Ta = _IQmpy2(v->Ta - _IQ(0.5)); - v->Tb = _IQmpy2(v->Tb - _IQ(0.5)); - v->Tc = _IQmpy2(v->Tc - _IQ(0.5)); - - if (v->Ta > iq_max) v->Ta = iq_max; - if (v->Tb > iq_max) v->Tb = iq_max; - if (v->Tc > iq_max) v->Tc = iq_max; - - if (v->Ta < -iq_max) v->Ta = -iq_max; - if (v->Tb < -iq_max) v->Tb = -iq_max; - if (v->Tc < -iq_max) v->Tc = -iq_max; - -} - - diff --git a/Inu/Src2/main/v_pwm24.c b/Inu/Src2/main/v_pwm24.c deleted file mode 100644 index 357cb98..0000000 --- a/Inu/Src2/main/v_pwm24.c +++ /dev/null @@ -1,1635 +0,0 @@ - - -#include "v_pwm24.h" -//#include "DSP281x_Device.h" // DSP281x Headerfile Include File -//#include "big_dsp_module.h" -//#include "rmp2cntl.h" // Include header for the VHZPROF object -//#include "rmp_cntl_my1.h" // Include header for the VHZPROF object -#include "pid_reg3.h" // Include header for the VHZPROF object - -#include "params.h" -// #include "PWMTools.h" -#include "adc_tools.h" -#include "v_pwm24.h" - -#include "dq_to_alphabeta_cos.h" - -#include "IQmathLib.h" -// #include "log_to_memory.h" -//Xilinx -//#include "x_parameters.h" -// #include "x_basic_types.h" -// #include "xp_project.h" -// #include "xp_cds_tk.h" -#include "svgen_dq.h" -#include "xp_write_xpwm_time.h" - -#include "def.h" - -#define DEF_FREQ_PWM_XTICS T1_PRD -#define DEF_PERIOD_MIN_XTICS (DT + 10e-6)*FTBCLK - -// xilinx (60000000 / 16 / FREQ_PWM = 3750000 / FREQ_PWM) -// #pragma DATA_SECTION(VAR_FREQ_PWM_XTICS,".fast_vars1"); -int VAR_FREQ_PWM_XTICS = DEF_FREQ_PWM_XTICS; - -// xilinx -// #pragma DATA_SECTION(VAR_PERIOD_MAX_XTICS,".fast_vars1"); -int VAR_PERIOD_MAX_XTICS = DEF_FREQ_PWM_XTICS - DEF_PERIOD_MIN_XTICS; - -// xilinx (mintime+deadtime) (F * T.. = (60 / 16 / 2) * T = (60 * T / 16 / 2)) -// #pragma DATA_SECTION(VAR_PERIOD_MIN_XTICS,".fast_vars1"); -int VAR_PERIOD_MIN_XTICS = DEF_PERIOD_MIN_XTICS;// - -// xilinx (mintime) (F * T.. = (60 / 16 / 2) * T = (60 * T / 16 / 2)) -// #pragma DATA_SECTION(VAR_PERIOD_MIN_BR_XTICS,".fast_vars1"); -// int VAR_PERIOD_MIN_BR_XTICS = DEF_PERIOD_MIN_BR_XTICS;// - -#define PWM_ONE_INTERRUPT_RUN 1 -#define PWM_TWICE_INTERRUPT_RUN 0 - - - -#define SQRT3 29058990 //1.7320508075688772935274463415059 -#define CONST_IQ_1 16777216 //1 -#define CONST_IQ_05 8388608 //0.5 -#define CONST_IQ_2 33554432 //2 - -#define CONST_IQ_PI6 8784530 //30 -#define CONST_IQ_PI3 17569060 // 60 -#define CONST_IQ_PI 52707178 // 180 -#define CONST_IQ_OUR1 35664138 // -#define CONST_IQ_2PI 105414357 // 360 -#define CONST_IQ_120G 35138119 // 120 grad -#define CONST_IQ_3 50331648 // 3 - -#define IQ_ALFA_SATURATION1 15099494//16441671//15099494 -#define IQ_ALFA_SATURATION2 1677721//16441671//15099494 - - -#define PI 3.1415926535897932384626433832795 - -// #pragma DATA_SECTION(iq_alfa_coef,".fast_vars"); -_iq iq_alfa_coef = 16777216; - -// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -//#pragma DATA_SECTION(freq_array,".v_24pwm_vars"); -//int freq_array[COUNT_VAR_FREQ]; -// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - -//#pragma DATA_SECTION(pidCur_Kp,".v_24pwm_vars"); -//_iq pidCur_Kp = 0; - -// #pragma DATA_SECTION(pidCur_Ki,".fast_vars"); -_iq pidCur_Ki = 0; - -// #pragma DATA_SECTION(ar_tph,".fast_vars"); -_iq ar_tph[7]; - -// #pragma DATA_SECTION(winding_displacement,".fast_vars"); -_iq winding_displacement = CONST_IQ_PI6; - -//#pragma DATA_SECTION(iq_koef_mod_korrect_1,".fast_vars");//v_24pwm_vars -//_iq iq_koef_mod_korrect_1; - -//#pragma DATA_SECTION(iq_koef_mod_korrect_2,".v_24pwm_vars"); -//_iq iq_koef_mod_korrect_2; - -//#pragma DATA_SECTION(ar_sa_all,".v_24pwm_vars"); -// #pragma DATA_SECTION(ar_sa_all,".fast_vars"); -int ar_sa_all[3][6][4][7] = { { - { - {-1,0,0,0,1,1,1},{0,1,1,1,0,0,0},{0,0,1,1,1,0,0},{0,1,1,1,0,0,0} - }, - { - {-1,-1,0,0,0,1,1},{-1,-1,0,0,0,0,0},{-1,0,0,0,1,0,0},{0,0,1,1,0,0,0} - }, - { - {-1,-1,-1,0,0,0,1},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0} - }, - { - {-1,-1,-1,0,0,0,1},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0} - }, - { - {-1,-1,0,0,0,1,1},{-1,-1,0,0,0,0,0},{-1,0,0,0,1,0,0},{0,0,1,1,0,0,0} - }, - { - {-1,0,0,0,1,1,1},{0,1,1,1,0,0,0},{0,0,1,1,1,0,0},{0,1,1,1,0,0,0} - } - }, - { - { - {-1,-1,0,0,0,1,1},{-1,-1,0,0,0,0,0},{-1,0,0,0,1,0,0},{0,0,1,1,0,0,0} - }, - { - {-1,0,0,0,1,1,1},{0,1,1,1,0,0,0},{0,0,1,1,1,0,0},{0,1,1,1,0,0,0} - }, - { - {-1,0,0,0,1,1,1},{0,1,1,1,0,0,0},{0,0,1,1,1,0,0},{0,1,1,1,0,0,0} - }, - { - {-1,-1,0,0,0,1,1},{-1,-1,0,0,0,0,0},{-1,0,0,0,1,0,0},{0,0,1,1,0,0,0} - }, - { - {-1,-1,-1,0,0,0,1},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0} - }, - { - {-1,-1,-1,0,0,0,1},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0} - } - }, - { - { - {-1,-1,-1,0,0,0,1},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0} - }, - { - {-1,-1,-1,0,0,0,1},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0},{-1,-1,-1,0,0,0,0} - }, - { - {-1,-1,0,0,0,1,1},{-1,-1,0,0,0,0,0},{-1,0,0,0,1,0,0},{0,0,1,1,0,0,0} - }, - { - {-1,0,0,0,1,1,1},{0,1,1,1,0,0,0},{0,0,1,1,1,0,0},{0,1,1,1,0,0,0} - }, - { - {-1,0,0,0,1,1,1},{0,1,1,1,0,0,0},{0,0,1,1,1,0,0},{0,1,1,1,0,0,0} - }, - { - {-1,-1,0,0,0,1,1},{-1,-1,0,0,0,0,0},{-1,0,0,0,1,0,0},{0,0,1,1,0,0,0} - } - } - }; - - -// #pragma DATA_SECTION(svgen_pwm24_1,".v_24pwm_vars"); -//#pragma DATA_SECTION(svgen_pwm24_1,".fast_vars"); -SVGEN_PWM24 svgen_pwm24_1 = SVGEN_PWM24_DEFAULTS; -// #pragma DATA_SECTION(svgen_pwm24_2,".v_24pwm_vars"); -//#pragma DATA_SECTION(svgen_pwm24_2,".fast_vars"); -SVGEN_PWM24 svgen_pwm24_2 = SVGEN_PWM24_DEFAULTS; - -// #pragma DATA_SECTION(svgen_dq_1,".v_24pwm_vars"); -SVGENDQ svgen_dq_1 = SVGENDQ_DEFAULTS; -// #pragma DATA_SECTION(svgen_dq_2,".v_24pwm_vars"); -SVGENDQ svgen_dq_2 = SVGENDQ_DEFAULTS; - -//#pragma DATA_SECTION(delta_t1_struct,".v_24pwm_vars"); -//#pragma DATA_SECTION(delta_t1_struct,".fast_vars"); -//PIDREG3 delta_t1_struct = PIDREG3_DEFAULTS; -//#pragma DATA_SECTION(delta_t2_struct,".v_24pwm_vars"); -//#pragma DATA_SECTION(delta_t2_struct,".fast_vars"); -//PIDREG3 delta_t2_struct = PIDREG3_DEFAULTS; - -void calc_t_abc(_iq k, _iq teta, int region, _iq *iq_tt0, _iq *iq_tt1, _iq *iq_tt2, _iq *iq_tt3, _iq *iq_tt4, _iq *iq_tt5); -void set_predel_dshim24(SVGEN_PWM24_TIME *T,int16 dmin,int16 dmax); -void set_predel_dshim24_simple0(SVGEN_PWM24_TIME *T,int16 dmin,int16 dmax); -void set_predel_dshim24_simple1(SVGEN_PWM24_TIME *T,int16 dmin,int16 dmax); -int detect_sec(_iq teta); -int detect_region(_iq k, _iq teta); -// - - - - -//void init_alpha(void) -//{ -// -//// power_ain1.init(&power_ain1); -//// power_ain2.init(&power_ain2); -// -// svgen_mf1.NewEntry = 0;//_IQ(0.5); -// svgen_mf2.NewEntry = 0; -// -// svgen_mf1.SectorPointer = 0; -// svgen_mf2.SectorPointer = 0; -// -//// 0 -// svgen_mf1.Alpha = _IQ(0); -// svgen_mf2.Alpha = _IQ(0); -// -// -// -// -// -//#if (SETUP_SDVIG_OBMOTKI == SDVIG_OBMOTKI_30_PLUS) -//// 30 . -// svgen_mf1.Alpha = _IQ(0.5); -// svgen_mf2.Alpha = _IQ(0); -// -// svgen_mf1.Full_Alpha = svgen_mf1.Alpha; -// svgen_mf2.Full_Alpha = svgen_mf2.Alpha; -//#else -// -// -//#if (SETUP_SDVIG_OBMOTKI == SDVIG_OBMOTKI_30_MINUS) -//// -30 . -// svgen_mf1.Alpha = _IQ(0); -// svgen_mf2.Alpha = _IQ(0.5); -// svgen_mf1.Full_Alpha = svgen_mf1.Alpha; -// svgen_mf2.Full_Alpha = svgen_mf2.Alpha; -//#else -// -//#if (SETUP_SDVIG_OBMOTKI == SDVIG_OBMOTKI_ZERO) -//// -30 . -// svgen_mf1.Alpha = _IQ(0); -// svgen_mf2.Alpha = _IQ(0); -// svgen_mf1.Full_Alpha = svgen_mf1.Alpha; -// svgen_mf2.Full_Alpha = svgen_mf2.Alpha; -//#else -// #error "!!!!!! SETUP_SDVIG_OBMOTKI params_motor.h!!!" -// -// -//#endif -// -//#endif -// -// -// -//#endif -// -// -// -//} - -void InitVariablesSvgen(unsigned int freq) -{ -////////// Inserted from 'initPWM_Variables' for modulation project ////////// - - // , , - xpwm_time.Tclosed_0 = 0; - xpwm_time.Tclosed_1 = VAR_FREQ_PWM_XTICS + 1; - xpwm_time.Tclosed_high = VAR_FREQ_PWM_XTICS + 1; - xpwm_time.pwm_tics = VAR_FREQ_PWM_XTICS; - // - // " =0x0 - // SAW_DIRECTbit = 0 > =0 - // - // SAW_DIRECTbit = 1 <= =0 - // - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - // 22220 0, .. . - xpwm_time.saw_direct.all = 0;//0x0555; - xpwm_time.one_or_two_interrupts_run = PWM_TWICE_INTERRUPT_RUN; - - initXpwmTimeStructure(&xpwm_time); - init_alpha_pwm24(VAR_FREQ_PWM_XTICS); -///////////////////////////////////////////////////////////// - - svgen_pwm24_1.pwm_minimal_impuls_zero_minus = 0;//(float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0;// DEF_PERIOD_MIN_XTICS_100;//DEF_PERIOD_MIN_XTICS_80; - svgen_pwm24_1.pwm_minimal_impuls_zero_plus = 0;//(float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0;// DEF_PERIOD_MIN_XTICS_80; - - svgen_pwm24_2.pwm_minimal_impuls_zero_minus = svgen_pwm24_1.pwm_minimal_impuls_zero_minus; - svgen_pwm24_2.pwm_minimal_impuls_zero_plus = svgen_pwm24_1.pwm_minimal_impuls_zero_plus; - - - svgen_pwm24_1.Tclosed_high = xpwm_time.Tclosed_1; - svgen_pwm24_2.Tclosed_high = xpwm_time.Tclosed_1; - - - - - svgen_dq_1.Ualpha = 0; - svgen_dq_1.Ubeta = 0; - - svgen_dq_2.Ualpha = 0; - svgen_dq_2.Ubeta = 0; - - - -} - - - - - - - - - - -// #pragma CODE_SECTION(recalc_time_pwm_minimal_2_xilinx_pwm24_l,".fast_run2"); -void recalc_time_pwm_minimal_2_xilinx_pwm24_l(SVGEN_PWM24 *pwm24, - _iq *T0, _iq *T1, - _iq timpuls_corr ) -{ - -//_iq pwm_t, timpuls_corr; - - volatile unsigned long pwm_t; - volatile unsigned int minimal_plus, minimal_minus; - - - - minimal_plus = pwm24->pwm_minimal_impuls_zero_plus; - minimal_minus = pwm24->pwm_minimal_impuls_zero_minus; - - // if (pwm24->prev_level == V_PWM24_PREV_PWM_CLOSE || pwm24->prev_level == V_PWM24_PREV_PWM_MIDDLE || pwm24->prev_level == V_PWM24_PREV_PWM_WORK_KM0) - // { - // minimal_plus *= 2; -// minimal_minus *= 2; -// } - - pwm_t = timpuls_corr / pwm24->XilinxFreq; - - // *T_imp = pwm_t; - -// if (pwm_t>(pwm24->Tclosed_high-4*minimal_minus)) -// pwm_t=(pwm24->Tclosed_high-4*minimal_minus); - - - if (timpuls_corr >= 0) - { - *T0 = pwm_t + minimal_plus; - // *T1 = 0; - *T1 = pwm24->Tclosed_high - minimal_minus; - } - else - { - *T0 = 0 + minimal_plus; - // *T1 = -pwm_t - minimal_minus; - *T1 = pwm24->Tclosed_high + pwm_t - minimal_minus; - } - - - // if (*T0 < minimal_plus) - // *T0 = minimal_plus; - - // if (*T0 > (pwm24->Tclosed_high - 2 * minimal_plus)) - // *T0 = (pwm24->Tclosed_high - 2 * minimal_plus); - - // if (*T1 < (2 * minimal_minus)) - // *T1 = 2 * minimal_minus; - - // if (*T1 > (pwm24->Tclosed_high - minimal_minus)) - // *T1 = (pwm24->Tclosed_high - minimal_minus); - -} - -static DQ_TO_ALPHABETA dq_to_ab = DQ_TO_ALPHABETA_DEFAULTS; - -// #pragma CODE_SECTION(test_calc_dq_pwm24,".v_24pwm_run"); -void test_calc_dq_pwm24(_iq Ud, _iq Uq, _iq Ud2, _iq Uq2, _iq tetta,_iq Uzad_max) -{ -// DQ_TO_ALPHABETA dq_to_ab = DQ_TO_ALPHABETA_DEFAULTS; - _iq maxUq = 0; - _iq Uzad_max_square = _IQmpy(Uzad_max, Uzad_max); - - if (tetta > CONST_IQ_2PI) - { - tetta -= CONST_IQ_2PI; - } - - if (tetta < 0) - { - tetta += CONST_IQ_2PI; - } - // - maxUq = _IQsqrt(Uzad_max_square - _IQmpy(Ud, Ud)); - if (Uq > maxUq) { Uq = maxUq;} - if (Uq < -maxUq) { Uq = -maxUq;} - - - //Reculct dq to alpha-beta - dq_to_ab.Tetta = tetta; - dq_to_ab.Ud = Ud; - dq_to_ab.Uq = Uq; - dq_to_ab.calc2(&dq_to_ab); - //Calc swgen times for 1-st winding - svgen_dq_1.Ualpha = dq_to_ab.Ualpha; - svgen_dq_1.Ubeta = dq_to_ab.Ubeta; - svgen_dq_1.calc(&svgen_dq_1); - - // - maxUq = _IQsqrt(Uzad_max_square - _IQmpy(Ud, Ud)); - if (Uq2 > maxUq) { Uq2 = maxUq;} - if (Uq2 < -maxUq) { Uq2 = -maxUq;} - - //Reculc dq to alpha-beta for 2-nd winding with winding displasement - dq_to_ab.Tetta = tetta - winding_displacement; - dq_to_ab.Ud = Ud2; - dq_to_ab.Uq = Uq2; - dq_to_ab.calc2(&dq_to_ab); - //Calc swgen times for 1-st winding - svgen_dq_2.Ualpha = dq_to_ab.Ualpha; - svgen_dq_2.Ubeta = dq_to_ab.Ubeta; - svgen_dq_2.calc(&svgen_dq_2); - - //1 winding - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_1, &svgen_pwm24_1.Ta_0.Ti, &svgen_pwm24_1.Ta_1.Ti, svgen_dq_1.Ta); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_1, &svgen_pwm24_1.Tb_0.Ti, &svgen_pwm24_1.Tb_1.Ti, svgen_dq_1.Tb); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_1, &svgen_pwm24_1.Tc_0.Ti, &svgen_pwm24_1.Tc_1.Ti, svgen_dq_1.Tc); - - // 2 winding - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_2, &svgen_pwm24_2.Ta_0.Ti, &svgen_pwm24_2.Ta_1.Ti, svgen_dq_2.Ta); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_2, &svgen_pwm24_2.Tb_0.Ti, &svgen_pwm24_2.Tb_1.Ti, svgen_dq_2.Tb); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_2, &svgen_pwm24_2.Tc_0.Ti, &svgen_pwm24_2.Tc_1.Ti, svgen_dq_2.Tc); - -// set_predel_dshim24_simple0(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -/* - set_predel_dshim24(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);*/ -} - - -// #pragma CODE_SECTION(test_calc_simple_dq_pwm24,".v_24pwm_run"); -void test_calc_simple_dq_pwm24(_iq uz1, _iq uz2, _iq fz1, _iq fz2,_iq Uzad_max) -{ - static _iq hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2); -// static _iq tetta = 0; - DQ_TO_ALPHABETA dq_to_ab = DQ_TO_ALPHABETA_DEFAULTS; - - _iq Ud = 0; - _iq Uq = _IQsat(uz1,Uzad_max,0); - - analog.tetta += _IQmpy(fz1, hz_to_angle); - - if (analog.tetta >= CONST_IQ_2PI) - { - analog.tetta -= CONST_IQ_2PI; - } - - if (analog.tetta < 0) - { - analog.tetta += CONST_IQ_2PI; - } - - dq_to_ab.Tetta = analog.tetta; - dq_to_ab.Ud = Ud; - dq_to_ab.Uq = Uq; - dq_to_ab.calc2(&dq_to_ab); - - svgen_dq_1.Ualpha = dq_to_ab.Ualpha; - svgen_dq_1.Ubeta = dq_to_ab.Ubeta; - -// svgen_dq_1.Ualpha = 0; -// svgen_dq_1.Ubeta = 0; - - svgen_dq_1.calc(&svgen_dq_1); - - dq_to_ab.Tetta = analog.tetta - winding_displacement; - dq_to_ab.Ud = Ud; - dq_to_ab.Uq = Uq; - dq_to_ab.calc2(&dq_to_ab); - - svgen_dq_2.Ualpha = dq_to_ab.Ualpha; - svgen_dq_2.Ubeta = dq_to_ab.Ubeta; - - svgen_dq_2.calc(&svgen_dq_2); - - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_1, &svgen_pwm24_1.Ta_0.Ti, &svgen_pwm24_1.Ta_1.Ti, svgen_dq_1.Ta); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_1, &svgen_pwm24_1.Tb_0.Ti, &svgen_pwm24_1.Tb_1.Ti, svgen_dq_1.Tb); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_1, &svgen_pwm24_1.Tc_0.Ti, &svgen_pwm24_1.Tc_1.Ti, svgen_dq_1.Tc); - - // 2 - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_2, &svgen_pwm24_2.Ta_0.Ti, &svgen_pwm24_2.Ta_1.Ti, svgen_dq_2.Ta); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_2, &svgen_pwm24_2.Tb_0.Ti, &svgen_pwm24_2.Tb_1.Ti, svgen_dq_2.Tb); - recalc_time_pwm_minimal_2_xilinx_pwm24_l (&svgen_pwm24_2, &svgen_pwm24_2.Tc_0.Ti, &svgen_pwm24_2.Tc_1.Ti, svgen_dq_2.Tc); - -// logpar.log1 = (int16)(_IQtoIQ15(uz1)); -// logpar.log2 = (int16)(_IQtoIQ15(fz1)); -// logpar.log3 = (int16)(_IQtoIQ15(Ud)); -// logpar.log4 = (int16)(_IQtoIQ15(Uq)); -// logpar.log5 = (int16)(_IQtoIQ15(svgen_dq_1.Ualpha)); -// logpar.log6 = (int16)(_IQtoIQ15(svgen_dq_1.Ubeta)); -// logpar.log7 = (int16)(_IQtoIQ15(svgen_dq_1.Ta)); -// logpar.log8 = (int16)(_IQtoIQ15(svgen_dq_1.Tb)); -// logpar.log9 = (int16)(_IQtoIQ15(svgen_dq_1.Tc)); -// logpar.log10 = (int16)(_IQtoIQ12(analog.tetta)); -// logpar.log11 = (int16)(svgen_pwm24_1.Ta_0.Ti); -// logpar.log12 = (int16)((svgen_pwm24_1.Ta_1.Ti)); -// logpar.log13 = (int16)(svgen_pwm24_1.Tb_0.Ti); -// logpar.log14 = (int16)((svgen_pwm24_1.Tb_1.Ti)); -// logpar.log15 = (int16)(svgen_pwm24_1.Tc_0.Ti); -// logpar.log16 = (int16)((svgen_pwm24_1.Tc_1.Ti)); - - -// svgen_pwm24_1.calc(&svgen_pwm24_1); -// svgen_pwm24_2.calc(&svgen_pwm24_2); - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - -// set_predel_dshim24_simple0(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// -// set_predel_dshim24_simple0(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); -// set_predel_dshim24_simple1(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - -} - -// #pragma CODE_SECTION(test_calc_pwm24,".v_24pwm_run"); -void test_calc_pwm24(_iq uz1, _iq uz2, _iq fz1) -{ - //static int i =0; - svgen_pwm24_1.Freq = fz1; - svgen_pwm24_2.Freq = fz1; - - svgen_pwm24_1.Gain = uz1;//_IQmpy(uz1,iq_koef_mod_korrect_1); - svgen_pwm24_2.Gain = uz2;//_IQmpy(uz2,iq_koef_mod_korrect_2); - - svgen_pwm24_1.delta_U = filter.iqU_1_fast - filter.iqU_2_fast; - svgen_pwm24_2.delta_U = filter.iqU_3_fast - filter.iqU_4_fast; - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - svgen_pwm24_1.delta_U = 0; - svgen_pwm24_2.delta_U = 0; - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - svgen_pwm24_1.Ia = analog.iqIa1_1; - svgen_pwm24_1.Ib = analog.iqIb1_1; - svgen_pwm24_1.Ic = analog.iqIc1_1; - - svgen_pwm24_2.Ia = analog.iqIa2_1; - svgen_pwm24_2.Ib = analog.iqIb2_1; - svgen_pwm24_2.Ic = analog.iqIc2_1; - - svgen_pwm24_1.calc(&svgen_pwm24_1); - svgen_pwm24_2.calc(&svgen_pwm24_2); - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - set_predel_dshim24(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - -// if(((svgen_pwm24_2.Ta_0.Ti > (VAR_FREQ_PWM_XTICS - VAR_PERIOD_MIN_XTICS)) && (svgen_pwm24_2.Ta_0.Ti <= (VAR_FREQ_PWM_XTICS))) || -// ((svgen_pwm24_2.Ta_1.Ti > (VAR_FREQ_PWM_XTICS - VAR_PERIOD_MIN_XTICS)) && (svgen_pwm24_2.Ta_1.Ti <= (VAR_FREQ_PWM_XTICS))) || -// ((svgen_pwm24_2.Tb_0.Ti > (VAR_FREQ_PWM_XTICS - VAR_PERIOD_MIN_XTICS)) && (svgen_pwm24_2.Tb_0.Ti <= (VAR_FREQ_PWM_XTICS))) || -// ((svgen_pwm24_2.Tb_1.Ti > (VAR_FREQ_PWM_XTICS - VAR_PERIOD_MIN_XTICS)) && (svgen_pwm24_2.Tb_1.Ti <= (VAR_FREQ_PWM_XTICS))) || -// ((svgen_pwm24_2.Tc_0.Ti > (VAR_FREQ_PWM_XTICS - VAR_PERIOD_MIN_XTICS)) && (svgen_pwm24_2.Tc_0.Ti <= (VAR_FREQ_PWM_XTICS))) || -// ((svgen_pwm24_2.Tc_1.Ti > (VAR_FREQ_PWM_XTICS - VAR_PERIOD_MIN_XTICS)) && (svgen_pwm24_2.Tc_1.Ti <= (VAR_FREQ_PWM_XTICS)))) -// { -// asm(" NOP "); -// } -// -// if( ((svgen_pwm24_2.Ta_0.Ti > 0) && (svgen_pwm24_2.Ta_0.Ti < (VAR_PERIOD_MIN_XTICS))) || -// ((svgen_pwm24_2.Ta_1.Ti > 0) && (svgen_pwm24_2.Ta_1.Ti < (VAR_PERIOD_MIN_XTICS))) || -// ((svgen_pwm24_2.Tb_0.Ti > 0) && (svgen_pwm24_2.Tb_0.Ti < (VAR_PERIOD_MIN_XTICS))) || -// ((svgen_pwm24_2.Tb_1.Ti > 0) && (svgen_pwm24_2.Tb_1.Ti < (VAR_PERIOD_MIN_XTICS))) || -// ((svgen_pwm24_2.Tc_0.Ti > 0) && (svgen_pwm24_2.Tc_0.Ti < (VAR_PERIOD_MIN_XTICS))) || -// ((svgen_pwm24_2.Tc_1.Ti > 0) && (svgen_pwm24_2.Tc_1.Ti < (VAR_PERIOD_MIN_XTICS)))) -// { -// asm(" NOP "); -// } -} - - -// #pragma CODE_SECTION(svgen_pwm24_calc,".v_24pwm_run"); -void svgen_pwm24_calc(SVGEN_PWM24 *vt) -{ - _iq StepAngle; - - StepAngle = _IQmpy(vt->Freq,vt->FreqMax); - - vt->Alpha = vt->Alpha + StepAngle; - - - if (vt->Alpha > CONST_IQ_2PI) - { - vt->Alpha -= CONST_IQ_2PI; - } - - if (vt->Alpha < 0) - { - vt->Alpha += CONST_IQ_2PI; - } - - - calc_time_one_tk(vt->Gain, vt->Alpha, vt->delta_U, vt->Ia, vt->Ib, vt->Ic, - vt->number_svgen, &vt->Ta_0, &vt->Ta_1,&vt->Tb_0, &vt->Tb_1,&vt->Tc_0, &vt->Tc_1); - - - - vt->Ta_0.Ti = vt->Ta_0.Ti/vt->XilinxFreq; - vt->Ta_1.Ti = vt->Ta_1.Ti/vt->XilinxFreq; - - vt->Tb_0.Ti = vt->Tb_0.Ti/vt->XilinxFreq; - vt->Tb_1.Ti = vt->Tb_1.Ti/vt->XilinxFreq; - - vt->Tc_0.Ti = vt->Tc_0.Ti/vt->XilinxFreq; - vt->Tc_1.Ti = vt->Tc_1.Ti/vt->XilinxFreq; - -} - - -// #pragma CODE_SECTION(calc_time_one_tk,".v_24pwm_run"); -void calc_time_one_tk(_iq gain, _iq teta, _iq delta_U, - _iq Ia, _iq Ib, _iq Ic, - unsigned int number, - SVGEN_PWM24_TIME *tk0, - SVGEN_PWM24_TIME *tk1, - SVGEN_PWM24_TIME *tk2, - SVGEN_PWM24_TIME *tk3, - SVGEN_PWM24_TIME *tk4, - SVGEN_PWM24_TIME *tk5) -{ - _iq iq_t0 = 0, iq_t1 = 0, iq_t2 = 0, iq_t3 = 0, iq_t4 = 0, iq_t5 = 0; - _iq t_pos = 0, t_neg = 0; - _iq delta_ttt; - _iq teta60; - - int sector, region; - int cur_sign, i, ki; - int updown_tk0; - int updown_tk1; - - volatile _iq t_tk0, t_tk1; - - updown_tk0 = 1; - updown_tk1 = 0; - - sector = detect_sec(teta); // - teta60 = teta - _IQmpy(_IQ(sector - 1),CONST_IQ_PI3); // - - if ((sector == 2) || (sector == 4) || (sector == 6)) - { - teta60 = CONST_IQ_PI3 - teta60; - } - - region = detect_region(gain, teta60); // - - calc_t_abc(gain, teta60, region, &iq_t0, &iq_t1, &iq_t2, &iq_t3, &iq_t4, &iq_t5); - - - delta_ttt = 0; //calc_delta_t(delta_U,number,region); - //delta_ttt = 0; - - //if (number == 1) - //{ - //logpar.log1 = (int16)(_IQtoIQ15(delta_U)); - //logpar.log2 = (int16)(_IQtoIQ15(delta_ttt)); - //} - //else - //{ - //logpar.log3 = (int16)(_IQtoIQ15(delta_U)); - //logpar.log4 = (int16)(_IQtoIQ15(delta_ttt)); - //} - - calc_arr_tph(sector, region, iq_t0, iq_t1, iq_t2, iq_t3, iq_t4, iq_t5, delta_ttt,number, Ia, Ib, Ic); - - for (ki = 0; ki < 3; ki++) - { - t_pos = 0; - t_neg = 0; - - for (i = 0; i < 7; i++) - { - cur_sign = ar_sa_all[ki][sector - 1][region - 1][i]; - - if (cur_sign > 0) - { - t_pos += ar_tph[i]; - } - - if (cur_sign < 0) - { - t_neg += ar_tph[i]; - } - } - - t_pos = t_pos << 1; - - t_neg = t_neg << 1; - - if (t_neg == 0) - { - t_tk0 = 0; - } - else - { - t_tk0 = t_neg; - } - - if (t_pos == 0) - { - t_tk1 = CONST_IQ_1; - } - else - { - t_tk1 = CONST_IQ_1 - t_pos; - } - - switch (ki) - { - case 0: - tk0->up_or_down = updown_tk0; - tk0->Ti = t_tk0; - - tk1->up_or_down = updown_tk1; - tk1->Ti = t_tk1; - - break; - - case 1: - tk2->up_or_down = updown_tk0; - tk2->Ti = t_tk0; - - tk3->up_or_down = updown_tk1; - tk3->Ti = t_tk1; - - break; - - case 2: - tk4->up_or_down = updown_tk0; - tk4->Ti = t_tk0; - - tk5->up_or_down = updown_tk1; - tk5->Ti = t_tk1; - - break; - - default: break; - } - } -} - - - - -// #pragma CODE_SECTION(detect_region,".v_24pwm_run"); -int detect_region(_iq k, _iq teta) -{ - volatile _iq x,y; - volatile int reg=0; - - x = _IQmpy(k,_IQcos(teta)); - y = _IQmpy(k,_IQsin(teta)); - - if (y>=CONST_IQ_05) reg=4; - else if (y < (CONST_IQ_1 - _IQmpy(x,SQRT3))) reg = 1; - else if (y < (_IQmpy(x,SQRT3) - CONST_IQ_1)) reg = 2; - else reg = 3; - - return reg; -} - - - - -// #pragma CODE_SECTION(detect_sec,".v_24pwm_run"); -int detect_sec(_iq teta) -{ - volatile _iq sector; - volatile int sectorint; - - sector = _IQdiv(teta,CONST_IQ_PI3); - sectorint = (sector >> 24) + 1; - - if (sectorint > 6) sectorint-=6; - - return sectorint; -} - - -#define nSIN_t(k,t) _IQmpy(k,_IQsin(t)) - -#define nSIN_p3pt(k,t) _IQmpy(k,_IQsin(CONST_IQ_PI3+t)) - -#define nSIN_p3mt(k,t) _IQmpy(k,_IQsin(CONST_IQ_PI3-t)) - -#define nSIN_tmp3(k,t) _IQmpy(k,_IQsin(t-CONST_IQ_PI3)) - -//k - (Uzad) -//teta - -//region - -/* - * iq_tt0 - time of vectors op, oo, on - * iq_tt1 - time of vectors ap, an - * iq_tt2 - time of vectors bp, bn - * iq_tt3 - time of vector c - * iq_tt4 - time of vector a - * iq_tt5 - time of vector b - */ -// #pragma CODE_SECTION(calc_t_abc,".v_24pwm_run"); -void calc_t_abc(_iq k, _iq teta, int region, _iq *iq_tt0, _iq *iq_tt1, _iq *iq_tt2, _iq *iq_tt3, _iq *iq_tt4, _iq *iq_tt5) -{ - switch(region) - { - case 1 : - *iq_tt0 = CONST_IQ_05 - nSIN_p3pt(k,teta); - *iq_tt1 = nSIN_p3mt(k,teta); - *iq_tt2 = nSIN_t(k,teta); - *iq_tt3 = 0; - *iq_tt4 = 0; - *iq_tt5 = 0; - break; - - case 2 : - *iq_tt0 = 0; - *iq_tt1 = CONST_IQ_1 - nSIN_p3pt(k,teta); - *iq_tt2 = 0; - *iq_tt3 = nSIN_t(k,teta); - *iq_tt4 = nSIN_p3mt(k,teta) - CONST_IQ_05; - *iq_tt5 = 0; - break; - - case 3 : - *iq_tt0 = 0; - *iq_tt1 = CONST_IQ_05 - nSIN_t(k,teta); - *iq_tt2 = CONST_IQ_05 - nSIN_p3mt(k,teta); - *iq_tt3 = nSIN_p3pt(k,teta) - CONST_IQ_05; - *iq_tt4 = 0; - *iq_tt5 = 0; - break; - - case 4 : - *iq_tt0 = 0; - *iq_tt1 = 0; - *iq_tt2 = CONST_IQ_1 - nSIN_p3pt(k,teta); - *iq_tt3 = nSIN_p3mt(k,teta); - *iq_tt4 = 0; - *iq_tt5 = nSIN_t(k,teta) - CONST_IQ_05; - break; - - default : - *iq_tt0 = 0; - *iq_tt1 = 0; - *iq_tt2 = 0; - *iq_tt3 = 0; - *iq_tt4 = 0; - *iq_tt5 = 0; - break; - } - - return; -} - -//sector -//region -//iq_ttt0 - iq_ttt5 - times from calc_t_abs -//delta_t - -//number_sv - -//iqIa, iqIb, iqIc -// #pragma CODE_SECTION(calc_arr_tph, ".v_24pwm_run"); -void calc_arr_tph(int sector,int region, _iq iq_ttt0, _iq iq_ttt1, _iq iq_ttt2, _iq iq_ttt3, _iq iq_ttt4, - _iq iq_ttt5, _iq delta_t, unsigned int number_sv, - _iq iqIa, _iq iqIb, _iq iqIc) -{ - _iq iq_var1 = 0; - _iq iqIx, iqIy, iqIz; - _iq iq_alfa_1_p = CONST_IQ_05, iq_alfa_1_n = CONST_IQ_05, iq_alfa_2_n = CONST_IQ_05, iq_alfa_2_p = CONST_IQ_05; - _iq iq_alfa = 0; -// _iq iqIa, iqIb, iqIc; - _iq iq_mpy1 = 0; - _iq iq_mpy3 = 0; - _iq summ = 0; - - - switch (sector) - { - case 1: - iqIx = iqIc; - iqIy = iqIa; - iqIz = iqIb; - - break; - case 2: - - iqIx = iqIb; - iqIy = iqIa; - iqIz = iqIc; - - break; - case 3: - - iqIx = iqIb; - iqIy = iqIc; - iqIz = iqIa; - - break; - case 4: - - iqIx = iqIa; - iqIy = iqIc; - iqIz = iqIb; - - break; - case 5: - - iqIx = iqIa; - iqIy = iqIb; - iqIz = iqIc; - - break; - case 6: - - iqIx = iqIc; - iqIy = iqIb; - iqIz = iqIa; - - break; - default: - - iqIx = 0; - iqIy = 0; - iqIz = 0; - - break; - } - - if (region == 1) - { -// if (delta_t != 0) // // -// { -// iq_alfa = _IQsat((CONST_IQ_05 - _IQmpy(delta_t,IQ_KP_DELTA_T)),IQ_ALFA_SATURATION1,IQ_ALFA_SATURATION2); - - //if (delta_t < 0) - //{ - //iq_alfa = IQ_ALFA_SATURATION1; - //} - //else - //{ - //iq_alfa = IQ_ALFA_SATURATION2; - //} -// } -// else - { - iq_alfa = CONST_IQ_05; - } - } - else - { - iq_mpy1 = _IQmpy(_IQabs(iqIx),iq_ttt1)+_IQmpy(_IQabs(iqIy),iq_ttt2); - iq_mpy3 = _IQmpy(iqIz,iq_ttt3); - - summ = _IQdiv((iq_mpy3),(iq_mpy1)); - - //iq_alfa = _IQsat((_IQmpy(CONST_IQ_05,(CONST_IQ_1 + summ)) - _IQmpy(delta_t,IQ_KP_DELTA_T)),IQ_ALFA_SATURATION1,IQ_ALFA_SATURATION2); - iq_alfa = CONST_IQ_05; //test - } - - - if (iqIx >= 0) - { - iq_alfa_1_p = iq_alfa; - iq_alfa_1_n = CONST_IQ_1 - iq_alfa; - } - else - { - iq_alfa_1_p = CONST_IQ_1 - iq_alfa; - iq_alfa_1_n = iq_alfa; - } - - if (iqIy >= 0) - { - iq_alfa_2_p = CONST_IQ_1 - iq_alfa; - iq_alfa_2_n = iq_alfa; - } - else - { - iq_alfa_2_p = iq_alfa; - iq_alfa_2_n = CONST_IQ_1 - iq_alfa; - } - - - //if (number_sv == 2) - //{ - //logpar.log1 = (int16)(sector); - //logpar.log2 = (int16)(region); - //logpar.log3 = (int16)(_IQtoIQ15(iq_alfa)); - //logpar.log4 = (int16)(_IQtoIQ15(iq_alfa_1_p)); - //logpar.log5 = (int16)(_IQtoIQ15(iq_alfa_2_p)); - //logpar.log6 = (int16)(_IQtoIQ13(summ)); - //logpar.log3 = (int16)(_IQtoIQ14(iq_ttt0)); - //logpar.log4 = (int16)(_IQtoIQ14(iq_ttt1)); - //logpar.log5 = (int16)(_IQtoIQ14(iq_ttt2)); - //logpar.log6 = (int16)(_IQtoIQ14(iq_ttt3)); - //logpar.log7 = (int16)(_IQtoIQ14(iq_ttt4)); - //logpar.log8 = (int16)(_IQtoIQ14(iq_ttt5)); - //logpar.log10 = (int16)(_IQtoIQ15(delta_t1_struct.Up)); - //logpar.log11 = (int16)(_IQtoIQ15(delta_t1_struct.Ui)); - //logpar.log12 = (int16)(_IQtoIQ15(delta_t1_struct.Ud)); - //logpar.log13 = (int16)(_IQtoIQ15(iqIx)); - //logpar.log14 = (int16)(_IQtoIQ15(iqIy)); - //logpar.log15 = (int16)(_IQtoIQ15(iqIz)); - - //logpar.log12 = (int16)(_IQtoIQ15(_IQmpy(iq_alfa_2_p,iq_ttt2))); - //logpar.log13 = (int16)(_IQtoIQ15(_IQmpy(iq_alfa_2_n,iq_ttt2))); - //logpar.log14 = (int16)(_IQtoIQ15(delta_t)); - //} - //else - //logpar.log15 = (int16)(_IQtoIQ15(delta_t)); - -// if (region == 1) -// { -// if (f.Rele3 == 1) -// { -// iq_alfa_1_p = CONST_IQ_05; -// iq_alfa_2_p = CONST_IQ_05; -// iq_alfa_1_n = CONST_IQ_05; -// iq_alfa_2_n = CONST_IQ_05; -// } -// } -// else -// { -// if (f.Down50 == 1) -// { -// iq_alfa_1_p = CONST_IQ_05; -// iq_alfa_2_p = CONST_IQ_05; -// iq_alfa_1_n = CONST_IQ_05; -// iq_alfa_2_n = CONST_IQ_05; -// } -// } - - switch (region) - { - case 1: - iq_var1 = _IQdiv(iq_ttt0,CONST_IQ_3); - - ar_tph[0] = iq_var1; - ar_tph[1] = _IQmpy(iq_alfa_1_n,iq_ttt1); - ar_tph[2] = _IQmpy(iq_alfa_2_n,iq_ttt2); - ar_tph[3] = iq_var1; - ar_tph[4] = _IQmpy(iq_alfa_1_p,iq_ttt1); - ar_tph[5] = _IQmpy(iq_alfa_2_p,iq_ttt2); - ar_tph[6] = iq_var1; - break; - - case 2: - ar_tph[0] = _IQmpy(iq_alfa_1_n,iq_ttt1); - ar_tph[1] = iq_ttt4; - ar_tph[2] = iq_ttt3; - ar_tph[3] = _IQmpy(iq_alfa_1_p,iq_ttt1); - ar_tph[4] = 0; - ar_tph[5] = 0; - ar_tph[6] = 0; - break; - - case 3: - ar_tph[0] = _IQmpy(iq_alfa_1_n,iq_ttt1); - ar_tph[1] = _IQmpy(iq_alfa_2_n,iq_ttt2); - ar_tph[2] = iq_ttt3; - ar_tph[3] = _IQmpy(iq_alfa_1_p,iq_ttt1); - ar_tph[4] = _IQmpy(iq_alfa_2_p,iq_ttt2); - ar_tph[5] = 0; - ar_tph[6] = 0; - break; - - case 4: - ar_tph[0] = _IQmpy(iq_alfa_2_n,iq_ttt2); - ar_tph[1] = iq_ttt3; - ar_tph[2] = iq_ttt5; - ar_tph[3] = _IQmpy(iq_alfa_2_p,iq_ttt2); - ar_tph[4] = 0; - ar_tph[5] = 0; - ar_tph[6] = 0; - break; - - default : - break; - } - - -} - -/* - // Function is commented because of in project 222220 should not be large voltage diviation -// #pragma CODE_SECTION(calc_delta_t,".v_24pwm_run"); -_iq calc_delta_t(_iq delta_1, unsigned int number,int region) -{ - if(_IQabs(delta_1) > MAX_LEVEL_DELTA_T) - { - // ConvErrors.m2.bit.Razbalans |= 1; - return 0; - } - - if (number == 1) - { - delta_t1_struct.Fdb = delta_1; - delta_t1_struct.calc(&delta_t1_struct); - - if (_IQabs(delta_t1_struct.Out) <= INSENSITIVE_LEVEL_DELTA_T) return 0; - else return delta_t1_struct.Out; - } - else - { - delta_t2_struct.Fdb = delta_1; - delta_t2_struct.calc(&delta_t2_struct); - - if (_IQabs(delta_t2_struct.Out) <= INSENSITIVE_LEVEL_DELTA_T) return 0; - else return delta_t2_struct.Out; - } -} - -*/ -//#pragma CODE_SECTION(set_predel_dshim24,".fast_run2"); - -// #pragma CODE_SECTION(set_predel_dshim24_simple0,".v_24pwm_run"); -void set_predel_dshim24_simple0(SVGEN_PWM24_TIME *T,int16 dmin,int16 dmax) -{ - dmax++; // , , - if (T->Ti < dmin) - { - if (T->Ti < dmin/2) - T->Ti = 0; - else - T->Ti = dmin; - - } else if (T->Ti >= (dmax - dmin)) { - T->Ti = (dmax - dmin); - } -} -// #pragma CODE_SECTION(set_predel_dshim24_simple1,".v_24pwm_run"); -void set_predel_dshim24_simple1(SVGEN_PWM24_TIME *T,int16 dmin,int16 dmax) -{ - dmax++; // , , - if (T->Ti >= (dmax - dmin)) - { - if (T->Ti >= (dmax - dmin/2)) - T->Ti = dmax; - else - T->Ti = dmax-dmin; -// T->Ti = dmax; - } else if (T->Ti <= dmin) { - T->Ti = dmin; - } -} - - -// #pragma CODE_SECTION(set_predel_dshim24,".v_24pwm_run"); -void set_predel_dshim24(SVGEN_PWM24_TIME *T,int16 dmin,int16 dmax) -{ - //static unsigned int counter_pass = 0; - //static unsigned int summ = 0; - //int16 dshim24 = 0; - dmax++; // , , - if (T->Ti < dmin) - { - T->impuls_lenght_max = 0; - T->counter_pass_max = 0; - - T->impuls_lenght_min = T->impuls_lenght_min + T->Ti; - T->counter_pass_min++; - - if (T->counter_pass_min <= 3) - { - if (T->impuls_lenght_min <= dmin) - { - T->Ti = 0; - } - else - { -// T->Ti = dmin; //T->impuls_lenght_min; -// T->impuls_lenght_min -= dmin;// = 0; - T->Ti = T->impuls_lenght_min; - T->impuls_lenght_min = 0; - T->counter_pass_min = 0; -// if (T->impuls_lenght_min < 0) { -// T->counter_pass_min = 0; -// T->impuls_lenght_min = 0; -// } else { -// T->counter_pass_min -= 1; -// } - } - } - else - { - T->counter_pass_min = 1; - T->impuls_lenght_min = T->Ti; - T->Ti = 0; - } - } - else - { - T->impuls_lenght_min = 0; - T->counter_pass_min = 0; - -// if (T->Ti > (dmax - dmin)) -// { -// dshim = dmax; -// } -// else -// { -// dshim = T->Ti; -// } - - if (T->Ti >= (dmax - dmin)) - { - T->impuls_lenght_max = T->impuls_lenght_max + (dmax - T->Ti - 1); - T->counter_pass_max++; - - if (T->counter_pass_max <= 3) - { - if (T->impuls_lenght_max <= dmin) - { - T->Ti = dmax; - } - else - { -// T->Ti = dmax - dmin; //T->impuls_lenght_max; -// T->impuls_lenght_max -= dmin;// = 0; - T->Ti = dmax - T->impuls_lenght_max; - T->impuls_lenght_max = 0; - T->counter_pass_max = 0; -// if (T->impuls_lenght_max < 0) { -// T->impuls_lenght_max = 0; -// T->counter_pass_max = 0; -// } else { -// T->counter_pass_max -= 1; -// } - } - } - else - { - T->counter_pass_max = 1; - T->impuls_lenght_max = dmax - T->Ti; - T->Ti = dmax; - } - } - else - { - T->counter_pass_max = 0; - T->impuls_lenght_max = 0; - } - } - - //return dshim24; -} - - - -void init_alpha_pwm24(int xFreq) -{ - xFreq = xFreq + 1; - - svgen_pwm24_1.number_svgen = 1; - svgen_pwm24_2.number_svgen = 2; - - //pidCur_Kp = _IQ(PID_KP_DELTA_KOMP_I); - //pidCur_Ki = _IQ(PID_KI_DELTA_KOMP_I); - -// svgen_pwm24_1.prev_level = V_PWM24_PREV_PWM_CLOSE; - svgen_pwm24_1.saw_direct.all = xpwm_time.saw_direct.all & 0x3f; - svgen_pwm24_1.Tclosed_saw_direct_0 = xpwm_time.Tclosed_saw_direct_0;// xpwm_time.Tclosed_high;//var_freq_pwm_xtics + 1; - svgen_pwm24_1.Tclosed_saw_direct_1 = xpwm_time.Tclosed_saw_direct_1; - svgen_pwm24_1.Tclosed_high = xpwm_time.Tclosed_high; - -// svgen_pwm24_2.prev_level = V_PWM24_PREV_PWM_CLOSE; - svgen_pwm24_2.saw_direct.all = (xpwm_time.saw_direct.all >> 6) & 0x3f; - svgen_pwm24_2.Tclosed_saw_direct_0 = xpwm_time.Tclosed_saw_direct_0;// xpwm_time.Tclosed_high;//var_freq_pwm_xtics + 1; - svgen_pwm24_2.Tclosed_saw_direct_1 = xpwm_time.Tclosed_saw_direct_1; - svgen_pwm24_2.Tclosed_high = xpwm_time.Tclosed_high; - - svgen_pwm24_1.FreqMax = _IQ(2.0*PI*F_STATOR_MAX/FREQ_PWM/2);// 2, .. 2 - svgen_pwm24_2.FreqMax = _IQ(2.0*PI*F_STATOR_MAX/FREQ_PWM/2); - - svgen_pwm24_1.XilinxFreq = CONST_IQ_1/xFreq; - svgen_pwm24_2.XilinxFreq = CONST_IQ_1/xFreq; - - // 30 - svgen_pwm24_1.Alpha = 0; //winding_displacement; - svgen_pwm24_2.Alpha = -winding_displacement; - - svgen_pwm24_1.delta_t = 0; - svgen_pwm24_2.delta_t = 0; -} - -/* -void init_freq_array(void) -{ - unsigned int i = 0; - //unsigned int j = 0; - int var1 = 0; - - var1 = 32767 / (FREQ_PWM_MAX - FREQ_PWM_MIN); - - for (i = 0; i < COUNT_VAR_FREQ; i++) - { - //j = rand() / 1023; - //freq_array[i] = array_optim_freq[j]; - //do - freq_array[i] = FREQ_PWM_MIN + (rand() / var1); - //while ((freq_array[i] < 945) && (freq_array[i] > 930)); - } - - //freq_array[0] = 991; - //freq_array[1] = 1430; -} -*/ - - -//#pragma CODE_SECTION(calc_freq_pwm,".v_24pwm_run"); -//#pragma CODE_SECTION(calc_freq_pwm,".fast_run"); -/*void calc_freq_pwm() -{ - static int prev_freq_pwm = 0; - static float pwm_period = 0; - static float var0 = 0; - //static int line = 0; - //static int i = 0; - static unsigned int proc_ticks = 1; - int var1 = 0; - //static int i = 0; - - if ((f.flag_change_pwm_freq == 1) && (f.flag_random_freq == 1)) - { - if (proc_ticks >= 1) - { - proc_ticks = 0; - - - if (line == 0) - { - VAR_FREQ_PWM_HZ = VAR_FREQ_PWM_HZ + 1; - if (VAR_FREQ_PWM_HZ > FREQ_PWM_MAX) - { - VAR_FREQ_PWM_HZ = FREQ_PWM_MAX; - line = 1; - } - } - else - { - VAR_FREQ_PWM_HZ = VAR_FREQ_PWM_HZ - 1; - if (VAR_FREQ_PWM_HZ < FREQ_PWM) - { - VAR_FREQ_PWM_HZ = FREQ_PWM; - line = 0; - } - } - - - - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - //VAR_FREQ_PWM_HZ = freq_array[i]; - //i_led2_on_off(1); - - var1 = 32767 / (freq_pwm_max_hz - freq_pwm_min_hz); - VAR_FREQ_PWM_HZ = freq_pwm_min_hz + (rand() / var1); - - //i_led2_on_off(0); - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - if (VAR_FREQ_PWM_HZ > freq_pwm_max_hz) - { - VAR_FREQ_PWM_HZ = freq_pwm_max_hz; - } - else - { - if (VAR_FREQ_PWM_HZ < freq_pwm_min_hz) - { - VAR_FREQ_PWM_HZ = freq_pwm_min_hz; - } - } - //i++; - - //if (i >= COUNT_VAR_FREQ) - //{ - //i = 0; - //} - - } - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - //if (VAR_FREQ_PWM_HZ == FREQ_PWM_MIN) - //{ - //VAR_FREQ_PWM_HZ = FREQ_PWM_MAX; - //} - //else - //{ - //VAR_FREQ_PWM_HZ = FREQ_PWM_MIN; - //} - - //if (f.Rele1 == 1) - //{ - //if (i == 0) - //{ - //VAR_FREQ_PWM_HZ = 1192;; - //i = 1; - //} - //else - //{ - //VAR_FREQ_PWM_HZ = 792; - //} - //} - //else - //{ - //i = 0; - //VAR_FREQ_PWM_HZ = 1192; - //} - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - } - //else - //{ - //VAR_FREQ_PWM_HZ = FREQ_PWM; - //} - - - if (prev_freq_pwm != VAR_FREQ_PWM_HZ) - { - prev_freq_pwm = VAR_FREQ_PWM_HZ; - FREQ_MAX = _IQ(2.0*PI*F_STATOR_MAX/VAR_FREQ_PWM_HZ); - - var0 = (float)VAR_FREQ_PWM_HZ; - //pwm_period = ((float64)HSPCLK) / ((float64)VAR_FREQ_PWM_HZ); - - pwm_period = HSPCLK / var0; - - pwm_period = pwm_period / 2.0; - - FREQ_PWM_XTICS = ((int) pwm_period) >> 3; - - XILINX_FREQ = 16777216/(FREQ_PWM_XTICS + 1); - - FLAG_CHANGE_FREQ_PWM = 1; - } - - proc_ticks++; -} -*/ - -void change_freq_pwm(_iq xFreq) { - svgen_pwm24_1.FreqMax = _IQ(2.0*PI*F_STATOR_MAX/3750000 / xFreq / 2 /2);// 2, .. 2 - svgen_pwm24_2.FreqMax = _IQ(2.0*PI*F_STATOR_MAX/ 3750000 / xFreq / 2 /2); - - xFreq += 1; - svgen_pwm24_1.XilinxFreq = CONST_IQ_1/xFreq; - svgen_pwm24_2.XilinxFreq = CONST_IQ_1/xFreq; -} - -// #pragma CODE_SECTION(test_calc_pwm24_dq,".v_24pwm_run"); -void test_calc_pwm24_dq(_iq U_zad1, _iq U_zad2,_iq teta) -{ - svgen_pwm24_1.Freq = 0; - svgen_pwm24_2.Freq = 0; - - svgen_pwm24_1.Gain = U_zad1; - svgen_pwm24_2.Gain = U_zad2; - - svgen_pwm24_1.Alpha = teta; - svgen_pwm24_2.Alpha = teta - winding_displacement; - - svgen_pwm24_1.delta_U = filter.iqU_1_fast - filter.iqU_2_fast; - svgen_pwm24_2.delta_U = filter.iqU_3_fast - filter.iqU_4_fast; - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - svgen_pwm24_1.delta_U = 0; - svgen_pwm24_2.delta_U = 0; - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - svgen_pwm24_1.Ia = analog.iqIa1_1; - svgen_pwm24_1.Ib = analog.iqIb1_1; - svgen_pwm24_1.Ic = analog.iqIc1_1; - - svgen_pwm24_2.Ia = analog.iqIa2_1; - svgen_pwm24_2.Ib = analog.iqIb2_1; - svgen_pwm24_2.Ic = analog.iqIc2_1; - - svgen_pwm24_1.calc_dq(&svgen_pwm24_1); - svgen_pwm24_2.calc_dq(&svgen_pwm24_2); - - - // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - set_predel_dshim24(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - set_predel_dshim24(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - set_predel_dshim24(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS); - - -} - - -// #pragma CODE_SECTION(svgen_pwm24_calc,".v_24pwm_run"); -void svgen_pwm24_calc_dq(SVGEN_PWM24 *vt) -{ - - if (vt->Alpha > CONST_IQ_2PI) - { - vt->Alpha -= CONST_IQ_2PI; - } - - if (vt->Alpha < 0) - { - vt->Alpha += CONST_IQ_2PI; - } - - - calc_time_one_tk(vt->Gain, vt->Alpha, vt->delta_U, vt->Ia, vt->Ib, vt->Ic, - vt->number_svgen, &vt->Ta_0, &vt->Ta_1,&vt->Tb_0, &vt->Tb_1,&vt->Tc_0, &vt->Tc_1); - - - vt->Ta_0.Ti = vt->Ta_0.Ti/vt->XilinxFreq; - vt->Ta_1.Ti = vt->Ta_1.Ti/vt->XilinxFreq; - - vt->Tb_0.Ti = vt->Tb_0.Ti/vt->XilinxFreq; - vt->Tb_1.Ti = vt->Tb_1.Ti/vt->XilinxFreq; - - vt->Tc_0.Ti = vt->Tc_0.Ti/vt->XilinxFreq; - vt->Tc_1.Ti = vt->Tc_1.Ti/vt->XilinxFreq; - -} - -void svgen_set_time_keys_closed(SVGEN_PWM24 *vt) -{ - vt->Ta_0.Ti = VAR_FREQ_PWM_XTICS + 1; - vt->Ta_1.Ti = 0; - - vt->Tb_0.Ti = VAR_FREQ_PWM_XTICS + 1; - vt->Tb_1.Ti = 0; - - vt->Tc_0.Ti = VAR_FREQ_PWM_XTICS + 1; - vt->Tc_1.Ti = 0; -} - -void svgen_set_time_middle_keys_open(SVGEN_PWM24 *vt) -{ - vt->Ta_0.Ti = 0; - vt->Ta_1.Ti = VAR_FREQ_PWM_XTICS + 1; - - vt->Tb_0.Ti = 0; - vt->Tb_1.Ti = VAR_FREQ_PWM_XTICS + 1; - - vt->Tc_0.Ti = 0; - vt->Tc_1.Ti = VAR_FREQ_PWM_XTICS + 1; -} diff --git a/Inu/Src2/main/v_pwm24.h b/Inu/Src2/main/v_pwm24.h deleted file mode 100644 index ff83ca4..0000000 --- a/Inu/Src2/main/v_pwm24.h +++ /dev/null @@ -1,190 +0,0 @@ -#ifndef _V_PWM24_H -#define _V_PWM24_H - -#ifdef __cplusplus - extern "C" { -#endif - -#include "IQmathLib.h" -// #include "DSP281x_Device.h" -#include "word_structurs.h" -#include "svgen_dq.h" - -//#define COUNT_VAR_FREQ 400 - -//#define IQ_KP_DELTA_T 134217728//0.004 -//#define IQ_KP_DELTA_COMP_I 134217728//0.004 - -//#define PID_KP_DELTA_T 0.5//1//20//2//0.001//11.18 //0.036 //0.018 //0.18 //0.095 // PID Kp -//#define PID_KI_DELTA_T 0.000005//0.01 //0.08 // 0.008 // PID Ki -//#define PID_KD_DELTA_T 2//5//0.01 //0.08 // 0.008 // PID Ki -//#define PID_KC_DELTA_T 0.005 //0.09 // PID Kc - -//#define PID_KP_DELTA_KOMP_I 1//0.12//0.06//5//10 -//#define PID_KI_DELTA_KOMP_I 0.005//0//0.005//0.01 -//#define PID_KC_DELTA_KOMP_I 0.005//0//0.005//0.01 -//#define PID_KD_DELTA_KOMP_I 0//0//0.005//0.01 -//#define PID_KD_DELTA_T 0.0000 //*100 // PID Kd - - - -//#define DELTA_T_MAX 1258291//15099494//0.9//8388608//0.5//13421772//0.8 //8388608// 0.5//13421772 // 0.8 -//#define DELTA_T_MIN -1258291//-15099494//0.9//-8388608//-0.5//-13421772//0.8 //-8388608// -0.5//-13421772 // -0.8 - -//#define DELTA_KOMP_I_MAX 1258291//6//1677721//0.1//3355443//0.2//1677721//200 A//4194304// 500 A -//#define DELTA_KOMP_I_MIN -1258291//-6//-1677721//-0.1//-3355443//-0.2//-1677721//-200 A//-4194304// -500 A - - -//#define INSENSITIVE_LEVEL_DELTA_T 83886 //10 V//167772// 20V //335544//40 V//83886 //10 V//335544//40 V//58720//7V//167772// 20V //83886 //10 V -//#define MAX_LEVEL_DELTA_T 1258291//150V//1677721 //200v//2516582//300 V//4194304//500 V//2516582 // 838860 //100 V - - -typedef struct { _iq Ti; // Output: reference phase-a switching function (pu) - int up_or_down; // Output: reference phase-b switching function (pu) - int impuls_lenght_max; - int impuls_lenght_min; - int counter_pass_max; - int counter_pass_min; - } SVGEN_PWM24_TIME; - - -typedef struct { - _iq Gain; // Input: reference gain voltage (pu) - //_iq Offset; // Input: reference offset voltage (pu) - _iq Freq; // Input: reference frequency (pu) - _iq FreqMax; // Parameter: Maximum step angle = 6*base_freq*T (pu) - _iq Alpha; // History: Sector angle (pu) - - _iq delta_U; - _iq delta_t; - int XilinxFreq; // Xilinx freq in TIC - - unsigned int pwm_minimal_impuls_zero_minus; - unsigned int pwm_minimal_impuls_zero_plus; - - WORD_UINT2BITS_STRUCT saw_direct; - - int prev_level; // , middle close - unsigned int Tclosed_high; - unsigned int Tclosed_saw_direct_0; - unsigned int Tclosed_saw_direct_1; - - _iq Ia; - _iq Ib; - _iq Ic; - unsigned int number_svgen; - SVGEN_PWM24_TIME Ta_0; // Output: reference phase-a switching function (pu) - SVGEN_PWM24_TIME Ta_1; // Output: reference phase-a switching function (pu) - SVGEN_PWM24_TIME Tb_0; // Output: reference phase-b switching function (pu) - SVGEN_PWM24_TIME Tb_1; // Output: reference phase-b switching function (pu) - SVGEN_PWM24_TIME Tc_0; // Output: reference phase-c switching function (pu) - SVGEN_PWM24_TIME Tc_1; // Output: reference phase-c switching function (pu) - void (*calc)(); // Pointer to calculation function - void (*calc_dq)(); // Pointer to calculation function which don`t calculate angle from freq -} SVGEN_PWM24; - -typedef SVGEN_PWM24 *SVGEN_PWM24_handle; - - -#define SVGEN_PWM24_TIME_DEFAULTS { 0,0,0,0 } - - -#define SVGEN_PWM24_DEFAULTS { 0,0,0,0,0,0,0,0,0, \ - {0}, 0,0,0,0,0,0,0,0,\ - SVGEN_PWM24_TIME_DEFAULTS,SVGEN_PWM24_TIME_DEFAULTS,SVGEN_PWM24_TIME_DEFAULTS, \ - SVGEN_PWM24_TIME_DEFAULTS,SVGEN_PWM24_TIME_DEFAULTS,SVGEN_PWM24_TIME_DEFAULTS, \ - (void (*)(unsigned int))svgen_pwm24_calc, (void (*)(unsigned int))svgen_pwm24_calc_dq } - -// extern int ar_sa_a[3][4][7]; - -extern SVGEN_PWM24 svgen_pwm24_1; -extern SVGEN_PWM24 svgen_pwm24_2; - -extern SVGENDQ svgen_dq_1; -extern SVGENDQ svgen_dq_2; - -// extern _iq pidCur_Kp; -// extern _iq pidCur_Ki; - -// extern _iq iq_alfa_coef; - -// extern _iq iq_koef_mod_korrect_1; -// extern _iq iq_koef_mod_korrect_2; - -//extern int freq_array[COUNT_VAR_FREQ]; - -void svgen_pwm24_calc(SVGEN_PWM24 *vt); -void svgen_pwm24_calc_dq(SVGEN_PWM24 *vt); - -void init_alpha_pwm24(int xFreq); -void test_calc_pwm24(_iq uz1, _iq uz2, _iq fz1/*, _iq fz2, int revers*/); -void calc_arr_tph(int sector,int region, _iq iq_ttt0, _iq iq_ttt1, - _iq iq_ttt2, _iq iq_ttt3, _iq iq_ttt4, _iq iq_ttt5, _iq delta_t, unsigned int number_sv, - _iq iqIa, _iq iqIb, _iq iqIc); -_iq calc_delta_t(_iq delta_1, unsigned int number,int region); - -//void change_freq_pwm(_iq FreqMax, int freq_pwm_xtics, _iq XilinxFreq); -void change_freq_pwm(_iq freq_pwm_xtics); - -//void calc_freq_pwm(); - -void calc_time_one_tk(_iq gain, _iq teta, _iq delta_U, - _iq Ia, _iq Ib, _iq Ic, - unsigned int number, - SVGEN_PWM24_TIME *tk0, - SVGEN_PWM24_TIME *tk1, - SVGEN_PWM24_TIME *tk2, - SVGEN_PWM24_TIME *tk3, - SVGEN_PWM24_TIME *tk4, - SVGEN_PWM24_TIME *tk5); - -void test_calc_pwm24_dq(_iq U_zad1, _iq U_zad2,_iq teta); - -void svgen_set_time_keys_closed(SVGEN_PWM24 *vt); -void svgen_set_time_middle_keys_open(SVGEN_PWM24 *vt); - - - -void InitVariablesSvgen(unsigned int freq); -//void init_alpha(void); -_iq correct_balance_uzpt_pwm24(_iq Tinput, _iq Kplus); -void recalc_time_pwm_minimal_2_xilinx_pwm24_l(SVGEN_PWM24 *pwm24, - _iq *T0, _iq *T1, - _iq timpuls_corr ); -void test_calc_simple_dq_pwm24(_iq uz1, _iq uz2, _iq fz1, _iq fz2, _iq Uzad_max); -void test_calc_dq_pwm24(_iq Ud, _iq Uq, _iq Ud2, _iq Uq2, _iq tetta,_iq Uzad_max); -//void test_calc_simple_uf_pwm24(_iq uz1, _iq uz2, _iq fz1, _iq fz2,_iq Uzad_max); - -//void init_freq_array(void); - -typedef union { - unsigned int all; - struct { - unsigned int k0; - unsigned int k1; - unsigned int k2; - unsigned int k3; - unsigned int k4; - unsigned int k5; - unsigned int k6; - unsigned int k7; - unsigned int k8; - unsigned int k9; - unsigned int k10; - unsigned int k11; - unsigned int k12; - unsigned int k13; - unsigned int k14; - unsigned int k15; - }bit; -} UP_OR_DOWN; - -extern UP_OR_DOWN up_down; -extern _iq winding_displacement; - - -#ifdef __cplusplus - } -#endif - -#endif /* _V_PWM24_H */ diff --git a/Inu/Src2/main/vector.h b/Inu/Src2/main/vector.h deleted file mode 100644 index 5245af8..0000000 --- a/Inu/Src2/main/vector.h +++ /dev/null @@ -1,256 +0,0 @@ -/* - ???? ??? (?) 2006 ?. - - Processor: TMS320C32 - - Filename: vector_troll.h - - ??????? ?????????? ?????????y - - Edit date: 04-12-02 - - Function: - - Revisions: -*/ - - -#ifndef _VECTOR_SEV -#define _VECTOR_SEV - - -#ifdef __cplusplus - extern "C" { -#endif - - -#include "IQmathLib.h" - - -typedef struct -{ - float W; /* */ - float Angle; /* */ - float Phi; /* */ - float k; /* . */ - float k1; /* . */ - float k2; /* . */ - float f; /* */ - - _iq iqk; - _iq iqk1; - _iq iqk2; - _iq iqf; - - - -} WINDING; - - - - -typedef struct -{ - unsigned int Prepare; - unsigned int terminal_prepare; - unsigned int prepareSVU; - unsigned int Test_Lamps; - unsigned int fault; - - unsigned int prevGo; - unsigned int Go; - unsigned int Stop; - unsigned int Mode; - unsigned int Revers; - unsigned int Is_Blocked; - - unsigned int Ready1; - unsigned int Ready2; - unsigned int Discharge; - unsigned int is_charging; - - unsigned int ErrorChannel1; - unsigned int ErrorChannel2; - unsigned int FaultChannel1; - unsigned int FaultChannel2; - - unsigned int secondPChState; - - unsigned int Set_power; - - unsigned int Impuls; - - unsigned int Obmotka1; - unsigned int Obmotka2; -// unsigned int Down50; - - unsigned int Power_over_Nominal; - unsigned int I_over_nominal; //????????? ?????? ?????? ? ??????????? ???????????? ???? - unsigned int I_over_1_6_nominal; //????????? ?????? ?????? ? ??????????? ???????????? ???? - unsigned int I_over_1_8_nominal; //????????? ?????? ?????? ? ??????????? ???????????? ???? - unsigned int Moment_over_1_6_noninal; //????????? ?????? ?????? ? ??????????? ???????????? ??????? ? 1.6 ??? - unsigned int Moment_over_1_8_nominal; //????????? ?????? ?????? ? ??????????? ???????????? ??????? ? 1.8 ??? - unsigned int DownToNominal; - unsigned int DownToNominalCurrent; - unsigned int DownToNominalMoment; - unsigned int DownTemperature; - unsigned int DownToNominalVoltage; - unsigned int DownToNominalFreq; - unsigned int nominal_I_exceeded_counter; //??????? ??? ????????? ??????????? ??? - unsigned int nominal_M_exceeded_counter; //??????? ??? ????????? ??????????? ?????? -// unsigned int I_zpt_over_nominal; - - unsigned int Up50; - unsigned int Ciclelog; - unsigned int pidD_set; - unsigned int Bpsi; - unsigned int Piregul1; - unsigned int Piregul2; - unsigned int Startstoplog; - unsigned int Setspeed; - - unsigned int BWC_turn_ON; - unsigned int BWC_Auto_mode; - - unsigned int Setsdvigfaza; - unsigned int Off_piregul; - - unsigned int Restart; - unsigned int stop_Log; - - unsigned int Work_net; - unsigned int Mask_impuls; - unsigned int Impuls_width; - - - unsigned int Work; - - unsigned int Auto; - - unsigned int Uzad; - unsigned int Umin; - - unsigned int RScount; - unsigned int vector_run; - unsigned int test_all_run; - - unsigned int decr_mzz_temp; -// unsigned int flag_decr_mzz_temp; - - unsigned int flag_Break_Resistor_Error; - unsigned int flag_local_control; //1 - local - unsigned int flag_leading; // - unsigned int flag_second_leading; // - unsigned int read_task_from_optical_bus; - unsigned int sync_rotor_from_optical_bus; - unsigned int sync_Iq_from_optical_bus; - unsigned int flag_distance; - unsigned int flag_second_PCH; - unsigned int leftShaft; - unsigned int inverter_number; - unsigned int ice_movement_limit; - unsigned int flag_batery_charged; - unsigned int flag_Pump_Is_On; //Gidropodpor - unsigned int flag_turn_On_Pump; - unsigned int flag_UMP_blocked; - unsigned int power_units_doors_closed; - unsigned int power_units_doors_locked; - - unsigned int flag_decr_mzz_power; - - unsigned int rotor_stopped; - - float decr_mzz_power; - _iq iq_decr_mzz_power; - - _iq iq_decr_mzz_voltage; - - float fzad; - float kzad; - float kzad_plus; - float fzad_provorot; - float Sdvigfaza; - - float mzz_zad; - float fr_zad; - float Power; - float p_zad; - - -// _iq iq_bpsi_zad; - _iq iq_mzz_zad; - _iq iq_fzad_provorot; - _iq iq_fzad; - _iq iq_p_zad; - _iq iq_p_rampa; - _iq iq_p_zad_electric; - _iq iq_p_limit_zad; - int p_limit_zad; // - - unsigned int flag_Enable_Prepare; - - union { - unsigned int all; - struct { - unsigned int BV1: 1; - unsigned int BV2: 1; - unsigned int BI1: 1; - unsigned int BI2: 1; - unsigned int UMU: 1; - unsigned int UKSI: 1; - unsigned int reserved: 10; - } UKSS; - } status_ready; - - unsigned int On_Power_QTV; - unsigned int Power_QTV_is_On; - - unsigned int RS_MPU_ERROR; - unsigned int MPU_Ready; - - unsigned int flag_tormog; - - int special_test_from_mpu; - - int MessageToCan1; - int MessageToCan2; - int flag_change_pwm_freq; - int flag_random_freq; - long tmp; - - unsigned int rele1; - - _iq cosinusTerminal; - _iq cosinusTerminalSquared; -// _iq cosinusFiOut; - - int setCosTerminal; - int setTettaKt; - - //Sync vals - int pwm_freq_plus_minus_zero; - int disable_sync; - int sync_ready; - int flag_sync_vipr1_vipr2; - int level_find_sync_zero; - int delta_error_sync; - int delta_capnum; - int count_error_sync; - int capnum0; - int PWMcounterVal; - - int build_version; - -} FLAG; - - -extern FLAG f; -extern WINDING a; - -#ifdef __cplusplus - } -#endif - -#endif /* _VECTOR_SEV */ - - diff --git a/Inu/Src2/main/word_structurs.h b/Inu/Src2/main/word_structurs.h deleted file mode 100644 index d94d243..0000000 --- a/Inu/Src2/main/word_structurs.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * word_structurs.h - * - * Created on: 5 . 2020 . - * Author: Yura - */ - -#ifndef SRC_MYLIBS_WORD_STRUCTURS_H_ -#define SRC_MYLIBS_WORD_STRUCTURS_H_ - -//////////////////////////////////////////////////////////////////////////////// -typedef union -{ - struct - { - unsigned int bit0: 1; - unsigned int bit1: 1; - unsigned int bit2: 1; - unsigned int bit3: 1; - unsigned int bit4: 1; - unsigned int bit5: 1; - unsigned int bit6: 1; - unsigned int bit7: 1; - unsigned int bit8: 1; - unsigned int bit9: 1; - unsigned int bit10: 1; - unsigned int bit11: 1; - unsigned int bit12: 1; - unsigned int bit13: 1; - unsigned int bit14: 1; - unsigned int bit15: 1; - } bits; // - int all; // -} WORD_INT2BITS_STRUCT; // -////// -//////////////////////////////////////////////////////////////////////////////// -typedef union -{ - struct - { - unsigned int bit0: 1; - unsigned int bit1: 1; - unsigned int bit2: 1; - unsigned int bit3: 1; - unsigned int bit4: 1; - unsigned int bit5: 1; - unsigned int bit6: 1; - unsigned int bit7: 1; - unsigned int bit8: 1; - unsigned int bit9: 1; - unsigned int bit10: 1; - unsigned int bit11: 1; - unsigned int bit12: 1; - unsigned int bit13: 1; - unsigned int bit14: 1; - unsigned int bit15: 1; - } bits; // - unsigned int all; // -} WORD_UINT2BITS_STRUCT; // -////// - - - -#endif /* SRC_MYLIBS_WORD_STRUCTURS_H_ */ diff --git a/Inu/Src2/main/xp_write_xpwm_time.c b/Inu/Src2/main/xp_write_xpwm_time.c deleted file mode 100644 index 093b88c..0000000 --- a/Inu/Src2/main/xp_write_xpwm_time.c +++ /dev/null @@ -1,361 +0,0 @@ -/* - * xp_write_xpwm_time.c - * - * Created on: 03 àïð. 2018 ã. - * Author: stud - */ - -#include "xp_write_xpwm_time.h" -// #include "MemoryFunctions.h" -// #include "Spartan2E_Adr.h" -// #include "PWMTMSHandle.h" - -#include "def.h" - - -// #pragma DATA_SECTION(xpwm_time,".fast_vars1"); -XPWM_TIME xpwm_time = DEFAULT_XPWM_TIME; - -#define set_default_tclosed(k,b) {if (b) k = p->Tclosed_saw_direct_1; else k = p->Tclosed_saw_direct_0;} - -void initXpwmTimeStructure(XPWM_TIME *p) { - p->Ta0_0 = p->Tclosed_0; - p->Ta0_1 = p->Tclosed_1; - p->Tb0_0 = p->Tclosed_0; - p->Tb0_1 = p->Tclosed_1; - p->Tc0_0 = p->Tclosed_0; - p->Tc0_1 = p->Tclosed_1; - - p->Ta1_0 = p->Tclosed_0; - p->Ta1_1 = p->Tclosed_1; - p->Tb1_0 = p->Tclosed_0; - p->Tb1_1 = p->Tclosed_1; - p->Tc1_0 = p->Tclosed_0; - p->Tc1_1 = p->Tclosed_1; - - p->Tbr0_0 = 0; - p->Tbr0_1 = 0; - p->Tbr1_0 = 0; - p->Tbr1_1 = 0; -} - - -// -void xpwm_write_1_2_winding_break_times_16_lines(XPWM_TIME *p) { - -} - -// void xpwm_write_1_2_winding_break_times_16_lines_split_eages(XPWM_TIME *p) { - -// } -/* -//#pragma CODE_SECTION(xpwm_write_1_2_winding_break_times_16_lines,".fast_run1"); -void xpwm_write_1_2_winding_break_times_16_lines(XPWM_TIME *p) -{ - if(!(ReadMemory(ADR_ERRORS_TOTAL_INFO))) - { - WriteMemory(ADR_PWM_KEY_NUMBER, 0); - WriteMemory(ADR_PWM_TIMING, p->Ta0_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 1); - WriteMemory(ADR_PWM_TIMING, p->Ta0_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 2); - WriteMemory(ADR_PWM_TIMING, p->Tb0_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 3); - WriteMemory(ADR_PWM_TIMING, p->Tb0_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 4); - WriteMemory(ADR_PWM_TIMING, p->Tc0_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 5); - WriteMemory(ADR_PWM_TIMING, p->Tc0_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 6); - WriteMemory(ADR_PWM_TIMING, p->Ta1_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 7); - WriteMemory(ADR_PWM_TIMING, p->Ta1_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 8); - WriteMemory(ADR_PWM_TIMING, p->Tb1_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 9); - WriteMemory(ADR_PWM_TIMING, p->Tb1_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 10); - WriteMemory(ADR_PWM_TIMING, p->Tc1_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 11); - WriteMemory(ADR_PWM_TIMING, p->Tc1_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 12); - WriteMemory(ADR_PWM_TIMING, p->Tbr0_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 13); - WriteMemory(ADR_PWM_TIMING, p->Tbr0_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 14); - WriteMemory(ADR_PWM_TIMING, p->Tbr1_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 15); - WriteMemory(ADR_PWM_TIMING, p->Tbr1_1); - } - else - { - WriteMemory(ADR_PWM_KEY_NUMBER, 0); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 1); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 2); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 3); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 4); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 5); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 6); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 7); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 8); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 9); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 10); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_0); - WriteMemory(ADR_PWM_KEY_NUMBER, 11); - WriteMemory(ADR_PWM_TIMING, p->Tclosed_1); - WriteMemory(ADR_PWM_KEY_NUMBER, 12); - WriteMemory(ADR_PWM_TIMING, 0); - WriteMemory(ADR_PWM_KEY_NUMBER, 13); - WriteMemory(ADR_PWM_TIMING, 0); - WriteMemory(ADR_PWM_KEY_NUMBER, 14); - WriteMemory(ADR_PWM_TIMING, 0); - WriteMemory(ADR_PWM_KEY_NUMBER, 15); - WriteMemory(ADR_PWM_TIMING, 0); - } -} -*/ -// #pragma CODE_SECTION(xpwm_write_1_2_winding_break_times_16_lines_split_eages,".fast_run1"); -void xpwm_write_1_2_winding_break_times_16_lines_split_eages(XPWM_TIME *p) -{ - // if (!(i_ReadMemory(ADR_ERRORS_TOTAL_INFO))) - { -//a - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit0==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A1_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Ta0_0); - EPwm2Regs.CMPA.half.CMPA = p->Ta0_0; - } - - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit1 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit1==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A1_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Ta0_1); - EPwm1Regs.CMPA.half.CMPA = p->Ta0_1; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit2 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit2==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B1_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tb0_0); - EPwm4Regs.CMPA.half.CMPA = p->Tb0_0; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit3 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit3==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B1_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tb0_1); - EPwm3Regs.CMPA.half.CMPA = p->Tb0_1; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit4 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit4==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C1_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tc0_0); - EPwm6Regs.CMPA.half.CMPA = p->Tc0_0; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit5 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit5==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C1_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tc0_1); - EPwm5Regs.CMPA.half.CMPA = p->Tc0_1; - } -//b - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit6 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit6==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A2_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Ta1_0); - EPwm8Regs.CMPA.half.CMPA = p->Ta1_0; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit7 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit7==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A2_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Ta1_1); - EPwm7Regs.CMPA.half.CMPA = p->Ta1_1; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit8 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit8==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B2_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tb1_0); - EPwm10Regs.CMPA.half.CMPA = p->Tb1_0; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit9 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit9==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B2_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tb1_1); - EPwm9Regs.CMPA.half.CMPA = p->Tb1_1; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit10 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit10==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C2_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tc1_0); - EPwm12Regs.CMPA.half.CMPA = p->Tc1_0; - } - if ((p->mode_reload==PWM_MODE_RELOAD_FORCE) - || (p->saw_direct.bits.bit11 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_HIGH) - || (p->saw_direct.bits.bit11==0 && p->mode_reload==PWM_MODE_RELOAD_LEVEL_LOW) ) - { - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C2_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tc1_1); - EPwm11Regs.CMPA.half.CMPA = p->Tc1_1; - } - -//br1 br2 - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR1_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tbr0_0); - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR1_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tbr0_1); - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR2_PLUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tbr1_0); - // i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR2_MINUS); - // i_WriteMemory(ADR_PWM_TIMING, p->Tbr1_1); - } -// else -// { -// // hard_stop_x24_pwm_all(); -// // stop_pwm(); -// xpwm_write_zero_winding_break_times_16_lines_split_eages(p); -// } -} -/* -// #pragma CODE_SECTION(xpwm_write_zero_1,".fast_run2"); -void xpwm_write_zero_1(XPWM_TIME *p) -{ - unsigned int tclose; - - //a - set_default_tclosed(tclose, p->saw_direct.bits.bit0); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A1_PLUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Ta0_0 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit1); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A1_MINUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Ta0_1 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit2); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B1_PLUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tb0_0 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit3); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B1_MINUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tb0_1 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit4); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C1_PLUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tc0_0 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit5); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C1_MINUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tc0_1 = tclose; - -} - -// #pragma CODE_SECTION(xpwm_write_zero_2,".fast_run1"); -void xpwm_write_zero_2(XPWM_TIME *p) -{ - unsigned int tclose; - -//b - set_default_tclosed(tclose, p->saw_direct.bits.bit6); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A2_PLUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Ta1_0 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit7); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_A2_MINUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Ta1_1 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit8); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B2_PLUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tb1_0 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit9); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_B2_MINUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tb1_1 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit10); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C2_PLUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tc1_0 = tclose; - - set_default_tclosed(tclose, p->saw_direct.bits.bit11); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_C2_MINUS); - i_WriteMemory(ADR_PWM_TIMING, tclose); - p->Tc1_1 = tclose; - -} - -// #pragma CODE_SECTION(xpwm_write_zero_break_1,".fast_run2"); -void xpwm_write_zero_break_1(XPWM_TIME *p) -{ - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR1_PLUS); - i_WriteMemory(ADR_PWM_TIMING, 0); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR1_MINUS); - i_WriteMemory(ADR_PWM_TIMING, 0); - - p->Tbr0_0 = 0; - p->Tbr0_1 = 0; - -} - -// #pragma CODE_SECTION(xpwm_write_zero_break_2,".fast_run2"); -void xpwm_write_zero_break_2(XPWM_TIME *p) -{ - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR2_PLUS); - i_WriteMemory(ADR_PWM_TIMING, 0); - i_WriteMemory(ADR_PWM_KEY_NUMBER, PWM_KEY_NUMBER_BR2_MINUS); - i_WriteMemory(ADR_PWM_TIMING, 0); - - p->Tbr1_0 = 0; - p->Tbr1_1 = 0; -} - -// #pragma CODE_SECTION(xpwm_write_zero_winding_break_times_16_lines_split_eages,".fast_run2"); -void xpwm_write_zero_winding_break_times_16_lines_split_eages(XPWM_TIME *p) -{ - xpwm_write_zero_1(p); - xpwm_write_zero_2(p); - xpwm_write_zero_break_1(p); - xpwm_write_zero_break_2(p); -} - -*/ - diff --git a/Inu/Src2/main/xp_write_xpwm_time.h b/Inu/Src2/main/xp_write_xpwm_time.h deleted file mode 100644 index 5e1159d..0000000 --- a/Inu/Src2/main/xp_write_xpwm_time.h +++ /dev/null @@ -1,183 +0,0 @@ -/* - * xp_write_xpwm_time.h - * - * Created on: 03 . 2018 . - * Author: stud - */ - -#ifndef XP_WRITE_TIME_H_ -#define XP_WRITE_TIME_H_ - - -#include "word_structurs.h" - - - -#define PWM_ERROR_LEVEL_INTERRUPT 0 // , !!! -#define PWM_LOW_LEVEL_INTERRUPT 1 // -#define PWM_HIGH_LEVEL_INTERRUPT 2 // - - -#define PWM_MODE_RELOAD_FORCE 0 // -#define PWM_MODE_RELOAD_LEVEL_LOW 1 // , saw_direct=1 -#define PWM_MODE_RELOAD_LEVEL_HIGH 2 // , saw_direct=0 - - - - - -#define PWM_KEY_NUMBER_A1_PLUS 0 -#define PWM_KEY_NUMBER_A1_MINUS 1 -#define PWM_KEY_NUMBER_B1_PLUS 2 -#define PWM_KEY_NUMBER_B1_MINUS 3 -#define PWM_KEY_NUMBER_C1_PLUS 4 -#define PWM_KEY_NUMBER_C1_MINUS 5 - -#define PWM_KEY_NUMBER_A2_PLUS 6 -#define PWM_KEY_NUMBER_A2_MINUS 7 -#define PWM_KEY_NUMBER_B2_PLUS 8 -#define PWM_KEY_NUMBER_B2_MINUS 9 -#define PWM_KEY_NUMBER_C2_PLUS 10 -#define PWM_KEY_NUMBER_C2_MINUS 11 - -#define PWM_KEY_NUMBER_BR1_PLUS 12 -#define PWM_KEY_NUMBER_BR1_MINUS 13 - -#define PWM_KEY_NUMBER_BR2_PLUS 14 -#define PWM_KEY_NUMBER_BR2_MINUS 15 - -////////////////////////////////////////////////////////////////////// -#define ENABLE_PWM_BREAK_ALL 0x0fff -#define ENABLE_PWM_BREAK_1 0xcfff -#define ENABLE_PWM_BREAK_2 0x3fff - -#define ENABLE_PWM_1 0xffc0 -#define ENABLE_PWM_2 0xf03f -#define ENABLE_PWM_1_2 0xf000 - -#define ENABLE_PWM_ALL 0x0000 - -// -#define DISABLE_PWM_BREAK_ALL 0xf000 -#define DISABLE_PWM_BREAK_1 0x3000 -#define DISABLE_PWM_BREAK_2 0xc000 - -#define DISABLE_PWM_1 0x003f -#define DISABLE_PWM_2 0x0fc0 -#define DISABLE_PWM_1_2 0x0fff - -#define DISABLE_PWM_ALL 0xffff - -/// -#define DISABLE_PWM_A1 0x0003 -#define DISABLE_PWM_B1 0x000c -#define DISABLE_PWM_C1 0x0030 - -#define DISABLE_PWM_A2 0x00c0 -#define DISABLE_PWM_B2 0x0300 -#define DISABLE_PWM_C2 0x0c00 -// -////////////////////////////////////////////////////////////////////// -/* - * PWM - Start Stop - * (15) - Soft start-stop m0de 1- soft mode enabled, 0 -disabled. , - * (0)- = 0, ( )., - . - * - soft mode . - * ! . - * (0) - 1 -start, 0 - stop - */ -#define PWM_START_SOFT 0x8001 -#define PWM_START_HARD 0x0001 - -#define PWM_STOP_SOFT 0x8000 -#define PWM_STOP_HARD 0x0000 - -///////////////////////////////////// - - - - -///////////////////////////////////// -typedef struct -{ - // Winding 1 times - unsigned int Ta0_0; - unsigned int Ta0_1; - unsigned int Tb0_0; - unsigned int Tb0_1; - unsigned int Tc0_0; - unsigned int Tc0_1; - // Winding 2 times - unsigned int Ta1_0; - unsigned int Ta1_1; - unsigned int Tb1_0; - unsigned int Tb1_1; - unsigned int Tc1_0; - unsigned int Tc1_1; - // Break transistors - unsigned int Tbr0_0; - unsigned int Tbr0_1; - unsigned int Tbr1_0; - unsigned int Tbr1_1; - //Level transistors closed - unsigned int Tclosed_0; - unsigned int Tclosed_1; - unsigned int Tclosed_high; - unsigned int pwm_tics; - unsigned int inited; - unsigned int freq_pwm; - unsigned int Tclosed_saw_direct_0; - unsigned int Tclosed_saw_direct_1; - unsigned int current_period; - unsigned int where_interrupt; - unsigned int mode_reload; - unsigned int one_or_two_interrupts_run; - WORD_UINT2BITS_STRUCT saw_direct; - void (*write_1_2_winding_break_times)(); - void (*write_1_2_winding_break_times_split)(); -} XPWM_TIME; - -#define DEFAULT_XPWM_TIME {0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,0,0,0,0, {0}, \ - xpwm_write_1_2_winding_break_times_16_lines, \ - xpwm_write_1_2_winding_break_times_16_lines_split_eages } - -void xpwm_write_1_2_winding_break_times_16_lines(); -void xpwm_write_1_2_winding_break_times_16_lines_split_eages(XPWM_TIME *p); -void xpwm_write_zero_winding_break_times_16_lines_split_eages(); -void initXpwmTimeStructure(XPWM_TIME *p); - -extern XPWM_TIME xpwm_time; - -#define write_winding1_fase_a(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 0); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 1); WriteMemory(ADR_PWM_TIMING, T1); - -#define write_winding1_fase_b(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 2); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 3); WriteMemory(ADR_PWM_TIMING, T1); - -#define write_winding1_fase_c(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 4); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 5); WriteMemory(ADR_PWM_TIMING, T1); - -#define write_winding2_fase_a(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 6); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 7); WriteMemory(ADR_PWM_TIMING, T1); - -#define write_winding2_fase_b(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 8); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 9); WriteMemory(ADR_PWM_TIMING, T1); - -#define write_winding2_fase_c(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 10); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 11); WriteMemory(ADR_PWM_TIMING, T1); -#define write_break_winding1(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 12); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 13); WriteMemory(ADR_PWM_TIMING, T1); - -#define write_break_winding2(T0, T1) \ - WriteMemory(ADR_PWM_KEY_NUMBER, 14); WriteMemory(ADR_PWM_TIMING, T0); \ - WriteMemory(ADR_PWM_KEY_NUMBER, 15); WriteMemory(ADR_PWM_TIMING, T1); - - -#endif /* XP_WRITE_TIME_H_ */ diff --git a/Inu/mcu_app_includes.h b/Inu/app_wrapper/app_includes.h similarity index 96% rename from Inu/mcu_app_includes.h rename to Inu/app_wrapper/app_includes.h index 0834ec4..30f650c 100644 --- a/Inu/mcu_app_includes.h +++ b/Inu/app_wrapper/app_includes.h @@ -1,6 +1,6 @@ /** ************************************************************************** -* @file mcu_app_includes.h +* @file app_includes.h * @brief Заголовочный файл для оболочки МК. ************************************************************************** @details diff --git a/Inu/main_matlab/init28335.c b/Inu/app_wrapper/app_init.c similarity index 94% rename from Inu/main_matlab/init28335.c rename to Inu/app_wrapper/app_init.c index 307a141..8a36879 100644 --- a/Inu/main_matlab/init28335.c +++ b/Inu/app_wrapper/app_init.c @@ -8,11 +8,11 @@ **************************************************************************/ -#include "init28335.h" +#include "app_init.h" #define FREQ_TIMER_3 (FREQ_PWM*2) -void init28335(void) { +void app_init(void) { init_simple_scalar(); edrk.flag_second_PCH = 0; @@ -55,7 +55,7 @@ void init28335(void) { control_station.setup_time_detect_active[CONTROL_STATION_TERMINAL_RS232] = 50; //analog_zero.iqU_1 = 2048; //analog_zero.iqU_2 = 2048; -} //void init28335(void) +} //void app_init(void) void edrk_init_variables_matlab(void) { @@ -178,4 +178,22 @@ void set_zadanie_u_charge_matlab(void) edrk.zadanie.iq_set_break_level = _IQ(NOMINAL_U_BREAK_LEVEL / NORMA_ACP); -} \ No newline at end of file +} + + +void init_flag_a(void) +{ + + //unsigned int i = 0; + //int* pStr = (int*)&f; + //for (i = 0; i < sizeof(f) / sizeof(int); i++) { + // *(pStr + i) = 0; + //} + // + //*pStr = (int*)&a; + //for (i = 0; i < sizeof(a) / sizeof(int); i++) { + // *(pStr + i) = 0; + //} + + +} diff --git a/Inu/main_matlab/init28335.h b/Inu/app_wrapper/app_init.h similarity index 83% rename from Inu/main_matlab/init28335.h rename to Inu/app_wrapper/app_init.h index b6d2e46..1100fa0 100644 --- a/Inu/main_matlab/init28335.h +++ b/Inu/app_wrapper/app_init.h @@ -3,10 +3,11 @@ #include "mcu_wrapper_conf.h" -void init28335(void); +void app_init(void); void edrk_init_matlab(void); void edrk_init_variables_matlab(void); void set_zadanie_u_charge_matlab(void); void init_Uin_rms(void); +void init_flag_a(void); #endif //INIT28335 diff --git a/Inu/main_matlab/param.c b/Inu/app_wrapper/app_io.c similarity index 99% rename from Inu/main_matlab/param.c rename to Inu/app_wrapper/app_io.c index a8261b1..75b5f29 100644 --- a/Inu/main_matlab/param.c +++ b/Inu/app_wrapper/app_io.c @@ -5,7 +5,7 @@ : 2021.11.08 **************************************************************************/ -#include "param.h" +#include "app_io.h" #include "pwm_sim.h" int Unites[UNIT_QUA_UNITS][UNIT_LEN]; diff --git a/Inu/main_matlab/param.h b/Inu/app_wrapper/app_io.h similarity index 100% rename from Inu/main_matlab/param.h rename to Inu/app_wrapper/app_io.h diff --git a/Inu/main_matlab/main_matlab.c b/Inu/app_wrapper/app_wrapper.c similarity index 96% rename from Inu/main_matlab/main_matlab.c rename to Inu/app_wrapper/app_wrapper.c index 09370d9..4295837 100644 --- a/Inu/main_matlab/main_matlab.c +++ b/Inu/app_wrapper/app_wrapper.c @@ -401,22 +401,4 @@ void optical_bus_read(){ void optical_bus_write(void){ -} - - -void init_flag_a(void) -{ - - unsigned int i = 0; - int *pStr = (int*)&f; - for (i = 0; i < sizeof(f) / sizeof(int); i++) { - *(pStr + i) = 0; - } - - *pStr = (int*)&a; - for (i = 0; i < sizeof(a) / sizeof(int); i++) { - *(pStr + i) = 0; - } - - -} +} \ No newline at end of file diff --git a/Inu/main_matlab/main_matlab.h b/Inu/app_wrapper/app_wrapper.h similarity index 76% rename from Inu/main_matlab/main_matlab.h rename to Inu/app_wrapper/app_wrapper.h index 59a6c39..b0f1f2a 100644 --- a/Inu/main_matlab/main_matlab.h +++ b/Inu/app_wrapper/app_wrapper.h @@ -5,7 +5,6 @@ void mcu_simulate_step(void); -void init_flag_a(void); diff --git a/Inu/main_matlab/def.h b/Inu/app_wrapper/def.h similarity index 96% rename from Inu/main_matlab/def.h rename to Inu/app_wrapper/def.h index 7336088..f901fdc 100644 --- a/Inu/main_matlab/def.h +++ b/Inu/app_wrapper/def.h @@ -11,11 +11,11 @@ // , (30 .) #define SHIFT -#define SIMULINK_SEQUENCE V_PWM24_PHASE_SEQ_NORMAL_ABC -/* V_PWM24_PHASE_SEQ_NORMAL_ABC, - - V_PWM24_PHASE_SEQ_NORMAL_BCA, - - V_PWM24_PHASE_SEQ_NORMAL_CAB, - - V_PWM24_PHASE_SEQ_REVERS_ACB, - +#define SIMULINK_SEQUENCE V_PWM24_PHASE_SEQ_REVERS_BAC +/* V_PWM24_PHASE_SEQ_NORMAL_ABC, - , + V_PWM24_PHASE_SEQ_NORMAL_BCA, - + V_PWM24_PHASE_SEQ_NORMAL_CAB, - + V_PWM24_PHASE_SEQ_REVERS_ACB, - V_PWM24_PHASE_SEQ_REVERS_CBA, - V_PWM24_PHASE_SEQ_REVERS_BAC - */ diff --git a/Inu/main_matlab/device_support/ReadMe.txt b/Inu/app_wrapper/device_support/ReadMe.txt similarity index 100% rename from Inu/main_matlab/device_support/ReadMe.txt rename to Inu/app_wrapper/device_support/ReadMe.txt diff --git a/Inu/main_matlab/device_support/include/C28x_FPU_FastRTS.h b/Inu/app_wrapper/device_support/include/C28x_FPU_FastRTS.h similarity index 100% rename from Inu/main_matlab/device_support/include/C28x_FPU_FastRTS.h rename to Inu/app_wrapper/device_support/include/C28x_FPU_FastRTS.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Adc.h b/Inu/app_wrapper/device_support/include/DSP281x_Adc.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Adc.h rename to Inu/app_wrapper/device_support/include/DSP281x_Adc.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_CpuTimers.h b/Inu/app_wrapper/device_support/include/DSP281x_CpuTimers.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_CpuTimers.h rename to Inu/app_wrapper/device_support/include/DSP281x_CpuTimers.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_DefaultISR.h b/Inu/app_wrapper/device_support/include/DSP281x_DefaultISR.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_DefaultISR.h rename to Inu/app_wrapper/device_support/include/DSP281x_DefaultISR.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_DevEmu.h b/Inu/app_wrapper/device_support/include/DSP281x_DevEmu.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_DevEmu.h rename to Inu/app_wrapper/device_support/include/DSP281x_DevEmu.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Device.h b/Inu/app_wrapper/device_support/include/DSP281x_Device.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Device.h rename to Inu/app_wrapper/device_support/include/DSP281x_Device.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_ECan.h b/Inu/app_wrapper/device_support/include/DSP281x_ECan.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_ECan.h rename to Inu/app_wrapper/device_support/include/DSP281x_ECan.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Ev.h b/Inu/app_wrapper/device_support/include/DSP281x_Ev.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Ev.h rename to Inu/app_wrapper/device_support/include/DSP281x_Ev.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Examples.h b/Inu/app_wrapper/device_support/include/DSP281x_Examples.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Examples.h rename to Inu/app_wrapper/device_support/include/DSP281x_Examples.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_GlobalPrototypes.h b/Inu/app_wrapper/device_support/include/DSP281x_GlobalPrototypes.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_GlobalPrototypes.h rename to Inu/app_wrapper/device_support/include/DSP281x_GlobalPrototypes.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Gpio.h b/Inu/app_wrapper/device_support/include/DSP281x_Gpio.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Gpio.h rename to Inu/app_wrapper/device_support/include/DSP281x_Gpio.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Mcbsp.h b/Inu/app_wrapper/device_support/include/DSP281x_Mcbsp.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Mcbsp.h rename to Inu/app_wrapper/device_support/include/DSP281x_Mcbsp.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_PieCtrl.h b/Inu/app_wrapper/device_support/include/DSP281x_PieCtrl.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_PieCtrl.h rename to Inu/app_wrapper/device_support/include/DSP281x_PieCtrl.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_PieVect.h b/Inu/app_wrapper/device_support/include/DSP281x_PieVect.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_PieVect.h rename to Inu/app_wrapper/device_support/include/DSP281x_PieVect.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_SWPrioritizedIsrLevels.h b/Inu/app_wrapper/device_support/include/DSP281x_SWPrioritizedIsrLevels.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_SWPrioritizedIsrLevels.h rename to Inu/app_wrapper/device_support/include/DSP281x_SWPrioritizedIsrLevels.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Sci.h b/Inu/app_wrapper/device_support/include/DSP281x_Sci.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Sci.h rename to Inu/app_wrapper/device_support/include/DSP281x_Sci.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Spi.h b/Inu/app_wrapper/device_support/include/DSP281x_Spi.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Spi.h rename to Inu/app_wrapper/device_support/include/DSP281x_Spi.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_SysCtrl.h b/Inu/app_wrapper/device_support/include/DSP281x_SysCtrl.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_SysCtrl.h rename to Inu/app_wrapper/device_support/include/DSP281x_SysCtrl.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_XIntrupt.h b/Inu/app_wrapper/device_support/include/DSP281x_XIntrupt.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_XIntrupt.h rename to Inu/app_wrapper/device_support/include/DSP281x_XIntrupt.h diff --git a/Inu/main_matlab/device_support/include/DSP281x_Xintf.h b/Inu/app_wrapper/device_support/include/DSP281x_Xintf.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP281x_Xintf.h rename to Inu/app_wrapper/device_support/include/DSP281x_Xintf.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_Adc.h b/Inu/app_wrapper/device_support/include/DSP2833x_Adc.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_Adc.h rename to Inu/app_wrapper/device_support/include/DSP2833x_Adc.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_CpuTimers.h b/Inu/app_wrapper/device_support/include/DSP2833x_CpuTimers.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_CpuTimers.h rename to Inu/app_wrapper/device_support/include/DSP2833x_CpuTimers.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_DMA.h b/Inu/app_wrapper/device_support/include/DSP2833x_DMA.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_DMA.h rename to Inu/app_wrapper/device_support/include/DSP2833x_DMA.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_DevEmu.h b/Inu/app_wrapper/device_support/include/DSP2833x_DevEmu.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_DevEmu.h rename to Inu/app_wrapper/device_support/include/DSP2833x_DevEmu.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_Device.h b/Inu/app_wrapper/device_support/include/DSP2833x_Device.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_Device.h rename to Inu/app_wrapper/device_support/include/DSP2833x_Device.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_ECan.h b/Inu/app_wrapper/device_support/include/DSP2833x_ECan.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_ECan.h rename to Inu/app_wrapper/device_support/include/DSP2833x_ECan.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_ECap.h b/Inu/app_wrapper/device_support/include/DSP2833x_ECap.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_ECap.h rename to Inu/app_wrapper/device_support/include/DSP2833x_ECap.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_EPwm.h b/Inu/app_wrapper/device_support/include/DSP2833x_EPwm.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_EPwm.h rename to Inu/app_wrapper/device_support/include/DSP2833x_EPwm.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_EQep.h b/Inu/app_wrapper/device_support/include/DSP2833x_EQep.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_EQep.h rename to Inu/app_wrapper/device_support/include/DSP2833x_EQep.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_Gpio.h b/Inu/app_wrapper/device_support/include/DSP2833x_Gpio.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_Gpio.h rename to Inu/app_wrapper/device_support/include/DSP2833x_Gpio.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_I2c.h b/Inu/app_wrapper/device_support/include/DSP2833x_I2c.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_I2c.h rename to Inu/app_wrapper/device_support/include/DSP2833x_I2c.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_Mcbsp.h b/Inu/app_wrapper/device_support/include/DSP2833x_Mcbsp.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_Mcbsp.h rename to Inu/app_wrapper/device_support/include/DSP2833x_Mcbsp.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_PieCtrl.h b/Inu/app_wrapper/device_support/include/DSP2833x_PieCtrl.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_PieCtrl.h rename to Inu/app_wrapper/device_support/include/DSP2833x_PieCtrl.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_PieVect.h b/Inu/app_wrapper/device_support/include/DSP2833x_PieVect.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_PieVect.h rename to Inu/app_wrapper/device_support/include/DSP2833x_PieVect.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_Sci.h b/Inu/app_wrapper/device_support/include/DSP2833x_Sci.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_Sci.h rename to Inu/app_wrapper/device_support/include/DSP2833x_Sci.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_Spi.h b/Inu/app_wrapper/device_support/include/DSP2833x_Spi.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_Spi.h rename to Inu/app_wrapper/device_support/include/DSP2833x_Spi.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_SysCtrl.h b/Inu/app_wrapper/device_support/include/DSP2833x_SysCtrl.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_SysCtrl.h rename to Inu/app_wrapper/device_support/include/DSP2833x_SysCtrl.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_XIntrupt.h b/Inu/app_wrapper/device_support/include/DSP2833x_XIntrupt.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_XIntrupt.h rename to Inu/app_wrapper/device_support/include/DSP2833x_XIntrupt.h diff --git a/Inu/main_matlab/device_support/include/DSP2833x_Xintf.h b/Inu/app_wrapper/device_support/include/DSP2833x_Xintf.h similarity index 100% rename from Inu/main_matlab/device_support/include/DSP2833x_Xintf.h rename to Inu/app_wrapper/device_support/include/DSP2833x_Xintf.h diff --git a/Inu/main_matlab/device_support/include/IQmathLib.h b/Inu/app_wrapper/device_support/include/IQmathLib.h similarity index 100% rename from Inu/main_matlab/device_support/include/IQmathLib.h rename to Inu/app_wrapper/device_support/include/IQmathLib.h diff --git a/Inu/main_matlab/device_support/include/SimSupport_GlobalPrototypes.h b/Inu/app_wrapper/device_support/include/SimSupport_GlobalPrototypes.h similarity index 100% rename from Inu/main_matlab/device_support/include/SimSupport_GlobalPrototypes.h rename to Inu/app_wrapper/device_support/include/SimSupport_GlobalPrototypes.h diff --git a/Inu/main_matlab/device_support/include/dmctype.h b/Inu/app_wrapper/device_support/include/dmctype.h similarity index 100% rename from Inu/main_matlab/device_support/include/dmctype.h rename to Inu/app_wrapper/device_support/include/dmctype.h diff --git a/Inu/main_matlab/device_support/source/C28x_FPU_FastRTS.c b/Inu/app_wrapper/device_support/source/C28x_FPU_FastRTS.c similarity index 100% rename from Inu/main_matlab/device_support/source/C28x_FPU_FastRTS.c rename to Inu/app_wrapper/device_support/source/C28x_FPU_FastRTS.c diff --git a/Inu/main_matlab/device_support/source/C28x_FPU_FastRTS.obj b/Inu/app_wrapper/device_support/source/C28x_FPU_FastRTS.obj similarity index 100% rename from Inu/main_matlab/device_support/source/C28x_FPU_FastRTS.obj rename to Inu/app_wrapper/device_support/source/C28x_FPU_FastRTS.obj diff --git a/Inu/main_matlab/device_support/source/DSP281x_GlobalVariableDefs.c b/Inu/app_wrapper/device_support/source/DSP281x_GlobalVariableDefs.c similarity index 100% rename from Inu/main_matlab/device_support/source/DSP281x_GlobalVariableDefs.c rename to Inu/app_wrapper/device_support/source/DSP281x_GlobalVariableDefs.c diff --git a/Inu/main_matlab/device_support/source/DSP2833x_GlobalVariableDefs.c b/Inu/app_wrapper/device_support/source/DSP2833x_GlobalVariableDefs.c similarity index 100% rename from Inu/main_matlab/device_support/source/DSP2833x_GlobalVariableDefs.c rename to Inu/app_wrapper/device_support/source/DSP2833x_GlobalVariableDefs.c diff --git a/Inu/main_matlab/device_support/source/DSP2833x_GlobalVariableDefs.obj b/Inu/app_wrapper/device_support/source/DSP2833x_GlobalVariableDefs.obj similarity index 100% rename from Inu/main_matlab/device_support/source/DSP2833x_GlobalVariableDefs.obj rename to Inu/app_wrapper/device_support/source/DSP2833x_GlobalVariableDefs.obj diff --git a/Inu/main_matlab/device_support/source/IQmathLib_matlab.c b/Inu/app_wrapper/device_support/source/IQmathLib_matlab.c similarity index 100% rename from Inu/main_matlab/device_support/source/IQmathLib_matlab.c rename to Inu/app_wrapper/device_support/source/IQmathLib_matlab.c diff --git a/Inu/main_matlab/pwm_sim.c b/Inu/main_matlab/pwm_sim.c deleted file mode 100644 index d6ba1f1..0000000 --- a/Inu/main_matlab/pwm_sim.c +++ /dev/null @@ -1,158 +0,0 @@ -#include "pwm_sim.h" - -TimerSimHandle t1sim; -TimerSimHandle t2sim; -TimerSimHandle t3sim; -TimerSimHandle t4sim; -TimerSimHandle t5sim; -TimerSimHandle t6sim; -TimerSimHandle t7sim; -TimerSimHandle t8sim; -TimerSimHandle t9sim; -TimerSimHandle t10sim; -TimerSimHandle t11sim; -TimerSimHandle t12sim; - -void Simulate_Timers(void) -{ - SimulateMainPWM(&t1sim, xpwm_time.Ta0_1); - SimulateSimplePWM(&t2sim, xpwm_time.Ta0_0); - SimulateSimplePWM(&t3sim, xpwm_time.Tb0_1); - SimulateSimplePWM(&t4sim, xpwm_time.Tb0_0); - SimulateSimplePWM(&t5sim, xpwm_time.Tc0_1); - SimulateSimplePWM(&t6sim, xpwm_time.Tc0_0); - SimulateSimplePWM(&t7sim, xpwm_time.Ta1_1); - SimulateSimplePWM(&t8sim, xpwm_time.Ta1_0); - SimulateSimplePWM(&t9sim, xpwm_time.Tb1_1); - SimulateSimplePWM(&t10sim, xpwm_time.Tb1_0); - SimulateSimplePWM(&t11sim, xpwm_time.Tc1_1); - SimulateSimplePWM(&t12sim, xpwm_time.Tc1_0); -} - -void Init_Timers(void) -{ - initSimulateTim(&t1sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t2sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t3sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t4sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t5sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t6sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t7sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t8sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t9sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t10sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t11sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); - initSimulateTim(&t12sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.SimSampleTime); -} - - - - -void initSimulateTim(TimerSimHandle* tsim, int period, double step) -{ - tsim->dtsim.stateDt = 1; - tsim->TPr = period; - tsim->TxCntPlus = step * 2; - tsim->dtsim.DtCntPeriod = (int)(DT / hmcu.SimSampleTime); -} - -void SimulateMainPWM(TimerSimHandle* tsim, int compare) -{ -#ifdef UNITED_COUNTER - tsim->tcntAuxPrev = tsim->tcntAux; - tsim->tcntAux += tsim->TxCntPlus; -#endif - if (simulateTimAndGetCompare(tsim, compare)) - mcu_simulate_step(); - simulateActionActionQualifierSubmodule(tsim); - simulateDeadBendSubmodule(tsim); - simulateTripZoneSubmodule(tsim); -} - -void SimulateSimplePWM(TimerSimHandle* tsim, int compare) -{ - simulateTimAndGetCompare(tsim, compare, 0); - simulateActionActionQualifierSubmodule(tsim); - //tsim->ciA = tsim->dtsim.ciA_DT; - //tsim->ciB = tsim->dtsim.ciB_DT; - simulateDeadBendSubmodule(tsim); - simulateTripZoneSubmodule(tsim); -} - - -int simulateTimAndGetCompare(TimerSimHandle* tsim, int compare) -{ - int interruptflag = 0; - -#ifdef UNITED_COUNTER - tsim->tcntAuxPrev = t1sim.tcntAuxPrev; - tsim->tcntAux = t1sim.tcntAux; -#else - tsim->tcntAuxPrev = tsim->tcntAux; - tsim->tcntAux += tsim->TxCntPlus; -#endif - - if (tsim->tcntAux > tsim->TPr) { - tsim->tcntAux -= tsim->TPr * 2.; - tsim->cmpA = compare; - interruptflag = 1; - } - if ((tsim->tcntAuxPrev < 0) && (tsim->tcntAux >= 0)) { - tsim->cmpA = compare; - interruptflag = 1; - } - tsim->tcnt = fabs(tsim->tcntAux); - return interruptflag; -} - - - -void simulateActionActionQualifierSubmodule(TimerSimHandle* tsim) -{ - // <20><> Action-Qualifier Submodule - if (tsim->cmpA > tsim->tcnt) { - tsim->dtsim.ciA_DT = 0; - tsim->dtsim.ciB_DT = 1; - } - else if (tsim->cmpA < tsim->tcnt) { - tsim->dtsim.ciA_DT = 1; - tsim->dtsim.ciB_DT = 0; - } -} - -void simulateDeadBendSubmodule(TimerSimHandle* tsim) -{ - // <20><> Dead-Band Submodule - if (tsim->dtsim.stateDt == 1) { - tsim->ciA = tsim->dtsim.ciA_DT; - tsim->ciB = 0; - if (tsim->dtsim.ciA_DT == 1) - tsim->dtsim.dtcnt = tsim->dtsim.DtCntPeriod; - if (tsim->dtsim.dtcnt > 0) - tsim->dtsim.dtcnt--; - else - tsim->dtsim.stateDt = 2; - } - else if (tsim->dtsim.stateDt == 2) { - tsim->ciA = 0; - tsim->ciB = tsim->dtsim.ciB_DT; - if (tsim->dtsim.ciB_DT == 1) - tsim->dtsim.dtcnt = tsim->dtsim.DtCntPeriod; - if (tsim->dtsim.dtcnt > 0) - tsim->dtsim.dtcnt--; - else - tsim->dtsim.stateDt = 1; - } -} - -void simulateTripZoneSubmodule(TimerSimHandle* tsim) -{ - //// <20><> Trip-Zone Submodule - // // ... clear flag for one-shot trip latch - //if (EPwm1Regs.TZCLR.all == 0x0004) { - // EPwm1Regs.TZCLR.all = 0x0000; - // EPwm1Regs.TZFRC.all = 0x0000; - //} // ... forces a one-shot trip event - //if (EPwm1Regs.TZFRC.all == 0x0004) - // ci1A_DT = ci1B_DT = 0; -} \ No newline at end of file diff --git a/Inu/mcu_wrapper.c b/Inu/mcu_wrapper.c index 1cd7d58..e3ffe1a 100644 --- a/Inu/mcu_wrapper.c +++ b/Inu/mcu_wrapper.c @@ -7,8 +7,8 @@ Данный файл содержит функции для симуляции МК в Simulink (S-Function). **************************************************************************/ #include "mcu_wrapper_conf.h" -#include "init28335.h" -#include "param.h" +#include "app_init.h" +#include "app_io.h" #include "pwm_sim.h" /** @@ -24,7 +24,7 @@ SIM__MCUHandleTypeDef hmcu; ///< Хендл для управления //-------------------------------------------------------------// //-----------------CONTROLLER SIMULATE FUNCTIONS---------------// /* THREAD FOR MCU APP */ -#ifdef RUN_APP_THRREAD +#ifdef RUN_APP_MAIN_FUNC_THREAD /** * @brief Главная функция приложения МК. * @details Функция с которой начинается выполнение кода МК. Выход из данной функции происходит только в конце симуляции @ref mdlTerminate @@ -39,7 +39,7 @@ unsigned __stdcall MCU_App_Thread(void) { return 0; // end thread // note: this return will reached only at the end of simulation, when all whiles will be skipped due to @ref sim_while } -#endif //RUN_APP_THRREAD +#endif //RUN_APP_MAIN_FUNC_THREAD /* SIMULATE MCU FOR ONE SIMULATION STEP */ /** * @brief Симуляция МК на один такт симуляции. @@ -53,7 +53,7 @@ unsigned __stdcall MCU_App_Thread(void) { */ void MCU_Step_Simulation(SimStruct* S, time_T time) { - hmcu.SystemClockDouble += hmcu.SystemClock_step; // emulate core clock + hmcu.SystemClockDouble += hmcu.sSystemClock_step; // emulate core clock hmcu.SystemClock = hmcu.SystemClockDouble; hmcu.SimTime = time; @@ -61,13 +61,13 @@ void MCU_Step_Simulation(SimStruct* S, time_T time) MCU_Periph_Simulation(); // simulate peripheral -#ifdef RUN_APP_THRREAD +#ifdef RUN_APP_MAIN_FUNC_THREAD ResumeThread(hmcu.hMCUThread); for (int i = DEKSTOP_CYCLES_FOR_MCU_APP; i > 0; i--) { } SuspendThread(hmcu.hMCUThread); -#endif //RUN_APP_THRREAD +#endif //RUN_APP_MAIN_FUNC_THREAD MCU_writeOutputs(S); // запись портов (по факту запись в буфер. запись в порты в mdlOutputs) } @@ -124,17 +124,17 @@ void MCU_writeOutputs(SimStruct* S) */ void SIM_Initialize_Simulation(void) { -#ifdef RUN_APP_THRREAD +#ifdef RUN_APP_MAIN_FUNC_THREAD // инициализация потока, который будет выполнять код МК hmcu.hMCUThread = (HANDLE)CreateThread(NULL, 0, MCU_App_Thread, 0, CREATE_SUSPENDED, &hmcu.idMCUThread); -#endif //RUN_APP_THRREAD +#endif //RUN_APP_MAIN_FUNC_THREAD /* user initialization */ Init_Timers(); - init28335(); + app_init(); /* clock step initialization */ - hmcu.SystemClock_step = MCU_CORE_CLOCK * hmcu.SimSampleTime; // set system clock step + hmcu.sSystemClock_step = MCU_CORE_CLOCK * hmcu.sSimSampleTime; // set system clock step } /* MCU WRAPPER DEINITIALIZATION */ /** diff --git a/Inu/mcu_wrapper_conf.h b/Inu/mcu_wrapper_conf.h index bfbce76..ad1be02 100644 --- a/Inu/mcu_wrapper_conf.h +++ b/Inu/mcu_wrapper_conf.h @@ -28,7 +28,7 @@ #include "simstruc.h" // For S-Function variables #include // For threads -#include "mcu_app_includes.h" +#include "app_includes.h" /** @@ -45,24 +45,22 @@ */ // Parametrs of MCU simulator -//#define RUN_APP_THRREAD -#define CREATE_SUSPENDED 0x00000004 ///< define from WinBase.h. We dont wanna include "Windows.h" or smth like this, because of HAL there are a lot of redefine errors. - +//#define RUN_APP_MAIN_FUNC_THREAD ///< Enable using thread for MCU main() func #define DEKSTOP_CYCLES_FOR_MCU_APP 0xFFFF ///< number of for() cycles after which MCU thread would be suspended +#define MCU_CORE_CLOCK 150000000 ///< MCU clock rate for simulation // Parameters of S_Function -#define NPARAMS 1 ///< number of input parametrs (only Ts) -#define IN_PORT_WIDTH 20 ///< width of input ports -#define IN_PORT_NUMB 1 ///< number of input ports -#define OUT_PORT_WIDTH 51 ///< width of output ports -#define OUT_PORT_NUMB 1 ///< number of output ports -#define DISC_STATES_WIDTH OUT_PORT_WIDTH*OUT_PORT_NUMB ///< width of discrete states array +#define NPARAMS 1 ///< number of input parametrs (only Ts) +#define IN_PORT_WIDTH 20 ///< width of input ports +#define IN_PORT_NUMB 1 ///< number of input ports +#define OUT_PORT_WIDTH 51 ///< width of output ports +#define OUT_PORT_NUMB 1 ///< number of output ports +#define DISC_STATES_WIDTH OUT_PORT_WIDTH*OUT_PORT_NUMB ///< width of discrete states array (outbup buffer) #define RWORK_0_WIDTH 5 //width of the real-work vector #define IWORK_0_WIDTH 5 //width of the integer-work vector -#define MCU_CORE_CLOCK 150000000 /** WRAPPER_CONF * @} */ @@ -73,6 +71,11 @@ * @{ */ +/** + * @brief Define for creating thread in suspended state. + * @details Define from WinBase.h. We dont wanna include "Windows.h" or smth like this, because of HAL there are a lot of redefine errors. + */ +#define CREATE_SUSPENDED 0x00000004 typedef void* HANDLE; ///< MCU handle typedef /** @@ -85,17 +88,19 @@ typedef struct { int idMCUThread; ///< id потока МК (unused) // Flags unsigned fMCU_Stop : 1; ///< флаг для выхода из потока программы МК - double SimSampleTime; ///< sample time of simulation + + double SimTime; ///< Текущее время симуляции + long SystemClock; ///< Счетчик тактов для симуляции системных тиков (в целочисленном формате) double SystemClockDouble; ///< Счетчик в формате double для точной симуляции системных тиков С промежуточными значений - long SystemClock; ///< Счетчик тактов для симуляции системных тиков (в целочисленном формате) - double SystemClock_step; ///< Шаг тиков для их симуляции, в формате double - double SimTime; ///< Текущее время симуляции + double sSystemClock_step; ///< Шаг тиков для их симуляции, в формате double + double sSimSampleTime; ///< Период дискретизации симуляции }SIM__MCUHandleTypeDef; extern SIM__MCUHandleTypeDef hmcu; // extern для видимости переменной во всех файлах //-------------------------------------------------------------// //------------------ SIMULINK WHILE DEFINES -----------------// +#ifdef RUN_APP_MAIN_FUNC_THREAD /* DEFINE TO WHILE WITH SIMULINK WHILE */ /** * @brief Redefine C while statement with sim_while() macro. @@ -103,6 +108,7 @@ extern SIM__MCUHandleTypeDef hmcu; // extern для видимос * @details Это while который будет использоваться в симулинке @ref sim_while для подробностей. */ #define while(_expression_) sim_while(_expression_) +#endif /* SIMULINK WHILE */ /** @@ -165,8 +171,8 @@ void SIM_writeOutput(SimStruct* S); * @file run_mex.bat * @brief Батник для компиляции оболочки МК. * @details - * Вызывается в матлабе из mexing.m. + * Вызывается в матлабе из allmex.m. * * Исходный код батника: - * @include F:\Work\Projects\MATLAB\matlab_stm_emulate\MCU_Wrapper\run_mex.bat + * @include run_mex.bat */ \ No newline at end of file diff --git a/run_mex.bat b/Inu/run_mex.bat similarity index 84% rename from run_mex.bat rename to Inu/run_mex.bat index f60a657..cb04f73 100644 --- a/run_mex.bat +++ b/Inu/run_mex.bat @@ -4,12 +4,7 @@ set defines=-D"ML" -D"__IQMATHLIB_H_INCLUDED__" -D"_MATLAB_SIMULATOR"^ -D"_MATLAB_FAST_SIMULATOR" -D"PROJECT_SHIP=1" :: -------------------------USERS PATHS AND CODE--------------------------- -set includes_USER=-I".\Inu\main_matlab\device_support\include"^ - -I".\"^ - -I".\Inu"^ - -I".\Inu\tms_matlab\"^ - -I".\Inu\main_matlab"^ - -I".\Inu\Src\main"^ +set includes_USER=-I".\Inu\Src\main"^ -I".\Inu\Src\N12_VectorControl"^ -I".\Inu\Src\N12_Libs"^ -I".\Inu\Src\N12_Xilinx" @@ -49,9 +44,9 @@ set params_libs_c=.\Inu\Src\N12_Libs\mathlib.c^ .\Inu\Src\N12_Xilinx\xp_write_xpwm_time.c -set params_device_support=.\Inu\main_matlab\device_support\source\C28x_FPU_FastRTS.obj^ - .\Inu\main_matlab\device_support\source\DSP281x_GlobalVariableDefs.c^ - .\Inu\main_matlab\device_support\source\IQmathLib_matlab.c +set params_device_support=.\Inu\app_wrapper\device_support\source\C28x_FPU_FastRTS.obj^ + .\Inu\app_wrapper\device_support\source\DSP281x_GlobalVariableDefs.c^ + .\Inu\app_wrapper\device_support\source\IQmathLib_matlab.c set code_USER=%params_main_c% %params_vectorcontorl_c% %params_libs_c% %params_device_support% @@ -61,13 +56,18 @@ set code_USER=%params_main_c% %params_vectorcontorl_c% %params_libs_c% %params_d :: -------------------------WRAPPER PATHS AND CODE--------------------------- :: оболочка, которая будет моделировать работу МК в симулинке -set includes_WRAPPER= -I".\Inu" +set includes_WRAPPER=-I"."^ + -I".\Inu"^ + -I".\Inu\app_wrapper"^ + -I".\Inu\app_wrapper\device_support\include"^ + -I".\Inu\xilinx_wrapper" + set code_WRAPPER= .\Inu\MCU.c^ .\Inu\mcu_wrapper.c^ - .\Inu\main_matlab\pwm_sim.c^ - .\Inu\main_matlab\init28335.c^ - .\Inu\main_matlab\param.c^ - .\Inu\main_matlab\main_matlab.c + .\Inu\app_wrapper\app_init.c^ + .\Inu\app_wrapper\app_io.c^ + .\Inu\app_wrapper\app_wrapper.c^ + .\Inu\xilinx_wrapper\pwm_sim.c ::------------------------------------------------------------------------- diff --git a/Inu/main_matlab/adc_sim.c b/Inu/xilinx_wrapper/adc_sim.c similarity index 100% rename from Inu/main_matlab/adc_sim.c rename to Inu/xilinx_wrapper/adc_sim.c diff --git a/Inu/main_matlab/adc_sim.h b/Inu/xilinx_wrapper/adc_sim.h similarity index 100% rename from Inu/main_matlab/adc_sim.h rename to Inu/xilinx_wrapper/adc_sim.h diff --git a/Inu/xilinx_wrapper/pwm_sim.c b/Inu/xilinx_wrapper/pwm_sim.c new file mode 100644 index 0000000..8cb6d13 --- /dev/null +++ b/Inu/xilinx_wrapper/pwm_sim.c @@ -0,0 +1,281 @@ +#include "pwm_sim.h" + +TimerSimHandle t1sim; +TimerSimHandle t2sim; +TimerSimHandle t3sim; +TimerSimHandle t4sim; +TimerSimHandle t5sim; +TimerSimHandle t6sim; +TimerSimHandle t7sim; +TimerSimHandle t8sim; +TimerSimHandle t9sim; +TimerSimHandle t10sim; +TimerSimHandle t11sim; +TimerSimHandle t12sim; + +#ifdef SIMULATION_MODE_XILINX +XilinkTkPhaseSimHandle XilinxTkPhaseA1; +XilinkTkPhaseSimHandle XilinxTkPhaseB1; +XilinkTkPhaseSimHandle XilinxTkPhaseC1; +XilinkTkPhaseSimHandle XilinxTkPhaseA2; +XilinkTkPhaseSimHandle XilinxTkPhaseB2; +XilinkTkPhaseSimHandle XilinxTkPhaseC2; +#endif + +void Simulate_Timers(void) +{ + SimulateMainPWM(&t1sim, xpwm_time.Ta0_1); + SimulateSimplePWM(&t2sim, xpwm_time.Ta0_0); + SimulateSimplePWM(&t3sim, xpwm_time.Tb0_1); + SimulateSimplePWM(&t4sim, xpwm_time.Tb0_0); + SimulateSimplePWM(&t5sim, xpwm_time.Tc0_1); + SimulateSimplePWM(&t6sim, xpwm_time.Tc0_0); + SimulateSimplePWM(&t7sim, xpwm_time.Ta1_1); + SimulateSimplePWM(&t8sim, xpwm_time.Ta1_0); + SimulateSimplePWM(&t9sim, xpwm_time.Tb1_1); + SimulateSimplePWM(&t10sim, xpwm_time.Tb1_0); + SimulateSimplePWM(&t11sim, xpwm_time.Tc1_1); + SimulateSimplePWM(&t12sim, xpwm_time.Tc1_0); +} + +void Init_Timers(void) +{ + initSimulateTim(&t1sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t2sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t3sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t4sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t5sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t6sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t7sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t8sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t9sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t10sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t11sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t12sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); +} + + +void Init_Xilinx(XilinkTkPhaseSimHandle tksim) +{ + initSimulateTim(&t1sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); + initSimulateTim(&t2sim, FREQ_INTERNAL_GENERATOR_XILINX_TMS / FREQ_PWM, FREQ_INTERNAL_GENERATOR_XILINX_TMS * hmcu.sSimSampleTime); +} + +void initSimulateTim(TimerSimHandle* tsim, int period, double step) +{ + tsim->dtsim.stateDt = 1; + tsim->TPr = period; + tsim->TxCntPlus = step * 2; + tsim->dtsim.DtCntPeriod = (int)(DT / hmcu.sSimSampleTime); +} + +void SimulateMainPWM(TimerSimHandle* tsim, int compare) +{ +#ifdef UNITED_COUNTER + tsim->tcntAuxPrev = tsim->tcntAux; + tsim->tcntAux += tsim->TxCntPlus; +#endif + if (simulateTimAndGetCompare(tsim, compare)) + mcu_simulate_step(); + +#ifdef SIMULATION_MODE_REGULAR_PWM + simulateActionActionQualifierSubmodule(tsim); + simulateDeadBendSubmodule(tsim); + simulateTripZoneSubmodule(tsim); +#endif +} + +void SimulateSimplePWM(TimerSimHandle* tsim, int compare) +{ + simulateTimAndGetCompare(tsim, compare, 0); + simulateActionActionQualifierSubmodule(tsim); + +#ifdef SIMULATION_MODE_REGULAR_PWM + simulateActionActionQualifierSubmodule(tsim); + simulateDeadBendSubmodule(tsim); + simulateTripZoneSubmodule(tsim); +#endif +} + + +int simulateTimAndGetCompare(TimerSimHandle* tsim, int compare) +{ + int interruptflag = 0; + +#ifdef UNITED_COUNTER + tsim->tcntAuxPrev = t1sim.tcntAuxPrev; + tsim->tcntAux = t1sim.tcntAux; +#else + tsim->tcntAuxPrev = tsim->tcntAux; + tsim->tcntAux += tsim->TxCntPlus; +#endif + + if (tsim->tcntAux > tsim->TPr) { + tsim->tcntAux -= tsim->TPr * 2.; + tsim->cmpA = compare; + interruptflag = 1; + } + if ((tsim->tcntAuxPrev < 0) && (tsim->tcntAux >= 0)) { + tsim->cmpA = compare; + interruptflag = 1; + } + tsim->tcnt = fabs(tsim->tcntAux); + return interruptflag; +} + + + + + + + +void convertSVGenTimesToTkLines(XilinkTkPhaseSimHandle *tksim) { + TimerSimHandle* tsim1 = tksim->tsim1; + TimerSimHandle* tsim2 = tksim->tsim2; + //Phase Uni + if ((tsim1->cmpA < tsim1->tcnt) && (tsim2->cmpA < tsim2->tcnt)) { + tsim1->tkLine = 0; + tsim2->tkLine = 1; + } + else if ((tsim1->cmpA > tsim1->tcnt) && (tsim2->cmpA > tsim2->tcnt)) { + tsim1->tkLine = 1; + tsim2->tkLine = 0; + } + else if ((tsim1->cmpA < tsim1->tcnt) && (tsim2->cmpA > tsim2->tcnt)) { + //. . . + tsim1->tkLine = 1; + tsim2->tkLine = 1; + } + else { + tsim1->tkLine = 0; + tsim2->tkLine = 1; + } + +} + + +void xilinxPwm3LevelSimulation(XilinkTkPhaseSimHandle *tksim) { + TimerSimHandle* tsim1 = tksim->tsim1; + TimerSimHandle* tsim2 = tksim->tsim2; + + // + //PhaseA Uni1 + if (tsim1->tkLine == 0 && tsim2->tkLine == 1) { + if ((tsim1->ciA == 0 || tsim1->ciB == 0) && tksim->dtsim.stateDt == stateDtReady) { + tsim2->ciA = 0; + tsim2->ciB = 0; + tksim->dtsim.dtcnt = tksim->dtsim.DtCntPeriod; + tksim->dtsim.stateDt = stateDtWait; + } + if (tksim->dtsim.stateDt == stateDtWait) { + if (tksim->dtsim.dtcnt > 0) + tksim->dtsim.dtcnt--; + else + tksim->dtsim.stateDt = stateDtReady; + } + if (tksim->dtsim.stateDt == stateDtReady) { + tsim1->ciA = 1; + tsim1->ciB = 1; + tsim2->ciA = 0; + tsim2->ciB = 0; + } + } + else if (tsim1->tkLine == 1 && tsim2->tkLine == 0) { + if ((tsim2->ciA == 0 || tsim2->ciB == 0) && tksim->dtsim.stateDt == stateDtReady) { + tsim1->ciA = 0; + tsim1->ciB = 0; + tksim->dtsim.dtcnt = tksim->dtsim.DtCntPeriod; + tksim->dtsim.stateDt = stateDtWait; + } + if (tksim->dtsim.stateDt == stateDtWait) { + if (tksim->dtsim.dtcnt > 0) + tksim->dtsim.dtcnt--; + else + tksim->dtsim.stateDt = stateDtReady; + } + if (tksim->dtsim.stateDt == stateDtReady) { + tsim1->ciA = 0; + tsim1->ciB = 0; + tsim2->ciA = 1; + tsim2->ciB = 1; + } + } + else if (tsim1->tkLine == 0 && tsim2->tkLine == 0) { + if ((tsim1->ciB == 0 || tsim2->ciA == 0) && tksim->dtsim.stateDt == stateDtReady) { + tsim1->ciA = 0; + tsim2->ciB = 0; + tksim->dtsim.dtcnt = tksim->dtsim.DtCntPeriod; + tksim->dtsim.stateDt = stateDtWait; + } + if (tksim->dtsim.stateDt == stateDtWait) { + if (tksim->dtsim.dtcnt > 0) + tksim->dtsim.dtcnt--; + else + tksim->dtsim.stateDt = stateDtReady; + } + if (tksim->dtsim.stateDt == stateDtReady) { + tsim1->ciA = 0; + tsim1->ciB = 1; + tsim2->ciA = 1; + tsim2->ciB = 0; + } + } + else { + tsim1->ciA = 0; + tsim1->ciB = 0; + tsim2->ciA = 0; + tsim2->ciB = 0; + } +} + + +void simulateActionActionQualifierSubmodule(TimerSimHandle* tsim) +{ + // Action-Qualifier Submodule + if (tsim->cmpA > tsim->tcnt) { + tsim->dtsim.pre_ciA = 0; + tsim->dtsim.pre_ciB = 1; + } + else if (tsim->cmpA < tsim->tcnt) { + tsim->dtsim.pre_ciA = 1; + tsim->dtsim.pre_ciB = 0; + } +} + +void simulateDeadBendSubmodule(TimerSimHandle* tsim) +{ + // Dead-Band Submodule + if (tsim->dtsim.stateDt == 1) { + tsim->ciA = tsim->dtsim.pre_ciA; + tsim->ciB = 0; + if (tsim->dtsim.pre_ciA == 1) + tsim->dtsim.dtcnt = tsim->dtsim.DtCntPeriod; + if (tsim->dtsim.dtcnt > 0) + tsim->dtsim.dtcnt--; + else + tsim->dtsim.stateDt = 2; + } + else if (tsim->dtsim.stateDt == 2) { + tsim->ciA = 0; + tsim->ciB = tsim->dtsim.pre_ciB; + if (tsim->dtsim.pre_ciB == 1) + tsim->dtsim.dtcnt = tsim->dtsim.DtCntPeriod; + if (tsim->dtsim.dtcnt > 0) + tsim->dtsim.dtcnt--; + else + tsim->dtsim.stateDt = 1; + } +} + +void simulateTripZoneSubmodule(TimerSimHandle* tsim) +{ + // Trip-Zone Submodule + // ... clear flag for one-shot trip latch + // // ... clear flag for one-shot trip latch + //if (EPwm1Regs.TZCLR.all == 0x0004) { + // EPwm1Regs.TZCLR.all = 0x0000; + // EPwm1Regs.TZFRC.all = 0x0000; + //} // ... forces a one-shot trip event + //if (EPwm1Regs.TZFRC.all == 0x0004) + // ci1A_DT = ci1B_DT = 0; +} \ No newline at end of file diff --git a/Inu/main_matlab/pwm_sim.h b/Inu/xilinx_wrapper/pwm_sim.h similarity index 77% rename from Inu/main_matlab/pwm_sim.h rename to Inu/xilinx_wrapper/pwm_sim.h index 7e3d406..f3b3329 100644 --- a/Inu/main_matlab/pwm_sim.h +++ b/Inu/xilinx_wrapper/pwm_sim.h @@ -4,8 +4,13 @@ #define PWM_SIM + #define UNITED_COUNTER + +#define SIMULATION_MODE_XILINX +//#define SIMULATION_MODE_REGULAR_PWM + // Для моделирования Event Manager // ... Dead-Band Submodule typedef struct @@ -13,9 +18,13 @@ typedef struct int DtCntPeriod; int stateDt; int dtcnt; - int ciA_DT; - int ciB_DT; + int pre_ciA; + int pre_ciB; }DeadBandSimHandle; +enum StateDeadTime { + stateDtWait = 0, + stateDtReady +}; // ... Time-Base Submodule, Counter-Compare Submodule и Event-Trigger Submodule typedef struct { @@ -30,8 +39,20 @@ typedef struct int ciA; int ciB; DeadBandSimHandle dtsim; + int tkLine; + + void *simulatePwm() }TimerSimHandle; + +// ... Time-Base Submodule, Counter-Compare Submodule и Event-Trigger Submodule +typedef struct +{ + TimerSimHandle* tsim1; + TimerSimHandle* tsim2; + DeadBandSimHandle dtsim; +}XilinkTkPhaseSimHandle; + extern TimerSimHandle t1sim; extern TimerSimHandle t2sim; extern TimerSimHandle t3sim; diff --git a/allmex.m b/allmex.m index 13c6373..13310d4 100644 --- a/allmex.m +++ b/allmex.m @@ -1,25 +1,15 @@ clear, clc % S-function -currFolder = cd; -%cd([currFolder, '\Inu']); delete("wrapper_inu.mexw64") delete("*.mexw64.pdb") -status=system('run_mex.bat debug') +status=system('Inu\run_mex.bat debug') + + if status==0 + beep else - error('Error!') + error('Error!'); end -% status=system('run_mex.bat wrapper_inu_2.c') -% if status==0 -% else -% error('Error!') -% end - -%mex -D"ML" -D"__IQMATHLIB_H_INCLUDED__" -D"_MATLAB_SIMULATOR" -I"..\device_support_ml\include" -I"." -I".\Inu" -I".\Inu\Src\main" -I".\Inu\Src\VectorControl" -I".\Inu\Src\main_matlab" -I".\Inu\Src\myLibs" -I".\Inu\Src\myXilinx" -outdir "." .\Inu\wrapper_inu_1.c .\Inu\controller.c .\Inu\init28335.c .\Inu\detcoeff.c .\Inu\isr.c .\Inu\main.c .\Inu\param.c .\Inu\upr.c .\Inu\Src\main_matlab\IQmathLib_matlab.c .\Inu\Src\main_matlab\main_matlab.c .\Inu\Src\main_matlab\adc_tools_matlab.c .\Inu\Src\myLibs\modbus_read_table.c .\Inu\Src\myLibs\my_filter.c .\Inu\Src\myLibs\pid_reg3.c .\Inu\Src\myLibs\svgen_dq_v2.c .\Inu\Src\myLibs\rmp_cntl_my1.c .\Inu\Src\myLibs\modbus_table.c .\Inu\Src\myLibs\mathlib.c .\Inu\Src\main_matlab\v_pwm24_matlab.c .\Inu\Src\main_matlab\xp_write_xpwm_time_matlab.c .\Inu\Src\main_matlab\errors_matlab.c .\Inu\Src\main\detect_overload.c .\Inu\Src\main\rotation_speed.c .\Inu\Src\main\global_time.c .\Inu\Src\main\PWMTools.c .\Inu\Src\VectorControl\pwm_vector_regul.c .\Inu\Src\VectorControl\abc_to_dq.c .\Inu\Src\VectorControl\regul_power.c .\Inu\Src\VectorControl\regul_turns.c .\Inu\Src\VectorControl\teta_calc.c .\Inu\Src\VectorControl\dq_to_alphabeta_cos.c ..\device_support_ml\source\C28x_FPU_FastRTS.obj ..\device_support_ml\source\DSP2833x_GlobalVariableDefs.obj -%mex -D"ML" -D"__IQMATHLIB_H_INCLUDED__" -D"_MATLAB_SIMULATOR" -I"..\device_support_ml\include" -I"." -I".\Inu" -I".\Inu\Src\main" -I".\Inu\Src\VectorControl" -I".\Inu\Src\main_matlab" -I".\Inu\Src\myLibs" -I".\Inu\Src\myXilinx" -outdir "." .\Inu\wrapper_inu_2.c .\Inu\controller.c .\Inu\init28335.c .\Inu\detcoeff.c .\Inu\isr.c .\Inu\main.c .\Inu\param.c .\Inu\upr.c .\Inu\Src\main_matlab\IQmathLib_matlab.c .\Inu\Src\main_matlab\main_matlab.c .\Inu\Src\main_matlab\adc_tools_matlab.c .\Inu\Src\myLibs\modbus_read_table.c .\Inu\Src\myLibs\my_filter.c .\Inu\Src\myLibs\pid_reg3.c .\Inu\Src\myLibs\svgen_dq_v2.c .\Inu\Src\myLibs\rmp_cntl_my1.c .\Inu\Src\myLibs\modbus_table.c .\Inu\Src\myLibs\mathlib.c .\Inu\Src\main_matlab\v_pwm24_matlab.c .\Inu\Src\main_matlab\xp_write_xpwm_time_matlab.c .\Inu\Src\main_matlab\errors_matlab.c .\Inu\Src\main\detect_overload.c .\Inu\Src\main\rotation_speed.c .\Inu\Src\main\global_time.c .\Inu\Src\main\PWMTools.c .\Inu\Src\VectorControl\pwm_vector_regul.c .\Inu\Src\VectorControl\abc_to_dq.c .\Inu\Src\VectorControl\regul_power.c .\Inu\Src\VectorControl\regul_turns.c .\Inu\Src\VectorControl\teta_calc.c .\Inu\Src\VectorControl\dq_to_alphabeta_cos.c ..\device_support_ml\source\C28x_FPU_FastRTS.obj ..\device_support_ml\source\DSP2833x_GlobalVariableDefs.obj - - -clear currFolder diff --git a/inu_23550.slx b/inu_23550.slx index 17a642446cbd711b64c6f4dd5682a1fb45e36601..29739dea5e7bf4343612242a116e7b815795f4f8 100644 GIT binary patch delta 16262 zcmZ9z1B_=+&^9`@ZQHhO+qUiBXvaIYZQHiZ9b?Bf@BZI6-~Dp$NvD(Qs#B-Ca=Md# zs-EE@@VsL1`Wz^*IiVeO zc_4@9UD>Wy4O74FzX5m%X;-&?NM_|M|^AKbunS= zHvI)wLl(5ks>XDGv{gf%Q1sBUn~Mq!ia2js-*pjTiNOEAtrm5hnU z&CzNL#$HALQunpf)gn!)ST#mKM>bVUrLj#>PZ(;vuD$I%XyHsTlpN5r!T484@d9uK zylD&g9M|13FVnbwm-)Qy`fXTeC#FxsCNB9&D`F)1g$y9|4;r-6jzLK)U~E4XEt7t! zG=A*K8St9LTZ{>RYYB3EfHX0|2ql0s*BiC_Cb?%(0OTDHIgon(YGQRRkx)k^W3*zd z7ZZjTYLRU`1QQOWN04KqY*TzamC#M4qO?eF&X_QAaN+f2c(cZ(uZ)GebiIORuCA=` zkjUcE=P$N6uWa+YyL_-t7RHT?RBpd-uXRaWbC_D9(0i!tPVv4jwtL{)cklW~9BMe` zCr_!o17`BvU3VGW^+!ua?tn|^4sD~Tl?}TLr=_F3gNqJK8n{BWQ_WxVgO1Jhy}fQ> zf7(Ob**wwdpM5P?6O6ggx%Z)5MUjf&NX=V>NS4&3$hn6$h=rgUZ1uLee!Pxx<^|AH zKF_Z?`fg;wXB-=!s$M%Jf5@@^UYv}|BD!*a0o-~C?oBx6%tcU&L;EAJ@$M7Ti8ILg zz#RtSiR%JyHnYZL95Xvz072A#P2uXF!(6Y|X5d?xDsO)G*!TNdqKT@=Dht>qV3bm& z)Z)`7C5_p%=IUsKQ@ z0Rn3^efD{Ws7!p9tqQz`S{?SFX=M+4N)sqiwDm51@cx3?3Sflc`9jG|10q9O9t6LW zbh|WrXe<(cO1iLA)<9$#poDcIg!~y?$=p*5*bu>mX6e_?#iSKwRum0C5Qp*V!hx`% zmx~h%mMsFYI3SKQLzQ%-2UYA)Q+|Vl0+32`j{W{fCF)Gy4-@66VC2(*DIrnK)bGK7 zE>H-f?yxSNrYWITii1exizV{qQ@~IwKquythqBO38O3x)N!rCs3mDBdKAfOXpm(qF z_1aK;r1hhbN$tx0UODcX9%d6<+v#+9e19x!kYvPRS(#E}g%eDZ`3VedGvj!<04&g^ zgs>-@2DLMzFe6)tMKV0GDXK7})K_G>(E&|WRt|}-@UQCI6df)ru5EQvBgt}&kn9)Z z*37azZH_-LwXVKgC4@bApZ|qZ+tG%UckW2Bzj(BSGRG~e)2qV06$08v^X%dCRv_k) zaPVNN%D6b(PBP&wMKq2A^~vuP1jq*+0>4G}Jk>%$y8|#4x5o~m5YK(8HVw$H2o>!g zJ2nj}KzWKnd~W)})#hACe(2m=7Z?S8}1i_kJt6}Ql^aAz`2v3E6@>m%f$^3? z*S2RZ4}Vr?jkolz^%Ei3XeMn3efl@{+=<$P7HU;`RxC4TgG9ECeyCONh|~6;P(Flc zX6n;;r*brTT0bo2InrwJ&XFI|y=*O0j1a1i_Y0SB2|o@)vM!2&Fq#8n@qos~3J#>M z8au%E$nCC1LE<8wL}yKRZR457!o~v5Ec!GV{4Qj$mmE`*VR0E0;Mtw*um&V2CHG0R zd&{ou^-E2(!b4&p8VFf_AeY;u&<;;pNBjA|wa zpAG-s{dym&n=j!ndPsklZ2*{ezwC7-LA!--W=3LPHiKl@nArJ(;7ak6%FP<&8kkP{g@1&)1>4Kx)3QK|)BYrf;N_~l_x(!PA^%~Q~sc3v(9boO} zCz?Kkik8o8-eT(-sn?(uq(ho6yE4G)nmmiARU*#Ou(R)Q$h~1vQw+Qr#VLaGI>)dz zcxnc&J0m|zvY=pSKtMoHKuz9tykamvCb+~Th(JIxNofs)zywKu9%A z#r8;T+I!a+K?h6~^^Hd<$E7`nfZ}paS<$G(E4#Kb5_r6<#c94wORnsKqclN(?XXSD z#Oe?(A9i8J$4H4naX(Bk#5#(s$AEK%03PpWmv8r+#N!E8d0+2}o`(>~hG@FM~HIPaWNX{RpfiFf&Ix-U0{ zpBMrP${*laPvk!o0ryXt;e?edutHVd-O?CjOZF~}1~rUd!-%3N2Kw1K6amezVskEf z-4zR-?BA`Ya@jBZ2<>tvIHZfJ0QuPiUXROe9Gt#?JU6xJ4S2lx<0aV+d=;+kHU!9NUYhBQbJ?f_ zeps1EI4OXm(q0#Z0xrpjot*lR@7y#yw;Jb~{Ki$#21(BNq==Ng*{h4rcbu%0_tB7Z z1>n()O&V+>LA7+^piO3B{?I}Xy*9Mn^$lCg4b2`M?L$a`ol&k0@U+7Z5`D>xh2GT= zA_$ktxxo|7**6XrYxX$y6v?}6Y*PEua8hZ%VPrhUMfddLOI1Y`iZ+J!k^guR49t67F8C%3qTJfP!^C#`=K zujo>O<{GW~HP_f_+??X^kA$*su{P<tJ(z!12jDMY%zGW1XsKSPRaIGRuGT`1z!X zn`aVy9lIP+u(iA3ZiJ3tiuoWrD@oREy5-UwT)Ge11e;<6m`5Ls3h=d zX;E!{5d_L&u}TCaiuwe_-ha(a*DYgAcYWqs<`Jhz^z zb-yhf2JT;FRU!8El237KcW;Qnu13EKC6Ygp83?akpPA_BE~;wSDlZK9VN#v>)2UXi zZ?+S-J<`cYqrC0shiE6+0i;i%lg%yV!>D@X#{UNLedE2g5qF?^BrpiRU2si!G@S5@ z4+t5Q%`y6S>-{={_3C}2eltb&dmfTdwOQ+BIQCGlPT>SVIN@%Pw;`vx;#QH>khV!W z#QnJ*xUzZs9T>c;@P}i4GT3!%o)-g?hP*yu>p%U=UaPh|Ciy8O~7br9Z-XI}6IR2uV`l-ehw z0OnR+`*m85PD;s_GGsbbN9#DZk3r|aPWUploLDvtM+9RY=W5TqfRXS%{j~~R+9B8q z)Jg-kop3d<_(1Qk8TzQcfnUI59E+E~`<m%pGN%j7lnM7JZ zd(DC2ItA4KH5rMehbT|_we7E=!O!dUyFlG`%obliqjard6x!3BxwGd_@hn!@m2;=32s&>7R~w)F)_gMtLXtDmKKr|gw>oG@ z(TakN{s&E-gtlEA7lWF(4JK7HBfoB!JUWuE3ohg;FRI)1!El(9PQATlnF|XP z*7ltM-ZO%2ZUgf9mE?RYiF*S!ulU9-G7#`9l*Q$v>TPgU_xUjP)Py1DSZaM+`8J>8 z!8GN6vDD6R3EOgUAh$DMJtPWx#d~z)&5JGxcDfI?8f<@MK3npYJ5zCU}|PuTCdmLs~gQN&7Q{36IMVp4105WEXC-h zzC`T{9qb9FuEo3kd`A)D6TK$W=4&SiKF2wi$9Wf_Dv2Pbe=$}CeQk5)AeN z!cI$w!jLglX!OPZ6A1Ud$Ikw*L}h+L;+7v{{n1 zZ4y(2P0%+G4drc>1WJq2g}PeXnvtc2-I_x`vo;=SMQ9Yp)$j0E3UTKotl3yQcY18D zyHz@xa0MYscDe}6@#)fj=`K^ zy7xf-?e4AIdxIPEc+(>j17-mLHX%H=ZF2`}9+~@?fO|cv@aZG&&I%I5mt;tD_YOW; z@!So?T+-qW;xccB*>h?IF}G(oS9EU=MQ@RN?pUcm=6GVC7}S2nXM#4Jr@92^KHT*N zI3MZ(^af(DGunkxz^^(LG1iuC_1c0qZatc(zs~YhTYC)3bHe}pod}Ks)ap+5bqY~VYJu>>co%<88rKN^t1Sr#E_MdN?HT`Hgj!0LBf9iP$7G%lmi zn*P20B~x&j`r&`fY0n34z(Ut^rULXe0i6mAQANz`yw;3`C?CxWuGnTxE;s(WN75M4 zj%G#r=xEqdZLvg-=I=s*Y_bJh&d@0%~K50nPTL1j>sQ*KwEa>5UfiwXY6B-U8 z>xz+cM-+We=!DEZKx3xm!=Y0uHOQbgx}0PD`qTz)LmKmxmNBan>Y^}(A=tz~Q9qQN zX>Oj^y}-I(z#Vy|s`036%FxNRr!9+NaFJ3L!0X?ZF@IGWT~DX;7A=-$!O)n+P#EPo zV>sNI{N0GDuLm8s+2IeHX7OansgLGG7RyM^@C9>%HF**Zh?LM$u07H8wg`r3Zme%N z@8|kcg4}LmbM`Cjl>8*VJ*6OKIZy-LgIw<1s@AKMRV?3&hl6JmF;`$*WFmX(Do6cC z_yiWzCvhEh6n+i0bwQ@5RHpfC&3Y)8$*(=Pt4rn@DJ689rnNk&3l?X7dpOtY{l;d@ z+e(TQ?;T+lVDr5m4LE;B>LFuZ)`%)V`DG@oZ$0UYDoO3j&izISEttfn^=fZzH_!LV z2<%DOfCwPBQyo1bGufR%@Czrwec)?C$F$P-vX`=3ny^a%-#(M8tOa8I0e-~3cJHI zq^EXHdZ!tKBW9>hC!4LtF9ycAH&8#t+%LrcZq%uYz&LGZAi!Y=Fu-Q|&l(UwK$>)E zdn~{N02WRT4hAoKyDJ@g$1M(|?_5LT@_vDlhH_8)*l3E1XdrFNVFQ?EDi|FLl)}bz z8JES&wHqq!MibTIz*&}=w`{p;^dJs9v)`5-^0XNi?7#e zd6sSXhJY=>^WV8kG1KponX9-qgm*F7=bCiG0PN<)4?~{shS49=wZEF6Z-&0x`>+>< zA+{(MMvmE7Z+)in9&J026FWa-``JYX_1L9>GY*cLGa7cIbBJO$hVf^7WL2ek*jeyS z=?!8xq(j=E#ZBH~hc`uU#ly6ODW#Ukuzj{~DWEy$Ye%VMLc|N0IobPOn;nI$nF(i( z0LOsdkHaPYKQ@He@&gub-p-X-xff^MT{zG2ltezQo;-&1Qw+r=2mbjYop|{# zG*DH?k`%n}^v8z$G#NRiIR#a)@;gq_&n03QOQ zY36TfmZguhJAP@a(rpU6(wY3J^7Fb1=LC~tcirn%?kWGHkS_6yDK1j!n4%gSL}w7h zf{@=00~;~J8%)G;$$UL=cVr~0O8Rsh0ikZ6T%R;P+zK7TaEs33j;e!QU%_-!0EtX! zP1UA78|bc1V!Z}$ChAVFefhesOyC}e+&W$)TAGz}(nHOas8knmm7dP9!=Cde^|~MV zp3Qt^Mc2J^TL!%l(F=E>B({qqorzXnCViV~6%6@R2M(-3Ts0iRm2VyKNE zqOY3`)V?Z46*3$S#TfS@;26sVz*tx4ffV^&I2$FIXH`CkQYU+2Y*d*`j2cBx24xDPup9udSi>01p^1K=eB!cqwhWK~?wl-NvIj3cuJ$lIi#|Jjeeewu6jP zDJdi-3g2)b)=27QFc!+n@^T%Ww|CFYwB%AvT~fylsS)fV<_j&yUj>z}B0=+r&-s(a zQBQmaD$!riyGd5ve{XD$TqeNK#dM;b`N28^fXB58B03vw#F#@0crp!`SF4t2b#CEk zqVX3mX_DHnp(XO?GF_y_;M`(o-+C6Yf({7WARZl=2 zpF~$9GAMp!0+G*+!KZ|(9%^uv1?R2KSy0_dgF)ywXbhq8iBBff5`hWrC?h}0ebZL( z2r}9=xJz1l06O6j7g%xUdjrAU2&eh$OE#*HLYj5%Vse>?r79e=H#Hof5ywk*f@5ix zz2}AO(oqvurt{@2ol2Xf$Y&4&GK-b>DHbl=0+XhN0Zari({NU^sO-zte)AZqzxNLq zQ#Z@a&o>OCBiJ?#&`5|!YCZFOfTGkLZ7yC%s;1fmKc+JgiAo2{1NFpNa=B)@yfQst zjRxm7e;_0qM_@DFL_hT?L)MlhX8WEtX^K$7MoAn6Q!U9qvBz6N|1B*#S4vbQDGkNQ zH|ITcvT*|l-qr}{+++%b&KsqJT6-PsLnNK^BJ z6I9!5Jy^qb(%zxHK}V0!--4O<;q@7j3g{k4KU&%q_oaFTfAxdRshx5t^Bf$<`yu2FZA}5b z(iNHot&uu=Un)W0eI$!^<_SA?Uezrz^q;C{`=0Z9@cx7$=UfXeHK+aE%wR34j$v2V1MaP4d>rDopHkH{AR ze)5N~=Vl57-SrsU-CL+yiCmq1MR`BM-Pmv!#TOWR1^HdV5(wR%frn+GJC`6Ga=uQBkpabhaIuJn?)m0f_}>b|L9T)kBBfOGD@vtd zi-+?wYH`pOli{5+0?fwe7r>}cJ8$<4SjrlBg}S5nOP)=q78sP)ZtUNTn%@R`VV&k6 z+68uYyCq5#j1eixk!gri(PpID2}b`Gr-K1)A#PG36(y#D7OB-Og@p1fEvG>N1TZ%T ziCCXfgk1af{H|H={Ur`j-5+5dMUIL|utM|c>mv@j`ej`jNXiLSWRP8w2YA1GA4%Yi z)VfCbMWm}SQaFhI2>jSyfs!g6cu*HctqN&X5R3Z4;uO3U2i@uZY;0(V!E@h36?y2s z$Ac0DzD+ugfp9l(tgmgFZ()V(<9$-eV1n+TkMRzm>dP zc^J@pT!$qMS6?eY<(5|3|9Zj9Q#f-I3YTPc^t$X=0EaKl`7*u9gm5U<2me;fYNYxr zUhBgRtg!j2pIZYeI7{k?PQN4YoNmKsxp;o;N@)~C7rbfRG%c|lJq!9?29WLh*>Yu| zbW?NC8%agg)Ke*-{C>fugcCQx9GdMdSRz0XF!Sz0>K(O>X_0(k)X4~no4v)wJj6)J zkYC+?suSr@rYMUh$W#10<%f6Fl2*;S$ZmuLK~QqFCHo=^n$ zk&BB^eQ2(hF$4jsFwPVb0-#O=fmofmU*ep-eR1BK>s|;F+#mf)RiOBO86Cf0jr)YP zoZd`v<^F_7179DCb2v5VMCI(fx{RazHjkyBc`W>EWh?!-2+AC$^JjJ+ON*X6QwLXR zmRi5eq!GjPv{Q`PqZ%I6g%uU>%Vi`T1l9Q_Dq~TDJj^?C;Jg&51F#a;cSb7%$FYDG zc3}rY28l?5Rfx`JdU01D!44$=lynHm!kM3j2$wmab2pTGF{)L8JBrV8^(bcg80-N? zX2QRT%r@t{q#)4R22aw&gC`CrVNbjU7v$p80&Rs4(5=#8DFVfA(b=$y>6Y9_=a*p$ zHI&T?y0}zkBCJFr0yObOx){}FqYt4sM_QoLTGr4*9vi^ZT`12>aT6{nf$&L%af*#) z^=IAbguBeAUr4N0p==8|^rLPIRe)P|ZVXF#&P146bTY$jy!NG0jU6G3VWs%0ut9{8 zmoNCP&AeD=u~_PZPrx@}r=<&hBd4v$$Nim`kf`5{ofD{T0Pv%AM%jr$(h0uE%TTvtd@*sY`n?4B|m}CWmJh<_`>ZJ(AA<} z%f%y}T*|wZcF$C+a2*|IMXU32at8(NX)|M?lkXgnC z+7;4{N%i>|i@f90qmq{B)30*>Beh&|kXwFhV&_3bOMU#g;Fp#*D#t)B# z9k66ow2ALXasZP)Nf3t=0Fvfz@cR5+3xw%EuO)_T*Kp@*9G;Sk{R|+%rEg0t`L=z{@t|HP%SctX63ps{_n4jfOpLIChSH2hj8NIB5$qNm(9CegZm^Yqj45KxO@Z9)K1Fw5mw?STV0Kk#)id}Fnsh&rvY z0Z>G!CPNm^=FY|1CL|iqYLFZY-s2`dm+Vbg%*`1{bkG}gke4Hu-kHc6`DCVrD9Ir6 zYu&nHNiK>XV{=KTBmMZkvt$k_f^kJIqI}N$0u}zEA>-!mr*L&mP~QxmY3=It!?SQk zt3fGt`r0@9P4hC(eLwOc0-KNXQG zK8e|t`(EUd&3C&7*NmI;n9Q?J7cn5xPp~-RRl=E%Fkqg!`rzZDjjVEz(K10mR60|Y zpw(%SRh^kCP0ETgwlhN~16|Yq?^I;F(?;h0O;=6gFj|p@&5rz&Y}hJw|7?JSbinl(+~&L;^lE1_uh89f`$; z$ZT0?{C^b8MGz(u@c$e@C^6fNl()qwYmO<4BvKhUp)EomXBmit0PyZy%VkJ^hcyvB zf*ObR5xfR4x$uw5HU8~O5Lk^gKILF`Gtw>{1%)%Zkg}9tgV0`krSp23<~D8dPkqVd zI$riV7OCzq2g%D8JWC;zLzK~`bAbz+{ka2uVGNt+o<`4t!h9WvR3G|u-hV=!^*L;Z zjx1Vm_`5&v#aBIfhdf__~+QIo)2UN406`u>c~Majp`$O7Y!$s#l8 zTU<%RS9^oO8<O3z29x5db zlULxL3&9f9&igr%SnGaUuE)UjtO2q!j(H!ZXyzTe@KyJ<@NS z;A*0oQ}n8@=%)V$_0pY`uT<+hfO*rA@<7^V7+%wz+@$nG)3+pz?2UYgMgGiP^S>W@ z=y5TNoDLKg#j#_*{S_gjR!48|550DTZA0v1`2(-L-@E@?Fa}r9@def(`&YQqGtvaX z4)>=0Z9Nt5D}d`V-=))T_DtpKA4=nU@ve;X=HlUvEsCLQf@dJ9M=cKM56;n=I0461 zyBQm?ya8L;#KB~fuar%I)tYqGaEk&3@XB^aV&0Ne=Tr2)X2=e2}k=e`{)pW2_B zvobU+9H%I;8Gz^ngDier8(h7c;HWohRzq-|eGqpr3WQH^HzJO1*uE9$x*uU@z(MA;gViYU1 zob{6P@6jX~#0t1vyrEuRpCOU(2YRiu%K6f3a4wPYH~puEk9IJmIb60hkDj3sKhXA{ z2=VuOSfj;hwJuwyMI3%mt|d-G;Xz-34~t=P0BAh?I&Lb3hzyk;jP^Iz{L ztcBB3`mFcef4~&8bpaUoECgrHj0U~ZV6$s@rHR(8j5^u7y_Hu|=;Yf3yEYgrs|?Zm zJ3Vjss4L+%wYS&gkX82~*&T$gBhqgkIS*bbzKR_$EFV8u+rCIo$Phl>R^-k%%%W0( z0B6UC4)|@~!9SG=aX4>$Bp@|^e9dE2uK9l{z~r=o=OmwoL{}aLw7s+&9`C4H91YAw zxZLpjyci(r=*;y$GjzNj@Y;0gxFk8=qn7rGa`pMOxtQ-p(L3?SXa8i;8R@kj>AS_1 zcPdJD{BT@->jt=od-e-g5A~XhcJ1^A04QSL`#A=BuFlgeg^1{73MYEtoLdVG)C(iJhb&9Of`s!122f@NGmlW*Z7UEWKOQXcVAn9Sx-1PXPK6cJ#$Kyko>((F0F1)M zBqVx(Ay}oSp_0uxTjc$@(02jw-bS z<_3~dLKgz$i4J#dCt&|dcw>x3B$~x6#&HH|*TJGqlso%FB|cp+pnIq|3piK!e(i02irB%?s(;IPZ%lW&L!MVyK z0Ca79CqnZ%^$f5RRgQ03ihmf!zIjeu*5AqKgJ6N(O4MGb?6fzTxzwExd!1cIw^35o zGF;w6hJg=!?-alE87nbaf(R1|Fa=PeXcYg1O2$MLzjq0%cuKpm zhT=qM?ghP}bh9&7ZAm|ZhW(lx1EG0TJ#CX8>sh}196Yu$>NT*Rv7J}@tC?fy{vCe) zImoVM)C*-hg`vT}Z}2>%)m;BrlD%$k*(G1xJ@AbF5U-YfYx8wNg9IjDyiH zifZjSNy|i3>1wJ(r#D;8(2Z20G(sKo%&6FDMMtO!qmT*JF4K*KAq7dHQwSGMDKp6~ zSfv-LgUc|-)`>tz7l;89?tsN|b}IxRmjO89ZyP z>Y23KEPI*jYj~1&r7$?LM>Rl|Y>(Td8L@MeK*Mw@GY^!0VQ7k~X66f>jey}IkV`%7 zjd0K`csgL+pR9ze&sQ9*Auo*YMN8d-IF(^SZv#|*N)3jjW?24NoncogA99J zc?Y0%buVt%;Oa&)Qldi4WrUPSUfw_nsW2^MHBG+MQ))(}x;AJ3`nW^v|sko?23{^0&Eko7_M zUj`?QNcmq1=Y$1#9|{_>+*yqe4hSeeZJrmH6oAW%5-?ZibXSi^=@q}+*%+$I0x^hT z=L9uDq5H8ng@qRrZi{WKWpcG^b*-Mi77wwg z3IO0>9OATXQK%f(A%FYIk0n{7;r%+XN=D;7(>HkW4T;XzP)wuDh%UAmM~gP6C{YDb zK5{0fc4NA~a=xMHu6nbmAc_Y$-) z#uh0%R=Qq2oH*HkFkQxCYQ8UD#LN-Oyg-;D*>2Gk{c1D5x%A6!do-vML=C|ce6d{5Som~ff?29`v?nvovj4s9sQG1ug~z6Fdd6Kj4wj&IUR;~zshzLsA&xY+lves#wdzpA(p?@+ z*N392{pw^fZTkrDkElf$%Bs&J-)FM%P{`!m7xtGBD$VTL9o;wL%LMp!qy`gcTW>s~ z@~VHyqUm3=|HD#UQM`zL?Q6)?0I)q1amP{aYq!L;7;cF&rvkAV;FBO?Tu{Cu>rudU zPtq4OXs+Z+T_~@dka{Vn5Ui|VIRgWtX)&lAu$EGxN!zvRfKGuzHTu~wMPw+;&@)DE zNZQZxa2+VuY8I}ntm~mC)Q!8b@xFt-4p)PWjOeAx+NYO^Q5}iY_mE&^03oSVGx=v# z{aQvTN~t%m)v(CY_PVBabyw*hj%1N z8VjYsm>R_RQHBGl!YaKOtF#(U=e_Z++jF2~vl{5q8L_vLJmhfrXSA&S7NApDCM@In zcs(w&3zrC%Ad6PlTA!0s{WNi(PcRU@M88<6(MhJqD|~^#_eM2Q0S*#)xaq=kz|cX+dicU|syurj~e z+FPZ1lhyLIk;fxfLbDG&l3edd>S>V})rB&EVr(#aB8|mL>iz$45x0H8VGc^a6v> z!DEcFWX#?p@?xrz0MSU)-$Q73glYpNjv~-N{{=$xuM7K~SpnAwO+r!OMtcfT`yf@& zL&dQv0VlAh3Uutis1`7rIB52zENCorm>{((g?LI`5SAuKyKHzNYEs}!vr`a;46P-b zhnkK7kked32p}Wk1tm~dV3WF#-pUPLu{i@&lA1YA*)n>OQ9RcQe7-mwx8S(^f>4dZ{s!%ScO_ZpQ0FD%A5r=umLWV zmFuf%a3l<#%tv~*Wgtr;ZA$RidM^Rb+Bk}?=C;K3pbsL`u2mjz4$fEWZDgIW3Ou_) zuahgqT*MCGG&Rj)ozKV%WP|nND-Pf(e<98YE%`w{Xn|Ns=K*=YvY^W%3QDBx@8PWy}Z(Lx_Q8D|8A<5%_-ca%}3!;8*x+W1Ph8$3JAbZ+O zsWx&HwSU_)BFH!p9*$E6i@b!ic#(k;d7c48X|z!P7ei9sF zp)3P3-Y+*PZJ-#BQJ1U?k|_m(Q?sIM@NPOQ^TX!iQgBr8r~)sD>S-}%OIYT{_}h@x zb<`n1jF~5#oU7%+1F&gwBrQ+}`b<5k9Y-{4BT920(3~g7d?G1adqW_Y?wA|2c061E z?^V5gLF$&p>^ktx#cQ*CRm@}h(DY9B3n|wEJNAcbsdr9iuhj62mPp6L^S49;3E}KP zup*|>RH3D2A~1OmM;BvLan+-ZDdLUxcp_84i|9uL%ynMj2$o5E6fq`8#=0L%miccA z4bo7whO&G=6fNMrbQ75lmJeRn3CX%5c_R2k+Wv`fut#|=La|gvHqg!K+7h~WKcuQm z@3~pfPp*3B;$ay0AP-eDq+D#rCRb#%&_GvmuB!!=VYf<ZIS}Yd~SLdgJ%JUxJr61lxk*oox$cq~a~HynJlpjjJ=lZ7r!qSO2& zZ=%E$%MgTod3m2Hmu_en>8Hopauz^+N*#D6q>iLFory8H6-Kqiw>}kB|Jjr%v~^=+ zw)B3r`5*KMZ#HMp_xQBT1{B2r*UrW>|DT}VA1cB zWO{Wh{1((;X}Tg~k11B?ub(JoSCmj^(2BCd;`i^69Vb@sMsP@e#~kX7Nr zIyeR+U9?c4p)yEW86G7-BW+_USN7Xy#F>DL2?6;p)s(odrF_0#rA1JmTNwJx66u>C zbUpoV28IP=D6$#p9Xy~bm?c1@Y>iCu@x#QJKnOfnF(VyklJH6`P_G6Xb=+*VlnTKi zByr61A;wO0h^n`_)BSjMLZy&~+~PN8?5A!4oux@QDJZO*ZOr&4$)|TkvsNvH)s)yY zBo`}ObHQ)4@x@yL6;#p=4jHQlw!K!4gC;BP|Qdmc;J>G-!^&{rRES4u{%N7pzsz6Wm0kbfX44luqgwtf7v6DoPFJ?y zeiG8N;_CQ;A&stA!*~M_pC``PSXYsAZ%wl%CU~-Lh8(!yAg}71)-J*`V(k&k;+|*} zH9mW_bYo&hs5ZZEcE8(WM*%mZkPJE5lBmnv0UQz&h*=X`US9YVd8?ReM1Z`u*Dm3o z)*akf$3x_d$`$)(Nzaw<7n-WSyO{f~Wx8L3ud`ZR<8E0;i=qIw;Q=EzbQPoN-xnX5 zDSg%NF5>|RKd-rP->`$09K(W@a+1e6F$+MfEc^zzs6I8kK2LX+ogSyYU!N9@qPsR@ zq&^=h4`GIr#;y8~Q966*25DhV9$2CkN?&XXPv?3 z#aqNB!*T4Me7ldrAAxe@e8FAL@t&3XdxbDbWBF$@V^{PGx1|B#d#;=N&e$Mm@Ug9W zK8LhaFo>iJ^-3YPy|b$xK6jbWDp5Z~J-Q>9BQARJ_u3XG;UeQ>0 zQHO4%#dB=stvLQ;{c}&$tzk&C9=h$2)#BAcPp{@&x>&=rwX%|mTD`%ZgS6F!`bp(s zYkFS3@B`XLBL%z%{6B2)HUt^q0O0>+D5oXJ0_y>zrCrMcO94BkamoSH;Qq&zbOHUx z)`9^7!v4qF`tJe$pMEUOPYzfD#HlVRx(+*S`=2pzSsIi)uo!TAn!G%)B#6sp((g;+ zv^M#Fn)S2;d0=Va*EBi>U_;=xv={|oX1xE)`~m_Z{2!nH$9LI7QV=9+T0Al^WZJ_& zQxFKyw1IyWZfQb_{{&$0{{-B$*na}>W?H`@usldB{C{KkX>>}!=y?CP0uBg>=>Ow5 z8~J}<^-}uhhY9O{g;6D7OuYYF&hY=A%nadwW;khcg#TuPqYO*~Lc;pLBSy-=Sa|>M aT}=%L2^+aPyR@=61thR02Ht*i&qk{4GUu#HDk+SS zR}4N>1YVy51@>)`GLu{YMGY`i{6~Z~y!%Rxv!g3>S(+3}-bu&o96662>nL^0VG;!4 z1nBK)w!PHDAiV7mLtSmn_@)>1vH3Wg?DNW>NRU4ze6%JUabsiqZGz6Vl za0@~AGH{x8w#U-!A+@20o!8aaZMB1g2`T^s-{6%)2?l7Be-{KtQ+A#}Uk}iDB0KQ# zcX&3Id35PFPU12gQNH=#zb){wr*`J101ZJXFU3UoYT^1CO`s{o-B+lQ+5^$hfptU- zvOi(hMG7~$e(l6>}9{)WeH1+f7edbFYY0OaT4)ZC9Rd+eX+)vg- ziRuFNwF=IOEW1~u{mo*;9ifgKw~}_rJYmDlJ-USJ1j1--G~kwya$&cty)D78PD|p4 zw0u}6%3&g^GSBkq5y0<9C9G}&uU`%l!>dLQ8VDt&lRVI z7iy8MpMpC^MQSAxFW4{C-Htto%Pdh?XB9GXaN$iKPjQ3R3J9)b4cFDe_S!R`HV%ys zvX{2>cJ#^h?tBME9l3fpz+|<5d+b(R`&`K-4QpgTy5IJC2k;9s8FwG>T6N|$dhM=y6PEwFHfn&|S^aqh@xc1w8e{;L$wIpFv`8m2> z?YpwM?#xqZSpojjGfd){| z_-6q;XU?hXeYYIC?;EO@D1cJS9_I^`TWIU_OFVGXKY!dn-h_ftln2571T`GU#U|^n zoeQJ04*#whX6K(a2CuL;4uOyr(+r+1^@%C3McBDlhNNu5W;)GQ9k6(h&9jp=-^=dEWWLazlPICBi)0B++?6wW>|7 zXNAVE&3amttKUgL@w?ky{wc*%-@8Sb*z0jvcylGomQe1rkQBiy{8KxZuZ-A7%%ROe zgOxM9%X0cf>UQz(=NZtmQzigHAwDs0UG#|@ZRuB^&yz5kcDw4b$Hq6g`4?bQ#JKz- zfj@_h-`w?PDD_Oo`Ji~Dfd*cnrIN351EfI#tW2p);hK7^;$qj-mxSxD?m%{^qC_mH zW9JpgOhH)M>i^VN!13VFG5{IwIebs=;pcsAIDVcjvzM5gYj}Q4sMLg>*AEAFGVGY2 zyk12GaNOiT`_45aM$&D7zxzb?RafI`-NbfMEoy87{)aTTlrkMk0-t142YuLhv8`to{WwLh?2-Pf}b7uYd zaj#k}a4=Oy^3(o>oZ^A4@!(!0Onwix!NEQ_a&3Y>p_tK9GJQZI^}uqt*c1{I7NFq9 zM~Q1Lm5xLt#n29ElJj^2z=cW1T5t>zAxEAcq+|lJZA7&I*oC=Id0#VWY3#6eW!4 z8X;1|MXVCQeQpE)@c1@rI(?!+1`7iKYnRWuJ(tmUI^fJr<-wsoVE(t3zU}~8v_||+ zYqgfcxD>ee6V?W4Nnml4un|+hH z)nmiic5<7gHnt7qx?Nnju5FJdUkb#CgY@1p)?_(Qo9&++o8_*aqGqc3$DS_0VxX>j zfUsX{C=o^=8p?7JTWv4QKNN5Dpvy-pjEtsi%boDz9OQTw2oN^$#>y9FKj~G>V1qSy z2ZWreX$ac);G1&pVA+vH0hU}k=rqJ<(N;9WS&w`p>7J%5%2{&>>nVQjL-c5v5Sk0= zb;SO3*-hb5o7y<#vR=w13$rf(e!WPmcI?RyI<8&HMDNzRLiaVV&HeZKkt8gPS~o`f z^zYZF1Usd{CRdrDCyn##UNzNYGKy`K^=sR}#9oK+VzCZMIo&r*#K3 zlE-jm{Si{1L$d{Wr2+1E)T@dzQR`MX&l!4ff|>n@F5(A&oTl%?B2DU>&mM)T!%Wq4 z(p~intRxExh6V%#1O?ROQ^zX?)7Sn)afSc{q??f3_s9vTTd~g)Pi}?;mBtye#%S*n z&*ze(siyNb?~wlaz87~)4MQW}T))YQVso;SW_@(Z-FFI0+Wmd#hHAOx(gt^aG7i}} zCxw@Md=rQu5R6$l|L4W>aeOxYxjsoV99_hj`SPREb%*)(?~E?@lbYMs?dkUZNT}#H zk+q=Do$VcP{k?xYGXDjl>6M#w`0UB^>6t%nz+QWA9vRupWuu2%mX~|)C*Q&yiy0sk z@8eZYsKI;kVIC+V zgO^b`VwmJB*X-_g#3pF&lMN{?0#5@}t@A`5P*_*luP)Gg|BwA$ZnlQk3{#CAEhW{b zCY7Y8ekmDOCuTCw>zGP3Fdi;RwpnzKB#DUG<)Xk_C&bn zV>zIIK*`9-_k~om1FSQB;Lb8C*ET%8O1wa*eM9U~S0jV=&*d*0FXFh}kB!Yu%JR?X{EZ;5vz?tCHCb~x>*@t&jJ7dxdV4M07`gF%@8wz^J72l*CvjIc_C_h4+gip+_W8 z8H^Vs5h3f+l7|QL9EMdne0XfgtG;~KOB4V%hwr%% zk=lig5>*}I4=lpJw~D6w-!FK8vNvp{&om04?I$z!%&t~? z)OqczSU%_Ms2Qtkxvw~La^HZiclu6$s6l=bGfZL)jhz`F7nndeSp`~D9n>2fB9{1D z_9L!gv1^(@)6_j8YAy_|uU$y4&*D^F6ZGKaGj)C7c zDTl{Cm(&O~fumd#)F_~IzA;a<+~JFwqRN3acSzYJBY|i767R9$_WtXww3tdRcru+Xj1QAh9Rjd4QLhP zyEl4%gjJzwtqR&l_fS+F*}h`Q8kH3=ptj!SPEPI7e3h!Q>#QsNnupdimG1ZX{g9vQ ztg1`H*?W_))4bLB>=%++=SHI3A=4R;xlGZ^gZ0gyK%WG zX(jJ)VGCjUYLi(*TzdIdqFl6cV%;zt5so<=`+f6TM!_TM`!w0?Er@xXxoQ-rI?cf1 zFP$=8z{4PHxE9IEvA;d>u;UB)yplmX)=!0e+{XJ$!2R3D!>*Eq%H#Po2EOa7%J z^vEIW7k)Uv_w(!iG_HE2F8>wsJ2+9iVPMDSBwKA*blLOh)b93Ug3OkI>hO2Y_ z^km<_cN7}~4klOx7-ZUuhN@>>NGkVLO=0n`0&Zg-A<-u=XO`+gn4g-ZqA<^|#A%2x zz~uz5PH4M)_54;0_pB}=Rg+d}0s8$6?C0!+SRwc8C|7N$bL3cGUl%nDIw-Brg8;6pYsG5w&U_Udc)0Z9)rJl(9JTiO3q3Xv$Y zWkPAgDRT-RbO{Ni=LAmORaQ3f(VYm-=K)*(UC+1q5)Y;+KSfd-$2H{4 z#ev+;fc20l=oN3#?Qc}}L3Gr(cUUifBXzW}afuvRztVqx+t?2wuK%eIdX}`03GQa_ zq`_%~oO3&F*>A_Li*5a!Rd)adV0rQ1`Rutv8ffNXS(fkU=MUPN9X+Dl;dWNuJuHu zol-u5QAdvYLGw49{ULl0!||5H5k?;h?7Y){&mU0;(^G@9z9PP|i7>zdn6%S}0UfKA zt{^-8o_Juy(PD6L2)Q@UF?A1$=)Xt_N`@2g_tS#~-+N&EsAU-7WRdgeL}Z)5C9XH} zgJ97(5H}k-@-ouX+X-kE7lo2<>Wo3yyB)s>!>?TXHJca~{hOcf-4actoP`cl8ms~J zxUjd}9!(0eD1xc5%L$DFV3}47V^LjNZ}$Ep25O%qZZDKpA%u&g#bTr3n7~S}wlr@y zi=qE9V$~%GMS4;rW{1n4zS}f*QW?l^=u{u=H~FL3mYew%=|HcmSYR1OV*+Fcw2AU3 z9=((^732a7Epdtkodo-7j8YtI2wM_BqegU=$J?O)YMR9oBH$K25CEqINoIQtBqe+D!30C5Ss_cz)Dx7@rWK zLyqY)78hjZpAG;QcluJiPsE>6-jmeSDze!#3P+IR1i9QQU7ClfxQF~|hGfgCv8K*( z*HAXjBevP0b^PJT2!%X{T9VmwDn%)QRrJWNm_HG_B<2KGAG4VZAF1J$1`O3{5&(3= zXiVb(*HkJMpk56sQ|>Daj!(Q|Q@xrftqhdNA~(*nTwrkR46CjGIYe!e-5xo5I| z&Q{WIhC>2`8bNmn7pgcPMRhdKQKJr6UO=Y4ESY87EwZV6vvKtWIquHMxsRx3!Lq5Z z=s}otloiOK3HwM1u;y`H%@>SJJU85!wGSi|VC0X@1<1O*vLmkiNBMF?5Z*;6>heBX zU!S=>dFFa#m;;ApB}gY;v;C0XeK^V zJ(EV?teUllgQ=2P2aMPnsI4^qCxxv%L~K#xX%FzN7>=rab|6lxSZ1ACld6TS8_uW( zRnoR~OVek4Kz)}Ka4w$(a;B(I567hS92vP;I&87q+^&A-m8>xsvbE-o_R?Rfk|O3H z;U`0`KT&K#aNQ+VF^aUdfqhCzbJq`XpI5;YX09t@7(_|8JgezU zU^7bVjJQyzdv|!mZ0gD5&{Yh4uA!l3^aFO=oSTvxzU9_~6|G|`6v ztVc4SLdR!z_=BeTJX`YWqj{0VF@iH}!vIbw{!X8w#?}|=&D1?ifjwK9XB?ygA3J49 z>_t@+U|tlZrHdYDgt4gb>!_cI4Hl=KM3r^zym2 zwDuWnXTA@yZx*R*K(X(1-;h(p5}?>I^G%`fS3~irIxAGTVJMgMTznc@1nBb4hywg^ z_UAmEQXfYZzEnLw&d4JR;#A;LsK{V1woCaG@l@cKsKz-KT6{uHc=gE!(AY6Y-1R*& zVZ4&KV4uh?I+voWHqS{A2Euj(B%V38IXboGRu7lm6JwNnj2r*6qk; z+go}JC53#yb8a5zVG67_60oQM%IKT0Fd4}*ufJ^sq#$s6TRkA6e9WLevz-Z=` zgZyY2B*={eTA;X9T@cZ>xxM=ru>P}V#J(W;DIpct3C zH5_>*E6DK3s?Xp6O;IkXvvyK8DSpc}aKN{aQc{0yVBpaF6_!G_*6rOKny18d3 zabTC8ASsi(GwOwsXdRnHI~cDNM;N{<#yCn{mM+XwduA`w4%nz0YqTrQaAIeNtxZ*0 zhTo5w{v$m<4D0B#y$o~a{^+v}tgSq*L5STqq0wASoAj{Ej(3|P#Yw#9zED@5yTmE_**%w zMpYSbdmluF7&MeYW|xX&hWh~IoY$5>Mh2DnjFsY^2p6n=A;Nb%w3w+Mz$=Lz5EfNt zka`Ft=(;G*LCGrDSBQuKhKFx)Xd`S*YewBfZw5HAfDS#Bd_!Bd@(VVp4I3l=z24T$ zRgK3Dn;*s+3!sehPPnbrR_qfWMeS|LHH<6L?(QJ3ht~b-KK_6E@m(R)hv(eUZ`fn3 zkt8omoqc{r*tmzh)i zd|{c_;JC55nYjj9H@|`N5t+@3&ba1tA~Wa}>`!Z+&kzbN5z^Z+@BKIR?e?fnWVFwK z3yZYskN)@pS=x91$XN1!BU5GOKl6K9uyEWAfZ!fChYG^SKcyz@jj>fN8S3Y>G&$lh zj{3muo8GY0i?-*YlL5z*ALzUM=>&#AM?-&kZ1L=n1A9l$8LAx6@v1;56Iy7`gu12g z*+ArS;qWP;s{a+Z%7XJ&=M1QBrokZe8x;Pb@k&4=)DnS-=qMvU%DvZC@CY*6HP}yn zm3{;|;SpGN=X(diT@SZ8^#vQz2O-V6a51_MLftNrdVT>s!7FsiPWn_plU zMn|x18laJwfYf^K`3Oa+JJKAIwvGXe2VggUBqSR{U^Cu8zw;G1SgKs<7T?zj{fRXX6`dtueb5D=NkKnr$I^o8M0qL(_@d4;VYyIk&;7#w~4#xMe^$DTM z^Cd~Iho2`-nMbXHu_J6RZ|w8w3=jZr#wN5iGWo+?AL=R~*-wGnv2S?)>F2qhKlOvEl(_+-dJsVDE-<1n-kX9B;sS zMyMu#)8ph%GUlI6BC^GA%BY6}J?^HmBlFWC+zn!Ye^t}3?(7xKm0&-`r&`ea^YD(h zFi+ebK6aRz!FAPqUMH`^V7&9Ln0lbq_B>1=gyP-!;HD-3NPvWN|G{?=-u4O@2wG3e)6o| z(*+uk?HholBp+B?SaM9qhmft-21Qz_U#|0nYFwE|zyx%f!|3>pe9!x-#r}L;=4iP; zO(&yrOLJvdy4_O_r#=a`Afi2{Q>Fw#8-bk`nSvM@ZDw6aGEh7C!%l}H7cHiWj0Rmt zg~FkoNHp|>irXv)6g2_HW=AiGujjkF-sT4YJ;A`91F>aOWy>hP(AbxpQ6=MAQXztq$$k+(|J7B6_3MR^e>dVX&X4yiK`#_p_65aRVIyNQ09 zn*dh^sJqZ9w7Xd}syN5pW9E#bV^TJKla20s;@)O8g#(+T4C=AAia0%f4xelZ5=z|m znd=Gd`yGluZey>8`kyW$62l`Yj@$s{;bC;21OL8;KY^4U%vV>JH0Rb^;cSkWTdP{J z^Dk##;xepv0|Wu4CR1iAT~b|LA1Sn)GRJB=hmVn%y`%&1H5Cudaefb@yqoyRT+HB& zKe}f}zR@+xQ0$V;BrJamc3J-gsWr#YxUS{*LsMHYF)fD#2B~#_J+<(E`CkEiSv}qM z4?}lC`Ae@Sdhr7OOoBY^LsB|1ng)hk={IfR!O52gKjMvJ~bs~SfIrWF)FszVNzPG9BHg! zQr}4}Wbs_&coBv2ha+>)DOnx>Y`GW2E!v!ILC21RF%K~SVPx_(7lZNikSoHvMirpt zx_w6LF4`elnM2?hle>rzE@mgen| zZw=SeM2{|a*@{ESc1h!cZokGLschRdx8--FsV`}}{nfG@8ta_nQsv#1 zkX)zhKj_dAYn8ObpD@!go&mcznz1QjHMh4&o?t!&q7BI!VyR*h)l!Y%>|B><`{3Hn zw^4~I#xA_abLA(`rDS#mYmco(^Ul3NG1HpL(W4=9<%{G|MU(`@`HjJ`ESMrjYjNAd zX;gA+VaURr>WiI8^fyGyM^D(1?O-?)T7ybd#)?3Q-cn@katAzqhy=;pZ9|8z#_{Ir zZa+{<-*?AhTtX;sTUoW(l&editFgX7J9B3mn`3V!6u2G_HBJASvm72PHT?ueh5o>t z`75@NNP$p0@*4oK+4UND{F8Bu%E@=PMQh3TSj2TesUP|URc6{}=*2;_-{-w=?Ut*U zw0cPB7$NKDTi7kA5G)Fi$aTLIL!Jd)jvx*orV@LYzaOVkmtiP8Vs>56^EzZ+-i~B- zAGG=ya@}G=n#kP{gi*PVrwS91x=9h$(N^V+p@0r?rVIu=en^pNIcXme+U**$(>I4*krB9t#N$sO>!Z_TQ4 z-bV0K4{fV{R@R6TV%BhjFXEMOI-A9&x9@#F)wqJd-nV96Cct^ya%H%JwP<(GgO8Eq>y zfaErLF775;FP!4Q18cF%>4RnhCpyfxFK{K%DI)_|{VPFWtLV&_`9Crf#DEx{L--{eV^u$o%suWiMoj6&YbOSc<8ej1LXx> z?PdV>*6k)TH9x@m^|EMtlW>yHuSN;Ju<6p%xzJoh*&WJf;KjF;rr_Dz3n)034#GfE z2&WkFrJ~ZPtL`=L>DkxmW9CxX4nVZ$|KY`Z}E|raIF} z>}<)GSpAlR#7Cj0^l(Jl?kXz}Wvnyt)~97SQATg3>Exj3M?+oM?yZoqW2fj*YexYU zY1r&1Z>cs+Hs#jXMXefd9aVIC2{Ae>7j8oJ7)221LLw|S-1+~b2`8b&Zb?@LWhjEs zlYnoJ!EwT7-(#`qH(v^zEC){eAA_F+{96o;lmtC|zS&ZevLkj8Lwr#nS%Km{6%i^6 zT+@p_l2JsSusVu0SN40*DZrKV%Wcp5XUv(JRXXoC>{DvEvsOn-GsqeT26ir}#D>Pc z6GN(N#B!#~@&&u*x~ckAjGA(U7PWAi9{tVgSGtxD;G;BVl548u_EUGt3s4T3F=E&i zOYeu|D95pgcdg4~TEQLWZ*K_yOLK4Hbe#puC^-d#C_R=9H_#z33+R7gpk$#b8^JK| zZ?d%YkqIm;b3nNc*dm+%NU24+&7aWNbMj~%GCyQL^JHTr+H-Yd=u)wj_@siTc?iG_5-VFubEpqE-ZEBRHuumSa94nX%R%c~`}TUY zy`aPq+2{YY6m3JyMl}PvY=4XYl<-5!cFY!^t=C+zRvfo^1J-0QxHe^14iCzTtFn_w zEf-=HyfYgO!=cF`^S{U#{lO{Y(`f5Wy92RrX;28f!D;c=Ng_MQP9leVXfc}`H9a$O zRb_$L_3RHQljvuXT3B!YNxvb-%aGB&-UOwrrvKZmKHE4_FciL-`C(P}k@KCHmZNs5 zKX!$}1WL^T06H|UDnid%Z6|a|b{&ZGj!Zo#Ms238ImsM{!MQfM_O~B~s4N*8IIw;H z#U&^dG3L8_ETN&{Zy*t__am&1-0#N^Zl6IpE67^pJ(6}=SMAxc#Upe=_?FdbT9kyhHYrI zu3D$a9ezQse9nq5%Mqz4>1osolf}v)Lljh=cEpc&imtbP!LqnasQ;68z7FSg zWTYGtaNl?fJ!$j2MnRG1_@d;H@z}%iNYDFX0#YQO9d4C-7CI=W$9l4m&$p6idgxtm z#62fHQr;kT-YDe)!kq%F9nLd*(X)1ahy>a=4LaMMxOs1X6ulAS+`d|_-*f}q!(V%a ziwAqHg{!!3Xh~1h^e#asfjnF3l(K>2*ckoq%Wd}e;JK@Pw*FPZT=^YtPL>bo?a|YL z0T{yi$+6q{vqs6>u6W$WYRq^?BQkS98)ai6S{N495=v>1T3a%z`&|SSx+Y%FelcH! z8;+t`1=otsqrwc_CMZhSC1f0Yq`ah=)Q%IglD7G7%;A^rv@#9s1BK;L*g?eCTRF-Z zPgQt)$Vt7kA472|6?^$NU>kGhVX?6A0lfn`55ZsDj9ht78z3s=$c~uEiYOI1q$=!X zsUEj6L!Fi`TkmG`XDmHq%B-@|sE)?3-f?Wi`4XriF>L4e-VtF|%v?Y?WkgBr$At;i z6x;$(hWY3a(Bnfz9zD#&$0k!aY!@)Oxr#E4%0KhF; zE^)OqU$Pg)eYHJb*T3NjdCec0|B4%g!ji${i~W22LCFezJ}om4iL5F#NV*y6X? z_3TK^b+MZDcL(cZVVx%qt$tBVYaHh+%1kygne7*^P)sA>7R96_Awt5shttQKU8C<8 zj-{>bJX@|qT-P3aSfOXqY5^`^nE0y}mPUsQ(BM1f(x)BJ;8PrFLeVw7CXiC5`^Pe4 zYR6{5*>Y6qTT{m}o9u|f_5p^A@J$;@VE8r(q&pb1$7KxOwMQMBGp?&Au_*|kfxZ!a zY%t&5{J64YM^|h|d@dthJ!I}_uj`J1Q-h^u7%vSr*iy_N6wO3BS%6LvaG8sVnw!Vx z+wWsM_)ZFJ{Gi%{uK@vx1b53WIA4>0laY z8(pyFa-gpoRJC%@n!3z28iy--{?DcQz?nqHYrlb{C&K#(ynPN@rq$X3yEU;sEl zI^)3RU1MjCm|%XE4&XjPFfst;yY(C>Eh;CkOAmf(waVTLKJ)6U@o{nnVE7*$1nd;1 zvHkjBrR3HmL5FPTTCP}heeq*wadB63@+-s2?$K=lV{ThQ?ZA8kMSFNJWXu_b&$3yb zRBU!8iDIckhONH%ud~m}9>a(3oVtUt=KI(6(HontzFE0EKoe7ZRU>2mg2aQKKae&R zbKLj5*1v;O2zmD#X+=`D}a=Yh~iz)fhEax8tIk$G*UA^XS zz6Pi8%({wnuu+PPvVTe@MmXUJ&ajWR4>Ps#DY7T}bB9O<=Yi84mu6)Nd%0CUF8O+J zr$J^NmpDGBG;(lbtDLG>Rpo}&LaR{5MG=XbbVIEMfPO}$w!IPF*>(kPj)))L8IQpk zTG{(D$`*Maq9&tMOG5;wK irrTEpLqfpGZHPkd;^7;RXbcVk;X7pdTIHp+&mp< z3Dl@&L5Q?6NjPGi|0ce{Xy2V`mgGouoPF)&aNCj^J?*X&iEgT_v-ty(R*7?2{!Dy# zBZurKfd9`OjYNsXH*pWzrkvUaElVgXrzzYce}5Mpt%DAQ&4O{_Oot)uG!6!*MH4a< z34U5xd*NrgmVE+=hHMw2$}oEFo`bwxVZUlB5$hA@ z5}o13#eSH@zQjAtHKBDcSB;wh%)|ru1iJ(@K({D6#Uq~0GJA!uPMUoho9?!K&1$K9 zexj4^RzibRKi>td2pOg;ah-0!#ZCt0$gRjP*!`29CcKnPIBww)dIe8r?_nUiB(3mv zi^*)d3UX36zy#~!qR{WJX$o#wCdZ_WnJ^Q>uq$lJfwz2(b}Se82(2%qmj||mF(C%;DfwK2m=v%ngAy=X=X76>csgWqgPH1N6BASqj*BPK zIKCqAM)K!R16S+7zTGuo_XZhrT}vzmg;xu_k!u^)N&jlY*vd8Ss zB38nz1_g0Lx{Q;{zsZCBMq_$|P`8dl@EIb~o&9&04aq}iQf@`# z1ONph?{^%=XX)?1V$@e#Q7d`AY_D!)R%|`?0h4C-}d$gHNvb3S%#;N3Z zIp=cwi?(Zft2>hC=p(EDh5Lx*YlZJ0W=mULma>pG=i4VcJ+^0uI>ET`ri$yYq3|Bz z+rn4Pt&EEIaU(R#s@{&=7yDigk6!(kWMjT+c0y{=Da{ePxYM&>0d-DJJPv?kqx$93 z$oL(`BcTIB8O?s5Y+vOo!@b zZhiu)qI#xg%bAy8=vKK2^lD=dq~AeVnnaqNcmk;u+q{l8yDEZ}oLl)XGjVl#?HFCa z>45zAh7)hEO{{~&ECep98pNTz=quzwEY2kM{)m$?IeLft*f1_8OCulL||zw4RCufInV)Dw`UZxFq}FRYhY;-hsi8thJkc~vvq1v!&dJ&c#1Kd zNfOi~eS?YC)WgKlo@SXiuj&ME^M0K_OFanfe8IS@WxYIVwpe|)fsGd zu&K~_2zjs~k9&_5?L&4nwb$+0jq6YN$>Ai1fBh{P#k z;nn=tF$=;66e}97LC|cpMPh@PB=liacHF;>Y5S8@iO4D@OvGU@`O|wIfg+PCD^|T@ z3jr|;%V04Y`?=L{S|Kb+vX|V{JjdGR zksx%q61rUK5+Ep9QYO|YorBrtm}?r6U-w}bRAy3k^mFY%XH=lJ>OW*OKn{ND`xpeZ zFk!f`F5y-Z1PFkrBJVwJ>y!kDBV$Pz`aQq5jw=!;FKlshg>3uJdGm%0`rjHWG+~o%T2(&790vU(<$G?2Ka;8 z5=~U3tI*lhuyBCd#lbexfVhkv8UKvSS|^xlqBEk0PecKzXV);15ez}70_}U@lz=Sb zm=5d`aQ=P%=WG!e9NLU_1_$Z+2dooJFA80WKq?x_n-NlKG723GoW*{qg*aOq{-4CZ zYE3(`ei@ZYHom1wJnsJ>3*W~Un@A5BLFFk7%le+^@YS&@DPWHJv33Jb5>ZjJwH8M% zvz0_-fUC3s(1LEMaSS8|yhK@)dd`dDkdkc)Lk(ivx_NT85U?tNzImC-&9ELA2-LY!)8_`5K1kKNRNr^04 zbJbIZK-UVBwfS(+J=wKB>%Y%|gN*p^F;HWItYNYOtk&4#jo@VAO#i~zdJD1rr6&RE zL+is9_{4;3BZ7cH;8;+TEiFO9NQ{!r&?@Ezj?dBN7)`s37?0>9@{F@I1S_uysT*Idi zf-`TTBI^M08g*DF*5T;hOiVBRU3OgP0 zl0ay#LuNEcNCN``AAx6iJv1;Nlsn<#-2Bu$AVs|z3=a|~+L_MG7?>@(&f+UanxOxD zQWV;%k?k*(MJ(|W#2eoP>E0w~9kQWwa)Q7oJ_DpA%_)qj-QgSZDKm>HKc3|}J)5I~29PNN)1 zk7fTIh&7J`& zY?8|}nULHCHnVRQ@olXe04MHwOKrvGdV-u=Z;E^V!i{@-=)e~PdEMT$c{$n^oy4}zTo&Q+O+%iO<8>)jJ}i9)#@?wJDt)$%9LjVoHJ9I0GK*s=9w`@ zp7m!YJewF0*Q4v>Z!}LUtW6g9aY=%N0`|hYaz|rM4%MBV~&-3@|y!`9Gu0gmwp0xBwT$o2LS&sc{uG#7FZ7iH7}Vd4?oRP4wweSp(r`J2s^D> z4jAcw2#sm|a=;kCBWbI0z!JcYX$11XV!)+orvDw<(#qw5C4rsO?&N{xfLGJ_6@aCI zU(>=BfDM6L(@qtDneqOQgargd_#blCe;e@s^rerX+es> zvLGX1{~dAC&J=-JK=PpfS3rURhDsAq`cHkn@c*m1g8z>uMhO@R@Bg4|KtM$Q6Z`*{ z!x;I0eHWDe6Y2}|e+43CU@W}<11kJ~>&hVfU$_5Y7nniDSkelVfm!hW|93MrARw&& USBehav|D9h8VDMx|4!@w0kAL%z5oCK