diff --git a/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx b/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx index 545ac3c..4e27622 100644 Binary files a/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx and b/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx differ diff --git a/01_Matlab/02_Figures/control_methods.png b/01_Matlab/02_Figures/control_methods.png index 303777e..ee612d8 100644 Binary files a/01_Matlab/02_Figures/control_methods.png and b/01_Matlab/02_Figures/control_methods.png differ diff --git a/01_Matlab/03_CreateParamTable/tableParamType.xlsx b/01_Matlab/03_CreateParamTable/tableParamType.xlsx index 466781f..c5f1b2c 100644 Binary files a/01_Matlab/03_CreateParamTable/tableParamType.xlsx and b/01_Matlab/03_CreateParamTable/tableParamType.xlsx differ diff --git a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx index 95cd909..fae0bc2 100644 Binary files a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx and b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx differ diff --git a/01_Matlab/init_model.m b/01_Matlab/init_model.m index 183ff24..647d4b6 100644 --- a/01_Matlab/init_model.m +++ b/01_Matlab/init_model.m @@ -10,7 +10,7 @@ % >> improved motor efficiency -> lower energy consumption % % Author: Emanuel FERU -% Copyright © 2019 Emanuel FERU +% Copyright © 2019-2020 Emanuel FERU % % This program is free software: you can redistribute it and/or modify % it under the terms of the GNU General Public License as published by @@ -59,7 +59,8 @@ r_cos_M1 = cos((a_elecAngle_XA + 30)*(pi/180)); %% Control Manager % Control type selection CTRL_COM = 0; % [-] Commutation Control -CTRL_FOC = 1; % [-] Field Oriented Control (FOC) +CTRL_SIN = 1; % [-] Sinusoidal Control +CTRL_FOC = 2; % [-] Field Oriented Control (FOC) z_ctrlTypSel = CTRL_FOC; % [-] Control Type Selection (default) % Control model request @@ -89,7 +90,7 @@ cf_currFilt = 0.12; % [%] Current filter coefficient [0, 1]. Lower v b_diagEna = 1; % [-] Diagnostics enable flag: 0 = Disabled, 1 = Enabled (default) t_errQual = 0.6 * f_ctrl; % [s] Error qualification time t_errDequal = 2.0 * f_ctrl; % [s] Error dequalification time -r_errInpTgtThres = 200; % [-] Error input target threshold (for "Blocked motor" detection) +r_errInpTgtThres = 400; % [-] Error input target threshold (for "Blocked motor" detection) %% F04_Field_Oriented_Control @@ -146,10 +147,30 @@ iq_maxSca_M1 = sqrt(1 - iq_maxSca_XA.^2); %------------------------------- %% F05_Control_Type_Management + % Commutation method z_commutMap_M1 = [-1 -1 0 1 1 0; % Phase A 1 0 -1 -1 0 1; % Phase B 0 1 1 0 -1 -1]; % Phase C [-] Commutation method map + +% Sinusoidal method +% The map below was experimentaly calibrated on the real motor. Objectives: minimum noise and minimum torque ripple +a_phaAdv_M1 = [0 0 0 0 0 2 3 5 9 16 25]; % [deg] Phase advance angle +r_phaAdv_XA = [0 100 200 300 400 500 600 700 800 900 1000]; % [-] Scaled input target grid +% plot(r_phaAdv_XA, a_phaAdv_M1); + +omega = a_elecAngle_XA*(pi/180); +pha_adv = 30; % [deg] Phase advance to mach commands with the Hall position +r_sinPhaA_M1 = -sin(omega + pha_adv*(pi/180)); +r_sinPhaB_M1 = -sin(omega - 120*(pi/180) + pha_adv*(pi/180)); +r_sinPhaC_M1 = -sin(omega + 120*(pi/180) + pha_adv*(pi/180)); + +% Sinusoidal 3rd armonic method +A = 1.15; % Sine amplitude (tunable to get the Saddle sin maximum to value 1000) +sin3Arm = -0.224*sin(3*(omega + pha_adv*(pi/180))); % 3rd armonic +r_sin3PhaA_M1 = sin3Arm + A*r_sinPhaA_M1; +r_sin3PhaB_M1 = sin3Arm + A*r_sinPhaB_M1; +r_sin3PhaC_M1 = sin3Arm + A*r_sinPhaC_M1; disp('---- BLDC_controller: Initialization OK ----'); @@ -158,64 +179,70 @@ show_fig = 0; if show_fig + % Apply scaling sca_factor = 1000; % [-] scalling factor (to avoid truncation approximations on integer data type) + r_sinPhaA_M1sca = sca_factor * r_sinPhaA_M1; + r_sinPhaB_M1sca = sca_factor * r_sinPhaB_M1; + r_sinPhaC_M1sca = sca_factor * r_sinPhaC_M1; + r_sin3PhaA_M1sca = sca_factor * r_sin3PhaA_M1; + r_sin3PhaB_M1sca = sca_factor * r_sin3PhaB_M1; + r_sin3PhaC_M1sca = sca_factor * r_sin3PhaC_M1; - % Trapezoidal method - a_trapElecAngle_XA = [0 60 120 180 240 300 360]; % [deg] Electrical angle grid - r_trapPhaA_M1 = sca_factor*[ 1 1 1 -1 -1 -1 1]; - r_trapPhaB_M1 = sca_factor*[-1 -1 1 1 1 -1 -1]; - r_trapPhaC_M1 = sca_factor*[ 1 -1 -1 -1 1 1 1]; - - % Sinusoidal method - a_sinElecAngle_XA = 0:10:360; - omega = a_sinElecAngle_XA*(pi/180); - pha_adv = 30; % [deg] Phase advance to mach commands with the Hall position - r_sinPhaA_M1 = -sca_factor*sin(omega + pha_adv*(pi/180)); - r_sinPhaB_M1 = -sca_factor*sin(omega - 120*(pi/180) + pha_adv*(pi/180)); - r_sinPhaC_M1 = -sca_factor*sin(omega + 120*(pi/180) + pha_adv*(pi/180)); + % Commutation method + a_commElecAngle_XA = [0 60 120 180 240 300 360]; % [deg] Electrical angle grid + hall_A = [0 0 0 1 1 1 1] + 4; + hall_B = [1 1 0 0 0 1 1] + 2; + hall_C = [0 1 1 1 0 0 0]; % SVM (Space Vector Modulation) calculation - SVM_vec = [r_sinPhaA_M1; r_sinPhaB_M1; r_sinPhaC_M1]; + SVM_vec = [r_sinPhaA_M1sca; r_sinPhaB_M1sca; r_sinPhaC_M1sca]; SVM_min = min(SVM_vec); SVM_max = max(SVM_vec); SVM_sum = SVM_min + SVM_max; SVM_vec = SVM_vec - 0.5*SVM_sum; SVM_vec = (2/sqrt(3))*SVM_vec; - - hall_A = [0 0 0 1 1 1 1] + 4; - hall_B = [1 1 0 0 0 1 1] + 2; - hall_C = [0 1 1 1 0 0 0]; - + color = ['m' 'g' 'b']; lw = 1.5; figure - s1 = subplot(221); hold on - stairs(a_trapElecAngle_XA, hall_A, color(1), 'Linewidth', lw); - stairs(a_trapElecAngle_XA, hall_B, color(2), 'Linewidth', lw); - stairs(a_trapElecAngle_XA, hall_C, color(3), 'Linewidth', lw); - xticks(a_trapElecAngle_XA); + s1 = subplot(231); hold on + stairs(a_commElecAngle_XA, hall_A, color(1), 'Linewidth', lw); + stairs(a_commElecAngle_XA, hall_B, color(2), 'Linewidth', lw); + stairs(a_commElecAngle_XA, hall_C, color(3), 'Linewidth', lw); + xticks(a_commElecAngle_XA); grid yticks(0:5); yticklabels({'0','1','0','1','0','1'}); title('Hall sensors'); legend('Phase A','Phase B','Phase C','Location','NorthEast'); - s2 = subplot(222); hold on - stairs(a_trapElecAngle_XA, hall_A, color(1), 'Linewidth', lw); - stairs(a_trapElecAngle_XA, hall_B, color(2), 'Linewidth', lw); - stairs(a_trapElecAngle_XA, hall_C, color(3), 'Linewidth', lw); - xticks(a_trapElecAngle_XA); + s2 = subplot(232); hold on + stairs(a_commElecAngle_XA, hall_A, color(1), 'Linewidth', lw); + stairs(a_commElecAngle_XA, hall_B, color(2), 'Linewidth', lw); + stairs(a_commElecAngle_XA, hall_C, color(3), 'Linewidth', lw); + xticks(a_commElecAngle_XA); grid yticks(0:5); yticklabels({'0','1','0','1','0','1'}); title('Hall sensors'); legend('Phase A','Phase B','Phase C','Location','NorthEast'); - s3 = subplot(223); hold on - stairs(a_trapElecAngle_XA, sca_factor*[z_commutMap_M1(1,:) z_commutMap_M1(1,1)] + 6000, color(1), 'Linewidth', lw); - stairs(a_trapElecAngle_XA, sca_factor*[z_commutMap_M1(2,:) z_commutMap_M1(2,1)] + 3000, color(2), 'Linewidth', lw); - stairs(a_trapElecAngle_XA, sca_factor*[z_commutMap_M1(3,:) z_commutMap_M1(3,1)], color(3), 'Linewidth', lw); - xticks(a_trapElecAngle_XA); + s3 = subplot(233); hold on + stairs(a_commElecAngle_XA, hall_A, color(1), 'Linewidth', lw); + stairs(a_commElecAngle_XA, hall_B, color(2), 'Linewidth', lw); + stairs(a_commElecAngle_XA, hall_C, color(3), 'Linewidth', lw); + xticks(a_commElecAngle_XA); + grid + yticks(0:5); + yticklabels({'0','1','0','1','0','1'}); + title('Hall sensors'); + legend('Phase A','Phase B','Phase C','Location','NorthEast'); + + s4 = subplot(234); hold on + stairs(a_commElecAngle_XA, sca_factor*[z_commutMap_M1(1,:) z_commutMap_M1(1,1)] + 6000, color(1), 'Linewidth', lw); + stairs(a_commElecAngle_XA, sca_factor*[z_commutMap_M1(2,:) z_commutMap_M1(2,1)] + 3000, color(2), 'Linewidth', lw); + stairs(a_commElecAngle_XA, sca_factor*[z_commutMap_M1(3,:) z_commutMap_M1(3,1)], color(3), 'Linewidth', lw); + xticks(a_commElecAngle_XA); yticks(-1000:1000:7000); yticklabels({'-1000','0','1000','-1000','0','1000','-1000','0','1000'}); ylim([-1000 7000]); @@ -223,16 +250,27 @@ if show_fig title('Commutation method [0]'); xlabel('Electrical angle [deg]'); - s4 = subplot(224); hold on - plot(a_sinElecAngle_XA, SVM_vec(1,:), color(1), 'Linewidth', lw); - plot(a_sinElecAngle_XA, SVM_vec(2,:), color(2), 'Linewidth', lw); - plot(a_sinElecAngle_XA, SVM_vec(3,:), color(3), 'Linewidth', lw); - xticks(a_trapElecAngle_XA); + s5 = subplot(235); hold on + plot(a_elecAngle_XA, r_sin3PhaA_M1sca, color(1), 'Linewidth', lw); + plot(a_elecAngle_XA, r_sin3PhaB_M1sca, color(2), 'Linewidth', lw); + plot(a_elecAngle_XA, r_sin3PhaC_M1sca, color(3), 'Linewidth', lw); + xticks(a_commElecAngle_XA); ylim([-1000 1000]) grid - title('FOC method [1]'); + title('SIN method [1]'); xlabel('Electrical angle [deg]'); - linkaxes([s1 s2 s3 s4],'x'); + + s6 = subplot(236); hold on + plot(a_elecAngle_XA, SVM_vec(1,:), color(1), 'Linewidth', lw); + plot(a_elecAngle_XA, SVM_vec(2,:), color(2), 'Linewidth', lw); + plot(a_elecAngle_XA, SVM_vec(3,:), color(3), 'Linewidth', lw); + xticks(a_commElecAngle_XA); + ylim([-1000 1000]) + grid + title('FOC method [2]'); + xlabel('Electrical angle [deg]'); + + linkaxes([s1 s2 s3 s4 s5 s6],'x'); xlim([0 360]); end diff --git a/Inc/BLDC_controller.h b/Inc/BLDC_controller.h index 6583d08..2c08169 100644 --- a/Inc/BLDC_controller.h +++ b/Inc/BLDC_controller.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1199 + * Model version : 1.1212 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Sun Nov 3 12:28:16 2019 + * C/C++ source code generated on : Sat Nov 30 08:54:28 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -30,122 +30,138 @@ /* Forward declaration for rtModel */ typedef struct tag_RTM RT_MODEL; -/* Block signals and states (auto storage) for system '/Counter' */ +/* Block signals and states (auto storage) for system '/Counter' */ typedef struct { - int16_T UnitDelay_DSTATE; /* '/UnitDelay' */ + int16_T UnitDelay_DSTATE; /* '/UnitDelay' */ } DW_Counter; -/* Block signals and states (auto storage) for system '/PI_clamp_fixdt_id' */ +/* Block signals and states (auto storage) for system '/PI_clamp_fixdt_id' */ typedef struct { - int32_T UnitDelay_DSTATE; /* '/UnitDelay' */ - boolean_T UnitDelay1_DSTATE; /* '/UnitDelay1' */ + int32_T UnitDelay_DSTATE; /* '/UnitDelay' */ + boolean_T UnitDelay1_DSTATE; /* '/UnitDelay1' */ } DW_PI_clamp_fixdt; -/* Block signals and states (auto storage) for system '/Low_Pass_Filter' */ +/* Block signals and states (auto storage) for system '/Low_Pass_Filter' */ typedef struct { - int16_T UnitDelay3_DSTATE[2]; /* '/UnitDelay3' */ + int16_T UnitDelay3_DSTATE[2]; /* '/UnitDelay3' */ } DW_Low_Pass_Filter; -/* Block signals and states (auto storage) for system '/PI_clamp_fixdt_n' */ +/* Block signals and states (auto storage) for system '/PI_clamp_fixdt_n' */ typedef struct { - int32_T UnitDelay_DSTATE; /* '/UnitDelay' */ - boolean_T UnitDelay1_DSTATE; /* '/UnitDelay1' */ + int32_T UnitDelay_DSTATE; /* '/UnitDelay' */ + boolean_T UnitDelay1_DSTATE; /* '/UnitDelay1' */ } DW_PI_clamp_fixdt_c; -/* Block signals and states (auto storage) for system '/Counter' */ +/* Block signals and states (auto storage) for system '/Counter' */ typedef struct { - uint16_T UnitDelay_DSTATE; /* '/UnitDelay' */ + uint16_T UnitDelay_DSTATE; /* '/UnitDelay' */ } DW_Counter_l; -/* Block signals and states (auto storage) for system '/either_edge' */ +/* Block signals and states (auto storage) for system '/either_edge' */ typedef struct { - boolean_T UnitDelay_DSTATE; /* '/UnitDelay' */ + boolean_T UnitDelay_DSTATE; /* '/UnitDelay' */ } DW_either_edge; /* Block signals and states (auto storage) for system '/Debounce_Filter' */ typedef struct { - DW_either_edge either_edge_k; /* '/either_edge' */ - DW_Counter_l Counter_h; /* '/Counter' */ - DW_Counter_l Counter_i0; /* '/Counter' */ - boolean_T UnitDelay_DSTATE; /* '/UnitDelay' */ + DW_either_edge either_edge_k; /* '/either_edge' */ + DW_Counter_l Counter_h; /* '/Counter' */ + DW_Counter_l Counter_i0; /* '/Counter' */ + boolean_T UnitDelay_DSTATE; /* '/UnitDelay' */ } DW_Debounce_Filter; /* Block signals and states (auto storage) for system '' */ typedef struct { DW_either_edge either_edge_a; /* '/either_edge' */ DW_Debounce_Filter Debounce_Filter_f;/* '/Debounce_Filter' */ - DW_PI_clamp_fixdt PI_clamp_fixdt_iq; /* '/PI_clamp_fixdt_iq' */ - DW_PI_clamp_fixdt_c PI_clamp_fixdt_n_o;/* '/PI_clamp_fixdt_n' */ - DW_Low_Pass_Filter Low_Pass_Filter_m;/* '/Low_Pass_Filter' */ - DW_PI_clamp_fixdt PI_clamp_fixdt_id; /* '/PI_clamp_fixdt_id' */ - DW_Counter Counter_e; /* '/Counter' */ - int32_T UnitDelay_DSTATE; /* '/UnitDelay' */ - int16_T Gain4[3]; /* '/Gain4' */ - int16_T Sum1[2]; /* '/Sum1' */ - int16_T z_counterRawPrev; /* '/z_counterRawPrev' */ + DW_PI_clamp_fixdt PI_clamp_fixdt_iq; /* '/PI_clamp_fixdt_iq' */ + DW_PI_clamp_fixdt_c PI_clamp_fixdt_n_o;/* '/PI_clamp_fixdt_n' */ + DW_Low_Pass_Filter Low_Pass_Filter_m;/* '/Low_Pass_Filter' */ + DW_PI_clamp_fixdt PI_clamp_fixdt_id; /* '/PI_clamp_fixdt_id' */ + DW_Counter Counter_e; /* '/Counter' */ + int32_T UnitDelay_DSTATE; /* '/UnitDelay' */ + int16_T Gain4[3]; /* '/Gain4' */ + int16_T Sum1[2]; /* '/Sum1' */ + int16_T z_counterRawPrev; /* '/z_counterRawPrev' */ int16_T Merge; /* '/Merge' */ - int16_T Divide1; /* '/Divide1' */ - int16_T Divide4; /* '/Divide4' */ - int16_T Switch1; /* '/Switch1' */ - int16_T Divide11; /* '/Divide11' */ - int16_T UnitDelay3_DSTATE; /* '/UnitDelay3' */ - int16_T UnitDelay4_DSTATE; /* '/UnitDelay4' */ - int16_T UnitDelay4_DSTATE_p; /* '/UnitDelay4' */ - int16_T UnitDelay2_DSTATE; /* '/UnitDelay2' */ - int16_T UnitDelay3_DSTATE_o; /* '/UnitDelay3' */ - int16_T UnitDelay5_DSTATE; /* '/UnitDelay5' */ - int16_T UnitDelay4_DSTATE_e; /* '/UnitDelay4' */ - int16_T UnitDelay4_DSTATE_er; /* '/UnitDelay4' */ - int8_T Switch2; /* '/Switch2' */ - int8_T UnitDelay2_DSTATE_b; /* '/UnitDelay2' */ + int16_T Divide1; /* '/Divide1' */ + int16_T Divide4; /* '/Divide4' */ + int16_T Switch1; /* '/Switch1' */ + int16_T Divide11; /* '/Divide11' */ + int16_T UnitDelay3_DSTATE; /* '/UnitDelay3' */ + int16_T UnitDelay4_DSTATE; /* '/UnitDelay4' */ + int16_T UnitDelay2_DSTATE; /* '/UnitDelay2' */ + int16_T UnitDelay3_DSTATE_o; /* '/UnitDelay3' */ + int16_T UnitDelay5_DSTATE; /* '/UnitDelay5' */ + int16_T UnitDelay4_DSTATE_e; /* '/UnitDelay4' */ + int16_T UnitDelay4_DSTATE_eu; /* '/UnitDelay4' */ + int8_T Switch2; /* '/Switch2' */ + int8_T UnitDelay2_DSTATE_b; /* '/UnitDelay2' */ int8_T If2_ActiveSubsystem; /* '/If2' */ + int8_T If2_ActiveSubsystem_j; /* '/If2' */ int8_T If1_ActiveSubsystem; /* '/If1' */ int8_T If2_ActiveSubsystem_a; /* '/If2' */ int8_T If1_ActiveSubsystem_e; /* '/If1' */ - int8_T If1_ActiveSubsystem_f; /* '/If1' */ - int8_T If2_ActiveSubsystem_c; /* '/If2' */ + int8_T If1_ActiveSubsystem_f; /* '/If1' */ + int8_T If2_ActiveSubsystem_c; /* '/If2' */ int8_T SwitchCase_ActiveSubsystem; /* '/Switch Case' */ - uint8_T UnitDelay3_DSTATE_fy; /* '/UnitDelay3' */ - uint8_T UnitDelay1_DSTATE; /* '/UnitDelay1' */ - uint8_T UnitDelay2_DSTATE_f; /* '/UnitDelay2' */ + uint8_T UnitDelay3_DSTATE_fy; /* '/UnitDelay3' */ + uint8_T UnitDelay1_DSTATE; /* '/UnitDelay1' */ + uint8_T UnitDelay2_DSTATE_f; /* '/UnitDelay2' */ uint8_T UnitDelay1_DSTATE_p; /* '/UnitDelay1' */ uint8_T UnitDelay_DSTATE_c; /* '/UnitDelay' */ uint8_T is_active_c1_BLDC_controller;/* '/F03_02_Control_Mode_Manager' */ uint8_T is_c1_BLDC_controller; /* '/F03_02_Control_Mode_Manager' */ uint8_T is_ACTIVE; /* '/F03_02_Control_Mode_Manager' */ - boolean_T Merge_n; /* '/Merge' */ - boolean_T dz_cntTrnsDet; /* '/dz_cntTrnsDet' */ - boolean_T UnitDelay_DSTATE_g; /* '/UnitDelay' */ - boolean_T UnitDelay1_DSTATE_n; /* '/UnitDelay1' */ - boolean_T n_commDeacv_Mode; /* '/n_commDeacv' */ - boolean_T n_fieldWeakAuth_Mode; /* '/n_fieldWeakAuth' */ - boolean_T dz_cntTrnsDet_Mode; /* '/dz_cntTrnsDet' */ + boolean_T Merge_n; /* '/Merge' */ + boolean_T dz_cntTrnsDet; /* '/dz_cntTrnsDet' */ + boolean_T UnitDelay_DSTATE_e; /* '/UnitDelay' */ + boolean_T UnitDelay1_DSTATE_n; /* '/UnitDelay1' */ + boolean_T n_commDeacv_Mode; /* '/n_commDeacv' */ + boolean_T n_fieldWeakAuth_Mode; /* '/n_fieldWeakAuth' */ + boolean_T n_fieldWeakAuth_Mode_m; /* '/n_fieldWeakAuth' */ + boolean_T dz_cntTrnsDet_Mode; /* '/dz_cntTrnsDet' */ } DW; /* Constant parameters (auto storage) */ typedef struct { - /* Computed Parameter: z_commutMap_M1_table - * Referenced by: '/z_commutMap_M1' + /* Computed Parameter: r_sin3PhaA_M1_Table + * Referenced by: '/r_sin3PhaA_M1' */ - int16_T z_commutMap_M1_table[18]; + int16_T r_sin3PhaA_M1_Table[181]; + + /* Computed Parameter: r_sin3PhaB_M1_Table + * Referenced by: '/r_sin3PhaB_M1' + */ + int16_T r_sin3PhaB_M1_Table[181]; + + /* Computed Parameter: r_sin3PhaC_M1_Table + * Referenced by: '/r_sin3PhaC_M1' + */ + int16_T r_sin3PhaC_M1_Table[181]; /* Computed Parameter: r_sin_M1_Table - * Referenced by: '/r_sin_M1' + * Referenced by: '/r_sin_M1' */ int16_T r_sin_M1_Table[181]; /* Computed Parameter: r_cos_M1_Table - * Referenced by: '/r_cos_M1' + * Referenced by: '/r_cos_M1' */ int16_T r_cos_M1_Table[181]; /* Computed Parameter: iq_maxSca_M1_Table - * Referenced by: '/iq_maxSca_M1' + * Referenced by: '/iq_maxSca_M1' */ uint16_T iq_maxSca_M1_Table[50]; + /* Computed Parameter: z_commutMap_M1_table + * Referenced by: '/z_commutMap_M1' + */ + int8_T z_commutMap_M1_table[18]; + /* Computed Parameter: vec_hallToPos_Value - * Referenced by: '/vec_hallToPos' + * Referenced by: '/vec_hallToPos' */ int8_T vec_hallToPos_Value[8]; } ConstP; @@ -178,27 +194,24 @@ typedef struct { /* Parameters (auto storage) */ struct P_ { int32_T dV_openRate; /* Variable: dV_openRate - * Referenced by: '/dV_openRate' + * Referenced by: '/dV_openRate' */ int16_T dz_cntTrnsDetHi; /* Variable: dz_cntTrnsDetHi - * Referenced by: '/dz_cntTrnsDet' + * Referenced by: '/dz_cntTrnsDet' */ int16_T dz_cntTrnsDetLo; /* Variable: dz_cntTrnsDetLo - * Referenced by: '/dz_cntTrnsDet' - */ - int16_T r_errInpTgtThres; /* Variable: r_errInpTgtThres - * Referenced by: '/r_errInpTgtThres' + * Referenced by: '/dz_cntTrnsDet' */ int16_T z_maxCntRst; /* Variable: z_maxCntRst * Referenced by: - * '/Counter' - * '/z_maxCntRst' - * '/z_maxCntRst2' - * '/UnitDelay3' - * '/z_counter' + * '/Counter' + * '/z_maxCntRst' + * '/z_maxCntRst2' + * '/UnitDelay3' + * '/z_counter' */ uint16_T cf_speedCoef; /* Variable: cf_speedCoef - * Referenced by: '/cf_speedCoef' + * Referenced by: '/cf_speedCoef' */ uint16_T t_errDequal; /* Variable: t_errDequal * Referenced by: '/t_errDequal' @@ -206,77 +219,90 @@ struct P_ { uint16_T t_errQual; /* Variable: t_errQual * Referenced by: '/t_errQual' */ - uint16_T cf_idKp; /* Variable: cf_idKp - * Referenced by: '/cf_idKp1' - */ - uint16_T cf_iqKp; /* Variable: cf_iqKp - * Referenced by: '/cf_iqKp' - */ - uint16_T cf_nKp; /* Variable: cf_nKp - * Referenced by: '/cf_nKp' - */ int16_T Vd_max; /* Variable: Vd_max * Referenced by: - * '/Vd_max1' - * '/Vd_max' + * '/Vd_max1' + * '/Vd_max' */ int16_T Vq_max_M1[46]; /* Variable: Vq_max_M1 - * Referenced by: '/Vq_max_M1' + * Referenced by: '/Vq_max_M1' */ int16_T Vq_max_XA[46]; /* Variable: Vq_max_XA - * Referenced by: '/Vq_max_XA' + * Referenced by: '/Vq_max_XA' */ int16_T i_max; /* Variable: i_max * Referenced by: - * '/i_max' - * '/i_max' + * '/i_max' + * '/i_max' */ int16_T id_fieldWeak_M1[12]; /* Variable: id_fieldWeak_M1 - * Referenced by: '/id_fieldWeak_M1' + * Referenced by: '/id_fieldWeak_M1' */ int16_T n_commAcvLo; /* Variable: n_commAcvLo - * Referenced by: '/n_commDeacv' + * Referenced by: '/n_commDeacv' */ int16_T n_commDeacvHi; /* Variable: n_commDeacvHi - * Referenced by: '/n_commDeacv' + * Referenced by: '/n_commDeacv' */ int16_T n_fieldWeakAuthHi; /* Variable: n_fieldWeakAuthHi - * Referenced by: '/n_fieldWeakAuth' + * Referenced by: + * '/n_fieldWeakAuth' + * '/n_fieldWeakAuth' */ int16_T n_fieldWeakAuthLo; /* Variable: n_fieldWeakAuthLo - * Referenced by: '/n_fieldWeakAuth' + * Referenced by: + * '/n_fieldWeakAuth' + * '/n_fieldWeakAuth' */ int16_T n_max; /* Variable: n_max * Referenced by: - * '/n_max1' - * '/n_max' + * '/n_max1' + * '/n_max' */ int16_T n_stdStillDet; /* Variable: n_stdStillDet - * Referenced by: '/n_stdStillDet' + * Referenced by: '/n_stdStillDet' + */ + int16_T r_errInpTgtThres; /* Variable: r_errInpTgtThres + * Referenced by: '/r_errInpTgtThres' */ int16_T r_fieldWeak_XA[12]; /* Variable: r_fieldWeak_XA - * Referenced by: '/r_fieldWeak_XA' + * Referenced by: '/r_fieldWeak_XA' + */ + int16_T r_phaAdv_XA[11]; /* Variable: r_phaAdv_XA + * Referenced by: '/r_phaAdv_XA' + */ + uint16_T cf_idKp; /* Variable: cf_idKp + * Referenced by: '/cf_idKp1' + */ + uint16_T cf_iqKp; /* Variable: cf_iqKp + * Referenced by: '/cf_iqKp' + */ + uint16_T cf_nKp; /* Variable: cf_nKp + * Referenced by: '/cf_nKp' */ uint16_T cf_currFilt; /* Variable: cf_currFilt - * Referenced by: '/cf_currFilt' + * Referenced by: '/cf_currFilt' */ uint16_T cf_idKi; /* Variable: cf_idKi - * Referenced by: '/cf_idKi1' + * Referenced by: '/cf_idKi1' */ uint16_T cf_iqKi; /* Variable: cf_iqKi - * Referenced by: '/cf_iqKi' + * Referenced by: '/cf_iqKi' */ uint16_T cf_iqKiLimProt; /* Variable: cf_iqKiLimProt - * Referenced by: '/cf_iqKiLimProt' + * Referenced by: '/cf_iqKiLimProt' */ uint16_T cf_nKi; /* Variable: cf_nKi - * Referenced by: '/cf_nKi' + * Referenced by: '/cf_nKi' */ uint16_T cf_iqKpLimProt; /* Variable: cf_iqKpLimProt - * Referenced by: '/cf_iqKpLimProt' + * Referenced by: '/cf_iqKpLimProt' */ uint16_T cf_nKpLimProt; /* Variable: cf_nKpLimProt - * Referenced by: '/cf_nKpLimProt' + * Referenced by: '/cf_nKpLimProt' + */ + int16_T a_phaAdv_M1[11]; /* Variable: a_phaAdv_M1 + * Referenced by: '/a_phaAdv_M1' */ uint8_T z_ctrlTypSel; /* Variable: z_ctrlTypSel * Referenced by: '/z_ctrlTypSel1' @@ -285,10 +311,12 @@ struct P_ { * Referenced by: '/b_diagEna' */ boolean_T b_fieldWeakEna; /* Variable: b_fieldWeakEna - * Referenced by: '/b_fieldWeakEna' + * Referenced by: + * '/b_fieldWeakEna' + * '/b_fieldWeakEna' */ boolean_T b_selPhaABCurrMeas; /* Variable: b_selPhaABCurrMeas - * Referenced by: '/b_selPhaABCurrMeas' + * Referenced by: '/b_selPhaABCurrMeas' */ }; @@ -313,26 +341,24 @@ extern void BLDC_controller_step(RT_MODEL *const rtM); /*- * These blocks were eliminated from the model due to optimizations: * - * Block '/Scope2' : Unused code path elimination - * Block '/Scope' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Propagation' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Propagation' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Propagation' : Unused code path elimination + * Block '/Scope2' : Unused code path elimination + * Block '/Scope' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Propagation' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Propagation' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Propagation' : Unused code path elimination * Block '/Scope12' : Unused code path elimination * Block '/Scope8' : Unused code path elimination * Block '/Scope9' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Propagation' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Propagation' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Propagation' : Unused code path elimination - * Block '/Data Type Conversion2' : Eliminate redundant data type conversion - * Block '/Data Type Conversion3' : Eliminate redundant data type conversion - * Block '/Data Type Conversion6' : Eliminate redundant data type conversion + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Propagation' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Propagation' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Propagation' : Unused code path elimination + * Block '/Scope' : Unused code path elimination */ /*- @@ -359,67 +385,74 @@ extern void BLDC_controller_step(RT_MODEL *const rtM); * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager' * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control' * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F05_Control_Type_Management' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_01_Input_Scaling' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_02_Edge_Detector' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_03_Position_Calculation' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_04_Direction_Detection' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_05_Speed_Estimation' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_06_Electrical_Angle_Estimation' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_01_Input_Scaling/Commutation_Control_Type' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_01_Input_Scaling/FOC_Control_Type' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_05_Speed_Estimation/Counter' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_05_Speed_Estimation/Raw_Motor_Speed_Estimation' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_05_Speed_Estimation/Counter/rst_Delay' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/either_edge' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Default' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Dequalification' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Qualification' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/either_edge' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Dequalification/Counter' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Dequalification/Counter/rst_Delay' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Qualification/Counter' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Qualification/Counter/rst_Delay' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_01_Mode_Transition_Calculation' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_02_Control_Mode_Manager' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Clarke_Transform' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Current_Filtering' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Field_Weakening' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Inv_Clarke_Transform' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Inv_Park_Transform' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Open_Mode' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Park_Transform' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Voltage_Mode' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Clarke_Transform/Clarke_PhasesAB' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Clarke_Transform/Clarke_PhasesBC' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Current_Filtering/Low_Pass_Filter' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Current_Limit_Protection' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Speed_Limit_Protection' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Current_Limit_Protection/Saturation Dynamic' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Speed_Limit_Protection/Saturation Dynamic1' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Open_Mode/Rate_Limiter' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Open_Mode/rising_edge_init' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Open_Mode/Rate_Limiter/Delay_Init1' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Open_Mode/Rate_Limiter/Saturation Dynamic' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n/Clamping_circuit' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n/Integrator' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n/Saturation_hit' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/Saturation Dynamic' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq/Clamping_circuit' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq/Integrator' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq/Saturation_hit' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/Saturation Dynamic' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id/Clamping_circuit' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id/Integrator' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id/Saturation_hit' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Voltage_Mode/Saturation Dynamic1' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_01_Edge_Detector' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_02_Position_Calculation' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_03_Direction_Detection' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_04_Speed_Estimation' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_05_Electrical_Angle_Estimation' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_04_Speed_Estimation/Counter' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_04_Speed_Estimation/Raw_Motor_Speed_Estimation' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F01_Estimations/F01_04_Speed_Estimation/Counter/rst_Delay' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/either_edge' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Default' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Dequalification' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Qualification' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/either_edge' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Dequalification/Counter' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Dequalification/Counter/rst_Delay' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Qualification/Counter' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F02_Diagnostics/Debounce_Filter/Qualification/Counter/rst_Delay' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_01_Mode_Transition_Calculation' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_02_Control_Mode_Manager' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/Default_Control_Type' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/Default_Mode' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/FOC_Control_Type' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/Open_Mode' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/Open_Mode/Rate_Limiter' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/Open_Mode/rising_edge_init' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/Open_Mode/Rate_Limiter/Delay_Init1' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F03_Control_Mode_Manager/F03_03_Input_Target_Synthesis/Open_Mode/Rate_Limiter/Saturation Dynamic' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Clarke_Transform' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Current_Filtering' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Field_Weakening' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Inv_Clarke_Transform' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Inv_Park_Transform' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Open_Mode' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Park_Transform' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Voltage_Mode' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Clarke_Transform/Clarke_PhasesAB' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Clarke_Transform/Clarke_PhasesBC' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Current_Filtering/Low_Pass_Filter' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Current_Limit_Protection' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Speed_Limit_Protection' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Current_Limit_Protection/Saturation Dynamic' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Motor_Limitations/Speed_Limit_Protection/Saturation Dynamic1' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n/Clamping_circuit' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n/Integrator' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Speed_Mode/PI_clamp_fixdt_n/Saturation_hit' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/Saturation Dynamic' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq/Clamping_circuit' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq/Integrator' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Torque_Mode/PI_clamp_fixdt_iq/Saturation_hit' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/Saturation Dynamic' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id/Clamping_circuit' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id/Integrator' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Vd_Calculation/PI_clamp_fixdt_id/Saturation_hit' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/Voltage_Mode/Saturation Dynamic1' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F05_Control_Type_Management/F05_00_COM_Method' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F05_Control_Type_Management/F05_01_SIN_Method' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F05_Control_Type_Management/F05_02_FOC_Method' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F05_Control_Type_Management/F05_01_SIN_Method/Phase_Advance_Calculation' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F05_Control_Type_Management/F05_01_SIN_Method/Phase_Advance_Calculation/Modulo_fixdt' */ #endif /* RTW_HEADER_BLDC_controller_h_ */ diff --git a/Inc/config.h b/Inc/config.h index a4669e3..00e4eee 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -142,8 +142,8 @@ // ############################### MOTOR CONTROL (overwrite) ######################### -#define CTRL_TYP_SEL 1 // [-] Control type selection: 0 = Commutation , 1 = FOC Field Oriented Control (default) -#define CTRL_MOD_REQ 1 // [-] Control mode request: 0 = Open mode, 1 = Voltage mode (default), 2 = Speed mode, 3 = Torque mode +#define CTRL_TYP_SEL 2 // [-] Control type selection: 0 = Commutation , 1 = Sinusoidal, 2 = FOC Field Oriented Control (default) +#define CTRL_MOD_REQ 1 // [-] Control mode request: 0 = Open mode, 1 = VOLTAGE mode (default), 2 = SPEED mode, 3 = TORQUE mode. Note: SPEED and TORQUE modes are only available for FOC! #define DIAG_ENA 1 // [-] Motor Diagnostics enable flag: 0 = Disabled, 1 = Enabled (default) #define FIELD_WEAK_ENA 0 // [-] Field Weakening enable flag: 0 = Disabled (default), 1 = Enabled #define I_MOT_MAX (15 * A2BIT_CONV) << 4 // [A] Maximum motor current limit (Change only the first number, the rest is needed for fixed-point conversion, fixdt(1,16,4)) diff --git a/Inc/rtwtypes.h b/Inc/rtwtypes.h index 2ffd35b..51b2686 100644 --- a/Inc/rtwtypes.h +++ b/Inc/rtwtypes.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1197 + * Model version : 1.1212 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Thu Oct 31 21:29:42 2019 + * C/C++ source code generated on : Sat Nov 30 08:54:28 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/README.md b/README.md index cfc50cf..36971d5 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ The main firmware architecture includes: - **Diagnostics**: implements error detection such as unconnected Hall sensor, motor blocked, MOSFET defective - **Control Manager**: manages the transitions between control modes (Voltage, Speed, Torque) - **FOC Algorithm**: implements the FOC strategy -- **Control Type Manager**: Manages the transition between Commutation and FOC Algorithm +- **Control Type Manager**: Manages the transition between Commutation, Sinusoidal, and FOC control type ![Firmware architecture](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/blob/master/docs/pictures/FW_architecture.png) @@ -29,15 +29,17 @@ The FOC algorithm architecture is illustrated in the figure below: ![FOC algorithm](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/blob/master/docs/pictures/FOC_algorithm.png) -In this firmware two control methods are available: -- Commutation method -- FOC method +In this firmware 3 control types are available: +- Commutation +- SIN (Sinusoidal) +- FOC (Field Oriented Control) ![Schematic representation of the available control methods](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/blob/master/01_Matlab/02_Figures/control_methods.png) -A short video showing the noise performance of the Commutation method vs Advanced control method: - -[â–ºVideo: Commutation method vs Advanced control](https://drive.google.com/file/d/1vC_kEkp2LE2lAaMCJcmK4z2m3jrPUoBD/view) +Demo videos: +[â–ºVideo: Commutation vs Advanced control (constant speed)](https://drive.google.com/open?id=1vC_kEkp2LE2lAaMCJcmK4z2m3jrPUoBD) +[â–ºVideo: Commutation vs Advanced control (variable speed)](https://drive.google.com/open?id=1rrQ4k5VLhhAWXQzDSCar_SmEdsbM-hq2) +[â–ºVideo: Reliable Serial Communication demo](https://drive.google.com/open?id=1mUM-p7SE6gmyTH7zhDHy5DUyczXvmy5d) ![Hoverboard wheel](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/blob/master/docs/pictures/hoverboard_wheel.JPG) @@ -71,7 +73,7 @@ Each motor is constantly monitored for errors. These errors are: - **Error 002**: Hall sensor short circuit - **Error 004**: Motor NOT able to spin (Possible causes: motor phase disconnected, MOSFET defective, operational Amplifier defective, motor blocked) -The error codes above are reported for each motor in the variables **errCode_Left** and **errCode_Right** for Left motor (long wired motor) and Right motor (short wired motor), respectively. +The error codes above are reported for each motor in the variables **errCode_Left** and **errCode_Right** for Left motor (long wired motor) and Right motor (short wired motor), respectively. In case of error, the motor power is reduced to 0, while an audible (fast beep) can be heard to notify the user. --- @@ -146,9 +148,6 @@ Currently supported: Wii Nunchuck, analog potentiometer and PPM-Sum signal from A good example of control via UART, eg. from an Arduino or raspberryPi, can be found here: https://github.com/p-h-a-i-l/hoverboard-firmware-hack -### Future work - - convert all calculations and remaining filters (for the battery voltage, current, and temperature) from floating point to fixed-point. This will reduce further the SMT32 computational load -> **DONE** - --- ## Acknowledgements diff --git a/Src/BLDC_controller.c b/Src/BLDC_controller.c index ba768e5..561ee33 100644 --- a/Src/BLDC_controller.c +++ b/Src/BLDC_controller.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1199 + * Model version : 1.1212 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Sun Nov 3 12:28:16 2019 + * C/C++ source code generated on : Sat Nov 30 08:54:28 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -93,14 +93,15 @@ preprocessor word size checks. #endif #endif -uint8_T plook_u8u16_evencka(uint16_T u, uint16_T bp0, uint16_T bpSpace, uint32_T - maxIndex); uint8_T plook_u8s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace, uint32_T maxIndex); -uint8_T plook_u8s16u8n6_evenc_s(int16_T u, int16_T bp0, uint16_T bpSpace, +uint8_T plook_u8u16_evencka(uint16_T u, uint16_T bp0, uint16_T bpSpace, uint32_T + maxIndex); +uint8_T plook_u8s16u8n7_evenc_s(int16_T u, int16_T bp0, uint16_T bpSpace, uint32_T maxIndex, uint8_T *fraction); -int16_T intrp1d_s16s32s32u8u8n6l_s(uint8_T bpIndex, uint8_T frac, const int16_T +int16_T intrp1d_s16s32s32u8u8n7l_s(uint8_T bpIndex, uint8_T frac, const int16_T table[]); +int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator); extern void Counter_Init(DW_Counter *localDW, int16_T rtp_z_cntInit); extern int16_T Counter(int16_T rtu_inc, int16_T rtu_max, boolean_T rtu_rst, DW_Counter *localDW); @@ -124,33 +125,6 @@ extern void Debounce_Filter_Init(DW_Debounce_Filter *localDW); extern void Debounce_Filter_Reset(DW_Debounce_Filter *localDW); extern void Debounce_Filter(boolean_T rtu_u, uint16_T rtu_tAcv, uint16_T rtu_tDeacv, boolean_T *rty_y, DW_Debounce_Filter *localDW); -uint8_T plook_u8u16_evencka(uint16_T u, uint16_T bp0, uint16_T bpSpace, uint32_T - maxIndex) -{ - uint8_T bpIndex; - uint16_T fbpIndex; - - /* Prelookup - Index only - Index Search method: 'even' - Extrapolation method: 'Clip' - Use previous index: 'off' - Use last breakpoint for index at or above upper limit: 'on' - Remove protection against out-of-range input in generated code: 'off' - */ - if (u <= bp0) { - bpIndex = 0U; - } else { - fbpIndex = (uint16_T)((uint32_T)(uint16_T)((uint32_T)u - bp0) / bpSpace); - if (fbpIndex < maxIndex) { - bpIndex = (uint8_T)fbpIndex; - } else { - bpIndex = (uint8_T)maxIndex; - } - } - - return bpIndex; -} - uint8_T plook_u8s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace, uint32_T maxIndex) { @@ -178,7 +152,34 @@ uint8_T plook_u8s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace, uint32_T return bpIndex; } -uint8_T plook_u8s16u8n6_evenc_s(int16_T u, int16_T bp0, uint16_T bpSpace, +uint8_T plook_u8u16_evencka(uint16_T u, uint16_T bp0, uint16_T bpSpace, uint32_T + maxIndex) +{ + uint8_T bpIndex; + uint16_T fbpIndex; + + /* Prelookup - Index only + Index Search method: 'even' + Extrapolation method: 'Clip' + Use previous index: 'off' + Use last breakpoint for index at or above upper limit: 'on' + Remove protection against out-of-range input in generated code: 'off' + */ + if (u <= bp0) { + bpIndex = 0U; + } else { + fbpIndex = (uint16_T)((uint32_T)(uint16_T)((uint32_T)u - bp0) / bpSpace); + if (fbpIndex < maxIndex) { + bpIndex = (uint8_T)fbpIndex; + } else { + bpIndex = (uint8_T)maxIndex; + } + } + + return bpIndex; +} + +uint8_T plook_u8s16u8n7_evenc_s(int16_T u, int16_T bp0, uint16_T bpSpace, uint32_T maxIndex, uint8_T *fraction) { uint8_T bpIndex; @@ -202,17 +203,17 @@ uint8_T plook_u8s16u8n6_evenc_s(int16_T u, int16_T bp0, uint16_T bpSpace, if (fbpIndex < maxIndex) { bpIndex = (uint8_T)fbpIndex; *fraction = (uint8_T)(((uint32_T)(uint16_T)((uint32_T)uAdjust - (uint16_T) - ((uint32_T)bpIndex * bpSpace)) << 6) / bpSpace); + ((uint32_T)bpIndex * bpSpace)) << 7) / bpSpace); } else { bpIndex = (uint8_T)(maxIndex - 1U); - *fraction = 64U; + *fraction = 128U; } } return bpIndex; } -int16_T intrp1d_s16s32s32u8u8n6l_s(uint8_T bpIndex, uint8_T frac, const int16_T +int16_T intrp1d_s16s32s32u8u8n7l_s(uint8_T bpIndex, uint8_T frac, const int16_T table[]) { uint32_T offset_0d; @@ -225,26 +226,32 @@ int16_T intrp1d_s16s32s32u8u8n6l_s(uint8_T bpIndex, uint8_T frac, const int16_T */ offset_0d = bpIndex; return (int16_T)((int16_T)(((table[offset_0d + 1U] - table[offset_0d]) * frac) - >> 6) + table[offset_0d]); + >> 7) + table[offset_0d]); } -/* System initialize for atomic system: '/Counter' */ +int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator) +{ + return (((numerator < 0) != (denominator < 0)) && (numerator % denominator != + 0) ? -1 : 0) + numerator / denominator; +} + +/* System initialize for atomic system: '/Counter' */ void Counter_Init(DW_Counter *localDW, int16_T rtp_z_cntInit) { - /* InitializeConditions for UnitDelay: '/UnitDelay' */ + /* InitializeConditions for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rtp_z_cntInit; } -/* Output and update for atomic system: '/Counter' */ +/* Output and update for atomic system: '/Counter' */ int16_T Counter(int16_T rtu_inc, int16_T rtu_max, boolean_T rtu_rst, DW_Counter * localDW) { int16_T rtu_rst_0; int16_T rty_cnt_0; - /* Switch: '/Switch1' incorporates: - * Constant: '/Constant23' - * UnitDelay: '/UnitDelay' + /* Switch: '/Switch1' incorporates: + * Constant: '/Constant23' + * UnitDelay: '/UnitDelay' */ if (rtu_rst) { rtu_rst_0 = 0; @@ -252,42 +259,42 @@ int16_T Counter(int16_T rtu_inc, int16_T rtu_max, boolean_T rtu_rst, DW_Counter rtu_rst_0 = localDW->UnitDelay_DSTATE; } - /* End of Switch: '/Switch1' */ + /* End of Switch: '/Switch1' */ - /* Sum: '/Sum1' */ + /* Sum: '/Sum1' */ rty_cnt_0 = (int16_T)(rtu_inc + rtu_rst_0); - /* MinMax: '/MinMax' */ + /* MinMax: '/MinMax' */ if (rty_cnt_0 < rtu_max) { - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rty_cnt_0; } else { - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rtu_max; } - /* End of MinMax: '/MinMax' */ + /* End of MinMax: '/MinMax' */ return rty_cnt_0; } /* * System reset for atomic system: - * '/PI_clamp_fixdt_id' - * '/PI_clamp_fixdt_iq' + * '/PI_clamp_fixdt_id' + * '/PI_clamp_fixdt_iq' */ void PI_clamp_fixdt_Reset(DW_PI_clamp_fixdt *localDW) { - /* InitializeConditions for UnitDelay: '/UnitDelay1' */ + /* InitializeConditions for UnitDelay: '/UnitDelay1' */ localDW->UnitDelay1_DSTATE = false; - /* InitializeConditions for UnitDelay: '/UnitDelay' */ + /* InitializeConditions for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = 0; } /* * Output and update for atomic system: - * '/PI_clamp_fixdt_id' - * '/PI_clamp_fixdt_iq' + * '/PI_clamp_fixdt_id' + * '/PI_clamp_fixdt_iq' */ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T rtu_satMax, int16_T rtu_satMin, int32_T rtu_ext_limProt, @@ -300,8 +307,8 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T int32_T tmp; int16_T tmp_0; - /* Sum: '/Sum2' incorporates: - * Product: '/Divide2' + /* Sum: '/Sum2' incorporates: + * Product: '/Divide2' */ q0 = rtu_err * rtu_I; if ((q0 < 0) && (rtu_ext_limProt < MIN_int32_T - q0)) { @@ -312,10 +319,10 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T q0 += rtu_ext_limProt; } - /* Switch: '/Switch1' incorporates: - * Constant: '/a_elecPeriod1' - * Sum: '/Sum2' - * UnitDelay: '/UnitDelay1' + /* Switch: '/Switch1' incorporates: + * Constant: '/a_elecPeriod1' + * Sum: '/Sum2' + * UnitDelay: '/UnitDelay1' */ if (localDW->UnitDelay1_DSTATE) { tmp = 0; @@ -323,14 +330,14 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T tmp = q0; } - /* End of Switch: '/Switch1' */ + /* End of Switch: '/Switch1' */ - /* Sum: '/Sum1' incorporates: - * UnitDelay: '/UnitDelay' + /* Sum: '/Sum1' incorporates: + * UnitDelay: '/UnitDelay' */ rtb_Sum1_b4 = tmp + localDW->UnitDelay_DSTATE; - /* Product: '/Divide5' */ + /* Product: '/Divide5' */ tmp = (rtu_err * rtu_P) >> 11; if (tmp > 32767) { tmp = 32767; @@ -340,9 +347,9 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T } } - /* Sum: '/Sum1' incorporates: - * DataTypeConversion: '/Data Type Conversion1' - * Product: '/Divide5' + /* Sum: '/Sum1' incorporates: + * DataTypeConversion: '/Data Type Conversion1' + * Product: '/Divide5' */ tmp = (((rtb_Sum1_b4 >> 16) << 1) + tmp) >> 1; if (tmp > 32767) { @@ -353,33 +360,33 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T } } - /* RelationalOperator: '/LowerRelop1' incorporates: - * Sum: '/Sum1' + /* RelationalOperator: '/LowerRelop1' incorporates: + * Sum: '/Sum1' */ rtb_LowerRelop1_e = ((int16_T)tmp > rtu_satMax); - /* RelationalOperator: '/UpperRelop' incorporates: - * Sum: '/Sum1' + /* RelationalOperator: '/UpperRelop' incorporates: + * Sum: '/Sum1' */ rtb_UpperRelop_f = ((int16_T)tmp < rtu_satMin); - /* Switch: '/Switch1' incorporates: - * Sum: '/Sum1' - * Switch: '/Switch3' + /* Switch: '/Switch1' incorporates: + * Sum: '/Sum1' + * Switch: '/Switch3' */ if (rtb_LowerRelop1_e) { *rty_out = rtu_satMax; } else if (rtb_UpperRelop_f) { - /* Switch: '/Switch3' */ + /* Switch: '/Switch3' */ *rty_out = rtu_satMin; } else { *rty_out = (int16_T)tmp; } - /* End of Switch: '/Switch1' */ + /* End of Switch: '/Switch1' */ - /* Signum: '/SignDeltaU2' incorporates: - * Sum: '/Sum2' + /* Signum: '/SignDeltaU2' incorporates: + * Sum: '/Sum2' */ if (q0 < 0) { q0 = -1; @@ -387,10 +394,10 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T q0 = (q0 > 0); } - /* End of Signum: '/SignDeltaU2' */ + /* End of Signum: '/SignDeltaU2' */ - /* Signum: '/SignDeltaU3' incorporates: - * Sum: '/Sum1' + /* Signum: '/SignDeltaU3' incorporates: + * Sum: '/Sum1' */ if ((int16_T)tmp < 0) { tmp_0 = -1; @@ -398,86 +405,86 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T tmp_0 = (int16_T)((int16_T)tmp > 0); } - /* End of Signum: '/SignDeltaU3' */ + /* End of Signum: '/SignDeltaU3' */ - /* Update for UnitDelay: '/UnitDelay1' incorporates: - * DataTypeConversion: '/DataTypeConv4' - * Logic: '/AND1' + /* Update for UnitDelay: '/UnitDelay1' incorporates: + * DataTypeConversion: '/DataTypeConv4' * Logic: '/AND1' - * RelationalOperator: '/Equal1' + * Logic: '/AND1' + * RelationalOperator: '/Equal1' */ localDW->UnitDelay1_DSTATE = ((q0 == tmp_0) && (rtb_LowerRelop1_e || rtb_UpperRelop_f)); - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rtb_Sum1_b4; } -/* System reset for atomic system: '/Low_Pass_Filter' */ +/* System reset for atomic system: '/Low_Pass_Filter' */ void Low_Pass_Filter_Reset(DW_Low_Pass_Filter *localDW) { - /* InitializeConditions for UnitDelay: '/UnitDelay3' */ + /* InitializeConditions for UnitDelay: '/UnitDelay3' */ localDW->UnitDelay3_DSTATE[0] = 0; localDW->UnitDelay3_DSTATE[1] = 0; } -/* Output and update for atomic system: '/Low_Pass_Filter' */ +/* Output and update for atomic system: '/Low_Pass_Filter' */ void Low_Pass_Filter(const int16_T rtu_u[2], uint16_T rtu_coef, int16_T rty_y[2], DW_Low_Pass_Filter *localDW) { uint16_T rtb_Sum5; - /* Sum: '/Sum5' */ + /* Sum: '/Sum5' */ rtb_Sum5 = (uint16_T)(65535U - rtu_coef); - /* Sum: '/Sum1' incorporates: - * Product: '/Divide1' - * Product: '/Divide2' - * UnitDelay: '/UnitDelay3' + /* Sum: '/Sum1' incorporates: + * Product: '/Divide1' + * Product: '/Divide2' + * UnitDelay: '/UnitDelay3' */ rty_y[0] = (int16_T)(((rtu_u[0] * rtu_coef) >> 16) + ((localDW->UnitDelay3_DSTATE[0] * rtb_Sum5) >> 16)); - /* Update for UnitDelay: '/UnitDelay3' */ + /* Update for UnitDelay: '/UnitDelay3' */ localDW->UnitDelay3_DSTATE[0] = rty_y[0]; - /* Sum: '/Sum1' incorporates: - * Product: '/Divide1' - * Product: '/Divide2' - * UnitDelay: '/UnitDelay3' + /* Sum: '/Sum1' incorporates: + * Product: '/Divide1' + * Product: '/Divide2' + * UnitDelay: '/UnitDelay3' */ rty_y[1] = (int16_T)(((rtu_u[1] * rtu_coef) >> 16) + ((localDW->UnitDelay3_DSTATE[1] * rtb_Sum5) >> 16)); - /* Update for UnitDelay: '/UnitDelay3' */ + /* Update for UnitDelay: '/UnitDelay3' */ localDW->UnitDelay3_DSTATE[1] = rty_y[1]; } -/* System reset for atomic system: '/PI_clamp_fixdt_n' */ +/* System reset for atomic system: '/PI_clamp_fixdt_n' */ void PI_clamp_fixdt_n_Reset(DW_PI_clamp_fixdt_c *localDW) { - /* InitializeConditions for UnitDelay: '/UnitDelay1' */ + /* InitializeConditions for UnitDelay: '/UnitDelay1' */ localDW->UnitDelay1_DSTATE = false; - /* InitializeConditions for UnitDelay: '/UnitDelay' */ + /* InitializeConditions for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = 0; } -/* Output and update for atomic system: '/PI_clamp_fixdt_n' */ +/* Output and update for atomic system: '/PI_clamp_fixdt_n' */ int16_T PI_clamp_fixdt_n(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T rtu_satMax, int16_T rtu_satMin, int16_T rtu_ext_limProt, DW_PI_clamp_fixdt_c *localDW) { boolean_T rtb_LowerRelop1_ge; - boolean_T rtb_UpperRelop_cm; + boolean_T rtb_UpperRelop_o; int32_T rtb_Sum1_mz; int32_T q0; int32_T q1; int16_T tmp; int16_T rty_out_0; - /* Sum: '/Sum2' incorporates: - * Product: '/Divide2' + /* Sum: '/Sum2' incorporates: + * Product: '/Divide2' */ q0 = rtu_err * rtu_I; q1 = rtu_ext_limProt << 10; @@ -489,10 +496,10 @@ int16_T PI_clamp_fixdt_n(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, q0 += q1; } - /* Switch: '/Switch1' incorporates: - * Constant: '/a_elecPeriod1' - * Sum: '/Sum2' - * UnitDelay: '/UnitDelay1' + /* Switch: '/Switch1' incorporates: + * Constant: '/a_elecPeriod1' + * Sum: '/Sum2' + * UnitDelay: '/UnitDelay1' */ if (localDW->UnitDelay1_DSTATE) { q1 = 0; @@ -500,14 +507,14 @@ int16_T PI_clamp_fixdt_n(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, q1 = q0; } - /* End of Switch: '/Switch1' */ + /* End of Switch: '/Switch1' */ - /* Sum: '/Sum1' incorporates: - * UnitDelay: '/UnitDelay' + /* Sum: '/Sum1' incorporates: + * UnitDelay: '/UnitDelay' */ rtb_Sum1_mz = q1 + localDW->UnitDelay_DSTATE; - /* Product: '/Divide5' */ + /* Product: '/Divide5' */ q1 = (rtu_err * rtu_P) >> 11; if (q1 > 32767) { q1 = 32767; @@ -517,9 +524,9 @@ int16_T PI_clamp_fixdt_n(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, } } - /* Sum: '/Sum1' incorporates: - * DataTypeConversion: '/Data Type Conversion1' - * Product: '/Divide5' + /* Sum: '/Sum1' incorporates: + * DataTypeConversion: '/Data Type Conversion1' + * Product: '/Divide5' */ q1 = (((rtb_Sum1_mz >> 16) << 1) + q1) >> 1; if (q1 > 32767) { @@ -530,33 +537,33 @@ int16_T PI_clamp_fixdt_n(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, } } - /* RelationalOperator: '/LowerRelop1' incorporates: - * Sum: '/Sum1' + /* RelationalOperator: '/LowerRelop1' incorporates: + * Sum: '/Sum1' */ rtb_LowerRelop1_ge = ((int16_T)q1 > rtu_satMax); - /* RelationalOperator: '/UpperRelop' incorporates: - * Sum: '/Sum1' + /* RelationalOperator: '/UpperRelop' incorporates: + * Sum: '/Sum1' */ - rtb_UpperRelop_cm = ((int16_T)q1 < rtu_satMin); + rtb_UpperRelop_o = ((int16_T)q1 < rtu_satMin); - /* Switch: '/Switch1' incorporates: - * Sum: '/Sum1' - * Switch: '/Switch3' + /* Switch: '/Switch1' incorporates: + * Sum: '/Sum1' + * Switch: '/Switch3' */ if (rtb_LowerRelop1_ge) { rty_out_0 = rtu_satMax; - } else if (rtb_UpperRelop_cm) { - /* Switch: '/Switch3' */ + } else if (rtb_UpperRelop_o) { + /* Switch: '/Switch3' */ rty_out_0 = rtu_satMin; } else { rty_out_0 = (int16_T)q1; } - /* End of Switch: '/Switch1' */ + /* End of Switch: '/Switch1' */ - /* Signum: '/SignDeltaU2' incorporates: - * Sum: '/Sum2' + /* Signum: '/SignDeltaU2' incorporates: + * Sum: '/Sum2' */ if (q0 < 0) { q0 = -1; @@ -564,10 +571,10 @@ int16_T PI_clamp_fixdt_n(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, q0 = (q0 > 0); } - /* End of Signum: '/SignDeltaU2' */ + /* End of Signum: '/SignDeltaU2' */ - /* Signum: '/SignDeltaU3' incorporates: - * Sum: '/Sum1' + /* Signum: '/SignDeltaU3' incorporates: + * Sum: '/Sum1' */ if ((int16_T)q1 < 0) { tmp = -1; @@ -575,37 +582,37 @@ int16_T PI_clamp_fixdt_n(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, tmp = (int16_T)((int16_T)q1 > 0); } - /* End of Signum: '/SignDeltaU3' */ + /* End of Signum: '/SignDeltaU3' */ - /* Update for UnitDelay: '/UnitDelay1' incorporates: - * DataTypeConversion: '/DataTypeConv4' - * Logic: '/AND1' - * Logic: '/AND1' - * RelationalOperator: '/Equal1' + /* Update for UnitDelay: '/UnitDelay1' incorporates: + * DataTypeConversion: '/DataTypeConv4' + * Logic: '/AND1' + * Logic: '/AND1' + * RelationalOperator: '/Equal1' */ localDW->UnitDelay1_DSTATE = ((q0 == tmp) && (rtb_LowerRelop1_ge || - rtb_UpperRelop_cm)); + rtb_UpperRelop_o)); - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rtb_Sum1_mz; return rty_out_0; } /* * System initialize for atomic system: - * '/Counter' - * '/Counter' + * '/Counter' + * '/Counter' */ void Counter_b_Init(DW_Counter_l *localDW, uint16_T rtp_z_cntInit) { - /* InitializeConditions for UnitDelay: '/UnitDelay' */ + /* InitializeConditions for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rtp_z_cntInit; } /* * Output and update for atomic system: - * '/Counter' - * '/Counter' + * '/Counter' + * '/Counter' */ uint16_T Counter_i(uint16_T rtu_inc, uint16_T rtu_max, boolean_T rtu_rst, DW_Counter_l *localDW) @@ -613,9 +620,9 @@ uint16_T Counter_i(uint16_T rtu_inc, uint16_T rtu_max, boolean_T rtu_rst, uint16_T rtu_rst_0; uint16_T rty_cnt_0; - /* Switch: '/Switch1' incorporates: - * Constant: '/Constant23' - * UnitDelay: '/UnitDelay' + /* Switch: '/Switch1' incorporates: + * Constant: '/Constant23' + * UnitDelay: '/UnitDelay' */ if (rtu_rst) { rtu_rst_0 = 0U; @@ -623,50 +630,50 @@ uint16_T Counter_i(uint16_T rtu_inc, uint16_T rtu_max, boolean_T rtu_rst, rtu_rst_0 = localDW->UnitDelay_DSTATE; } - /* End of Switch: '/Switch1' */ + /* End of Switch: '/Switch1' */ - /* Sum: '/Sum1' */ + /* Sum: '/Sum1' */ rty_cnt_0 = (uint16_T)((uint32_T)rtu_inc + rtu_rst_0); - /* MinMax: '/MinMax' */ + /* MinMax: '/MinMax' */ if (rty_cnt_0 < rtu_max) { - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rty_cnt_0; } else { - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rtu_max; } - /* End of MinMax: '/MinMax' */ + /* End of MinMax: '/MinMax' */ return rty_cnt_0; } /* * System reset for atomic system: - * '/either_edge' + * '/either_edge' * '/either_edge' */ void either_edge_Reset(DW_either_edge *localDW) { - /* InitializeConditions for UnitDelay: '/UnitDelay' */ + /* InitializeConditions for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = false; } /* * Output and update for atomic system: - * '/either_edge' + * '/either_edge' * '/either_edge' */ boolean_T either_edge(boolean_T rtu_u, DW_either_edge *localDW) { boolean_T rty_y_0; - /* RelationalOperator: '/Relational Operator' incorporates: - * UnitDelay: '/UnitDelay' + /* RelationalOperator: '/Relational Operator' incorporates: + * UnitDelay: '/UnitDelay' */ rty_y_0 = (rtu_u != localDW->UnitDelay_DSTATE); - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = rtu_u; return rty_y_0; } @@ -674,35 +681,35 @@ boolean_T either_edge(boolean_T rtu_u, DW_either_edge *localDW) /* System initialize for atomic system: '/Debounce_Filter' */ void Debounce_Filter_Init(DW_Debounce_Filter *localDW) { - /* SystemInitialize for IfAction SubSystem: '/Qualification' */ + /* SystemInitialize for IfAction SubSystem: '/Qualification' */ - /* SystemInitialize for Atomic SubSystem: '/Counter' */ + /* SystemInitialize for Atomic SubSystem: '/Counter' */ Counter_b_Init(&localDW->Counter_i0, 0U); - /* End of SystemInitialize for SubSystem: '/Counter' */ + /* End of SystemInitialize for SubSystem: '/Counter' */ - /* End of SystemInitialize for SubSystem: '/Qualification' */ + /* End of SystemInitialize for SubSystem: '/Qualification' */ - /* SystemInitialize for IfAction SubSystem: '/Dequalification' */ + /* SystemInitialize for IfAction SubSystem: '/Dequalification' */ - /* SystemInitialize for Atomic SubSystem: '/Counter' */ + /* SystemInitialize for Atomic SubSystem: '/Counter' */ Counter_b_Init(&localDW->Counter_h, 0U); - /* End of SystemInitialize for SubSystem: '/Counter' */ + /* End of SystemInitialize for SubSystem: '/Counter' */ - /* End of SystemInitialize for SubSystem: '/Dequalification' */ + /* End of SystemInitialize for SubSystem: '/Dequalification' */ } /* System reset for atomic system: '/Debounce_Filter' */ void Debounce_Filter_Reset(DW_Debounce_Filter *localDW) { - /* InitializeConditions for UnitDelay: '/UnitDelay' */ + /* InitializeConditions for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = false; - /* SystemReset for Atomic SubSystem: '/either_edge' */ + /* SystemReset for Atomic SubSystem: '/either_edge' */ either_edge_Reset(&localDW->either_edge_k); - /* End of SystemReset for SubSystem: '/either_edge' */ + /* End of SystemReset for SubSystem: '/either_edge' */ } /* Output and update for atomic system: '/Debounce_Filter' */ @@ -712,69 +719,69 @@ void Debounce_Filter(boolean_T rtu_u, uint16_T rtu_tAcv, uint16_T rtu_tDeacv, uint16_T rtb_Sum1_l; boolean_T rtb_RelationalOperator_f; - /* Outputs for Atomic SubSystem: '/either_edge' */ + /* Outputs for Atomic SubSystem: '/either_edge' */ rtb_RelationalOperator_f = either_edge(rtu_u, &localDW->either_edge_k); - /* End of Outputs for SubSystem: '/either_edge' */ + /* End of Outputs for SubSystem: '/either_edge' */ - /* If: '/If2' incorporates: - * Constant: '/Constant6' - * Constant: '/Constant6' - * Inport: '/yPrev' - * Logic: '/Logical Operator1' - * Logic: '/Logical Operator2' - * Logic: '/Logical Operator3' - * Logic: '/Logical Operator4' - * UnitDelay: '/UnitDelay' + /* If: '/If2' incorporates: + * Constant: '/Constant6' + * Constant: '/Constant6' + * Inport: '/yPrev' + * Logic: '/Logical Operator1' + * Logic: '/Logical Operator2' + * Logic: '/Logical Operator3' + * Logic: '/Logical Operator4' + * UnitDelay: '/UnitDelay' */ if (rtu_u && (!localDW->UnitDelay_DSTATE)) { - /* Outputs for IfAction SubSystem: '/Qualification' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Qualification' incorporates: + * ActionPort: '/Action Port' */ - /* Outputs for Atomic SubSystem: '/Counter' */ + /* Outputs for Atomic SubSystem: '/Counter' */ rtb_Sum1_l = (uint16_T) Counter_i(1U, rtu_tAcv, rtb_RelationalOperator_f, &localDW->Counter_i0); - /* End of Outputs for SubSystem: '/Counter' */ + /* End of Outputs for SubSystem: '/Counter' */ - /* Switch: '/Switch2' incorporates: - * Constant: '/Constant6' - * RelationalOperator: '/Relational Operator2' + /* Switch: '/Switch2' incorporates: + * Constant: '/Constant6' + * RelationalOperator: '/Relational Operator2' */ *rty_y = ((rtb_Sum1_l > rtu_tAcv) || localDW->UnitDelay_DSTATE); - /* End of Outputs for SubSystem: '/Qualification' */ + /* End of Outputs for SubSystem: '/Qualification' */ } else if ((!rtu_u) && localDW->UnitDelay_DSTATE) { - /* Outputs for IfAction SubSystem: '/Dequalification' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Dequalification' incorporates: + * ActionPort: '/Action Port' */ - /* Outputs for Atomic SubSystem: '/Counter' */ + /* Outputs for Atomic SubSystem: '/Counter' */ rtb_Sum1_l = (uint16_T) Counter_i(1U, rtu_tDeacv, rtb_RelationalOperator_f, &localDW->Counter_h); - /* End of Outputs for SubSystem: '/Counter' */ + /* End of Outputs for SubSystem: '/Counter' */ - /* Switch: '/Switch2' incorporates: - * Constant: '/Constant6' - * RelationalOperator: '/Relational Operator2' + /* Switch: '/Switch2' incorporates: + * Constant: '/Constant6' + * RelationalOperator: '/Relational Operator2' */ *rty_y = ((!(rtb_Sum1_l > rtu_tDeacv)) && localDW->UnitDelay_DSTATE); - /* End of Outputs for SubSystem: '/Dequalification' */ + /* End of Outputs for SubSystem: '/Dequalification' */ } else { - /* Outputs for IfAction SubSystem: '/Default' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Default' incorporates: + * ActionPort: '/Action Port' */ *rty_y = localDW->UnitDelay_DSTATE; - /* End of Outputs for SubSystem: '/Default' */ + /* End of Outputs for SubSystem: '/Default' */ } - /* End of If: '/If2' */ + /* End of If: '/If2' */ - /* Update for UnitDelay: '/UnitDelay' */ + /* Update for UnitDelay: '/UnitDelay' */ localDW->UnitDelay_DSTATE = *rty_y; } @@ -792,13 +799,17 @@ void BLDC_controller_step(RT_MODEL *const rtM) boolean_T rtb_RelationalOperator1_m; uint8_T rtb_Sum_l; uint8_T rtb_r_fieldWeak_XA_o1; - int16_T rtb_Merge; - int16_T rtb_Switch2_fv; + int16_T rtb_Switch2_k; int16_T rtb_Abs5; + int16_T rtb_Switch2_fl; int16_T rtb_Switch1_l; - int16_T rtb_Switch2_d; - uint16_T rtb_Divide2_h; + int16_T rtb_Merge; + int16_T rtb_Merge1; + int32_T rtb_DataTypeConversion; int16_T rtb_Saturation; + int32_T rtb_Switch1; + int32_T rtb_Sum1; + int32_T rtb_Gain3; int16_T rtb_toNegative; int16_T rtb_Gain4; uint8_T rtb_r_fieldWeak_XA_o2; @@ -806,17 +817,13 @@ void BLDC_controller_step(RT_MODEL *const rtM) int16_T rtb_id_fieldWeak_M1; int16_T rtb_MinMax2; int16_T rtb_TmpSignalConversionAtLow_Pa[2]; - int32_T rtb_DataTypeConversion; - int32_T rtb_Switch1; - int32_T rtb_Sum1; - int32_T rtb_Gain3; int16_T tmp[4]; int8_T UnitDelay3; /* Outputs for Atomic SubSystem: '/BLDC_controller' */ - /* Sum: '/Sum' incorporates: - * Gain: '/g_Ha' - * Gain: '/g_Hb' + /* Sum: '/Sum' incorporates: + * Gain: '/g_Ha' + * Gain: '/g_Hb' * Inport: '/b_hallA ' * Inport: '/b_hallB' * Inport: '/b_hallC' @@ -824,128 +831,46 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtb_Sum = (uint8_T)((uint32_T)(uint8_T)((uint32_T)(uint8_T)(rtU->b_hallA << 2) + (uint8_T)(rtU->b_hallB << 1)) + rtU->b_hallC); - /* Saturate: '/Saturation2' incorporates: - * Inport: '/r_inpTgt' - */ - rtb_Gain3 = rtU->r_inpTgt << 4; - - /* If: '/If1' incorporates: - * Constant: '/z_ctrlTypSel1' - * Inport: '/r_inpTgt' - * Inport: '/r_inpTgt' - * Saturate: '/Saturation2' - */ - if (rtP->z_ctrlTypSel == 1) { - /* Outputs for IfAction SubSystem: '/FOC_Control_Type' incorporates: - * ActionPort: '/Action Port' - */ - /* SignalConversion: '/TmpSignal ConversionAtSelectorInport1' incorporates: - * Constant: '/Vd_max' - * Constant: '/constant1' - * Constant: '/i_max' - * Constant: '/n_max' - */ - tmp[0] = 0; - tmp[1] = rtP->Vd_max; - tmp[2] = rtP->n_max; - tmp[3] = rtP->i_max; - - /* End of Outputs for SubSystem: '/FOC_Control_Type' */ - - /* Saturate: '/Saturation2' incorporates: - * Inport: '/r_inpTgt' - */ - if (rtb_Gain3 >= 16000) { - rtb_toNegative = 16000; - } else if (rtb_Gain3 <= -16000) { - rtb_toNegative = -16000; - } else { - rtb_toNegative = (int16_T)(rtU->r_inpTgt << 4); - } - - /* Outputs for IfAction SubSystem: '/FOC_Control_Type' incorporates: - * ActionPort: '/Action Port' - */ - /* Product: '/Divide1' incorporates: - * Inport: '/z_ctrlModReq' - * Product: '/Divide4' - * Selector: '/Selector' - */ - rtb_Merge = (int16_T)(((uint16_T)((tmp[rtU->z_ctrlModReq] << 5) / 125) * - rtb_toNegative) >> 12); - - /* End of Outputs for SubSystem: '/FOC_Control_Type' */ - } else if (rtb_Gain3 >= 16000) { - /* Outputs for IfAction SubSystem: '/Commutation_Control_Type' incorporates: - * ActionPort: '/Action Port' - */ - /* Saturate: '/Saturation2' incorporates: - * Inport: '/r_inpTgt' - */ - rtb_Merge = 16000; - - /* End of Outputs for SubSystem: '/Commutation_Control_Type' */ - } else if (rtb_Gain3 <= -16000) { - /* Outputs for IfAction SubSystem: '/Commutation_Control_Type' incorporates: - * ActionPort: '/Action Port' - */ - /* Saturate: '/Saturation2' incorporates: - * Inport: '/r_inpTgt' - */ - rtb_Merge = -16000; - - /* End of Outputs for SubSystem: '/Commutation_Control_Type' */ - } else { - /* Outputs for IfAction SubSystem: '/Commutation_Control_Type' incorporates: - * ActionPort: '/Action Port' - */ - rtb_Merge = (int16_T)(rtU->r_inpTgt << 4); - - /* End of Outputs for SubSystem: '/Commutation_Control_Type' */ - } - - /* End of If: '/If1' */ - - /* Logic: '/Logical Operator' incorporates: + /* Logic: '/Logical Operator' incorporates: * Inport: '/b_hallA ' * Inport: '/b_hallB' * Inport: '/b_hallC' - * UnitDelay: '/UnitDelay1' - * UnitDelay: '/UnitDelay2' - * UnitDelay: '/UnitDelay3' + * UnitDelay: '/UnitDelay1' + * UnitDelay: '/UnitDelay2' + * UnitDelay: '/UnitDelay3' */ rtb_LogicalOperator = (boolean_T)((rtU->b_hallA != 0) ^ (rtU->b_hallB != 0) ^ (rtU->b_hallC != 0) ^ (rtDW->UnitDelay3_DSTATE_fy != 0) ^ (rtDW->UnitDelay1_DSTATE != 0)) ^ (rtDW->UnitDelay2_DSTATE_f != 0); - /* If: '/If2' incorporates: + /* If: '/If2' incorporates: * If: '/If2' - * Inport: '/z_counterRawPrev' - * UnitDelay: '/UnitDelay3' + * Inport: '/z_counterRawPrev' + * UnitDelay: '/UnitDelay3' */ if (rtb_LogicalOperator) { - /* Outputs for IfAction SubSystem: '/F01_04_Direction_Detection' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/F01_03_Direction_Detection' incorporates: + * ActionPort: '/Action Port' */ - /* UnitDelay: '/UnitDelay3' */ + /* UnitDelay: '/UnitDelay3' */ UnitDelay3 = rtDW->Switch2; - /* Sum: '/Sum2' incorporates: - * Constant: '/vec_hallToPos' - * Selector: '/Selector' - * UnitDelay: '/UnitDelay2' + /* Sum: '/Sum2' incorporates: + * Constant: '/vec_hallToPos' + * Selector: '/Selector' + * UnitDelay: '/UnitDelay2' */ rtb_Sum2_h = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Sum] - rtDW->UnitDelay2_DSTATE_b); - /* Switch: '/Switch2' incorporates: - * Constant: '/Constant20' - * Constant: '/Constant23' - * Constant: '/Constant24' - * Constant: '/Constant8' - * Logic: '/Logical Operator3' - * RelationalOperator: '/Relational Operator1' - * RelationalOperator: '/Relational Operator6' + /* Switch: '/Switch2' incorporates: + * Constant: '/Constant20' + * Constant: '/Constant23' + * Constant: '/Constant24' + * Constant: '/Constant8' + * Logic: '/Logical Operator3' + * RelationalOperator: '/Relational Operator1' + * RelationalOperator: '/Relational Operator6' */ if ((rtb_Sum2_h == 1) || (rtb_Sum2_h == -5)) { rtDW->Switch2 = 1; @@ -953,39 +878,38 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->Switch2 = -1; } - /* End of Switch: '/Switch2' */ + /* End of Switch: '/Switch2' */ - /* Update for UnitDelay: '/UnitDelay2' incorporates: - * Constant: '/vec_hallToPos' - * Selector: '/Selector' + /* Update for UnitDelay: '/UnitDelay2' incorporates: + * Constant: '/vec_hallToPos' + * Selector: '/Selector' */ rtDW->UnitDelay2_DSTATE_b = rtConstP.vec_hallToPos_Value[rtb_Sum]; - /* End of Outputs for SubSystem: '/F01_04_Direction_Detection' */ + /* End of Outputs for SubSystem: '/F01_03_Direction_Detection' */ - /* Outputs for IfAction SubSystem: '/Raw_Motor_Speed_Estimation' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Raw_Motor_Speed_Estimation' incorporates: + * ActionPort: '/Action Port' */ rtDW->z_counterRawPrev = rtDW->UnitDelay3_DSTATE; - /* Sum: '/Sum7' incorporates: - * Inport: '/z_counterRawPrev' - * UnitDelay: '/UnitDelay3' - * UnitDelay: '/UnitDelay4' + /* Sum: '/Sum7' incorporates: + * Inport: '/z_counterRawPrev' + * UnitDelay: '/UnitDelay3' + * UnitDelay: '/UnitDelay4' */ - rtb_Switch2_fv = (int16_T)(rtDW->z_counterRawPrev - - rtDW->UnitDelay4_DSTATE_p); + rtb_Switch2_k = (int16_T)(rtDW->z_counterRawPrev - rtDW->UnitDelay4_DSTATE); - /* Abs: '/Abs2' */ - if (rtb_Switch2_fv < 0) { - rtb_Switch1_l = (int16_T)-rtb_Switch2_fv; + /* Abs: '/Abs2' */ + if (rtb_Switch2_k < 0) { + rtb_Switch1_l = (int16_T)-rtb_Switch2_k; } else { - rtb_Switch1_l = rtb_Switch2_fv; + rtb_Switch1_l = rtb_Switch2_k; } - /* End of Abs: '/Abs2' */ + /* End of Abs: '/Abs2' */ - /* Relay: '/dz_cntTrnsDet' */ + /* Relay: '/dz_cntTrnsDet' */ if (rtb_Switch1_l >= rtP->dz_cntTrnsDetHi) { rtDW->dz_cntTrnsDet_Mode = true; } else { @@ -996,111 +920,111 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->dz_cntTrnsDet = rtDW->dz_cntTrnsDet_Mode; - /* End of Relay: '/dz_cntTrnsDet' */ + /* End of Relay: '/dz_cntTrnsDet' */ - /* RelationalOperator: '/Relational Operator4' */ + /* RelationalOperator: '/Relational Operator4' */ rtb_RelationalOperator4_d = (rtDW->Switch2 != UnitDelay3); - /* Switch: '/Switch3' incorporates: - * Constant: '/Constant4' - * Logic: '/Logical Operator1' - * Switch: '/Switch1' - * Switch: '/Switch2' - * UnitDelay: '/UnitDelay1' + /* Switch: '/Switch3' incorporates: + * Constant: '/Constant4' + * Logic: '/Logical Operator1' + * Switch: '/Switch1' + * Switch: '/Switch2' + * UnitDelay: '/UnitDelay1' */ if (rtb_RelationalOperator4_d && rtDW->UnitDelay1_DSTATE_n) { rtb_Switch1_l = 0; } else if (rtb_RelationalOperator4_d) { - /* Switch: '/Switch2' incorporates: - * UnitDelay: '/UnitDelay4' + /* Switch: '/Switch2' incorporates: + * UnitDelay: '/UnitDelay4' */ rtb_Switch1_l = rtDW->UnitDelay4_DSTATE_e; } else if (rtDW->dz_cntTrnsDet) { - /* Switch: '/Switch1' incorporates: - * Constant: '/cf_speedCoef' - * Product: '/Divide14' - * Switch: '/Switch2' + /* Switch: '/Switch1' incorporates: + * Constant: '/cf_speedCoef' + * Product: '/Divide14' + * Switch: '/Switch2' */ rtb_Switch1_l = (int16_T)((rtP->cf_speedCoef << 4) / rtDW->z_counterRawPrev); } else { - /* Switch: '/Switch1' incorporates: - * Constant: '/cf_speedCoef' - * Gain: '/g_Ha' - * Product: '/Divide13' - * Sum: '/Sum13' - * Switch: '/Switch2' - * UnitDelay: '/UnitDelay2' - * UnitDelay: '/UnitDelay3' - * UnitDelay: '/UnitDelay5' + /* Switch: '/Switch1' incorporates: + * Constant: '/cf_speedCoef' + * Gain: '/g_Ha' + * Product: '/Divide13' + * Sum: '/Sum13' + * Switch: '/Switch2' + * UnitDelay: '/UnitDelay2' + * UnitDelay: '/UnitDelay3' + * UnitDelay: '/UnitDelay5' */ rtb_Switch1_l = (int16_T)(((uint16_T)(rtP->cf_speedCoef << 2) << 4) / (int16_T)(((rtDW->UnitDelay2_DSTATE + rtDW->UnitDelay3_DSTATE_o) + rtDW->UnitDelay5_DSTATE) + rtDW->z_counterRawPrev)); } - /* End of Switch: '/Switch3' */ + /* End of Switch: '/Switch3' */ - /* Product: '/Divide11' */ + /* Product: '/Divide11' */ rtDW->Divide11 = (int16_T)(rtb_Switch1_l * rtDW->Switch2); - /* Update for UnitDelay: '/UnitDelay4' */ - rtDW->UnitDelay4_DSTATE_p = rtDW->z_counterRawPrev; + /* Update for UnitDelay: '/UnitDelay4' */ + rtDW->UnitDelay4_DSTATE = rtDW->z_counterRawPrev; - /* Update for UnitDelay: '/UnitDelay2' incorporates: - * UnitDelay: '/UnitDelay3' + /* Update for UnitDelay: '/UnitDelay2' incorporates: + * UnitDelay: '/UnitDelay3' */ rtDW->UnitDelay2_DSTATE = rtDW->UnitDelay3_DSTATE_o; - /* Update for UnitDelay: '/UnitDelay3' incorporates: - * UnitDelay: '/UnitDelay5' + /* Update for UnitDelay: '/UnitDelay3' incorporates: + * UnitDelay: '/UnitDelay5' */ rtDW->UnitDelay3_DSTATE_o = rtDW->UnitDelay5_DSTATE; - /* Update for UnitDelay: '/UnitDelay5' */ + /* Update for UnitDelay: '/UnitDelay5' */ rtDW->UnitDelay5_DSTATE = rtDW->z_counterRawPrev; - /* Update for UnitDelay: '/UnitDelay1' */ + /* Update for UnitDelay: '/UnitDelay1' */ rtDW->UnitDelay1_DSTATE_n = rtb_RelationalOperator4_d; - /* End of Outputs for SubSystem: '/Raw_Motor_Speed_Estimation' */ + /* End of Outputs for SubSystem: '/Raw_Motor_Speed_Estimation' */ } - /* End of If: '/If2' */ + /* End of If: '/If2' */ - /* Outputs for Atomic SubSystem: '/Counter' */ + /* Outputs for Atomic SubSystem: '/Counter' */ - /* Constant: '/Constant6' incorporates: - * Constant: '/z_maxCntRst2' + /* Constant: '/Constant6' incorporates: + * Constant: '/z_maxCntRst2' */ rtb_Switch1_l = (int16_T) Counter(1, rtP->z_maxCntRst, rtb_LogicalOperator, &rtDW->Counter_e); - /* End of Outputs for SubSystem: '/Counter' */ + /* End of Outputs for SubSystem: '/Counter' */ - /* Switch: '/Switch2' incorporates: - * Constant: '/Constant4' - * Constant: '/z_maxCntRst' - * RelationalOperator: '/Relational Operator2' + /* Switch: '/Switch2' incorporates: + * Constant: '/Constant4' + * Constant: '/z_maxCntRst' + * RelationalOperator: '/Relational Operator2' */ if (rtb_Switch1_l > rtP->z_maxCntRst) { - rtb_Switch2_fv = 0; + rtb_Switch2_k = 0; } else { - rtb_Switch2_fv = rtDW->Divide11; + rtb_Switch2_k = rtDW->Divide11; } - /* End of Switch: '/Switch2' */ + /* End of Switch: '/Switch2' */ - /* Abs: '/Abs5' */ - if (rtb_Switch2_fv < 0) { - rtb_Abs5 = (int16_T)-rtb_Switch2_fv; + /* Abs: '/Abs5' */ + if (rtb_Switch2_k < 0) { + rtb_Abs5 = (int16_T)-rtb_Switch2_k; } else { - rtb_Abs5 = rtb_Switch2_fv; + rtb_Abs5 = rtb_Switch2_k; } - /* End of Abs: '/Abs5' */ + /* End of Abs: '/Abs5' */ - /* Relay: '/n_commDeacv' */ + /* Relay: '/n_commDeacv' */ if (rtb_Abs5 >= rtP->n_commDeacvHi) { rtDW->n_commDeacv_Mode = true; } else { @@ -1109,18 +1033,79 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Logic: '/Logical Operator2' incorporates: - * Constant: '/CTRL_COMM' - * Constant: '/z_ctrlTypSel1' - * Logic: '/Logical Operator1' - * RelationalOperator: '/Relational Operator3' - * Relay: '/n_commDeacv' + /* Logic: '/Logical Operator2' incorporates: + * Logic: '/Logical Operator1' + * Relay: '/n_commDeacv' */ - rtb_LogicalOperator = ((rtP->z_ctrlTypSel == 1) && rtDW->n_commDeacv_Mode && ( - !rtDW->dz_cntTrnsDet)); + rtb_LogicalOperator = (rtDW->n_commDeacv_Mode && (!rtDW->dz_cntTrnsDet)); - /* RelationalOperator: '/Relational Operator9' incorporates: - * Constant: '/n_stdStillDet' + /* Switch: '/Switch2' incorporates: + * Constant: '/Constant16' + * Product: '/Divide1' + * Product: '/Divide3' + * RelationalOperator: '/Relational Operator7' + * Sum: '/Sum3' + * Switch: '/Switch3' + */ + if (rtb_LogicalOperator) { + /* MinMax: '/MinMax' */ + rtb_Switch2_fl = rtb_Switch1_l; + if (!(rtb_Switch2_fl < rtDW->z_counterRawPrev)) { + rtb_Switch2_fl = rtDW->z_counterRawPrev; + } + + /* End of MinMax: '/MinMax' */ + + /* Switch: '/Switch3' incorporates: + * Constant: '/Constant16' + * Constant: '/vec_hallToPos' + * RelationalOperator: '/Relational Operator7' + * Selector: '/Selector' + * Sum: '/Sum1' + */ + if (rtDW->Switch2 == 1) { + rtb_Sum2_h = rtConstP.vec_hallToPos_Value[rtb_Sum]; + } else { + rtb_Sum2_h = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Sum] + 1); + } + + rtb_Switch2_fl = (int16_T)(((int16_T)((int16_T)((rtb_Switch2_fl << 14) / + rtDW->z_counterRawPrev) * rtDW->Switch2) + (rtb_Sum2_h << 14)) >> 2); + } else { + if (rtDW->Switch2 == 1) { + /* Switch: '/Switch3' incorporates: + * Constant: '/vec_hallToPos' + * Selector: '/Selector' + */ + rtb_Sum2_h = rtConstP.vec_hallToPos_Value[rtb_Sum]; + } else { + /* Switch: '/Switch3' incorporates: + * Constant: '/vec_hallToPos' + * Selector: '/Selector' + * Sum: '/Sum1' + */ + rtb_Sum2_h = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Sum] + 1); + } + + rtb_Switch2_fl = (int16_T)(rtb_Sum2_h << 12); + } + + /* End of Switch: '/Switch2' */ + + /* MinMax: '/MinMax1' incorporates: + * Constant: '/Constant1' + */ + if (!(rtb_Switch2_fl > 0)) { + rtb_Switch2_fl = 0; + } + + /* End of MinMax: '/MinMax1' */ + + /* Product: '/Divide2' */ + rtb_Switch2_fl = (int16_T)((15 * rtb_Switch2_fl) >> 4); + + /* RelationalOperator: '/Relational Operator9' incorporates: + * Constant: '/n_stdStillDet' */ rtb_RelationalOperator4_d = (rtb_Abs5 < rtP->n_stdStillDet); @@ -1195,16 +1180,16 @@ void BLDC_controller_step(RT_MODEL *const rtM) if ((rtDW->UnitDelay_DSTATE_c & 4) != 0) { rtb_RelationalOperator1_m = true; } else { - if (rtDW->UnitDelay4_DSTATE < 0) { + if (rtDW->UnitDelay4_DSTATE_eu < 0) { /* Abs: '/Abs4' incorporates: * UnitDelay: '/UnitDelay4' */ - rtb_toNegative = (int16_T)-rtDW->UnitDelay4_DSTATE; + rtb_toNegative = (int16_T)-rtDW->UnitDelay4_DSTATE_eu; } else { /* Abs: '/Abs4' incorporates: * UnitDelay: '/UnitDelay4' */ - rtb_toNegative = rtDW->UnitDelay4_DSTATE; + rtb_toNegative = rtDW->UnitDelay4_DSTATE_eu; } rtb_RelationalOperator1_m = ((rtb_toNegative > rtP->r_errInpTgtThres) && @@ -1264,16 +1249,16 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* End of If: '/If2' */ - /* Logic: '/Logical Operator4' incorporates: - * Constant: '/constant2' - * Constant: '/constant8' + /* Logic: '/Logical Operator4' incorporates: + * Constant: '/constant2' + * Constant: '/constant8' * Inport: '/b_motEna' * Inport: '/z_ctrlModReq' - * Logic: '/Logical Operator1' - * Logic: '/Logical Operator7' - * RelationalOperator: '/Relational Operator10' - * RelationalOperator: '/Relational Operator11' - * RelationalOperator: '/Relational Operator2' + * Logic: '/Logical Operator1' + * Logic: '/Logical Operator7' + * RelationalOperator: '/Relational Operator10' + * RelationalOperator: '/Relational Operator11' + * RelationalOperator: '/Relational Operator2' * UnitDelay: '/UnitDelay1' */ rtb_RelationalOperator1_m = ((!rtU->b_motEna) || rtDW->Merge_n || @@ -1281,20 +1266,20 @@ void BLDC_controller_step(RT_MODEL *const rtM) && (rtDW->UnitDelay1_DSTATE_p != 0))); /* Chart: '/F03_02_Control_Mode_Manager' incorporates: - * Constant: '/constant' - * Constant: '/constant1' - * Constant: '/constant5' - * Constant: '/constant6' - * Constant: '/constant7' + * Constant: '/constant' + * Constant: '/constant1' + * Constant: '/constant5' + * Constant: '/constant6' + * Constant: '/constant7' * Inport: '/z_ctrlModReq' - * Logic: '/Logical Operator3' - * Logic: '/Logical Operator6' - * Logic: '/Logical Operator9' - * RelationalOperator: '/Relational Operator1' - * RelationalOperator: '/Relational Operator3' - * RelationalOperator: '/Relational Operator4' - * RelationalOperator: '/Relational Operator5' - * RelationalOperator: '/Relational Operator6' + * Logic: '/Logical Operator3' + * Logic: '/Logical Operator6' + * Logic: '/Logical Operator9' + * RelationalOperator: '/Relational Operator1' + * RelationalOperator: '/Relational Operator3' + * RelationalOperator: '/Relational Operator4' + * RelationalOperator: '/Relational Operator5' + * RelationalOperator: '/Relational Operator6' */ if (rtDW->is_active_c1_BLDC_controller == 0U) { rtDW->is_active_c1_BLDC_controller = 1U; @@ -1341,70 +1326,212 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* End of Chart: '/F03_02_Control_Mode_Manager' */ - /* Switch: '/Switch2' incorporates: - * Constant: '/Constant16' - * Product: '/Divide1' - * Product: '/Divide3' - * RelationalOperator: '/Relational Operator7' - * Sum: '/Sum3' - * Switch: '/Switch3' + /* Saturate: '/Saturation2' incorporates: + * Inport: '/r_inpTgt' */ - if (rtb_LogicalOperator) { - /* MinMax: '/MinMax' */ - rtb_Switch2_d = rtb_Switch1_l; - if (!(rtb_Switch2_d < rtDW->z_counterRawPrev)) { - rtb_Switch2_d = rtDW->z_counterRawPrev; - } + rtb_Gain3 = rtU->r_inpTgt << 4; - /* End of MinMax: '/MinMax' */ - - /* Switch: '/Switch3' incorporates: - * Constant: '/Constant16' - * Constant: '/vec_hallToPos' - * RelationalOperator: '/Relational Operator7' - * Selector: '/Selector' - * Sum: '/Sum1' + /* If: '/If1' incorporates: + * Constant: '/z_ctrlTypSel1' + * Inport: '/r_inpTgt' + * Inport: '/r_inpTgt' + * Saturate: '/Saturation2' + */ + if (rtP->z_ctrlTypSel == 2) { + /* Outputs for IfAction SubSystem: '/FOC_Control_Type' incorporates: + * ActionPort: '/Action Port' */ - if (rtDW->Switch2 == 1) { - rtb_Sum2_h = rtConstP.vec_hallToPos_Value[rtb_Sum]; + /* SignalConversion: '/TmpSignal ConversionAtSelectorInport1' incorporates: + * Constant: '/Vd_max' + * Constant: '/constant1' + * Constant: '/i_max' + * Constant: '/n_max' + */ + tmp[0] = 0; + tmp[1] = rtP->Vd_max; + tmp[2] = rtP->n_max; + tmp[3] = rtP->i_max; + + /* End of Outputs for SubSystem: '/FOC_Control_Type' */ + + /* Saturate: '/Saturation2' incorporates: + * Inport: '/r_inpTgt' + */ + if (rtb_Gain3 >= 16000) { + rtb_toNegative = 16000; + } else if (rtb_Gain3 <= -16000) { + rtb_toNegative = -16000; } else { - rtb_Sum2_h = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Sum] + 1); + rtb_toNegative = (int16_T)(rtU->r_inpTgt << 4); } - rtb_Switch2_d = (int16_T)(((int16_T)((int16_T)((rtb_Switch2_d << 14) / - rtDW->z_counterRawPrev) * rtDW->Switch2) + (rtb_Sum2_h << 14)) >> 2); + /* Outputs for IfAction SubSystem: '/FOC_Control_Type' incorporates: + * ActionPort: '/Action Port' + */ + /* Product: '/Divide1' incorporates: + * Inport: '/z_ctrlModReq' + * Product: '/Divide4' + * Selector: '/Selector' + */ + rtb_Merge = (int16_T)(((uint16_T)((tmp[rtU->z_ctrlModReq] << 5) / 125) * + rtb_toNegative) >> 12); + + /* End of Outputs for SubSystem: '/FOC_Control_Type' */ + } else if (rtb_Gain3 >= 16000) { + /* Outputs for IfAction SubSystem: '/Default_Control_Type' incorporates: + * ActionPort: '/Action Port' + */ + /* Saturate: '/Saturation2' incorporates: + * Inport: '/r_inpTgt' + */ + rtb_Merge = 16000; + + /* End of Outputs for SubSystem: '/Default_Control_Type' */ + } else if (rtb_Gain3 <= -16000) { + /* Outputs for IfAction SubSystem: '/Default_Control_Type' incorporates: + * ActionPort: '/Action Port' + */ + /* Saturate: '/Saturation2' incorporates: + * Inport: '/r_inpTgt' + */ + rtb_Merge = -16000; + + /* End of Outputs for SubSystem: '/Default_Control_Type' */ } else { - if (rtDW->Switch2 == 1) { - /* Switch: '/Switch3' incorporates: - * Constant: '/vec_hallToPos' - * Selector: '/Selector' + /* Outputs for IfAction SubSystem: '/Default_Control_Type' incorporates: + * ActionPort: '/Action Port' + */ + rtb_Merge = (int16_T)(rtU->r_inpTgt << 4); + + /* End of Outputs for SubSystem: '/Default_Control_Type' */ + } + + /* End of If: '/If1' */ + + /* If: '/If2' incorporates: + * Inport: '/r_inpTgtScaRaw' + */ + rtb_Sum2_h = rtDW->If2_ActiveSubsystem_j; + UnitDelay3 = (int8_T)!(rtb_Sum_l == 0); + rtDW->If2_ActiveSubsystem_j = UnitDelay3; + switch (UnitDelay3) { + case 0: + if (UnitDelay3 != rtb_Sum2_h) { + /* SystemReset for IfAction SubSystem: '/Open_Mode' incorporates: + * ActionPort: '/Action Port' */ - rtb_Sum2_h = rtConstP.vec_hallToPos_Value[rtb_Sum]; - } else { - /* Switch: '/Switch3' incorporates: - * Constant: '/vec_hallToPos' - * Selector: '/Selector' - * Sum: '/Sum1' + /* SystemReset for Atomic SubSystem: '/rising_edge_init' */ + /* SystemReset for If: '/If2' incorporates: + * UnitDelay: '/UnitDelay' + * UnitDelay: '/UnitDelay' */ - rtb_Sum2_h = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Sum] + 1); + rtDW->UnitDelay_DSTATE_e = true; + + /* End of SystemReset for SubSystem: '/rising_edge_init' */ + + /* SystemReset for Atomic SubSystem: '/Rate_Limiter' */ + rtDW->UnitDelay_DSTATE = 0; + + /* End of SystemReset for SubSystem: '/Rate_Limiter' */ + /* End of SystemReset for SubSystem: '/Open_Mode' */ } - rtb_Switch2_d = (int16_T)(rtb_Sum2_h << 12); + /* Outputs for IfAction SubSystem: '/Open_Mode' incorporates: + * ActionPort: '/Action Port' + */ + /* DataTypeConversion: '/Data Type Conversion' incorporates: + * UnitDelay: '/UnitDelay4' + */ + rtb_Gain3 = rtDW->UnitDelay4_DSTATE_eu << 12; + rtb_DataTypeConversion = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | + -134217728 : rtb_Gain3 & 134217727; + + /* Outputs for Atomic SubSystem: '/rising_edge_init' */ + /* UnitDelay: '/UnitDelay' */ + rtb_RelationalOperator4_d = rtDW->UnitDelay_DSTATE_e; + + /* Update for UnitDelay: '/UnitDelay' incorporates: + * Constant: '/Constant' + */ + rtDW->UnitDelay_DSTATE_e = false; + + /* End of Outputs for SubSystem: '/rising_edge_init' */ + + /* Outputs for Atomic SubSystem: '/Rate_Limiter' */ + /* Switch: '/Switch1' incorporates: + * UnitDelay: '/UnitDelay' + */ + if (rtb_RelationalOperator4_d) { + rtb_Switch1 = rtb_DataTypeConversion; + } else { + rtb_Switch1 = rtDW->UnitDelay_DSTATE; + } + + /* End of Switch: '/Switch1' */ + + /* Sum: '/Sum1' */ + rtb_Gain3 = -rtb_Switch1; + rtb_Sum1 = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | -134217728 : rtb_Gain3 + & 134217727; + + /* Switch: '/Switch2' incorporates: + * Constant: '/dV_openRate' + * RelationalOperator: '/LowerRelop1' + */ + if (rtb_Sum1 > rtP->dV_openRate) { + rtb_Sum1 = rtP->dV_openRate; + } else { + /* Gain: '/Gain3' */ + rtb_Gain3 = -rtP->dV_openRate; + rtb_Gain3 = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | -134217728 : + rtb_Gain3 & 134217727; + + /* Switch: '/Switch' incorporates: + * RelationalOperator: '/UpperRelop' + */ + if (rtb_Sum1 < rtb_Gain3) { + rtb_Sum1 = rtb_Gain3; + } + + /* End of Switch: '/Switch' */ + } + + /* End of Switch: '/Switch2' */ + + /* Sum: '/Sum2' */ + rtb_Gain3 = rtb_Sum1 + rtb_Switch1; + rtb_Switch1 = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | -134217728 : + rtb_Gain3 & 134217727; + + /* Switch: '/Switch2' */ + if (rtb_RelationalOperator4_d) { + /* Update for UnitDelay: '/UnitDelay' */ + rtDW->UnitDelay_DSTATE = rtb_DataTypeConversion; + } else { + /* Update for UnitDelay: '/UnitDelay' */ + rtDW->UnitDelay_DSTATE = rtb_Switch1; + } + + /* End of Switch: '/Switch2' */ + /* End of Outputs for SubSystem: '/Rate_Limiter' */ + + /* DataTypeConversion: '/Data Type Conversion1' */ + rtb_Merge1 = (int16_T)(rtb_Switch1 >> 12); + + /* End of Outputs for SubSystem: '/Open_Mode' */ + break; + + case 1: + /* Outputs for IfAction SubSystem: '/Default_Mode' incorporates: + * ActionPort: '/Action Port' + */ + rtb_Merge1 = rtb_Merge; + + /* End of Outputs for SubSystem: '/Default_Mode' */ + break; } - /* End of Switch: '/Switch2' */ - - /* MinMax: '/MinMax1' incorporates: - * Constant: '/Constant1' - */ - if (!(rtb_Switch2_d > 0)) { - rtb_Switch2_d = 0; - } - - /* End of MinMax: '/MinMax1' */ - - /* Product: '/Divide2' */ - rtb_Divide2_h = (uint16_T)((15 * rtb_Switch2_d) >> 4); + /* End of If: '/If2' */ /* Saturate: '/Saturation' incorporates: * Inport: '/i_phaAB' @@ -1425,11 +1552,11 @@ void BLDC_controller_step(RT_MODEL *const rtM) */ rtb_Gain3 = rtU->i_phaBC << 4; if (rtb_Gain3 >= 32000) { - rtb_Switch2_d = 32000; + rtb_Merge = 32000; } else if (rtb_Gain3 <= -32000) { - rtb_Switch2_d = -32000; + rtb_Merge = -32000; } else { - rtb_Switch2_d = (int16_T)(rtU->i_phaBC << 4); + rtb_Merge = (int16_T)(rtU->i_phaBC << 4); } /* End of Saturate: '/Saturation1' */ @@ -1439,7 +1566,7 @@ void BLDC_controller_step(RT_MODEL *const rtM) */ rtb_Sum2_h = rtDW->If1_ActiveSubsystem; UnitDelay3 = -1; - if (rtP->z_ctrlTypSel == 1) { + if (rtP->z_ctrlTypSel == 2) { UnitDelay3 = 0; } @@ -1447,10 +1574,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) if ((rtb_Sum2_h != UnitDelay3) && (rtb_Sum2_h == 0)) { /* Disable for If: '/If2' */ if (rtDW->If2_ActiveSubsystem_a == 0) { - /* Disable for Outport: '/iq' */ + /* Disable for Outport: '/iq' */ rtDW->Sum1[0] = 0; - /* Disable for Outport: '/id' */ + /* Disable for Outport: '/id' */ rtDW->Sum1[1] = 0; } @@ -1460,7 +1587,7 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* Disable for If: '/If1' */ if (rtDW->If1_ActiveSubsystem_e == 0) { - /* Disable for Outport: '/Vd' */ + /* Disable for Outport: '/Vd' */ rtDW->Switch1 = 0; } @@ -1468,36 +1595,32 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* End of Disable for If: '/If1' */ - /* Disable for If: '/If1' */ + /* Disable for If: '/If1' */ if (rtDW->If1_ActiveSubsystem_f == 0) { - /* Disable for Outport: '/iq_limProt' */ + /* Disable for Outport: '/iq_limProt' */ rtDW->Divide4 = 0; } rtDW->If1_ActiveSubsystem_f = -1; - /* End of Disable for If: '/If1' */ + /* End of Disable for If: '/If1' */ - /* Disable for If: '/If2' */ + /* Disable for If: '/If2' */ if (rtDW->If2_ActiveSubsystem_c == 0) { - /* Disable for Outport: '/n_limProt' */ + /* Disable for Outport: '/n_limProt' */ rtDW->Divide1 = 0; } rtDW->If2_ActiveSubsystem_c = -1; - /* End of Disable for If: '/If2' */ + /* End of Disable for If: '/If2' */ /* Disable for SwitchCase: '/Switch Case' */ rtDW->SwitchCase_ActiveSubsystem = -1; - /* Disable for Outport: '/r_phaA' */ + /* Disable for Outport: '/V_phaABC_FOC' */ rtDW->Gain4[0] = 0; - - /* Disable for Outport: '/r_phaB' */ rtDW->Gain4[1] = 0; - - /* Disable for Outport: '/r_phaC ' */ rtDW->Gain4[2] = 0; /* Disable for Outport: '/Vq' */ @@ -1511,22 +1634,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) } if (UnitDelay3 == 0) { - if (0 != rtb_Sum2_h) { - /* InitializeConditions for IfAction SubSystem: '/F04_Field_Oriented_Control' incorporates: - * ActionPort: '/Action Port' - */ - /* InitializeConditions for If: '/If1' incorporates: - * UnitDelay: '/UnitDelay4' - */ - rtDW->UnitDelay4_DSTATE_er = 0; - - /* End of InitializeConditions for SubSystem: '/F04_Field_Oriented_Control' */ - } - /* Outputs for IfAction SubSystem: '/F04_Field_Oriented_Control' incorporates: * ActionPort: '/Action Port' */ - /* Relay: '/n_fieldWeakAuth' */ + /* Relay: '/n_fieldWeakAuth' */ if (rtb_Abs5 >= rtP->n_fieldWeakAuthHi) { rtDW->n_fieldWeakAuth_Mode = true; } else { @@ -1535,60 +1646,60 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Switch: '/Switch1' incorporates: - * Constant: '/a_elecPeriod1' - * Constant: '/b_fieldWeakEna' - * Logic: '/Logical Operator2' - * Relay: '/n_fieldWeakAuth' + /* Switch: '/Switch1' incorporates: + * Constant: '/a_elecPeriod1' + * Constant: '/b_fieldWeakEna' + * Logic: '/Logical Operator2' + * Relay: '/n_fieldWeakAuth' */ if (rtP->b_fieldWeakEna && rtDW->n_fieldWeakAuth_Mode) { - /* Abs: '/Abs5' */ - if (rtb_Merge < 0) { - rtb_id_fieldWeak_M1 = (int16_T)-rtb_Merge; + /* Abs: '/Abs5' */ + if (rtb_Merge1 < 0) { + rtb_id_fieldWeak_M1 = (int16_T)-rtb_Merge1; } else { - rtb_id_fieldWeak_M1 = rtb_Merge; + rtb_id_fieldWeak_M1 = rtb_Merge1; } - /* End of Abs: '/Abs5' */ + /* End of Abs: '/Abs5' */ - /* PreLookup: '/r_fieldWeak_XA' */ - rtb_r_fieldWeak_XA_o1 = plook_u8s16u8n6_evenc_s(rtb_id_fieldWeak_M1, + /* PreLookup: '/r_fieldWeak_XA' */ + rtb_r_fieldWeak_XA_o1 = plook_u8s16u8n7_evenc_s(rtb_id_fieldWeak_M1, rtP->r_fieldWeak_XA[0], (uint16_T)(rtP->r_fieldWeak_XA[1] - rtP->r_fieldWeak_XA[0]), 11U, &rtb_r_fieldWeak_XA_o2); - /* Interpolation_n-D: '/id_fieldWeak_M1' */ - rtb_id_fieldWeak_M1 = intrp1d_s16s32s32u8u8n6l_s(rtb_r_fieldWeak_XA_o1, + /* Interpolation_n-D: '/id_fieldWeak_M1' */ + rtb_id_fieldWeak_M1 = intrp1d_s16s32s32u8u8n7l_s(rtb_r_fieldWeak_XA_o1, rtb_r_fieldWeak_XA_o2, rtP->id_fieldWeak_M1); } else { rtb_id_fieldWeak_M1 = 0; } - /* End of Switch: '/Switch1' */ + /* End of Switch: '/Switch1' */ - /* Gain: '/toNegative' */ + /* Gain: '/toNegative' */ rtb_toNegative = (int16_T)-rtb_id_fieldWeak_M1; - /* Gain: '/Gain4' incorporates: - * Constant: '/i_max' + /* Gain: '/Gain4' incorporates: + * Constant: '/i_max' */ rtb_Gain4 = (int16_T)-rtP->i_max; - /* If: '/If1' incorporates: - * Constant: '/b_selPhaABCurrMeas' + /* If: '/If1' incorporates: + * Constant: '/b_selPhaABCurrMeas' */ if (rtP->b_selPhaABCurrMeas) { - /* Outputs for IfAction SubSystem: '/Clarke_PhasesAB' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Clarke_PhasesAB' incorporates: + * ActionPort: '/Action Port' */ - /* Gain: '/Gain4' */ + /* Gain: '/Gain4' */ rtb_Gain3 = 18919 * rtb_Saturation; - /* Gain: '/Gain2' */ - rtb_DataTypeConversion = 18919 * rtb_Switch2_d; + /* Gain: '/Gain2' */ + rtb_DataTypeConversion = 18919 * rtb_Merge; - /* Sum: '/Sum1' incorporates: - * Gain: '/Gain2' - * Gain: '/Gain4' + /* Sum: '/Sum1' incorporates: + * Gain: '/Gain2' + * Gain: '/Gain4' */ rtb_Gain3 = (((rtb_Gain3 < 0 ? 32767 : 0) + rtb_Gain3) >> 15) + (int16_T) (((rtb_DataTypeConversion < 0 ? 16383 : 0) + rtb_DataTypeConversion) >> @@ -1603,14 +1714,14 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtb_Gain2_f = (int16_T)rtb_Gain3; - /* End of Sum: '/Sum1' */ - /* End of Outputs for SubSystem: '/Clarke_PhasesAB' */ + /* End of Sum: '/Sum1' */ + /* End of Outputs for SubSystem: '/Clarke_PhasesAB' */ } else { - /* Outputs for IfAction SubSystem: '/Clarke_PhasesBC' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Clarke_PhasesBC' incorporates: + * ActionPort: '/Action Port' */ - /* Sum: '/Sum3' */ - rtb_Gain3 = rtb_Saturation - rtb_Switch2_d; + /* Sum: '/Sum3' */ + rtb_Gain3 = rtb_Saturation - rtb_Merge; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; } else { @@ -1619,14 +1730,14 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Gain: '/Gain2' incorporates: - * Sum: '/Sum3' + /* Gain: '/Gain2' incorporates: + * Sum: '/Sum3' */ rtb_Gain3 *= 18919; rtb_Gain2_f = (int16_T)(((rtb_Gain3 < 0 ? 32767 : 0) + rtb_Gain3) >> 15); - /* Sum: '/Sum1' */ - rtb_Gain3 = -rtb_Saturation - rtb_Switch2_d; + /* Sum: '/Sum1' */ + rtb_Gain3 = -rtb_Saturation - rtb_Merge; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; } else { @@ -1637,23 +1748,23 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtb_Saturation = (int16_T)rtb_Gain3; - /* End of Sum: '/Sum1' */ - /* End of Outputs for SubSystem: '/Clarke_PhasesBC' */ + /* End of Sum: '/Sum1' */ + /* End of Outputs for SubSystem: '/Clarke_PhasesBC' */ } - /* End of If: '/If1' */ + /* End of If: '/If1' */ - /* PreLookup: '/a_elecAngle_XA' */ - rtb_r_fieldWeak_XA_o1 = plook_u8u16_evencka(rtb_Divide2_h, 0U, 128U, 180U); + /* PreLookup: '/a_elecAngle_XA' */ + rtb_r_fieldWeak_XA_o1 = plook_u8s16_evencka(rtb_Switch2_fl, 0, 128U, 180U); - /* Interpolation_n-D: '/r_sin_M1' */ + /* Interpolation_n-D: '/r_sin_M1' */ rtb_MinMax2 = rtConstP.r_sin_M1_Table[rtb_r_fieldWeak_XA_o1]; - /* Interpolation_n-D: '/r_cos_M1' */ - rtb_Switch2_d = rtConstP.r_cos_M1_Table[rtb_r_fieldWeak_XA_o1]; + /* Interpolation_n-D: '/r_cos_M1' */ + rtb_Merge = rtConstP.r_cos_M1_Table[rtb_r_fieldWeak_XA_o1]; /* If: '/If2' incorporates: - * Constant: '/cf_currFilt' + * Constant: '/cf_currFilt' * Inport: '/b_motEna' */ rtb_Sum2_h = rtDW->If2_ActiveSubsystem_a; @@ -1664,34 +1775,34 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->If2_ActiveSubsystem_a = UnitDelay3; if ((rtb_Sum2_h != UnitDelay3) && (rtb_Sum2_h == 0)) { - /* Disable for Outport: '/iq' */ + /* Disable for Outport: '/iq' */ rtDW->Sum1[0] = 0; - /* Disable for Outport: '/id' */ + /* Disable for Outport: '/id' */ rtDW->Sum1[1] = 0; } if (UnitDelay3 == 0) { if (0 != rtb_Sum2_h) { /* SystemReset for IfAction SubSystem: '/Current_Filtering' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* SystemReset for Atomic SubSystem: '/Low_Pass_Filter' */ + /* SystemReset for Atomic SubSystem: '/Low_Pass_Filter' */ /* SystemReset for If: '/If2' */ Low_Pass_Filter_Reset(&rtDW->Low_Pass_Filter_m); - /* End of SystemReset for SubSystem: '/Low_Pass_Filter' */ + /* End of SystemReset for SubSystem: '/Low_Pass_Filter' */ /* End of SystemReset for SubSystem: '/Current_Filtering' */ } - /* Sum: '/Sum6' incorporates: - * Interpolation_n-D: '/r_cos_M1' - * Interpolation_n-D: '/r_sin_M1' - * Product: '/Divide1' - * Product: '/Divide4' + /* Sum: '/Sum6' incorporates: + * Interpolation_n-D: '/r_cos_M1' + * Interpolation_n-D: '/r_sin_M1' + * Product: '/Divide1' + * Product: '/Divide4' */ rtb_Gain3 = (int16_T)((rtb_Gain2_f * rtConstP.r_cos_M1_Table[rtb_r_fieldWeak_XA_o1]) >> 14) - (int16_T) @@ -1705,20 +1816,20 @@ void BLDC_controller_step(RT_MODEL *const rtM) } /* Outputs for IfAction SubSystem: '/Current_Filtering' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* SignalConversion: '/TmpSignal ConversionAtLow_Pass_FilterInport1' incorporates: - * Sum: '/Sum6' + /* SignalConversion: '/TmpSignal ConversionAtLow_Pass_FilterInport1' incorporates: + * Sum: '/Sum6' */ rtb_TmpSignalConversionAtLow_Pa[0] = (int16_T)rtb_Gain3; /* End of Outputs for SubSystem: '/Current_Filtering' */ - /* Sum: '/Sum1' incorporates: - * Interpolation_n-D: '/r_cos_M1' - * Interpolation_n-D: '/r_sin_M1' - * Product: '/Divide2' - * Product: '/Divide3' + /* Sum: '/Sum1' incorporates: + * Interpolation_n-D: '/r_cos_M1' + * Interpolation_n-D: '/r_sin_M1' + * Product: '/Divide2' + * Product: '/Divide3' */ rtb_Gain3 = (int16_T)((rtb_Saturation * rtConstP.r_cos_M1_Table[rtb_r_fieldWeak_XA_o1]) >> 14) + (int16_T) @@ -1732,18 +1843,18 @@ void BLDC_controller_step(RT_MODEL *const rtM) } /* Outputs for IfAction SubSystem: '/Current_Filtering' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* SignalConversion: '/TmpSignal ConversionAtLow_Pass_FilterInport1' incorporates: - * Sum: '/Sum1' + /* SignalConversion: '/TmpSignal ConversionAtLow_Pass_FilterInport1' incorporates: + * Sum: '/Sum1' */ rtb_TmpSignalConversionAtLow_Pa[1] = (int16_T)rtb_Gain3; - /* Outputs for Atomic SubSystem: '/Low_Pass_Filter' */ + /* Outputs for Atomic SubSystem: '/Low_Pass_Filter' */ Low_Pass_Filter(rtb_TmpSignalConversionAtLow_Pa, rtP->cf_currFilt, rtDW->Sum1, &rtDW->Low_Pass_Filter_m); - /* End of Outputs for SubSystem: '/Low_Pass_Filter' */ + /* End of Outputs for SubSystem: '/Low_Pass_Filter' */ /* End of Outputs for SubSystem: '/Current_Filtering' */ } @@ -1751,12 +1862,12 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* End of If: '/If2' */ /* If: '/If1' incorporates: - * Constant: '/Vd_max1' - * Constant: '/cf_idKi1' - * Constant: '/cf_idKp1' - * Constant: '/constant1' - * Gain: '/Gain3' - * Sum: '/Sum3' + * Constant: '/Vd_max1' + * Constant: '/cf_idKi1' + * Constant: '/cf_idKp1' + * Constant: '/constant1' + * Gain: '/Gain3' + * Sum: '/Sum3' */ rtb_Sum2_h = rtDW->If1_ActiveSubsystem_e; UnitDelay3 = -1; @@ -1766,47 +1877,47 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->If1_ActiveSubsystem_e = UnitDelay3; if ((rtb_Sum2_h != UnitDelay3) && (rtb_Sum2_h == 0)) { - /* Disable for Outport: '/Vd' */ + /* Disable for Outport: '/Vd' */ rtDW->Switch1 = 0; } if (UnitDelay3 == 0) { if (0 != rtb_Sum2_h) { /* SystemReset for IfAction SubSystem: '/Vd_Calculation' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* SystemReset for Atomic SubSystem: '/PI_clamp_fixdt_id' */ + /* SystemReset for Atomic SubSystem: '/PI_clamp_fixdt_id' */ /* SystemReset for If: '/If1' */ PI_clamp_fixdt_Reset(&rtDW->PI_clamp_fixdt_id); - /* End of SystemReset for SubSystem: '/PI_clamp_fixdt_id' */ + /* End of SystemReset for SubSystem: '/PI_clamp_fixdt_id' */ /* End of SystemReset for SubSystem: '/Vd_Calculation' */ } /* Outputs for IfAction SubSystem: '/Vd_Calculation' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* Switch: '/Switch2' incorporates: - * Constant: '/i_max' - * RelationalOperator: '/LowerRelop1' - * RelationalOperator: '/UpperRelop' - * Switch: '/Switch' + /* Switch: '/Switch2' incorporates: + * Constant: '/i_max' + * RelationalOperator: '/LowerRelop1' + * RelationalOperator: '/UpperRelop' + * Switch: '/Switch' */ if (rtb_toNegative > rtP->i_max) { rtb_toNegative = rtP->i_max; } else { if (rtb_toNegative < rtb_Gain4) { - /* Switch: '/Switch' */ + /* Switch: '/Switch' */ rtb_toNegative = rtb_Gain4; } } - /* End of Switch: '/Switch2' */ + /* End of Switch: '/Switch2' */ - /* Sum: '/Sum3' */ + /* Sum: '/Sum3' */ rtb_Gain3 = rtb_toNegative - rtDW->Sum1[1]; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; @@ -1816,39 +1927,39 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Outputs for Atomic SubSystem: '/PI_clamp_fixdt_id' */ + /* Outputs for Atomic SubSystem: '/PI_clamp_fixdt_id' */ PI_clamp_fixdt((int16_T)rtb_Gain3, rtP->cf_idKp, rtP->cf_idKi, rtP->Vd_max, (int16_T)-rtP->Vd_max, 0, &rtDW->Switch1, &rtDW->PI_clamp_fixdt_id); - /* End of Outputs for SubSystem: '/PI_clamp_fixdt_id' */ + /* End of Outputs for SubSystem: '/PI_clamp_fixdt_id' */ /* End of Outputs for SubSystem: '/Vd_Calculation' */ } /* End of If: '/If1' */ - /* Abs: '/Abs5' */ + /* Abs: '/Abs5' */ if (rtDW->Switch1 < 0) { rtb_toNegative = (int16_T)-rtDW->Switch1; } else { rtb_toNegative = rtDW->Switch1; } - /* End of Abs: '/Abs5' */ + /* End of Abs: '/Abs5' */ - /* PreLookup: '/Vq_max_XA' */ + /* PreLookup: '/Vq_max_XA' */ rtb_r_fieldWeak_XA_o1 = plook_u8s16_evencka(rtb_toNegative, rtP->Vq_max_XA[0], (uint16_T)(rtP->Vq_max_XA[1] - rtP->Vq_max_XA[0]), 45U); - /* Gain: '/Gain5' incorporates: - * Interpolation_n-D: '/Vq_max_M1' + /* Gain: '/Gain5' incorporates: + * Interpolation_n-D: '/Vq_max_M1' */ rtb_Gain2_f = (int16_T)-rtP->Vq_max_M1[rtb_r_fieldWeak_XA_o1]; - /* Interpolation_n-D: '/iq_maxSca_M1' incorporates: - * Constant: '/i_max' - * Product: '/Divide4' + /* Interpolation_n-D: '/iq_maxSca_M1' incorporates: + * Constant: '/i_max' + * Product: '/Divide4' */ rtb_Gain3 = rtb_id_fieldWeak_M1 << 16; rtb_Gain3 = (rtb_Gain3 == MIN_int32_T) && (rtP->i_max == -1) ? MAX_int32_T : @@ -1861,25 +1972,25 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Product: '/Divide1' incorporates: - * Constant: '/i_max' - * Interpolation_n-D: '/iq_maxSca_M1' - * PreLookup: '/iq_maxSca_XA' - * Product: '/Divide4' + /* Product: '/Divide1' incorporates: + * Constant: '/i_max' + * Interpolation_n-D: '/iq_maxSca_M1' + * PreLookup: '/iq_maxSca_XA' + * Product: '/Divide4' */ rtb_id_fieldWeak_M1 = (int16_T) ((rtConstP.iq_maxSca_M1_Table[plook_u8u16_evencka((uint16_T)rtb_Gain3, 0U, 1311U, 49U)] * rtP->i_max) >> 16); - /* Gain: '/Gain1' */ + /* Gain: '/Gain1' */ rtb_Saturation = (int16_T)-rtb_id_fieldWeak_M1; - /* If: '/If1' incorporates: - * Constant: '/CTRL_COMM' - * Constant: '/CTRL_COMM1' - * Logic: '/Logical Operator2' - * RelationalOperator: '/Relational Operator1' - * RelationalOperator: '/Relational Operator2' + /* If: '/If1' incorporates: + * Constant: '/CTRL_COMM' + * Constant: '/CTRL_COMM1' + * Logic: '/Logical Operator2' + * RelationalOperator: '/Relational Operator1' + * RelationalOperator: '/Relational Operator2' */ rtb_Sum2_h = rtDW->If1_ActiveSubsystem_f; UnitDelay3 = -1; @@ -1889,33 +2000,33 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->If1_ActiveSubsystem_f = UnitDelay3; if ((rtb_Sum2_h != UnitDelay3) && (rtb_Sum2_h == 0)) { - /* Disable for Outport: '/iq_limProt' */ + /* Disable for Outport: '/iq_limProt' */ rtDW->Divide4 = 0; } if (UnitDelay3 == 0) { - /* Outputs for IfAction SubSystem: '/Current_Limit_Protection' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Current_Limit_Protection' incorporates: + * ActionPort: '/Action Port' */ - /* Switch: '/Switch2' incorporates: - * RelationalOperator: '/LowerRelop1' - * RelationalOperator: '/UpperRelop' - * Switch: '/Switch' + /* Switch: '/Switch2' incorporates: + * RelationalOperator: '/LowerRelop1' + * RelationalOperator: '/UpperRelop' + * Switch: '/Switch' */ if (rtDW->Sum1[0] > rtb_id_fieldWeak_M1) { rtb_toNegative = rtb_id_fieldWeak_M1; } else if (rtDW->Sum1[0] < rtb_Saturation) { - /* Switch: '/Switch' */ + /* Switch: '/Switch' */ rtb_toNegative = rtb_Saturation; } else { rtb_toNegative = rtDW->Sum1[0]; } - /* End of Switch: '/Switch2' */ + /* End of Switch: '/Switch2' */ - /* Product: '/Divide4' incorporates: - * Constant: '/cf_iqKpLimProt' - * Sum: '/Sum3' + /* Product: '/Divide4' incorporates: + * Constant: '/cf_iqKpLimProt' + * Sum: '/Sum3' */ rtb_Gain3 = ((int16_T)(rtb_toNegative - rtDW->Sum1[0]) * rtP->cf_iqKpLimProt) >> 8; @@ -1929,23 +2040,23 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->Divide4 = (int16_T)rtb_Gain3; - /* End of Product: '/Divide4' */ - /* End of Outputs for SubSystem: '/Current_Limit_Protection' */ + /* End of Product: '/Divide4' */ + /* End of Outputs for SubSystem: '/Current_Limit_Protection' */ } - /* End of If: '/If1' */ + /* End of If: '/If1' */ - /* Gain: '/Gain6' incorporates: - * Constant: '/n_max1' + /* Gain: '/Gain6' incorporates: + * Constant: '/n_max1' */ rtb_toNegative = (int16_T)-rtP->n_max; - /* If: '/If2' incorporates: - * Constant: '/CTRL_COMM2' - * Constant: '/CTRL_COMM3' - * Logic: '/Logical Operator1' - * RelationalOperator: '/Relational Operator3' - * RelationalOperator: '/Relational Operator4' + /* If: '/If2' incorporates: + * Constant: '/CTRL_COMM2' + * Constant: '/CTRL_COMM3' + * Logic: '/Logical Operator1' + * RelationalOperator: '/Relational Operator3' + * RelationalOperator: '/Relational Operator4' */ rtb_Sum2_h = rtDW->If2_ActiveSubsystem_c; UnitDelay3 = -1; @@ -1955,35 +2066,35 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->If2_ActiveSubsystem_c = UnitDelay3; if ((rtb_Sum2_h != UnitDelay3) && (rtb_Sum2_h == 0)) { - /* Disable for Outport: '/n_limProt' */ + /* Disable for Outport: '/n_limProt' */ rtDW->Divide1 = 0; } if (UnitDelay3 == 0) { - /* Outputs for IfAction SubSystem: '/Speed_Limit_Protection' incorporates: - * ActionPort: '/Action Port' + /* Outputs for IfAction SubSystem: '/Speed_Limit_Protection' incorporates: + * ActionPort: '/Action Port' */ - /* Switch: '/Switch2' incorporates: - * Constant: '/n_max1' - * RelationalOperator: '/LowerRelop1' - * RelationalOperator: '/UpperRelop' - * Switch: '/Switch' + /* Switch: '/Switch2' incorporates: + * Constant: '/n_max1' + * RelationalOperator: '/LowerRelop1' + * RelationalOperator: '/UpperRelop' + * Switch: '/Switch' */ - if (rtb_Switch2_fv > rtP->n_max) { + if (rtb_Switch2_k > rtP->n_max) { rtb_toNegative = rtP->n_max; } else { - if (!(rtb_Switch2_fv < rtb_toNegative)) { - rtb_toNegative = rtb_Switch2_fv; + if (!(rtb_Switch2_k < rtb_toNegative)) { + rtb_toNegative = rtb_Switch2_k; } } - /* End of Switch: '/Switch2' */ + /* End of Switch: '/Switch2' */ - /* Product: '/Divide1' incorporates: - * Constant: '/cf_nKpLimProt' - * Sum: '/Sum1' + /* Product: '/Divide1' incorporates: + * Constant: '/cf_nKpLimProt' + * Sum: '/Sum1' */ - rtb_Gain3 = ((int16_T)(rtb_toNegative - rtb_Switch2_fv) * + rtb_Gain3 = ((int16_T)(rtb_toNegative - rtb_Switch2_k) * rtP->cf_nKpLimProt) >> 8; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; @@ -1995,20 +2106,21 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->Divide1 = (int16_T)rtb_Gain3; - /* End of Product: '/Divide1' */ - /* End of Outputs for SubSystem: '/Speed_Limit_Protection' */ + /* End of Product: '/Divide1' */ + /* End of Outputs for SubSystem: '/Speed_Limit_Protection' */ } - /* End of If: '/If2' */ + /* End of If: '/If2' */ /* SwitchCase: '/Switch Case' incorporates: - * Constant: '/cf_iqKiLimProt' - * Constant: '/cf_nKi' - * Constant: '/cf_nKp' - * Interpolation_n-D: '/Vq_max_M1' - * Product: '/Divide1' - * SignalConversion: '/Signal Conversion2' - * Sum: '/Sum3' + * Constant: '/cf_iqKiLimProt' + * Constant: '/cf_nKi' + * Constant: '/cf_nKp' + * Inport: '/r_inpTgtSca' + * Interpolation_n-D: '/Vq_max_M1' + * Product: '/Divide1' + * SignalConversion: '/Signal Conversion2' + * Sum: '/Sum3' */ rtb_Sum2_h = rtDW->SwitchCase_ActiveSubsystem; switch (rtb_Sum_l) { @@ -2033,10 +2145,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) switch (UnitDelay3) { case 0: /* Outputs for IfAction SubSystem: '/Voltage_Mode' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* Sum: '/Sum3' */ - rtb_Gain3 = (rtb_Merge + rtDW->Divide4) + rtDW->Divide1; + /* Sum: '/Sum3' */ + rtb_Gain3 = (rtb_Merge1 + rtDW->Divide4) + rtDW->Divide1; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; } else { @@ -2045,51 +2157,51 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Switch: '/Switch2' incorporates: - * Interpolation_n-D: '/Vq_max_M1' - * RelationalOperator: '/LowerRelop1' - * RelationalOperator: '/UpperRelop' - * Sum: '/Sum3' - * Switch: '/Switch' + /* Switch: '/Switch2' incorporates: + * Interpolation_n-D: '/Vq_max_M1' + * RelationalOperator: '/LowerRelop1' + * RelationalOperator: '/UpperRelop' + * Sum: '/Sum3' + * Switch: '/Switch' */ if ((int16_T)rtb_Gain3 > rtP->Vq_max_M1[rtb_r_fieldWeak_XA_o1]) { - /* SignalConversion: '/Signal Conversion2' */ + /* SignalConversion: '/Signal Conversion2' */ rtDW->Merge = rtP->Vq_max_M1[rtb_r_fieldWeak_XA_o1]; } else if ((int16_T)rtb_Gain3 < rtb_Gain2_f) { - /* Switch: '/Switch' incorporates: - * SignalConversion: '/Signal Conversion2' + /* Switch: '/Switch' incorporates: + * SignalConversion: '/Signal Conversion2' */ rtDW->Merge = rtb_Gain2_f; } else { - /* SignalConversion: '/Signal Conversion2' */ + /* SignalConversion: '/Signal Conversion2' */ rtDW->Merge = (int16_T)rtb_Gain3; } - /* End of Switch: '/Switch2' */ + /* End of Switch: '/Switch2' */ /* End of Outputs for SubSystem: '/Voltage_Mode' */ break; case 1: if (UnitDelay3 != rtb_Sum2_h) { /* SystemReset for IfAction SubSystem: '/Speed_Mode' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* SystemReset for Atomic SubSystem: '/PI_clamp_fixdt_n' */ + /* SystemReset for Atomic SubSystem: '/PI_clamp_fixdt_n' */ /* SystemReset for SwitchCase: '/Switch Case' */ PI_clamp_fixdt_n_Reset(&rtDW->PI_clamp_fixdt_n_o); - /* End of SystemReset for SubSystem: '/PI_clamp_fixdt_n' */ + /* End of SystemReset for SubSystem: '/PI_clamp_fixdt_n' */ /* End of SystemReset for SubSystem: '/Speed_Mode' */ } /* Outputs for IfAction SubSystem: '/Speed_Mode' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* Sum: '/Sum3' */ - rtb_Gain3 = rtb_Merge - rtb_Switch2_fv; + /* Sum: '/Sum3' */ + rtb_Gain3 = rtb_Merge1 - rtb_Switch2_k; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; } else { @@ -2098,13 +2210,13 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Outputs for Atomic SubSystem: '/PI_clamp_fixdt_n' */ + /* Outputs for Atomic SubSystem: '/PI_clamp_fixdt_n' */ rtDW->Merge = (int16_T) PI_clamp_fixdt_n((int16_T)rtb_Gain3, rtP->cf_nKp, rtP->cf_nKi, rtP->Vq_max_M1[rtb_r_fieldWeak_XA_o1], rtb_Gain2_f, (int16_T)((rtDW->Divide4 * rtP->cf_iqKiLimProt) >> 10), &rtDW->PI_clamp_fixdt_n_o); - /* End of Outputs for SubSystem: '/PI_clamp_fixdt_n' */ + /* End of Outputs for SubSystem: '/PI_clamp_fixdt_n' */ /* End of Outputs for SubSystem: '/Speed_Mode' */ break; @@ -2112,24 +2224,24 @@ void BLDC_controller_step(RT_MODEL *const rtM) case 2: if (UnitDelay3 != rtb_Sum2_h) { /* SystemReset for IfAction SubSystem: '/Torque_Mode' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* SystemReset for Atomic SubSystem: '/PI_clamp_fixdt_iq' */ + /* SystemReset for Atomic SubSystem: '/PI_clamp_fixdt_iq' */ /* SystemReset for SwitchCase: '/Switch Case' */ PI_clamp_fixdt_Reset(&rtDW->PI_clamp_fixdt_iq); - /* End of SystemReset for SubSystem: '/PI_clamp_fixdt_iq' */ + /* End of SystemReset for SubSystem: '/PI_clamp_fixdt_iq' */ /* End of SystemReset for SubSystem: '/Torque_Mode' */ } /* Outputs for IfAction SubSystem: '/Torque_Mode' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* Sum: '/Sum2' */ - rtb_Gain3 = rtb_Merge + rtDW->Divide1; + /* Sum: '/Sum2' */ + rtb_Gain3 = rtb_Merge1 + rtDW->Divide1; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; } else { @@ -2138,13 +2250,13 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Switch: '/Switch2' incorporates: - * RelationalOperator: '/LowerRelop1' - * Sum: '/Sum2' + /* Switch: '/Switch2' incorporates: + * RelationalOperator: '/LowerRelop1' + * Sum: '/Sum2' */ if (!((int16_T)rtb_Gain3 > rtb_id_fieldWeak_M1)) { - /* Switch: '/Switch' incorporates: - * RelationalOperator: '/UpperRelop' + /* Switch: '/Switch' incorporates: + * RelationalOperator: '/UpperRelop' */ if ((int16_T)rtb_Gain3 < rtb_Saturation) { rtb_id_fieldWeak_M1 = rtb_Saturation; @@ -2152,12 +2264,12 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtb_id_fieldWeak_M1 = (int16_T)rtb_Gain3; } - /* End of Switch: '/Switch' */ + /* End of Switch: '/Switch' */ } - /* End of Switch: '/Switch2' */ + /* End of Switch: '/Switch2' */ - /* Sum: '/Sum1' */ + /* Sum: '/Sum1' */ rtb_Gain3 = rtb_id_fieldWeak_M1 - rtDW->Sum1[0]; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; @@ -2167,126 +2279,29 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Outputs for Atomic SubSystem: '/PI_clamp_fixdt_iq' */ + /* Outputs for Atomic SubSystem: '/PI_clamp_fixdt_iq' */ - /* SignalConversion: '/Signal Conversion2' incorporates: - * Constant: '/cf_iqKi' - * Constant: '/cf_iqKp' - * Constant: '/constant' - * Interpolation_n-D: '/Vq_max_M1' - * Sum: '/Sum1' + /* SignalConversion: '/Signal Conversion2' incorporates: + * Constant: '/cf_iqKi' + * Constant: '/cf_iqKp' + * Constant: '/constant' + * Interpolation_n-D: '/Vq_max_M1' + * Sum: '/Sum1' */ PI_clamp_fixdt((int16_T)rtb_Gain3, rtP->cf_iqKp, rtP->cf_iqKi, rtP->Vq_max_M1[rtb_r_fieldWeak_XA_o1], rtb_Gain2_f, 0, &rtDW->Merge, &rtDW->PI_clamp_fixdt_iq); - /* End of Outputs for SubSystem: '/PI_clamp_fixdt_iq' */ + /* End of Outputs for SubSystem: '/PI_clamp_fixdt_iq' */ /* End of Outputs for SubSystem: '/Torque_Mode' */ break; case 3: - if (UnitDelay3 != rtb_Sum2_h) { - /* SystemReset for IfAction SubSystem: '/Open_Mode' incorporates: - * ActionPort: '/Action Port' - */ - /* SystemReset for Atomic SubSystem: '/rising_edge_init' */ - /* SystemReset for SwitchCase: '/Switch Case' incorporates: - * UnitDelay: '/UnitDelay' - * UnitDelay: '/UnitDelay' - */ - rtDW->UnitDelay_DSTATE_g = true; - - /* End of SystemReset for SubSystem: '/rising_edge_init' */ - - /* SystemReset for Atomic SubSystem: '/Rate_Limiter' */ - rtDW->UnitDelay_DSTATE = 0; - - /* End of SystemReset for SubSystem: '/Rate_Limiter' */ - /* End of SystemReset for SubSystem: '/Open_Mode' */ - } - /* Outputs for IfAction SubSystem: '/Open_Mode' incorporates: - * ActionPort: '/Action Port' + * ActionPort: '/Action Port' */ - /* DataTypeConversion: '/Data Type Conversion' incorporates: - * UnitDelay: '/UnitDelay4' - */ - rtb_Gain3 = rtDW->UnitDelay4_DSTATE_er << 12; - rtb_DataTypeConversion = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | - -134217728 : rtb_Gain3 & 134217727; - - /* Outputs for Atomic SubSystem: '/rising_edge_init' */ - /* UnitDelay: '/UnitDelay' */ - rtb_RelationalOperator4_d = rtDW->UnitDelay_DSTATE_g; - - /* Update for UnitDelay: '/UnitDelay' incorporates: - * Constant: '/Constant' - */ - rtDW->UnitDelay_DSTATE_g = false; - - /* End of Outputs for SubSystem: '/rising_edge_init' */ - - /* Outputs for Atomic SubSystem: '/Rate_Limiter' */ - /* Switch: '/Switch1' incorporates: - * UnitDelay: '/UnitDelay' - */ - if (rtb_RelationalOperator4_d) { - rtb_Switch1 = rtb_DataTypeConversion; - } else { - rtb_Switch1 = rtDW->UnitDelay_DSTATE; - } - - /* End of Switch: '/Switch1' */ - - /* Sum: '/Sum1' */ - rtb_Gain3 = -rtb_Switch1; - rtb_Sum1 = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | -134217728 : - rtb_Gain3 & 134217727; - - /* Switch: '/Switch2' incorporates: - * Constant: '/dV_openRate' - * RelationalOperator: '/LowerRelop1' - */ - if (rtb_Sum1 > rtP->dV_openRate) { - rtb_Sum1 = rtP->dV_openRate; - } else { - /* Gain: '/Gain3' */ - rtb_Gain3 = -rtP->dV_openRate; - rtb_Gain3 = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | -134217728 : - rtb_Gain3 & 134217727; - - /* Switch: '/Switch' incorporates: - * RelationalOperator: '/UpperRelop' - */ - if (rtb_Sum1 < rtb_Gain3) { - rtb_Sum1 = rtb_Gain3; - } - - /* End of Switch: '/Switch' */ - } - - /* End of Switch: '/Switch2' */ - - /* Sum: '/Sum2' */ - rtb_Gain3 = rtb_Sum1 + rtb_Switch1; - rtb_Switch1 = (rtb_Gain3 & 134217728) != 0 ? rtb_Gain3 | -134217728 : - rtb_Gain3 & 134217727; - - /* Switch: '/Switch2' */ - if (rtb_RelationalOperator4_d) { - /* Update for UnitDelay: '/UnitDelay' */ - rtDW->UnitDelay_DSTATE = rtb_DataTypeConversion; - } else { - /* Update for UnitDelay: '/UnitDelay' */ - rtDW->UnitDelay_DSTATE = rtb_Switch1; - } - - /* End of Switch: '/Switch2' */ - /* End of Outputs for SubSystem: '/Rate_Limiter' */ - - /* DataTypeConversion: '/Data Type Conversion1' */ - rtDW->Merge = (int16_T)(rtb_Switch1 >> 12); + rtDW->Merge = rtb_Merge1; /* End of Outputs for SubSystem: '/Open_Mode' */ break; @@ -2294,11 +2309,11 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* End of SwitchCase: '/Switch Case' */ - /* Sum: '/Sum6' incorporates: - * Product: '/Divide1' - * Product: '/Divide4' + /* Sum: '/Sum6' incorporates: + * Product: '/Divide1' + * Product: '/Divide4' */ - rtb_Gain3 = (int16_T)((rtDW->Switch1 * rtb_Switch2_d) >> 14) - (int16_T) + rtb_Gain3 = (int16_T)((rtDW->Switch1 * rtb_Merge) >> 14) - (int16_T) ((rtDW->Merge * rtb_MinMax2) >> 14); if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; @@ -2308,12 +2323,12 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Sum: '/Sum1' incorporates: - * Product: '/Divide2' - * Product: '/Divide3' + /* Sum: '/Sum1' incorporates: + * Product: '/Divide2' + * Product: '/Divide3' */ rtb_DataTypeConversion = (int16_T)((rtDW->Switch1 * rtb_MinMax2) >> 14) + - (int16_T)((rtDW->Merge * rtb_Switch2_d) >> 14); + (int16_T)((rtDW->Merge * rtb_Merge) >> 14); if (rtb_DataTypeConversion > 32767) { rtb_DataTypeConversion = 32767; } else { @@ -2322,15 +2337,15 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Gain: '/Gain1' incorporates: - * Sum: '/Sum1' + /* Gain: '/Gain1' incorporates: + * Sum: '/Sum1' */ rtb_DataTypeConversion = 14189 * (int16_T)rtb_DataTypeConversion; - /* Sum: '/Sum6' incorporates: - * Gain: '/Gain1' - * Gain: '/Gain3' - * Sum: '/Sum6' + /* Sum: '/Sum6' incorporates: + * Gain: '/Gain1' + * Gain: '/Gain3' + * Sum: '/Sum6' */ rtb_DataTypeConversion = (((rtb_DataTypeConversion < 0 ? 16383 : 0) + rtb_DataTypeConversion) >> 14) - ((int16_T)(((int16_T)rtb_Gain3 < 0) + @@ -2343,9 +2358,9 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Sum: '/Sum2' incorporates: - * Sum: '/Sum6' - * Sum: '/Sum6' + /* Sum: '/Sum2' incorporates: + * Sum: '/Sum6' + * Sum: '/Sum6' */ rtb_Switch1 = -(int16_T)rtb_Gain3 - (int16_T)rtb_DataTypeConversion; if (rtb_Switch1 > 32767) { @@ -2356,24 +2371,24 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* MinMax: '/MinMax1' incorporates: - * Sum: '/Sum2' - * Sum: '/Sum6' - * Sum: '/Sum6' + /* MinMax: '/MinMax1' incorporates: + * Sum: '/Sum2' + * Sum: '/Sum6' + * Sum: '/Sum6' */ - rtb_Switch2_d = (int16_T)rtb_Gain3; + rtb_Merge = (int16_T)rtb_Gain3; if (!((int16_T)rtb_Gain3 < (int16_T)rtb_DataTypeConversion)) { - rtb_Switch2_d = (int16_T)rtb_DataTypeConversion; + rtb_Merge = (int16_T)rtb_DataTypeConversion; } - if (!(rtb_Switch2_d < (int16_T)rtb_Switch1)) { - rtb_Switch2_d = (int16_T)rtb_Switch1; + if (!(rtb_Merge < (int16_T)rtb_Switch1)) { + rtb_Merge = (int16_T)rtb_Switch1; } - /* MinMax: '/MinMax2' incorporates: - * Sum: '/Sum2' - * Sum: '/Sum6' - * Sum: '/Sum6' + /* MinMax: '/MinMax2' incorporates: + * Sum: '/Sum2' + * Sum: '/Sum6' + * Sum: '/Sum6' */ rtb_Saturation = (int16_T)rtb_Gain3; if (!((int16_T)rtb_Gain3 > (int16_T)rtb_DataTypeConversion)) { @@ -2384,11 +2399,11 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtb_Saturation = (int16_T)rtb_Switch1; } - /* Sum: '/Add' incorporates: - * MinMax: '/MinMax1' - * MinMax: '/MinMax2' + /* Sum: '/Add' incorporates: + * MinMax: '/MinMax1' + * MinMax: '/MinMax2' */ - rtb_Sum1 = rtb_Switch2_d + rtb_Saturation; + rtb_Sum1 = rtb_Merge + rtb_Saturation; if (rtb_Sum1 > 32767) { rtb_Sum1 = 32767; } else { @@ -2397,13 +2412,13 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Gain: '/Gain2' incorporates: - * Sum: '/Add' + /* Gain: '/Gain2' incorporates: + * Sum: '/Add' */ rtb_Gain2_f = (int16_T)(rtb_Sum1 >> 1); - /* Sum: '/Add1' incorporates: - * Sum: '/Sum6' + /* Sum: '/Add1' incorporates: + * Sum: '/Sum6' */ rtb_Gain3 = (int16_T)rtb_Gain3 - rtb_Gain2_f; if (rtb_Gain3 > 32767) { @@ -2414,13 +2429,13 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Gain: '/Gain4' incorporates: - * Sum: '/Add1' + /* Gain: '/Gain4' incorporates: + * Sum: '/Add1' */ - rtDW->Gain4[0] = (int16_T)((18919 * rtb_Gain3) >> 18); + rtDW->Gain4[0] = (int16_T)((18919 * rtb_Gain3) >> 14); - /* Sum: '/Add1' incorporates: - * Sum: '/Sum6' + /* Sum: '/Add1' incorporates: + * Sum: '/Sum6' */ rtb_Gain3 = (int16_T)rtb_DataTypeConversion - rtb_Gain2_f; if (rtb_Gain3 > 32767) { @@ -2431,13 +2446,13 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Gain: '/Gain4' incorporates: - * Sum: '/Add1' + /* Gain: '/Gain4' incorporates: + * Sum: '/Add1' */ - rtDW->Gain4[1] = (int16_T)((18919 * rtb_Gain3) >> 18); + rtDW->Gain4[1] = (int16_T)((18919 * rtb_Gain3) >> 14); - /* Sum: '/Add1' incorporates: - * Sum: '/Sum2' + /* Sum: '/Add1' incorporates: + * Sum: '/Sum2' */ rtb_Gain3 = (int16_T)rtb_Switch1 - rtb_Gain2_f; if (rtb_Gain3 > 32767) { @@ -2448,13 +2463,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) } } - /* Gain: '/Gain4' incorporates: - * Sum: '/Add1' + /* Gain: '/Gain4' incorporates: + * Sum: '/Add1' */ - rtDW->Gain4[2] = (int16_T)((18919 * rtb_Gain3) >> 18); - - /* Update for UnitDelay: '/UnitDelay4' */ - rtDW->UnitDelay4_DSTATE_er = rtDW->Merge; + rtDW->Gain4[2] = (int16_T)((18919 * rtb_Gain3) >> 14); /* End of Outputs for SubSystem: '/F04_Field_Oriented_Control' */ } @@ -2464,123 +2476,217 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* Switch: '/Switch2' incorporates: * Constant: '/z_ctrlTypSel1' * Constant: '/CTRL_COMM1' - * DataTypeConversion: '/Data Type Conversion10' - * DataTypeConversion: '/Data Type Conversion8' * RelationalOperator: '/Relational Operator6' */ - if (rtP->z_ctrlTypSel == 1) { - rtb_Merge = (int16_T)(rtDW->Merge >> 4); + if (rtP->z_ctrlTypSel == 2) { + rtb_Merge = rtDW->Merge; } else { - rtb_Merge = (int16_T)(rtb_Merge >> 4); + rtb_Merge = rtb_Merge1; } /* End of Switch: '/Switch2' */ - /* Switch: '/Switch1' incorporates: - * Constant: '/vec_hallToPos' - * LookupNDDirect: '/z_commutMap_M1' - * Product: '/Divide2' - * Selector: '/Selector' + /* If: '/If' incorporates: + * Constant: '/z_ctrlTypSel1' + * Constant: '/CTRL_COMM2' + * Constant: '/CTRL_COMM3' + * Constant: '/vec_hallToPos' + * Inport: '/V_phaABC_FOC_in' + * Logic: '/Logical Operator1' + * Logic: '/Logical Operator2' + * LookupNDDirect: '/z_commutMap_M1' + * RelationalOperator: '/Relational Operator1' + * RelationalOperator: '/Relational Operator2' + * Selector: '/Selector' * - * About '/z_commutMap_M1': + * About '/z_commutMap_M1': * 2-dimensional Direct Look-Up returning a Column */ - if (rtb_LogicalOperator) { - rtb_Switch2_d = rtDW->Gain4[0]; - rtb_Saturation = rtDW->Gain4[1]; - rtb_id_fieldWeak_M1 = rtDW->Gain4[2]; + if (rtb_LogicalOperator && (rtP->z_ctrlTypSel == 2)) { + /* Outputs for IfAction SubSystem: '/F05_02_FOC_Method' incorporates: + * ActionPort: '/Action Port' + */ + rtb_Saturation = rtDW->Gain4[0]; + rtb_id_fieldWeak_M1 = rtDW->Gain4[1]; + rtb_Merge1 = rtDW->Gain4[2]; + + /* End of Outputs for SubSystem: '/F05_02_FOC_Method' */ + } else if (rtb_LogicalOperator && (rtP->z_ctrlTypSel == 1)) { + /* Outputs for IfAction SubSystem: '/F05_01_SIN_Method' incorporates: + * ActionPort: '/Action Port' + */ + /* Relay: '/n_fieldWeakAuth' */ + if (rtb_Abs5 >= rtP->n_fieldWeakAuthHi) { + rtDW->n_fieldWeakAuth_Mode_m = true; + } else { + if (rtb_Abs5 <= rtP->n_fieldWeakAuthLo) { + rtDW->n_fieldWeakAuth_Mode_m = false; + } + } + + /* Switch: '/Switch_PhaAdv' incorporates: + * Constant: '/b_fieldWeakEna' + * Logic: '/Logical Operator1' + * Product: '/Divide2' + * Product: '/Divide3' + * Relay: '/n_fieldWeakAuth' + * Sum: '/Sum3' + */ + if (rtP->b_fieldWeakEna && rtDW->n_fieldWeakAuth_Mode_m) { + /* Abs: '/Abs5' */ + if (rtb_Merge1 < 0) { + rtb_id_fieldWeak_M1 = (int16_T)-rtb_Merge1; + } else { + rtb_id_fieldWeak_M1 = rtb_Merge1; + } + + /* End of Abs: '/Abs5' */ + + /* PreLookup: '/r_phaAdv_XA' */ + rtb_Sum = plook_u8s16u8n7_evenc_s(rtb_id_fieldWeak_M1, rtP->r_phaAdv_XA[0], + (uint16_T)(rtP->r_phaAdv_XA[1] - rtP->r_phaAdv_XA[0]), 10U, + &rtb_r_fieldWeak_XA_o1); + + /* Interpolation_n-D: '/a_phaAdv_M1' */ + rtb_MinMax2 = intrp1d_s16s32s32u8u8n7l_s(rtb_Sum, rtb_r_fieldWeak_XA_o1, + rtP->a_phaAdv_M1); + + /* Sum: '/Sum3' incorporates: + * Product: '/Product2' + */ + rtb_MinMax2 = (int16_T)(((int16_T)(rtb_MinMax2 * rtDW->Switch2) >> 2) + + rtb_Switch2_fl); + rtb_MinMax2 -= (int16_T)(23040 * (int16_T)div_nde_s32_floor(rtb_MinMax2, + 23040)); + } else { + rtb_MinMax2 = rtb_Switch2_fl; + } + + /* End of Switch: '/Switch_PhaAdv' */ + + /* PreLookup: '/a_elecAngle_XA' */ + rtb_Sum = plook_u8s16_evencka(rtb_MinMax2, 0, 128U, 180U); + + /* Product: '/Divide2' incorporates: + * Interpolation_n-D: '/r_sin3PhaA_M1' + * Interpolation_n-D: '/r_sin3PhaB_M1' + * Interpolation_n-D: '/r_sin3PhaC_M1' + */ + rtb_Saturation = (int16_T)((rtb_Merge1 * + rtConstP.r_sin3PhaA_M1_Table[rtb_Sum]) >> 14); + rtb_id_fieldWeak_M1 = (int16_T)((rtb_Merge1 * + rtConstP.r_sin3PhaB_M1_Table[rtb_Sum]) >> 14); + rtb_Merge1 = (int16_T)((rtb_Merge1 * rtConstP.r_sin3PhaC_M1_Table[rtb_Sum]) >> + 14); + + /* End of Outputs for SubSystem: '/F05_01_SIN_Method' */ } else { + /* Outputs for IfAction SubSystem: '/F05_00_COM_Method' incorporates: + * ActionPort: '/Action Port' + */ if (rtConstP.vec_hallToPos_Value[rtb_Sum] > 5) { - /* LookupNDDirect: '/z_commutMap_M1' + /* LookupNDDirect: '/z_commutMap_M1' * - * About '/z_commutMap_M1': + * About '/z_commutMap_M1': * 2-dimensional Direct Look-Up returning a Column */ rtb_Sum2_h = 5; } else if (rtConstP.vec_hallToPos_Value[rtb_Sum] < 0) { - /* LookupNDDirect: '/z_commutMap_M1' + /* LookupNDDirect: '/z_commutMap_M1' * - * About '/z_commutMap_M1': + * About '/z_commutMap_M1': * 2-dimensional Direct Look-Up returning a Column */ rtb_Sum2_h = 0; } else { - /* LookupNDDirect: '/z_commutMap_M1' incorporates: - * Constant: '/vec_hallToPos' - * Selector: '/Selector' + /* LookupNDDirect: '/z_commutMap_M1' incorporates: + * Constant: '/vec_hallToPos' + * Selector: '/Selector' * - * About '/z_commutMap_M1': + * About '/z_commutMap_M1': * 2-dimensional Direct Look-Up returning a Column */ rtb_Sum2_h = rtConstP.vec_hallToPos_Value[rtb_Sum]; } - /* LookupNDDirect: '/z_commutMap_M1' incorporates: - * Constant: '/vec_hallToPos' - * Selector: '/Selector' + /* LookupNDDirect: '/z_commutMap_M1' incorporates: + * Constant: '/vec_hallToPos' + * Selector: '/Selector' * - * About '/z_commutMap_M1': + * About '/z_commutMap_M1': * 2-dimensional Direct Look-Up returning a Column */ rtb_DataTypeConversion = rtb_Sum2_h * 3; - rtb_Switch2_d = (int16_T)(rtb_Merge * + + /* Product: '/Divide2' incorporates: + * LookupNDDirect: '/z_commutMap_M1' + * + * About '/z_commutMap_M1': + * 2-dimensional Direct Look-Up returning a Column + */ + rtb_Saturation = (int16_T)(rtb_Merge * rtConstP.z_commutMap_M1_table[rtb_DataTypeConversion]); - rtb_Saturation = (int16_T)(rtConstP.z_commutMap_M1_table[1 + + rtb_id_fieldWeak_M1 = (int16_T)(rtConstP.z_commutMap_M1_table[1 + rtb_DataTypeConversion] * rtb_Merge); - rtb_id_fieldWeak_M1 = (int16_T)(rtConstP.z_commutMap_M1_table[2 + + rtb_Merge1 = (int16_T)(rtConstP.z_commutMap_M1_table[2 + rtb_DataTypeConversion] * rtb_Merge); + + /* End of Outputs for SubSystem: '/F05_00_COM_Method' */ } - /* End of Switch: '/Switch1' */ + /* End of If: '/If' */ - /* Update for UnitDelay: '/UnitDelay3' incorporates: + /* Outport: '/DC_phaA' incorporates: + * DataTypeConversion: '/Data Type Conversion6' + */ + rtY->DC_phaA = (int16_T)(rtb_Saturation >> 4); + + /* Outport: '/DC_phaB' incorporates: + * DataTypeConversion: '/Data Type Conversion6' + */ + rtY->DC_phaB = (int16_T)(rtb_id_fieldWeak_M1 >> 4); + + /* Update for UnitDelay: '/UnitDelay3' incorporates: * Inport: '/b_hallA ' */ rtDW->UnitDelay3_DSTATE_fy = rtU->b_hallA; - /* Update for UnitDelay: '/UnitDelay1' incorporates: + /* Update for UnitDelay: '/UnitDelay1' incorporates: * Inport: '/b_hallB' */ rtDW->UnitDelay1_DSTATE = rtU->b_hallB; - /* Update for UnitDelay: '/UnitDelay2' incorporates: + /* Update for UnitDelay: '/UnitDelay2' incorporates: * Inport: '/b_hallC' */ rtDW->UnitDelay2_DSTATE_f = rtU->b_hallC; - /* Update for UnitDelay: '/UnitDelay3' */ + /* Update for UnitDelay: '/UnitDelay3' */ rtDW->UnitDelay3_DSTATE = rtb_Switch1_l; - /* Update for UnitDelay: '/UnitDelay4' */ + /* Update for UnitDelay: '/UnitDelay4' */ rtDW->UnitDelay4_DSTATE_e = rtb_Abs5; /* Update for UnitDelay: '/UnitDelay4' */ - rtDW->UnitDelay4_DSTATE = rtb_Merge; + rtDW->UnitDelay4_DSTATE_eu = rtb_Merge; /* Update for UnitDelay: '/UnitDelay1' */ rtDW->UnitDelay1_DSTATE_p = rtb_Sum_l; - /* End of Outputs for SubSystem: '/BLDC_controller' */ + /* Outport: '/DC_phaC' incorporates: + * DataTypeConversion: '/Data Type Conversion6' + */ + rtY->DC_phaC = (int16_T)(rtb_Merge1 >> 4); - /* Outport: '/DC_phaA' */ - rtY->DC_phaA = rtb_Switch2_d; - - /* Outport: '/DC_phaB' */ - rtY->DC_phaB = rtb_Saturation; - - /* Outport: '/DC_phaC' */ - rtY->DC_phaC = rtb_id_fieldWeak_M1; - - /* Outputs for Atomic SubSystem: '/BLDC_controller' */ /* Outport: '/n_mot' incorporates: * DataTypeConversion: '/Data Type Conversion1' */ - rtY->n_mot = (int16_T)(rtb_Switch2_fv >> 4); + rtY->n_mot = (int16_T)(rtb_Switch2_k >> 4); /* Outport: '/a_elecAngle' incorporates: * DataTypeConversion: '/Data Type Conversion7' */ - rtY->a_elecAngle = (int16_T)((uint32_T)rtb_Divide2_h >> 6); + rtY->a_elecAngle = (int16_T)(rtb_Switch2_fl >> 6); /* Outport: '/r_devSignal1' incorporates: * DataTypeConversion: '/Data Type Conversion4' @@ -2605,6 +2711,9 @@ void BLDC_controller_initialize(RT_MODEL *const rtM) /* Start for If: '/If2' */ rtDW->If2_ActiveSubsystem = -1; + /* Start for If: '/If2' */ + rtDW->If2_ActiveSubsystem_j = -1; + /* Start for If: '/If1' */ rtDW->If1_ActiveSubsystem = -1; @@ -2615,10 +2724,10 @@ void BLDC_controller_initialize(RT_MODEL *const rtM) /* Start for If: '/If1' */ rtDW->If1_ActiveSubsystem_e = -1; - /* Start for If: '/If1' */ + /* Start for If: '/If1' */ rtDW->If1_ActiveSubsystem_f = -1; - /* Start for If: '/If2' */ + /* Start for If: '/If2' */ rtDW->If2_ActiveSubsystem_c = -1; /* Start for SwitchCase: '/Switch Case' */ @@ -2628,19 +2737,19 @@ void BLDC_controller_initialize(RT_MODEL *const rtM) /* End of Start for SubSystem: '/BLDC_controller' */ /* SystemInitialize for Atomic SubSystem: '/BLDC_controller' */ - /* InitializeConditions for UnitDelay: '/UnitDelay3' */ + /* InitializeConditions for UnitDelay: '/UnitDelay3' */ rtDW->UnitDelay3_DSTATE = rtP->z_maxCntRst; - /* SystemInitialize for IfAction SubSystem: '/Raw_Motor_Speed_Estimation' */ - /* SystemInitialize for Outport: '/z_counter' */ + /* SystemInitialize for IfAction SubSystem: '/Raw_Motor_Speed_Estimation' */ + /* SystemInitialize for Outport: '/z_counter' */ rtDW->z_counterRawPrev = rtP->z_maxCntRst; - /* End of SystemInitialize for SubSystem: '/Raw_Motor_Speed_Estimation' */ + /* End of SystemInitialize for SubSystem: '/Raw_Motor_Speed_Estimation' */ - /* SystemInitialize for Atomic SubSystem: '/Counter' */ + /* SystemInitialize for Atomic SubSystem: '/Counter' */ Counter_Init(&rtDW->Counter_e, rtP->z_maxCntRst); - /* End of SystemInitialize for SubSystem: '/Counter' */ + /* End of SystemInitialize for SubSystem: '/Counter' */ /* SystemInitialize for IfAction SubSystem: '/F02_Diagnostics' */ @@ -2651,15 +2760,13 @@ void BLDC_controller_initialize(RT_MODEL *const rtM) /* End of SystemInitialize for SubSystem: '/F02_Diagnostics' */ - /* SystemInitialize for IfAction SubSystem: '/F04_Field_Oriented_Control' */ - /* SystemInitialize for IfAction SubSystem: '/Open_Mode' */ - /* SystemInitialize for Atomic SubSystem: '/rising_edge_init' */ - /* InitializeConditions for UnitDelay: '/UnitDelay' */ - rtDW->UnitDelay_DSTATE_g = true; + /* SystemInitialize for IfAction SubSystem: '/Open_Mode' */ + /* SystemInitialize for Atomic SubSystem: '/rising_edge_init' */ + /* InitializeConditions for UnitDelay: '/UnitDelay' */ + rtDW->UnitDelay_DSTATE_e = true; - /* End of SystemInitialize for SubSystem: '/rising_edge_init' */ - /* End of SystemInitialize for SubSystem: '/Open_Mode' */ - /* End of SystemInitialize for SubSystem: '/F04_Field_Oriented_Control' */ + /* End of SystemInitialize for SubSystem: '/rising_edge_init' */ + /* End of SystemInitialize for SubSystem: '/Open_Mode' */ /* End of SystemInitialize for SubSystem: '/BLDC_controller' */ } diff --git a/Src/BLDC_controller_data.c b/Src/BLDC_controller_data.c index 7c7dd15..16b45f7 100644 --- a/Src/BLDC_controller_data.c +++ b/Src/BLDC_controller_data.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1199 + * Model version : 1.1212 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Sun Nov 3 12:28:16 2019 + * C/C++ source code generated on : Sat Nov 30 08:54:28 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -21,13 +21,74 @@ /* Constant parameters (auto storage) */ const ConstP rtConstP = { - /* Computed Parameter: z_commutMap_M1_table - * Referenced by: '/z_commutMap_M1' + /* Computed Parameter: r_sin3PhaA_M1_Table + * Referenced by: '/r_sin3PhaA_M1' */ - { -1, 1, 0, -1, 0, 1, 0, -1, 1, 1, -1, 0, 1, 0, -1, 0, 1, -1 }, + { -13091, -13634, -14126, -14565, -14953, -15289, -15577, -15816, -16009, + -16159, -16269, -16340, -16377, -16383, -16362, -16317, -16253, -16172, + -16079, -15977, -15870, -15762, -15656, -15555, -15461, -15377, -15306, + -15248, -15206, -15180, -15172, -15180, -15206, -15248, -15306, -15377, + -15461, -15555, -15656, -15762, -15870, -15977, -16079, -16172, -16253, + -16317, -16362, -16383, -16377, -16340, -16269, -16159, -16009, -15816, + -15577, -15289, -14953, -14565, -14126, -13634, -13091, -12496, -11849, + -11154, -10411, -9623, -8791, -7921, -7014, -6075, -5107, -4115, -3104, + -2077, -1041, 0, 1041, 2077, 3104, 4115, 5107, 6075, 7014, 7921, 8791, 9623, + 10411, 11154, 11849, 12496, 13091, 13634, 14126, 14565, 14953, 15289, 15577, + 15816, 16009, 16159, 16269, 16340, 16377, 16383, 16362, 16317, 16253, 16172, + 16079, 15977, 15870, 15762, 15656, 15555, 15461, 15377, 15306, 15248, 15206, + 15180, 15172, 15180, 15206, 15248, 15306, 15377, 15461, 15555, 15656, 15762, + 15870, 15977, 16079, 16172, 16253, 16317, 16362, 16383, 16377, 16340, 16269, + 16159, 16009, 15816, 15577, 15289, 14953, 14565, 14126, 13634, 13091, 12496, + 11849, 11154, 10411, 9623, 8791, 7921, 7014, 6075, 5107, 4115, 3104, 2077, + 1041, 0, -1041, -2077, -3104, -4115, -5107, -6075, -7014, -7921, -8791, + -9623, -10411, -11154, -11849, -12496, -13091 }, + + /* Computed Parameter: r_sin3PhaB_M1_Table + * Referenced by: '/r_sin3PhaB_M1' + */ + { 15172, 15180, 15206, 15248, 15306, 15377, 15461, 15555, 15656, 15762, 15870, + 15977, 16079, 16172, 16253, 16317, 16362, 16383, 16377, 16340, 16269, 16159, + 16009, 15816, 15577, 15289, 14953, 14565, 14126, 13634, 13091, 12496, 11849, + 11154, 10411, 9623, 8791, 7921, 7014, 6075, 5107, 4115, 3104, 2077, 1041, 0, + -1041, -2077, -3104, -4115, -5107, -6075, -7014, -7921, -8791, -9623, -10411, + -11154, -11849, -12496, -13091, -13634, -14126, -14565, -14953, -15289, + -15577, -15816, -16009, -16159, -16269, -16340, -16377, -16383, -16362, + -16317, -16253, -16172, -16079, -15977, -15870, -15762, -15656, -15555, + -15461, -15377, -15306, -15248, -15206, -15180, -15172, -15180, -15206, + -15248, -15306, -15377, -15461, -15555, -15656, -15762, -15870, -15977, + -16079, -16172, -16253, -16317, -16362, -16383, -16377, -16340, -16269, + -16159, -16009, -15816, -15577, -15289, -14953, -14565, -14126, -13634, + -13091, -12496, -11849, -11154, -10411, -9623, -8791, -7921, -7014, -6075, + -5107, -4115, -3104, -2077, -1041, 0, 1041, 2077, 3104, 4115, 5107, 6075, + 7014, 7921, 8791, 9623, 10411, 11154, 11849, 12496, 13091, 13634, 14126, + 14565, 14953, 15289, 15577, 15816, 16009, 16159, 16269, 16340, 16377, 16383, + 16362, 16317, 16253, 16172, 16079, 15977, 15870, 15762, 15656, 15555, 15461, + 15377, 15306, 15248, 15206, 15180, 15172 }, + + /* Computed Parameter: r_sin3PhaC_M1_Table + * Referenced by: '/r_sin3PhaC_M1' + */ + { -13091, -12496, -11849, -11154, -10411, -9623, -8791, -7921, -7014, -6075, + -5107, -4115, -3104, -2077, -1041, 0, 1041, 2077, 3104, 4115, 5107, 6075, + 7014, 7921, 8791, 9623, 10411, 11154, 11849, 12496, 13091, 13634, 14126, + 14565, 14953, 15289, 15577, 15816, 16009, 16159, 16269, 16340, 16377, 16383, + 16362, 16317, 16253, 16172, 16079, 15977, 15870, 15762, 15656, 15555, 15461, + 15377, 15306, 15248, 15206, 15180, 15172, 15180, 15206, 15248, 15306, 15377, + 15461, 15555, 15656, 15762, 15870, 15977, 16079, 16172, 16253, 16317, 16362, + 16383, 16377, 16340, 16269, 16159, 16009, 15816, 15577, 15289, 14953, 14565, + 14126, 13634, 13091, 12496, 11849, 11154, 10411, 9623, 8791, 7921, 7014, + 6075, 5107, 4115, 3104, 2077, 1041, 0, -1041, -2077, -3104, -4115, -5107, + -6075, -7014, -7921, -8791, -9623, -10411, -11154, -11849, -12496, -13091, + -13634, -14126, -14565, -14953, -15289, -15577, -15816, -16009, -16159, + -16269, -16340, -16377, -16383, -16362, -16317, -16253, -16172, -16079, + -15977, -15870, -15762, -15656, -15555, -15461, -15377, -15306, -15248, + -15206, -15180, -15172, -15180, -15206, -15248, -15306, -15377, -15461, + -15555, -15656, -15762, -15870, -15977, -16079, -16172, -16253, -16317, + -16362, -16383, -16377, -16340, -16269, -16159, -16009, -15816, -15577, + -15289, -14953, -14565, -14126, -13634, -13091 }, /* Computed Parameter: r_sin_M1_Table - * Referenced by: '/r_sin_M1' + * Referenced by: '/r_sin_M1' */ { 8192, 8682, 9162, 9630, 10087, 10531, 10963, 11381, 11786, 12176, 12551, 12911, 13255, 13583, 13894, 14189, 14466, 14726, 14968, 15191, 15396, 15582, @@ -48,7 +109,7 @@ const ConstP rtConstP = { 2280, 2845, 3406, 3964, 4516, 5063, 5604, 6138, 6664, 7182, 7692, 8192 }, /* Computed Parameter: r_cos_M1_Table - * Referenced by: '/r_cos_M1' + * Referenced by: '/r_cos_M1' */ { 14189, 13894, 13583, 13255, 12911, 12551, 12176, 11786, 11381, 10963, 10531, 10087, 9630, 9162, 8682, 8192, 7692, 7182, 6664, 6138, 5604, 5063, 4516, @@ -69,7 +130,7 @@ const ConstP rtConstP = { 16026, 15897, 15749, 15582, 15396, 15191, 14968, 14726, 14466, 14189 }, /* Computed Parameter: iq_maxSca_M1_Table - * Referenced by: '/iq_maxSca_M1' + * Referenced by: '/iq_maxSca_M1' */ { 65535U, 65523U, 65484U, 65418U, 65326U, 65207U, 65062U, 64890U, 64691U, 64465U, 64211U, 63930U, 63620U, 63281U, 62913U, 62516U, 62088U, 61630U, @@ -78,45 +139,45 @@ const ConstP rtConstP = { 45470U, 44069U, 42581U, 40997U, 39307U, 37494U, 35541U, 33422U, 31105U, 28540U, 25655U, 22323U, 18304U, 12974U }, + /* Computed Parameter: z_commutMap_M1_table + * Referenced by: '/z_commutMap_M1' + */ + { -1, 1, 0, -1, 0, 1, 0, -1, 1, 1, -1, 0, 1, 0, -1, 0, 1, -1 }, + /* Computed Parameter: vec_hallToPos_Value - * Referenced by: '/vec_hallToPos' + * Referenced by: '/vec_hallToPos' */ { 0, 2, 0, 1, 4, 3, 5, 0 } }; P rtP_Left = { /* Variable: dV_openRate - * Referenced by: '/dV_openRate' + * Referenced by: '/dV_openRate' */ 4096, /* Variable: dz_cntTrnsDetHi - * Referenced by: '/dz_cntTrnsDet' + * Referenced by: '/dz_cntTrnsDet' */ 40, /* Variable: dz_cntTrnsDetLo - * Referenced by: '/dz_cntTrnsDet' + * Referenced by: '/dz_cntTrnsDet' */ 20, - /* Variable: r_errInpTgtThres - * Referenced by: '/r_errInpTgtThres' - */ - 200, - /* Variable: z_maxCntRst * Referenced by: - * '/Counter' - * '/z_maxCntRst' - * '/z_maxCntRst2' - * '/UnitDelay3' - * '/z_counter' + * '/Counter' + * '/z_maxCntRst' + * '/z_maxCntRst2' + * '/UnitDelay3' + * '/z_counter' */ 2000, /* Variable: cf_speedCoef - * Referenced by: '/cf_speedCoef' + * Referenced by: '/cf_speedCoef' */ 10667U, @@ -130,30 +191,15 @@ P rtP_Left = { */ 9600U, - /* Variable: cf_idKp - * Referenced by: '/cf_idKp1' - */ - 819U, - - /* Variable: cf_iqKp - * Referenced by: '/cf_iqKp' - */ - 2048U, - - /* Variable: cf_nKp - * Referenced by: '/cf_nKp' - */ - 4833U, - /* Variable: Vd_max * Referenced by: - * '/Vd_max1' - * '/Vd_max' + * '/Vd_max1' + * '/Vd_max' */ 14400, /* Variable: Vq_max_M1 - * Referenced by: '/Vq_max_M1' + * Referenced by: '/Vq_max_M1' */ { 14400, 14396, 14386, 14368, 14343, 14311, 14271, 14225, 14171, 14109, 14040, 13963, 13879, 13786, 13685, 13576, 13459, 13333, 13198, 13053, 12900, 12736, @@ -161,7 +207,7 @@ P rtP_Left = { 9790, 9433, 9051, 8640, 8196, 7713, 7184, 6597, 5935, 5170, 4245, 3019, 0 }, /* Variable: Vq_max_XA - * Referenced by: '/Vq_max_XA' + * Referenced by: '/Vq_max_XA' */ { 0, 320, 640, 960, 1280, 1600, 1920, 2240, 2560, 2880, 3200, 3520, 3840, 4160, 4480, 4800, 5120, 5440, 5760, 6080, 6400, 6720, 7040, 7360, 7680, 8000, 8320, @@ -170,93 +216,127 @@ P rtP_Left = { /* Variable: i_max * Referenced by: - * '/i_max' - * '/i_max' + * '/i_max' + * '/i_max' */ 12000, /* Variable: id_fieldWeak_M1 - * Referenced by: '/id_fieldWeak_M1' + * Referenced by: '/id_fieldWeak_M1' */ { 0, 80, 240, 560, 1040, 1680, 2400, 3040, 3520, 3840, 4000, 4000 }, /* Variable: n_commAcvLo - * Referenced by: '/n_commDeacv' + * Referenced by: '/n_commDeacv' */ 240, /* Variable: n_commDeacvHi - * Referenced by: '/n_commDeacv' + * Referenced by: '/n_commDeacv' */ 480, /* Variable: n_fieldWeakAuthHi - * Referenced by: '/n_fieldWeakAuth' + * Referenced by: + * '/n_fieldWeakAuth' + * '/n_fieldWeakAuth' */ 3200, /* Variable: n_fieldWeakAuthLo - * Referenced by: '/n_fieldWeakAuth' + * Referenced by: + * '/n_fieldWeakAuth' + * '/n_fieldWeakAuth' */ 2240, /* Variable: n_max * Referenced by: - * '/n_max1' - * '/n_max' + * '/n_max1' + * '/n_max' */ 12800, /* Variable: n_stdStillDet - * Referenced by: '/n_stdStillDet' + * Referenced by: '/n_stdStillDet' */ 48, + /* Variable: r_errInpTgtThres + * Referenced by: '/r_errInpTgtThres' + */ + 6400, + /* Variable: r_fieldWeak_XA - * Referenced by: '/r_fieldWeak_XA' + * Referenced by: '/r_fieldWeak_XA' */ { 9120, 9600, 10080, 10560, 11040, 11520, 12000, 12480, 12960, 13440, 13920, 14400 }, + /* Variable: r_phaAdv_XA + * Referenced by: '/r_phaAdv_XA' + */ + { 0, 1600, 3200, 4800, 6400, 8000, 9600, 11200, 12800, 14400, 16000 }, + + /* Variable: cf_idKp + * Referenced by: '/cf_idKp1' + */ + 819U, + + /* Variable: cf_iqKp + * Referenced by: '/cf_iqKp' + */ + 2048U, + + /* Variable: cf_nKp + * Referenced by: '/cf_nKp' + */ + 4833U, + /* Variable: cf_currFilt - * Referenced by: '/cf_currFilt' + * Referenced by: '/cf_currFilt' */ 7864U, /* Variable: cf_idKi - * Referenced by: '/cf_idKi1' + * Referenced by: '/cf_idKi1' */ 246U, /* Variable: cf_iqKi - * Referenced by: '/cf_iqKi' + * Referenced by: '/cf_iqKi' */ 410U, /* Variable: cf_iqKiLimProt - * Referenced by: '/cf_iqKiLimProt' + * Referenced by: '/cf_iqKiLimProt' */ 167U, /* Variable: cf_nKi - * Referenced by: '/cf_nKi' + * Referenced by: '/cf_nKi' */ 84U, /* Variable: cf_iqKpLimProt - * Referenced by: '/cf_iqKpLimProt' + * Referenced by: '/cf_iqKpLimProt' */ 1843U, /* Variable: cf_nKpLimProt - * Referenced by: '/cf_nKpLimProt' + * Referenced by: '/cf_nKpLimProt' */ 1280U, + /* Variable: a_phaAdv_M1 + * Referenced by: '/a_phaAdv_M1' + */ + { 0, 0, 0, 0, 0, 512, 768, 1280, 2304, 4096, 6400 }, + /* Variable: z_ctrlTypSel * Referenced by: '/z_ctrlTypSel1' */ - 1U, + 2U, /* Variable: b_diagEna * Referenced by: '/b_diagEna' @@ -264,12 +344,14 @@ P rtP_Left = { 1, /* Variable: b_fieldWeakEna - * Referenced by: '/b_fieldWeakEna' + * Referenced by: + * '/b_fieldWeakEna' + * '/b_fieldWeakEna' */ 0, /* Variable: b_selPhaABCurrMeas - * Referenced by: '/b_selPhaABCurrMeas' + * Referenced by: '/b_selPhaABCurrMeas' */ 1 }; /* Modifiable parameters */ diff --git a/Src/main.c b/Src/main.c index d50a2e8..8c3b85a 100644 --- a/Src/main.c +++ b/Src/main.c @@ -74,7 +74,7 @@ typedef struct{ uint16_t checksum; } Serialcommand; static volatile Serialcommand command; -static int16_t timeoutCnt = 0 // Timeout counter for Rx Serial command +static int16_t timeoutCnt = 0; // Timeout counter for Rx Serial command #endif static uint8_t timeoutFlag = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) @@ -99,7 +99,8 @@ static uint8_t serialSendCounter; // serial send counter static uint8_t button1, button2; #endif -uint8_t ctrlModReq = CTRL_MOD_REQ; +uint8_t ctrlModReqRaw = CTRL_MOD_REQ; +uint8_t ctrlModReq; // Final control mode request static int cmd1; // normalized input value. -1000 to 1000 static int cmd2; // normalized input value. -1000 to 1000 static int16_t steer; // local variable for steering. -1000 to 1000 @@ -349,7 +350,7 @@ int main(void) { cmd1 = 0; cmd2 = 0; } else { - ctrlModReq = CTRL_MOD_REQ; // Follow the Mode request + ctrlModReq = ctrlModReqRaw; // Follow the Mode request } timeout = 0; diff --git a/build/firmware.bin b/build/firmware.bin index 7593664..58f8369 100644 Binary files a/build/firmware.bin and b/build/firmware.bin differ diff --git a/build/firmware.elf b/build/firmware.elf index 67bfa45..f26bded 100644 Binary files a/build/firmware.elf and b/build/firmware.elf differ diff --git a/docs/pictures/paramTable.png b/docs/pictures/paramTable.png index 780e6ce..8ac96f6 100644 Binary files a/docs/pictures/paramTable.png and b/docs/pictures/paramTable.png differ