commit da2cd65e9a05b7c279a5aef82cd0a7492ba13ccf Author: Razvalyaev Date: Fri Dec 27 10:50:32 2024 +0300 init diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..16adf2c --- /dev/null +++ b/.gitignore @@ -0,0 +1,25 @@ +/.vs/Inu_im_2wnd_3lvl_23550/v16/.suo +/.vs/Inu_im_2wnd_3lvl_23550/v16/Browse.VC.db +/.vs/Inu_im_2wnd_3lvl_23550/v16/Browse.VC.db-shm +/.vs/Inu_im_2wnd_3lvl_23550/v16/Browse.VC.db-wal +/.vs/Inu_im_2wnd_3lvl_23550/v16/Browse.VC.opendb +/.vs/ProjectSettings.json +/.vs/slnx.sqlite +/slprj/_cgxe/inu_im_2wnd_3lvl/inu_im_2wnd_3lvl_Cache.mat +/slprj/_jitprj/4jSHq6tpkAOru8OWjpPNFB.l +/slprj/_jitprj/4jSHq6tpkAOru8OWjpPNFB.mat +/slprj/accel/inu_im_2wnd_3lvl/amsi_serial.mat +/slprj/accel/inu_im_2wnd_3lvl/inu_im_2wnd_3lvl_instrumentation_settings.mat +/slprj/accel/inu_im_2wnd_3lvl/inu_im_2wnd_3lvl_SolverChangeInfo.mat +/slprj/accel/inu_im_2wnd_3lvl/inu_im_2wnd_3lvl_top_vm.bc +/slprj/accel/inu_im_2wnd_3lvl/inu_im_2wnd_3lvl_top_vm.ll +/slprj/accel/inu_im_2wnd_3lvl/tmwinternal/simulink_cache.xml +/slprj/sim/varcache/inu_im_2wnd_3lvl/checksumOfCache.mat +/slprj/sim/varcache/inu_im_2wnd_3lvl/tmwinternal/simulink_cache.xml +/slprj/sim/varcache/inu_im_2wnd_3lvl/varInfo.mat +/slprj/sl_proj.tmw + + + +/slprj/ +/.vs/ \ No newline at end of file diff --git a/Inu/Src/.vscode/c_cpp_properties.json b/Inu/Src/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..c6f5321 --- /dev/null +++ b/Inu/Src/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "name": "Win32", + "includePath": [ + "${workspaceFolder}/**;", + "C:/ti/ccs1011/ccs/tools/compiler/ti-cgt-c2000_20.2.1.LTS/**" + ], + "defines": [ + "_DEBUG", + "UNICODE", + "_UNICODE" + ], + "cStandard": "c99", + "cppStandard": "c++17", + "intelliSenseMode": "clang-x64", + "compilerPath": "C:/ti/ccs1011/ccs/tools/compiler/ti-cgt-c2000_20.2.1.LTS/bin/cl2000.exe" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/Inu/Src/VectorControl/abc_to_alphabeta.c b/Inu/Src/VectorControl/abc_to_alphabeta.c new file mode 100644 index 0000000..e0ea922 --- /dev/null +++ b/Inu/Src/VectorControl/abc_to_alphabeta.c @@ -0,0 +1,23 @@ +#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/Src/VectorControl/abc_to_alphabeta.h b/Inu/Src/VectorControl/abc_to_alphabeta.h new file mode 100644 index 0000000..8d3f23b --- /dev/null +++ b/Inu/Src/VectorControl/abc_to_alphabeta.h @@ -0,0 +1,39 @@ +#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/Src/VectorControl/abc_to_dq.c b/Inu/Src/VectorControl/abc_to_dq.c new file mode 100644 index 0000000..8044f2c --- /dev/null +++ b/Inu/Src/VectorControl/abc_to_dq.c @@ -0,0 +1,39 @@ +#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/Src/VectorControl/abc_to_dq.h b/Inu/Src/VectorControl/abc_to_dq.h new file mode 100644 index 0000000..57e3de6 --- /dev/null +++ b/Inu/Src/VectorControl/abc_to_dq.h @@ -0,0 +1,42 @@ +#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/Src/VectorControl/alg_pll.c b/Inu/Src/VectorControl/alg_pll.c new file mode 100644 index 0000000..b7f3a9c --- /dev/null +++ b/Inu/Src/VectorControl/alg_pll.c @@ -0,0 +1,577 @@ +#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/Src/VectorControl/alg_pll.h b/Inu/Src/VectorControl/alg_pll.h new file mode 100644 index 0000000..0a63dde --- /dev/null +++ b/Inu/Src/VectorControl/alg_pll.h @@ -0,0 +1,188 @@ +#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/Src/VectorControl/alphabeta_to_dq.c b/Inu/Src/VectorControl/alphabeta_to_dq.c new file mode 100644 index 0000000..ffa5a73 --- /dev/null +++ b/Inu/Src/VectorControl/alphabeta_to_dq.c @@ -0,0 +1,24 @@ +#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/Src/VectorControl/alphabeta_to_dq.h b/Inu/Src/VectorControl/alphabeta_to_dq.h new file mode 100644 index 0000000..8484e14 --- /dev/null +++ b/Inu/Src/VectorControl/alphabeta_to_dq.h @@ -0,0 +1,32 @@ +#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/Src/VectorControl/dq_to_alphabeta_cos.c b/Inu/Src/VectorControl/dq_to_alphabeta_cos.c new file mode 100644 index 0000000..d075df4 --- /dev/null +++ b/Inu/Src/VectorControl/dq_to_alphabeta_cos.c @@ -0,0 +1,39 @@ +#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/Src/VectorControl/dq_to_alphabeta_cos.h b/Inu/Src/VectorControl/dq_to_alphabeta_cos.h new file mode 100644 index 0000000..b261ced --- /dev/null +++ b/Inu/Src/VectorControl/dq_to_alphabeta_cos.h @@ -0,0 +1,40 @@ + + + +#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/Src/VectorControl/params_pll.h b/Inu/Src/VectorControl/params_pll.h new file mode 100644 index 0000000..2ec9466 --- /dev/null +++ b/Inu/Src/VectorControl/params_pll.h @@ -0,0 +1,41 @@ +#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/Src/VectorControl/regul_power.c b/Inu/Src/VectorControl/regul_power.c new file mode 100644 index 0000000..d78f216 --- /dev/null +++ b/Inu/Src/VectorControl/regul_power.c @@ -0,0 +1,88 @@ +/* + * 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/Src/VectorControl/regul_power.h b/Inu/Src/VectorControl/regul_power.h new file mode 100644 index 0000000..80d3e06 --- /dev/null +++ b/Inu/Src/VectorControl/regul_power.h @@ -0,0 +1,30 @@ +/* + * 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/Src/VectorControl/regul_turns.c b/Inu/Src/VectorControl/regul_turns.c new file mode 100644 index 0000000..95ca611 --- /dev/null +++ b/Inu/Src/VectorControl/regul_turns.c @@ -0,0 +1,143 @@ +#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/Src/VectorControl/regul_turns.h b/Inu/Src/VectorControl/regul_turns.h new file mode 100644 index 0000000..f765b88 --- /dev/null +++ b/Inu/Src/VectorControl/regul_turns.h @@ -0,0 +1,29 @@ +#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/Src/VectorControl/smooth.c b/Inu/Src/VectorControl/smooth.c new file mode 100644 index 0000000..dcf63c8 --- /dev/null +++ b/Inu/Src/VectorControl/smooth.c @@ -0,0 +1,180 @@ +#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/Src/VectorControl/smooth.h b/Inu/Src/VectorControl/smooth.h new file mode 100644 index 0000000..c91e6f1 --- /dev/null +++ b/Inu/Src/VectorControl/smooth.h @@ -0,0 +1,78 @@ +#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/Src/VectorControl/teta_calc.c b/Inu/Src/VectorControl/teta_calc.c new file mode 100644 index 0000000..8a64daf --- /dev/null +++ b/Inu/Src/VectorControl/teta_calc.c @@ -0,0 +1,91 @@ +#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/Src/VectorControl/teta_calc.h b/Inu/Src/VectorControl/teta_calc.h new file mode 100644 index 0000000..d2e7b03 --- /dev/null +++ b/Inu/Src/VectorControl/teta_calc.h @@ -0,0 +1,36 @@ +#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/Src/VectorControl/vector_control.c b/Inu/Src/VectorControl/vector_control.c new file mode 100644 index 0000000..901b75d --- /dev/null +++ b/Inu/Src/VectorControl/vector_control.c @@ -0,0 +1,297 @@ +/* + * 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/Src/VectorControl/vector_control.h b/Inu/Src/VectorControl/vector_control.h new file mode 100644 index 0000000..3307fb1 --- /dev/null +++ b/Inu/Src/VectorControl/vector_control.h @@ -0,0 +1,82 @@ +/* + * 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/Src/main/281xEvTimersInit.c b/Inu/Src/main/281xEvTimersInit.c new file mode 100644 index 0000000..59b7005 --- /dev/null +++ b/Inu/Src/main/281xEvTimersInit.c @@ -0,0 +1,636 @@ +// 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/Src/main/281xEvTimersInit.h b/Inu/Src/main/281xEvTimersInit.h new file mode 100644 index 0000000..2498fc9 --- /dev/null +++ b/Inu/Src/main/281xEvTimersInit.h @@ -0,0 +1,18 @@ +#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/Src/main/CAN_project.h b/Inu/Src/main/CAN_project.h new file mode 100644 index 0000000..3454f1e --- /dev/null +++ b/Inu/Src/main/CAN_project.h @@ -0,0 +1,256 @@ +/* + * 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/Src/main/Main.c b/Inu/Src/main/Main.c new file mode 100644 index 0000000..9716206 --- /dev/null +++ b/Inu/Src/main/Main.c @@ -0,0 +1,126 @@ +#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/Src/main/PWMTMSHandle.c b/Inu/Src/main/PWMTMSHandle.c new file mode 100644 index 0000000..2fe71b1 --- /dev/null +++ b/Inu/Src/main/PWMTMSHandle.c @@ -0,0 +1,510 @@ + +#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/Src/main/PWMTMSHandle.h b/Inu/Src/main/PWMTMSHandle.h new file mode 100644 index 0000000..ab1675c --- /dev/null +++ b/Inu/Src/main/PWMTMSHandle.h @@ -0,0 +1,25 @@ +#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/Src/main/PWMTools.c b/Inu/Src/main/PWMTools.c new file mode 100644 index 0000000..b62f7b3 --- /dev/null +++ b/Inu/Src/main/PWMTools.c @@ -0,0 +1,2438 @@ +#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/Src/main/PWMTools.h b/Inu/Src/main/PWMTools.h new file mode 100644 index 0000000..67f30b2 --- /dev/null +++ b/Inu/Src/main/PWMTools.h @@ -0,0 +1,54 @@ +#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/Src/main/adc_internal.h b/Inu/Src/main/adc_internal.h new file mode 100644 index 0000000..c74a174 --- /dev/null +++ b/Inu/Src/main/adc_internal.h @@ -0,0 +1,33 @@ + + +#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/Src/main/adc_tools.c b/Inu/Src/main/adc_tools.c new file mode 100644 index 0000000..80b3b8d --- /dev/null +++ b/Inu/Src/main/adc_tools.c @@ -0,0 +1,1412 @@ +#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/Src/main/alarm_log.c b/Inu/Src/main/alarm_log.c new file mode 100644 index 0000000..7dd9e2e --- /dev/null +++ b/Inu/Src/main/alarm_log.c @@ -0,0 +1,196 @@ +/* + * 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/Src/main/alarm_log.h b/Inu/Src/main/alarm_log.h new file mode 100644 index 0000000..c8fef58 --- /dev/null +++ b/Inu/Src/main/alarm_log.h @@ -0,0 +1,16 @@ +/* + * 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/Src/main/alg_simple_scalar.c b/Inu/Src/main/alg_simple_scalar.c new file mode 100644 index 0000000..7fc3c8f --- /dev/null +++ b/Inu/Src/main/alg_simple_scalar.c @@ -0,0 +1,976 @@ +/* + * 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, - результат расчета - частота напрyжениy в статоре + real *Uz1, - результат расчета - коэф. модулyции в 1 обмотке статора + real *Uz2) - результат расчета - коэф. модулyции в 2 обмотке статора + + + Используетсy регулyтор скорости, который выполнyет задание fzad по скорости + + Заводитсy постоyнное скольжение = 0.5 + + Идет расчет напрyжениy через модуль тока по одной из 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/Src/main/alg_simple_scalar.h b/Inu/Src/main/alg_simple_scalar.h new file mode 100644 index 0000000..f05ca38 --- /dev/null +++ b/Inu/Src/main/alg_simple_scalar.h @@ -0,0 +1,101 @@ +/* + * 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/Src/main/alg_uf_const.c b/Inu/Src/main/alg_uf_const.c new file mode 100644 index 0000000..3e5b8fa --- /dev/null +++ b/Inu/Src/main/alg_uf_const.c @@ -0,0 +1,80 @@ +/* + * 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/Src/main/alg_uf_const.h b/Inu/Src/main/alg_uf_const.h new file mode 100644 index 0000000..50276f0 --- /dev/null +++ b/Inu/Src/main/alg_uf_const.h @@ -0,0 +1,34 @@ +/* + * 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/Src/main/another_bs.c b/Inu/Src/main/another_bs.c new file mode 100644 index 0000000..542df74 --- /dev/null +++ b/Inu/Src/main/another_bs.c @@ -0,0 +1,458 @@ +/* + * 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/Src/main/break_regul.h b/Inu/Src/main/break_regul.h new file mode 100644 index 0000000..1e79cdd --- /dev/null +++ b/Inu/Src/main/break_regul.h @@ -0,0 +1,70 @@ +#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/Src/main/calc_rms_vals.c b/Inu/Src/main/calc_rms_vals.c new file mode 100644 index 0000000..7363ce2 --- /dev/null +++ b/Inu/Src/main/calc_rms_vals.c @@ -0,0 +1,265 @@ +/* + * 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/Src/main/calc_rms_vals.h b/Inu/Src/main/calc_rms_vals.h new file mode 100644 index 0000000..a01b3da --- /dev/null +++ b/Inu/Src/main/calc_rms_vals.h @@ -0,0 +1,66 @@ +/* + * 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/Src/main/calc_tempers.c b/Inu/Src/main/calc_tempers.c new file mode 100644 index 0000000..a5988cc --- /dev/null +++ b/Inu/Src/main/calc_tempers.c @@ -0,0 +1,284 @@ +/* + * 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/Src/main/can_bs2bs.h b/Inu/Src/main/can_bs2bs.h new file mode 100644 index 0000000..b3d69cf --- /dev/null +++ b/Inu/Src/main/can_bs2bs.h @@ -0,0 +1,21 @@ +/* + * 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/Src/main/can_protocol_ukss.h b/Inu/Src/main/can_protocol_ukss.h new file mode 100644 index 0000000..18bf157 --- /dev/null +++ b/Inu/Src/main/can_protocol_ukss.h @@ -0,0 +1,63 @@ +/* + * 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/Src/main/control_station_project.c b/Inu/Src/main/control_station_project.c new file mode 100644 index 0000000..877b4fb --- /dev/null +++ b/Inu/Src/main/control_station_project.c @@ -0,0 +1,2493 @@ +/* + * 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/Src/main/detect_error_3_phase.c b/Inu/Src/main/detect_error_3_phase.c new file mode 100644 index 0000000..9b6fd87 --- /dev/null +++ b/Inu/Src/main/detect_error_3_phase.c @@ -0,0 +1,167 @@ +/* + * 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/Src/main/detect_error_3_phase.h b/Inu/Src/main/detect_error_3_phase.h new file mode 100644 index 0000000..bdb7307 --- /dev/null +++ b/Inu/Src/main/detect_error_3_phase.h @@ -0,0 +1,148 @@ +/* + * 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/Src/main/detect_errors.c b/Inu/Src/main/detect_errors.c new file mode 100644 index 0000000..07ff17c --- /dev/null +++ b/Inu/Src/main/detect_errors.c @@ -0,0 +1,1606 @@ +/* + * 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/Src/main/detect_errors.h b/Inu/Src/main/detect_errors.h new file mode 100644 index 0000000..48e3126 --- /dev/null +++ b/Inu/Src/main/detect_errors.h @@ -0,0 +1,80 @@ +/* + * 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/Src/main/detect_errors_adc.c b/Inu/Src/main/detect_errors_adc.c new file mode 100644 index 0000000..634ea2f --- /dev/null +++ b/Inu/Src/main/detect_errors_adc.c @@ -0,0 +1,310 @@ +/* + * 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/Src/main/detect_errors_adc.h b/Inu/Src/main/detect_errors_adc.h new file mode 100644 index 0000000..6aa850d --- /dev/null +++ b/Inu/Src/main/detect_errors_adc.h @@ -0,0 +1,45 @@ +/* + * 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/Src/main/detect_overload.c b/Inu/Src/main/detect_overload.c new file mode 100644 index 0000000..fce38dd --- /dev/null +++ b/Inu/Src/main/detect_overload.c @@ -0,0 +1,92 @@ +/* + * 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/Src/main/detect_overload.h b/Inu/Src/main/detect_overload.h new file mode 100644 index 0000000..897e24a --- /dev/null +++ b/Inu/Src/main/detect_overload.h @@ -0,0 +1,32 @@ +/* + * 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/Src/main/detect_phase_break.c b/Inu/Src/main/detect_phase_break.c new file mode 100644 index 0000000..69f0b55 --- /dev/null +++ b/Inu/Src/main/detect_phase_break.c @@ -0,0 +1,112 @@ +/* + * 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/Src/main/detect_phase_break.h b/Inu/Src/main/detect_phase_break.h new file mode 100644 index 0000000..66104d8 --- /dev/null +++ b/Inu/Src/main/detect_phase_break.h @@ -0,0 +1,36 @@ +/* + * 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/Src/main/detect_phase_break2.c b/Inu/Src/main/detect_phase_break2.c new file mode 100644 index 0000000..4cfb921 --- /dev/null +++ b/Inu/Src/main/detect_phase_break2.c @@ -0,0 +1,203 @@ +/* + * 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/Src/main/detect_phase_break2.h b/Inu/Src/main/detect_phase_break2.h new file mode 100644 index 0000000..c32608e --- /dev/null +++ b/Inu/Src/main/detect_phase_break2.h @@ -0,0 +1,50 @@ +/* + * 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/Src/main/digital_filters.c b/Inu/Src/main/digital_filters.c new file mode 100644 index 0000000..a94d46b --- /dev/null +++ b/Inu/Src/main/digital_filters.c @@ -0,0 +1,103 @@ +/* + * 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/Src/main/digital_filters.h b/Inu/Src/main/digital_filters.h new file mode 100644 index 0000000..bc24cac --- /dev/null +++ b/Inu/Src/main/digital_filters.h @@ -0,0 +1,19 @@ +/* + * 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/Src/main/edrk_main.c b/Inu/Src/main/edrk_main.c new file mode 100644 index 0000000..c89a473 --- /dev/null +++ b/Inu/Src/main/edrk_main.c @@ -0,0 +1,2736 @@ +#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/Src/main/edrk_main.h b/Inu/Src/main/edrk_main.h new file mode 100644 index 0000000..435baca --- /dev/null +++ b/Inu/Src/main/edrk_main.h @@ -0,0 +1,1838 @@ + +#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/Src/main/f281xbmsk.h b/Inu/Src/main/f281xbmsk.h new file mode 100644 index 0000000..f9dd78f --- /dev/null +++ b/Inu/Src/main/f281xbmsk.h @@ -0,0 +1,244 @@ +/* ================================================================================== +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/Src/main/f281xpwm.c b/Inu/Src/main/f281xpwm.c new file mode 100644 index 0000000..1103b7e --- /dev/null +++ b/Inu/Src/main/f281xpwm.c @@ -0,0 +1,288 @@ +/* ================================================================================== +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/Src/main/f281xpwm.h b/Inu/Src/main/f281xpwm.h new file mode 100644 index 0000000..bf9499f --- /dev/null +++ b/Inu/Src/main/f281xpwm.h @@ -0,0 +1,163 @@ +/* ================================================================================== +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/Src/main/limit_lib.c b/Inu/Src/main/limit_lib.c new file mode 100644 index 0000000..859b644 --- /dev/null +++ b/Inu/Src/main/limit_lib.c @@ -0,0 +1,50 @@ +/* + * 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/Src/main/limit_lib.h b/Inu/Src/main/limit_lib.h new file mode 100644 index 0000000..3b1e6aa --- /dev/null +++ b/Inu/Src/main/limit_lib.h @@ -0,0 +1,15 @@ +/* + * 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/Src/main/limit_power.c b/Inu/Src/main/limit_power.c new file mode 100644 index 0000000..840019f --- /dev/null +++ b/Inu/Src/main/limit_power.c @@ -0,0 +1,237 @@ +/* + * 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/Src/main/logs_hmi.h b/Inu/Src/main/logs_hmi.h new file mode 100644 index 0000000..e7637f5 --- /dev/null +++ b/Inu/Src/main/logs_hmi.h @@ -0,0 +1,86 @@ +/* + * 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/Src/main/manch.h b/Inu/Src/main/manch.h new file mode 100644 index 0000000..d634beb --- /dev/null +++ b/Inu/Src/main/manch.h @@ -0,0 +1,182 @@ +#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/Src/main/master_slave.c b/Inu/Src/main/master_slave.c new file mode 100644 index 0000000..4aa4297 --- /dev/null +++ b/Inu/Src/main/master_slave.c @@ -0,0 +1,586 @@ +/* + * 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/Src/main/message2.h b/Inu/Src/main/message2.h new file mode 100644 index 0000000..0f93c42 --- /dev/null +++ b/Inu/Src/main/message2.h @@ -0,0 +1,33 @@ +//////////////////////////////////////////// +// 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/Src/main/message2can.c b/Inu/Src/main/message2can.c new file mode 100644 index 0000000..5b0a883 --- /dev/null +++ b/Inu/Src/main/message2can.c @@ -0,0 +1,278 @@ +/* + * 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/Src/main/message2can.h b/Inu/Src/main/message2can.h new file mode 100644 index 0000000..14ad7da --- /dev/null +++ b/Inu/Src/main/message2can.h @@ -0,0 +1,18 @@ +/* + * 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/Src/main/message2test.c b/Inu/Src/main/message2test.c new file mode 100644 index 0000000..fbcff9e --- /dev/null +++ b/Inu/Src/main/message2test.c @@ -0,0 +1,678 @@ +#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/Src/main/message2test.h b/Inu/Src/main/message2test.h new file mode 100644 index 0000000..3f1406c --- /dev/null +++ b/Inu/Src/main/message2test.h @@ -0,0 +1,33 @@ +//////////////////////////////////////////// +// 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/Src/main/message_modbus.c b/Inu/Src/main/message_modbus.c new file mode 100644 index 0000000..024db30 --- /dev/null +++ b/Inu/Src/main/message_modbus.c @@ -0,0 +1,897 @@ +#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/Src/main/message_modbus.h b/Inu/Src/main/message_modbus.h new file mode 100644 index 0000000..971e5a9 --- /dev/null +++ b/Inu/Src/main/message_modbus.h @@ -0,0 +1,61 @@ + +#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/Src/main/message_terminals_can.c b/Inu/Src/main/message_terminals_can.c new file mode 100644 index 0000000..c21892a --- /dev/null +++ b/Inu/Src/main/message_terminals_can.c @@ -0,0 +1,128 @@ +/* + * 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/Src/main/message_terminals_can.h b/Inu/Src/main/message_terminals_can.h new file mode 100644 index 0000000..90cf1c6 --- /dev/null +++ b/Inu/Src/main/message_terminals_can.h @@ -0,0 +1,15 @@ +/* + * 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/Src/main/modbus_hmi.c b/Inu/Src/main/modbus_hmi.c new file mode 100644 index 0000000..f28cb25 --- /dev/null +++ b/Inu/Src/main/modbus_hmi.c @@ -0,0 +1,393 @@ +#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/Src/main/modbus_hmi.h b/Inu/Src/main/modbus_hmi.h new file mode 100644 index 0000000..887b537 --- /dev/null +++ b/Inu/Src/main/modbus_hmi.h @@ -0,0 +1,39 @@ +#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/Src/main/modbus_hmi_read.c b/Inu/Src/main/modbus_hmi_read.c new file mode 100644 index 0000000..9718943 --- /dev/null +++ b/Inu/Src/main/modbus_hmi_read.c @@ -0,0 +1,222 @@ +/* + * 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/Src/main/modbus_hmi_read.h b/Inu/Src/main/modbus_hmi_read.h new file mode 100644 index 0000000..7f3e0db --- /dev/null +++ b/Inu/Src/main/modbus_hmi_read.h @@ -0,0 +1,15 @@ +/* + * 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/Src/main/modbus_hmi_update.c b/Inu/Src/main/modbus_hmi_update.c new file mode 100644 index 0000000..7c1e83c --- /dev/null +++ b/Inu/Src/main/modbus_hmi_update.c @@ -0,0 +1,2022 @@ +#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/Src/main/modbus_hmi_update.h b/Inu/Src/main/modbus_hmi_update.h new file mode 100644 index 0000000..3ed0c05 --- /dev/null +++ b/Inu/Src/main/modbus_hmi_update.h @@ -0,0 +1,39 @@ +#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/Src/main/modbus_svu_update.c b/Inu/Src/main/modbus_svu_update.c new file mode 100644 index 0000000..d9e6fe2 --- /dev/null +++ b/Inu/Src/main/modbus_svu_update.c @@ -0,0 +1,710 @@ +#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/Src/main/modbus_svu_update.h b/Inu/Src/main/modbus_svu_update.h new file mode 100644 index 0000000..d58d103 --- /dev/null +++ b/Inu/Src/main/modbus_svu_update.h @@ -0,0 +1,17 @@ +/* + * 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/Src/main/not_use/log_to_mem.c b/Inu/Src/main/not_use/log_to_mem.c new file mode 100644 index 0000000..8a5c223 --- /dev/null +++ b/Inu/Src/main/not_use/log_to_mem.c @@ -0,0 +1,249 @@ +/****************************************************************/ +/* 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/Src/main/optical_bus.h b/Inu/Src/main/optical_bus.h new file mode 100644 index 0000000..ce30d7c --- /dev/null +++ b/Inu/Src/main/optical_bus.h @@ -0,0 +1,109 @@ +/* + * 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/Src/main/optical_bus_tools.c b/Inu/Src/main/optical_bus_tools.c new file mode 100644 index 0000000..431ade9 --- /dev/null +++ b/Inu/Src/main/optical_bus_tools.c @@ -0,0 +1,791 @@ +/* + * 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/Src/main/optical_bus_tools.h b/Inu/Src/main/optical_bus_tools.h new file mode 100644 index 0000000..89c6bc6 --- /dev/null +++ b/Inu/Src/main/optical_bus_tools.h @@ -0,0 +1,14 @@ +/* + * 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/Src/main/overheat_limit.c b/Inu/Src/main/overheat_limit.c new file mode 100644 index 0000000..e2c2720 --- /dev/null +++ b/Inu/Src/main/overheat_limit.c @@ -0,0 +1,80 @@ +/* + * 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/Src/main/overheat_limit.h b/Inu/Src/main/overheat_limit.h new file mode 100644 index 0000000..dc1199d --- /dev/null +++ b/Inu/Src/main/overheat_limit.h @@ -0,0 +1,13 @@ +/* + * 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/Src/main/params.h b/Inu/Src/main/params.h new file mode 100644 index 0000000..7abc592 --- /dev/null +++ b/Inu/Src/main/params.h @@ -0,0 +1,182 @@ +#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/Src/main/params_alg.h b/Inu/Src/main/params_alg.h new file mode 100644 index 0000000..8529a80 --- /dev/null +++ b/Inu/Src/main/params_alg.h @@ -0,0 +1,218 @@ +/* + * 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/Src/main/params_bsu.h b/Inu/Src/main/params_bsu.h new file mode 100644 index 0000000..a58cc2c --- /dev/null +++ b/Inu/Src/main/params_bsu.h @@ -0,0 +1,123 @@ +/* + * 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/Src/main/params_hwp.h b/Inu/Src/main/params_hwp.h new file mode 100644 index 0000000..d3ebed4 --- /dev/null +++ b/Inu/Src/main/params_hwp.h @@ -0,0 +1,9 @@ + + +#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/Src/main/params_motor.h b/Inu/Src/main/params_motor.h new file mode 100644 index 0000000..1e8c030 --- /dev/null +++ b/Inu/Src/main/params_motor.h @@ -0,0 +1,59 @@ +/* + * 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/Src/main/params_norma.h b/Inu/Src/main/params_norma.h new file mode 100644 index 0000000..9c3e790 --- /dev/null +++ b/Inu/Src/main/params_norma.h @@ -0,0 +1,70 @@ +/* + * 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/Src/main/params_protect_adc.h b/Inu/Src/main/params_protect_adc.h new file mode 100644 index 0000000..b5772bd --- /dev/null +++ b/Inu/Src/main/params_protect_adc.h @@ -0,0 +1,25 @@ +/* + * 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/Src/main/params_pwm24.h b/Inu/Src/main/params_pwm24.h new file mode 100644 index 0000000..a2571a6 --- /dev/null +++ b/Inu/Src/main/params_pwm24.h @@ -0,0 +1,48 @@ +/* + * 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/Src/main/params_temper_p.h b/Inu/Src/main/params_temper_p.h new file mode 100644 index 0000000..2864649 --- /dev/null +++ b/Inu/Src/main/params_temper_p.h @@ -0,0 +1,58 @@ + +#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/Src/main/pll_tools.c b/Inu/Src/main/pll_tools.c new file mode 100644 index 0000000..decc342 --- /dev/null +++ b/Inu/Src/main/pll_tools.c @@ -0,0 +1,105 @@ +/* + * 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/Src/main/pll_tools.h b/Inu/Src/main/pll_tools.h new file mode 100644 index 0000000..1659e90 --- /dev/null +++ b/Inu/Src/main/pll_tools.h @@ -0,0 +1,22 @@ +/* + * 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/Src/main/project.c b/Inu/Src/main/project.c new file mode 100644 index 0000000..a8fcec9 --- /dev/null +++ b/Inu/Src/main/project.c @@ -0,0 +1,1049 @@ +#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/Src/main/project.h b/Inu/Src/main/project.h new file mode 100644 index 0000000..32dba9a --- /dev/null +++ b/Inu/Src/main/project.h @@ -0,0 +1,74 @@ +#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/Src/main/project_setup.h b/Inu/Src/main/project_setup.h new file mode 100644 index 0000000..4f3ec93 --- /dev/null +++ b/Inu/Src/main/project_setup.h @@ -0,0 +1,414 @@ +#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/Src/main/protect_levels.h b/Inu/Src/main/protect_levels.h new file mode 100644 index 0000000..b9bf78f --- /dev/null +++ b/Inu/Src/main/protect_levels.h @@ -0,0 +1,119 @@ +/* + * 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/Src/main/pump_control.c b/Inu/Src/main/pump_control.c new file mode 100644 index 0000000..212473c --- /dev/null +++ b/Inu/Src/main/pump_control.c @@ -0,0 +1,255 @@ +/* + * 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/Src/main/pump_control.h b/Inu/Src/main/pump_control.h new file mode 100644 index 0000000..92a5931 --- /dev/null +++ b/Inu/Src/main/pump_control.h @@ -0,0 +1,27 @@ +/* + * 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/Src/main/pwm_logs.c b/Inu/Src/main/pwm_logs.c new file mode 100644 index 0000000..42ca3e7 --- /dev/null +++ b/Inu/Src/main/pwm_logs.c @@ -0,0 +1,476 @@ +/* + * 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/Src/main/pwm_logs.h b/Inu/Src/main/pwm_logs.h new file mode 100644 index 0000000..8eb5f07 --- /dev/null +++ b/Inu/Src/main/pwm_logs.h @@ -0,0 +1,16 @@ +/* + * 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/Src/main/pwm_test_lines.c b/Inu/Src/main/pwm_test_lines.c new file mode 100644 index 0000000..ef24f64 --- /dev/null +++ b/Inu/Src/main/pwm_test_lines.c @@ -0,0 +1,36 @@ + +#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/Src/main/pwm_test_lines.h b/Inu/Src/main/pwm_test_lines.h new file mode 100644 index 0000000..d411550 --- /dev/null +++ b/Inu/Src/main/pwm_test_lines.h @@ -0,0 +1,63 @@ + + + + + + +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/Src/main/ramp_zadanie_tools.c b/Inu/Src/main/ramp_zadanie_tools.c new file mode 100644 index 0000000..61a6689 --- /dev/null +++ b/Inu/Src/main/ramp_zadanie_tools.c @@ -0,0 +1,417 @@ +/* + * 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/Src/main/ramp_zadanie_tools.h b/Inu/Src/main/ramp_zadanie_tools.h new file mode 100644 index 0000000..c90acb6 --- /dev/null +++ b/Inu/Src/main/ramp_zadanie_tools.h @@ -0,0 +1,20 @@ +/* + * 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/Src/main/sbor_shema.c b/Inu/Src/main/sbor_shema.c new file mode 100644 index 0000000..7157fd1 --- /dev/null +++ b/Inu/Src/main/sbor_shema.c @@ -0,0 +1,1764 @@ +/* + * 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/Src/main/sbor_shema.h b/Inu/Src/main/sbor_shema.h new file mode 100644 index 0000000..fdde9ff --- /dev/null +++ b/Inu/Src/main/sbor_shema.h @@ -0,0 +1,24 @@ +/* + * 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/Src/main/sim_model.c b/Inu/Src/main/sim_model.c new file mode 100644 index 0000000..a84fa72 --- /dev/null +++ b/Inu/Src/main/sim_model.c @@ -0,0 +1,222 @@ +/* + * 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/Src/main/sim_model.h b/Inu/Src/main/sim_model.h new file mode 100644 index 0000000..9a27c91 --- /dev/null +++ b/Inu/Src/main/sim_model.h @@ -0,0 +1,21 @@ +/* + * 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/Src/main/sync_tools.c b/Inu/Src/main/sync_tools.c new file mode 100644 index 0000000..ee9ee9d --- /dev/null +++ b/Inu/Src/main/sync_tools.c @@ -0,0 +1,520 @@ +#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/Src/main/sync_tools.h b/Inu/Src/main/sync_tools.h new file mode 100644 index 0000000..e41ee87 --- /dev/null +++ b/Inu/Src/main/sync_tools.h @@ -0,0 +1,113 @@ + + +#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/Src/main/synhro_tools.c b/Inu/Src/main/synhro_tools.c new file mode 100644 index 0000000..47bbc5e --- /dev/null +++ b/Inu/Src/main/synhro_tools.c @@ -0,0 +1,144 @@ +/* + * 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/Src/main/synhro_tools.h b/Inu/Src/main/synhro_tools.h new file mode 100644 index 0000000..e6de9f1 --- /dev/null +++ b/Inu/Src/main/synhro_tools.h @@ -0,0 +1,19 @@ +/* + * 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/Src/main/temper_p_tools.c b/Inu/Src/main/temper_p_tools.c new file mode 100644 index 0000000..c6a773a --- /dev/null +++ b/Inu/Src/main/temper_p_tools.c @@ -0,0 +1,77 @@ +/* + * 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/Src/main/temper_p_tools.h b/Inu/Src/main/temper_p_tools.h new file mode 100644 index 0000000..d99cb89 --- /dev/null +++ b/Inu/Src/main/temper_p_tools.h @@ -0,0 +1,22 @@ +/* + * 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/Src/main/tk_Test.c b/Inu/Src/main/tk_Test.c new file mode 100644 index 0000000..fd5cc61 --- /dev/null +++ b/Inu/Src/main/tk_Test.c @@ -0,0 +1,369 @@ + +#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/Src/main/tk_Test.h b/Inu/Src/main/tk_Test.h new file mode 100644 index 0000000..2a44453 --- /dev/null +++ b/Inu/Src/main/tk_Test.h @@ -0,0 +1,12 @@ + +#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/Src/main/ukss_tools.c b/Inu/Src/main/ukss_tools.c new file mode 100644 index 0000000..097fc1e --- /dev/null +++ b/Inu/Src/main/ukss_tools.c @@ -0,0 +1,605 @@ +/* + * 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/Src/main/uom_tools.h b/Inu/Src/main/uom_tools.h new file mode 100644 index 0000000..134648f --- /dev/null +++ b/Inu/Src/main/uom_tools.h @@ -0,0 +1,15 @@ +/* + * 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/Src/main/v_pwm24_v2.c b/Inu/Src/main/v_pwm24_v2.c new file mode 100644 index 0000000..147c2c9 --- /dev/null +++ b/Inu/Src/main/v_pwm24_v2.c @@ -0,0 +1,948 @@ +#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/Src/main/v_pwm24_v2.h b/Inu/Src/main/v_pwm24_v2.h new file mode 100644 index 0000000..e0e1baa --- /dev/null +++ b/Inu/Src/main/v_pwm24_v2.h @@ -0,0 +1,161 @@ +#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/Src/main/v_rotor.c b/Inu/Src/main/v_rotor.c new file mode 100644 index 0000000..1da2a2f --- /dev/null +++ b/Inu/Src/main/v_rotor.c @@ -0,0 +1,1095 @@ +#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/Src/main/v_rotor.h b/Inu/Src/main/v_rotor.h new file mode 100644 index 0000000..aad2216 --- /dev/null +++ b/Inu/Src/main/v_rotor.h @@ -0,0 +1,185 @@ +#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/Src/main/v_rotor_22220.c b/Inu/Src/main/v_rotor_22220.c new file mode 100644 index 0000000..c31de31 --- /dev/null +++ b/Inu/Src/main/v_rotor_22220.c @@ -0,0 +1,666 @@ + +#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/Src/main/v_rotor_22220.h b/Inu/Src/main/v_rotor_22220.h new file mode 100644 index 0000000..aec28c5 --- /dev/null +++ b/Inu/Src/main/v_rotor_22220.h @@ -0,0 +1,54 @@ +#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/Src/main/vector.h b/Inu/Src/main/vector.h new file mode 100644 index 0000000..25bc4f1 --- /dev/null +++ b/Inu/Src/main/vector.h @@ -0,0 +1,254 @@ +/* + ???? ??? (?) 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/Src/main/xPlatesAddress.h b/Inu/Src/main/xPlatesAddress.h new file mode 100644 index 0000000..fa0a62c --- /dev/null +++ b/Inu/Src/main/xPlatesAddress.h @@ -0,0 +1,21 @@ +#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/Src/main_matlab/DSP281x_Device.h b/Inu/Src/main_matlab/DSP281x_Device.h new file mode 100644 index 0000000..557612b --- /dev/null +++ b/Inu/Src/main_matlab/DSP281x_Device.h @@ -0,0 +1,3 @@ + +#include "DSP2833x_Device.h" +//#define int16 int diff --git a/Inu/Src/main_matlab/DSP281x_Ev.h b/Inu/Src/main_matlab/DSP281x_Ev.h new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Inu/Src/main_matlab/DSP281x_Ev.h @@ -0,0 +1 @@ + diff --git a/Inu/Src/main_matlab/DSP281x_Examples.h b/Inu/Src/main_matlab/DSP281x_Examples.h new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Inu/Src/main_matlab/DSP281x_Examples.h @@ -0,0 +1 @@ + diff --git a/Inu/Src/main_matlab/DSP281x_GlobalPrototypes.h b/Inu/Src/main_matlab/DSP281x_GlobalPrototypes.h new file mode 100644 index 0000000..139597f --- /dev/null +++ b/Inu/Src/main_matlab/DSP281x_GlobalPrototypes.h @@ -0,0 +1,2 @@ + + diff --git a/Inu/Src/main_matlab/DSP281x_SWPrioritizedIsrLevels.h b/Inu/Src/main_matlab/DSP281x_SWPrioritizedIsrLevels.h new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Inu/Src/main_matlab/DSP281x_SWPrioritizedIsrLevels.h @@ -0,0 +1 @@ + diff --git a/Inu/Src/main_matlab/IQmathLib.h b/Inu/Src/main_matlab/IQmathLib.h new file mode 100644 index 0000000..67bfcf2 --- /dev/null +++ b/Inu/Src/main_matlab/IQmathLib.h @@ -0,0 +1,684 @@ +//#define __IQMATHLIB_H_INCLUDED__ + + +/** +* Иммитация библиотеки 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 divide19(long num, long den); +long divideN(long num, long den, unsigned int d); +long sin_fixed(long x); +long cos_fixed(long x); +long sqrt_fixed(long x); +long exp_fixed(long x); +long exp_fixedN(long x, unsigned int n); + +#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 _IQ18mpy(A,B) multiply_fixed_base_select(A,B,18) + +#define _IQdiv(A,B) divide(A,B) +#define _IQ19div(A,B) divide19(A,B) +#define _IQ18div(A,B) divideN(A,B,18) +#define _IQsin(A) sin_fixed(A) +#define _IQcos(A) cos_fixed(A) +#define _IQsqrt(A) sqrt_fixed(A) +#define _IQexp(A) exp_fixed(A) +#define _IQ18exp(A) exp_fixedN(A,18) + + +#define _IQmpyI32(A,B) ((A)*(B)) + + +#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 + +//########################################################################### +// If FLOAT_MATH is used, the IQmath library function are replaced by +// equivalent floating point operations: +//=========================================================================== + +#define _IQ15sqrt(A) sqrt(A) + +#endif //IQ_MATH_LIB diff --git a/Inu/Src/main_matlab/IQmathLib_matlab.c b/Inu/Src/main_matlab/IQmathLib_matlab.c new file mode 100644 index 0000000..f77984b --- /dev/null +++ b/Inu/Src/main_matlab/IQmathLib_matlab.c @@ -0,0 +1,229 @@ +#include "IQmathLib.h" +#include + + +// Преобразование числа СЃ плавающей точкой РІ число СЃ фиксированной точкой +#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 _IQmag(long a, long b) +{ + return _IQsqrt(_IQmpy(a, a) + _IQmpy(b, b)); +} + +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; +} + +long divide19(long num, long den) +{ + long long numLong = (long long)num; + long long quotient = (numLong << 19) / den; + return (long)quotient; +} + +long divideN(long num, long den, unsigned int d) +{ + long long numLong = (long long)num; + long long quotient = (numLong << d) / 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; +} + + +long exp_fixed(long x) +{ + // static long FIXED_2PI = float_to_fixed(TWO_PI); + float f = _IQtoF(x); + float r1 = exp(f); + if (r1>127) r1=127; + if (r1<-127) r1=-127; + long r2 = _IQ(r1); + + return r2; +} + + +long exp_fixedN(long x, unsigned int n) +{ + if (n==18) + { + float f = _IQ18toF(x); + float r1 = exp(f); + if (r1>8100) r1=8100; + if (r1<-8100) r1=-8100; + long r2 = _IQ(r1); + + return r2; + } +} \ No newline at end of file diff --git a/Inu/Src/main_matlab/adc_tools.h b/Inu/Src/main_matlab/adc_tools.h new file mode 100644 index 0000000..598df53 --- /dev/null +++ b/Inu/Src/main_matlab/adc_tools.h @@ -0,0 +1,445 @@ +#ifndef _ADC_TOOLS +#define _ADC_TOOLS + +#include "IQmathLib.h" +#include "xp_project.h" + +#define COUNT_DETECT_ZERO 3000 + +#define COUNT_ARR_ADC_BUF_FAST_POINT 10 + +#define NORMA_ACP 3000.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 + +#define DELTA_ACP_TEMPER 0.0 // датчики блоки pt100 даеют постоянное смещение 0.0 градусов, так настроен блок SG3013 + +#define NORMA_ACP_P 100.0 +#define ADC_READ_FROM_PARALLEL_BUS 1 + +#define DEFAULT_ZERO_ADC 2048 + +#ifndef USE_INTERNAL_ADC +#define USE_INTERNAL_ADC 0 +#endif + + +#if (USE_INTERNAL_ADC==1) +#define COUNT_ARR_ADC_BUF (C_adc_number+1) +#else +#define COUNT_ARR_ADC_BUF C_adc_number +#endif + +#define COUNT_ARR_ADC_BUF_EXTERNAL C_adc_number + + + +// 23550.3 + +#if(C_adc_number>=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; + + _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; + + + _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; + + + /* + _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; + +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; + +#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; +extern ANALOG_RAW_DATA rawData; +extern _iq iq_norm_ADC[COUNT_ARR_ADC_BUF][16]; + +//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); +void init_Adc_Variables(void); +void detect_zero_analog(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 const R_ADC[COUNT_ARR_ADC_BUF][16]; +extern unsigned int const K_LEM_ADC[COUNT_ARR_ADC_BUF][16]; +extern float K_NORMA_ADC[COUNT_ARR_ADC_BUF][16]; + +//void norma_all_adc(void); + + +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/Src/main_matlab/adc_tools_matlab.c b/Inu/Src/main_matlab/adc_tools_matlab.c new file mode 100644 index 0000000..c2ffe34 --- /dev/null +++ b/Inu/Src/main_matlab/adc_tools_matlab.c @@ -0,0 +1,748 @@ +// #include "project.h" +#include "adc_tools.h" +// #include "xp_project.h" +#include "IQmathLib.h" +#include "math.h" +#include "filter_v1.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 + + + +#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 const 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 const 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 + +#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; + + +ANALOG_RAW_DATA rawData = {0}; + + + + +void calc_norm_ADC(void); +void analog_values_calc(void); +_iq im_calc( _iq ia, _iq ib, _iq ic); + +#define Shift_Filter 1 //2 +void read_adc() +{ + static unsigned int adr_adc = 0; + if(project.adc[0].status == component_Ready) + { + 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; + 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; + } + if(project.adc[1].status == component_Ready) + { + project.adc->read_pbus(&project.adc[1]); + // adr_adc = project.adc[1].adr_pbus.adr_table[0]; + ADC_f[1][0] = project.adc[1].read.pbus.adc_value[0]; + ADC_f[1][1] = project.adc[1].read.pbus.adc_value[1]; + ADC_f[1][2] = project.adc[1].read.pbus.adc_value[2]; + ADC_f[1][3] = project.adc[1].read.pbus.adc_value[3]; + + 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; + /* Not used + ADC_sf[1][4] += (ADC_f[1][4] - ADC_sf[1][4]) >> Shift_Filter; + ADC_sf[1][5] += (ADC_f[1][5] - ADC_sf[1][5]) >> Shift_Filter; + ADC_sf[1][6] += (ADC_f[1][6] - ADC_sf[1][6]) >> Shift_Filter; + ADC_sf[1][7] += (ADC_f[1][7] - ADC_sf[1][7]) >> Shift_Filter; + ADC_sf[1][8] += (ADC_f[1][8] - ADC_sf[1][8]) >> Shift_Filter; + ADC_sf[1][9] += (ADC_f[1][9] - ADC_sf[1][9]) >> Shift_Filter; + ADC_sf[1][10] += (ADC_f[1][10] - ADC_sf[1][10]) >> Shift_Filter; + ADC_sf[1][11] += (ADC_f[1][11] - ADC_sf[1][11]) >> Shift_Filter; + ADC_sf[1][12] += (ADC_f[1][12] - ADC_sf[1][12]) >> Shift_Filter; + ADC_sf[1][13] += (ADC_f[1][13] - ADC_sf[1][13]) >> Shift_Filter; + ADC_sf[1][14] += (ADC_f[1][14] - ADC_sf[1][14]) >> Shift_Filter; + ADC_sf[1][15] += (ADC_f[1][15] - ADC_sf[1][15]) >> Shift_Filter; + */ + } +} + +void acp_Handler(void) +{ + //read_adc(); + calc_norm_ADC_0(0); + //analog_values_calc(); + // Read_Fast_Errors(); +} + +//#define COUNT_DETECT_ZERO 500 + + + +void init_Adc_Variables(void) +{ + unsigned int k, i, j; + int* panalog, * pfilter; + volatile float k_f; + + + for (i = 0; i < COUNT_ARR_ADC_BUF; i++) + { + for (k = 0; k < 16; k++) + { + ADC_f[i][k] = 0; + ADC_sf[i][k] = 0; + + for (j = 0; j < COUNT_ARR_ADC_BUF_FAST_POINT; j++) + ADC_fast[i][k][j] = 0; + + k_f = K_LEM_ADC[i][k] * 2.5 / R_ADC[i][k] / 4096.0; + k_norm_ADC[i][k] = _IQ19(k_f); + + k_f = K_LEM_ADC[i][k] * 250.0 / R_ADC[i][k] / K_NORMA_ADC[i][k] / 4096.0; + iq19_k_norm_ADC[i][k] = _IQ19(k_f); + + zero_ADC[i][k] = DEFAULT_ZERO_ADC;//1835; + + iq19_zero_ADC[i][k] = _IQ19(zero_ADC[i][k]); //_IQ19(2030);//_IQ19(1770); + + } + } + + + panalog = (int*)&analog; + pfilter = (int*)&filter; + for (k = 0; k < sizeof(ANALOG_VALUE) / sizeof(int); k++) + { + + *(panalog + k) = 0; + *(pfilter + k) = 0; + } + + + for (i = 0; i < COUNT_ARR_ADC_BUF; i++) + { + if (project.adc[i].status >= 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; i < COUNT_ARR_ADC_BUF; i++) + { + if ((zero_ADC[i][k] > 2200) || (zero_ADC[i][k] < 1900)) + zero_ADC[i][k] = DEFAULT_ZERO_ADC; + } + } + + + + for (k = 0; k < 16; k++) + { + for (i = 0; i < COUNT_ARR_ADC_BUF; i++) + { + iq19_zero_ADC[i][k] = _IQ19(zero_ADC[i][k]);//_IQ19(1770); + } + } + + + // koef_allADC_filter = _IQ19(0.00002/0.00003185);//koef_ADC_filter[0]; + // koef_zero_ADC_filter=_IQ19(0.00002/0.0003185); + koef_Uzpt_long_filter = _IQ(0.001 / 0.016666); + koef_Uzpt_fast_filter = _IQ(0.001 / 0.002); //_IQ(0.001/0.00131); + koef_Uin_filter = _IQ(0.001 / 0.00931); + + // koef_Im_filter = _IQ(0.001/0.006); + koef_Im_filter = _IQ(0.001 / 0.065); + koef_Power_filter = _IQ(0.001 / 0.065); + koef_Power_filter2 = _IQ(0.001 / 0.2); + + // koef_Iabc_filter = _IQ(0.001/0.006); + + + filter.iqU_1_fast = 0; + filter.iqU_1_long = 0; + + filter.iqU_2_fast = 0; + filter.iqU_2_long = 0; + + filter.iqUin_m1 = 0; + filter.iqUin_m2 = 0; + + + // filter.iqU_3_fast = 0; + // filter.iqU_4_fast = 0; + + // filter.iqU_1_long = 0; + // filter.iqU_2_long = 0; + // filter.iqU_3_long = 0; + // filter.iqU_4_long = 0; + + // filter.iqIin_1 = 0; + // filter.iqIin_2 = 0; + + + filter.iqIm_1 = 0; + filter.iqIm_2 = 0; + // filter.iqUin_m1 = 0; + // filter.iqUin_m2 = 0; + + + for (i = 0; i < COUNT_ARR_ADC_BUF; i++) + { + mask_err_adc_protect[i].plus.all = 0; + mask_err_adc_protect[i].minus.all = 0x0; + + err_adc_protect[i].plus.all = 0; + err_adc_protect[i].minus.all = 0x0; + } + +#if (USE_INTERNAL_ADC==1) + Init_Internal_Adc(); +#endif + + +} + +inline _iq norma_adc(int plane, int chan, int n_norm) +{ + // 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_sf[plane][chan]<<19) - iq19_zero_ADC[n_norm]),iq19_k_norm_ADC[n_norm])); + return _IQ19toIQ(_IQ19mpy((((long)ADC_sf[plane][chan]<<19)),iq19_k_norm_ADC[n_norm])); +} +//////////////////////////////////////////////////////////////////// + +#pragma CODE_SECTION(norma_adc_nc,".fast_run"); +#define SDVIG_K_FILTER_S 2 //1//(27.08.2009) //3 +void norma_adc_nc(int nc) +{ + int k; + // int bb; + + 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])); + } +} + +//////////////////////////////////////////////////////////////////// +#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); + +} +//////////////////////////////////////////////////////////////////// +void analog_values_calc(void) +{ + +//#if (USE_ADC_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); + + +} + +#define CONST_IQ_11PI6 96629827 //11Pi/6 +#define CONST_IQ_PI6 8784529 // Pi/6 + + +/********************************************************************/ +/* Определение РЅСѓР»y показаний АЦП */ +/********************************************************************/ +#pragma DATA_SECTION(acp_zero,".slow_vars") +_iq19 acp_zero[16]; +#pragma DATA_SECTION(acp_summ,".slow_vars") +long acp_summ[16]; +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 < COUNT_DETECT_ZERO; i++) + { + // norma_all_adc(); + norma_adc_nc(nc); + // norma_adc_nc(1); + + + for (k = 0; k < 16; k++) + { + acp_zero[k] = exp_regul_iq(koef_zero_ADC_filter, acp_zero[k], ((long)ADC_f[nc][k] << 19)); + acp_summ[k] = acp_summ[k] + ADC_f[nc][k]; + } + + + + pause_1000(1000); + } + + // 16 Рё 7 канал пропускаем С‚.Рє. это канал напрyжениy + for (k = 0; k < 16; k++) + { + + // if ((k==15)) + // { + // if (ADC_sf[k]in_plane.write.regs.comand_reg.bit.update_registers = 1; +// // write_command_reg(&rs->in_plane); +//// rs->in_plane.write.regs.comand_reg.bit.update_registers = 0; +//} +//void angle_sensor_read(T_cds_angle_sensor *as) +//{} +// +//void angle_plane_set(T_cds_angle_sensor *rs) +//{} +//void in_plane_set(T_cds_in_rotation_sensor* rs) +//{} +// +//void in_sensor_read1(T_cds_in_rotation_sensor *rs) +//{} +// +//void in_sensor_read2(T_cds_in_rotation_sensor *rs) +//{} + + + + +// unsigned int BWC_Started() +// { + +// } + +void inc_RS_timeout_cicle(void) +{ + +} + +void inc_CAN_timeout_cicle(void) +{ + +} + +void pause_1000(void) +{ + +} + +int xerror(unsigned int er_ID, void* CallBackRef) +{ +}; +unsigned int ReadMemory(unsigned long addr) +{ + return (*(volatile int *)(addr)); +} + + +void WriteMemory(unsigned long addr, unsigned int data) +{ + (*(volatile int *)( addr )) = data; +} + +void start_pwm(void) +{ + // mPWM_a = 1; + // mPWM_b = 1; +} + +void stop_pwm(void) +{ + // mPWM_a = 0; + // mPWM_b = 0; + + // svgen_set_time_keys_closed(&svgen_pwm24_1); + // svgen_set_time_keys_closed(&svgen_pwm24_2); + // WriteMemory(ADR_TK_MASK_0, 0xFFFF); + // WriteMemory(ADR_PWM_START_STOP, 0x8000); +} +void start_break_pwm() { + +} + + +void stop_break_pwm() { + +} + +void start_pwm_b() { + +} +void start_pwm_a() { + +} +void stop_pwm_b() { + +} +void stop_pwm_a() { + +} + +void fillADClogs() { + +} +void break_resistor_managment_calc(){ + +} + + +void break_resistor_managment_init(){ + +} + + +void break_resistor_managment_update(){ + +} + + +void break_resistor_recup_calc(){ + +} + + +void break_resistor_set_closed(){ + +} + + +void DetectI_Out_BreakFase(){ + +} + + +void test_mem_limit(){ + +} + + +void set_start_mem(){ + +} + + +void getFastLogs(){ + +} + + +//void detect_I_M_overload{ +// +// } + + +void sync_inc_error(){ + +} + + +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; + } + + +} diff --git a/Inu/Src/main_matlab/main_matlab.h b/Inu/Src/main_matlab/main_matlab.h new file mode 100644 index 0000000..fe79cab --- /dev/null +++ b/Inu/Src/main_matlab/main_matlab.h @@ -0,0 +1,22 @@ + + +#include "vector.h" +#include "edrk_main.h" + +typedef union { + //unsigned int all; + int all; +// struct MODBUS_BITS_STRUCT bit; +// struct MODBUS_WORD_STRUCT byte; +} MODBUS_REG_STRUCT; + + +//extern MODBUS_REG_STRUCT modbus_table_in[1024]; +//extern MODBUS_REG_STRUCT modbus_table_out[1024]; + +void init_flag_a(void); + + + + + diff --git a/Inu/Src/main_matlab/old/PWMTools.c b/Inu/Src/main_matlab/old/PWMTools.c new file mode 100644 index 0000000..561b909 --- /dev/null +++ b/Inu/Src/main_matlab/old/PWMTools.c @@ -0,0 +1,719 @@ +#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/Src/main_matlab/old/v_pwm24.c b/Inu/Src/main_matlab/old/v_pwm24.c new file mode 100644 index 0000000..e15475e --- /dev/null +++ b/Inu/Src/main_matlab/old/v_pwm24.c @@ -0,0 +1,1642 @@ + + +#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, +_iq* maxUq1, _iq* maxUq2, _iq* Uq1Out, _iq* Uq2Out) +{ +// DQ_TO_ALPHABETA dq_to_ab = DQ_TO_ALPHABETA_DEFAULTS; + //_iq maxUq = 0; + + *maxUq1 = 0; + *maxUq2 = 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; + } + //Ограничил коэффициент модуляции + *maxUq1 = _IQsqrt(Uzad_max_square - _IQmpy(Ud, Ud)); + if (Uq > (*maxUq1)) { Uq = (*maxUq1);} + if (Uq < -(*maxUq1)) { Uq = -(*maxUq1);} + *Uq1Out = Uq; + + //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); + + //Ограничил коэффициент модуляции + *maxUq2 = _IQsqrt(Uzad_max_square - _IQmpy(Ud, Ud)); + if (Uq2 > *maxUq2) { Uq2 = *maxUq2;} + if (Uq2 < -(*maxUq2)) { Uq2 = -(*maxUq2);} + +*Uq2Out = Uq2; + + //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/Src/main_matlab/old/v_pwm24.h b/Inu/Src/main_matlab/old/v_pwm24.h new file mode 100644 index 0000000..da8f527 --- /dev/null +++ b/Inu/Src/main_matlab/old/v_pwm24.h @@ -0,0 +1,190 @@ +#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, _iq* maxUq1, _iq* maxUq2, _iq* Uq1Out, _iq* Uq2Out); +//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/Src/main_matlab/old/xp_write_xpwm_time.h b/Inu/Src/main_matlab/old/xp_write_xpwm_time.h new file mode 100644 index 0000000..5e1159d --- /dev/null +++ b/Inu/Src/main_matlab/old/xp_write_xpwm_time.h @@ -0,0 +1,183 @@ +/* + * 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/Src/main_matlab/rotation_speed_matlab.c b/Inu/Src/main_matlab/rotation_speed_matlab.c new file mode 100644 index 0000000..74c91d5 --- /dev/null +++ b/Inu/Src/main_matlab/rotation_speed_matlab.c @@ -0,0 +1,428 @@ +#include "rotation_speed.h" +#include "xp_rotation_sensor.h" +#include "IQmathLib.h" +#include "params.h" +#include "adc_tools.h" +#include "rmp_cntl_my1.h" +#include "my_filter.h" +#include "vector.h" +#include "TuneUpPlane.h" //светодиодик +#include "errors.h" +#include "log_to_memory.h" +#include "DSP281x_Device.h" //for int16 + +#pragma DATA_SECTION(rotor,".fast_vars1"); +ROTOR_VALUE rotor = ROTOR_VALUE_DEFAULTS; + +RMP_MY1 rmp_wrot = RMP_MY1_DEFAULTS; + +_iq koef_Wout_filter = _IQ(0.2); //_IQ(0.15); +_iq koef_Wout_filter_long = _IQ(0.12);//_IQ(0.03); + + +static _iq impulses_To_iqF(unsigned int time, unsigned int impulses); +static _iq counter_To_iqF(unsigned int count, unsigned int freq_mode); +static _iq delta_Angle_To_iqF(unsigned long delta, unsigned int period); +static void sort_F_array(_iq *array, unsigned int size); + +void rotorInit(void) +{ + unsigned int i = 0, size = 0, *pint = 0; + + rmp_wrot.RampLowLimit = 0; + rmp_wrot.RampHighLimit = _IQ(1); + 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; + } + + koef_Wout_filter = _IQ(0.2); //_IQ(0.15); + koef_Wout_filter_long = _IQ(0.001); //_IQ(0.0005); +} + +#pragma CODE_SECTION(update_rot_sensors,".fast_run"); +void update_rot_sensors() +{ +// rotation_sensor.update_registers(&rotation_sensor); +} + +#pragma CODE_SECTION(Rotor_measure,".fast_run"); +void Rotor_measure(void) +{ + static unsigned long PrevAngle1 = 0, PrevAngle2 = 0, PrevAngle3 = 0, PrevAngle4 = 0; + static unsigned int peroid_shim_mks = (unsigned int)(1000000L / FREQ_PWM / 2.0); + static _iq prev_wrot = 0; + static unsigned int prev_wrot_count = 0; + static int direct_accum = 0; + static int sens1_err_count = 0; + static int sens2_err_count = 0; + + +// unsigned int s_number = 0, begin_data = 0, end_data = 0, i; +// _iq accumulator = 0, deltaF = 0, deltaAngle = 0; +// rotation_sensor.read_sensors(&rotation_sensor); +// if(rotation_sensor.in_plane.read.regs.comand_reg.bit.update_registers) +// { +// rotor.error_update_count ++; +// } +// // logpar.log22 = rotation_sensor.in_plane.out.CountOne1; +// // logpar.log23 = rotation_sensor.in_plane.out.CountZero1; +// if(rotation_sensor.use_sensor1) +// { +// if((rotation_sensor.in_plane.out.CountOne1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1) +// || rotation_sensor.in_plane.out.CountOne1 == 65535) +// { rotation_sensor.in_plane.out.CountOne1 = 0; } +// if((rotation_sensor.in_plane.out.CountZero1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1) +// || rotation_sensor.in_plane.out.CountZero1 == 65535) +// { rotation_sensor.in_plane.out.CountZero1 = 0; } + +// //Когда импульсов мало считаем по периоду +// if (rotation_sensor.in_plane.out.Impulses1 < 5) { +// if(rotation_sensor.in_plane.out.CountOne1) { +// rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne1, +// rotation_sensor.in_plane.out.counter_freq1); +// } +// if(rotation_sensor.in_plane.out.CountZero1) { +// rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero1, +// rotation_sensor.in_plane.out.counter_freq1); +// } +// } + +// if(rotation_sensor.in_plane.out.Impulses1 > 2) +// { +// rotor.iqFsensors[s_number++] = impulses_To_iqF(rotation_sensor.in_plane.out.Time1, +// rotation_sensor.in_plane.out.Impulses1); +// // logpar.log11 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]); +// } else { +// // logpar.log11 = 0; +// } + +// if (rotor.iqF > 139810L)//10 rpm +// { +// if (rotation_sensor.in_plane.out.CountOne1 == 0 && rotation_sensor.in_plane.out.CountZero1 == 0 +// && rotation_sensor.in_plane.out.Impulses1 == 0) { +// sens1_err_count += 1; +// } else { +// sens1_err_count = 0; +// } +// if (sens1_err_count > 50) { +// sens1_err_count = 50; +// faults.faults4.bit.Speed_Datchik_1_Off |= 1; +// } +// } else { +// sens1_err_count = 0; +// } +// } + +// // logpar.log4 = rotation_sensor.in_plane.out.CountOne2; +// // logpar.log20 = rotation_sensor.in_plane.out.CountZero2; +// if(rotation_sensor.use_sensor2) +// { +// if((rotation_sensor.in_plane.out.CountOne2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) +// || rotation_sensor.in_plane.out.CountOne2 == 65535) +// { rotation_sensor.in_plane.out.CountOne2 = 0; } +// if((rotation_sensor.in_plane.out.CountZero2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) +// || rotation_sensor.in_plane.out.CountZero2 == 65535) +// { rotation_sensor.in_plane.out.CountZero2 = 0; } + +// //Кодда импульсов мало, считаем по периоду +// if (rotation_sensor.in_plane.out.Impulses2 < 5) { +// if(rotation_sensor.in_plane.out.CountOne2) { +// rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne2, +// rotation_sensor.in_plane.out.counter_freq2); +// } +// if(rotation_sensor.in_plane.out.CountZero2) { +// rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero2, +// rotation_sensor.in_plane.out.counter_freq2); +// } +// } + +// if(rotation_sensor.in_plane.out.Impulses2 > 2) +// { +// rotor.iqFsensors[s_number++] = impulses_To_iqF(rotation_sensor.in_plane.out.Time2, +// rotation_sensor.in_plane.out.Impulses2); +// // logpar.log16 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]); +// } + +// if (rotor.iqF > 139810L)//10 rpm +// { +// if (rotation_sensor.in_plane.out.CountOne2 == 0 && rotation_sensor.in_plane.out.CountZero2 == 0 +// && rotation_sensor.in_plane.out.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; +// } +// } else { +// sens2_err_count = 0; +// } + +// } + +// if(rotation_sensor.use_angle_plane && (s_number < SENSORS_NUMBER)) +// { +// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel1_enable && rotor.iqFsensors[0] && (s_number < (SENSORS_NUMBER - 1))) +// { +// if(rotation_sensor.rotation_plane.out.Delta_angle1 < 1500) +// { +// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle1, +// rotation_sensor.rotation_plane.out.survey_time_mks); + +// } + +// deltaAngle = _IQabs(rotation_sensor.rotation_plane.out.Current_angle1 - PrevAngle1); +// if(deltaAngle > 1500) +// { +// deltaAngle = 262144 - deltaAngle; +// } +// if((deltaAngle < 291) && (deltaAngle >= 0)) +// { +// rotor.iqFsensors[s_number++] = +// delta_Angle_To_iqF(deltaAngle, peroid_shim_mks); +// } +// PrevAngle1 = rotation_sensor.rotation_plane.out.Current_angle1; +// } +// // else +// // { +// // rotor.iqFsensors[s_number++] = 0; +// // rotor.iqFsensors[s_number++] = 0; +// // } +// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel2_enable && (s_number < (SENSORS_NUMBER - 1))) +// { +// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle2, +// rotation_sensor.rotation_plane.out.survey_time_mks); +// rotor.iqFsensors[s_number++] = +// delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle2 - PrevAngle2), +// peroid_shim_mks); +// PrevAngle2 = rotation_sensor.rotation_plane.out.Current_angle2; +// } +// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel3_enable && (s_number < (SENSORS_NUMBER - 1))) +// { +// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle3, +// rotation_sensor.rotation_plane.out.survey_time_mks); +// rotor.iqFsensors[s_number++] = +// delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle3 - PrevAngle3), +// peroid_shim_mks); +// PrevAngle3 = rotation_sensor.rotation_plane.out.Current_angle3; +// } +// if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel4_enable && (s_number < (SENSORS_NUMBER - 1))) +// { +// rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle4, +// rotation_sensor.rotation_plane.out.survey_time_mks); +// rotor.iqFsensors[s_number++] = +// delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle4 - PrevAngle4), +// peroid_shim_mks); +// PrevAngle2 = rotation_sensor.rotation_plane.out.Current_angle4; +// } +// } +// // logpar.log22 = (int16) _IQtoIQ15(rotor.iqFsensors[0]); +// // logpar.log23 = (int16) _IQtoIQ15(rotor.iqFsensors[1]); +// // logpar.log24 = (int16) _IQtoIQ15(rotor.iqFsensors[2]); +// // logpar.log25 = (int16) _IQtoIQ15(rotor.iqFsensors[3]); +// // logpar.log29 = (int16) _IQtoIQ15(rotor.iqFsensors[4]); +// // logpar.log30 = (int16) _IQtoIQ15(rotor.iqFsensors[5]); + +// 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.iqFsensors, s_number); +// deltaF = rotor.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.iqFout - rotor.iqFsensors[i]) >= deltaF) +// { +// i++; +// } +// else +// { +// break; +// } +// } +// if(i < s_number) { begin_data = i; } +// else {begin_data = 0;} +// while((i < s_number) && (_IQabs(rotor.iqFout - rotor.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.iqFsensors[i]; +// } + +// if(end_data != begin_data) +// { +// rotor.iqF = accumulator / (end_data - begin_data); +// prev_wrot_count = 0; +// } +// else +// { +// rotor.iqF = prev_wrot; +// prev_wrot_count += 1; +// } + +// // logpar.log19 = (int16)(_IQtoIQ15(rotor.iqF)); + +// if (prev_wrot != rotor.iqF || rotor.iqF==0 ) +// { +// rmp_wrot.DesiredInput = rotor.iqF; +// rmp_wrot.calc(&rmp_wrot); +// rotor.iqF = rmp_wrot.Out; +// } +// prev_wrot=rotor.iqF; +// //От зависаниЙа старого значениЙа. +// if (prev_wrot_count > 10) { +// prev_wrot = 0; +// prev_wrot_count = 10; +// } + +// rotor.iqFout = exp_regul_iq(koef_Wout_filter, rotor.iqFout, rotor.iqF); +// rotor.iqFlong = exp_regul_iq(koef_Wout_filter_long, rotor.iqFlong, rotor.iqF); + +// rotor.direct_rotor_in1 = rotation_sensor.in_plane.out.direction1; +// rotor.direct_rotor_in2 = rotation_sensor.in_plane.out.direction2; +// rotor.direct_rotor_angle = rotation_sensor.rotation_plane.out.direction; + +// rotor.error.sens_err1 = rotation_sensor.in_plane.read.pbus.direction.bit.sens_err1; +// rotor.error.sens_err2 = rotation_sensor.in_plane.read.pbus.direction.bit.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.iqFout >139810L) //10ob/min +// { +// if((rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0) +// { +// direct_accum++; +// } +// else if((rotor.direct_rotor_in1 + rotor.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.direct_rotor = direct_accum > 0 ? 1 : +// direct_accum < 0 ? -1 : +// 0; +// // if (f.flag_second_PCH) { +// // rotor.direct_rotor = - rotor.direct_rotor; +// // } +// } +// else +// { +// 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 (f.flag_second_PCH) { +// // rotor.direct_rotor = - rotor.direct_rotor; +// // } +// direct_accum = rotor.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; +// } +} + +#pragma CODE_SECTION(impulses_To_iqF,".fast_run"); +_iq impulses_To_iqF(unsigned int time, unsigned int impulses) //time mks. impulses count +{ + //Flong = (impulses / time / IMPULSES_PER_TURN) * _IQ(1) / NORMA_FROTOR; + static unsigned long long koeff_to_F = 16777216LL / IMPULSES_PER_TURN * 1000000LL / NORMA_FROTOR; + long long Flong = (long long)impulses * koeff_to_F / time; + return (_iq)Flong; +} + +//Frot = Fimp / IMPULSES_PER_TURN / NORMA_FROTOR = +// = counter_freq / counter / 2 / IMPULSES_PER_TURN / NORMA_FROTOR +//делю на 2, т.к. считаем длительность половины периода +//дромножаю на (1LL << 24) чтобы привести к iq24 +#pragma CODE_SECTION(counter_To_iqF,".fast_run"); +_iq counter_To_iqF(unsigned int count, unsigned int freq_mode) +{ + static long long koeff_to_F = 60000000LL * _IQ(1) / IMPULSES_PER_TURN / 2 / NORMA_FROTOR; + long long Flong = 0; + if(freq_mode == 0) // 60Mhz + { + Flong = koeff_to_F / count / 100; + } + else //600KHz + { + Flong = koeff_to_F / count; + } + return Flong; +} + +#pragma CODE_SECTION(delta_Angle_To_iqF,".fast_run"); +_iq delta_Angle_To_iqF(unsigned long delta, unsigned int period) +{ + // iqF = (delta / ANGLE_RESOLUTION) / (period * 10^-6) * (1LL << 24) / NORMA_FROTOR; + // iqF = delta * 10^6 * (1LL << 24) / ANGLE_RESOLUTION / period / NORMA_FROTOR; + static long long koeff_to_F = 1000000LL * (1LL << 24) / ANGLE_RESOLUTION / NORMA_FROTOR; + return (_iq)(koeff_to_F * delta / period); +} + +#pragma CODE_SECTION(sort_F_array,".fast_run2"); +void sort_F_array(_iq *array, unsigned int size) +{ + unsigned int i, j; + _iq tmp = 0; + for(i = size; i > 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; + } + } + } +} + diff --git a/Inu/Src/main_matlab/v_pwm24_matlab.c b/Inu/Src/main_matlab/v_pwm24_matlab.c new file mode 100644 index 0000000..9df9ad3 --- /dev/null +++ b/Inu/Src/main_matlab/v_pwm24_matlab.c @@ -0,0 +1,1644 @@ + + +#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 maxUq1 = 0; + _iq maxUq2 = 0; + _iq Uq1Out = 0; + _iq Uq2Out = 0; + + maxUq1 = 0; + maxUq2 = 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; + } + //Ограничил коэффициент модуляции + maxUq1 = _IQsqrt(Uzad_max_square - _IQmpy(Ud, Ud)); + if (Uq > (maxUq1)) { Uq = (maxUq1);} + if (Uq < -(maxUq1)) { Uq = -(maxUq1);} + Uq1Out = Uq; + + //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); + + //Ограничил коэффициент модуляции + maxUq2 = _IQsqrt(Uzad_max_square - _IQmpy(Ud, Ud)); + if (Uq2 > maxUq2) { Uq2 = maxUq2;} + if (Uq2 < -(maxUq2)) { Uq2 = -(maxUq2);} + +Uq2Out = Uq2; + + //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/Src/main_matlab/xp_write_xpwm_time_matlab.c b/Inu/Src/main_matlab/xp_write_xpwm_time_matlab.c new file mode 100644 index 0000000..093b88c --- /dev/null +++ b/Inu/Src/main_matlab/xp_write_xpwm_time_matlab.c @@ -0,0 +1,361 @@ +/* + * 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/VectorControl/abc_to_alphabeta.c b/Inu/Src2/VectorControl/abc_to_alphabeta.c new file mode 100644 index 0000000..7f37475 --- /dev/null +++ b/Inu/Src2/VectorControl/abc_to_alphabeta.c @@ -0,0 +1,23 @@ +#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 new file mode 100644 index 0000000..eb16b58 --- /dev/null +++ b/Inu/Src2/VectorControl/abc_to_alphabeta.h @@ -0,0 +1,39 @@ +#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 new file mode 100644 index 0000000..39c414d --- /dev/null +++ b/Inu/Src2/VectorControl/abc_to_dq.c @@ -0,0 +1,38 @@ +#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 new file mode 100644 index 0000000..1afd618 --- /dev/null +++ b/Inu/Src2/VectorControl/abc_to_dq.h @@ -0,0 +1,42 @@ +#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 new file mode 100644 index 0000000..a161888 --- /dev/null +++ b/Inu/Src2/VectorControl/alphabeta_to_abc.c @@ -0,0 +1,24 @@ +#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 new file mode 100644 index 0000000..c5dca95 --- /dev/null +++ b/Inu/Src2/VectorControl/alphabeta_to_abc.h @@ -0,0 +1,38 @@ +#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 new file mode 100644 index 0000000..026a212 --- /dev/null +++ b/Inu/Src2/VectorControl/alphabeta_to_dq.c @@ -0,0 +1,22 @@ +#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 new file mode 100644 index 0000000..cd7ac27 --- /dev/null +++ b/Inu/Src2/VectorControl/alphabeta_to_dq.h @@ -0,0 +1,32 @@ +#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 new file mode 100644 index 0000000..e0dd2f8 --- /dev/null +++ b/Inu/Src2/VectorControl/dq_to_alphabeta_cos.c @@ -0,0 +1,39 @@ +#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 new file mode 100644 index 0000000..b261ced --- /dev/null +++ b/Inu/Src2/VectorControl/dq_to_alphabeta_cos.h @@ -0,0 +1,40 @@ + + + +#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 new file mode 100644 index 0000000..16ab749 --- /dev/null +++ b/Inu/Src2/VectorControl/efficiency_compensation.c @@ -0,0 +1,107 @@ +#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 new file mode 100644 index 0000000..f1d744b --- /dev/null +++ b/Inu/Src2/VectorControl/efficiency_compensation.h @@ -0,0 +1,13 @@ +/* + * 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 new file mode 100644 index 0000000..5f050ca --- /dev/null +++ b/Inu/Src2/VectorControl/params_motor.h @@ -0,0 +1,23 @@ +/* + * 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 new file mode 100644 index 0000000..3a45fc4 --- /dev/null +++ b/Inu/Src2/VectorControl/pwm_vector_regul.c @@ -0,0 +1,666 @@ +#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 new file mode 100644 index 0000000..9b7c079 --- /dev/null +++ b/Inu/Src2/VectorControl/pwm_vector_regul.h @@ -0,0 +1,81 @@ +#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 new file mode 100644 index 0000000..8ffcb42 --- /dev/null +++ b/Inu/Src2/VectorControl/regul_power.c @@ -0,0 +1,269 @@ +#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 new file mode 100644 index 0000000..2f1b127 --- /dev/null +++ b/Inu/Src2/VectorControl/regul_power.h @@ -0,0 +1,32 @@ +#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 new file mode 100644 index 0000000..444547f --- /dev/null +++ b/Inu/Src2/VectorControl/regul_turns.c @@ -0,0 +1,185 @@ +#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 new file mode 100644 index 0000000..57529b5 --- /dev/null +++ b/Inu/Src2/VectorControl/regul_turns.h @@ -0,0 +1,12 @@ +#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 new file mode 100644 index 0000000..d76bfe5 --- /dev/null +++ b/Inu/Src2/VectorControl/teta_calc.c @@ -0,0 +1,262 @@ +#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 new file mode 100644 index 0000000..b04dda5 --- /dev/null +++ b/Inu/Src2/VectorControl/teta_calc.h @@ -0,0 +1,39 @@ +#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 new file mode 100644 index 0000000..5720d7e --- /dev/null +++ b/Inu/Src2/main/IQmathLib.c @@ -0,0 +1,182 @@ +#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 new file mode 100644 index 0000000..0e52bad --- /dev/null +++ b/Inu/Src2/main/IQmathLib.h @@ -0,0 +1,661 @@ +/** +* Иммитация библиотеки 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 new file mode 100644 index 0000000..561b909 --- /dev/null +++ b/Inu/Src2/main/PWMTools.c @@ -0,0 +1,719 @@ +#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 new file mode 100644 index 0000000..593f723 --- /dev/null +++ b/Inu/Src2/main/PWMTools.h @@ -0,0 +1,19 @@ +#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 new file mode 100644 index 0000000..4af68a7 --- /dev/null +++ b/Inu/Src2/main/adc_tools.c @@ -0,0 +1,528 @@ +// #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 new file mode 100644 index 0000000..137324f --- /dev/null +++ b/Inu/Src2/main/adc_tools.h @@ -0,0 +1,137 @@ +#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 new file mode 100644 index 0000000..95f314f --- /dev/null +++ b/Inu/Src2/main/dmctype.h @@ -0,0 +1,31 @@ +/* ================================================================================= +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 new file mode 100644 index 0000000..3fc444f --- /dev/null +++ b/Inu/Src2/main/my_filter.c @@ -0,0 +1,122 @@ +// #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 new file mode 100644 index 0000000..cb8b6d3 --- /dev/null +++ b/Inu/Src2/main/my_filter.h @@ -0,0 +1,76 @@ +#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 new file mode 100644 index 0000000..e34752d --- /dev/null +++ b/Inu/Src2/main/params.h @@ -0,0 +1,181 @@ +#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 new file mode 100644 index 0000000..4cc46c1 --- /dev/null +++ b/Inu/Src2/main/pid_reg3.c @@ -0,0 +1,107 @@ +/*===================================================================================== + 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 new file mode 100644 index 0000000..20b68c6 --- /dev/null +++ b/Inu/Src2/main/pid_reg3.h @@ -0,0 +1,117 @@ +/* ================================================================================= +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 new file mode 100644 index 0000000..a28fa49 --- /dev/null +++ b/Inu/Src2/main/rotation_speed.h @@ -0,0 +1,44 @@ +#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 new file mode 100644 index 0000000..2949fd1 --- /dev/null +++ b/Inu/Src2/main/svgen_dq.h @@ -0,0 +1,38 @@ +/* ================================================================================= +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 new file mode 100644 index 0000000..cb8b85c --- /dev/null +++ b/Inu/Src2/main/svgen_dq_v2.c @@ -0,0 +1,120 @@ +/*===================================================================================== + 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 new file mode 100644 index 0000000..357cb98 --- /dev/null +++ b/Inu/Src2/main/v_pwm24.c @@ -0,0 +1,1635 @@ + + +#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 new file mode 100644 index 0000000..ff83ca4 --- /dev/null +++ b/Inu/Src2/main/v_pwm24.h @@ -0,0 +1,190 @@ +#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 new file mode 100644 index 0000000..5245af8 --- /dev/null +++ b/Inu/Src2/main/vector.h @@ -0,0 +1,256 @@ +/* + ???? ??? (?) 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 new file mode 100644 index 0000000..d94d243 --- /dev/null +++ b/Inu/Src2/main/word_structurs.h @@ -0,0 +1,64 @@ +/* + * 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 new file mode 100644 index 0000000..093b88c --- /dev/null +++ b/Inu/Src2/main/xp_write_xpwm_time.c @@ -0,0 +1,361 @@ +/* + * 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 new file mode 100644 index 0000000..5e1159d --- /dev/null +++ b/Inu/Src2/main/xp_write_xpwm_time.h @@ -0,0 +1,183 @@ +/* + * 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/controller.c b/Inu/controller.c new file mode 100644 index 0000000..e306f9b --- /dev/null +++ b/Inu/controller.c @@ -0,0 +1,1355 @@ +/************************************************************************** + Description: Программа моделирует работу процессора - осуществляет + вызов функций init28335, detcoeff, isr. + Также моделирует различные периферийные устройства процессора + TMS320F28335/TMS320F28379D (ADC, PWM, QEP и т.д.). + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.11.08 +**************************************************************************/ + + +#include "simstruc.h" +#include "wrapper_inu.h" +#include "def.h" +#include "controller.h" +#include "pwm_vector_regul.h" + + +extern UMotorMeasure motor; + +extern TimerSimHandle t1sim +extern TimerSimHandle t2sim; +extern TimerSimHandle t3sim; +extern TimerSimHandle t4sim; +extern TimerSimHandle t5sim; +extern TimerSimHandle t6sim; +extern TimerSimHandle t7sim; +extern TimerSimHandle t8sim; +extern TimerSimHandle t9sim; +extern TimerSimHandle t10sim; +extern TimerSimHandle t11sim; +extern TimerSimHandle t12sim; + +extern DeadBandSimHandle dt1sim +extern DeadBandSimHandle dt2sim; +extern DeadBandSimHandle dt3sim; +extern DeadBandSimHandle dt4sim; +extern DeadBandSimHandle dt5sim; +extern DeadBandSimHandle dt6sim; +extern DeadBandSimHandle dt7sim; +extern DeadBandSimHandle dt8sim; +extern DeadBandSimHandle dt9sim; +extern DeadBandSimHandle dt10sim; +extern DeadBandSimHandle dt11sim; +extern DeadBandSimHandle dt12sim; + +extern void init28335(void); +extern void detcoeff(void); +extern void isr(void); +extern void input_param(unsigned short num, unsigned short val); +// extern void init_DQ_pid(); +int calcAlgUpr = 0; + +int timers_adc = 0; +int timers_pwm = 0; + +void readInputParameters(const real_T *u) { + // int t; + //// ВХОДЫ (begin) + //nn = 0; + //// аналоговые величины + //motor.udc1_ml = u[nn++];//В + //motor.udc2_ml = u[nn++];//В + + //motor.ia1_ml = u[nn++];//А + //motor.ib1_ml = u[nn++];//А + //motor.ic1_ml = u[nn++];//А + //motor.ia2_ml = u[nn++];//А + //motor.ib2_ml = u[nn++];//А + //motor.ic2_ml = u[nn++];//А + //motor.wm_ml = u[nn++];//рад/с + + //// управление (например, с ВУ) + //mst.faultReset = (unsigned short)u[nn++]; + //mst.start = (unsigned short)u[nn++]; + //mst.pzMode = (unsigned short)u[nn++]; + //mst.wmZz = u[nn++];//o.e. (от N_BAZ) + //mst.pmZz = u[nn++]*(P_NOM/S_BAZ);//o.e. (от S_BAZ) + //mst.wmLim = u[nn++];//o.e. (от N_BAZ) + //mst.pmLim = u[nn++]*(P_NOM/S_BAZ);//o.e. (от S_BAZ) + //mst.pIncrMaxTy = u[nn++]*TY*DECIM_PSI_WM_PM*(P_NOM/S_BAZ);//o.e. (от S_BAZ) + //mst.pDecrMaxTy = u[nn++]*TY*DECIM_PSI_WM_PM*(P_NOM/S_BAZ);//o.e. (от S_BAZ) + + //// + //mst.off_curr_pi = (unsigned short)u[nn++]; + //mst.only_one_km = (unsigned short)u[nn++]; + //mst.enable_compens_iq1_iq2 = (unsigned short)u[nn++]; + //mst.pi_iq_ki = u[nn++]; + //mst.pi_id_ki = u[nn++]; + //t = (unsigned short)u[nn++]; + //t = (unsigned short)u[nn++]; + //t = (unsigned short)u[nn++]; + //t = (unsigned short)u[nn++]; + //t = (unsigned short)u[nn++]; + + + //// параметры (например, с ПУ) + //paramNo = FIRST_WRITE_PAR_NUM; + //paramNew[paramNo++] = (unsigned short)u[nn++]; + //paramNew[paramNo++] = (unsigned short)u[nn++]; + + + //// ВХОДЫ (end) + + +} + + +void processSFunctionIfChanged(SimStruct *S, int_T *iW) { + + // обрабатываем параметры S-Function каждый раз, когда они изменились + if ( iW[0] == 1 ) { + iW[0] = 0; + kkk = 0; + for ( lll = 0; lll < NPARAMS; lll++ ) { + // определяем кол-во элементов в параметре + dimen = mxGetNumberOfElements(ssGetSFcnParam(S,lll)); + // обрабатываем параметр в зависимости от его размера + if ( dimen > LEN_PARAM_MATR*2 ) { + ssSetErrorStatus(S,"В параметре-массиве слишком много элементов"); + return; + } + else if ( dimen > 1 ) { + // запоминаем кол-во элементов параметра-матрицы + paramMatrDimen = dimen; + // запоминаем значения элементов параметра-матрицы + for ( mmm = 0; mmm < dimen; mmm++ ) + paramMatr[mmm] = mxGetPr(ssGetSFcnParam(S,lll))[mmm]; + } + else { + // запоминаем значения параметров-скаляров + paramScal[kkk++] = mxGetPr(ssGetSFcnParam(S,lll))[0]; + } + } + // ПАРАМЕТРЫ (begin) + nn = 0; + dt = paramScal[nn++];//шаг дискретизации (всегда должен передаваться в S-function последним!) + // ПАРАМЕТРЫ (end) + } //if ( iW[0] == 1 ) + +} + +void initialisationOnStart(int_T *iW) { +//// кое-что выполняем один раз при запуске модели +// if ( iW[1] == 1 ) { +// iW[1] = 0; +// +// timers_adc = 0; +// timers_pwm = 0; +// +// // инициализация процессора +// init28335(); +// +// init_DQ_pid(); +// +// // имитация считывания параметров из EEPROM +// // ... параметры из модели (см. блок "Parameters") +// for ( j = FIRST_WRITE_PAR_NUM; j < paramNo; j++ ) { +// param[j] = paramNew[j]; +// } +// // ... параметры из файла +// param[180] = 930;//rf.PsiZ, %*10 от PSI_BAZ +// +// param[200] = 2048;//offset.Ia1, ед. АЦП +// param[201] = 2048;//offset.Ib1, ед. АЦП +// param[202] = 2048;//offset.Ic1, ед. АЦП +// param[203] = 2048;//offset.Udc1, ед. АЦП +// param[206] = 2048;//offset.Ia2, ед. АЦП +// param[207] = 2048;//offset.Ib2, ед. АЦП +// param[208] = 2048;//offset.Ic2, ед. АЦП +// param[209] = 2048;//offset.Udc2, ед. АЦП +// +// param[210] = 100;//cc.Kp, % +// param[211] = 100;//cc.Ki, % +// param[212] = 100;//cf.Kp, % +// param[213] = 100;//cf.Ki, % +// param[214] = 100;//csp.Kp, % +// param[215] = 100;//csp.Ki, % +// +// param[220] = 99;//protect.IacMax, % от IAC_SENS_MAX +// param[221] = 130;//protect.UdcMax, % от U_NOM +// param[222] = 110;//IzLim, % от I_BAZ (д.б. больше cf.IdLim) +// param[223] = 105;//cf.IdLim, % от I_BAZ (д.б. меньше IzLim) +// param[224] = 105;//csp.IqLim, % от I_BAZ +// param[225] = 97;//protect.UdcMin, % от U_NOM +// param[226] = 115;//protect.WmMax, % от N_NOM +// param[228] = 103;//rf.WmNomPsi, % от N_NOM +// param[229] = 97;//rf.YlimPsi, % от Y_LIM +// param[231] = 300;//protect.TudcMin, мс +// param[233] = 1000;//protect.TwmMax, мс +// +// param[244] = 26000;//rs.WlimIncr, мс +// param[245] = 2000;//csp.IlimIncr, мс +// param[248] = 6000;//rp.PlimIncr, мс +// +// param[269] = 9964;//9700;//KmeCorr, %*100 +// +// param[285] = 10;//Kudc, мс*10 +// param[286] = 700;//Kwm, мс*10 +// param[288] = 250;//rs.Kwmz, мс +// param[289] = 50;//rf.Kpsiz, мс +// param[290] = 40;//Kme, мс +// param[292] = 80;//rp.Kpmz, мс +// +// param[303] = (unsigned short)(19200.);//sgmPar.Rs, мкОм +// param[304] = (unsigned short)(19364.);//sgmPar.Lls, мкГн*10 +// param[305] = (unsigned short)(8500.);//sgmPar.Rr, мкОм +// param[306] = (unsigned short)(10212.);//sgmPar.Llr, мкГн*10 +// param[307] = (unsigned short)(35810.);//sgmPar.Lm, мкГн +// +// // инициализация программы +// detcoeff(); +// +// // для моделирования таймеров +// T1Pr = (double)EPwm1Regs.TBPRD; +// T2Pr = (double)EPwm2Regs.TBPRD; +// T3Pr = (double)EPwm3Regs.TBPRD; +// T4Pr = (double)EPwm4Regs.TBPRD; +// T5Pr = (double)EPwm5Regs.TBPRD; +// T6Pr = (double)EPwm6Regs.TBPRD; +// T7Pr = (double)EPwm7Regs.TBPRD; +// T8Pr = (double)EPwm8Regs.TBPRD; +// T9Pr = (double)EPwm9Regs.TBPRD; +// T10Pr = (double)EPwm10Regs.TBPRD; +// T11Pr = (double)EPwm11Regs.TBPRD; +// T12Pr = (double)EPwm12Regs.TBPRD; +// t1cntAux = (double)EPwm1Regs.TBCTR; +// t2cntAux = (double)EPwm2Regs.TBCTR; +// t3cntAux = (double)EPwm3Regs.TBCTR; +// t4cntAux = (double)EPwm4Regs.TBCTR; +// t5cntAux = (double)EPwm5Regs.TBCTR; +// t6cntAux = (double)EPwm6Regs.TBCTR; +// t7cntAux = (double)EPwm7Regs.TBCTR; +// t8cntAux = (double)EPwm8Regs.TBCTR; +// t9cntAux = (double)EPwm9Regs.TBCTR; +// t10cntAux = (double)EPwm10Regs.TBCTR; +// t11cntAux = (double)EPwm11Regs.TBCTR; +// t12cntAux = (double)EPwm12Regs.TBCTR; +// // ... приращение счётчиков таймеров за шаг дискретизации +// TxCntPlus = FTBCLK*dt; +// +// // для моделирования eQEP +// Qposmax = (double)EQep2Regs.QPOSMAX; +// qposcnt = 1.;//(double)EQep2Regs.QPOSCNT; +// +// // для моделирования АЦП +// // (на счёт 1e-6 см. SetupAdc(), хотя там скорее не 1.0 мкс, а 0.8 мкс) +//// Tadc = (int)(1e-6/dt); +// Tadc = (int)(1/FREQ_ADC_TIMER/dt); +// +// // ... на всякий случай +// if ( Tadc < 1 ) +// Tadc = 1; +// tAdc = 1; +// // ... чтобы АЦП ждал запуска +// nAdc = 0; +// +// // для моделирования Dead-Band Unit +// CntDt = (int)(DT/dt); +// stateDt1 = stateDt2 = stateDt3 = stateDt4 = stateDt5 = stateDt6 = 1; +// stateDt7 = stateDt8 = stateDt9 = stateDt10 = stateDt11 = stateDt12 = 1; +// cntDt1 = cntDt2 = cntDt3 = cntDt4 = cntDt5 = cntDt6 = 0; +// cntDt7 = cntDt8 = cntDt9 = cntDt10 = cntDt11 = cntDt12 = 0; +// +// // для защит +// DI_24V_SOURCE_FAULT = 0; +// +// // для вывода +// inuWork = 0; +// ivc.psi = 0; +// rf.psiZ = 0; +// rs.wmZ = 0; +// csp.wmLimZi = 0; +// pm = 0; +// rp.pmZ = 0; +// csp.pmLimZi = 0; +// id1 = 0; +// iq1 = 0; +// id2 = 0; +// iq2 = 0; +// idZ = 0; +// iqZ = 0; +// cf.idP = 0; +// cf.idFF = 0; +// cf.idI = 0; +// csp.iqP = 0; +// csp.iqFF = 0; +// csp.iqI = 0; +// cc.yd1 = 0; +// cc.yq1 = 0; +// cc.y1 = 0; +// cc.y2 = 0; +// } //if ( iW[1] == 1 ) + + +} + +void simulatePWMcounterAndReadComarators(void) { + + //// Моделируем Time-Base Submodule, Counter-Compare Submodule и + //// Event-Trigger Submodule + //// ePWM1 (up-down-count mode) + //// ------------------------- + //t1cntAuxPrev = t1cntAux; + //t1cntAux += TxCntPlus; + //if ( t1cntAux > T1Pr ) { + // t1cntAux -= T1Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp1A = (double)EPwm1Regs.CMPA.half.CMPA; + // // запуск АЦП + // // tAdc = Tadc; + // // nAdc = 0; + // calcAlgUpr = 1; // ШИМ прерывание + //} + //if ( (t1cntAuxPrev < 0) && (t1cntAux >= 0) ) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp1A = (double)EPwm1Regs.CMPA.half.CMPA; + // // запуск АЦП + // // tAdc = Tadc; + // // nAdc = 0; + // calcAlgUpr = 1; // ШИМ прерывание + //} + //t1cnt = fabs(t1cntAux); + + //// ePWM2 (up-down-count mode) + //// ------------------------- + //t2cntAuxPrev = t2cntAux; + //t2cntAux += TxCntPlus; + //if ( t2cntAux > T2Pr ) { + // t2cntAux -= T2Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp2A = (double)EPwm2Regs.CMPA.half.CMPA; + //} + //if ( (t2cntAuxPrev < 0) && (t2cntAux >= 0) ) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp2A = (double)EPwm2Regs.CMPA.half.CMPA; + //} + //t2cnt = fabs(t2cntAux); + + //// ePWM3 (up-down-count mode) + //// ------------------------- + //t3cntAuxPrev = t3cntAux; + //t3cntAux += TxCntPlus; + //if ( t3cntAux > T3Pr ) { + // t3cntAux -= T3Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp3A = (double)EPwm3Regs.CMPA.half.CMPA; + //} + //if ( (t3cntAuxPrev < 0) && (t3cntAux >= 0) ) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp3A = (double)EPwm3Regs.CMPA.half.CMPA; + //} + //t3cnt = fabs(t3cntAux); + + //// ePWM4 (up-down-count mode) + //// ------------------------- + //t4cntAuxPrev = t4cntAux; + //t4cntAux += TxCntPlus; + //if ( t4cntAux > T4Pr ) { + // t4cntAux -= T4Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp4A = (double)EPwm4Regs.CMPA.half.CMPA; + //} + //if ( (t4cntAuxPrev < 0) && (t4cntAux >= 0) ) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp4A = (double)EPwm4Regs.CMPA.half.CMPA; + //} + //t4cnt = fabs(t4cntAux); + + //// ePWM5 (up-down-count mode) + //// ------------------------- + //t5cntAuxPrev = t5cntAux; + //t5cntAux += TxCntPlus; + //if ( t5cntAux > T5Pr ) { + // t5cntAux -= T5Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp5A = (double)EPwm5Regs.CMPA.half.CMPA; + //} + //if ( (t5cntAuxPrev < 0) && (t5cntAux >= 0) ) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp5A = (double)EPwm5Regs.CMPA.half.CMPA; + //} + //t5cnt = fabs(t5cntAux); + + //// ePWM6 (up-down-count mode) + //// ------------------------- + //t6cntAuxPrev = t6cntAux; + //t6cntAux += TxCntPlus; + //if ( t6cntAux > T6Pr ) { + // t6cntAux -= T6Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp6A = (double)EPwm6Regs.CMPA.half.CMPA; + //} + //if ( (t6cntAuxPrev < 0) && (t6cntAux >= 0) ) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp6A = (double)EPwm6Regs.CMPA.half.CMPA; + //} + //t6cnt = fabs(t6cntAux); + + //// ePWM7 (up-down-count mode) + //// ------------------------- + //t7cntAuxPrev = t7cntAux; + //t7cntAux += TxCntPlus; + //if (t7cntAux > T7Pr) { + // t7cntAux -= T7Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp7A = (double)EPwm7Regs.CMPA.half.CMPA; + //} + //if ( (t7cntAuxPrev < 0) && (t7cntAux >= 0) ) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp7A = (double)EPwm7Regs.CMPA.half.CMPA; + //} + //t7cnt = fabs(t7cntAux); + + //// ePWM8 (up-down-count mode) + //// ------------------------- + //t8cntAuxPrev = t8cntAux; + //t8cntAux += TxCntPlus; + //if(t8cntAux > T8Pr) { + // t8cntAux -= T8Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp8A = (double)EPwm8Regs.CMPA.half.CMPA; + //} + //if((t8cntAuxPrev < 0) && (t8cntAux >= 0)) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp8A = (double)EPwm8Regs.CMPA.half.CMPA; + //} + //t8cnt = fabs(t8cntAux); + + //// ePWM9 (up-down-count mode) + //// ------------------------- + //t9cntAuxPrev = t9cntAux; + //t9cntAux += TxCntPlus; + //if(t9cntAux > T9Pr) { + // t9cntAux -= T9Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp9A = (double)EPwm9Regs.CMPA.half.CMPA; + //} + //if((t9cntAuxPrev < 0) && (t9cntAux >= 0)) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp9A = (double)EPwm9Regs.CMPA.half.CMPA; + //} + //t9cnt = fabs(t9cntAux); + + //// ePWM10 (up-down-count mode) + //// ------------------------- + //t10cntAuxPrev = t10cntAux; + //t10cntAux += TxCntPlus; + //if(t10cntAux > T10Pr) { + // t10cntAux -= T10Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp10A = (double)EPwm10Regs.CMPA.half.CMPA; + //} + //if((t10cntAuxPrev < 0) && (t10cntAux >= 0)) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp10A = (double)EPwm10Regs.CMPA.half.CMPA; + //} + //t10cnt = fabs(t10cntAux); + + //// ePWM11 (up-down-count mode) + //// ------------------------- + //t11cntAuxPrev = t11cntAux; + //t11cntAux += TxCntPlus; + //if(t11cntAux > T11Pr) { + // t11cntAux -= T11Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp11A = (double)EPwm11Regs.CMPA.half.CMPA; + //} + //if((t11cntAuxPrev < 0) && (t11cntAux >= 0)) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp11A = (double)EPwm11Regs.CMPA.half.CMPA; + //} + //t11cnt = fabs(t11cntAux); + + //// ePWM12 (up-down-count mode) + //// ------------------------- + //t12cntAuxPrev = t12cntAux; + //t12cntAux += TxCntPlus; + //if(t12cntAux > T12Pr) { + // t12cntAux -= T12Pr*2.; + // // active CMPA load from shadow when TBCTR == TBPRD + // cmp12A = (double)EPwm12Regs.CMPA.half.CMPA; + //} + //if((t12cntAuxPrev < 0) && (t12cntAux >= 0)) { + // // active CMPA load from shadow when TBCTR == 0 + // cmp12A = (double)EPwm12Regs.CMPA.half.CMPA; + //} + //t12cnt = fabs(t12cntAux); + + + //// Моделируем работу счётчика в eQEP + //qposcnt += wm_ml/PI2*NOP*4.*dt; + //if ( qposcnt >= (Qposmax + 1.) ) + // qposcnt -= (Qposmax + 1.); + //else if ( qposcnt < 0 ) + // qposcnt += (Qposmax + 1.); + //EQep2Regs.QPOSCNT = (short)qposcnt; + +} + + +void update_norm_ADC_array() +{ + //// Udc1 + //if ( udc1_ml > UDC_SENS_MAX ) + // udc1_ml = UDC_SENS_MAX; + //else if ( udc1_ml < -UDC_SENS_MAX ) + // udc1_ml = -UDC_SENS_MAX; + //iq_norm_ADC[0] = _IQ(udc1_ml/NORMA_ACP); + //// Udc2 + //if ( udc2_ml > UDC_SENS_MAX ) + // udc2_ml = UDC_SENS_MAX; + //else if ( udc2_ml < -UDC_SENS_MAX ) + // udc2_ml = -UDC_SENS_MAX; + //iq_norm_ADC[1] = _IQ(udc2_ml/NORMA_ACP); + //// Udc3 + //if ( udc3_ml > UDC_SENS_MAX ) + // udc3_ml = UDC_SENS_MAX; + //else if ( udc3_ml < -UDC_SENS_MAX ) + // udc3_ml = -UDC_SENS_MAX; + //iq_norm_ADC[7] = _IQ(udc3_ml/NORMA_ACP); + //// Udc4 + //if ( udc4_ml > UDC_SENS_MAX ) + // udc4_ml = UDC_SENS_MAX; + //else if ( udc4_ml < -UDC_SENS_MAX ) + // udc4_ml = -UDC_SENS_MAX; + //iq_norm_ADC[8] = _IQ(udc4_ml/NORMA_ACP); + //// Idc1 + //if ( idc1_ml > IDC_SENS_MAX ) + // idc1_ml = IDC_SENS_MAX; + //else if ( idc1_ml < -IDC_SENS_MAX ) + // idc1_ml = -IDC_SENS_MAX; + //iq_norm_ADC[2] = _IQ(idc1_ml/NORMA_ACP); + //// Idc2 + //if ( idc2_ml > IDC_SENS_MAX ) + // idc2_ml = IDC_SENS_MAX; + //else if ( idc2_ml < -IDC_SENS_MAX ) + // idc2_ml = -IDC_SENS_MAX; + //iq_norm_ADC[3] = _IQ(idc2_ml/NORMA_ACP); + //// Idc3 + //if ( idc3_ml > IDC_SENS_MAX ) + // idc3_ml = IDC_SENS_MAX; + //else if ( idc3_ml < -IDC_SENS_MAX ) + // idc3_ml = -IDC_SENS_MAX; + //iq_norm_ADC[9] = _IQ(idc3_ml/NORMA_ACP); + //// Idc4 + //if ( idc4_ml > IDC_SENS_MAX ) + // idc4_ml = IDC_SENS_MAX; + //else if ( idc4_ml < -IDC_SENS_MAX ) + // idc4_ml = -IDC_SENS_MAX; + //iq_norm_ADC[10] = _IQ(idc4_ml/NORMA_ACP); + + //// Ia1 + //if ( ia1_ml > IAC_SENS_MAX ) + // ia1_ml = IAC_SENS_MAX; + //else if ( ia1_ml < -IAC_SENS_MAX ) + // ia1_ml = -IAC_SENS_MAX; + //iq_norm_ADC[4] = _IQ(ia1_ml/NORMA_ACP); + + //// Ib1 + //if ( ib1_ml > IAC_SENS_MAX ) + // ib1_ml = IAC_SENS_MAX; + //else if ( ib1_ml < -IAC_SENS_MAX ) + // ib1_ml = -IAC_SENS_MAX; + //iq_norm_ADC[5] = _IQ(ib1_ml/NORMA_ACP); + + //// Ic1 + //if ( ic1_ml > IAC_SENS_MAX ) + // ic1_ml = IAC_SENS_MAX; + //else if ( ic1_ml < -IAC_SENS_MAX ) + // ic1_ml = -IAC_SENS_MAX; + //iq_norm_ADC[6] = _IQ(ic1_ml/NORMA_ACP); + + + //// Ia2 + //if ( ia2_ml > IAC_SENS_MAX ) + // ia2_ml = IAC_SENS_MAX; + //else if ( ia2_ml < -IAC_SENS_MAX ) + // ia2_ml = -IAC_SENS_MAX; + //iq_norm_ADC[11] = _IQ(ia2_ml/NORMA_ACP); + + //// Ib2 + //if ( ib2_ml > IAC_SENS_MAX ) + // ib2_ml = IAC_SENS_MAX; + //else if ( ib2_ml < -IAC_SENS_MAX ) + // ib2_ml = -IAC_SENS_MAX; + //iq_norm_ADC[12] = _IQ(ib2_ml/NORMA_ACP); + + //// Ic2 + //if ( ic2_ml > IAC_SENS_MAX ) + // ic2_ml = IAC_SENS_MAX; + //else if ( ic2_ml < -IAC_SENS_MAX ) + // ic2_ml = -IAC_SENS_MAX; + //iq_norm_ADC[13] = _IQ(ic2_ml/NORMA_ACP); + + + + //vect_control.off_curr_pi = mst.off_curr_pi; + //vect_control.only_one_km = mst.only_one_km; + //vect_control.enable_compens_iq1_iq2 = mst.enable_compens_iq1_iq2; +} + +void simulateAdcAndCallIsr() { + + ///* Моделируем преобразования измеряемых величин датчиками, + //операционниками и АЦП (с помощью nAdc учитываем сдвиг по времени + //между АЦП разных сигналов) */ + //if ( tAdc < Tadc ) { + // tAdc++; + //} + //else { + + // tAdc = 1; + // timers_adc++; + // if (timers_adc>=FREQ_ADC_TIMER) + // timers_adc = 0; + + // update_norm_ADC_array(); + // // после завершения серии АЦП вызываем isr() + // acp_Handler(); + // //isr(); + + + // // nAdc++; + // // switch ( nAdc ) { + // // case 5: + // + // // break; + // // case 6: + + // // break; + + // // case 7: + // // // как бы с ПУ + // // for ( j = FIRST_WRITE_PAR_NUM; j < paramNo; j++ ) { + // // if ( paramNew[j] != param[j] ) { + // // input_param((short)j, paramNew[j]); + // // break; + // // } + // // } + // // // после завершения серии АЦП вызываем isr() + // // isr(); + // // break; + // // } //switch ( nAdc ) + + //} //tAdc + + // if (calcAlgUpr) { + // // реализует алгоритм управления + // upr(); + // calcAlgUpr = 0; + // timers_pwm++; + // if (timers_pwm>=FREQ_PWM_TIMER) + // timers_pwm = 0; + // } + + +} + +void simulateActionActionQualifierSubmodule(void) { + //// Моделируем Action-Qualifier Submodule + //// ... ePWM1 + //if ( cmp1A > t1cnt ) { + // ci1A = 0; + // ci1B = 1; + //} + //else if ( cmp1A < t1cnt ) { + // ci1A = 1; + // ci1B = 0; + //} + //// ... ePWM2 + //if ( cmp2A > t2cnt ) { + // ci2A = 0; + // ci2B = 1; + //} + //else if ( cmp2A < t2cnt ) { + // ci2A = 1; + // ci2B = 0; + //} + //// ... ePWM3 + //if ( cmp3A > t3cnt ) { + // ci3A = 0; + // ci3B = 1; + //} + //else if ( cmp3A < t3cnt ) { + // ci3A = 1; + // ci3B = 0; + //} + //// ... ePWM4 + //if ( cmp4A > t4cnt ) { + // ci4A = 0; + // ci4B = 1; + //} + //else if ( cmp4A < t4cnt ) { + // ci4A = 1; + // ci4B = 0; + //} + //// ... ePWM5 + //if ( cmp5A > t5cnt ) { + // ci5A = 0; + // ci5B = 1; + //} + //else if ( cmp5A < t5cnt ) { + // ci5A = 1; + // ci5B = 0; + //} + //// ... ePWM6 + //if ( cmp6A > t6cnt ) { + // ci6A = 0; + // ci6B = 1; + //} + //else if ( cmp6A < t6cnt ) { + // ci6A = 1; + // ci6B = 0; + //} + //// ... ePWM7 + //if ( cmp7A > t7cnt ) { + // ci7A = 0; + // ci7B = 1; + //} + //else if ( cmp7A < t7cnt ) { + // ci7A = 1; + // ci7B = 0; + //} + //// ... ePWM8 + //if ( cmp8A > t8cnt ) { + // ci8A = 0; + // ci8B = 1; + //} + //else if ( cmp8A < t8cnt ) { + // ci8A = 1; + // ci8B = 0; + //} + //// ... ePWM9 + //if ( cmp9A > t9cnt ) { + // ci9A = 0; + // ci9B = 1; + //} + //else if ( cmp9A < t9cnt ) { + // ci9A = 1; + // ci9B = 0; + //} + //// ... ePWM10 + //if ( cmp10A > t10cnt ) { + // ci10A = 0; + // ci10B = 1; + //} + //else if ( cmp10A < t10cnt ) { + // ci10A = 1; + // ci10B = 0; + //} + //// ... ePWM11 + //if ( cmp11A > t11cnt ) { + // ci11A = 0; + // ci11B = 1; + //} + //else if ( cmp11A < t11cnt ) { + // ci11A = 1; + // ci11B = 0; + //} + //// ... ePWM12 + //if ( cmp12A > t12cnt ) { + // ci12A = 0; + // ci12B = 1; + //} + //else if ( cmp12A < t12cnt ) { + // ci12A = 1; + // ci12B = 0; + //} + + +} + +void simulateDeadBendSubmodule(void) { + + //// Моделируем Dead-Band Submodule + //// ... ePWM1 + //if ( stateDt1 == 1 ) { + // ci1A_DT = ci1A; + // ci1B_DT = 0; + // if ( ci1A == 1 ) + // cntDt1 = CntDt; + // if ( cntDt1 > 0 ) + // cntDt1--; + // else + // stateDt1 = 2; + //} + //else if ( stateDt1 == 2 ) { + // ci1A_DT = 0; + // ci1B_DT = ci1B; + // if ( ci1B == 1 ) + // cntDt1 = CntDt; + // if ( cntDt1 > 0 ) + // cntDt1--; + // else + // stateDt1 = 1; + //} + //// ... ePWM2 + //if ( stateDt2 == 1 ) { + // ci2A_DT = ci2A; + // ci2B_DT = 0; + // if ( ci2A == 1 ) + // cntDt2 = CntDt; + // if ( cntDt2 > 0 ) + // cntDt2--; + // else + // stateDt2 = 2; + //} + //else if ( stateDt2 == 2 ) { + // ci2A_DT = 0; + // ci2B_DT = ci2B; + // if ( ci2B == 1 ) + // cntDt2 = CntDt; + // if ( cntDt2 > 0 ) + // cntDt2--; + // else + // stateDt2 = 1; + //} + //// ... ePWM3 + //if ( stateDt3 == 1 ) { + // ci3A_DT = ci3A; + // ci3B_DT = 0; + // if ( ci3A == 1 ) + // cntDt3 = CntDt; + // if ( cntDt3 > 0 ) + // cntDt3--; + // else + // stateDt3 = 2; + //} + //else if ( stateDt3 == 2 ) { + // ci3A_DT = 0; + // ci3B_DT = ci3B; + // if ( ci3B == 1 ) + // cntDt3 = CntDt; + // if ( cntDt3 > 0 ) + // cntDt3--; + // else + // stateDt3 = 1; + //} + //// ... ePWM4 + //if ( stateDt4 == 1 ) { + // ci4A_DT = ci4A; + // ci4B_DT = 0; + // if ( ci4A == 1 ) + // cntDt4 = CntDt; + // if ( cntDt4 > 0 ) + // cntDt4--; + // else + // stateDt4 = 2; + //} + //else if ( stateDt4 == 2 ) { + // ci4A_DT = 0; + // ci4B_DT = ci4B; + // if ( ci4B == 1 ) + // cntDt4 = CntDt; + // if ( cntDt4 > 0 ) + // cntDt4--; + // else + // stateDt4 = 1; + //} + //// ... ePWM5 + //if ( stateDt5 == 1 ) { + // ci5A_DT = ci5A; + // ci5B_DT = 0; + // if ( ci5A == 1 ) + // cntDt5 = CntDt; + // if ( cntDt5 > 0 ) + // cntDt5--; + // else + // stateDt5 = 2; + //} + //else if ( stateDt5 == 2 ) { + // ci5A_DT = 0; + // ci5B_DT = ci5B; + // if ( ci5B == 1 ) + // cntDt5 = CntDt; + // if ( cntDt5 > 0 ) + // cntDt5--; + // else + // stateDt5 = 1; + //} + //// ... ePWM6 + //if ( stateDt6 == 1 ) { + // ci6A_DT = ci6A; + // ci6B_DT = 0; + // if ( ci6A == 1 ) + // cntDt6 = CntDt; + // if ( cntDt6 > 0 ) + // cntDt6--; + // else + // stateDt6 = 2; + //} + //else if ( stateDt6 == 2 ) { + // ci6A_DT = 0; + // ci6B_DT = ci6B; + // if ( ci6B == 1 ) + // cntDt6 = CntDt; + // if ( cntDt6 > 0 ) + // cntDt6--; + // else + // stateDt6 = 1; + //} + //// ... ePWM7 + //if ( stateDt7 == 1 ) { + // ci7A_DT = ci7A; + // ci7B_DT = 0; + // if ( ci7A == 1 ) + // cntDt7 = CntDt; + // if ( cntDt7 > 0 ) + // cntDt7--; + // else + // stateDt7 = 2; + //} + //else if ( stateDt7 == 2 ) { + // ci7A_DT = 0; + // ci7B_DT = ci7B; + // if ( ci7B == 1 ) + // cntDt7 = CntDt; + // if ( cntDt7 > 0 ) + // cntDt7--; + // else + // stateDt7 = 1; + //} + //// ... ePWM8 + //if ( stateDt8 == 1 ) { + // ci8A_DT = ci8A; + // ci8B_DT = 0; + // if ( ci8A == 1 ) + // cntDt8 = CntDt; + // if ( cntDt8 > 0 ) + // cntDt8--; + // else + // stateDt8 = 2; + //} + //else if ( stateDt8 == 2 ) { + // ci8A_DT = 0; + // ci8B_DT = ci8B; + // if ( ci8B == 1 ) + // cntDt8 = CntDt; + // if ( cntDt8 > 0 ) + // cntDt8--; + // else + // stateDt8 = 1; + //} + //// ... ePWM9 + //if ( stateDt9 == 1 ) { + // ci9A_DT = ci9A; + // ci9B_DT = 0; + // if ( ci9A == 1 ) + // cntDt9 = CntDt; + // if ( cntDt9 > 0 ) + // cntDt9--; + // else + // stateDt9 = 2; + //} + //else if ( stateDt9 == 2 ) { + // ci9A_DT = 0; + // ci9B_DT = ci9B; + // if ( ci9B == 1 ) + // cntDt9 = CntDt; + // if ( cntDt9 > 0 ) + // cntDt9--; + // else + // stateDt9 = 1; + //} + //// ... ePWM10 + //if ( stateDt10 == 1 ) { + // ci10A_DT = ci10A; + // ci10B_DT = 0; + // if ( ci10A == 1 ) + // cntDt10 = CntDt; + // if ( cntDt10 > 0 ) + // cntDt10--; + // else + // stateDt10 = 2; + //} + //else if ( stateDt10 == 2 ) { + // ci10A_DT = 0; + // ci10B_DT = ci10B; + // if ( ci10B == 1 ) + // cntDt10 = CntDt; + // if ( cntDt10 > 0 ) + // cntDt10--; + // else + // stateDt10 = 1; + //} + //// ... ePWM11 + //if ( stateDt11 == 1 ) { + // ci11A_DT = ci11A; + // ci11B_DT = 0; + // if ( ci11A == 1 ) + // cntDt11 = CntDt; + // if ( cntDt11 > 0 ) + // cntDt11--; + // else + // stateDt11 = 2; + //} + //else if ( stateDt11 == 2 ) { + // ci11A_DT = 0; + // ci11B_DT = ci11B; + // if ( ci11B == 1 ) + // cntDt11 = CntDt; + // if ( cntDt11 > 0 ) + // cntDt11--; + // else + // stateDt11 = 1; + //} + //// ... ePWM12 + //if ( stateDt12 == 1 ) { + // ci12A_DT = ci12A; + // ci12B_DT = 0; + // if ( ci12A == 1 ) + // cntDt12 = CntDt; + // if ( cntDt12 > 0 ) + // cntDt12--; + // else + // stateDt12 = 2; + //} + //else if ( stateDt12 == 2 ) { + // ci12A_DT = 0; + // ci12B_DT = ci12B; + // if ( ci12B == 1 ) + // cntDt12 = CntDt; + // if ( cntDt12 > 0 ) + // cntDt12--; + // else + // stateDt12 = 1; + //} + +} + + +void simulateTripZoneSubmodule(void) { + +//// Моделируем Trip-Zone Submodule +// // ... clear flag for one-shot trip latch +// if ( EPwm1Regs.TZCLR.all == 0x0004 ) { +// EPwm1Regs.TZCLR.all = 0x0000; +// EPwm1Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm2Regs.TZCLR.all == 0x0004 ) { +// EPwm2Regs.TZCLR.all = 0x0000; +// EPwm2Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm3Regs.TZCLR.all == 0x0004 ) { +// EPwm3Regs.TZCLR.all = 0x0000; +// EPwm3Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm4Regs.TZCLR.all == 0x0004 ) { +// EPwm4Regs.TZCLR.all = 0x0000; +// EPwm4Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm5Regs.TZCLR.all == 0x0004 ) { +// EPwm5Regs.TZCLR.all = 0x0000; +// EPwm5Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm6Regs.TZCLR.all == 0x0004 ) { +// EPwm6Regs.TZCLR.all = 0x0000; +// EPwm6Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm7Regs.TZCLR.all == 0x0004 ) { +// EPwm7Regs.TZCLR.all = 0x0000; +// EPwm7Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm8Regs.TZCLR.all == 0x0004 ) { +// EPwm8Regs.TZCLR.all = 0x0000; +// EPwm8Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm9Regs.TZCLR.all == 0x0004 ) { +// EPwm9Regs.TZCLR.all = 0x0000; +// EPwm9Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm10Regs.TZCLR.all == 0x0004 ) { +// EPwm10Regs.TZCLR.all = 0x0000; +// EPwm10Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm11Regs.TZCLR.all == 0x0004 ) { +// EPwm11Regs.TZCLR.all = 0x0000; +// EPwm11Regs.TZFRC.all = 0x0000; +// } +// if ( EPwm12Regs.TZCLR.all == 0x0004 ) { +// EPwm12Regs.TZCLR.all = 0x0000; +// EPwm12Regs.TZFRC.all = 0x0000; +// } +// // ... forces a one-shot trip event +// if ( EPwm1Regs.TZFRC.all == 0x0004 ) +// ci1A_DT = ci1B_DT = 0; +// if ( EPwm2Regs.TZFRC.all == 0x0004 ) +// ci2A_DT = ci2B_DT = 0; +// if ( EPwm3Regs.TZFRC.all == 0x0004 ) +// ci3A_DT = ci3B_DT = 0; +// if ( EPwm4Regs.TZFRC.all == 0x0004 ) +// ci4A_DT = ci4B_DT = 0; +// if ( EPwm5Regs.TZFRC.all == 0x0004 ) +// ci5A_DT = ci5B_DT = 0; +// if ( EPwm6Regs.TZFRC.all == 0x0004 ) +// ci6A_DT = ci6B_DT = 0; +// if ( EPwm7Regs.TZFRC.all == 0x0004 ) +// ci7A_DT = ci7B_DT = 0; +// if ( EPwm8Regs.TZFRC.all == 0x0004 ) +// ci8A_DT = ci8B_DT = 0; +// if ( EPwm9Regs.TZFRC.all == 0x0004 ) +// ci9A_DT = ci9B_DT = 0; +// if ( EPwm10Regs.TZFRC.all == 0x0004 ) +// ci10A_DT = ci10B_DT = 0; +// if ( EPwm11Regs.TZFRC.all == 0x0004 ) +// ci11A_DT = ci11B_DT = 0; +// if ( EPwm12Regs.TZFRC.all == 0x0004 ) +// ci12A_DT = ci12B_DT = 0; + + +} + + +void writeOutputParameters(real_T *xD) { + +// // ВЫХОДЫ (begin) +// nn = 0; +// // Управление +// // ... INU1 +// xD[nn++] = ci1A_DT; +// xD[nn++] = ci2A_DT; +// xD[nn++] = ci1B_DT; +// xD[nn++] = ci2B_DT; +// +// xD[nn++] = ci3A_DT; +// xD[nn++] = ci4A_DT; +// xD[nn++] = ci3B_DT; +// xD[nn++] = ci4B_DT; +// +// xD[nn++] = ci5A_DT; +// xD[nn++] = ci6A_DT; +// xD[nn++] = ci5B_DT; +// xD[nn++] = ci6B_DT; +// // ... INU2 +// xD[nn++] = ci7A_DT; +// xD[nn++] = ci8A_DT; +// xD[nn++] = ci7B_DT; +// xD[nn++] = ci8B_DT; +// +// xD[nn++] = ci9A_DT; +// xD[nn++] = ci10A_DT; +// xD[nn++] = ci9B_DT; +// xD[nn++] = ci10B_DT; +// +// xD[nn++] = ci11A_DT; +// xD[nn++] = ci12A_DT; +// xD[nn++] = ci11B_DT; +// xD[nn++] = ci12B_DT; +// +// +//// выходы ацп для контроля +// xD[nn++] = udc1_ml; +// xD[nn++] = udc2_ml; +// xD[nn++] = udc3_ml; +// xD[nn++] = udc4_ml; +// +// xD[nn++] = idc1_ml; +// xD[nn++] = idc2_ml; +// xD[nn++] = idc3_ml; +// xD[nn++] = idc4_ml; +// +// xD[nn++] = _IQtoF(analog.iqIa1_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(analog.iqIb1_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(analog.iqIc1_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(analog.iqIa2_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(analog.iqIb2_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(analog.iqIc2_1) * NORMA_ACP; +// +//// timers out +// +// xD[nn++] = timers_adc; +// xD[nn++] = timers_pwm; +// xD[nn++] = Tadc; +// xD[nn++] = dt; +// +// // Только для просмотра +// xD[nn++] = _IQtoF(rp.pmZ); +// xD[nn++] = _IQtoF(rs.wmZ); +// +// xD[nn++] = mst.start; +// xD[nn++] = inuWork; +// xD[nn++] = mst.pzMode; +// +// xD[nn++] = psi; +// xD[nn++] = rf.psiZ; +// +// xD[nn++] = wm; +// xD[nn++] = _IQtoF(vect_control.koeff_correct_Id);//rs.wmZ; +// xD[nn++] = _IQtoF(vect_control.iqFrot) * NORMA_FROTOR * 60.0 / N_BAZ;//csp.wmLimZi; +// +// xD[nn++] = pm*S_BAZ;///P_NOM; +// xD[nn++] = rp.pmZ*S_BAZ;///P_NOM; +// xD[nn++] = csp.pmLimZi*S_BAZ;///P_NOM; +// +// xD[nn++] = _IQtoF(analog.iqId1)* NORMA_ACP;//_IQtoF(vect_control.iqPzad); +// xD[nn++] = _IQtoF(analog.iqIq1)* NORMA_ACP;// * NORMA_ACP; +// xD[nn++] = _IQtoF(analog.iqId2)* NORMA_ACP;//_IQtoF(vect_control.iqUqCompensation1);// +// xD[nn++] = _IQtoF(analog.iqIq2)* NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.iqId_zad)* NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.iqIq_zad)* NORMA_ACP;//iqZ; +// +// xD[nn++] = me*M_BAZ/M_NOM; +// +// xD[nn++] = _IQtoF(vect_control.iqPzad) * NORMA_ACP * NORMA_ACP / 1000.0;; +// xD[nn++] = _IQtoF(vect_control.iqPizm) * NORMA_ACP * NORMA_ACP / 1000.0;; +// // xD[nn++] = sqrt(idZ*idZ + iqZ*iqZ); +// // xD[nn++] = IzLim; +// +// // xD[nn++] = EPwm2Regs.CMPA.half.CMPA;//xpwm_time.Ta0_0;//cc.yd1; +// // xD[nn++] = xpwm_time.Ta0_0; +// // xD[nn++] = xpwm_time.Ta0_1; +// // xD[nn++] = _IQtoF(cos_fi.cos_fi_nom);//cc.yd1; +// // xD[nn++] = _IQtoF(cos_fi.cos_fi_nom_squared);//cc.yq1; +// // xD[nn++] = _IQtoF(tetta_calc.k_t); +// xD[nn++] = _IQtoF(vect_control.iqUzad1);//cc.yd1; +// xD[nn++] = _IQtoF(vect_control.iqUzad2);//cc.yd1; +//// xD[nn++] = _IQtoF(vect_control.iqUdKm1Out);//cc.yq1; +//// xD[nn++] = _IQtoF(vect_control.iqUqKm1Out); +// +// xD[nn++] = _IQtoF(vect_control.iqUdKm1);//sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1); +// xD[nn++] = _IQtoF(vect_control.iqUqKm1);//sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2); +// xD[nn++] = _IQtoF(vect_control.iqUdKm2);//Y_LIM; +// xD[nn++] = _IQtoF(vect_control.iqUqKm2);//Y_LIM; +// +// +// xD[nn++] = _IQtoF(vect_control.iqUdKm1Out);//sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1); +// xD[nn++] = _IQtoF(vect_control.iqUqKm1Out);//sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2); +// xD[nn++] = _IQtoF(vect_control.iqUdKm2Out);//Y_LIM; +// xD[nn++] = _IQtoF(vect_control.iqUqKm2Out);//Y_LIM; +// +// xD[nn++] = _IQtoF(vect_control.k_modul_max);//Y_LIM; +// +// xD[nn++] = _IQtoF(vect_control.k_modul_max_def); +// xD[nn++] = _IQtoF(vect_control.maxUq1); +// xD[nn++] = _IQtoF(vect_control.maxUq2); +// xD[nn++] = _IQtoF(vect_control.Uq1Out); +// xD[nn++] = _IQtoF(vect_control.Uq2Out); +//// xD[nn++] = _IQtoF(vect_control.K_MODUL_MAX); +//// xD[nn++] = _IQtoF(vect_control.K_MODUL_MAX); +//// xD[nn++] = _IQtoF(vect_control.K_MODUL_MAX); +// +// +// // xD[nn++] = sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1); +// // xD[nn++] = sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2); +// // xD[nn++] = Y_LIM; +// // ВЫХОДЫ (end) +// +// +///////////////// new +// xD[nn++] = _IQtoF(f.iq_mzz_zad) * NORMA_ACP;// +// xD[nn++] = _IQtoF(pidPvect.OutMax) * NORMA_ACP; +// xD[nn++] = _IQtoF(pidPvect.OutMin) * NORMA_ACP; +// xD[nn++] = _IQtoF(pidPvect.Out) * NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.mzz_zad_int) * NORMA_ACP ; +// xD[nn++] = _IQtoF(filter.iqIm_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(filter.iqIm_2) * NORMA_ACP; +// +// xD[nn++] = _IQtoF(vect_control.Pzad_rmp) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(f.iq_p_rampa) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(analog.iqPvsi1) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(analog.iqPvsi1) * NORMA_ACP * NORMA_ACP / 1000.0; +// +// xD[nn++] = _IQtoF(analog.iqW1) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(analog.iqW2) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(analog.iqW) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(filter.iqW1) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(filter.iqW2) * NORMA_ACP * NORMA_ACP / 1000.0; +// xD[nn++] = _IQtoF(filter.iqW) * NORMA_ACP * NORMA_ACP / 1000.0; +// +// +// +// xD[nn++] = _IQtoF(vect_control.iqFrot) * NORMA_FROTOR * 60.0; +// xD[nn++] = vect_control.flag_reverse;//_IQtoF(0); +// +// xD[nn++] = _IQtoF(vect_control.koeff_correct_Id); +// xD[nn++] = _IQtoF(cos_fi.cos_fi_nom); +// +// xD[nn++] = _IQtoF(filter.Fsl) * NORMA_FROTOR; +// +// xD[nn++] = _IQtoF(filter.iqIa1_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(filter.iqIb1_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(filter.iqIc1_1) * NORMA_ACP; +// +// xD[nn++] = _IQtoF(filter.iqIa2_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(filter.iqIb2_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(filter.iqIc2_1) * NORMA_ACP; +// +// xD[nn++] = _IQtoF(tetta_calc.Imds); +// xD[nn++] = _IQtoF(tetta_calc.tetta); +// +// xD[nn++] = _IQtoF(vect_control.Is) * NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.Ids) * NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.Id_ref_fw) * NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.Id_ref_1) * NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.Is_max) * NORMA_ACP; +// +// xD[nn++] = vect_control.flag_fw; +// +// xD[nn++] = _IQtoF(vect_control.ws_Iq1) * NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.ws_Id_filter1) * NORMA_ACP; +// xD[nn++] = _IQtoF(vect_control.ws_Id_filter2) * NORMA_ACP; +// +// xD[nn++] = _IQtoF(vect_control.ws_ws_t1) * NORMA_FROTOR; +// +// xD[nn++] = _IQtoF(analog.Fsl) * NORMA_FROTOR; +// xD[nn++] = _IQtoF(filter.Fsl) * NORMA_FROTOR; +// +// xD[nn++] = vect_control.tmp1; +// xD[nn++] = vect_control.tmp2; +// xD[nn++] = vect_control.tmp3; +// //xD[nn++] = _IQtoF(0) * NORMA_ACP; +// //xD[nn++] = _IQtoF(0) * NORMA_ACP; +// xD[nn++] = _IQtoF(0) * NORMA_ACP; +// +// +// +// xD[nn++] = _IQtoF(svgen_dq_1.Ta)*1000.0; +// xD[nn++] = _IQtoF(svgen_dq_1.Tb)*1000.0; +// xD[nn++] = _IQtoF(svgen_dq_1.Tc)*1000.0; +// +// xD[nn++] = _IQtoF(svgen_dq_2.Ta)*1000.0; +// xD[nn++] = _IQtoF(svgen_dq_2.Tb)*1000.0; +// xD[nn++] = _IQtoF(svgen_dq_2.Tc)*1000.0; +// + + + + +} + + + +void controller(SimStruct *S, const real_T *u, real_T *xD, real_T *rW, int_T *iW) { + + readInputParameters(u); + processSFunctionIfChanged(S, iW); + initialisationOnStart(iW); + + simulatePWMcounterAndReadComarators(); + + + simulateAdcAndCallIsr(); + simulateActionActionQualifierSubmodule(); + + // convertSVGenTimesToTkLines(); + simulateDeadBendSubmodule(); + + // xilinxPwm3LevelSimulation(); + simulateTripZoneSubmodule(); + + + writeOutputParameters(xD); + +} //void controller(SimStruct ... diff --git a/Inu/controller.h b/Inu/controller.h new file mode 100644 index 0000000..c81e487 --- /dev/null +++ b/Inu/controller.h @@ -0,0 +1,222 @@ +// Максимальная длина параметра-вектора +#define LEN_PARAM_MATR 21 + +// Массивы с параметрами S_Function +double paramScal[NPARAMS]; +double paramMatr[LEN_PARAM_MATR*2]; +int paramMatrDimen; + +// Индекс входного и выходного массива, а также массива параметров +int nn; +// Шаг интегрирования +double dt; +// Для обработки параметров +int kkk, lll, mmm, dimen; + + + +// Переменные, которые определены в controller.c (begin) +//######################################################################### +// Параметры +//double ; + +// Входы +typedef struct +{ + double udc1_ml; + double udc2_ml; + double ia1_ml; + double ib1_ml; + double ic1_ml; + double ia2_ml; + double ib2_ml; + double ic2_ml; + double wm_ml; +}UMotorMeasure; +extern UMotorMeasure motor; + +// Для имитации обмена с ПУ +int j; +unsigned short paramNo; +unsigned short paramNew[PAR_NUMBER]; + +// Для моделирования Event Manager +// ... Time-Base Submodule, Counter-Compare Submodule и Event-Trigger Submodule +typedef struct +{ + double TxCntPlus; + double TPr; + double tcntAux; + double tcntAuxPrev; + double tcnt; + double cmp1A; + double cmp1B; +}TimerSimHandle; +extern TimerSimHandle t1sim +extern TimerSimHandle t2sim; +extern TimerSimHandle t3sim; +extern TimerSimHandle t4sim; +extern TimerSimHandle t5sim; +extern TimerSimHandle t6sim; +extern TimerSimHandle t7sim; +extern TimerSimHandle t8sim; +extern TimerSimHandle t9sim; +extern TimerSimHandle t10sim; +extern TimerSimHandle t11sim; +extern TimerSimHandle t12sim; + + +// ... Action-Qualifier Submodule +int ci1A; +int ci1B; +int ci2A; +int ci2B; +int ci3A; +int ci3B; +int ci4A; +int ci4B; +int ci5A; +int ci5B; +int ci6A; +int ci6B; +int ci7A; +int ci7B; +int ci8A; +int ci8B; +int ci9A; +int ci9B; +int ci10A; +int ci10B; +int ci11A; +int ci11B; +int ci12A; +int ci12B; +// ... Dead-Band Submodule +typedef struct +{ + int CntDt; + int stateDt; + int cntDt; + int ciA_DT; + int ciB_DT; +}DeadBandSimHandle; +extern DeadBandSimHandle dt1sim +extern DeadBandSimHandle dt2sim; +extern DeadBandSimHandle dt3sim; +extern DeadBandSimHandle dt4sim; +extern DeadBandSimHandle dt5sim; +extern DeadBandSimHandle dt6sim; +extern DeadBandSimHandle dt7sim; +extern DeadBandSimHandle dt8sim; +extern DeadBandSimHandle dt9sim; +extern DeadBandSimHandle dt10sim; +extern DeadBandSimHandle dt11sim; +extern DeadBandSimHandle dt12sim; + +// Для моделирования eQEP +double Qposmax; +double qposcnt; + +// Для моделирования ADC +int tAdc; +int Tadc; +int nAdc; +//######################################################################### +// Переменные, которые определены в controller.c (end) + + + + +// Переменные, которые объявлены в controller.c (begin) +//######################################################################### +// Для isr.c +//------------------------------------------------------------------------- +extern struct Offset offset; +extern volatile struct Result result; +extern volatile short state; +extern volatile short faultNo; +extern volatile struct Out out; +// Udc +extern float Kudc; +extern volatile float udc1Nf; +extern volatile float udc1; +extern volatile float udc2Nf; +extern volatile float udc2; +// Iac +extern volatile float ia1Nf; +extern volatile float ib1Nf; +extern volatile float ix1; +extern volatile float iy1; +extern volatile float iac1Nf; +extern volatile float ia2Nf; +extern volatile float ib2Nf; +extern volatile float ix2; +extern volatile float iy2; +extern volatile float iac2Nf; +// Wm +extern float Kwm; +extern volatile float wmNf; +extern volatile float wm; +extern volatile float wmAbs; +// Me +extern volatile float kMe; +extern float KmeCorr; +extern float Kme; +extern volatile float meNf; +extern volatile float me; +// Pm +extern volatile float pm; +// защиты +extern struct Protect protect; +extern volatile struct Emerg emerg; +extern short csmSuccess; +// управляющая логика +extern volatile short onceShutdown; +extern volatile short testParamFaultNo; +extern volatile short onceFaultReset; +extern volatile short stopPause; +extern volatile short inuWork; +// обмен +extern struct Mst mst; + + + +// Для main.c +//------------------------------------------------------------------------- +extern struct Eprom eprom; + + + +// Для upr.c +//------------------------------------------------------------------------- +extern volatile short onceUpr; +extern struct SgmPar sgmPar; +extern struct Rf rf; +extern struct Rs rs; +extern struct Rp rp; + +extern float IzLim; +extern volatile float psi; +extern float idZ; +extern float iqZ; +extern float iZ; +extern float ws; +extern float sinTheta; +extern float cosTheta; +extern float id1; +extern float iq1; +extern float id2; +extern float iq2; +extern struct Cc cc; +extern struct Cf cf; +extern struct Csp csp; +extern struct Ivc ivc; +extern struct Ip ip; + + + +// Для param.c +//------------------------------------------------------------------------- +extern unsigned short param[]; +//######################################################################### +// Переменные, которые объявлены в controller.c (end) diff --git a/Inu/def.h b/Inu/def.h new file mode 100644 index 0000000..233c12a --- /dev/null +++ b/Inu/def.h @@ -0,0 +1,482 @@ +/************************************************************************** + Description: Всякие разные переключатели и уставки. + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.11.08 +**************************************************************************/ + +#ifndef DEF +#define DEF + +// раскомментировать, если есть сдвиг между обмотками ГЭД (30 град.) +#define SHIFT + + +// режимы работы (для state) +#define STATE_SHUTDOWN 0 //аварийная остановка +#define STATE_STOP 1 //штатная остановка +#define STATE_WORK 2 //работа + + +// частота тактовых импульсов процессора, Гц +#define FSYSCLKOUT 200e6 //150e6 // +// prescaled version of the system clock and is used by +// all submodules within the ePWM, Гц +// (см. EPwmxRegs.TBCTL.bit.CLKDIV и EPwmxRegs.TBCTL.bit.HSPCLKDIV) +#define FTBCLK (FSYSCLKOUT*0.5*0.5) +//#define FTBCLK (FSYSCLKOUT*0.5*0.5*0.5*0.5) +// период ШИМ, c +#define T_PWM 2220e-6 //F_PWM = 450 Гц +//#define T_PWM 6000e-6 //F_PWM = 166.7 Гц +// период вызова основной программы, с +#define TY (T_PWM*0.5) +// "мертвое время", с +#define DT 30e-6 +//#define DT 60e-6 +// Time-Base Period Register, ед. счётчика таймера +#define T1_PRD (FTBCLK*T_PWM*0.5) +// максимальное значение амплитуды напряжения управления устанавливаем так, +// чтобы минимальная ширина импульса была 10 мкс, ед. счётчика таймера +#define Y_LIM (T1_PRD - (DT + 10e-6)*FTBCLK) + + +// константы для вычисления всякого +#define PI2 6.283185307179586476925286766559 //pi*2 +#define SQRT2 1.4142135623730950488016887242097 //sqrt(2) +#define SQRT3 1.7320508075688772935274463415059 //sqrt(3) +#define ISQRT3 0.57735026918962576450914878050196 //1./sqrt(3) + +// Номинальные величины ГЭД +// ... мощность на валу, Вт +#define P_NOM (5000e3*2.) +// ... линейное напряжение, В (ampl) +#define U_NOM (3000.*SQRT2) +// ... механическая скорость, об/мин +#define N_NOM 165. +// ... число пар полюсов +#define PP 6. +// ... коэффициент мощности +#define COS_FI 0.89 +// ... КПД +#define EFF 0.962 +// ... приведенный к валу момент инерции, кг*м^2 +#define J (87e3*0.50) +// ... полная мощность, ВА +#define S_NOM (P_NOM/(COS_FI*EFF)) +// ... механическая скорость, рад/с +#define WM_NOM (N_NOM/60.*PI2) +// ... момент на валу, Н*м +#define M_NOM (P_NOM/WM_NOM) + + +// Базовые величины ГЭД +// ... полная мощность, BA +#define S_BAZ S_NOM +// ... линейное напряжение, В (ampl) +#define U_BAZ U_NOM +// ... фазный ток, А (ampl) +#define I_BAZ (S_BAZ*2./(U_BAZ*SQRT3)*0.5) //0.5 - т.к. обмоток две +// ... механическая скорость, об/мин +#define N_BAZ N_NOM +// ... механическая скорость, рад/с +#define WM_BAZ (N_BAZ/60.*PI2) +// ... электрическая скорость, рад/с +#define WE_BAZ (WM_BAZ*PP) +// ... момент на валу, Н*м +#define M_BAZ (S_BAZ/WM_BAZ) +// ... потокосцепление статора, Вб +#define PSI_BAZ (U_BAZ/(WE_BAZ*SQRT3)) +// ... индуктивность, Гн +#define L_BAZ (PSI_BAZ/I_BAZ) +// ... сопротивление, Ом +#define R_BAZ (U_BAZ/(I_BAZ*SQRT3)) + + +// для пересчёта из амплитуды фазного напряжения в единицы сигнала управления +#define U_2_Y (T1_PRD*SQRT3/U_BAZ) + +// напряжение в звене пост. тока, которое дало бы на выходе АЦП знач. 2048, В +#define UDC_SENS_MAX (U_BAZ*1.15*1.3) +// выходной ток, который дал бы на выходе АЦП знач. 2048, А (ampl) +#define IAC_SENS_MAX (I_BAZ*1.5) +// number of pulses per rev. (from tacho, Hall, optical sensor...etc) +#define NOP 1024. +// приращение счётчика QEP за TY сек. при частоте вращ. WM_BAZ +#define QEP_CNT_DEL_NOM (NOP*4.*TY*WM_BAZ/PI2) + + +// коэффициенты для перевода измеренных величин в o.e. +#define GAIN_UDC (UDC_SENS_MAX/(2048.*U_BAZ)) +#define GAIN_IAC (IAC_SENS_MAX/(2048.*I_BAZ)) +#define GAIN_WM (1.0/QEP_CNT_DEL_NOM) + + +// минимальная скорость для пересчёта мощности в ток, o.e. +#define WM_MIN 0.03 //0.003 //? +// для прореживания регуляторов потока, скорости и мощности +#define DECIM_PSI_WM_PM 2. //1. //5. //? + + +// for specify the PLL +#define PLLSTS_DIVSEL 2 +#define PLLCR_DIV 10 + +// для вывода +#define CONTROLLER_BIAS 3.2 +#define CONTROLLER_GAIN 2500. + +// общее количество параметров +#define PAR_NUMBER 400 +// номер первого редактируемого параметра +#define FIRST_WRITE_PAR_NUM 150 + + + +// Дискретные входы/выходы (begin) +//------------------------------------------------------------------------- +// входы +// ---------------------------- +#define DI_24V_SOURCE_FAULT GpioDataRegs.GPBDAT.bit.GPIO50 + +// выходы +// ---------------------------- +// ... разное +#define DO_GPIO00_SET GpioDataRegs.GPASET.bit.GPIO0 = 1 +#define DO_GPIO00_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO0 = 1 +#define DO_GPIO01_SET GpioDataRegs.GPASET.bit.GPIO1 = 1 +#define DO_GPIO01_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO1 = 1 +#define DO_GPIO02_SET GpioDataRegs.GPASET.bit.GPIO2 = 1 +#define DO_GPIO02_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO2 = 1 +#define DO_GPIO03_SET GpioDataRegs.GPASET.bit.GPIO3 = 1 +#define DO_GPIO03_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO3 = 1 +#define DO_GPIO04_SET GpioDataRegs.GPASET.bit.GPIO4 = 1 +#define DO_GPIO04_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO4 = 1 +#define DO_GPIO05_SET GpioDataRegs.GPASET.bit.GPIO5 = 1 +#define DO_GPIO05_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO5 = 1 +#define DO_GPIO06_SET GpioDataRegs.GPASET.bit.GPIO6 = 1 +#define DO_GPIO06_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO6 = 1 +#define DO_GPIO07_SET GpioDataRegs.GPASET.bit.GPIO7 = 1 +#define DO_GPIO07_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO7 = 1 +#define DO_GPIO08_SET GpioDataRegs.GPASET.bit.GPIO8 = 1 +#define DO_GPIO08_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO8 = 1 +#define DO_GPIO09_SET GpioDataRegs.GPASET.bit.GPIO9 = 1 +#define DO_GPIO09_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO9 = 1 +#define DO_GPIO10_SET GpioDataRegs.GPASET.bit.GPIO10 = 1 +#define DO_GPIO10_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO10 = 1 +#define DO_GPIO11_SET GpioDataRegs.GPASET.bit.GPIO11 = 1 +#define DO_GPIO11_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO11 = 1 +// ... не используются +#define DO_GPIO019_SET GpioDataRegs.GPASET.bit.GPIO19 = 1 +#define DO_GPIO019_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO19 = 1 +#define DO_GPIO020_SET GpioDataRegs.GPASET.bit.GPIO20 = 1 +#define DO_GPIO020_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO20 = 1 +#define DO_GPIO022_SET GpioDataRegs.GPASET.bit.GPIO22 = 1 +#define DO_GPIO022_CLEAR GpioDataRegs.GPACLEAR.bit.GPIO22 = 1 +#define DO_GPIO48_SET GpioDataRegs.GPBSET.bit.GPIO48 = 1 +#define DO_GPIO48_CLEAR GpioDataRegs.GPBCLEAR.bit.GPIO48 = 1 +#define DO_GPIO49_SET GpioDataRegs.GPBSET.bit.GPIO49 = 1 +#define DO_GPIO49_CLEAR GpioDataRegs.GPBCLEAR.bit.GPIO49 = 1 +// ... для управления ножкой CS EEPROM +#define CS_SET GpioDataRegs.GPBSET.bit.GPIO57 = 1 +#define CS_CLEAR GpioDataRegs.GPBCLEAR.bit.GPIO57 = 1 +// ... светодиоды +// (зелёный "Готовность") +#define LED_GREEN1_ON GpioDataRegs.GPBCLEAR.bit.GPIO59 = 1 +#define LED_GREEN1_OFF GpioDataRegs.GPBSET.bit.GPIO59 = 1 +#define LED_GREEN1_TOGGLE GpioDataRegs.GPBTOGGLE.bit.GPIO59 = 1 +// (зелёный "Работа") +#define LED_GREEN2_ON GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1 +#define LED_GREEN2_OFF GpioDataRegs.GPBSET.bit.GPIO60 = 1 +#define LED_GREEN2_TOGGLE GpioDataRegs.GPBTOGGLE.bit.GPIO60 = 1 +// (красный "Авария") +#define LED_RED_ON GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1 +#define LED_RED_OFF GpioDataRegs.GPBSET.bit.GPIO61 = 1 +#define LED_RED_TOGGLE GpioDataRegs.GPBTOGGLE.bit.GPIO61 = 1 +// ... не используется +#define DO_GPIO63_SET GpioDataRegs.GPBSET.bit.GPIO63 = 1 +#define DO_GPIO63_CLEAR GpioDataRegs.GPBCLEAR.bit.GPIO63 = 1 +//------------------------------------------------------------------------- +// Дискретные входы/выходы (end) + + +#include "DSP2833x_Device.h" +#include "math.h" +#include "C28x_FPU_FastRTS.h" + + +// структура для регуляторов тока (см. control_current()) +struct Cc { + short once; + float KpOrig; + float KiOrig; + float Kp; + float Ki; + float K1; + float K2; + float K3; + float Xyff; + float yffAux2; + float yffAux3; + float del; + float kYlim; + float yd1P; + float yd1I; + float yd1FF; + float yd1; + float yq1P; + float yq1I; + float yq1FF; + float yq1; + float y1; + float yd2P; + float yd2I; + float yd2FF; + float yd2; + float yq2P; + float yq2I; + float yq2FF; + float yq2; + float y2; + short y1LimFlag; + short y2LimFlag; +}; //Cc + + +// структура для регулятора потока (см. control_flux()) +struct Cf { + short once; + float KpOrig; + float KiOrig; + float Kp; + float Ki; + float IdLim; + float IdLimNeg; + float del; + float idP; + float idI; + float idFF; + short idLimFlag; +}; //Cf + + +// структура для регуляторов скорости и мощности (см. control_speed_power()) +struct Csp { + short once; + float KpOrig; + float KiOrig; + float Kp; + float Ki; + float kMeNom; + float del; + float iqP; + float iqI; + float iqFF; + float IlimIncr; + float iqLimAux; + float iqLimZi; + float IqLim; + float IqLimNeg; + float iqLim; + float iqLimNeg; + float delWmAbs; + float KizIncr; + float pmZiRampDown; + float wmLimZi; + float pmLimZi; + short iqLimFlag; +}; //Csp + + +// структура для записи в EEPROM всякого +struct Eprom { + short writeRequestNumber; + short readFaultNo; +}; //Eprom + + +// структура для запоминания величин в момент срабатывания защиты +struct Emerg { + float udc1; + float udc2; + float iac1; + float iac2; + float me; + float wm; + float pm; +}; //Emerg + + +// структура для inverse park (см. ipark()) +struct Ip { + float yx1Aux; + float yy1Aux; + float yx2Aux; + float yy2Aux; + float theta; + float sinTheta; + float cosTheta; + float yx1; + float yy1; + float yx2; + float yy2; +}; //Ip + + +// структура для indirect vector control (см. indirect_vector_control()) +struct Ivc { + short once; + float im; + float wr; + float wsNf; + float ws; + float sinTheta; + float cosTheta; + float id1; + float iq1; + float psi; + float id2; + float iq2; +}; //Ivc + + +// структура для данных, полученных с ВУ +struct Mst { + short pzMode; + short faultReset; + short start; + float wmZz; + float pmZz; + float wmLim; + float pmLim; + float pIncrMaxTy; + float pDecrMaxTy; +}; //Mst + + +// структура для смещений нуля датчиков +struct Offset { + short Udc1; + short Udc2; + short Ic1; + short Ic2; + short Ia1; + short Ia2; + short Ib1; + short Ib2; +}; //Offset + + +// структура для вывода +struct Out { + float K; + float udc1; + float udc2; + float iac1; + float iac2; + float me; + float wm; + float pm; +}; //Out + + +// структура для защит +struct Protect { + short IacMax; + short IacMin; + unsigned short Tdi24VSource; + volatile unsigned short tDI24VSource; + unsigned short TudcMin; + volatile unsigned short tUdc1Min; + volatile unsigned short tUdc2Min; + unsigned short TwmMax; + volatile unsigned short tWmMax; + float UdcMin; + float UdcMax; + float WmMax; +}; //Protect + + +// структура для результатов АЦП +struct Result { + short udc1; + short udc2; + short ic1; + short ic2; + short ia1; + short ia2; + short ib1; + short ib2; +}; //Result + + +// структура для заданного потока (см. reference_flux()) +struct Rf { + short once; + float PsiZ; + float PsizIncr; + float psiZi; + float psiZCorr; + float psiSub; + float psiZCorr2; + float psiZ; + float KpsiSub; + float Kpsiz; + float WmNomPsi; + float YlimPsi; + float pPsiZ; + float psiZPrev1; + float psiZPrev2; + float psiZPrev3; +}; //Rf + + +// структура для заданной мощности (см. reference_power()) +struct Rp { + short once; + float pmZz; + float pmZi; + float pmZ; + float Kpmz; + float PlimIncr; + float KpIncrDecr; + volatile float pmEqv; +}; //Rp + + +// структура для заданной скорости (см. reference_speed()) +struct Rs { + short once; + float wmZz; + float wmZi; + float wmZ; + float Kwmz; + float WlimIncr; + float wzIncrNf; + float wzIncr; + float pWmZ; + float wmZPrev1; + float wmZPrev2; + float wmZPrev3; + short tPwmZ; +}; //Rs + + +// структура для параметров ГЭД +struct SgmPar { + float Rs; + float Lls; + float Rr; + float Llr; + float Lm; + float Ls; + float Lr; + float SigmaLs; + float LmInv; + float LrInv; + float Tr; + float TrInv; + float Kl; + float KlInv; +}; //SgmPar + +#endif //DEF diff --git a/Inu/detcoeff.c b/Inu/detcoeff.c new file mode 100644 index 0000000..7e14b81 --- /dev/null +++ b/Inu/detcoeff.c @@ -0,0 +1,242 @@ +/************************************************************************** + Description: Функция вызывается один раз в начале выполнения + программы и подготавливает данные необходимые для её работы. + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.11.08 +**************************************************************************/ + + +#include "def.h" +#include "detcoeff.h" + + +void process_sgm_parameters(void); +short read_eeprom(void); +short test_param(void); + + + +void detcoeff(void) { + float Tci; + float Tcf; + float Tcs; + float Km; + + // для сигнализации + testParamFaultNo = 0; + + // чтение EEPROM + // ( -> param[]) + eprom.readFaultNo = read_eeprom(); + if ( eprom.readFaultNo != 0 ) { + faultNo = 1; + state = STATE_SHUTDOWN; + } + else { + // проверяем массив param[] + testParamFaultNo = test_param(); + if ( testParamFaultNo != 0 ) { + faultNo = 4; + state = STATE_SHUTDOWN; + } + else { + faultNo = 0; + state = STATE_STOP; + } + } + + + rf.PsiZ = (float)param[180]*0.001;//%*10 -> o.e. + + // смещения нуля датчиков, ед. АЦП + offset.Ia1 = param[200]; + offset.Ib1 = param[201]; + offset.Ic1 = param[202]; + offset.Udc1 = param[203]; + offset.Ia2 = param[206]; + offset.Ib2 = param[207]; + offset.Ic2 = param[208]; + offset.Udc2 = param[209]; + + // параметры АД + sgmPar.Rs = (float)param[303]*1e-6;//мкОм -> Ом + sgmPar.Lls = (float)param[304]*1e-7;//мкГн*10 -> Гн + sgmPar.Rr = (float)param[305]*1e-6;//мкОм -> Ом + sgmPar.Llr = (float)param[306]*1e-7;//мкГн*10 -> Гн + sgmPar.Lm = (float)param[307]*1e-6;//мкГн -> Гн + // получаем из параметров АД всякие вспомогательные величины + process_sgm_parameters(); + + // коэффициенты регуляторов Id и Iq + // ... постоянная времени контура тока, с + Tci = TY*3.8; + cc.KpOrig = 0.01*sgmPar.SigmaLs/Tci*I_BAZ*U_2_Y; + cc.Kp = cc.KpOrig*(float)param[210]; + cc.KiOrig = 0.01*(sgmPar.Rs + sgmPar.Rr*sgmPar.Kl*sgmPar.Kl)/Tci*I_BAZ*U_2_Y*TY; + cc.Ki = cc.KiOrig*(float)param[211]; + + // коэффициенты регулятора Psi + // ... постоянная времени контура потока, с + Tcf = 50e-3; + cf.KpOrig = 0.01*sgmPar.KlInv/(sgmPar.Rr*Tcf)*PSI_BAZ/I_BAZ; + cf.Kp = cf.KpOrig*(float)param[212]; + cf.KiOrig = 0.01/(sgmPar.Lm*Tcf)*PSI_BAZ/I_BAZ*TY*DECIM_PSI_WM_PM; + cf.Ki = cf.KiOrig*(float)param[213]; + + // коэффициенты регулятора N + // ... постоянная времени контура скорости, с + Tcs = 200e-3; + // ... коэффициент для пересчёта тока в момент + Km = 1.5*PP*sgmPar.Kl*PSI_BAZ*rf.PsiZ; + csp.KpOrig = 0.01*J/(Km*Tcs)*WM_BAZ/I_BAZ; + csp.Kp = csp.KpOrig*(float)param[214]; + csp.KiOrig = 0.01*J/(Km*Tcs*Tcs*5.)*WM_BAZ/I_BAZ*TY*DECIM_PSI_WM_PM; + csp.Ki = csp.KiOrig*(float)param[215]; + + // всякие уставки + protect.IacMax = (short)(2047.*(float)param[220]*0.01);//% -> ед. АЦП + protect.IacMin = -protect.IacMax; + protect.UdcMax = (float)param[221]*0.01;//% -> o.e. + IzLim = (float)param[222]*0.01;//% -> o.e. + cf.IdLim = (float)param[223]*0.01;//% -> o.e. + cf.IdLimNeg = cf.IdLim*(-0.4); + csp.IqLim = (float)param[224]*0.01;//% -> o.e. + csp.IqLimNeg = -csp.IqLim; + protect.UdcMin = (float)param[225]*0.01;//% -> o.e. + protect.WmMax = (float)param[226]*0.01;//% -> o.e. + rf.WmNomPsi = (float)param[228]*0.01;//% -> o.e. + rf.YlimPsi = (float)param[229]*0.01*Y_LIM;//% -> ед. счётчика таймера + protect.TudcMin = (unsigned short)((float)param[231]*0.001/TY); + protect.TwmMax = (unsigned short)((float)param[233]*0.001/TY); + + // для разных ЗИ + rs.WlimIncr = 1.0*TY*DECIM_PSI_WM_PM/((float)param[244]*0.001); + csp.IlimIncr = 1.0*TY*DECIM_PSI_WM_PM/((float)param[245]*0.001); + rp.PlimIncr = 1.0*TY*DECIM_PSI_WM_PM/((float)param[248]*0.001); + rf.PsizIncr = 1.0*TY*DECIM_PSI_WM_PM/2.0; + + // для коррекции + KmeCorr = (float)param[269]*0.0001;//%*100 -> o.e. + + // для разных фильтров + Kudc = (TY*10000.)/(float)param[285]; + if ( Kudc > 1.0 ) + Kudc = 1.0; + Kwm = (TY*10000.)/(float)param[286]; + if ( Kwm > 1.0 ) + Kwm = 1.0; + rs.Kwmz = (TY*DECIM_PSI_WM_PM*1000.)/(float)param[288]; + rf.Kpsiz = (TY*DECIM_PSI_WM_PM*1000.)/(float)param[289]; + Kme = (TY*1000.)/(float)param[290]; + rp.Kpmz = (TY*DECIM_PSI_WM_PM*1000.)/(float)param[292]; + out.K = TY/100e-3; + + // выдержки + protect.Tdi24VSource = (unsigned short)(5.0/TY); + + // всё в исходное положение + udc1 = 0; + udc2 = 0; + wmNf = 0; + wm = 0; + me = 0; + out.udc1 = 0; + out.udc2 = 0; + out.iac1 = 0; + out.iac2 = 0; + out.wm = 0; + out.me = 0; + out.pm = 0; + protect.tWmMax = 0; + protect.tDI24VSource = 0; + onceShutdown = 0; + onceFaultReset = 0; + stopPause = 1; + mst.pzMode = 0; + mst.faultReset = 0; + mst.start = 0; + mst.wmZz = 0; + mst.pmZz = 0; + mst.wmLim = 0; + mst.pmLim = 0; + mst.pIncrMaxTy = 0; + mst.pDecrMaxTy = 0; +} //void detcoeff(void) + + + +// Вычисляет из параметров АД всякие вспомогательные величины +void process_sgm_parameters(void) { + // полная индуктивность фазы статора, Гн + sgmPar.Ls = sgmPar.Lm + sgmPar.Lls; + // полная индуктивность фазы ротора, Гн + sgmPar.Lr = sgmPar.Lm + sgmPar.Llr; + // всяко-разно + sgmPar.SigmaLs = (1. - sgmPar.Lm*sgmPar.Lm/(sgmPar.Ls*sgmPar.Lr))*sgmPar.Ls; + sgmPar.LmInv = 1.0/sgmPar.Lm; + sgmPar.LrInv = 1.0/sgmPar.Lr; + sgmPar.Kl = sgmPar.Lm*sgmPar.LrInv; + sgmPar.KlInv = 1.0/sgmPar.Kl; + sgmPar.Tr = sgmPar.Lr/sgmPar.Rr; + sgmPar.TrInv = sgmPar.Rr*sgmPar.LrInv; +} //void process_sgm_parameters(void) + + + +// Читает PAR_NUMBER параметров из EEPROM и сохраняет в массиве param[] +// ( -> param[]) +short read_eeprom(void) { + return 0; +} + + + +// Проверяет допустимость значений параметров, полученных из EEPROM +// (в param.c у параметров д.б. те же границы допустимых значений!) +short test_param(void) { + if ( param[180] > 2000 ) return 180;//rf.PsiZ + + if ( (param[200]<1748) || (param[200]>2348) ) return 200;//offset.Ia1 + if ( (param[201]<1748) || (param[201]>2348) ) return 201;//offset.Ib1 + if ( (param[202]<1748) || (param[202]>2348) ) return 202;//offset.Ic1 + if ( (param[203]<1748) || (param[203]>2348) ) return 203;//offset.Udc1 + if ( (param[206]<1748) || (param[206]>2348) ) return 206;//offset.Ia2 + if ( (param[207]<1748) || (param[207]>2348) ) return 207;//offset.Ib2 + if ( (param[208]<1748) || (param[208]>2348) ) return 208;//offset.Ic2 + if ( (param[209]<1748) || (param[209]>2348) ) return 209;//offset.Udc2 + + if ( param[210] > 5000 ) return 210;//cc.Kp + if ( param[211] > 5000 ) return 211;//cc.Ki + if ( param[212] > 5000 ) return 212;//cf.Kp + if ( param[213] > 5000 ) return 213;//cf.Ki + if ( param[214] > 5000 ) return 214;//csp.Kp + if ( param[215] > 5000 ) return 215;//csp.Ki + + if ( param[220] > 99 ) return 220;//protect.IacMax + if ( param[221] > 136 ) return 221;//protect.UdcMax + if ( param[222] > 200 ) return 222;//IzLim + if ( param[223] > 200 ) return 223;//cf.IdLim + if ( param[224] > 200 ) return 224;//csp.IqLim + if ( param[225] > 110 ) return 225;//protect.UdcMin + if ( param[226] > 200 ) return 226;//protect.WmMax + if ( param[228] > 200 ) return 228;//rf.WmNomPsi + if ( param[229] > 101 ) return 229;//rf.YlimPsi + if ( (param[231]<1) || (param[231]>8500) ) return 231;//protect.TudcMin + if ( (param[233]<1) || (param[233]>8500) ) return 233;//protect.TwmMax + + if ( param[244] < 1 ) return 244;//rs.WlimIncr + if ( param[245] < 1 ) return 245;//csp.IlimIncr + if ( param[248] < 1 ) return 248;//rp.PlimIncr + + if ( (param[269]<5000) || (param[269]>20000) ) return 269;//KmeCorr + + if ( (param[285]<1) || (param[285]>20000) ) return 285;//Kudc + if ( (param[286]<1) || (param[286]>20000) ) return 286;//Kwm + if ( (param[288]<1) || (param[288]>20000) ) return 288;//rs.Kwmz + if ( (param[289]<1) || (param[289]>20000) ) return 289;//rf.Kpsiz + if ( (param[290]<1) || (param[290]>20000) ) return 290;//Kme + if ( (param[292]<1) || (param[292]>20000) ) return 292;//rp.Kpmz + + return 0; +} //short test_param(void) diff --git a/Inu/detcoeff.h b/Inu/detcoeff.h new file mode 100644 index 0000000..c3465ec --- /dev/null +++ b/Inu/detcoeff.h @@ -0,0 +1,47 @@ +#ifndef DETCOEFF +#define DETCOEFF + +// Переменные, которые определены в detcoeff.c (begin) +//######################################################################### +//######################################################################### +// Переменные, которые определены в detcoeff.c (end) + + + + +// Переменные, которые объявлены в detcoeff.c (begin) +//######################################################################### +extern short testParamFaultNo; +extern struct Eprom eprom; +extern volatile short faultNo; +extern volatile short state; +extern unsigned short param[]; + +extern struct Rf rf; +extern struct Offset offset; +extern struct SgmPar sgmPar; +extern struct Cc cc; +extern struct Cf cf; +extern struct Csp csp; +extern float IzLim; +extern struct Protect protect; +extern struct Rs rs; +extern struct Rp rp; +extern float KmeCorr; +extern float Kudc; +extern float Kwm; +extern float Kme; + +extern volatile struct Out out; +extern volatile float udc1; +extern volatile float udc2; +extern volatile float wmNf; +extern volatile float wm; +extern volatile float me; +extern short onceShutdown; +extern short onceFaultReset; +extern short stopPause; +extern struct Mst mst; +//######################################################################### +// Переменные, которые объявлены в detcoeff.c (end) +#endif //DETCOEFF diff --git a/Inu/init28335.c b/Inu/init28335.c new file mode 100644 index 0000000..42dfaa5 --- /dev/null +++ b/Inu/init28335.c @@ -0,0 +1,1621 @@ +/************************************************************************** + Description: После загрузки процессора функция вызывается один раз + и инициализирует управляющие регистры процессора + TMS320F28335/TMS320F28379D. + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.10.04 +**************************************************************************/ + + +#include "def.h" +#include "init28335.h" + + +#ifndef ML +extern void InitPll(Uint16 val, Uint16 divsel); +extern void InitPieCtrl(void); +extern void InitPieVectTable(void); +extern void ADC_cal(void); +extern void MemCopy(Uint16 *SourceAddr, Uint16* SourceEndAddr, Uint16* DestAddr); +extern void InitFlash(void); +extern short CsmUnlock(void); +void InitPeripheralClocks(void); +void SetupGpio(void); +extern interrupt void isr(void); +#endif //ML +void SetupAdc(void); +void SetupEpwm(void); +void SetupEqep(void); + + +void init28335(void) { +#ifndef ML + // Global Disable all Interrupts + DINT; + // Disable CPU interrupts + IER = 0x0000; + // Clear all CPU interrupt flags + IFR = 0x0000; + + // Initialize the PLL control: PLLCR[DIV] and PLLSTS[DIVSEL] + InitPll(PLLCR_DIV, PLLSTS_DIVSEL); + + // Initialize interrupt controller and Vector Table + // to defaults for now. Application ISR mapping done later. + InitPieCtrl(); + InitPieVectTable(); + + // Initialize the peripheral clocks + InitPeripheralClocks(); + + // Ulock the CSM + csmSuccess = CsmUnlock(); + + /* Copy time critical code and Flash setup code to RAM + (the RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart, + RamfuncsLoadStart2, RamfuncsLoadEnd2 and RamfuncsRunStart2 + symbols are created by the linker) */ + MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart); + MemCopy(&RamfuncsLoadStart2, &RamfuncsLoadEnd2, &RamfuncsRunStart2); + /* Copy the .switch section */ + // (the SwitchLoadStart, SwitchLoadEnd and SwitchRunStart + // symbols are created by the linker) + MemCopy(&SwitchLoadStart, &SwitchLoadEnd, &SwitchRunStart); + /* Copy the .econst section */ + // (the EconstLoadStart, EconstLoadEnd and EconstRunStart + // symbols are created by the linker) + MemCopy(&EconstLoadStart, &EconstLoadEnd, &EconstRunStart); + // Call Flash Initialization to setup flash waitstates + // (this function must reside in RAM) + InitFlash(); +#endif //ML + + // Setup ePWM + SetupEpwm(); + // Setup ADC + SetupAdc(); + // Setup eQEP + SetupEqep(); + +#ifndef ML + // Setup GPIO + SetupGpio(); + + // Reassign ISR + EALLOW; + PieVectTable.ADCINT = &isr; + EDIS; +#endif //ML +} //void init28335(void) + + + +#ifndef ML +/* This function initializes the clocks to the peripheral modules. +First the high and low clock prescalers are set +Second the clocks are enabled to each peripheral. +To reduce power, leave clocks to unused peripherals disabled + +Note: If a peripherals clock is not enabled then you cannot +read or write to the registers for that peripheral */ +void InitPeripheralClocks(void) { + EALLOW; + + // HISPCP/LOSPCP prescale register settings, normally it will be set + // to default values + // High speed clock = FSYSCLKOUT/2 + SysCtrlRegs.HISPCP.all = 0x0001; + // Low speed clock = FSYSCLKOUT/4 + SysCtrlRegs.LOSPCP.all = 0x0002; + + /* Peripheral clock enables set for the selected peripherals. + If you are not using a peripheral leave the clock off + to save on power. + + This function is not written to be an example of efficient code */ + + SysCtrlRegs.PCLKCR0.bit.ADCENCLK = 1; // ADC + + /* IMPORTANT + The ADC_cal function, which copies the ADC calibration values from + TI reserved OTP into the ADCREFSEL and ADCOFFTRIM registers, occurs + automatically in the Boot ROM. If the boot ROM code is bypassed + during the debug process, the following function MUST be called for + the ADC to function according to specification. The clocks to the + ADC MUST be enabled before calling this function. + See the device data manual and/or the ADC Reference + Manual for more information */ + ADC_cal(); + + SysCtrlRegs.PCLKCR0.bit.I2CAENCLK = 0; // I2C + SysCtrlRegs.PCLKCR0.bit.SCIAENCLK = 0; // SCI-A + SysCtrlRegs.PCLKCR0.bit.SCIBENCLK = 0; // SCI-B + SysCtrlRegs.PCLKCR0.bit.SCICENCLK = 0; // SCI-C + SysCtrlRegs.PCLKCR0.bit.SPIAENCLK = 0; // SPI-A + SysCtrlRegs.PCLKCR0.bit.MCBSPAENCLK = 0;// McBSP-A + SysCtrlRegs.PCLKCR0.bit.MCBSPBENCLK = 0;// McBSP-B + SysCtrlRegs.PCLKCR0.bit.ECANAENCLK = 0; // eCAN-A + SysCtrlRegs.PCLKCR0.bit.ECANBENCLK = 0; // eCAN-B + + SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 0; // Disable TBCLK within the ePWM + SysCtrlRegs.PCLKCR1.bit.EPWM1ENCLK = 1; // ePWM1 + SysCtrlRegs.PCLKCR1.bit.EPWM2ENCLK = 1; // ePWM2 + SysCtrlRegs.PCLKCR1.bit.EPWM3ENCLK = 1; // ePWM3 + SysCtrlRegs.PCLKCR1.bit.EPWM4ENCLK = 1; // ePWM4 + SysCtrlRegs.PCLKCR1.bit.EPWM5ENCLK = 1; // ePWM5 + SysCtrlRegs.PCLKCR1.bit.EPWM6ENCLK = 1; // ePWM6 + SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 1; // Enable TBCLK within the ePWM + + SysCtrlRegs.PCLKCR1.bit.ECAP3ENCLK = 0; // eCAP3 + SysCtrlRegs.PCLKCR1.bit.ECAP4ENCLK = 0; // eCAP4 + SysCtrlRegs.PCLKCR1.bit.ECAP5ENCLK = 0; // eCAP5 + SysCtrlRegs.PCLKCR1.bit.ECAP6ENCLK = 0; // eCAP6 + SysCtrlRegs.PCLKCR1.bit.ECAP1ENCLK = 0; // eCAP1 + SysCtrlRegs.PCLKCR1.bit.ECAP2ENCLK = 0; // eCAP2 + SysCtrlRegs.PCLKCR1.bit.EQEP1ENCLK = 0; // eQEP1 + SysCtrlRegs.PCLKCR1.bit.EQEP2ENCLK = 1; // eQEP2 + + SysCtrlRegs.PCLKCR3.bit.CPUTIMER0ENCLK = 0; // CPU Timer 0 + SysCtrlRegs.PCLKCR3.bit.CPUTIMER1ENCLK = 0; // CPU Timer 1 + SysCtrlRegs.PCLKCR3.bit.CPUTIMER2ENCLK = 0; // CPU Timer 2 + + SysCtrlRegs.PCLKCR3.bit.DMAENCLK = 0; // DMA Clock + SysCtrlRegs.PCLKCR3.bit.XINTFENCLK = 1; // XTIMCLK + SysCtrlRegs.PCLKCR3.bit.GPIOINENCLK = 1; // GPIO input clock + + EDIS; +} //void InitPeripheralClocks(void) + + + +// Настраивает GPIO +void SetupGpio(void) { + EALLOW; + // GPIO and Peripheral Multiplexing + // GPIO0 ... GPIO15 + GpioCtrlRegs.GPAMUX1.bit.GPIO0 = 1;//EPWM1A (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO1 = 1;//EPWM1B (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO2 = 1;//EPWM2A (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO3 = 1;//EPWM2B (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO4 = 1;//EPWM3A (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO5 = 1;//EPWM3B (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO6 = 1;//EPWM4A (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO7 = 1;//EPWM4B (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO8 = 1;//EPWM5A (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO9 = 1;//EPWM5B (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO10 = 1;//EPWM6A (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO11 = 1;//EPWM6B (INU) + GpioCtrlRegs.GPAMUX1.bit.GPIO12 = 0;//DI + GpioCtrlRegs.GPAMUX1.bit.GPIO13 = 0;//DO + GpioCtrlRegs.GPAMUX1.bit.GPIO14 = 0;//DI + GpioCtrlRegs.GPAMUX1.bit.GPIO15 = 0;//DO + // GPIO16 ... GPIO31 + GpioCtrlRegs.GPAMUX2.bit.GPIO16 = 0;//DI + GpioCtrlRegs.GPAMUX2.bit.GPIO17 = 0;//DO + GpioCtrlRegs.GPAMUX2.bit.GPIO18 = 0;//DI + GpioCtrlRegs.GPAMUX2.bit.GPIO19 = 0;//DO + GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 0;//DO + GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 0;//DI + GpioCtrlRegs.GPAMUX2.bit.GPIO22 = 3;//SCITXDB + GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 3;//SCIRXDB + GpioCtrlRegs.GPAMUX2.bit.GPIO24 = 2;//EQEP2A + GpioCtrlRegs.GPAMUX2.bit.GPIO25 = 2;//EQEP2B + GpioCtrlRegs.GPAMUX2.bit.GPIO26 = 2;//EQEP2I + GpioCtrlRegs.GPAMUX2.bit.GPIO27 = 0;//DI + GpioCtrlRegs.GPAMUX2.bit.GPIO28 = 1;//SCIRXDA + GpioCtrlRegs.GPAMUX2.bit.GPIO29 = 1;//SCITXDA + GpioCtrlRegs.GPAMUX2.bit.GPIO30 = 0;//DO + GpioCtrlRegs.GPAMUX2.bit.GPIO31 = 3;//XA17 + // GPIO32 ... GPIO47 + GpioCtrlRegs.GPBMUX1.bit.GPIO32 = 2;//EPWMSYNCI + GpioCtrlRegs.GPBMUX1.bit.GPIO33 = 2;//EPWMSYNCO + GpioCtrlRegs.GPBMUX1.bit.GPIO34 = 0;//DO + GpioCtrlRegs.GPBMUX1.bit.GPIO35 = 0;//DO + GpioCtrlRegs.GPBMUX1.bit.GPIO36 = 3;//XZCS0 + GpioCtrlRegs.GPBMUX1.bit.GPIO37 = 3;//XZCS7 + GpioCtrlRegs.GPBMUX1.bit.GPIO38 = 3;//XWE0 + GpioCtrlRegs.GPBMUX1.bit.GPIO39 = 3;//XA16 + GpioCtrlRegs.GPBMUX1.bit.GPIO40 = 3;//XA0/XWE1 + GpioCtrlRegs.GPBMUX1.bit.GPIO41 = 3;//XA1 + GpioCtrlRegs.GPBMUX1.bit.GPIO42 = 3;//XA2 + GpioCtrlRegs.GPBMUX1.bit.GPIO43 = 3;//XA3 + GpioCtrlRegs.GPBMUX1.bit.GPIO44 = 3;//XA4 + GpioCtrlRegs.GPBMUX1.bit.GPIO45 = 3;//XA5 + GpioCtrlRegs.GPBMUX1.bit.GPIO46 = 3;//XA6 + GpioCtrlRegs.GPBMUX1.bit.GPIO47 = 3;//XA7 + // GPIO48 ... GPIO63 + GpioCtrlRegs.GPBMUX2.bit.GPIO48 = 0;//DO + GpioCtrlRegs.GPBMUX2.bit.GPIO49 = 0;//DO + GpioCtrlRegs.GPBMUX2.bit.GPIO50 = 0;//DI (неисправность источника питания +24 В) + GpioCtrlRegs.GPBMUX2.bit.GPIO51 = 0;//DI + GpioCtrlRegs.GPBMUX2.bit.GPIO52 = 0;//DI + GpioCtrlRegs.GPBMUX2.bit.GPIO53 = 0;//DI + GpioCtrlRegs.GPBMUX2.bit.GPIO54 = 1;//SPISIMOA + GpioCtrlRegs.GPBMUX2.bit.GPIO55 = 1;//SPISOMIA + GpioCtrlRegs.GPBMUX2.bit.GPIO56 = 1;//SPICLKA + GpioCtrlRegs.GPBMUX2.bit.GPIO57 = 0;//DO + GpioCtrlRegs.GPBMUX2.bit.GPIO58 = 0;//DO + GpioCtrlRegs.GPBMUX2.bit.GPIO59 = 0;//DO (зеленый светодиод "Готовность") + GpioCtrlRegs.GPBMUX2.bit.GPIO60 = 0;//DO (зеленый светодиод "Работа") + GpioCtrlRegs.GPBMUX2.bit.GPIO61 = 0;//DO (красный светодиод "Авария") + GpioCtrlRegs.GPBMUX2.bit.GPIO62 = 1;//SCIRXDC + GpioCtrlRegs.GPBMUX2.bit.GPIO63 = 1;//SCITXDC + // GPIO64 ... GPIO79 + GpioCtrlRegs.GPCMUX1.bit.GPIO64 = 3;//XD15 + GpioCtrlRegs.GPCMUX1.bit.GPIO65 = 3;//XD14 + GpioCtrlRegs.GPCMUX1.bit.GPIO66 = 3;//XD13 + GpioCtrlRegs.GPCMUX1.bit.GPIO67 = 3;//XD12 + GpioCtrlRegs.GPCMUX1.bit.GPIO68 = 3;//XD11 + GpioCtrlRegs.GPCMUX1.bit.GPIO69 = 3;//XD10 + GpioCtrlRegs.GPCMUX1.bit.GPIO70 = 3;//XD9 + GpioCtrlRegs.GPCMUX1.bit.GPIO71 = 3;//XD8 + GpioCtrlRegs.GPCMUX1.bit.GPIO72 = 3;//XD7 + GpioCtrlRegs.GPCMUX1.bit.GPIO73 = 3;//XD6 + GpioCtrlRegs.GPCMUX1.bit.GPIO74 = 3;//XD5 + GpioCtrlRegs.GPCMUX1.bit.GPIO75 = 3;//XD4 + GpioCtrlRegs.GPCMUX1.bit.GPIO76 = 3;//XD3 + GpioCtrlRegs.GPCMUX1.bit.GPIO77 = 3;//XD2 + GpioCtrlRegs.GPCMUX1.bit.GPIO78 = 3;//XD1 + GpioCtrlRegs.GPCMUX1.bit.GPIO79 = 3;//XD0 + // GPIO80 ... GPIO87 + GpioCtrlRegs.GPCMUX2.bit.GPIO80 = 3;//XA8 + GpioCtrlRegs.GPCMUX2.bit.GPIO81 = 3;//XA9 + GpioCtrlRegs.GPCMUX2.bit.GPIO82 = 3;//XA10 + GpioCtrlRegs.GPCMUX2.bit.GPIO83 = 3;//XA11 + GpioCtrlRegs.GPCMUX2.bit.GPIO84 = 3;//XA12 + GpioCtrlRegs.GPCMUX2.bit.GPIO85 = 3;//XA13 + GpioCtrlRegs.GPCMUX2.bit.GPIO86 = 3;//XA14 + GpioCtrlRegs.GPCMUX2.bit.GPIO87 = 3;//XA15 + + // выбираем состояние цифровых выходов + DO_GPIO019_CLEAR; + DO_GPIO020_CLEAR; + DO_GPIO022_CLEAR; + // ... светодиоды выключаем + LED_GREEN1_OFF; + LED_GREEN2_OFF; + LED_RED_OFF; + DO_GPIO63_CLEAR; + + // Select the direction of the GPIO pins + // GPIO0 ... GPIO31 + GpioCtrlRegs.GPADIR.bit.GPIO0 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO1 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO2 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO3 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO4 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO5 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO6 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO7 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO8 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO9 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO10 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO11 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO12 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO13 = 1; + GpioCtrlRegs.GPADIR.bit.GPIO14 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO15 = 1; + GpioCtrlRegs.GPADIR.bit.GPIO16 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO17 = 1; + GpioCtrlRegs.GPADIR.bit.GPIO18 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO19 = 1; + GpioCtrlRegs.GPADIR.bit.GPIO20 = 1; + GpioCtrlRegs.GPADIR.bit.GPIO21 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO22 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO23 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO24 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO25 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO26 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO27 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO28 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO29 = 0; + GpioCtrlRegs.GPADIR.bit.GPIO30 = 1; + GpioCtrlRegs.GPADIR.bit.GPIO31 = 0; + // GPIO32 ... GPIO63 + GpioCtrlRegs.GPBDIR.bit.GPIO32 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO33 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO34 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO35 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO36 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO37 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO38 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO39 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO40 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO41 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO42 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO43 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO44 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO45 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO46 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO47 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO48 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO49 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO50 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO51 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO52 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO53 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO54 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO55 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO56 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO57 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO58 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO59 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO60 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO61 = 1; + GpioCtrlRegs.GPBDIR.bit.GPIO62 = 0; + GpioCtrlRegs.GPBDIR.bit.GPIO63 = 1; + // GPIO64 ... GPIO87 + GpioCtrlRegs.GPCDIR.bit.GPIO64 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO65 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO66 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO67 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO68 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO69 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO70 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO71 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO72 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO73 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO74 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO75 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO76 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO77 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO78 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO79 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO80 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO81 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO82 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO83 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO84 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO85 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO86 = 0; + GpioCtrlRegs.GPCDIR.bit.GPIO87 = 0; + + // Each input can have different qualification: + // 0 - Synchronize to FSYSCLKOUT only; + // 1 - Qualification using 3 samples; + // 2 - Qualification using 6 samples; + // 3 - Asynchronous. + // GPIO0 ... GPIO15 + GpioCtrlRegs.GPAQSEL1.bit.GPIO0 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO1 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO2 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO3 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO4 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO5 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO6 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO7 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO8 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO9 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO10 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO11 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO12 = 2; + GpioCtrlRegs.GPAQSEL1.bit.GPIO13 = 0; + GpioCtrlRegs.GPAQSEL1.bit.GPIO14 = 2; + GpioCtrlRegs.GPAQSEL1.bit.GPIO15 = 0; + // GPIO16 ... GPIO31 + GpioCtrlRegs.GPAQSEL2.bit.GPIO16 = 2; + GpioCtrlRegs.GPAQSEL2.bit.GPIO17 = 0; + GpioCtrlRegs.GPAQSEL2.bit.GPIO18 = 2; + GpioCtrlRegs.GPAQSEL2.bit.GPIO19 = 0; + GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 0; + GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2; + GpioCtrlRegs.GPAQSEL2.bit.GPIO22 = 3; + GpioCtrlRegs.GPAQSEL2.bit.GPIO23 = 3; + GpioCtrlRegs.GPAQSEL2.bit.GPIO24 = 0; + GpioCtrlRegs.GPAQSEL2.bit.GPIO25 = 0; + GpioCtrlRegs.GPAQSEL2.bit.GPIO26 = 0; + GpioCtrlRegs.GPAQSEL2.bit.GPIO27 = 2; + GpioCtrlRegs.GPAQSEL2.bit.GPIO28 = 3; + GpioCtrlRegs.GPAQSEL2.bit.GPIO29 = 3; + GpioCtrlRegs.GPAQSEL2.bit.GPIO30 = 0; + GpioCtrlRegs.GPAQSEL2.bit.GPIO31 = 0; + // GPIO32 ... GPIO47 + GpioCtrlRegs.GPBQSEL1.bit.GPIO32 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO33 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO34 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO35 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO36 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO37 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO38 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO39 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO40 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO41 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO42 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO43 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO44 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO45 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO46 = 0; + GpioCtrlRegs.GPBQSEL1.bit.GPIO47 = 0; + // GPIO48 ... GPIO63 + GpioCtrlRegs.GPBQSEL2.bit.GPIO48 = 0; + GpioCtrlRegs.GPBQSEL2.bit.GPIO49 = 0; + GpioCtrlRegs.GPBQSEL2.bit.GPIO50 = 2; + GpioCtrlRegs.GPBQSEL2.bit.GPIO51 = 2; + GpioCtrlRegs.GPBQSEL2.bit.GPIO52 = 2; + GpioCtrlRegs.GPBQSEL2.bit.GPIO53 = 2; + GpioCtrlRegs.GPBQSEL2.bit.GPIO54 = 3; + GpioCtrlRegs.GPBQSEL2.bit.GPIO55 = 3; + GpioCtrlRegs.GPBQSEL2.bit.GPIO56 = 3; + GpioCtrlRegs.GPBQSEL2.bit.GPIO57 = 3; + GpioCtrlRegs.GPBQSEL2.bit.GPIO58 = 0; + GpioCtrlRegs.GPBQSEL2.bit.GPIO59 = 0; + GpioCtrlRegs.GPBQSEL2.bit.GPIO60 = 0; + GpioCtrlRegs.GPBQSEL2.bit.GPIO61 = 0; + GpioCtrlRegs.GPBQSEL2.bit.GPIO62 = 2; + GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 0; + + // Qualification Control (sampling period = (1/FSYSCLKOUT)*QUALPRDx*2) + // ( (1/150e6)*255*2 = 3.4 мкс ) + // Port A + GpioCtrlRegs.GPACTRL.bit.QUALPRD0 = 255;//GPIO0 ... GPIO7 + GpioCtrlRegs.GPACTRL.bit.QUALPRD1 = 255;//GPIO8 ... GPIO15 + GpioCtrlRegs.GPACTRL.bit.QUALPRD2 = 255;//GPIO16 ... GPIO23 + GpioCtrlRegs.GPACTRL.bit.QUALPRD3 = 255;//GPIO24 ... GPIO31 + // Port B + GpioCtrlRegs.GPBCTRL.bit.QUALPRD0 = 255;//GPIO32 ... GPIO39 + GpioCtrlRegs.GPBCTRL.bit.QUALPRD1 = 255;//GPIO40 ... GPIO47 + GpioCtrlRegs.GPBCTRL.bit.QUALPRD2 = 255;//GPIO48 ... GPIO55 + GpioCtrlRegs.GPBCTRL.bit.QUALPRD3 = 255;//GPIO56 ... GPIO63 + + // Pull-ups (the internal pullup запрещён для ШИМ-выходов) + // GPIO0 ... GPIO31 + GpioCtrlRegs.GPAPUD.all = 0x00000FFF; + // GPIO32 ... GPIO63 + GpioCtrlRegs.GPBPUD.all = 0x00000000; + // GPIO64 ... GPIO87 + GpioCtrlRegs.GPCPUD.all = 0x00000000; + EDIS; +} //void SetupGpio(void) +#endif //ML + + + +// Настраивает ePWM +void SetupEpwm(void) { + // ePWM1 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm1Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm1Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm1Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm1Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm1Regs.TBCTL.bit.SYNCOSEL = 1;//TBCTR==0 -> EPWMxSYNCO + EPwm1Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm1Regs.TBCTL.bit.PHSEN = 0;//do not load TBCTR from TBPHS + EPwm1Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm1Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm1Regs.TBPHS.half.TBPHS = 0; + // Time-Base Counter Register + EPwm1Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm1Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm1Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm1Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm1Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm1Regs.CMPCTL.bit.LOADBMODE = 0;//active CMPB load from shadow - load on CTR = Zero + EPwm1Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm1Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm1Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm1Regs.AQCTLA.bit.CAD = 2;//set - force EPWMxA output high + EPwm1Regs.AQCTLA.bit.CAU = 1;//clear - force EPWMxA output low + EPwm1Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm1Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm1Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm1Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm1Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm1Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm1Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm1Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm1Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm1Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm1Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm1Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm1Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm1Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm1Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm1Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm1Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm1Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm1Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm1Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm1Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm1Regs.ETSEL.bit.SOCBEN = 1;//enable EPWMxSOCB pulse + EPwm1Regs.ETSEL.bit.SOCBSEL = 1;//EPWMxSOCB selection - enable event CTR == 0 + EPwm1Regs.ETSEL.bit.SOCAEN = 1;//enable EPWMxSOCA pulse + EPwm1Regs.ETSEL.bit.SOCASEL = 2;//EPWMxSOCA selection - enable event CTR == PRD + EPwm1Regs.ETSEL.bit.INTEN = 0;//disable EPWMx_INT generation + EPwm1Regs.ETSEL.bit.INTSEL = 0;//reserved + // Event-Trigger Prescale Register + EPwm1Regs.ETPS.bit.SOCBPRD = 1;//generate the EPWMxSOCB pulse on the first event + EPwm1Regs.ETPS.bit.SOCAPRD = 1;//generate the EPWMxSOCA pulse on the first event + EPwm1Regs.ETPS.bit.INTPRD = 0;//disable the interrupt event counter (no interrupt will be generated) + + // ePWM2 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm2Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm2Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm2Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm2Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm2Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm2Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm2Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm2Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm2Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm2Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm2Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm2Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm2Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm2Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm2Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm2Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm2Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm2Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm2Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm2Regs.AQCTLA.bit.CAD = 1;//clear - force EPWMxA output low + EPwm2Regs.AQCTLA.bit.CAU = 2;//set - force EPWMxA output high + EPwm2Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm2Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm2Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm2Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm2Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm2Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm2Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm2Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm2Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm2Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm2Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm2Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm2Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm2Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm2Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm2Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm2Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm2Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm2Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm2Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm2Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm2Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm2Regs.ETPS.all = 0; + + // ePWM3 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm3Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm3Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm3Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm3Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm3Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm3Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm3Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm3Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm3Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm3Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm3Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm3Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm3Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm3Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm3Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm3Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm3Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm3Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm3Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm3Regs.AQCTLA.bit.CAD = 2;//set - force EPWMxA output high + EPwm3Regs.AQCTLA.bit.CAU = 1;//clear - force EPWMxA output low + EPwm3Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm3Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm3Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm3Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm3Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm3Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm3Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm3Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm3Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm3Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm3Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm3Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm3Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm3Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm3Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm3Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm3Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm3Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm3Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm3Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm3Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm3Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm3Regs.ETPS.all = 0; + + // ePWM4 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm4Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm4Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm4Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm4Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm4Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm4Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm4Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm4Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm4Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm4Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm4Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm4Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm4Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm4Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm4Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm4Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm4Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm4Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm4Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm4Regs.AQCTLA.bit.CAD = 1;//clear - force EPWMxA output low + EPwm4Regs.AQCTLA.bit.CAU = 2;//set - force EPWMxA output high + EPwm4Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm4Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm4Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm4Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm4Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm4Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm4Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm4Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm4Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm4Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm4Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm4Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm4Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm4Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm4Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm4Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm4Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm4Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm4Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm4Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm4Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm4Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm4Regs.ETPS.all = 0; + + // ePWM5 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm5Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm5Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm5Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm5Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm5Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm5Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm5Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm5Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm5Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm5Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm5Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm5Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm5Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm5Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm5Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm5Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm5Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm5Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm5Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm5Regs.AQCTLA.bit.CAD = 2;//set - force EPWMxA output high + EPwm5Regs.AQCTLA.bit.CAU = 1;//clear - force EPWMxA output low + EPwm5Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm5Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm5Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm5Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm5Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm5Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm5Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm5Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm5Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm5Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm5Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm5Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm5Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm5Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm5Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm5Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm5Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm5Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm5Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm5Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm5Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm5Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm5Regs.ETPS.all = 0; + + // ePWM6 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm6Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm6Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm6Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm6Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm6Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm6Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm6Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm6Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm6Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm6Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm6Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm6Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm6Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm6Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm6Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm6Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm6Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm6Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm6Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm6Regs.AQCTLA.bit.CAD = 1;//clear - force EPWMxA output low + EPwm6Regs.AQCTLA.bit.CAU = 2;//set - force EPWMxA output high + EPwm6Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm6Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm6Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm6Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm6Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm6Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm6Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm6Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm6Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm6Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm6Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm6Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm6Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm6Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm6Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm6Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm6Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm6Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm6Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm6Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm6Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm6Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm6Regs.ETPS.all = 0; + +#ifdef ML + // ePWM7 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm7Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm7Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm7Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm7Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm7Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm7Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm7Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm7Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm7Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm7Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm7Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm7Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm7Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm7Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm7Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm7Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm7Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm7Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm7Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm7Regs.AQCTLA.bit.CAD = 2;//set - force EPWMxA output high + EPwm7Regs.AQCTLA.bit.CAU = 1;//clear - force EPWMxA output low + EPwm7Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm7Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm7Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm7Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm7Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm7Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm7Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm7Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm7Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm7Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm7Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm7Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm7Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm7Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm7Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm7Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm7Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm7Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm7Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm7Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm7Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm7Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm7Regs.ETPS.all = 0; + + // ePWM8 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm8Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm8Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm8Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm8Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm8Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm8Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm8Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm8Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm8Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm8Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm8Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm8Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm8Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm8Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm8Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm8Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm8Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm8Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm8Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm8Regs.AQCTLA.bit.CAD = 1;//clear - force EPWMxA output low + EPwm8Regs.AQCTLA.bit.CAU = 2;//set - force EPWMxA output high + EPwm8Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm8Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm8Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm8Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm8Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm8Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm8Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm8Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm8Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm8Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm8Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm8Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm8Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm8Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm8Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm8Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm8Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm8Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm8Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm8Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm8Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm8Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm8Regs.ETPS.all = 0; + + // ePWM9 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm9Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm9Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm9Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm9Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm9Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm9Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm9Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm9Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm9Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm9Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm9Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm9Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm9Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm9Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm9Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm9Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm9Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm9Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm9Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm9Regs.AQCTLA.bit.CAD = 2;//set - force EPWMxA output high + EPwm9Regs.AQCTLA.bit.CAU = 1;//clear - force EPWMxA output low + EPwm9Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm9Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm9Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm9Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm9Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm9Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm9Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm9Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm9Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm9Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm9Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm9Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm9Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm9Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm9Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm9Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm9Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm9Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm9Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm9Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm9Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm9Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm9Regs.ETPS.all = 0; + + // ePWM10 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm10Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm10Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm10Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm10Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm10Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm10Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm10Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm10Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm10Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm10Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm10Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm10Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm10Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm10Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm10Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm10Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm10Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm10Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm10Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm10Regs.AQCTLA.bit.CAD = 1;//clear - force EPWMxA output low + EPwm10Regs.AQCTLA.bit.CAU = 2;//set - force EPWMxA output high + EPwm10Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm10Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm10Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm10Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm10Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm10Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm10Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm10Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm10Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm10Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm10Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm10Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm10Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm10Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm10Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm10Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm10Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm10Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm10Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm10Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm10Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm10Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm10Regs.ETPS.all = 0; + + // ePWM11 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm11Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm11Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm11Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm11Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm11Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm11Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm11Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm11Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm11Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm11Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm11Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm11Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm11Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm11Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm11Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm11Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm11Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm11Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm11Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm11Regs.AQCTLA.bit.CAD = 2;//set - force EPWMxA output high + EPwm11Regs.AQCTLA.bit.CAU = 1;//clear - force EPWMxA output low + EPwm11Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm11Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm11Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm11Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm11Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm11Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm11Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm11Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm11Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm11Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm11Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm11Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm11Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm11Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm11Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm11Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm11Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm11Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm11Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm11Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm11Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm11Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm11Regs.ETPS.all = 0; + + // ePWM12 + // #################################################################### + // Time-Base (TB) Submodule + // -------------------------------------------------------------------- + // Time-Base Control Register + EPwm12Regs.TBCTL.bit.FREE_SOFT = 0;//emulation mode - stop after the next time-base counter increment or decrement + EPwm12Regs.TBCTL.bit.PHSDIR = 1;//count up after the synchronization event + EPwm12Regs.TBCTL.bit.CLKDIV = 0;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm12Regs.TBCTL.bit.HSPCLKDIV = 2;//TBCLK = SYSCLKOUT/(HSPCLKDIV*CLKDIV) + EPwm12Regs.TBCTL.bit.SYNCOSEL = 0;//SYNCO = SYNCI + EPwm12Regs.TBCTL.bit.PRDLD = 1;//load the TBPRD register immediately without using a shadow register + EPwm12Regs.TBCTL.bit.PHSEN = 1;//load TBCTR with TBPHS when EPWMxSYNCI input signal occurs + EPwm12Regs.TBCTL.bit.CTRMODE = 3;//stop-freeze counter operation + // Time-Base Period Register + EPwm12Regs.TBPRD = (unsigned short)T1_PRD; + // Time-Base Phase Register + EPwm12Regs.TBPHS.half.TBPHS = 2; + // Time-Base Counter Register + EPwm12Regs.TBCTR = 1; + // Counter-Compare (CC) Submodule + // -------------------------------------------------------------------- + // Counter-Compare A Register + EPwm12Regs.CMPA.half.CMPA = 0; + // Counter-Compare B Register + EPwm12Regs.CMPB = 0; + // Counter-Compare Control Register + EPwm12Regs.CMPCTL.bit.SHDWBMODE = 0;//CMPB operating mode - shadow + EPwm12Regs.CMPCTL.bit.SHDWAMODE = 0;//CMPA operating mode - shadow + EPwm12Regs.CMPCTL.bit.LOADBMODE = 0;//has no effect in immediate mode + EPwm12Regs.CMPCTL.bit.LOADAMODE = 2;//active CMPA load from shadow - load on CTR = Zero or CTR = PRD + // Action-Qualifier (AQ) Submodule + // -------------------------------------------------------------------- + // Action-Qualifier Output A Control Register + EPwm12Regs.AQCTLA.bit.CBD = 0;//do nothing + EPwm12Regs.AQCTLA.bit.CBU = 0;//do nothing + EPwm12Regs.AQCTLA.bit.CAD = 1;//clear - force EPWMxA output low + EPwm12Regs.AQCTLA.bit.CAU = 2;//set - force EPWMxA output high + EPwm12Regs.AQCTLA.bit.PRD = 0;//do nothing + EPwm12Regs.AQCTLA.bit.ZRO = 0;//do nothing + // Action-Qualifier Output B Control Register + EPwm12Regs.AQCTLB.bit.CBD = 0;//do nothing + EPwm12Regs.AQCTLB.bit.CBU = 0;//do nothing + EPwm12Regs.AQCTLB.bit.CAD = 0;//do nothing + EPwm12Regs.AQCTLB.bit.CAU = 0;//do nothing + EPwm12Regs.AQCTLB.bit.PRD = 0;//do nothing + EPwm12Regs.AQCTLB.bit.ZRO = 0;//do nothing + // Action-Qualifier Software Force Register + EPwm12Regs.AQSFRC.all = 0; + // Action-Qualifier Continuous Software Force Register + EPwm12Regs.AQCSFRC.all = 0; + // Dead-Band Generator (DB) Submodule + // -------------------------------------------------------------------- + // Dead-Band Generator Control Register + EPwm12Regs.DBCTL.bit.IN_MODE = 0;//EPWMxA In (from the action-qualifier) is the source for both falling-edge and rising-edge delay + EPwm12Regs.DBCTL.bit.POLSEL = 2;//active high complementary (AHC) mode + EPwm12Regs.DBCTL.bit.OUT_MODE = 3;//dead-band is fully enabled for both rising-edge delay on output EPWMxA and falling-edge delay on output EPWMxB + // Dead-Band Generator Rising Edge Delay Register + EPwm12Regs.DBRED = (unsigned short)(FTBCLK*DT); + // Dead-Band Generator Falling Edge Delay Register + EPwm12Regs.DBFED = (unsigned short)(FTBCLK*DT); + // PWM-Chopper (PC) Submodule + // -------------------------------------------------------------------- + // PWM-Chopper Control Register + EPwm12Regs.PCCTL.all = 0; + // Trip-Zone (TZ) Submodule + // -------------------------------------------------------------------- + EALLOW; + // Trip-Zone Select Register + EPwm12Regs.TZSEL.all = 0; + // Trip-Zone Control Register + EPwm12Regs.TZCTL.bit.TZB = 2;//when a trip event occurs the following action is taken on output EPWMxB - force EPWMxB to a low state + EPwm12Regs.TZCTL.bit.TZA = 2;//when a trip event occurs the following action is taken on output EPWMxA - force EPWMxA to a low state + // Trip-Zone Enable Interrupt Register + EPwm12Regs.TZEINT.all = 0; + // Trip-Zone Force Register + EPwm12Regs.TZFRC.all = 0x0004;//forces a one-shot trip event via software and sets the TZFLG[OST] bit + EDIS; + // Event-Trigger (ET) Submodule + // -------------------------------------------------------------------- + // Event-Trigger Selection Register + EPwm12Regs.ETSEL.all = 0; + // Event-Trigger Prescale Register + EPwm12Regs.ETPS.all = 0; +#endif //ML +} //void SetupEpwm(void) + + + +// Настраивает ADC +void SetupAdc(void) { +#ifndef ML + unsigned short i; + + // ADC Control Register 1 + AdcRegs.ADCTRL1.bit.SUSMOD = 0;//emulation suspend is ignored + AdcRegs.ADCTRL1.bit.ACQ_PS = 3;//width of SOC pulse is (ACQ_PS+1) times the ADCLK period + AdcRegs.ADCTRL1.bit.CPS = 0;//ADCCLK = HSPCLK/(2*ADCCLKPS*(CPS+1)) + AdcRegs.ADCTRL1.bit.CONT_RUN = 0;//start-stop mode + AdcRegs.ADCTRL1.bit.SEQ_OVRD = 0;//sequencer override disabled + AdcRegs.ADCTRL1.bit.SEQ_CASC = 1;//cascaded mode + + // ADC Control Register 2 + AdcRegs.ADCTRL2.bit.EPWM_SOCB_SEQ = 1;//allows SEQ to be started by ePWMx SOCB trigger + AdcRegs.ADCTRL2.bit.SOC_SEQ1 = 0;//clears a pending SOC trigger + AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1 = 1;//interrupt request by INT_SEQ1 is enabled + AdcRegs.ADCTRL2.bit.INT_MOD_SEQ1 = 0;//INT_SEQ1 is set at the end of every SEQ1 sequence + AdcRegs.ADCTRL2.bit.EPWM_SOCA_SEQ1 = 1;//allows SEQ1/SEQ to be started by ePWMx SOCA trigger + AdcRegs.ADCTRL2.bit.EXT_SOC_SEQ1 = 0;//disables an ADC autoconversion sequence to be started by a signal from a GPIO Port A pin + AdcRegs.ADCTRL2.bit.SOC_SEQ2 = 0;//clears a pending SOC trigger + AdcRegs.ADCTRL2.bit.INT_ENA_SEQ2 = 0;//interrupt request by INT_SEQ2 is disabled + AdcRegs.ADCTRL2.bit.INT_MOD_SEQ2 = 0;//INT_SEQ2 is set at the end of every SEQ2 sequence + AdcRegs.ADCTRL2.bit.EPWM_SOCB_SEQ2 = 0;//SEQ2 cannot be started by ePWMx SOCB trigger + + /* The ADC resets to the ADC off state. When powering up the ADC, use + the following sequence: + 1. If external reference is desired, enable this mode using bits + 15-14 in the ADCREFSEL Register. This mode must be enabled before + band gap is powered; + 2. Power up the reference, bandgap, and analog circuits together by + setting bits 7-5 (ADCBGRFDN[1:0], ADCPWDN) in the ADCTRL3 register; + 3. Before performing the first conversion, a delay of 5 ms is required */ + + // ADC Reference Select Register + AdcRegs.ADCREFSEL.bit.REF_SEL = 1;//external reference, 2.048 V on ADCREFIN + + // ADC Control Register 3 + AdcRegs.ADCTRL3.all = 0x00E0;//power up the reference, bandgap, and analog circuits together + AdcRegs.ADCTRL3.bit.ADCCLKPS = 5;//ADCCLK = HSPCLK/(2*ADCCLKPS*(CPS+1)) -> ADCCLK = 75e6/(2*5*(0+1)) = 7.5e6 Гц + AdcRegs.ADCTRL3.bit.SMODE_SEL = 1;//simultaneous sampling mode + + // Delay before converting ADC channels + for ( i = 0; i < 65500; i++ ) + ; +#endif //ML + + // Maximum Conversion Channels Register + AdcRegs.ADCMAXCONV.bit.MAX_CONV1 = 5;//6 double conv's (12 total) + // ADC Input Channel Select Sequencing Control Registers + AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 0; + AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 1; + AdcRegs.ADCCHSELSEQ1.bit.CONV02 = 2; + AdcRegs.ADCCHSELSEQ1.bit.CONV03 = 3; + AdcRegs.ADCCHSELSEQ2.bit.CONV04 = 4; + AdcRegs.ADCCHSELSEQ2.bit.CONV05 = 5; + AdcRegs.ADCCHSELSEQ2.bit.CONV06 = 6; + AdcRegs.ADCCHSELSEQ2.bit.CONV07 = 7; + AdcRegs.ADCCHSELSEQ3.bit.CONV08 = 0; + AdcRegs.ADCCHSELSEQ3.bit.CONV09 = 0; + AdcRegs.ADCCHSELSEQ3.bit.CONV10 = 0; + AdcRegs.ADCCHSELSEQ3.bit.CONV11 = 0; + AdcRegs.ADCCHSELSEQ4.bit.CONV12 = 0; + AdcRegs.ADCCHSELSEQ4.bit.CONV13 = 0; + AdcRegs.ADCCHSELSEQ4.bit.CONV14 = 0; + AdcRegs.ADCCHSELSEQ4.bit.CONV15 = 0; +} //void SetupAdc(void) + + + +// Настраивает eQEP +void SetupEqep(void) { + // eQEP Decoder Control Register + EQep2Regs.QDECCTL.bit.QSRC = 0;//Position-counter source selection: Quadrature count mode (QCLK = iCLK, QDIR = iDIR) + EQep2Regs.QDECCTL.bit.SOEN = 0;//Sync output-enable: Disable position-compare sync output + EQep2Regs.QDECCTL.bit.SPSEL = 0;//Sync output pin selection: Index pin is used for sync output + EQep2Regs.QDECCTL.bit.XCR = 0;//External clock rate: 2x resolution: Count the rising/falling edge + EQep2Regs.QDECCTL.bit.SWAP = 0;//Swap quadrature clock inputs: Quadrature-clock inputs are not swapped + EQep2Regs.QDECCTL.bit.IGATE = 0;//Index pulse gating option: Disable gating of Index pulse + EQep2Regs.QDECCTL.bit.QAP = 0;//QEPA input polarity: No effect + EQep2Regs.QDECCTL.bit.QBP = 0;//QEPB input polarity: No effect + EQep2Regs.QDECCTL.bit.QIP = 0;//QEPI input polarity: No effect + EQep2Regs.QDECCTL.bit.QSP = 0;//QEPS input polarity: No effect + // eQEP Control Register + EQep2Regs.QEPCTL.bit.FREE_SOFT = 0;//Emulation Control Bits: all stops immediately + EQep2Regs.QEPCTL.bit.PCRM = 1;//Position counter reset mode: position counter reset on the maximum position + EQep2Regs.QEPCTL.bit.SEI = 0;//Strobe event initialization of position counter: does nothing (action disabled) + EQep2Regs.QEPCTL.bit.IEI = 0;//Index event initialization of position counter: do nothing (action disabled) + EQep2Regs.QEPCTL.bit.SWI = 0;//Software initialization of position counter: do nothing (action disabled) + EQep2Regs.QEPCTL.bit.SEL = 0;//Strobe event latch of position counter: the position counter is latched on the rising edge of QEPS strobe + EQep2Regs.QEPCTL.bit.IEL = 1;//Index event latch of position counter (software index marker): latches position counter on rising edge of the index signal + EQep2Regs.QEPCTL.bit.QPEN = 1;//Quadrature position counter enable/software reset: eQEP position counter is enabled + EQep2Regs.QEPCTL.bit.QCLM = 0;//eQEP capture latch mode: latch on position counter read by CPU + EQep2Regs.QEPCTL.bit.UTE = 1;//eQEP unit timer enable: Enable unit timer + EQep2Regs.QEPCTL.bit.WDE = 0;//eQEP watchdog enable: disable the eQEP watchdog timer + // eQEP Position-compare Control Register + EQep2Regs.QPOSCTL.bit.PCSHDW = 0;//Position-compare shadow enable: Shadow disabled, load Immediate + EQep2Regs.QPOSCTL.bit.PCLOAD = 0;//Position-compare shadow load mode: Load on QPOSCNT = 0 + EQep2Regs.QPOSCTL.bit.PCPOL = 0;//Polarity of sync output: Active HIGH pulse output + EQep2Regs.QPOSCTL.bit.PCE = 0;//Position-compare enable/disable: Disable position compare unit + EQep2Regs.QPOSCTL.bit.PCSPW = 0;//Select-position-compare sync output pulse width: 1 * 4 * SYSCLKOUT cycles + // eQEP Capture Control Register + EQep2Regs.QCAPCTL.bit.CEN = 1;//Enable eQEP capture: eQEP capture unit is enabled + EQep2Regs.QCAPCTL.bit.CCPS = 2;//eQEP capture timer clock prescaler: CAPCLK = SYSCLKOUT/4 + EQep2Regs.QCAPCTL.bit.UPPS = 0;//Unit position event prescaler: UPEVNT = QCLK/1 + // eQEP Position Counter Register + EQep2Regs.QPOSCNT = 0x00000000; + // eQEP Maximum Position Count Register Register + EQep2Regs.QPOSMAX = 0x7FFF; + // eQEP Position-compare Register + EQep2Regs.QPOSCMP = 0x00000000; + // eQEP Unit Timer Register + EQep2Regs.QUTMR = 0x00000000; + // eQEP Register Unit Period Register + EQep2Regs.QUPRD = 0x00000000; + // eQEP Watchdog Timer Register + EQep2Regs.QWDTMR = 0x0000; + // eQEP Watchdog Period Register + EQep2Regs.QWDPRD = 0x0000; + // eQEP Interrupt Enable Register + EQep2Regs.QEINT.all = 0x0000; + // eQEP Capture Timer Register + EQep2Regs.QCTMR = 0x0000; + // eQEP Capture Period Register + EQep2Regs.QCPRD = 0x0000; +} //void SetupEqep(void) diff --git a/Inu/init28335.h b/Inu/init28335.h new file mode 100644 index 0000000..d75c3cf --- /dev/null +++ b/Inu/init28335.h @@ -0,0 +1,24 @@ +#ifndef INIT28335 +#define INIT28335 + +// Переменные, которые определены в init28335.c (begin) +//######################################################################### +//######################################################################### +// Переменные, которые определены в init28335.c (end) + + + + +// Переменные, которые объявлены в init28335.c (begin) +//######################################################################### +#ifndef ML +extern Uint16 RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart; +extern Uint16 RamfuncsLoadStart2, RamfuncsLoadEnd2, RamfuncsRunStart2; +extern Uint16 SwitchLoadStart, SwitchLoadEnd, SwitchRunStart; +extern Uint16 EconstLoadStart, EconstLoadEnd, EconstRunStart; +extern short csmSuccess; +#endif //ML +//######################################################################### +// Переменные, которые объявлены в init28335.c (end) + +#endif //INIT28335 diff --git a/Inu/isr.c b/Inu/isr.c new file mode 100644 index 0000000..9f6f162 Binary files /dev/null and b/Inu/isr.c differ diff --git a/Inu/isr.h b/Inu/isr.h new file mode 100644 index 0000000..f0a8d7a --- /dev/null +++ b/Inu/isr.h @@ -0,0 +1,72 @@ +#ifndef ISR +#define ISR + +// Переменные, которые определены в isr.c (begin) +//######################################################################### +struct Offset offset; +volatile struct Result result; +volatile short state; +volatile short faultNo; +volatile struct Out out; +// Udc +float Kudc; +volatile float udc1Nf; +volatile float udc1; +volatile float udc2Nf; +volatile float udc2; +// Iac +volatile float ia1Nf; +volatile float ib1Nf; +volatile float ic1Nf; +volatile float ix1; +volatile float iy1; +volatile float iac1Nf; +volatile float ia2Nf; +volatile float ib2Nf; +volatile float ic2Nf; +volatile float ix2; +volatile float iy2; +volatile float iac2Nf; +// Wm +float Kwm; +volatile float wmNf; +volatile float wm; +volatile float wmAbs; +// Me +volatile float kMe; +float KmeCorr; +float Kme; +volatile float meNf; +volatile float me; +// Pm +volatile float pm; +// защиты +struct Protect protect; +volatile struct Emerg emerg; +short csmSuccess; +// управляющая логика +volatile short onceShutdown; +volatile short testParamFaultNo; +volatile short onceFaultReset; +volatile short stopPause; +volatile short inuWork; +// обмен +struct Mst mst; +//######################################################################### +// Переменные, которые определены в isr.c (end) + + + + +// Переменные, которые объявлены в isr.c (begin) +//######################################################################### +extern struct SgmPar sgmPar; +extern unsigned short param[]; +extern volatile short onceUpr; +extern volatile float psi; +extern float iq1; +extern float iq2; +extern struct Rp rp; +//######################################################################### +// Переменные, которые объявлены в isr.c (end) +#endif //ISR diff --git a/Inu/main.c b/Inu/main.c new file mode 100644 index 0000000..05cade6 --- /dev/null +++ b/Inu/main.c @@ -0,0 +1,144 @@ +/************************************************************************** + Description: Программа управления INU. + Вызывает п/п инициализации процессора и п/п обмена. + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.10.05 +**************************************************************************/ + + +#include "def.h" +#include "main.h" + + +void control_processor_led(void); +void talk_with_desk(void); +void talk_with_mst(void); +void write_eeprom(void); +extern void detcoeff(void); +extern void init28335(void); + + +#ifndef ML +void main(void) { + // disable the watchdog + EALLOW; + SysCtrlRegs.WDCR = 0x0068; + EDIS; + + // инициализация процессора + init28335(); + + // инициализация программы + detcoeff(); + + // re-enable the watchdog + EALLOW; + SysCtrlRegs.WDCR = 0x00A8; + // ... clear the WD counter + SysCtrlRegs.WDKEY = 0x55; + SysCtrlRegs.WDKEY = 0xAA; + EDIS; + + // clear Interrupt Flag ADC Sequencer 1 + AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1; + // clear PIEIFR1 register + PieCtrlRegs.PIEIFR1.all = 0; + + // before we can start we have to enable interrupt mask in the PIE unit + PieCtrlRegs.PIEIER1.bit.INTx6 = 1; + // core line 1 (INT1) + IER |= M_INT1; + // enable global interrupts and higher priority real-time debug events + EINT; + ERTM; + + // запускаем таймеры (up-down-count mode) + EPwm1Regs.TBCTL.bit.CTRMODE = 2; + EPwm2Regs.TBCTL.bit.CTRMODE = 2; + EPwm3Regs.TBCTL.bit.CTRMODE = 2; + EPwm4Regs.TBCTL.bit.CTRMODE = 2; + EPwm5Regs.TBCTL.bit.CTRMODE = 2; + EPwm6Regs.TBCTL.bit.CTRMODE = 2; +#ifdef ML + EPwm7Regs.TBCTL.bit.CTRMODE = 2; + EPwm8Regs.TBCTL.bit.CTRMODE = 2; + EPwm9Regs.TBCTL.bit.CTRMODE = 2; + EPwm10Regs.TBCTL.bit.CTRMODE = 2; + EPwm11Regs.TBCTL.bit.CTRMODE = 2; + EPwm12Regs.TBCTL.bit.CTRMODE = 2; +#endif + + // loop forever + while(1) { + // обмен с ВУ + // ( -> mst) + talk_with_mst(); + // обмен с ПУ + // ( -> param[], eprom.writeRequestNumber) + talk_with_desk(); + // запись в EEPROM + // (param[], eprom.writeRequestNumber -> eprom.writeRequestNumber) + if ( eprom.writeRequestNumber > 0 ) { + write_eeprom(); + } + + // управляем светодиодами на процессорной плате + control_processor_led(); + } //while(1) +} //void main(void) + + + +// Управляет светодиодами на процессорной плате +void control_processor_led(void) { + static unsigned short Tled = (unsigned short)(0.5/TY); + static unsigned short tLed = 0; + + if ( tLed < Tled ) { + tLed++; + } + else { + tLed = 1; + // в аварийном реж. моргаем красным светодиодом + if ( state == STATE_SHUTDOWN ) { + LED_GREEN1_OFF; + LED_GREEN2_OFF; + LED_RED_TOGGLE; + } + // в реж. остановки моргаем первым зелёным светодиодом + else if ( state == STATE_STOP ) { + LED_GREEN1_TOGGLE; + LED_GREEN2_OFF; + LED_RED_OFF; + } + // в рабочем реж. моргаем вторым зелёным светодиодом + else { + LED_GREEN1_OFF; + LED_GREEN2_TOGGLE; + LED_RED_OFF; + } + } +} //void control_processor_led(void) + + + +// Получает параметры с ПУ +// ( -> param[], eprom.writeRequestNumber) +void talk_with_desk(void) { +} + + + +// Получает команды с ВУ +// ( -> mst) +void talk_with_mst(void) { +} + + + +// Записывает параметры в EEPROM +// (param[], eprom.writeRequestNumber -> eprom.writeRequestNumber) +void write_eeprom(void) { +} +#endif //ML diff --git a/Inu/main.h b/Inu/main.h new file mode 100644 index 0000000..f93fa02 --- /dev/null +++ b/Inu/main.h @@ -0,0 +1,17 @@ +#ifndef MAIN +#define MAIN + +// Переменные, которые определены в main.c (begin) +//######################################################################### +struct Eprom eprom; +//######################################################################### +// Переменные, которые определены в main.c (end) + + + +// Переменные, которые объявлены в main.c (begin) +//######################################################################### +extern volatile short state; +//######################################################################### +// Переменные, которые объявлены в main.c (end) +#endif //MAIN diff --git a/Inu/param.c b/Inu/param.c new file mode 100644 index 0000000..4ebbffa --- /dev/null +++ b/Inu/param.c @@ -0,0 +1,426 @@ +/************************************************************************** + Description: Функции для приёма и выдачи параметров. + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.11.08 +**************************************************************************/ + + +#include "def.h" +#include "param.h" + + +#pragma CODE_SECTION(input_param, "ramfuncs"); +#pragma CODE_SECTION(output_param, "ramfuncs"); + + +extern short test_param(void); +extern void process_sgm_parameters(void); + + +// Изменяет значение параметра +void input_param(unsigned short num, unsigned short val) { + switch ( num ) { + case 180://rf.PsiZ, %*10 от PSI_BAZ + if ( (val <= 2000) && (val != param[num]) ) { + param[num] = val; + rf.PsiZ = (float)val*0.001;//%*10 -> o.e. + eprom.writeRequestNumber += 1; + } + break; + case 200://offset.Ia1, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Ia1 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 201://offset.Ib1, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Ib1 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 202://offset.Ic1, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Ic1 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 203://offset.Udc1, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Udc1 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 206://offset.Ia2, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Ia2 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 207://offset.Ib2, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Ib2 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 208://offset.Ic2, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Ic2 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 209://offset.Udc2, ед. АЦП + if ( (val >= 1748) && (val <= 4096) && (val != param[num]) ) { + offset.Udc2 = param[num] = val; + eprom.writeRequestNumber += 1; + } + break; + case 210://cc.Kp, % + if ( (val <= 5000) && (val != param[num]) ) { + param[num] = val; + cc.Kp = (float)val*cc.KpOrig; + eprom.writeRequestNumber += 1; + } + break; + case 211://cc.Ki, % + if ( (val <= 5000) && (val != param[num]) ) { + param[num] = val; + cc.Ki = (float)val*cc.KiOrig; + eprom.writeRequestNumber += 1; + } + break; + case 212://cf.Kp, % + if ( (val <= 5000) && (val != param[num]) ) { + param[num] = val; + cf.Kp = (float)val*cf.KpOrig; + eprom.writeRequestNumber += 1; + } + break; + case 213://cf.Ki, % + if ( (val <= 5000) && (val != param[num]) ) { + param[num] = val; + cf.Ki = (float)val*cf.KiOrig; + eprom.writeRequestNumber += 1; + } + break; + case 214://csp.Kp, % + if ( (val <= 5000) && (val != param[num]) ) { + param[num] = val; + csp.Kp = (float)val*csp.KpOrig; + eprom.writeRequestNumber += 1; + } + break; + case 215://csp.Ki, % + if ( (val <= 5000) && (val != param[num]) ) { + param[num] = val; + csp.Ki = (float)val*csp.KiOrig; + eprom.writeRequestNumber += 1; + } + break; + case 220://protect.IacMax, % от IAC_SENS_MAX + if ( (val <= 99) && (val != param[num]) ) { + param[num] = val; + protect.IacMax = (short)(2047.*(float)val*0.01);//% -> ед. АЦП + protect.IacMin = -protect.IacMax; + eprom.writeRequestNumber += 1; + } + break; + case 221://protect.UdcMax, % от U_NOM + if ( (val <= 136) && (val != param[num]) ) { + param[num] = val; + protect.UdcMax = (float)val*0.01;//% -> o.e. + eprom.writeRequestNumber += 1; + } + break; + case 222://IzLim, % от I_BAZ + if ( (val <= 200) && (val != param[num]) ) { + param[num] = val; + IzLim = (float)val*0.01;//% -> o.e. + eprom.writeRequestNumber += 1; + } + break; + case 223://cf.IdLim, % от I_BAZ + if ( (val <= 200) && (val != param[num]) ) { + param[num] = val; + cf.IdLim = (float)val*0.01;//% -> o.e. + cf.IdLimNeg = cf.IdLim*(-0.4); + eprom.writeRequestNumber += 1; + } + break; + case 224://csp.IqLim, % от I_BAZ + if ( (val <= 200) && (val != param[num]) ) { + param[num] = val; + csp.IqLim = (float)val*0.01;//% -> o.e. + csp.IqLimNeg = -csp.IqLim; + eprom.writeRequestNumber += 1; + } + break; + case 225://protect.UdcMin, % от U_NOM + if ( (val <= 110) && (val != param[num]) ) { + param[num] = val; + protect.UdcMin = (float)val*0.01;//% -> o.e. + eprom.writeRequestNumber += 1; + } + break; + case 226://protect.WmMax, % от N_NOM + if ( (val <= 200) && (val != param[num]) ) { + param[num] = val; + protect.WmMax = (float)val*0.01;//% -> o.e. + eprom.writeRequestNumber += 1; + } + break; + case 228://rf.WmNomPsi, % от N_NOM + if ( (val <= 200) && (val != param[num]) ) { + param[num] = val; + rf.WmNomPsi = (float)val*0.01;//% -> o.e. + eprom.writeRequestNumber += 1; + } + break; + case 229://rf.YlimPsi, % от Y_LIM + if ( (val <= 101) && (val != param[num]) ) { + param[num] = val; + rf.YlimPsi = (float)val*0.01*Y_LIM;//% -> ед. счётчика таймера + eprom.writeRequestNumber += 1; + } + break; + case 231://protect.TudcMin, мс + if ( (val >= 1) && (val <= 8500) && (val != param[num]) ) { + param[num] = val; + protect.TudcMin = (unsigned short)((float)val*0.001/TY); + eprom.writeRequestNumber += 1; + } + break; + case 233://protect.TwmMax, мс + if ( (val >= 1) && (val <= 8500) && (val != param[num]) ) { + param[num] = val; + protect.TwmMax = (unsigned short)((float)val*0.001/TY); + eprom.writeRequestNumber += 1; + } + break; + case 244://rs.WlimIncr, мс + if ( (val >= 1) && (val != param[num]) ) { + param[num] = val; + // изм. на 1.0 за столько-то мс + rs.WlimIncr = 1.0*TY*DECIM_PSI_WM_PM/((float)val*0.001); + eprom.writeRequestNumber += 1; + } + break; + case 245://csp.IlimIncr, мс + if ( (val >= 1) && (val != param[num]) ) { + param[num] = val; + // изм. на I_BAZ за столько-то мс + csp.IlimIncr = 1.0*TY*DECIM_PSI_WM_PM/((float)val*0.001); + eprom.writeRequestNumber += 1; + } + break; + case 248://rp.PlimIncr, мс + if ( (val >= 1) && (val != param[num]) ) { + param[num] = val; + // изм. на 1.0 за столько-то мс + rp.PlimIncr = 1.0*TY*DECIM_PSI_WM_PM/((float)val*0.001); + eprom.writeRequestNumber += 1; + } + break; + case 269://KmeCorr, %*100 + if ( (val >= 5000) && (val <= 20000) && (val != param[num]) ) { + param[num] = val; + KmeCorr = (float)val*0.0001;//%*100 -> o.e. + eprom.writeRequestNumber += 1; + } + break; + case 285://Kudc, мс*10 + if ( (val >= 1) && (val <= 20000) && (val != param[num]) ) { + param[num] = val; + Kudc = (TY*10000.)/(float)val; + if ( Kudc > 1.0 ) + Kudc = 1.0; + eprom.writeRequestNumber += 1; + } + break; + case 286://Kwm, мс*10 + if ( (val >= 1) && (val <= 20000) && (val != param[num]) ) { + param[num] = val; + Kwm = (TY*10000.)/(float)val; + if ( Kwm > 1.0 ) + Kwm = 1.0; + eprom.writeRequestNumber += 1; + } + break; + case 288://rs.Kwmz, мс + if ( (val >= 1) && (val <= 20000) && (val != param[num]) ) { + param[num] = val; + rs.Kwmz = (TY*DECIM_PSI_WM_PM*1000.)/(float)val; + eprom.writeRequestNumber += 1; + } + break; + case 289://rf.Kpsiz, мс + if ( (val >= 1) && (val <= 20000) && (val != param[num]) ) { + param[num] = val; + rf.Kpsiz = (TY*DECIM_PSI_WM_PM*1000.)/(float)val; + eprom.writeRequestNumber += 1; + } + break; + case 290://Kme, мс + if ( (val >= 1) && (val <= 20000) && (val != param[num]) ) { + param[num] = val; + Kme = (TY*1000.)/(float)val; + eprom.writeRequestNumber += 1; + } + break; + case 292://rp.Kpmz, мс + if ( (val >= 1) && (val <= 20000) && (val != param[num]) ) { + param[num] = val; + rp.Kpmz = (TY*DECIM_PSI_WM_PM*1000.)/(float)val; + eprom.writeRequestNumber += 1; + } + break; + case 303://sgmPar.Rs, мкОм + if ( val != param[num] ) { + param[num] = val; + sgmPar.Rs = (float)val*1e-6;//мкОм -> Ом + eprom.writeRequestNumber += 1; + } + break; + case 304://sgmPar.Lls, мкГн*10 + if ( val != param[num] ) { + param[num] = val; + sgmPar.Lls = (float)val*1e-7;//мкГн*10 -> Гн + process_sgm_parameters(); + eprom.writeRequestNumber += 1; + } + break; + case 305://sgmPar.Rr, мкОм + if ( val != param[num] ) { + param[num] = val; + sgmPar.Rr = (float)val*1e-6;//мкОм -> Ом + process_sgm_parameters(); + eprom.writeRequestNumber += 1; + } + break; + case 306://sgmPar.Llr, мкГн*10 + if ( val != param[num] ) { + param[num] = val; + sgmPar.Llr = (float)val*1e-7;//мкГн*10 -> Гн + process_sgm_parameters(); + eprom.writeRequestNumber += 1; + } + break; + case 307://sgmPar.Lm, мкГн + if ( val != param[num] ) { + param[num] = val; + sgmPar.Lm = (float)val*1e-6;//мкГн -> Гн + process_sgm_parameters(); + eprom.writeRequestNumber += 1; + } + break; + default: + if ( num < PAR_NUMBER ) { + param[num] = val; + } + break; + } //switch ( num ) +} //void input_param(unsigned short num, unsigned short val) + + + +// Выдаёт значение параметра +unsigned short output_param(unsigned short num) { + static unsigned short output; + + switch ( num ) { + case 1: //udc1, o.e. -> o.e.*CONTROLLER_GAIN + if ( state == STATE_SHUTDOWN ) { + output = (unsigned short)(emerg.udc1*CONTROLLER_GAIN); + } + else { + output = (unsigned short)(out.udc1*CONTROLLER_GAIN); + } + break; + case 2: //udc2, o.e. -> o.e.*CONTROLLER_GAIN + if ( state == STATE_SHUTDOWN ) { + output = (unsigned short)(emerg.udc2*CONTROLLER_GAIN); + } + else { + output = (unsigned short)(out.udc2*CONTROLLER_GAIN); + } + break; + case 5: //iac1, o.e. -> o.e.*CONTROLLER_GAIN + if ( state == STATE_SHUTDOWN ) { + output = (unsigned short)(emerg.iac1*CONTROLLER_GAIN); + } + else { + output = (unsigned short)(out.iac1*CONTROLLER_GAIN); + } + break; + case 6: //iac2, o.e. -> o.e.*CONTROLLER_GAIN + if ( state == STATE_SHUTDOWN ) { + output = (unsigned short)(emerg.iac2*CONTROLLER_GAIN); + } + else { + output = (unsigned short)(out.iac2*CONTROLLER_GAIN); + } + break; + case 7: //me, o.e. -> (o.e. + CONTROLLER_BIAS)*CONTROLLER_GAIN + if ( state == STATE_SHUTDOWN ) { + if ( emerg.me > CONTROLLER_BIAS ) + output = (unsigned short)((CONTROLLER_BIAS + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else if ( emerg.me > -CONTROLLER_BIAS ) + output = (unsigned short)((emerg.me + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else + output = 0; + } + else { + if ( out.me > CONTROLLER_BIAS ) + output = (unsigned short)((CONTROLLER_BIAS + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else if ( out.me > -CONTROLLER_BIAS ) + output = (unsigned short)((out.me + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else + output = 0; + } + break; + case 8: //nm, o.e. -> (o.e. + CONTROLLER_BIAS)*CONTROLLER_GAIN + if ( state == STATE_SHUTDOWN ) { + if ( emerg.wm > CONTROLLER_BIAS ) + output = (unsigned short)((CONTROLLER_BIAS + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else if ( emerg.wm > -CONTROLLER_BIAS ) + output = (unsigned short)((emerg.wm + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else + output = 0; + } + else { + if ( out.wm > CONTROLLER_BIAS ) + output = (unsigned short)((CONTROLLER_BIAS + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else if ( out.wm > -CONTROLLER_BIAS ) + output = (unsigned short)((out.wm + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else + output = 0; + } + break; + case 9: //pm, o.e. -> (o.e. + CONTROLLER_BIAS)*CONTROLLER_GAIN + if ( state == STATE_SHUTDOWN ) { + if ( emerg.pm > CONTROLLER_BIAS ) + output = (unsigned short)((CONTROLLER_BIAS + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else if ( emerg.pm > -CONTROLLER_BIAS ) + output = (unsigned short)((emerg.pm + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else + output = 0; + } + else { + if ( out.pm > CONTROLLER_BIAS ) + output = (unsigned short)((CONTROLLER_BIAS + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else if ( out.pm > -CONTROLLER_BIAS ) + output = (unsigned short)((out.pm + CONTROLLER_BIAS)*CONTROLLER_GAIN); + else + output = 0; + } + break; + case 10: //compound + output = faultNo + (inuWork<<7); + break; + default: + output = param[num]; + break; + } //switch ( num ) + return output; +} //unsigned short output_param(unsigned short num) diff --git a/Inu/param.h b/Inu/param.h new file mode 100644 index 0000000..9dc480e --- /dev/null +++ b/Inu/param.h @@ -0,0 +1,41 @@ +#ifndef PARAM +#define PARAM + +// Переменные, которые определены в param.c (begin) +//######################################################################### +unsigned short param[PAR_NUMBER]; +//######################################################################### +// Переменные, которые определены в param.c (end) + + + + +// Переменные, которые объявлены в param.c (begin) +//######################################################################### +extern volatile short state; +extern volatile short faultNo; +extern short onceFaultReset; +extern struct SgmPar sgmPar; +extern struct Offset offset; +extern float Kudc; +extern float Kwm; +extern short testParamFaultNo; +extern float IzLim; +extern float Kme; +extern struct Protect protect; +extern float KmeCorr; +extern volatile struct Out out; +extern volatile struct Emerg emerg; +extern struct Rf rf; +extern struct Rs rs; +extern struct Rp rp; +extern struct Cf cf; +extern struct Csp csp; +extern struct Cc cc; +// для передачи на ВУ +extern volatile short inuWork; +// для работы с EEPROM +extern struct Eprom eprom; +//######################################################################### +// Переменные, которые объявлены в param.c (end) +#endif //PARAM diff --git a/Inu/upr.c b/Inu/upr.c new file mode 100644 index 0000000..6d48221 --- /dev/null +++ b/Inu/upr.c @@ -0,0 +1,974 @@ +/************************************************************************** + Description: Функция реализует алгоритм управления INU + (отработка N и P). + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.11.08 +**************************************************************************/ + + +#include "def.h" +#include "upr.h" +#include "pwm_vector_regul.h" +#include "IQmathLib.h" +#include "adc_tools.h" +#include "params.h" +#include "vector.h" +#include "v_pwm24.h" +#include "xp_write_xpwm_time.h" +#include "rotation_speed.h" + + +#pragma CODE_SECTION(control_current, "ramfuncs"); +#pragma CODE_SECTION(control_flux, "ramfuncs"); +#pragma CODE_SECTION(control_speed_power, "ramfuncs"); +#pragma CODE_SECTION(indirect_vector_control, "ramfuncs"); +#pragma CODE_SECTION(ipark, "ramfuncs"); +#pragma CODE_SECTION(limit_current, "ramfuncs2"); +#pragma CODE_SECTION(pwm, "ramfuncs2"); +#pragma CODE_SECTION(reference_flux, "ramfuncs"); +#pragma CODE_SECTION(reference_power, "ramfuncs"); +#pragma CODE_SECTION(reference_speed, "ramfuncs"); +#pragma CODE_SECTION(select_feedback, "ramfuncs"); +#pragma CODE_SECTION(upr, "ramfuncs"); + + +void control_current(void); +void control_flux(void); +void control_speed_power(void); +void indirect_vector_control(void); +void ipark(void); +void limit_current(void); +void pwm(void); +void reference_flux(void); +void reference_power(void); +void reference_speed(void); +void select_feedback(void); + +void write_swgen_pwm_times_split_eages(void); +void write_CMP_tims(void); + +int reset = 1; +extern double wm_ml; + +void upr(void) { + static short decim_psi_wm_pm; + static int calcPWMtimes = 0; + + if ( onceUpr == 0 ) { + onceUpr = 1; + decim_psi_wm_pm = (short)DECIM_PSI_WM_PM; + psi = 0; + rf.once = 0; + rs.once = 0; + rp.once = 0; + cf.once = 0; + csp.once = 0; + ivc.once = 0; + cc.once = 0; + reset = 1; + init_DQ_pid(); + InitVariablesSvgen(FREQ_PWM); + pwm_vector_model_titov(0, 0, 0, 0, 0, 1, calcPWMtimes); + analog.tetta = 0; + } else { + reset = 0; + } + + // внешние контуры регулирования (по потоку, оборотам и мощности) + // не должны быть такими же быстрыми как внутренние (по току) (?) + if ( decim_psi_wm_pm < (short)DECIM_PSI_WM_PM ) { + decim_psi_wm_pm++; + calcPWMtimes = 0; + } + else { + decim_psi_wm_pm = 1; + calcPWMtimes = 1; + + // заданное потокосцепление + // (rf.PsiZ -> rf.psiZ, rf.pPsiZ) + reference_flux(); + + // заданная скорость + // (mst.wmZz, mst.wmLim -> rs.wmZ, rs.pWmZ) + reference_speed(); + + // заданная мощность + // (mst.pmZz, mst.pmLim -> rp.pmZ) + reference_power(); + + // заданный ток (idZ, iqZ) + // ... регулятор потокосцепления + // (rf.psiZ, psi, rf.pPsiZ, cf.IdLim, cf.IdLimNeg -> idZ) + control_flux(); + // ... регуляторы скорости и мощности + // (rs.wmZ, wm, rs.pWmZ, rp.pmZ, mst.wmLim, mst.pmLim, csp.IqLim, + // csp.IqLimNeg -> iqZ, inuWork) + control_speed_power(); + // ... ограничение полного тока + // (idZ, iqZ, IzLim -> idZ, iqZ, csp.iqLimFlag) + limit_current(); + } //decim_psi_wm_pm + + + if ( mst.start == 1 ) { + inuWork = 1; + } else { + inuWork = 0; + } + + + + _iq Pzad = _IQ(rp.pmZ * 1.1); + // _iq Pzad = _IQ(1.1); + _iq Fzad = _IQ(rs.wmZ / (PI2 * 1.1574233675198942802148869545233)); + // _iq Frot = _IQ(10.0 / 6.0 / NORMA_FROTOR); + _iq Frot = _IQ(wm_ml / PI2 / NORMA_FROTOR); + int direction = Frot >= 0 ? 1 : -1; + rotor.iqFout = Frot; + int mode = 2; + // int reset = 0; + f.Go = 1; + if (mode != 0) { + limit_mzz_zad_power(Frot); + // set_cos_tetta_calc_params(); + pwm_vector_model_titov(Pzad, Fzad, direction, labs(Frot), mode, 0, calcPWMtimes); + } else { + // U/f=Const + #define K_MODUL_MAX 15435038LL + vect_control.iqUqKm1 = _IQ(0.6); + vect_control.iqUqKm2 = _IQ(0.6); + _iq Fconst = IQ_F_STATOR_NOM / 10; + test_calc_simple_dq_pwm24(vect_control.iqUqKm1, vect_control.iqUqKm2, Fconst, Fconst, K_MODUL_MAX); + } + + + reset = 0; + float theta = _IQtoF(analog.tetta); + sincos(theta, &sinTheta, &cosTheta); + + if (calcPWMtimes) + write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_HIGH); + else + write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_LOW); + write_CMP_tims(calcPWMtimes); + // cc.yd1 = _IQtoF(vect_control.iqUdKm1Out) * Y_LIM; + // cc.yq1 = _IQtoF(vect_control.iqUqKm1Out) * Y_LIM; + // cc.yd2 = _IQtoF(vect_control.iqUdKm2Out) * Y_LIM; + // cc.yq2 = _IQtoF(vect_control.iqUqKm2Out) * Y_LIM; + + + + // indirect vector control + // (wmNf, wm, ix1, iy1, ix2, iy2 -> ivc.ws, ivc.sinTheta, ivc.cosTheta, + // ivc.id1, ivc.iq1, ivc.id2, ivc.iq2, ivc.psi) + // indirect_vector_control(); + + + // выбор сигналов о.с. + // (... -> ws, sinTheta, cosTheta, id1, iq1, id2, iq2, psi) + // select_feedback(); + + +// idZ = 0.1; +// iqZ = 0.2; + // регуляторы тока + // (idZ, iqZ, id1, iq1, id2, iq2, ws, wm, psi -> + // -> cc.yd1, cc.yq1, cc.yd2, cc.yq2) + // control_current(); + + + // перевод сигналов управления из с.к. d-q в с.к. x-y + // (cc.yd1, cc.yq1, cc.yd2, cc.yq2, sinTheta, cosTheta, ws -> + // -> ip.yx1, ip.yy1, ip.yx2, ip.yy2) + // ipark(); + + // ШИМ + // (ip.yx1, ip.yy1, ip.yx2, ip.yy2 -> + // -> EPwm1Regs.CMPA.half.CMPA, EPwm2Regs.CMPA.half.CMPA, + // EPwm3Regs.CMPA.half.CMPA, EPwm4Regs.CMPA.half.CMPA, + // EPwm5Regs.CMPA.half.CMPA, EPwm6Regs.CMPA.half.CMPA, + // EPwm7Regs.CMPA.half.CMPA, EPwm8Regs.CMPA.half.CMPA, + // EPwm9Regs.CMPA.half.CMPA, EPwm10Regs.CMPA.half.CMPA, + // EPwm11Regs.CMPA.half.CMPA, EPwm12Regs.CMPA.half.CMPA) + // pwm(); +} //void upr(void) + + + +// Регулирует ток +// (idZ, iqZ, id1, iq1, id2, iq2, ws, wm, psi -> +// -> cc.yd1, cc.yq1, cc.yd2, cc.yq2) +void control_current(void) { + if ( cc.once == 0 ) { + cc.once = 1; + cc.y1LimFlag = 0; + cc.yd1I = 0; + cc.yq1I = 0; + cc.yd1 = 0; + cc.yq1 = 0; + cc.y2LimFlag = 0; + cc.yd2I = 0; + cc.yq2I = 0; + cc.yd2 = 0; + cc.yq2 = 0; + // для сокращения вычислений + cc.K1 = sgmPar.SigmaLs*I_BAZ*WE_BAZ*U_2_Y; + cc.K2 = sgmPar.Rr*sgmPar.Lm/(sgmPar.Lr*sgmPar.Lr)*PSI_BAZ*U_2_Y; + cc.K3 = sgmPar.Kl*PSI_BAZ*WE_BAZ*U_2_Y; + } + + // для сокращения вычислений + cc.Xyff = ws*cc.K1; + cc.yffAux2 = psi*cc.K2; + cc.yffAux3 = psi*wm*cc.K3; + + // регулятор Id1 + cc.del = idZ - id1; + cc.yd1P = cc.del*cc.Kp; + if ( (cc.y1LimFlag == 0) || (cc.yd1*cc.del < 0) ) + cc.yd1I += cc.del*cc.Ki; + cc.yd1FF = -iq1*cc.Xyff - cc.yffAux2; + cc.yd1 = cc.yd1P + cc.yd1I + cc.yd1FF; + // регулятор Iq1 + cc.del = iqZ - iq1; + cc.yq1P = cc.del*cc.Kp; + if ( (cc.y1LimFlag == 0) || (cc.yq1*cc.del < 0) ) + cc.yq1I += cc.del*cc.Ki; + cc.yq1FF = id1*cc.Xyff + cc.yffAux3; + cc.yq1 = cc.yq1P + cc.yq1I + cc.yq1FF; + // ограничение + cc.y1 = sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1); + if ( cc.y1 > Y_LIM ) { + cc.kYlim = Y_LIM/cc.y1; + cc.yd1 *= cc.kYlim; + cc.yq1 *= cc.kYlim; + cc.y1LimFlag = 1; + } + else { + cc.y1LimFlag = 0; + } + + // регулятор Id2 + cc.del = idZ - id2; + cc.yd2P = cc.del*cc.Kp; + if ( (cc.y2LimFlag == 0) || (cc.yd2*cc.del < 0) ) + cc.yd2I += cc.del*cc.Ki; + cc.yd2FF = -iq2*cc.Xyff - cc.yffAux2; + cc.yd2 = cc.yd2P + cc.yd2I + cc.yd2FF; + // регулятор Iq2 + cc.del = iqZ - iq2; + cc.yq2P = cc.del*cc.Kp; + if ( (cc.y2LimFlag == 0) || (cc.yq2*cc.del < 0) ) + cc.yq2I += cc.del*cc.Ki; + cc.yq2FF = id2*cc.Xyff + cc.yffAux3; + cc.yq2 = cc.yq2P + cc.yq2I + cc.yq2FF; + // ограничение + cc.y2 = sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2); + if ( cc.y2 > Y_LIM ) { + cc.kYlim = Y_LIM/cc.y2; + cc.yd2 *= cc.kYlim; + cc.yq2 *= cc.kYlim; + cc.y2LimFlag = 1; + } + else { + cc.y2LimFlag = 0; + } +} //void control_current(void) + + + +// Регулирует потокосцепление +// (rf.psiZ, psi, rf.pPsiZ, cf.IdLim, cf.IdLimNeg -> idZ) +void control_flux(void) { + if ( cf.once == 0 ) { + cf.once = 1; + cf.idLimFlag = 0; + cf.idI = 0; + idZ = 0; + } + + // регулятор Psi + cf.del = rf.psiZ - psi; + cf.idP = cf.del*cf.Kp; + if ( (cf.idLimFlag == 0) || (idZ*cf.del < 0) ) + cf.idI += cf.del*cf.Ki; + cf.idFF = (rf.psiZ + rf.pPsiZ*sgmPar.Tr)*sgmPar.LmInv*PSI_BAZ/I_BAZ; + idZ = cf.idP + cf.idI + cf.idFF; + // ограничение потокообразующего тока + if ( idZ > cf.IdLim ) { + idZ = cf.IdLim; + cf.idLimFlag = 1; + } + else if ( idZ < cf.IdLimNeg ) { + idZ = cf.IdLimNeg; + cf.idLimFlag = 1; + } + else { + cf.idLimFlag = 0; + } +} //void control_flux(void) + + + +// Регулирует скорость или мощность +// (rs.wmZ, wm, rs.pWmZ, rp.pmZ, mst.wmLim, mst.pmLim, csp.IqLim, +// csp.IqLimNeg -> iqZ, inuWork) +void control_speed_power(void) { + if ( csp.once == 0 ) { + csp.once = 1; + csp.wmLimZi = mst.wmLim; + csp.pmLimZi = mst.pmLim; + csp.iqLimFlag = 0; + csp.iqI = 0; + iqZ = 0; + csp.iqLimZi = csp.IqLim; + csp.iqLim = csp.IqLim; + csp.pmZiRampDown = 0; + } + + // для ограничения скорости + if ( mst.wmLim - csp.wmLimZi > rs.WlimIncr ) { + csp.wmLimZi += rs.WlimIncr; + } + else if ( csp.wmLimZi - mst.wmLim > rs.WlimIncr ) { + csp.wmLimZi -= rs.WlimIncr; + } + else { + csp.wmLimZi = mst.wmLim; + } + // для ограничения мощности + if ( mst.pmLim - csp.pmLimZi > rp.PlimIncr ) { + csp.pmLimZi += rp.PlimIncr; + } + else if ( csp.pmLimZi - mst.pmLim > rp.PlimIncr ) { + csp.pmLimZi -= rp.PlimIncr; + } + else { + csp.pmLimZi = mst.pmLim; + } + + if ( inuWork == 0 ) { + if ( mst.start == 1 ) { + // ГЭД намагничен, можно переходить к отработке N или P + //if ( (rf.psiZ > rf.PsiZ*0.97) && (psi > rf.psiZ*0.97) ) + inuWork = 1; + } + else { + // всё выключаем + inuWork = 2; + } + // чтобы стартануть без бросков тока + rs.wmZi = rs.wmZ = wm; + rp.pmZi = rp.pmZ = 0; + iqZ = 0; + } + else if ( inuWork == 1 ) { + if ( mst.start == 1 ) { + // регулятор N -------------- + if ( mst.pzMode == 0 ) { + csp.del = rs.wmZ - wm; + csp.iqP = csp.del*csp.Kp; + if ( (csp.iqLimFlag == 0) || (iqZ*csp.del < 0) ) + csp.iqI += csp.del*csp.Ki; + csp.iqFF = rs.pWmZ/kMe*(WM_BAZ*J/M_BAZ); + iqZ = csp.iqP + csp.iqI + csp.iqFF; + // ограничение тока для ограничения мощности + if ( wmAbs > WM_MIN ) { + csp.iqLimAux = csp.pmLimZi/(wmAbs*kMe); + } + else { + csp.iqLimAux = csp.pmLimZi/(WM_MIN*kMe); + } + if ( csp.iqLimAux < csp.IqLim ) { + csp.iqLim = csp.iqLimAux; + } + else { + csp.iqLim = csp.IqLim; + } + } + // регулятор P -------------- + else { //if ( mst.pzMode == 1 ) + if ( wmAbs <= WM_MIN ) { + iqZ = rp.pmZ/(WM_MIN*kMe); + } + else if ( wmAbs <= rf.WmNomPsi ) { + iqZ = rp.pmZ/(wmAbs*kMe); + csp.kMeNom = kMe; + } + else { + iqZ = rp.pmZ/(wmAbs*csp.kMeNom); + } + // ограничение тока для ограничения оборотов + if ( wmAbs < csp.wmLimZi*0.98 ) { + csp.iqLimAux = fabs(iqZ); + } + else if ( wmAbs > csp.wmLimZi*1.02 ) { + csp.iqLimAux = 0; + } + else { + csp.iqLimAux = csp.iqLimZi; + } + // ... меняем скорость изменения ограничения тока (?) + csp.delWmAbs = fabs(wmAbs - csp.wmLimZi); + if ( csp.delWmAbs > 0.12 ) + csp.KizIncr = 10.0; + else if ( csp.delWmAbs < 0.02 ) + csp.KizIncr = 0.1; + else + csp.KizIncr = 0.1 + (csp.delWmAbs - 0.02)*(10.0 - 0.1)/(0.12 - 0.02); + // ... ЗИ + if ( csp.iqLimAux - csp.iqLimZi > csp.IlimIncr*csp.KizIncr ) + csp.iqLimZi += csp.IlimIncr*csp.KizIncr; + else if ( csp.iqLimZi - csp.iqLimAux > csp.IlimIncr*csp.KizIncr ) + csp.iqLimZi -= csp.IlimIncr*csp.KizIncr; + else + csp.iqLimZi = csp.iqLimAux; + if ( csp.iqLimZi < csp.IqLim ) { + csp.iqLim = csp.iqLimZi; + } + else { + csp.iqLim = csp.IqLim; + } + } //mst.pzMode + // для плавной остановки + csp.pmZiRampDown = rp.pmEqv; + } + else { //if ( mst.start == 0 ) + // снижаем заданную мощность + if ( 0 - csp.pmZiRampDown > mst.pDecrMaxTy ) { + csp.pmZiRampDown += mst.pDecrMaxTy; + } + else if ( csp.pmZiRampDown - 0 > mst.pDecrMaxTy ) { + csp.pmZiRampDown -= mst.pDecrMaxTy; + } + else { + csp.pmZiRampDown = 0; + // ток снижен - завершаем работу + inuWork = 2; + } + // формируем заданный ток + if ( wmAbs > WM_MIN ) { + iqZ = csp.pmZiRampDown/(wmAbs*kMe); + } + else { + iqZ = csp.pmZiRampDown/(WM_MIN*kMe); + } + // на случай, если mst.start восстановится раньше + // завершения работы (inuWork = 2) + rs.wmZi = rs.wmZ = wm; + csp.iqI = iqZ; + rp.pmZi = rp.pmZ = rp.pmEqv; + } //mst.start + + // уставка ограничения снизу + if ( -csp.iqLim > csp.IqLimNeg ) + csp.iqLimNeg = -csp.iqLim; + else + csp.iqLimNeg = csp.IqLimNeg; + // ограничение моментообразующего тока + if ( iqZ > csp.iqLim ) { + iqZ = csp.iqLim; + csp.iqLimFlag = 1; + } + else if ( iqZ < csp.iqLimNeg ) { + iqZ = csp.iqLimNeg; + csp.iqLimFlag = 1; + } + else { + csp.iqLimFlag = 0; + } + // для плавного перехода + if ( mst.pzMode == 0 ) { + // ... в режим регулирования P + rp.pmZ = iqZ*kMe*wmAbs; + rp.pmZi = rp.pmZ; + csp.iqLimZi = fabs(iqZ); + } + else { + // ... в режим регулирования N + csp.iqI = iqZ; + csp.iqFF = 0; + rs.wmZ = wm; + rs.wmZi = rs.wmZ + csp.iqFF*0.05; + } + } //inuWork +} //void control_speed_power(void) + + + +// Расчёты для indirect vector control +// (wmNf, wm, ix1, iy1, ix2, iy2 -> ivc.ws, ivc.sinTheta, ivc.cosTheta, +// ivc.id1, ivc.iq1, ivc.id2, ivc.iq2, ivc.psi) +void indirect_vector_control(void) { + static float theta; + + if ( ivc.once == 0 ) { + ivc.once = 1; + ivc.im = 0; + ivc.iq1 = 0; + ivc.iq2 = 0; + ivc.wr = 0; + theta = 0; + } + + // частота роторной ЭДС, o.e. + if ( ivc.im > 4e-3 ) { + ivc.wr = (ivc.iq1 + ivc.iq2)*0.5/ivc.im*sgmPar.TrInv*(1.0/WE_BAZ); + } + // частота поля статора, о.е. + ivc.wsNf = wmNf + ivc.wr; + ivc.ws = wm + ivc.wr; + // электрический угол, рад. + theta += ivc.wsNf*WE_BAZ*TY; + if ( theta > PI2 ) + theta -= PI2; + else if ( theta < 0 ) + theta += PI2; + + vect_control.theta = theta; + // для координатных преобразований + sincos(theta, &ivc.sinTheta, &ivc.cosTheta); + // park transformations, о.е. + ivc.id1 = ix1*ivc.cosTheta + iy1*ivc.sinTheta; + ivc.iq1 = -ix1*ivc.sinTheta + iy1*ivc.cosTheta; + ivc.id2 = ix2*ivc.cosTheta + iy2*ivc.sinTheta; + ivc.iq2 = -ix2*ivc.sinTheta + iy2*ivc.cosTheta; + // ток намагничивания, o.e. + ivc.im += ((ivc.id1 + ivc.id2)*0.5 - ivc.im)*sgmPar.TrInv*TY; + // амплитуда потока, o.e. + ivc.psi = ivc.im*sgmPar.Lm*(1.0/L_BAZ); +} //void indirect_vector_control(void) + + + +// Переводит сигналы управления из с.к. d-q в с.к. x-y +// (cc.yd1, cc.yq1, cc.yd2, cc.yq2, sinTheta, cosTheta, ws -> +// -> ip.yx1, ip.yy1, ip.yx2, ip.yy2) +void ipark(void) { + ip.yx1Aux = cc.yd1*cosTheta - cc.yq1*sinTheta; + ip.yy1Aux = cc.yd1*sinTheta + cc.yq1*cosTheta; + ip.yx2Aux = cc.yd2*cosTheta - cc.yq2*sinTheta; + ip.yy2Aux = cc.yd2*sinTheta + cc.yq2*cosTheta; + // коррекция, связанная с дискретностью СУ + ip.theta = ws*WE_BAZ*TY*1.5;//рад. + sincos(ip.theta, &ip.sinTheta, &ip.cosTheta); + ip.yx1 = ip.yx1Aux*ip.cosTheta - ip.yy1Aux*ip.sinTheta; + ip.yy1 = ip.yx1Aux*ip.sinTheta + ip.yy1Aux*ip.cosTheta; + ip.yx2 = ip.yx2Aux*ip.cosTheta - ip.yy2Aux*ip.sinTheta; + ip.yy2 = ip.yx2Aux*ip.sinTheta + ip.yy2Aux*ip.cosTheta; +} //void ipark(void) + + + +// Ограничивает полный ток +// (idZ, iqZ, IzLim -> idZ, iqZ, csp.iqLimFlag) +void limit_current(void) { + iZ = sqrt(idZ*idZ + iqZ*iqZ); + if ( iZ > IzLim ) { + if ( iqZ >= 0 ) { + iqZ = sqrt(IzLim*IzLim - idZ*idZ); + } + else { + iqZ = -sqrt(IzLim*IzLim - idZ*idZ); + } + csp.iqLimFlag = 1; + } +} //void limit_current(void) + + + +// ШИМ +// (ip.yx1, ip.yy1, ip.yx2, ip.yy2 -> +// -> EPwm1Regs.CMPA.half.CMPA, EPwm2Regs.CMPA.half.CMPA, +// EPwm3Regs.CMPA.half.CMPA, EPwm4Regs.CMPA.half.CMPA, +// EPwm5Regs.CMPA.half.CMPA, EPwm6Regs.CMPA.half.CMPA, +// EPwm7Regs.CMPA.half.CMPA, EPwm8Regs.CMPA.half.CMPA, +// EPwm9Regs.CMPA.half.CMPA, EPwm10Regs.CMPA.half.CMPA, +// EPwm11Regs.CMPA.half.CMPA, EPwm12Regs.CMPA.half.CMPA) +void pwm(void) { + static float yAux1; + static float yAux2; + static float ya; + static float yb; + static float yc; + static float yPredm = 0; + static float yaPredm; + static float ybPredm; + static float ycPredm; + + // переводим из с.к. x-y в с.к. a-b-c + yAux1 = ip.yx1*(-0.5*ISQRT3); + yAux2 = ip.yy1*0.5; + ya = ip.yx1*ISQRT3; + yb = yAux1 + yAux2; + yc = yAux1 - yAux2; + // предмодулирующее воздействие + if ((ya >= yb) && (ya <= yc)) { + yPredm = ya*0.5; + } + else if ((yc >= yb) && (yc <= ya)) { + yPredm = yc*0.5; + } + else if ((yb >= yc) && (yb <= ya)) { + yPredm = yb*0.5; + } + else if ((ya >= yc) && (ya <= yb)) { + yPredm = ya*0.5; + } + else if ((yc >= ya) && (yc <= yb)) { + yPredm = yc*0.5; + } + else if ((yb >= ya) && (yb <= yc)) { + yPredm = yb*0.5; + } + yaPredm = (ya + yPredm)*2.; + ybPredm = (yb + yPredm)*2.; + ycPredm = (yc + yPredm)*2.; + // full compare unit compare registers + if (yaPredm >= 0) { + EPwm1Regs.CMPA.half.CMPA = (unsigned short)yaPredm; + EPwm2Regs.CMPA.half.CMPA = 0; + } + else { + EPwm1Regs.CMPA.half.CMPA = 0; + EPwm2Regs.CMPA.half.CMPA = (unsigned short)(-yaPredm); + } + if (ybPredm >= 0) { + EPwm3Regs.CMPA.half.CMPA = (unsigned short)ybPredm; + EPwm4Regs.CMPA.half.CMPA = 0; + } + else { + EPwm3Regs.CMPA.half.CMPA = 0; + EPwm4Regs.CMPA.half.CMPA = (unsigned short)(-ybPredm); + } + if (ycPredm >= 0) { + EPwm5Regs.CMPA.half.CMPA = (unsigned short)ycPredm; + EPwm6Regs.CMPA.half.CMPA = 0; + } + else { + EPwm5Regs.CMPA.half.CMPA = 0; + EPwm6Regs.CMPA.half.CMPA = (unsigned short)(-ycPredm); + } + + // переводим из с.к. x-y в с.к. a-b-c +#ifndef SHIFT + yAux1 = ip.yx2*(-0.5*ISQRT3); + yAux2 = ip.yy2*0.5; + ya = ip.yx2*ISQRT3; + yb = yAux1 + yAux2; + yc = yAux1 - yAux2; +#else //SHIFT + yAux1 = ip.yx2*0.5; + yAux2 = ip.yy2*0.5*ISQRT3; + ya = yAux1 + yAux2; + yb = -yAux1 + yAux2; + yc = ip.yy2*(-ISQRT3); +#endif //SHIFT + // предмодулирующее воздействие + if ((ya >= yb) && (ya <= yc)) { + yPredm = ya*0.5; + } + else if ((yc >= yb) && (yc <= ya)) { + yPredm = yc*0.5; + } + else if ((yb >= yc) && (yb <= ya)) { + yPredm = yb*0.5; + } + else if ((ya >= yc) && (ya <= yb)) { + yPredm = ya*0.5; + } + else if ((yc >= ya) && (yc <= yb)) { + yPredm = yc*0.5; + } + else if ((yb >= ya) && (yb <= yc)) { + yPredm = yb*0.5; + } + yaPredm = (ya + yPredm)*2.; + ybPredm = (yb + yPredm)*2.; + ycPredm = (yc + yPredm)*2.; +#ifdef ML + // full compare unit compare registers + if (yaPredm >= 0) { + EPwm7Regs.CMPA.half.CMPA = (unsigned short)yaPredm; + EPwm8Regs.CMPA.half.CMPA = 0; + } + else { + EPwm7Regs.CMPA.half.CMPA = 0; + EPwm8Regs.CMPA.half.CMPA = (unsigned short)(-yaPredm); + } + if (ybPredm >= 0) { + EPwm9Regs.CMPA.half.CMPA = (unsigned short)ybPredm; + EPwm10Regs.CMPA.half.CMPA = 0; + } + else { + EPwm9Regs.CMPA.half.CMPA = 0; + EPwm10Regs.CMPA.half.CMPA = (unsigned short)(-ybPredm); + } + if (ycPredm >= 0) { + EPwm11Regs.CMPA.half.CMPA = (unsigned short)ycPredm; + EPwm12Regs.CMPA.half.CMPA = 0; + } + else { + EPwm11Regs.CMPA.half.CMPA = 0; + EPwm12Regs.CMPA.half.CMPA = (unsigned short)(-ycPredm); + } +#endif //ML + + // разрешаем импульсы + EALLOW; + EPwm1Regs.TZCLR.all = 0x0004; + EPwm2Regs.TZCLR.all = 0x0004; + EPwm3Regs.TZCLR.all = 0x0004; + EPwm4Regs.TZCLR.all = 0x0004; + EPwm5Regs.TZCLR.all = 0x0004; + EPwm6Regs.TZCLR.all = 0x0004; +#ifdef ML + EPwm7Regs.TZCLR.all = 0x0004; + EPwm8Regs.TZCLR.all = 0x0004; + EPwm9Regs.TZCLR.all = 0x0004; + EPwm10Regs.TZCLR.all = 0x0004; + EPwm11Regs.TZCLR.all = 0x0004; + EPwm12Regs.TZCLR.all = 0x0004; +#endif //ML + EDIS; +} //void pwm(void) + + + +// Формирует заданный поток +// (rf.PsiZ -> rf.psiZ, rf.pPsiZ) +void reference_flux(void) { + if ( rf.once == 0 ) { + rf.once = 1; + rf.KpsiSub = TY*DECIM_PSI_WM_PM/6.0; + rf.psiZi = 0; + cc.y1 = 0; + cc.y2 = 0; + rf.psiSub = 0; + rf.psiZ = 0; + rf.psiZPrev1 = 0; + rf.psiZPrev2 = 0; + rf.psiZPrev3 = 0; + } + + // ЗИ + if ( rf.PsiZ - rf.psiZi > rf.PsizIncr ) { + rf.psiZi += rf.PsizIncr; + } + else if ( rf.psiZi - rf.PsiZ > rf.PsizIncr ) { + rf.psiZi -= rf.PsizIncr; + } + else { + rf.psiZi = rf.PsiZ; + } + // коррекция в соответствии со скоростью + if ( wmAbs <= rf.WmNomPsi ) + rf.psiZCorr = rf.psiZi; + else + rf.psiZCorr = rf.psiZi*rf.WmNomPsi/wmAbs; + // коррекция в соответствии с противоЭДС + if ( (cc.y1 > rf.YlimPsi) || (cc.y2 > rf.YlimPsi) ) { + rf.psiSub += (rf.psiZCorr - rf.psiSub)*rf.KpsiSub; + } + else { + rf.psiSub += (0 - rf.psiSub)*rf.KpsiSub; + } + rf.psiZCorr2 = rf.psiZCorr - rf.psiSub; + // чтобы задание менялось чуть плавнее + rf.psiZ += (rf.psiZCorr2 - rf.psiZ)*rf.Kpsiz; + + // производная заданного потокосцепления + rf.pPsiZ = (rf.psiZ - rf.psiZPrev3)/(TY*DECIM_PSI_WM_PM*3.); + rf.psiZPrev3 = rf.psiZPrev2; + rf.psiZPrev2 = rf.psiZPrev1; + rf.psiZPrev1 = rf.psiZ; +} //void reference_flux(void) + + + +// Формирует заданную мощность +// (mst.pmZz, mst.pmLim -> rp.pmZ) +void reference_power(void) { + if ( rp.once == 0 ) { + rp.once = 1; + rp.pmZi = 0; + rp.pmZ = 0; + } + + // ограничение + if ( fabs(mst.pmZz) > mst.pmLim ) { + if ( mst.pmZz >= 0 ) + rp.pmZz = mst.pmLim; + else + rp.pmZz = -mst.pmLim; + } + else { + rp.pmZz = mst.pmZz; + } + // для ограничения приращения мощности (?) + if ( fabs(rp.pmZi - rp.pmEqv) > 0.02 ) + rp.KpIncrDecr = 0.10; + else + rp.KpIncrDecr = 1.00; + // ЗИ + if ( rp.pmZz - rp.pmZi > mst.pIncrMaxTy*rp.KpIncrDecr ) { + rp.pmZi += mst.pIncrMaxTy*rp.KpIncrDecr; + } + else if ( rp.pmZi - rp.pmZz > mst.pDecrMaxTy*rp.KpIncrDecr ) { + rp.pmZi -= mst.pDecrMaxTy*rp.KpIncrDecr; + } + else { + rp.pmZi = rp.pmZz; + } + // чтобы задание менялось чуть плавнее + // rp.pmZ += (rp.pmZi - rp.pmZ)*rp.Kpmz; + rp.pmZ = rp.pmZz; +} //void reference_power(void) + + + +// Формирует заданную скорость +// (mst.wmZz, mst.wmLim -> rs.wmZ, rs.pWmZ) +void reference_speed(void) { + if ( rs.once == 0 ) { + rs.once = 1; + rs.wmZi = rs.wmZ = wm; + rs.wzIncr = rs.WlimIncr; + rs.wmZPrev1 = rs.wmZ; + rs.wmZPrev2 = rs.wmZ; + rs.wmZPrev3 = rs.wmZ; + rs.tPwmZ = 0; + } + + // ограничение + if ( fabs(mst.wmZz) > mst.wmLim ) { + if ( mst.wmZz >= 0 ) + rs.wmZz = mst.wmLim; + else + rs.wmZz = -mst.wmLim; + } + else { + rs.wmZz = mst.wmZz; + } + // для ограничения приращения мощности (?) + if ( fabs(rs.wmZi) < 0.5 ) + rs.wzIncrNf = rs.WlimIncr*3.5; + else if ( fabs(rs.wmZi) < 0.8 ) + rs.wzIncrNf = rs.WlimIncr*2.0; + else + rs.wzIncrNf = rs.WlimIncr; + rs.wzIncr += (rs.wzIncrNf - rs.wzIncr)*(TY*DECIM_PSI_WM_PM)/0.25; + // ЗИ + if ( rs.wmZz - rs.wmZi > rs.wzIncr ) { + rs.wmZi += rs.wzIncr; + } + else if ( rs.wmZi - rs.wmZz > rs.wzIncr ) { + rs.wmZi -= rs.wzIncr; + } + else { + rs.wmZi = rs.wmZz; + } + // чтобы задание менялось чуть плавнее + // rs.wmZ += (rs.wmZi - rs.wmZ)*rs.Kwmz; + rs.wmZ = rs.wmZz; + + // производная заданной скорости + rs.pWmZ = (rs.wmZ - rs.wmZPrev3)/(TY*DECIM_PSI_WM_PM*3.); + rs.wmZPrev3 = rs.wmZPrev2; + rs.wmZPrev2 = rs.wmZPrev1; + rs.wmZPrev1 = rs.wmZ; + // ... чтобы избежать бросков при входе в рабочий режим + // if ( (inuWork == 0) || (mst.start == 0) || (mst.pzMode == 1) ) + // rs.tPwmZ = 0; + // if ( rs.tPwmZ <= 3 ) { + // rs.tPwmZ++; + // rs.pWmZ = 0; + // } +} //void reference_speed(void) + + + +// Выбирает о.с. +// (... -> ws, sinTheta, cosTheta, id1, iq1, id2, iq2, psi) +void select_feedback(void) { + ws = ivc.ws; + sinTheta = ivc.sinTheta; + cosTheta = ivc.cosTheta; + id1 = ivc.id1; + iq1 = ivc.iq1; + id2 = ivc.id2; + iq2 = ivc.iq2; + psi = ivc.psi; +} //void select_feedback(void) + +void write_swgen_pwm_times_split_eages(unsigned int mode_reload) { + + xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Ta_0.Ti; + xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Ta_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.Tc_0.Ti; + xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tc_1.Ti; + + xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Ta_0.Ti; + xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Ta_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.Tc_0.Ti; + xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tc_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 = PWM_MODE_RELOAD_FORCE;// mode_reload; + + xpwm_time.write_1_2_winding_break_times_split(&xpwm_time); +} + +void write_CMP_tims(int calcPWMtimes) { + + // if (calcPWMtimes == 0) { + // return; + // } + + // EPwm1Regs.CMPA.half.CMPA = xpwm_time.Ta0_1; + // EPwm2Regs.CMPA.half.CMPA = xpwm_time.Ta0_0; + + // EPwm3Regs.CMPA.half.CMPA = xpwm_time.Tb0_1; + // EPwm4Regs.CMPA.half.CMPA = xpwm_time.Tb0_0; + + // EPwm5Regs.CMPA.half.CMPA = xpwm_time.Tc0_1; + // EPwm6Regs.CMPA.half.CMPA = xpwm_time.Tc0_0; + + + // EPwm7Regs.CMPA.half.CMPA = xpwm_time.Ta1_1; + // EPwm8Regs.CMPA.half.CMPA = xpwm_time.Ta1_0; + + // EPwm9Regs.CMPA.half.CMPA = xpwm_time.Tb1_1; + // EPwm10Regs.CMPA.half.CMPA = xpwm_time.Tb1_0; + + // EPwm11Regs.CMPA.half.CMPA = xpwm_time.Tc1_1; + // EPwm12Regs.CMPA.half.CMPA = xpwm_time.Tc1_0; + + // разрешаем импульсы + EALLOW; + EPwm1Regs.TZCLR.all = 0x0004; + EPwm2Regs.TZCLR.all = 0x0004; + EPwm3Regs.TZCLR.all = 0x0004; + EPwm4Regs.TZCLR.all = 0x0004; + EPwm5Regs.TZCLR.all = 0x0004; + EPwm6Regs.TZCLR.all = 0x0004; +#ifdef ML + EPwm7Regs.TZCLR.all = 0x0004; + EPwm8Regs.TZCLR.all = 0x0004; + EPwm9Regs.TZCLR.all = 0x0004; + EPwm10Regs.TZCLR.all = 0x0004; + EPwm11Regs.TZCLR.all = 0x0004; + EPwm12Regs.TZCLR.all = 0x0004; +#endif //ML + EDIS; +} + diff --git a/Inu/upr.h b/Inu/upr.h new file mode 100644 index 0000000..aba741d --- /dev/null +++ b/Inu/upr.h @@ -0,0 +1,50 @@ +#ifndef UPR +#define UPR + +// Переменные, которые определены в upr.c (begin) +//######################################################################### +volatile short onceUpr; +struct SgmPar sgmPar; +struct Rf rf; +struct Rs rs; +struct Rp rp; + +float IzLim; +volatile float psi; +float idZ; +float iqZ; +float iZ; +float ws; +float sinTheta; +float cosTheta; +float id1; +float iq1; +float id2; +float iq2; +struct Cc cc; +struct Cf cf; +struct Csp csp; +struct Ivc ivc; +struct Ip ip; +//######################################################################### +// Переменные, которые определены в upr.c (end) + + + + +// Переменные, которые объявлены в upr.c (begin) +//######################################################################### +extern volatile float wmNf; +extern volatile float wm; +extern volatile float wmAbs; +extern volatile float ix1; +extern volatile float iy1; +extern volatile float ix2; +extern volatile float iy2; +extern volatile float kMe; +extern volatile short inuWork; +extern struct Mst mst; +extern volatile short faultNo; +//######################################################################### +// Переменные, которые объявлены в upr.c (end) +#endif //UPR diff --git a/Inu/wrapper_inu.c b/Inu/wrapper_inu.c new file mode 100644 index 0000000..9b07a3b --- /dev/null +++ b/Inu/wrapper_inu.c @@ -0,0 +1,267 @@ +/************************************************************************** + Description: Программа - упаковщик. + + Автор: Улитовский Д.И. + Дата последнего обновления: 2021.09.23 +**************************************************************************/ + + + +#define S_FUNCTION_NAME wrapper_inu +#define S_FUNCTION_LEVEL 2 +#include "simstruc.h" +#include "math.h" +#include "wrapper_inu.h" + + + +#define MDL_UPDATE +/* Function: mdlUpdate ==================================================== + * Abstract: + * This function is called once for every major integration time step. + * Discrete states are typically updated here, but this function is useful + * for performing any tasks that should only take place once per + * integration step. + */ +static void mdlUpdate(SimStruct *S, int_T tid) +{ + const real_T *u = (const real_T*) ssGetInputPortRealSignal(S,0); + real_T *xD = ssGetDiscStates(S); + real_T *rW = ssGetRWork(S); + int_T *iW = ssGetIWork(S); + + controller(S, u, xD, rW, iW); + +}//end mdlUpdate + + + +/* Function: mdlCheckParameters =========================================== +* Abstract: +* mdlCheckParameters verifies new parameter settings whenever parameter +* change or are re-evaluated during a simulation. +*/ +#define MDL_CHECK_PARAMETERS /* Change to #undef to remove function */ +#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) +static void mdlCheckParameters(SimStruct *S) +{ + int i; + + // Проверяем и принимаем параметры и разрешаем или запрещаем их менять + // в процессе моделирования + for (i=0; i 2) + { + ssSetErrorStatus(S,"Параметр м.б. только скаляром, вектором или матрицей"); + return; + } + + // Parameter not tunable +// ssSetSFcnParamTunable(S, i, SS_PRM_NOT_TUNABLE); + // Parameter tunable (we must create a corresponding run-time parameter) + ssSetSFcnParamTunable(S, i, SS_PRM_TUNABLE); + // Parameter tunable only during simulation +// ssSetSFcnParamTunable(S, i, SS_PRM_SIM_ONLY_TUNABLE); + + }//for (i=0; i and are documented in + * matlabroot/simulink/include/simstruc.h. The options should be + * bitwise or'd together as in + * ssSetOptions(S, (SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_name2)) + */ + // С помощью опции SS_OPTION_EXCEPTION_FREE_CODE обещаем, что в + // нашей программе нет "исключений" + ssSetOptions(S, (SS_OPTION_EXCEPTION_FREE_CODE)); + +}//end mdlInitializeSizes + + + +/* Function: mdlInitializeSampleTimes ===================================== + * Abstract: + * This function is used to specify the sample time(s) for your + * S-function. You must register the same number of sample times as + * specified in ssSetNumSampleTimes. + */ +static void mdlInitializeSampleTimes(SimStruct *S) +{ + double dt; + + // Шаг дискретизации + dt = mxGetPr(ssGetSFcnParam(S,NPARAMS-1))[0]; + + // Register one pair for each sample time + ssSetSampleTime(S, 0, dt); + ssSetOffsetTime(S, 0, 0.0); + +}//end mdlInitializeSampleTimes + + +#define MDL_START // Change to #undef to remove function +#if defined(MDL_START) +/* Function: mdlStart ===================================================== + * Abstract: + * This function is called once at start of model execution. If you + * have states that should be initialized once, this is the place + * to do it. + */ +static void mdlStart(SimStruct *S) +{ + int_T *iW = ssGetIWork(S); + + iW[0] = 1;//processParameters + iW[1] = 1;//start +} +#endif // MDL_START + + + +/* Function: mdlOutputs =================================================== + * Abstract: + * In this function, you compute the outputs of your S-function + * block. Generally outputs are placed in the output vector(s), + * ssGetOutputPortSignal. + */ +static void mdlOutputs(SimStruct *S, int_T tid) +{ + real_T *y = ssGetOutputPortRealSignal(S,0); + real_T *xD = ssGetDiscStates(S); + + int i; + + // OUTPUTS + for (i=0; i