diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvoptx b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvoptx index 17d1d0f4b5..475e06d379 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvoptx +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvoptx @@ -770,7 +770,7 @@ stm32hal - 1 + 0 0 0 0 @@ -922,7 +922,7 @@ embot::hw - 0 + 1 0 0 0 @@ -1602,7 +1602,7 @@ mbd - 0 + 1 0 0 0 diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvprojx b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvprojx index ec5884669d..ff517642d1 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvprojx +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application.uvprojx @@ -10,7 +10,7 @@ amcbldc-app-osal-ulpro 0x4 ARM-ADS - 6180000::V6.18::ARMCLANG + 6190000::V6.19::ARMCLANG 1 diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.cpp index 2f872c3a50..b1c6a8feec 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.cpp @@ -1,675 +1,675 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: AMC_BLDC.cpp -// -// Code generated for Simulink model 'AMC_BLDC'. -// -// Model version : 5.1 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:48 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "AMC_BLDC.h" -#include "rtw_mutex.h" -#include "AMC_BLDC_types.h" -#include "rtwtypes.h" -#define estimation_velocity_MDLREF_HIDE_CHILD_ -#include "estimation_velocity.h" -#define filter_current_MDLREF_HIDE_CHILD_ -#include "filter_current.h" -#define control_foc_MDLREF_HIDE_CHILD_ -#include "control_foc.h" -#define can_decoder_MDLREF_HIDE_CHILD_ -#include "can_decoder.h" -#define can_encoder_MDLREF_HIDE_CHILD_ -#include "can_encoder.h" -#define control_outer_MDLREF_HIDE_CHILD_ -#include "control_outer.h" -#define SupervisorFSM_RX_MDLREF_HIDE_CHILD_ -#include "SupervisorFSM_RX.h" -#define SupervisorFSM_TX_MDLREF_HIDE_CHILD_ -#include "SupervisorFSM_TX.h" - -// Exported block parameters -ConfigurationParameters InitConfParams = { - { - true, - true, - false, - false, - true, - false, - 16000, - 0, - 0U, - 7U, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 24.0F, - 24.0F - }, - - { - EstimationVelocityModes_MovingAverage - }, - - { - 0.0F, - 0.0F, - 2.0F, - 500.0F, - 0.0F, - 10.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 0.0F, - 0.0F, - -3.0e-5F, - -3.0e-5F, - 0.0F, - 10.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 1.0F, - 359.0F, - 0.0F, - 360.0F, - 0.0F, - 0.0F, - 40000.0F, - 10U, - 2.0F, - 5.0F, - 10.0F, - 32000U - } -} ; // Variable: InitConfParams - // Referenced by: '/SupervisorFSM_RX' - - -real32_T CAN_ANGLE_DEG2ICUB = 182.044449F;// Variable: CAN_ANGLE_DEG2ICUB - // Referenced by: '/CAN_Encoder' - // 2^16/360 - -real32_T CAN_ANGLE_ICUB2DEG = 0.00549316406F;// Variable: CAN_ANGLE_ICUB2DEG - // Referenced by: '/CAN_Decoder' - // 360/2^16 - -uint8_T CAN_ID_AMC = 3U; // Variable: CAN_ID_AMC - // Referenced by: - // '/CAN_Decoder' - // '/CAN_Encoder' - // 4 bits defining the ID of the AMC_BLDC board. - - -// Block signals (default storage) -B_AMC_BLDC_T AMC_BLDC_B; - -// Block states (default storage) -DW_AMC_BLDC_T AMC_BLDC_DW; - -// External inputs (root inport signals with default storage) -ExtU_AMC_BLDC_T AMC_BLDC_U; - -// External outputs (root outports fed by signals with default storage) -ExtY_AMC_BLDC_T AMC_BLDC_Y; - -// Real-time model -RT_MODEL_AMC_BLDC_T AMC_BLDC_M_ = RT_MODEL_AMC_BLDC_T(); -RT_MODEL_AMC_BLDC_T *const AMC_BLDC_M = &AMC_BLDC_M_; - -// Model step function for TID0 -void AMC_BLDC_step0(void) // Sample time: [1.1428571428571438E-6s, 0.0s] -{ - // (no output/update code required) -} - -// Model step function for TID1 -void AMC_BLDC_step_FOC(void) // Sample time: [3.65714285714286E-5s, 0.0s] -{ - FOCSlowInputs rtb_BusConversion_InsertedFor_F; - int8_T wrBufIdx; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_p = - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_hj; - rtw_mutex_unlock(); - - // BusCreator generated from: '/FOC' incorporates: - // RateTransition generated from: '/Adapter2' - - rtb_BusConversion_InsertedFor_F.flags = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_l[AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_p]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_m3 = - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mp; - rtw_mutex_unlock(); - - // BusCreator generated from: '/FOC' incorporates: - // RateTransition generated from: '/Adapter2' - - rtb_BusConversion_InsertedFor_F.configurationparameters = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedF[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_m3]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_ko = - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_b2; - rtw_mutex_unlock(); - - // BusCreator generated from: '/FOC' incorporates: - // RateTransition generated from: '/Adapter2' - - rtb_BusConversion_InsertedFor_F.estimateddata = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_k[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_ko]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mb = - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_jj; - rtw_mutex_unlock(); - - // BusCreator generated from: '/FOC' incorporates: - // RateTransition generated from: '/Adapter2' - - rtb_BusConversion_InsertedFor_F.targets = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_m[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mb]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_bw = - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_p5; - rtw_mutex_unlock(); - - // BusCreator generated from: '/FOC' incorporates: - // RateTransition generated from: '/Adapter2' - - rtb_BusConversion_InsertedFor_F.controlouteroutputs = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_i[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_bw]; - - // ModelReference: '/FOC' incorporates: - // Inport generated from: '/In Bus Element5' - // Outport generated from: '/Out Bus Element' - - control_foc(&AMC_BLDC_U.SensorsData_p, &rtb_BusConversion_InsertedFor_F, - &AMC_BLDC_Y.ControlOutputs_p); - - // RateTransition generated from: '/Adapter1' incorporates: - // Outport generated from: '/Out Bus Element' - - rtw_mutex_lock(); - wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_js + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_a) { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - switch (wrBufIdx) { - case 0: - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_d = AMC_BLDC_Y.ControlOutputs_p; - break; - - case 1: - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_j2 = AMC_BLDC_Y.ControlOutputs_p; - break; - - case 2: - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_o = AMC_BLDC_Y.ControlOutputs_p; - break; - } - - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_js = wrBufIdx; - - // End of RateTransition generated from: '/Adapter1' - - // RateTransition generated from: '/Adapter3' incorporates: - // Inport generated from: '/In Bus Element5' - - rtw_mutex_lock(); - wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_g + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_pa) { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - switch (wrBufIdx) { - case 0: - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_e = AMC_BLDC_U.SensorsData_p; - break; - - case 1: - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_c = AMC_BLDC_U.SensorsData_p; - break; - - case 2: - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_j = AMC_BLDC_U.SensorsData_p; - break; - } - - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_g = wrBufIdx; - - // End of RateTransition generated from: '/Adapter3' -} - -// Model step function for TID2 -void AMC_BLDC_step_Time(void) // Sample time: [0.001s, 0.0s] -{ - // local block i/o variables - Targets rtb_SupervisorFSM_RX_o2; - ControlOuterOutputs rtb_OuterControl; - BUS_STATUS_TX rtb_SupervisorFSM_TX_o2; - ControlOutputs rtb_BusConversion_InsertedFor_F; - int8_T wrBufIdx; - - // RateTransition generated from: '/Adapter1' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_a = - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_js; - rtw_mutex_unlock(); - switch (AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_a) { - case 0: - // RateTransition generated from: '/Adapter1' - AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_d; - break; - - case 1: - // RateTransition generated from: '/Adapter1' - AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a = - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_j2; - break; - - case 2: - // RateTransition generated from: '/Adapter1' - AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_o; - break; - } - - // End of RateTransition generated from: '/Adapter1' - - // BusCreator generated from: '/Filter_Current' - rtb_BusConversion_InsertedFor_F.Vq = 0.0F; - rtb_BusConversion_InsertedFor_F.Vabc[0] = 0.0F; - rtb_BusConversion_InsertedFor_F.Vabc[1] = 0.0F; - rtb_BusConversion_InsertedFor_F.Vabc[2] = 0.0F; - rtb_BusConversion_InsertedFor_F.Iq_fbk = - AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a.Iq_fbk; - - // RateTransition generated from: '/Adapter3' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_pa = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_g; - rtw_mutex_unlock(); - switch (AMC_BLDC_DW.RTBInsertedForAdapter_Insert_pa) { - case 0: - // RateTransition generated from: '/Adapter3' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedF = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_e; - break; - - case 1: - // RateTransition generated from: '/Adapter3' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedF = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_c; - break; - - case 2: - // RateTransition generated from: '/Adapter3' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedF = - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_j; - break; - } - - // End of RateTransition generated from: '/Adapter3' - - // UnitDelay generated from: '/Adapter4' - AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse = - AMC_BLDC_DW.ZOHBlockInsertedForAdapter_Inse; - - // ModelReference: '/Estimation_Velocity' - estimation_velocity(&AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, - &AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse, - &AMC_BLDC_Y.EstimatedData_p.jointvelocities); - - // ModelReference: '/Filter_Current' - filter_current(&rtb_BusConversion_InsertedFor_F, - &AMC_BLDC_Y.EstimatedData_p.Iq_filtered); - - // RateTransition generated from: '/Adapter2' incorporates: - // Outport generated from: '/Out Bus Element2' - - rtw_mutex_lock(); - wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_b2 + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_ko) { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_k[wrBufIdx] = - AMC_BLDC_Y.EstimatedData_p; - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_b2 = wrBufIdx; - - // ModelReference: '/CAN_Decoder' incorporates: - // Inport generated from: '/In Bus Element2' - - can_decoder(&AMC_BLDC_U.PacketsRx, &AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse, - &AMC_BLDC_B.CAN_Decoder_o1, &AMC_BLDC_B.CAN_Decoder_o2, - &AMC_BLDC_B.CAN_Decoder_o3); - - // ModelReference: '/SupervisorFSM_RX' incorporates: - // Inport generated from: '/In Bus Element' - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element2' - // Outport generated from: '/Out Bus Element4' - - SupervisorFSM_RX(&AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, - &AMC_BLDC_U.ExternalFlags_p, - &AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a, - &AMC_BLDC_B.CAN_Decoder_o1, &AMC_BLDC_B.CAN_Decoder_o2, - &AMC_BLDC_B.CAN_Decoder_o3, &AMC_BLDC_Y.Flags_p, - &rtb_SupervisorFSM_RX_o2, - &AMC_BLDC_Y.ConfigurationParameters_p); - - // ModelReference: '/SupervisorFSM_TX' incorporates: - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element2' - // Outport generated from: '/Out Bus Element4' - - SupervisorFSM_TX(&AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, - &AMC_BLDC_Y.EstimatedData_p, &AMC_BLDC_Y.Flags_p, - &AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a, - &AMC_BLDC_B.MessagesTx, &rtb_SupervisorFSM_TX_o2); - - // ModelReference: '/CAN_Encoder' incorporates: - // Outport generated from: '/Out Bus Element1' - - can_encoder(&AMC_BLDC_B.MessagesTx, &rtb_SupervisorFSM_TX_o2, - &AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse, &AMC_BLDC_Y.PacketsTx); - - // ModelReference: '/OuterControl' incorporates: - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element2' - // Outport generated from: '/Out Bus Element4' - - control_outer(&AMC_BLDC_Y.Flags_p, &AMC_BLDC_Y.ConfigurationParameters_p, - &rtb_SupervisorFSM_RX_o2, - &AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, - &AMC_BLDC_Y.EstimatedData_p, &rtb_OuterControl); - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_p5 + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_bw) { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_i[wrBufIdx] = rtb_OuterControl; - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_p5 = wrBufIdx; - - // RateTransition generated from: '/Adapter2' incorporates: - // Outport generated from: '/Out Bus Element4' - - rtw_mutex_lock(); - wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_hj + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_p) { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_l[wrBufIdx] = AMC_BLDC_Y.Flags_p; - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_hj = wrBufIdx; - - // RateTransition generated from: '/Adapter2' incorporates: - // Outport generated from: '/Out Bus Element3' - - rtw_mutex_lock(); - wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mp + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_m3) { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedF[wrBufIdx] = - AMC_BLDC_Y.ConfigurationParameters_p; - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mp = wrBufIdx; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_jj + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mb) { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_m[wrBufIdx] = - rtb_SupervisorFSM_RX_o2; - AMC_BLDC_DW.RTBInsertedForAdapter_Insert_jj = wrBufIdx; - - // Update for UnitDelay generated from: '/Adapter4' incorporates: - // Outport generated from: '/Out Bus Element3' - - AMC_BLDC_DW.ZOHBlockInsertedForAdapter_Inse = - AMC_BLDC_Y.ConfigurationParameters_p; -} - -// Model initialize function -void AMC_BLDC_initialize(void) -{ - // Model Initialize function for ModelReference Block: '/Estimation_Velocity' - estimation_velocity_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Model Initialize function for ModelReference Block: '/Filter_Current' - filter_current_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Model Initialize function for ModelReference Block: '/FOC' - control_foc_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Model Initialize function for ModelReference Block: '/CAN_Decoder' - can_decoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Model Initialize function for ModelReference Block: '/CAN_Encoder' - can_encoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Model Initialize function for ModelReference Block: '/OuterControl' - control_outer_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Model Initialize function for ModelReference Block: '/SupervisorFSM_RX' - SupervisorFSM_RX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Model Initialize function for ModelReference Block: '/SupervisorFSM_TX' - SupervisorFSM_TX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter1' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter3' - rtw_mutex_init(); - - // SystemInitialize for ModelReference: '/Estimation_Velocity' - estimation_velocity_Init(); - - // SystemInitialize for ModelReference: '/Filter_Current' - filter_current_Init(); - - // SystemInitialize for ModelReference: '/FOC' incorporates: - // Inport generated from: '/In Bus Element5' - // Outport generated from: '/Out Bus Element' - - control_foc_Init(); - - // SystemInitialize for ModelReference: '/CAN_Decoder' incorporates: - // Inport generated from: '/In Bus Element2' - - can_decoder_Init(); - - // SystemInitialize for ModelReference: '/OuterControl' incorporates: - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element4' - - control_outer_Init(); - - // SystemInitialize for ModelReference: '/SupervisorFSM_RX' incorporates: - // Inport generated from: '/In Bus Element' - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element4' - - SupervisorFSM_RX_Init(&AMC_BLDC_Y.Flags_p, - &AMC_BLDC_Y.ConfigurationParameters_p); - - // SystemInitialize for ModelReference: '/SupervisorFSM_TX' incorporates: - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element4' - - SupervisorFSM_TX_Init(&AMC_BLDC_B.MessagesTx); - - // Enable for ModelReference: '/OuterControl' incorporates: - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element2' - // Outport generated from: '/Out Bus Element4' - - control_outer_Enable(); -} - -// Model terminate function -void AMC_BLDC_terminate(void) -{ - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter1' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter3' - rtw_mutex_destroy(); - - // Terminate for ModelReference: '/Filter_Current' - filter_current_Term(); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: AMC_BLDC.cpp +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 5.1 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:32 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "AMC_BLDC.h" +#include "rtw_mutex.h" +#include "AMC_BLDC_types.h" +#include "rtwtypes.h" +#define estimation_velocity_MDLREF_HIDE_CHILD_ +#include "estimation_velocity.h" +#define filter_current_MDLREF_HIDE_CHILD_ +#include "filter_current.h" +#define control_foc_MDLREF_HIDE_CHILD_ +#include "control_foc.h" +#define can_decoder_MDLREF_HIDE_CHILD_ +#include "can_decoder.h" +#define can_encoder_MDLREF_HIDE_CHILD_ +#include "can_encoder.h" +#define control_outer_MDLREF_HIDE_CHILD_ +#include "control_outer.h" +#define SupervisorFSM_RX_MDLREF_HIDE_CHILD_ +#include "SupervisorFSM_RX.h" +#define SupervisorFSM_TX_MDLREF_HIDE_CHILD_ +#include "SupervisorFSM_TX.h" + +// Exported block parameters +ConfigurationParameters InitConfParams = { + { + true, + true, + false, + false, + true, + false, + 16000, + 0, + 0U, + 7U, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 24.0F, + 24.0F + }, + + { + EstimationVelocityModes_MovingAverage + }, + + { + 0.0F, + 0.0F, + 2.0F, + 500.0F, + 0.0F, + 10.0F, + 0.0F, + 0.0F, + 0U + }, + + { + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0U + }, + + { + 0.0F, + 0.0F, + -3.0e-5F, + -3.0e-5F, + 0.0F, + 10.0F, + 0.0F, + 0.0F, + 0U + }, + + { + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0U + }, + + { + 1.0F, + 359.0F, + 0.0F, + 360.0F, + 0.0F, + 0.0F, + 40000.0F, + 10U, + 2.0F, + 5.0F, + 10.0F, + 32000U + } +} ; // Variable: InitConfParams + // Referenced by: '/SupervisorFSM_RX' + + +real32_T CAN_ANGLE_DEG2ICUB = 182.044449F;// Variable: CAN_ANGLE_DEG2ICUB + // Referenced by: '/CAN_Encoder' + // 2^16/360 + +real32_T CAN_ANGLE_ICUB2DEG = 0.00549316406F;// Variable: CAN_ANGLE_ICUB2DEG + // Referenced by: '/CAN_Decoder' + // 360/2^16 + +uint8_T CAN_ID_AMC = 3U; // Variable: CAN_ID_AMC + // Referenced by: + // '/CAN_Decoder' + // '/CAN_Encoder' + // 4 bits defining the ID of the AMC_BLDC board. + + +// Block signals (default storage) +B_AMC_BLDC_T AMC_BLDC_B; + +// Block states (default storage) +DW_AMC_BLDC_T AMC_BLDC_DW; + +// External inputs (root inport signals with default storage) +ExtU_AMC_BLDC_T AMC_BLDC_U; + +// External outputs (root outports fed by signals with default storage) +ExtY_AMC_BLDC_T AMC_BLDC_Y; + +// Real-time model +RT_MODEL_AMC_BLDC_T AMC_BLDC_M_ = RT_MODEL_AMC_BLDC_T(); +RT_MODEL_AMC_BLDC_T *const AMC_BLDC_M = &AMC_BLDC_M_; + +// Model step function for TID0 +void AMC_BLDC_step0(void) // Sample time: [1.1428571428571438E-6s, 0.0s] +{ + // (no output/update code required) +} + +// Model step function for TID1 +void AMC_BLDC_step_FOC(void) // Sample time: [3.65714285714286E-5s, 0.0s] +{ + FOCSlowInputs rtb_BusConversion_InsertedFor_F; + int8_T wrBufIdx; + + // RateTransition generated from: '/Adapter2' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_p = + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_hj; + rtw_mutex_unlock(); + + // BusCreator generated from: '/FOC' incorporates: + // RateTransition generated from: '/Adapter2' + + rtb_BusConversion_InsertedFor_F.flags = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_l[AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_p]; + + // RateTransition generated from: '/Adapter2' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_m3 = + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mp; + rtw_mutex_unlock(); + + // BusCreator generated from: '/FOC' incorporates: + // RateTransition generated from: '/Adapter2' + + rtb_BusConversion_InsertedFor_F.configurationparameters = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedF[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_m3]; + + // RateTransition generated from: '/Adapter2' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_ko = + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_b2; + rtw_mutex_unlock(); + + // BusCreator generated from: '/FOC' incorporates: + // RateTransition generated from: '/Adapter2' + + rtb_BusConversion_InsertedFor_F.estimateddata = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_k[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_ko]; + + // RateTransition generated from: '/Adapter2' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mb = + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_jj; + rtw_mutex_unlock(); + + // BusCreator generated from: '/FOC' incorporates: + // RateTransition generated from: '/Adapter2' + + rtb_BusConversion_InsertedFor_F.targets = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_m[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mb]; + + // RateTransition generated from: '/Adapter2' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_bw = + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_p5; + rtw_mutex_unlock(); + + // BusCreator generated from: '/FOC' incorporates: + // RateTransition generated from: '/Adapter2' + + rtb_BusConversion_InsertedFor_F.controlouteroutputs = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_i[AMC_BLDC_DW.RTBInsertedForAdapter_Insert_bw]; + + // ModelReference: '/FOC' incorporates: + // Inport generated from: '/In Bus Element5' + // Outport generated from: '/Out Bus Element' + + control_foc(&AMC_BLDC_U.SensorsData_p, &rtb_BusConversion_InsertedFor_F, + &AMC_BLDC_Y.ControlOutputs_p); + + // RateTransition generated from: '/Adapter1' incorporates: + // Outport generated from: '/Out Bus Element' + + rtw_mutex_lock(); + wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_js + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_a) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + switch (wrBufIdx) { + case 0: + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_d = AMC_BLDC_Y.ControlOutputs_p; + break; + + case 1: + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_j2 = AMC_BLDC_Y.ControlOutputs_p; + break; + + case 2: + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_o = AMC_BLDC_Y.ControlOutputs_p; + break; + } + + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_js = wrBufIdx; + + // End of RateTransition generated from: '/Adapter1' + + // RateTransition generated from: '/Adapter3' incorporates: + // Inport generated from: '/In Bus Element5' + + rtw_mutex_lock(); + wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_g + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_pa) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + switch (wrBufIdx) { + case 0: + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_e = AMC_BLDC_U.SensorsData_p; + break; + + case 1: + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_c = AMC_BLDC_U.SensorsData_p; + break; + + case 2: + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_j = AMC_BLDC_U.SensorsData_p; + break; + } + + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_g = wrBufIdx; + + // End of RateTransition generated from: '/Adapter3' +} + +// Model step function for TID2 +void AMC_BLDC_step_Time(void) // Sample time: [0.001s, 0.0s] +{ + // local block i/o variables + Targets rtb_SupervisorFSM_RX_o2; + ControlOuterOutputs rtb_OuterControl; + BUS_STATUS_TX rtb_SupervisorFSM_TX_o2; + ControlOutputs rtb_BusConversion_InsertedFor_F; + int8_T wrBufIdx; + + // RateTransition generated from: '/Adapter1' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_a = + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_js; + rtw_mutex_unlock(); + switch (AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_a) { + case 0: + // RateTransition generated from: '/Adapter1' + AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_d; + break; + + case 1: + // RateTransition generated from: '/Adapter1' + AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a = + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_j2; + break; + + case 2: + // RateTransition generated from: '/Adapter1' + AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_o; + break; + } + + // End of RateTransition generated from: '/Adapter1' + + // BusCreator generated from: '/Filter_Current' + rtb_BusConversion_InsertedFor_F.Vq = 0.0F; + rtb_BusConversion_InsertedFor_F.Vabc[0] = 0.0F; + rtb_BusConversion_InsertedFor_F.Vabc[1] = 0.0F; + rtb_BusConversion_InsertedFor_F.Vabc[2] = 0.0F; + rtb_BusConversion_InsertedFor_F.Iq_fbk = + AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a.Iq_fbk; + + // RateTransition generated from: '/Adapter3' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_pa = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_g; + rtw_mutex_unlock(); + switch (AMC_BLDC_DW.RTBInsertedForAdapter_Insert_pa) { + case 0: + // RateTransition generated from: '/Adapter3' + AMC_BLDC_B.RTBInsertedForAdapter_InsertedF = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_e; + break; + + case 1: + // RateTransition generated from: '/Adapter3' + AMC_BLDC_B.RTBInsertedForAdapter_InsertedF = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_c; + break; + + case 2: + // RateTransition generated from: '/Adapter3' + AMC_BLDC_B.RTBInsertedForAdapter_InsertedF = + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_j; + break; + } + + // End of RateTransition generated from: '/Adapter3' + + // UnitDelay generated from: '/Adapter4' + AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse = + AMC_BLDC_DW.ZOHBlockInsertedForAdapter_Inse; + + // ModelReference: '/Estimation_Velocity' + estimation_velocity(&AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, + &AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse, + &AMC_BLDC_Y.EstimatedData_p.jointvelocities); + + // ModelReference: '/Filter_Current' + filter_current(&rtb_BusConversion_InsertedFor_F, + &AMC_BLDC_Y.EstimatedData_p.Iq_filtered); + + // RateTransition generated from: '/Adapter2' incorporates: + // Outport generated from: '/Out Bus Element2' + + rtw_mutex_lock(); + wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_b2 + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_ko) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_k[wrBufIdx] = + AMC_BLDC_Y.EstimatedData_p; + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_b2 = wrBufIdx; + + // ModelReference: '/CAN_Decoder' incorporates: + // Inport generated from: '/In Bus Element2' + + can_decoder(&AMC_BLDC_U.PacketsRx, &AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse, + &AMC_BLDC_B.CAN_Decoder_o1, &AMC_BLDC_B.CAN_Decoder_o2, + &AMC_BLDC_B.CAN_Decoder_o3); + + // ModelReference: '/SupervisorFSM_RX' incorporates: + // Inport generated from: '/In Bus Element' + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element2' + // Outport generated from: '/Out Bus Element4' + + SupervisorFSM_RX(&AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, + &AMC_BLDC_U.ExternalFlags_p, + &AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a, + &AMC_BLDC_B.CAN_Decoder_o1, &AMC_BLDC_B.CAN_Decoder_o2, + &AMC_BLDC_B.CAN_Decoder_o3, &AMC_BLDC_Y.Flags_p, + &rtb_SupervisorFSM_RX_o2, + &AMC_BLDC_Y.ConfigurationParameters_p); + + // ModelReference: '/SupervisorFSM_TX' incorporates: + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element2' + // Outport generated from: '/Out Bus Element4' + + SupervisorFSM_TX(&AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, + &AMC_BLDC_Y.EstimatedData_p, &AMC_BLDC_Y.Flags_p, + &AMC_BLDC_B.RTBInsertedForAdapter_Inserte_a, + &AMC_BLDC_B.MessagesTx, &rtb_SupervisorFSM_TX_o2); + + // ModelReference: '/CAN_Encoder' incorporates: + // Outport generated from: '/Out Bus Element1' + + can_encoder(&AMC_BLDC_B.MessagesTx, &rtb_SupervisorFSM_TX_o2, + &AMC_BLDC_B.ZOHBlockInsertedForAdapter_Inse, &AMC_BLDC_Y.PacketsTx); + + // ModelReference: '/OuterControl' incorporates: + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element2' + // Outport generated from: '/Out Bus Element4' + + control_outer(&AMC_BLDC_Y.Flags_p, &AMC_BLDC_Y.ConfigurationParameters_p, + &rtb_SupervisorFSM_RX_o2, + &AMC_BLDC_B.RTBInsertedForAdapter_InsertedF, + &AMC_BLDC_Y.EstimatedData_p, &rtb_OuterControl); + + // RateTransition generated from: '/Adapter2' + rtw_mutex_lock(); + wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_p5 + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_bw) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_i[wrBufIdx] = rtb_OuterControl; + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_p5 = wrBufIdx; + + // RateTransition generated from: '/Adapter2' incorporates: + // Outport generated from: '/Out Bus Element4' + + rtw_mutex_lock(); + wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_hj + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_p) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_l[wrBufIdx] = AMC_BLDC_Y.Flags_p; + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_hj = wrBufIdx; + + // RateTransition generated from: '/Adapter2' incorporates: + // Outport generated from: '/Out Bus Element3' + + rtw_mutex_lock(); + wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mp + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_m3) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedF[wrBufIdx] = + AMC_BLDC_Y.ConfigurationParameters_p; + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mp = wrBufIdx; + + // RateTransition generated from: '/Adapter2' + rtw_mutex_lock(); + wrBufIdx = static_cast(AMC_BLDC_DW.RTBInsertedForAdapter_Insert_jj + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == AMC_BLDC_DW.RTBInsertedForAdapter_Insert_mb) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + AMC_BLDC_DW.RTBInsertedForAdapter_Inserte_m[wrBufIdx] = + rtb_SupervisorFSM_RX_o2; + AMC_BLDC_DW.RTBInsertedForAdapter_Insert_jj = wrBufIdx; + + // Update for UnitDelay generated from: '/Adapter4' incorporates: + // Outport generated from: '/Out Bus Element3' + + AMC_BLDC_DW.ZOHBlockInsertedForAdapter_Inse = + AMC_BLDC_Y.ConfigurationParameters_p; +} + +// Model initialize function +void AMC_BLDC_initialize(void) +{ + // Model Initialize function for ModelReference Block: '/Estimation_Velocity' + estimation_velocity_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/Filter_Current' + filter_current_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/FOC' + control_foc_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/CAN_Decoder' + can_decoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/CAN_Encoder' + can_encoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/OuterControl' + control_outer_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/SupervisorFSM_RX' + SupervisorFSM_RX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/SupervisorFSM_TX' + SupervisorFSM_TX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Start for RateTransition generated from: '/Adapter2' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter2' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter2' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter2' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter2' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter1' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter3' + rtw_mutex_init(); + + // SystemInitialize for ModelReference: '/Estimation_Velocity' + estimation_velocity_Init(); + + // SystemInitialize for ModelReference: '/Filter_Current' + filter_current_Init(); + + // SystemInitialize for ModelReference: '/FOC' incorporates: + // Inport generated from: '/In Bus Element5' + // Outport generated from: '/Out Bus Element' + + control_foc_Init(); + + // SystemInitialize for ModelReference: '/CAN_Decoder' incorporates: + // Inport generated from: '/In Bus Element2' + + can_decoder_Init(); + + // SystemInitialize for ModelReference: '/OuterControl' incorporates: + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element4' + + control_outer_Init(); + + // SystemInitialize for ModelReference: '/SupervisorFSM_RX' incorporates: + // Inport generated from: '/In Bus Element' + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element4' + + SupervisorFSM_RX_Init(&AMC_BLDC_Y.Flags_p, + &AMC_BLDC_Y.ConfigurationParameters_p); + + // SystemInitialize for ModelReference: '/SupervisorFSM_TX' incorporates: + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element4' + + SupervisorFSM_TX_Init(&AMC_BLDC_B.MessagesTx); + + // Enable for ModelReference: '/OuterControl' incorporates: + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element2' + // Outport generated from: '/Out Bus Element4' + + control_outer_Enable(); +} + +// Model terminate function +void AMC_BLDC_terminate(void) +{ + // Terminate for RateTransition generated from: '/Adapter2' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter2' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter2' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter2' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter2' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter1' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter3' + rtw_mutex_destroy(); + + // Terminate for ModelReference: '/Filter_Current' + filter_current_Term(); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.h index 1c0554878a..79cb75b710 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC.h @@ -1,258 +1,258 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: AMC_BLDC.h -// -// Code generated for Simulink model 'AMC_BLDC'. -// -// Model version : 5.1 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:48 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_AMC_BLDC_h_ -#define RTW_HEADER_AMC_BLDC_h_ -#include "rtwtypes.h" -#include "AMC_BLDC_types.h" -#include -#include "zero_crossing_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) ((const char_T **)(&((rtm)->errorStatus))) -#endif - -#ifndef rtmStepTask -#define rtmStepTask(rtm, idx) ((rtm)->Timing.TaskCounters.TID[(idx)] == 0) -#endif - -#ifndef rtmTaskCounter -#define rtmTaskCounter(rtm, idx) ((rtm)->Timing.TaskCounters.TID[(idx)]) -#endif - -// Block signals (default storage) -struct B_AMC_BLDC_T { - BUS_MESSAGES_RX_MULTIPLE CAN_Decoder_o1;// '/CAN_Decoder' - ConfigurationParameters ZOHBlockInsertedForAdapter_Inse;// '/Adapter4' - BUS_MESSAGES_TX MessagesTx; // '/SupervisorFSM_TX' - SensorsData RTBInsertedForAdapter_InsertedF;// '/Adapter3' - BUS_STATUS_RX_MULTIPLE CAN_Decoder_o2;// '/CAN_Decoder' - BUS_CAN_RX_ERRORS_MULTIPLE CAN_Decoder_o3;// '/CAN_Decoder' - ControlOutputs RTBInsertedForAdapter_Inserte_a;// '/Adapter1' -}; - -// Block states (default storage) for system '' -struct DW_AMC_BLDC_T { - ConfigurationParameters ZOHBlockInsertedForAdapter_Inse;// synthesized block - ConfigurationParameters RTBInsertedForAdapter_InsertedF[3];// synthesized block - SensorsData RTBInsertedForAdapter_Inserte_e;// synthesized block - SensorsData RTBInsertedForAdapter_Inserte_c;// synthesized block - SensorsData RTBInsertedForAdapter_Inserte_j;// synthesized block - Targets RTBInsertedForAdapter_Inserte_m[3];// synthesized block - ControlOuterOutputs RTBInsertedForAdapter_Inserte_i[3];// synthesized block - ControlOutputs RTBInsertedForAdapter_Inserte_d;// synthesized block - ControlOutputs RTBInsertedForAdapter_Insert_j2;// synthesized block - ControlOutputs RTBInsertedForAdapter_Inserte_o;// synthesized block - EstimatedData RTBInsertedForAdapter_Inserte_k[3];// synthesized block - Flags RTBInsertedForAdapter_Inserte_l[3];// synthesized block - void* RTBInsertedForAdapter_Insert_mf;// synthesized block - void* RTBInsertedForAdapter_Inserte_b;// synthesized block - void* RTBInsertedForAdapter_Inserte_f;// synthesized block - void* RTBInsertedForAdapter_Insert_ey;// synthesized block - void* RTBInsertedForAdapter_Insert_ci;// synthesized block - void* RTBInsertedForAdapter_Inserte_h;// synthesized block - void* RTBInsertedForAdapter_Insert_bz;// synthesized block - int8_T RTBInsertedForAdapter_Insert_hj;// synthesized block - int8_T RTBInsertedForAdapter_Inserte_p;// synthesized block - int8_T RTBInsertedForAdapter_Insert_mp;// synthesized block - int8_T RTBInsertedForAdapter_Insert_m3;// synthesized block - int8_T RTBInsertedForAdapter_Insert_b2;// synthesized block - int8_T RTBInsertedForAdapter_Insert_ko;// synthesized block - int8_T RTBInsertedForAdapter_Insert_jj;// synthesized block - int8_T RTBInsertedForAdapter_Insert_mb;// synthesized block - int8_T RTBInsertedForAdapter_Insert_p5;// synthesized block - int8_T RTBInsertedForAdapter_Insert_bw;// synthesized block - int8_T RTBInsertedForAdapter_Insert_js;// synthesized block - int8_T RTBInsertedForAdapter_Inserte_a;// synthesized block - int8_T RTBInsertedForAdapter_Inserte_g;// synthesized block - int8_T RTBInsertedForAdapter_Insert_pa;// synthesized block -}; - -// External inputs (root inport signals with default storage) -struct ExtU_AMC_BLDC_T { - SensorsData SensorsData_p; // '/SensorsData' - ExternalFlags ExternalFlags_p; // '/ExternalFlags' - BUS_CAN_MULTIPLE PacketsRx; // '/PacketsRx' -}; - -// External outputs (root outports fed by signals with default storage) -struct ExtY_AMC_BLDC_T { - ControlOutputs ControlOutputs_p; // '/ControlOutputs' - ConfigurationParameters ConfigurationParameters_p;// '/ConfigurationParameters' - Flags Flags_p; // '/Flags' - EstimatedData EstimatedData_p; // '/EstimatedData' - BUS_CAN_MULTIPLE PacketsTx; // '/PacketsTx' -}; - -// Real-time Model Data Structure -struct tag_RTM_AMC_BLDC_T { - const char_T *errorStatus; - - // - // Timing: - // The following substructure contains information regarding - // the timing information for the model. - - struct { - struct { - uint32_T TID[3]; - } TaskCounters; - } Timing; -}; - -// Block signals (default storage) -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern struct B_AMC_BLDC_T AMC_BLDC_B; - -#ifdef __cplusplus - -} - -#endif - -// Block states (default storage) -extern struct DW_AMC_BLDC_T AMC_BLDC_DW; - -#ifdef __cplusplus - -extern "C" -{ - -#endif - - // External inputs (root inport signals with default storage) - extern struct ExtU_AMC_BLDC_T AMC_BLDC_U; - - // External outputs (root outports fed by signals with default storage) - extern struct ExtY_AMC_BLDC_T AMC_BLDC_Y; - -#ifdef __cplusplus - -} - -#endif - -// -// Exported Global Parameters -// -// Note: Exported global parameters are tunable parameters with an exported -// global storage class designation. Code generation will declare the memory for -// these parameters and exports their symbols. -// - -extern ConfigurationParameters InitConfParams;// Variable: InitConfParams - // Referenced by: '/SupervisorFSM_RX' - -extern real32_T CAN_ANGLE_DEG2ICUB; // Variable: CAN_ANGLE_DEG2ICUB - // Referenced by: '/CAN_Encoder' - // 2^16/360 - -extern real32_T CAN_ANGLE_ICUB2DEG; // Variable: CAN_ANGLE_ICUB2DEG - // Referenced by: '/CAN_Decoder' - // 360/2^16 - -extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC - // Referenced by: - // '/CAN_Decoder' - // '/CAN_Encoder' - // 4 bits defining the ID of the AMC_BLDC board. - - -#ifdef __cplusplus - -extern "C" -{ - -#endif - - // Model entry point functions - extern void AMC_BLDC_initialize(void); - extern void AMC_BLDC_step0(void); - extern void AMC_BLDC_step_FOC(void); - extern void AMC_BLDC_step_Time(void); - extern void AMC_BLDC_terminate(void); - -#ifdef __cplusplus - -} - -#endif - -// Real-time Model object -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern RT_MODEL_AMC_BLDC_T *const AMC_BLDC_M; - -#ifdef __cplusplus - -} - -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'AMC_BLDC' -// '' : 'AMC_BLDC/Adapter1' -// '' : 'AMC_BLDC/Adapter2' -// '' : 'AMC_BLDC/Adapter3' -// '' : 'AMC_BLDC/Adapter4' -// '' : 'AMC_BLDC/Estimation' -// '' : 'AMC_BLDC/Messaging' -// '' : 'AMC_BLDC/Supervision' -// '' : 'AMC_BLDC/Estimation/Adapter' -// '' : 'AMC_BLDC/Estimation/Mux' - -#endif // RTW_HEADER_AMC_BLDC_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: AMC_BLDC.h +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 5.1 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:32 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_AMC_BLDC_h_ +#define RTW_HEADER_AMC_BLDC_h_ +#include "rtwtypes.h" +#include "AMC_BLDC_types.h" +#include +#include "zero_crossing_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) ((const char_T **)(&((rtm)->errorStatus))) +#endif + +#ifndef rtmStepTask +#define rtmStepTask(rtm, idx) ((rtm)->Timing.TaskCounters.TID[(idx)] == 0) +#endif + +#ifndef rtmTaskCounter +#define rtmTaskCounter(rtm, idx) ((rtm)->Timing.TaskCounters.TID[(idx)]) +#endif + +// Block signals (default storage) +struct B_AMC_BLDC_T { + BUS_MESSAGES_RX_MULTIPLE CAN_Decoder_o1;// '/CAN_Decoder' + ConfigurationParameters ZOHBlockInsertedForAdapter_Inse;// '/Adapter4' + BUS_MESSAGES_TX MessagesTx; // '/SupervisorFSM_TX' + SensorsData RTBInsertedForAdapter_InsertedF;// '/Adapter3' + BUS_STATUS_RX_MULTIPLE CAN_Decoder_o2;// '/CAN_Decoder' + BUS_CAN_RX_ERRORS_MULTIPLE CAN_Decoder_o3;// '/CAN_Decoder' + ControlOutputs RTBInsertedForAdapter_Inserte_a;// '/Adapter1' +}; + +// Block states (default storage) for system '' +struct DW_AMC_BLDC_T { + ConfigurationParameters ZOHBlockInsertedForAdapter_Inse;// synthesized block + ConfigurationParameters RTBInsertedForAdapter_InsertedF[3];// synthesized block + SensorsData RTBInsertedForAdapter_Inserte_e;// synthesized block + SensorsData RTBInsertedForAdapter_Inserte_c;// synthesized block + SensorsData RTBInsertedForAdapter_Inserte_j;// synthesized block + Targets RTBInsertedForAdapter_Inserte_m[3];// synthesized block + ControlOuterOutputs RTBInsertedForAdapter_Inserte_i[3];// synthesized block + ControlOutputs RTBInsertedForAdapter_Inserte_d;// synthesized block + ControlOutputs RTBInsertedForAdapter_Insert_j2;// synthesized block + ControlOutputs RTBInsertedForAdapter_Inserte_o;// synthesized block + EstimatedData RTBInsertedForAdapter_Inserte_k[3];// synthesized block + Flags RTBInsertedForAdapter_Inserte_l[3];// synthesized block + void* RTBInsertedForAdapter_Insert_mf;// synthesized block + void* RTBInsertedForAdapter_Inserte_b;// synthesized block + void* RTBInsertedForAdapter_Inserte_f;// synthesized block + void* RTBInsertedForAdapter_Insert_ey;// synthesized block + void* RTBInsertedForAdapter_Insert_ci;// synthesized block + void* RTBInsertedForAdapter_Inserte_h;// synthesized block + void* RTBInsertedForAdapter_Insert_bz;// synthesized block + int8_T RTBInsertedForAdapter_Insert_hj;// synthesized block + int8_T RTBInsertedForAdapter_Inserte_p;// synthesized block + int8_T RTBInsertedForAdapter_Insert_mp;// synthesized block + int8_T RTBInsertedForAdapter_Insert_m3;// synthesized block + int8_T RTBInsertedForAdapter_Insert_b2;// synthesized block + int8_T RTBInsertedForAdapter_Insert_ko;// synthesized block + int8_T RTBInsertedForAdapter_Insert_jj;// synthesized block + int8_T RTBInsertedForAdapter_Insert_mb;// synthesized block + int8_T RTBInsertedForAdapter_Insert_p5;// synthesized block + int8_T RTBInsertedForAdapter_Insert_bw;// synthesized block + int8_T RTBInsertedForAdapter_Insert_js;// synthesized block + int8_T RTBInsertedForAdapter_Inserte_a;// synthesized block + int8_T RTBInsertedForAdapter_Inserte_g;// synthesized block + int8_T RTBInsertedForAdapter_Insert_pa;// synthesized block +}; + +// External inputs (root inport signals with default storage) +struct ExtU_AMC_BLDC_T { + SensorsData SensorsData_p; // '/SensorsData' + ExternalFlags ExternalFlags_p; // '/ExternalFlags' + BUS_CAN_MULTIPLE PacketsRx; // '/PacketsRx' +}; + +// External outputs (root outports fed by signals with default storage) +struct ExtY_AMC_BLDC_T { + ControlOutputs ControlOutputs_p; // '/ControlOutputs' + ConfigurationParameters ConfigurationParameters_p;// '/ConfigurationParameters' + Flags Flags_p; // '/Flags' + EstimatedData EstimatedData_p; // '/EstimatedData' + BUS_CAN_MULTIPLE PacketsTx; // '/PacketsTx' +}; + +// Real-time Model Data Structure +struct tag_RTM_AMC_BLDC_T { + const char_T *errorStatus; + + // + // Timing: + // The following substructure contains information regarding + // the timing information for the model. + + struct { + struct { + uint32_T TID[3]; + } TaskCounters; + } Timing; +}; + +// Block signals (default storage) +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern struct B_AMC_BLDC_T AMC_BLDC_B; + +#ifdef __cplusplus + +} + +#endif + +// Block states (default storage) +extern struct DW_AMC_BLDC_T AMC_BLDC_DW; + +#ifdef __cplusplus + +extern "C" +{ + +#endif + + // External inputs (root inport signals with default storage) + extern struct ExtU_AMC_BLDC_T AMC_BLDC_U; + + // External outputs (root outports fed by signals with default storage) + extern struct ExtY_AMC_BLDC_T AMC_BLDC_Y; + +#ifdef __cplusplus + +} + +#endif + +// +// Exported Global Parameters +// +// Note: Exported global parameters are tunable parameters with an exported +// global storage class designation. Code generation will declare the memory for +// these parameters and exports their symbols. +// + +extern ConfigurationParameters InitConfParams;// Variable: InitConfParams + // Referenced by: '/SupervisorFSM_RX' + +extern real32_T CAN_ANGLE_DEG2ICUB; // Variable: CAN_ANGLE_DEG2ICUB + // Referenced by: '/CAN_Encoder' + // 2^16/360 + +extern real32_T CAN_ANGLE_ICUB2DEG; // Variable: CAN_ANGLE_ICUB2DEG + // Referenced by: '/CAN_Decoder' + // 360/2^16 + +extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC + // Referenced by: + // '/CAN_Decoder' + // '/CAN_Encoder' + // 4 bits defining the ID of the AMC_BLDC board. + + +#ifdef __cplusplus + +extern "C" +{ + +#endif + + // Model entry point functions + extern void AMC_BLDC_initialize(void); + extern void AMC_BLDC_step0(void); + extern void AMC_BLDC_step_FOC(void); + extern void AMC_BLDC_step_Time(void); + extern void AMC_BLDC_terminate(void); + +#ifdef __cplusplus + +} + +#endif + +// Real-time Model object +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern RT_MODEL_AMC_BLDC_T *const AMC_BLDC_M; + +#ifdef __cplusplus + +} + +#endif + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'AMC_BLDC' +// '' : 'AMC_BLDC/Adapter1' +// '' : 'AMC_BLDC/Adapter2' +// '' : 'AMC_BLDC/Adapter3' +// '' : 'AMC_BLDC/Adapter4' +// '' : 'AMC_BLDC/Estimation' +// '' : 'AMC_BLDC/Messaging' +// '' : 'AMC_BLDC/Supervision' +// '' : 'AMC_BLDC/Estimation/Adapter' +// '' : 'AMC_BLDC/Estimation/Mux' + +#endif // RTW_HEADER_AMC_BLDC_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_private.h index 5419f19a72..c25fab8834 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_private.h @@ -1,30 +1,30 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: AMC_BLDC_private.h -// -// Code generated for Simulink model 'AMC_BLDC'. -// -// Model version : 5.1 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:48 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_AMC_BLDC_private_h_ -#define RTW_HEADER_AMC_BLDC_private_h_ -#include "rtwtypes.h" -#include "zero_crossing_types.h" -#include "AMC_BLDC_types.h" -#endif // RTW_HEADER_AMC_BLDC_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: AMC_BLDC_private.h +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 5.1 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:32 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_AMC_BLDC_private_h_ +#define RTW_HEADER_AMC_BLDC_private_h_ +#include "rtwtypes.h" +#include "zero_crossing_types.h" +#include "AMC_BLDC_types.h" +#endif // RTW_HEADER_AMC_BLDC_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_types.h index 8b5a880884..17a6af0129 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/amc-bldc/AMC_BLDC_types.h @@ -1,784 +1,784 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: AMC_BLDC_types.h -// -// Code generated for Simulink model 'AMC_BLDC'. -// -// Model version : 5.1 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:48 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_AMC_BLDC_types_h_ -#define RTW_HEADER_AMC_BLDC_types_h_ -#include "rtwtypes.h" - -// Includes for objects with custom storage classes -#include "rtw_defines.h" - -// -// Registered constraints for dimension variants - -#if CAN_MAX_NUM_PACKETS <= 0 -# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be greater than '0'" -#endif - -#if CAN_MAX_NUM_PACKETS >= 16 -# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be less than '16'" -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ - -struct SensorsData -{ - // position encoders - JointPositions jointpositions; - MotorSensors motorsensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ -#define DEFINED_TYPEDEF_FOR_ControlModes_ - -typedef enum { - ControlModes_NotConfigured = 0, // Default value - ControlModes_Idle, - ControlModes_Position, - ControlModes_PositionDirect, - ControlModes_Current, - ControlModes_Velocity, - ControlModes_Voltage, - ControlModes_Torque, - ControlModes_HwFaultCM -} ControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ - -struct Flags -{ - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocities - JointVelocities jointvelocities; - MotorCurrent Iq_filtered; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ - -struct ControlOuterOutputs -{ - boolean_T vel_en; - boolean_T cur_en; - boolean_T out_en; - MotorCurrent motorcurrent; - real32_T current_limiter; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_FOCSlowInputs_ -#define DEFINED_TYPEDEF_FOR_FOCSlowInputs_ - -struct FOCSlowInputs -{ - Flags flags; - ConfigurationParameters configurationparameters; - EstimatedData estimateddata; - Targets targets; - ControlOuterOutputs controlouteroutputs; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs -{ - // control effort (quadrature) - real32_T Vq; - - // control effort (3-phases) - real32_T Vabc[3]; - - // quadrature current - MotorCurrent Iq_fbk; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ -#define DEFINED_TYPEDEF_FOR_ExternalFlags_ - -struct ExternalFlags -{ - // External Fault Button (1 == pressed) - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ -#define DEFINED_TYPEDEF_FOR_MCControlModes_ - -typedef enum { - MCControlModes_Idle = 0, // Default value - MCControlModes_OpenLoop = 80, - MCControlModes_SpeedVoltage = 10, - MCControlModes_SpeedCurrent = 11, - MCControlModes_Current = 6, - MCControlModes_NotConfigured = 176, - MCControlModes_HWFault = 160 -} MCControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ - -// Fields of a CONTROL_MODE message. -struct BUS_MSG_CONTROL_MODE -{ - // Motor selector. - boolean_T motor; - - // Control mode. - MCControlModes mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ - -// Fields of a CURRENT_LIMIT message. -struct BUS_MSG_CURRENT_LIMIT -{ - // Motor selector. - boolean_T motor; - - // Nominal current in A. - real32_T nominal; - - // Peak current in A. - real32_T peak; - - // Overload current in A. - real32_T overload; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ - -// Fields of a DESIRED_TARGETS message. -struct BUS_MSG_DESIRED_TARGETS -{ - // Target current in A. - real32_T current; - - // Target voltage in %. - real32_T voltage; - - // Target veocity in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ - -// Fields of a CURRENT_PID message. -struct BUS_MSG_PID -{ - // Motor selector. - boolean_T motor; - - // Proportional gain. - real32_T Kp; - - // Integral gain. - real32_T Ki; - - // Derivative gain. - real32_T Kd; - - // Shift factor. - uint8_T Ks; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ - -struct BUS_MSG_MOTOR_CONFIG -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - - // Number of polese of the motor. - uint8_T number_poles; - - // Encoder tolerance. - uint8_T encoder_tolerance; - - // Resolution of rotor encoder. - int16_T rotor_encoder_resolution; - - // Offset of the rotor encoder. - int16_T rotor_index_offset; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ - -// Aggregate of all CAN received messages. -struct BUS_MESSAGES_RX -{ - BUS_MSG_CONTROL_MODE control_mode; - BUS_MSG_CURRENT_LIMIT current_limit; - BUS_MSG_DESIRED_TARGETS desired_targets; - BUS_MSG_PID pid; - BUS_MSG_MOTOR_CONFIG motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ - -struct BUS_MESSAGES_RX_MULTIPLE -{ - BUS_MESSAGES_RX messages[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ - -// Aggregate of all events specifying types of received messages. -struct BUS_STATUS_RX -{ - boolean_T control_mode; - boolean_T current_limit; - boolean_T desired_targets; - boolean_T current_pid; - boolean_T velocity_pid; - boolean_T motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ - -struct BUS_STATUS_RX_MULTIPLE -{ - BUS_STATUS_RX status[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ -#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ - -typedef enum { - CANErrorTypes_No_Error = 0, // Default value - CANErrorTypes_Packet_Not4Us, - CANErrorTypes_Packet_Unrecognized, - CANErrorTypes_Packet_Malformed, - CANErrorTypes_Packet_MultiFunctionsDetected, - CANErrorTypes_Mode_Unrecognized -} CANErrorTypes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ - -// Specifies the CAN error types. -struct BUS_CAN_RX_ERRORS -{ - // True if an error has been detected. - boolean_T event; - CANErrorTypes type; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ - -struct BUS_CAN_RX_ERRORS_MULTIPLE -{ - BUS_CAN_RX_ERRORS errors[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ - -// Fields of a FOC message. -struct BUS_MSG_FOC -{ - // Current feedback in A. - real32_T current; - - // Position feedback in deg. - real32_T position; - - // Velocity feedback in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ - -struct BUS_FLAGS_TX -{ - boolean_T dirty; - boolean_T stuck; - boolean_T index_broken; - boolean_T phase_broken; - real32_T not_calibrated; - boolean_T ExternalFaultAsserted; - boolean_T UnderVoltageFailure; - boolean_T OverVoltageFailure; - boolean_T OverCurrentFailure; - boolean_T DHESInvalidValue; - boolean_T AS5045CSumError; - boolean_T DHESInvalidSequence; - boolean_T CANInvalidProtocol; - boolean_T CAN_BufferOverRun; - boolean_T SetpointExpired; - boolean_T CAN_TXIsPasv; - boolean_T CAN_RXIsPasv; - boolean_T CAN_IsWarnTX; - boolean_T CAN_IsWarnRX; - boolean_T OverHeating; - boolean_T ADCCalFailure; - boolean_T I2TFailure; - boolean_T EMUROMFault; - boolean_T EMUROMCRCFault; - boolean_T EncoderFault; - boolean_T FirmwareSPITimingError; - boolean_T AS5045CalcError; - boolean_T FirmwarePWMFatalError; - boolean_T CAN_TXWasPasv; - boolean_T CAN_RXWasPasv; - boolean_T CAN_RTRFlagActive; - boolean_T CAN_WasWarn; - boolean_T CAN_DLCError; - boolean_T SiliconRevisionFault; - boolean_T PositionLimitUpper; - boolean_T PositionLimitLower; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ - -struct BUS_MSG_STATUS -{ - MCControlModes control_mode; - real32_T pwm_fbk; - BUS_FLAGS_TX flags; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ - -// Aggregate of all CAN transmitted messages. -struct BUS_MESSAGES_TX -{ - BUS_MSG_FOC foc; - BUS_MSG_STATUS status; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ - -// Aggregate of all events specifying types of transmitted messages. -struct BUS_STATUS_TX -{ - boolean_T foc; - boolean_T status; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ - -// Fields of a transmitted CAN packet. -struct BUS_CAN_PACKET -{ - // ID of the CAN packet. - uint16_T ID; - - // PAYLOAD of the CAN packet. - uint8_T PAYLOAD[8]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_ - -struct BUS_CAN -{ - // If true, the packet is available to be processed. - boolean_T available; - uint8_T length; - BUS_CAN_PACKET packet; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ - -struct BUS_CAN_MULTIPLE -{ - BUS_CAN packets[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_CANClassTypes_ -#define DEFINED_TYPEDEF_FOR_CANClassTypes_ - -typedef enum { - CANClassTypes_Motor_Control_Command = 0,// Default value - CANClassTypes_Motor_Control_Streaming = 1, - CANClassTypes_Analog_Sensors_Command = 2, - CANClassTypes_Skin_Sensor_Streaming = 4, - CANClassTypes_Inertial_Sensor_Streaming = 5, - CANClassTypes_Future_Use = 6, - CANClassTypes_Management_Bootloader = 7 -} CANClassTypes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ - -struct BUS_CAN_ID_RX -{ - // 3 bits defining the message class type. - CANClassTypes CLS; - - // 4 bits defining the source ID. - uint8_T SRC; - - // 4 bits definint the destination ID or the message sub-type. - uint8_T DST_TYP; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ - -struct BUS_CAN_CMD -{ - // 1 bits for motor selector. - boolean_T M; - - // 7 bits defining the operational code of the command. - uint8_T OPC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ - -struct BUS_CAN_PAYLOAD_RX -{ - // Actual length of the total PAYLOAD field. - uint8_T LEN; - BUS_CAN_CMD CMD; - - // 8 bytes for the command argument in order to account also message of type streaming. - uint8_T ARG[8]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ - -// Fields of a received CAN packet. -struct BUS_CAN_PACKET_RX -{ - // ID of the CAN packet. - BUS_CAN_ID_RX ID; - - // PAYLOAD of the CAN packet. - BUS_CAN_PAYLOAD_RX PAYLOAD; -}; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_AMC_BLDC_T RT_MODEL_AMC_BLDC_T; - -#endif // RTW_HEADER_AMC_BLDC_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: AMC_BLDC_types.h +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 5.1 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:32 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_AMC_BLDC_types_h_ +#define RTW_HEADER_AMC_BLDC_types_h_ +#include "rtwtypes.h" + +// Includes for objects with custom storage classes +#include "rtw_defines.h" + +// +// Registered constraints for dimension variants + +#if CAN_MAX_NUM_PACKETS <= 0 +# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be greater than '0'" +#endif + +#if CAN_MAX_NUM_PACKETS >= 16 +# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be less than '16'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ +#define DEFINED_TYPEDEF_FOR_JointPositions_ + +struct JointPositions +{ + // joint positions + real32_T position; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_Torque, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + // control mode + ControlModes control_mode; + boolean_T enable_sending_msg_status; + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ +#define DEFINED_TYPEDEF_FOR_JointVelocities_ + +struct JointVelocities +{ + // joint velocities + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ +#define DEFINED_TYPEDEF_FOR_MotorCurrent_ + +struct MotorCurrent +{ + // motor current + real32_T current; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocities + JointVelocities jointvelocities; + MotorCurrent Iq_filtered; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ +#define DEFINED_TYPEDEF_FOR_MotorVoltage_ + +struct MotorVoltage +{ + // motor voltage + real32_T voltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + JointPositions jointpositions; + JointVelocities jointvelocities; + MotorCurrent motorcurrent; + MotorVoltage motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ + +struct ControlOuterOutputs +{ + boolean_T vel_en; + boolean_T cur_en; + boolean_T out_en; + MotorCurrent motorcurrent; + real32_T current_limiter; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_FOCSlowInputs_ +#define DEFINED_TYPEDEF_FOR_FOCSlowInputs_ + +struct FOCSlowInputs +{ + Flags flags; + ConfigurationParameters configurationparameters; + EstimatedData estimateddata; + Targets targets; + ControlOuterOutputs controlouteroutputs; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOutputs_ + +struct ControlOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + MotorCurrent Iq_fbk; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ +#define DEFINED_TYPEDEF_FOR_ExternalFlags_ + +struct ExternalFlags +{ + // External Fault Button (1 == pressed) + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ +#define DEFINED_TYPEDEF_FOR_MCControlModes_ + +typedef enum { + MCControlModes_Idle = 0, // Default value + MCControlModes_OpenLoop = 80, + MCControlModes_SpeedVoltage = 10, + MCControlModes_SpeedCurrent = 11, + MCControlModes_Current = 6, + MCControlModes_NotConfigured = 176, + MCControlModes_HWFault = 160 +} MCControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ + +// Fields of a CONTROL_MODE message. +struct BUS_MSG_CONTROL_MODE +{ + // Motor selector. + boolean_T motor; + + // Control mode. + MCControlModes mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ + +// Fields of a CURRENT_LIMIT message. +struct BUS_MSG_CURRENT_LIMIT +{ + // Motor selector. + boolean_T motor; + + // Nominal current in A. + real32_T nominal; + + // Peak current in A. + real32_T peak; + + // Overload current in A. + real32_T overload; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ + +// Fields of a DESIRED_TARGETS message. +struct BUS_MSG_DESIRED_TARGETS +{ + // Target current in A. + real32_T current; + + // Target voltage in %. + real32_T voltage; + + // Target veocity in deg/s. + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ + +// Fields of a CURRENT_PID message. +struct BUS_MSG_PID +{ + // Motor selector. + boolean_T motor; + + // Proportional gain. + real32_T Kp; + + // Integral gain. + real32_T Ki; + + // Derivative gain. + real32_T Kd; + + // Shift factor. + uint8_T Ks; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ + +struct BUS_MSG_MOTOR_CONFIG +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + + // Number of polese of the motor. + uint8_T number_poles; + + // Encoder tolerance. + uint8_T encoder_tolerance; + + // Resolution of rotor encoder. + int16_T rotor_encoder_resolution; + + // Offset of the rotor encoder. + int16_T rotor_index_offset; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ + +// Aggregate of all CAN received messages. +struct BUS_MESSAGES_RX +{ + BUS_MSG_CONTROL_MODE control_mode; + BUS_MSG_CURRENT_LIMIT current_limit; + BUS_MSG_DESIRED_TARGETS desired_targets; + BUS_MSG_PID pid; + BUS_MSG_MOTOR_CONFIG motor_config; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ + +struct BUS_MESSAGES_RX_MULTIPLE +{ + BUS_MESSAGES_RX messages[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ + +// Aggregate of all events specifying types of received messages. +struct BUS_STATUS_RX +{ + boolean_T control_mode; + boolean_T current_limit; + boolean_T desired_targets; + boolean_T current_pid; + boolean_T velocity_pid; + boolean_T motor_config; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ + +struct BUS_STATUS_RX_MULTIPLE +{ + BUS_STATUS_RX status[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ +#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ + +typedef enum { + CANErrorTypes_No_Error = 0, // Default value + CANErrorTypes_Packet_Not4Us, + CANErrorTypes_Packet_Unrecognized, + CANErrorTypes_Packet_Malformed, + CANErrorTypes_Packet_MultiFunctionsDetected, + CANErrorTypes_Mode_Unrecognized +} CANErrorTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ + +// Specifies the CAN error types. +struct BUS_CAN_RX_ERRORS +{ + // True if an error has been detected. + boolean_T event; + CANErrorTypes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ + +struct BUS_CAN_RX_ERRORS_MULTIPLE +{ + BUS_CAN_RX_ERRORS errors[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ + +// Fields of a FOC message. +struct BUS_MSG_FOC +{ + // Current feedback in A. + real32_T current; + + // Position feedback in deg. + real32_T position; + + // Velocity feedback in deg/s. + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ + +struct BUS_FLAGS_TX +{ + boolean_T dirty; + boolean_T stuck; + boolean_T index_broken; + boolean_T phase_broken; + real32_T not_calibrated; + boolean_T ExternalFaultAsserted; + boolean_T UnderVoltageFailure; + boolean_T OverVoltageFailure; + boolean_T OverCurrentFailure; + boolean_T DHESInvalidValue; + boolean_T AS5045CSumError; + boolean_T DHESInvalidSequence; + boolean_T CANInvalidProtocol; + boolean_T CAN_BufferOverRun; + boolean_T SetpointExpired; + boolean_T CAN_TXIsPasv; + boolean_T CAN_RXIsPasv; + boolean_T CAN_IsWarnTX; + boolean_T CAN_IsWarnRX; + boolean_T OverHeating; + boolean_T ADCCalFailure; + boolean_T I2TFailure; + boolean_T EMUROMFault; + boolean_T EMUROMCRCFault; + boolean_T EncoderFault; + boolean_T FirmwareSPITimingError; + boolean_T AS5045CalcError; + boolean_T FirmwarePWMFatalError; + boolean_T CAN_TXWasPasv; + boolean_T CAN_RXWasPasv; + boolean_T CAN_RTRFlagActive; + boolean_T CAN_WasWarn; + boolean_T CAN_DLCError; + boolean_T SiliconRevisionFault; + boolean_T PositionLimitUpper; + boolean_T PositionLimitLower; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ + +struct BUS_MSG_STATUS +{ + MCControlModes control_mode; + real32_T pwm_fbk; + BUS_FLAGS_TX flags; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ + +// Aggregate of all CAN transmitted messages. +struct BUS_MESSAGES_TX +{ + BUS_MSG_FOC foc; + BUS_MSG_STATUS status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ + +// Aggregate of all events specifying types of transmitted messages. +struct BUS_STATUS_TX +{ + boolean_T foc; + boolean_T status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ + +// Fields of a transmitted CAN packet. +struct BUS_CAN_PACKET +{ + // ID of the CAN packet. + uint16_T ID; + + // PAYLOAD of the CAN packet. + uint8_T PAYLOAD[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ + +struct BUS_CAN +{ + // If true, the packet is available to be processed. + boolean_T available; + uint8_T length; + BUS_CAN_PACKET packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ + +struct BUS_CAN_MULTIPLE +{ + BUS_CAN packets[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_CANClassTypes_ +#define DEFINED_TYPEDEF_FOR_CANClassTypes_ + +typedef enum { + CANClassTypes_Motor_Control_Command = 0,// Default value + CANClassTypes_Motor_Control_Streaming = 1, + CANClassTypes_Analog_Sensors_Command = 2, + CANClassTypes_Skin_Sensor_Streaming = 4, + CANClassTypes_Inertial_Sensor_Streaming = 5, + CANClassTypes_Future_Use = 6, + CANClassTypes_Management_Bootloader = 7 +} CANClassTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ + +struct BUS_CAN_ID_RX +{ + // 3 bits defining the message class type. + CANClassTypes CLS; + + // 4 bits defining the source ID. + uint8_T SRC; + + // 4 bits definint the destination ID or the message sub-type. + uint8_T DST_TYP; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ + +struct BUS_CAN_CMD +{ + // 1 bits for motor selector. + boolean_T M; + + // 7 bits defining the operational code of the command. + uint8_T OPC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ + +struct BUS_CAN_PAYLOAD_RX +{ + // Actual length of the total PAYLOAD field. + uint8_T LEN; + BUS_CAN_CMD CMD; + + // 8 bytes for the command argument in order to account also message of type streaming. + uint8_T ARG[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ + +// Fields of a received CAN packet. +struct BUS_CAN_PACKET_RX +{ + // ID of the CAN packet. + BUS_CAN_ID_RX ID; + + // PAYLOAD of the CAN packet. + BUS_CAN_PAYLOAD_RX PAYLOAD; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_AMC_BLDC_T RT_MODEL_AMC_BLDC_T; + +#endif // RTW_HEADER_AMC_BLDC_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.cpp index 84ba35896d..859afe973d 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.cpp @@ -1,843 +1,843 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_decoder.cpp -// -// Code generated for Simulink model 'can_decoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:53 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "can_decoder.h" -#include "rtwtypes.h" -#include "can_decoder_types.h" -#include -#include -#include "can_decoder_private.h" -#include "rtw_defines.h" - -// Named constants for Chart: '/Decoding Logic' -const int32_T ca_event_ev_error_pck_malformed = 1; -const int32_T can_d_event_ev_error_pck_not4us = 2; -const int32_T can_decoder_CALL_EVENT = -1; -const uint8_T can_decoder_IN_Event_Error = 1U; -const uint8_T can_decoder_IN_Home = 1U; -const uint8_T can_decoder_IN_Home_d = 2U; -const int32_T event_ev_error_mode_unrecognize = 0; -MdlrefDW_can_decoder_T can_decoder_MdlrefDW; - -// Block signals (default storage) -B_can_decoder_c_T can_decoder_B; - -// Block states (default storage) -DW_can_decoder_f_T can_decoder_DW; - -// Forward declaration for local functions -static int32_T can_de_safe_cast_to_MCStreaming(int32_T input); -static void can_decoder_ERROR_HANDLING(boolean_T rtu_pck_available, - B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW); -static int16_T can_decoder_merge_2bytes_signed(uint16_T bl, uint16_T bh); -static boolean_T can_d_is_controlmode_recognized(int32_T mode); -static int32_T can_safe_cast_to_MCControlModes(int32_T input); -static uint16_T can_decod_merge_2bytes_unsigned(uint16_T bl, uint16_T bh); - -// Forward declaration for local functions -static CANClassTypes c_convert_to_enum_CANClassTypes(int32_T input); -static int32_T can_de_safe_cast_to_MCStreaming(int32_T input) -{ - int32_T output; - - // Initialize output value to default value for MCStreaming (Desired_Targets) - output = 15; - if ((input == 0) || (input == 15)) { - // Set output value to input value if it is a member of MCStreaming - output = input; - } - - return output; -} - -// Function for Chart: '/Decoding Logic' -static void can_decoder_ERROR_HANDLING(boolean_T rtu_pck_available, - B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW) -{ - boolean_T guard1 = false; - guard1 = false; - switch (localDW->is_ERROR_HANDLING) { - case can_decoder_IN_Event_Error: - localDW->is_ERROR_HANDLING = can_decoder_IN_Home_d; - localDW->cmd_processed = 0U; - break; - - case can_decoder_IN_Home_d: - if (localDW->sfEvent == can_d_event_ev_error_pck_not4us) { - localB->error_type = CANErrorTypes_Packet_Not4Us; - localDW->ev_errorEventCounter++; - guard1 = true; - } else if (localDW->sfEvent == ca_event_ev_error_pck_malformed) { - localB->error_type = CANErrorTypes_Packet_Malformed; - localDW->ev_errorEventCounter++; - guard1 = true; - } else if (localDW->sfEvent == event_ev_error_mode_unrecognize) { - localB->error_type = CANErrorTypes_Mode_Unrecognized; - localDW->ev_errorEventCounter++; - guard1 = true; - } else { - if (!localDW->ev_async) { - if (rtu_pck_available && (localDW->cmd_processed == 0)) { - localB->error_type = CANErrorTypes_Packet_Unrecognized; - localDW->ev_errorEventCounter++; - } else if (localDW->cmd_processed > 1) { - localB->error_type = CANErrorTypes_Packet_MultiFunctionsDetected; - localDW->ev_errorEventCounter++; - } else { - localB->error_type = CANErrorTypes_No_Error; - } - } - - localDW->ev_async = false; - localDW->is_ERROR_HANDLING = can_decoder_IN_Home_d; - localDW->cmd_processed = 0U; - } - break; - } - - if (guard1) { - localDW->is_ERROR_HANDLING = can_decoder_IN_Event_Error; - localDW->ev_async = true; - } -} - -// Function for Chart: '/Decoding Logic' -static int16_T can_decoder_merge_2bytes_signed(uint16_T bl, uint16_T bh) -{ - int16_T sw; - uint16_T x; - x = static_cast(static_cast(bh << 8) | bl); - std::memcpy((void *)&sw, (void *)&x, (size_t)1 * sizeof(int16_T)); - return sw; -} - -// Function for Chart: '/Decoding Logic' -static boolean_T can_d_is_controlmode_recognized(int32_T mode) -{ - return (mode == static_cast(MCControlModes_Idle)) || (mode == - static_cast(MCControlModes_OpenLoop)) || (mode == - static_cast(MCControlModes_SpeedVoltage)) || (mode == - static_cast(MCControlModes_Current)); -} - -static int32_T can_safe_cast_to_MCControlModes(int32_T input) -{ - int32_T output; - - // Initialize output value to default value for MCControlModes (Idle) - output = 0; - if ((input == 0) || (input == 6) || ((input >= 10) && (input <= 11)) || (input - == 80) || (input == 160) || (input == 176)) { - // Set output value to input value if it is a member of MCControlModes - output = input; - } - - return output; -} - -// Function for Chart: '/Decoding Logic' -static uint16_T can_decod_merge_2bytes_unsigned(uint16_T bl, uint16_T bh) -{ - return static_cast(static_cast(bh << 8) | bl); -} - -// System initialize for atomic system: '/Decoding Logic' -void can_decoder_DecodingLogic_Init(B_DecodingLogic_can_decoder_T *localB, - DW_DecodingLogic_can_decoder_T *localDW) -{ - localDW->sfEvent = can_decoder_CALL_EVENT; - localB->msg_set_control_mode.motor = false; - localB->msg_set_control_mode.mode = MCControlModes_Idle; - localB->msg_set_current_limit.motor = false; - localB->msg_set_current_limit.nominal = 0.0F; - localB->msg_set_current_limit.peak = 0.0F; - localB->msg_set_current_limit.overload = 0.0F; - localB->msg_desired_targets.current = 0.0F; - localB->msg_desired_targets.voltage = 0.0F; - localB->msg_desired_targets.velocity = 0.0F; - localB->msg_set_pid.motor = false; - localB->msg_set_pid.Kp = 0.0F; - localB->msg_set_pid.Ki = 0.0F; - localB->msg_set_pid.Kd = 0.0F; - localB->msg_set_pid.Ks = 0U; - localB->msg_set_motor_config.has_hall_sens = false; - localB->msg_set_motor_config.has_quadrature_encoder = false; - localB->msg_set_motor_config.has_speed_quadrature_encoder = false; - localB->msg_set_motor_config.has_torque_sens = false; - localB->msg_set_motor_config.use_index = false; - localB->msg_set_motor_config.enable_verbosity = false; - localB->msg_set_motor_config.number_poles = 0U; - localB->msg_set_motor_config.encoder_tolerance = 0U; - localB->msg_set_motor_config.rotor_encoder_resolution = 0; - localB->msg_set_motor_config.rotor_index_offset = 0; -} - -// Output and update for atomic system: '/Decoding Logic' -void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const - BUS_CAN_PACKET_RX *rtu_pck_input, const ConfigurationParameters - *rtu_ConfigurationParameters, uint8_T rtu_CAN_ID_DST, uint8_T - rtu_CAN_VOLT_REF_SHIFT, real32_T rtu_CAN_VOLT_REF_GAIN, - B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW) -{ - localDW->sfEvent = can_decoder_CALL_EVENT; - - // Chart: '/Decoding Logic' - // This state chart is responsible for decoding incoming CAN packets. - if (localDW->is_active_c3_can_decoder == 0U) { - localDW->is_active_c3_can_decoder = 1U; - localDW->is_active_SET_CONTROL_MODE = 1U; - localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; - localDW->is_active_DESIRED_TARGETS = 1U; - localDW->is_DESIRED_TARGETS = can_decoder_IN_Home; - localDW->is_active_SET_OPTIONS = 1U; - localDW->is_SET_OPTIONS = can_decoder_IN_Home; - localDW->is_active_SET_MOTOR_CONFIG = 1U; - localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; - localDW->is_active_ERROR_HANDLING = 1U; - localDW->ev_async = false; - localDW->is_ERROR_HANDLING = can_decoder_IN_Home_d; - localDW->cmd_processed = 0U; - } else { - if ((localDW->is_active_SET_CONTROL_MODE != 0U) && - (localDW->is_SET_CONTROL_MODE == can_decoder_IN_Home) && - (rtu_pck_available && (rtu_pck_input->ID.CLS == - CANClassTypes_Motor_Control_Command))) { - if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { - if (rtu_pck_input->PAYLOAD.LEN >= 1) { - if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast - (MCOPC_Set_Control_Mode)) { - if (rtu_pck_input->PAYLOAD.LEN >= 2) { - if (can_d_is_controlmode_recognized(static_cast - (rtu_pck_input->PAYLOAD.ARG[1]))) { - localB->msg_set_control_mode.motor = - rtu_pck_input->PAYLOAD.CMD.M; - localB->msg_set_control_mode.mode = static_cast - (can_safe_cast_to_MCControlModes(rtu_pck_input->PAYLOAD.ARG[1])); - localDW->cmd_processed = static_cast - (localDW->cmd_processed + 1); - localDW->ev_set_control_modeEventCounter++; - } else { - localDW->sfEvent = event_ev_error_mode_unrecognize; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; - } - } else { - localDW->sfEvent = ca_event_ev_error_pck_malformed; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; - } - } - } else { - localDW->sfEvent = ca_event_ev_error_pck_malformed; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; - } - } else { - localDW->sfEvent = can_d_event_ev_error_pck_not4us; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; - } - } - - if ((localDW->is_active_DESIRED_TARGETS != 0U) && - (localDW->is_DESIRED_TARGETS == can_decoder_IN_Home) && - (rtu_pck_available && (rtu_pck_input->ID.CLS == - CANClassTypes_Motor_Control_Streaming) && - (can_de_safe_cast_to_MCStreaming(rtu_pck_input->ID.DST_TYP) == - static_cast(MCStreaming_Desired_Targets)))) { - if ((rtu_pck_input->PAYLOAD.LEN == 8) && (rtu_CAN_ID_DST <= 4)) { - int16_T tmp_merged; - uint8_T idx; - idx = static_cast((rtu_CAN_ID_DST - 1) << 1); - tmp_merged = can_decoder_merge_2bytes_signed(static_cast - (rtu_pck_input->PAYLOAD.ARG[idx]), static_cast - (rtu_pck_input->PAYLOAD.ARG[idx + 1])); - localB->msg_desired_targets.current = 0.001F * static_cast - (tmp_merged); - localB->msg_desired_targets.voltage = static_cast(static_cast< - int16_T>(tmp_merged >> (rtu_CAN_VOLT_REF_SHIFT - - rtu_ConfigurationParameters->CurLoopPID.shift_factor))) / - rtu_CAN_VOLT_REF_GAIN; - localB->msg_desired_targets.velocity = 1000.0F * static_cast - (tmp_merged) * CAN_ANGLE_ICUB2DEG; - localDW->cmd_processed = static_cast(localDW->cmd_processed + - 1); - localDW->ev_desired_targetsEventCounter++; - } else { - localDW->sfEvent = ca_event_ev_error_pck_malformed; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_DESIRED_TARGETS = can_decoder_IN_Home; - } - } - - if ((localDW->is_active_SET_OPTIONS != 0U) && (localDW->is_SET_OPTIONS == - can_decoder_IN_Home) && (rtu_pck_available && (rtu_pck_input->ID.CLS == - CANClassTypes_Motor_Control_Command))) { - if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { - if (rtu_pck_input->PAYLOAD.LEN >= 1) { - boolean_T guard1 = false; - guard1 = false; - if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast - (MCOPC_Set_Current_Limit)) { - if (rtu_pck_input->PAYLOAD.LEN == 8) { - localB->msg_set_current_limit.motor = rtu_pck_input->PAYLOAD.CMD.M; - localB->msg_set_current_limit.nominal = 0.001F * - static_cast(can_decoder_merge_2bytes_signed( - static_cast(rtu_pck_input->PAYLOAD.ARG[2]), - static_cast(rtu_pck_input->PAYLOAD.ARG[3]))); - localB->msg_set_current_limit.peak = 0.001F * static_cast - (can_decod_merge_2bytes_unsigned(static_cast - (rtu_pck_input->PAYLOAD.ARG[4]), static_cast - (rtu_pck_input->PAYLOAD.ARG[5]))); - localB->msg_set_current_limit.overload = 0.001F * - static_cast(can_decod_merge_2bytes_unsigned( - static_cast(rtu_pck_input->PAYLOAD.ARG[6]), - static_cast(rtu_pck_input->PAYLOAD.ARG[7]))); - localDW->cmd_processed = static_cast - (localDW->cmd_processed + 1); - localDW->ev_set_current_limitEventCounte++; - } else { - guard1 = true; - } - } else if ((rtu_pck_input->PAYLOAD.CMD.OPC == static_cast - (MCOPC_Set_Current_PID)) || - (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast - (MCOPC_Set_Velocity_PID))) { - if (rtu_pck_input->PAYLOAD.LEN == 8) { - real32_T c; - localB->msg_set_pid.motor = rtu_pck_input->PAYLOAD.CMD.M; - localB->msg_set_pid.Ks = rtu_pck_input->PAYLOAD.ARG[7]; - c = static_cast(0x01 << (15 - localB->msg_set_pid.Ks)) / - 32768.0F; - localB->msg_set_pid.Kp = c * static_cast - (can_decoder_merge_2bytes_signed(static_cast - (rtu_pck_input->PAYLOAD.ARG[1]), static_cast - (rtu_pck_input->PAYLOAD.ARG[2]))); - localB->msg_set_pid.Ki = c * static_cast - (can_decoder_merge_2bytes_signed(static_cast - (rtu_pck_input->PAYLOAD.ARG[3]), static_cast - (rtu_pck_input->PAYLOAD.ARG[4]))); - localB->msg_set_pid.Kd = c * static_cast - (can_decoder_merge_2bytes_signed(static_cast - (rtu_pck_input->PAYLOAD.ARG[5]), static_cast - (rtu_pck_input->PAYLOAD.ARG[6]))); - localDW->cmd_processed = static_cast - (localDW->cmd_processed + 1); - if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast - (MCOPC_Set_Current_PID)) { - localDW->ev_set_current_pidEventCounter++; - } else { - localB->msg_set_pid.Kp *= 0.001F; - localB->msg_set_pid.Ki *= 0.001F; - localB->msg_set_pid.Kd *= 0.001F; - localDW->ev_set_velocity_pidEventCounter++; - } - } else { - guard1 = true; - } - } - - if (guard1) { - localDW->sfEvent = ca_event_ev_error_pck_malformed; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_OPTIONS = can_decoder_IN_Home; - } - } else { - localDW->sfEvent = ca_event_ev_error_pck_malformed; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_OPTIONS = can_decoder_IN_Home; - } - } else { - localDW->sfEvent = can_d_event_ev_error_pck_not4us; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_OPTIONS = can_decoder_IN_Home; - } - } - - if ((localDW->is_active_SET_MOTOR_CONFIG != 0U) && - (localDW->is_SET_MOTOR_CONFIG == can_decoder_IN_Home) && - (rtu_pck_available && (rtu_pck_input->ID.CLS == - CANClassTypes_Motor_Control_Command))) { - if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { - if (rtu_pck_input->PAYLOAD.LEN >= 1) { - if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast - (MCOPC_Set_Motor_Config)) { - if (rtu_pck_input->PAYLOAD.LEN == 8) { - localB->msg_set_motor_config.has_quadrature_encoder = - ((rtu_pck_input->PAYLOAD.ARG[1] & 1U) != 0U); - localB->msg_set_motor_config.has_hall_sens = - ((rtu_pck_input->PAYLOAD.ARG[1] & 2U) != 0U); - localB->msg_set_motor_config.has_torque_sens = - ((rtu_pck_input->PAYLOAD.ARG[1] & 4U) != 0U); - localB->msg_set_motor_config.use_index = - ((rtu_pck_input->PAYLOAD.ARG[1] & 8U) != 0U); - localB->msg_set_motor_config.has_speed_quadrature_encoder = - ((rtu_pck_input->PAYLOAD.ARG[1] & 16U) != 0U); - localB->msg_set_motor_config.enable_verbosity = - ((rtu_pck_input->PAYLOAD.ARG[1] & 32U) != 0U); - localB->msg_set_motor_config.rotor_encoder_resolution = - can_decoder_merge_2bytes_signed(static_cast - (rtu_pck_input->PAYLOAD.ARG[2]), static_cast - (rtu_pck_input->PAYLOAD.ARG[3])); - localB->msg_set_motor_config.rotor_index_offset = - can_decoder_merge_2bytes_signed(static_cast - (rtu_pck_input->PAYLOAD.ARG[4]), static_cast - (rtu_pck_input->PAYLOAD.ARG[5])); - localB->msg_set_motor_config.number_poles = - rtu_pck_input->PAYLOAD.ARG[6]; - localB->msg_set_motor_config.encoder_tolerance = - rtu_pck_input->PAYLOAD.ARG[7]; - localDW->cmd_processed = static_cast - (localDW->cmd_processed + 1); - localDW->ev_set_motor_configEventCounter++; - } else { - localDW->sfEvent = ca_event_ev_error_pck_malformed; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; - } - } - } else { - localDW->sfEvent = ca_event_ev_error_pck_malformed; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; - } - } else { - localDW->sfEvent = can_d_event_ev_error_pck_not4us; - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(true, localB, localDW); - } - - localDW->sfEvent = -1; - localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; - } - } - - if (localDW->is_active_ERROR_HANDLING != 0U) { - can_decoder_ERROR_HANDLING(rtu_pck_available, localB, localDW); - } - } - - // End of Chart: '/Decoding Logic' - if (localDW->ev_errorEventCounter > 0U) { - localB->ev_error = !localB->ev_error; - localDW->ev_errorEventCounter--; - } - - if (localDW->ev_set_control_modeEventCounter > 0U) { - localB->ev_set_control_mode = !localB->ev_set_control_mode; - localDW->ev_set_control_modeEventCounter--; - } - - if (localDW->ev_set_current_limitEventCounte > 0U) { - localB->ev_set_current_limit = !localB->ev_set_current_limit; - localDW->ev_set_current_limitEventCounte--; - } - - if (localDW->ev_desired_targetsEventCounter > 0U) { - localB->ev_desired_targets = !localB->ev_desired_targets; - localDW->ev_desired_targetsEventCounter--; - } - - if (localDW->ev_set_current_pidEventCounter > 0U) { - localB->ev_set_current_pid = !localB->ev_set_current_pid; - localDW->ev_set_current_pidEventCounter--; - } - - if (localDW->ev_set_velocity_pidEventCounter > 0U) { - localB->ev_set_velocity_pid = !localB->ev_set_velocity_pid; - localDW->ev_set_velocity_pidEventCounter--; - } - - if (localDW->ev_set_motor_configEventCounter > 0U) { - localB->ev_set_motor_config = !localB->ev_set_motor_config; - localDW->ev_set_motor_configEventCounter--; - } -} - -// Function for MATLAB Function: '/RAW2STRUCT Decoding Logic' -static CANClassTypes c_convert_to_enum_CANClassTypes(int32_T input) -{ - CANClassTypes output; - - // Initialize output value to default value for CANClassTypes (Motor_Control_Command) - output = CANClassTypes_Motor_Control_Command; - if (((input >= 0) && (input <= 2)) || ((input >= 4) && (input <= 7))) { - // Set output value to input value if it is a member of CANClassTypes - output = static_cast(input); - } - - return output; -} - -// System initialize for referenced model: 'can_decoder' -void can_decoder_Init(void) -{ - static const BUS_CAN_RX tmp = { false,// available - { { CANClassTypes_Motor_Control_Command,// CLS - 0U, // SRC - 0U // DST_TYP - }, // ID - - { 0U, // LEN - { false, // M - 0U // OPC - }, // CMD - - { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U }// ARG - } // PAYLOAD - } // packet - }; - - // SystemInitialize for Iterator SubSystem: '/Cycling Decoder' - for (int32_T ForEach_itr = 0; ForEach_itr < CAN_MAX_NUM_PACKETS; ForEach_itr++) - { - // SystemInitialize for Atomic SubSystem: '/CAN_RX_RAW2STRUCT' - // SystemInitialize for MATLAB Function: '/RAW2STRUCT Decoding Logic' - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct = tmp; - - // End of SystemInitialize for SubSystem: '/CAN_RX_RAW2STRUCT' - - // SystemInitialize for Atomic SubSystem: '/CAN_Decoder' - // SystemInitialize for Chart: '/Decoding Logic' - can_decoder_DecodingLogic_Init(&can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic, &can_decoder_DW.CoreSubsys[ForEach_itr].sf_DecodingLogic); - - // End of SystemInitialize for SubSystem: '/CAN_Decoder' - } - - // End of SystemInitialize for SubSystem: '/Cycling Decoder' -} - -// Output and update for referenced model: 'can_decoder' -void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const - ConfigurationParameters *rtu_ConfigurationParameters, - BUS_MESSAGES_RX_MULTIPLE *rty_messages_rx, - BUS_STATUS_RX_MULTIPLE *rty_status_rx, - BUS_CAN_RX_ERRORS_MULTIPLE *rty_errors_rx) -{ - // Outputs for Iterator SubSystem: '/Cycling Decoder' incorporates: - // ForEach: '/For Each' - - for (int32_T ForEach_itr = 0; ForEach_itr < CAN_MAX_NUM_PACKETS; ForEach_itr++) - { - uint8_T minval; - uint8_T x_idx_1; - - // Outputs for Atomic SubSystem: '/CAN_RX_RAW2STRUCT' - // MATLAB Function: '/RAW2STRUCT Decoding Logic' incorporates: - // ForEachSliceSelector generated from: '/pck_rx_raw' - - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.available = - rtu_pck_rx_raw->packets[ForEach_itr].available; - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.CLS = - c_convert_to_enum_CANClassTypes(static_cast(static_cast - (static_cast(rtu_pck_rx_raw->packets[ForEach_itr].packet.ID & - 1792) >> 8))); - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.SRC = - static_cast(static_cast(rtu_pck_rx_raw-> - packets[ForEach_itr].packet.ID & 240) >> 4); - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.DST_TYP = - static_cast(rtu_pck_rx_raw->packets[ForEach_itr].packet.ID & 15); - x_idx_1 = rtu_pck_rx_raw->packets[ForEach_itr].length; - minval = 8U; - if (x_idx_1 < 8) { - minval = x_idx_1; - } - - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = 0U; - if (minval > 0) { - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = - minval; - } - - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.M = - ((rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[0] & 128U) != 0U); - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.OPC = - static_cast(rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[0] - & 127); - for (int32_T i = 0; i < 8; i++) { - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.ARG[i] = - rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[i]; - } - - boolean_T rtb_FixPtRelationalOperator; - boolean_T rtb_FixPtRelationalOperator_a; - boolean_T rtb_FixPtRelationalOperator_d; - boolean_T rtb_FixPtRelationalOperator_e; - boolean_T rtb_FixPtRelationalOperator_i; - boolean_T rtb_FixPtRelationalOperator_m; - boolean_T rtb_FixPtRelationalOperator_p; - - // End of MATLAB Function: '/RAW2STRUCT Decoding Logic' - // End of Outputs for SubSystem: '/CAN_RX_RAW2STRUCT' - - // Outputs for Atomic SubSystem: '/CAN_Decoder' - // Chart: '/Decoding Logic' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - - can_decoder_DecodingLogic(can_decoder_B.CoreSubsys[ForEach_itr]. - pck_rx_struct.available, &can_decoder_B.CoreSubsys[ForEach_itr]. - pck_rx_struct.packet, rtu_ConfigurationParameters, CAN_ID_AMC, 5, 10.0F, - &can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic, - &can_decoder_DW.CoreSubsys[ForEach_itr].sf_DecodingLogic); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_control_mode != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_d = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_current_limit != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_n); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_a = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_desired_targets != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_nk); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_p = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_current_pid != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_h); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_m = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_velocity_pid != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_l); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_e = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_motor_config != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_d); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_i = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_error != can_decoder_DW.CoreSubsys[ForEach_itr]. - DelayInput1_DSTATE_j); - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_control_mode; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_n = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_current_limit; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_nk = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_desired_targets; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_h = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_current_pid; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_l = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_velocity_pid; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_d = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_motor_config; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_j = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_error; - - // ForEachSliceAssignment generated from: '/messages_rx' incorporates: - // BusCreator: '/Bus Creator2' - - rty_messages_rx->messages[ForEach_itr].control_mode = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.msg_set_control_mode; - rty_messages_rx->messages[ForEach_itr].current_limit = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.msg_set_current_limit; - rty_messages_rx->messages[ForEach_itr].desired_targets = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_desired_targets; - rty_messages_rx->messages[ForEach_itr].pid = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_set_pid; - rty_messages_rx->messages[ForEach_itr].motor_config = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.msg_set_motor_config; - - // ForEachSliceAssignment generated from: '/events_rx' incorporates: - // BusCreator: '/Bus Creator1' - - rty_status_rx->status[ForEach_itr].control_mode = - rtb_FixPtRelationalOperator; - rty_status_rx->status[ForEach_itr].current_limit = - rtb_FixPtRelationalOperator_d; - rty_status_rx->status[ForEach_itr].desired_targets = - rtb_FixPtRelationalOperator_a; - rty_status_rx->status[ForEach_itr].current_pid = - rtb_FixPtRelationalOperator_p; - rty_status_rx->status[ForEach_itr].velocity_pid = - rtb_FixPtRelationalOperator_m; - rty_status_rx->status[ForEach_itr].motor_config = - rtb_FixPtRelationalOperator_e; - - // ForEachSliceAssignment generated from: '/errors_rx' incorporates: - // BusCreator: '/Bus Creator3' - - rty_errors_rx->errors[ForEach_itr].event = rtb_FixPtRelationalOperator_i; - rty_errors_rx->errors[ForEach_itr].type = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.error_type; - - // End of Outputs for SubSystem: '/CAN_Decoder' - } - - // End of Outputs for SubSystem: '/Cycling Decoder' -} - -// Model initialize function -void can_decoder_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_can_decoder_T *const can_decoder_M = &(can_decoder_MdlrefDW.rtm); - - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(can_decoder_M, rt_errorStatus); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_decoder.cpp +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:41 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "can_decoder.h" +#include "rtwtypes.h" +#include "can_decoder_types.h" +#include +#include +#include "can_decoder_private.h" +#include "rtw_defines.h" + +// Named constants for Chart: '/Decoding Logic' +const int32_T ca_event_ev_error_pck_malformed = 1; +const int32_T can_d_event_ev_error_pck_not4us = 2; +const int32_T can_decoder_CALL_EVENT = -1; +const uint8_T can_decoder_IN_Event_Error = 1U; +const uint8_T can_decoder_IN_Home = 1U; +const uint8_T can_decoder_IN_Home_d = 2U; +const int32_T event_ev_error_mode_unrecognize = 0; +MdlrefDW_can_decoder_T can_decoder_MdlrefDW; + +// Block signals (default storage) +B_can_decoder_c_T can_decoder_B; + +// Block states (default storage) +DW_can_decoder_f_T can_decoder_DW; + +// Forward declaration for local functions +static int32_T can_de_safe_cast_to_MCStreaming(int32_T input); +static void can_decoder_ERROR_HANDLING(boolean_T rtu_pck_available, + B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW); +static int16_T can_decoder_merge_2bytes_signed(uint16_T bl, uint16_T bh); +static boolean_T can_d_is_controlmode_recognized(int32_T mode); +static int32_T can_safe_cast_to_MCControlModes(int32_T input); +static uint16_T can_decod_merge_2bytes_unsigned(uint16_T bl, uint16_T bh); + +// Forward declaration for local functions +static CANClassTypes c_convert_to_enum_CANClassTypes(int32_T input); +static int32_T can_de_safe_cast_to_MCStreaming(int32_T input) +{ + int32_T output; + + // Initialize output value to default value for MCStreaming (Desired_Targets) + output = 15; + if ((input == 0) || (input == 15)) { + // Set output value to input value if it is a member of MCStreaming + output = input; + } + + return output; +} + +// Function for Chart: '/Decoding Logic' +static void can_decoder_ERROR_HANDLING(boolean_T rtu_pck_available, + B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW) +{ + boolean_T guard1 = false; + guard1 = false; + switch (localDW->is_ERROR_HANDLING) { + case can_decoder_IN_Event_Error: + localDW->is_ERROR_HANDLING = can_decoder_IN_Home_d; + localDW->cmd_processed = 0U; + break; + + case can_decoder_IN_Home_d: + if (localDW->sfEvent == can_d_event_ev_error_pck_not4us) { + localB->error_type = CANErrorTypes_Packet_Not4Us; + localDW->ev_errorEventCounter++; + guard1 = true; + } else if (localDW->sfEvent == ca_event_ev_error_pck_malformed) { + localB->error_type = CANErrorTypes_Packet_Malformed; + localDW->ev_errorEventCounter++; + guard1 = true; + } else if (localDW->sfEvent == event_ev_error_mode_unrecognize) { + localB->error_type = CANErrorTypes_Mode_Unrecognized; + localDW->ev_errorEventCounter++; + guard1 = true; + } else { + if (!localDW->ev_async) { + if (rtu_pck_available && (localDW->cmd_processed == 0)) { + localB->error_type = CANErrorTypes_Packet_Unrecognized; + localDW->ev_errorEventCounter++; + } else if (localDW->cmd_processed > 1) { + localB->error_type = CANErrorTypes_Packet_MultiFunctionsDetected; + localDW->ev_errorEventCounter++; + } else { + localB->error_type = CANErrorTypes_No_Error; + } + } + + localDW->ev_async = false; + localDW->is_ERROR_HANDLING = can_decoder_IN_Home_d; + localDW->cmd_processed = 0U; + } + break; + } + + if (guard1) { + localDW->is_ERROR_HANDLING = can_decoder_IN_Event_Error; + localDW->ev_async = true; + } +} + +// Function for Chart: '/Decoding Logic' +static int16_T can_decoder_merge_2bytes_signed(uint16_T bl, uint16_T bh) +{ + int16_T sw; + uint16_T x; + x = static_cast(static_cast(bh << 8) | bl); + std::memcpy((void *)&sw, (void *)&x, (size_t)1 * sizeof(int16_T)); + return sw; +} + +// Function for Chart: '/Decoding Logic' +static boolean_T can_d_is_controlmode_recognized(int32_T mode) +{ + return (mode == static_cast(MCControlModes_Idle)) || (mode == + static_cast(MCControlModes_OpenLoop)) || (mode == + static_cast(MCControlModes_SpeedVoltage)) || (mode == + static_cast(MCControlModes_Current)); +} + +static int32_T can_safe_cast_to_MCControlModes(int32_T input) +{ + int32_T output; + + // Initialize output value to default value for MCControlModes (Idle) + output = 0; + if ((input == 0) || (input == 6) || ((input >= 10) && (input <= 11)) || (input + == 80) || (input == 160) || (input == 176)) { + // Set output value to input value if it is a member of MCControlModes + output = input; + } + + return output; +} + +// Function for Chart: '/Decoding Logic' +static uint16_T can_decod_merge_2bytes_unsigned(uint16_T bl, uint16_T bh) +{ + return static_cast(static_cast(bh << 8) | bl); +} + +// System initialize for atomic system: '/Decoding Logic' +void can_decoder_DecodingLogic_Init(B_DecodingLogic_can_decoder_T *localB, + DW_DecodingLogic_can_decoder_T *localDW) +{ + localDW->sfEvent = can_decoder_CALL_EVENT; + localB->msg_set_control_mode.motor = false; + localB->msg_set_control_mode.mode = MCControlModes_Idle; + localB->msg_set_current_limit.motor = false; + localB->msg_set_current_limit.nominal = 0.0F; + localB->msg_set_current_limit.peak = 0.0F; + localB->msg_set_current_limit.overload = 0.0F; + localB->msg_desired_targets.current = 0.0F; + localB->msg_desired_targets.voltage = 0.0F; + localB->msg_desired_targets.velocity = 0.0F; + localB->msg_set_pid.motor = false; + localB->msg_set_pid.Kp = 0.0F; + localB->msg_set_pid.Ki = 0.0F; + localB->msg_set_pid.Kd = 0.0F; + localB->msg_set_pid.Ks = 0U; + localB->msg_set_motor_config.has_hall_sens = false; + localB->msg_set_motor_config.has_quadrature_encoder = false; + localB->msg_set_motor_config.has_speed_quadrature_encoder = false; + localB->msg_set_motor_config.has_torque_sens = false; + localB->msg_set_motor_config.use_index = false; + localB->msg_set_motor_config.enable_verbosity = false; + localB->msg_set_motor_config.number_poles = 0U; + localB->msg_set_motor_config.encoder_tolerance = 0U; + localB->msg_set_motor_config.rotor_encoder_resolution = 0; + localB->msg_set_motor_config.rotor_index_offset = 0; +} + +// Output and update for atomic system: '/Decoding Logic' +void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const + BUS_CAN_PACKET_RX *rtu_pck_input, const ConfigurationParameters + *rtu_ConfigurationParameters, uint8_T rtu_CAN_ID_DST, uint8_T + rtu_CAN_VOLT_REF_SHIFT, real32_T rtu_CAN_VOLT_REF_GAIN, + B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW) +{ + localDW->sfEvent = can_decoder_CALL_EVENT; + + // Chart: '/Decoding Logic' + // This state chart is responsible for decoding incoming CAN packets. + if (localDW->is_active_c3_can_decoder == 0U) { + localDW->is_active_c3_can_decoder = 1U; + localDW->is_active_SET_CONTROL_MODE = 1U; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + localDW->is_active_DESIRED_TARGETS = 1U; + localDW->is_DESIRED_TARGETS = can_decoder_IN_Home; + localDW->is_active_SET_OPTIONS = 1U; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + localDW->is_active_SET_MOTOR_CONFIG = 1U; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + localDW->is_active_ERROR_HANDLING = 1U; + localDW->ev_async = false; + localDW->is_ERROR_HANDLING = can_decoder_IN_Home_d; + localDW->cmd_processed = 0U; + } else { + if ((localDW->is_active_SET_CONTROL_MODE != 0U) && + (localDW->is_SET_CONTROL_MODE == can_decoder_IN_Home) && + (rtu_pck_available && (rtu_pck_input->ID.CLS == + CANClassTypes_Motor_Control_Command))) { + if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { + if (rtu_pck_input->PAYLOAD.LEN >= 1) { + if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Control_Mode)) { + if (rtu_pck_input->PAYLOAD.LEN >= 2) { + if (can_d_is_controlmode_recognized(static_cast + (rtu_pck_input->PAYLOAD.ARG[1]))) { + localB->msg_set_control_mode.motor = + rtu_pck_input->PAYLOAD.CMD.M; + localB->msg_set_control_mode.mode = static_cast + (can_safe_cast_to_MCControlModes(rtu_pck_input->PAYLOAD.ARG[1])); + localDW->cmd_processed = static_cast + (localDW->cmd_processed + 1); + localDW->ev_set_control_modeEventCounter++; + } else { + localDW->sfEvent = event_ev_error_mode_unrecognize; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = ca_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } + } else { + localDW->sfEvent = ca_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_d_event_ev_error_pck_not4us; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } + + if ((localDW->is_active_DESIRED_TARGETS != 0U) && + (localDW->is_DESIRED_TARGETS == can_decoder_IN_Home) && + (rtu_pck_available && (rtu_pck_input->ID.CLS == + CANClassTypes_Motor_Control_Streaming) && + (can_de_safe_cast_to_MCStreaming(rtu_pck_input->ID.DST_TYP) == + static_cast(MCStreaming_Desired_Targets)))) { + if ((rtu_pck_input->PAYLOAD.LEN == 8) && (rtu_CAN_ID_DST <= 4)) { + int16_T tmp_merged; + uint8_T idx; + idx = static_cast((rtu_CAN_ID_DST - 1) << 1); + tmp_merged = can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[idx]), static_cast + (rtu_pck_input->PAYLOAD.ARG[idx + 1])); + localB->msg_desired_targets.current = 0.001F * static_cast + (tmp_merged); + localB->msg_desired_targets.voltage = static_cast(static_cast< + int16_T>(tmp_merged >> (rtu_CAN_VOLT_REF_SHIFT - + rtu_ConfigurationParameters->CurLoopPID.shift_factor))) / + rtu_CAN_VOLT_REF_GAIN; + localB->msg_desired_targets.velocity = 1000.0F * static_cast + (tmp_merged) * CAN_ANGLE_ICUB2DEG; + localDW->cmd_processed = static_cast(localDW->cmd_processed + + 1); + localDW->ev_desired_targetsEventCounter++; + } else { + localDW->sfEvent = ca_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_DESIRED_TARGETS = can_decoder_IN_Home; + } + } + + if ((localDW->is_active_SET_OPTIONS != 0U) && (localDW->is_SET_OPTIONS == + can_decoder_IN_Home) && (rtu_pck_available && (rtu_pck_input->ID.CLS == + CANClassTypes_Motor_Control_Command))) { + if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { + if (rtu_pck_input->PAYLOAD.LEN >= 1) { + boolean_T guard1 = false; + guard1 = false; + if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Current_Limit)) { + if (rtu_pck_input->PAYLOAD.LEN == 8) { + localB->msg_set_current_limit.motor = rtu_pck_input->PAYLOAD.CMD.M; + localB->msg_set_current_limit.nominal = 0.001F * + static_cast(can_decoder_merge_2bytes_signed( + static_cast(rtu_pck_input->PAYLOAD.ARG[2]), + static_cast(rtu_pck_input->PAYLOAD.ARG[3]))); + localB->msg_set_current_limit.peak = 0.001F * static_cast + (can_decod_merge_2bytes_unsigned(static_cast + (rtu_pck_input->PAYLOAD.ARG[4]), static_cast + (rtu_pck_input->PAYLOAD.ARG[5]))); + localB->msg_set_current_limit.overload = 0.001F * + static_cast(can_decod_merge_2bytes_unsigned( + static_cast(rtu_pck_input->PAYLOAD.ARG[6]), + static_cast(rtu_pck_input->PAYLOAD.ARG[7]))); + localDW->cmd_processed = static_cast + (localDW->cmd_processed + 1); + localDW->ev_set_current_limitEventCounte++; + } else { + guard1 = true; + } + } else if ((rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Current_PID)) || + (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Velocity_PID))) { + if (rtu_pck_input->PAYLOAD.LEN == 8) { + real32_T c; + localB->msg_set_pid.motor = rtu_pck_input->PAYLOAD.CMD.M; + localB->msg_set_pid.Ks = rtu_pck_input->PAYLOAD.ARG[7]; + c = static_cast(0x01 << (15 - localB->msg_set_pid.Ks)) / + 32768.0F; + localB->msg_set_pid.Kp = c * static_cast + (can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[1]), static_cast + (rtu_pck_input->PAYLOAD.ARG[2]))); + localB->msg_set_pid.Ki = c * static_cast + (can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[3]), static_cast + (rtu_pck_input->PAYLOAD.ARG[4]))); + localB->msg_set_pid.Kd = c * static_cast + (can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[5]), static_cast + (rtu_pck_input->PAYLOAD.ARG[6]))); + localDW->cmd_processed = static_cast + (localDW->cmd_processed + 1); + if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Current_PID)) { + localDW->ev_set_current_pidEventCounter++; + } else { + localB->msg_set_pid.Kp *= 0.001F; + localB->msg_set_pid.Ki *= 0.001F; + localB->msg_set_pid.Kd *= 0.001F; + localDW->ev_set_velocity_pidEventCounter++; + } + } else { + guard1 = true; + } + } + + if (guard1) { + localDW->sfEvent = ca_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = ca_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_d_event_ev_error_pck_not4us; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + } + } + + if ((localDW->is_active_SET_MOTOR_CONFIG != 0U) && + (localDW->is_SET_MOTOR_CONFIG == can_decoder_IN_Home) && + (rtu_pck_available && (rtu_pck_input->ID.CLS == + CANClassTypes_Motor_Control_Command))) { + if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { + if (rtu_pck_input->PAYLOAD.LEN >= 1) { + if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Motor_Config)) { + if (rtu_pck_input->PAYLOAD.LEN == 8) { + localB->msg_set_motor_config.has_quadrature_encoder = + ((rtu_pck_input->PAYLOAD.ARG[1] & 1U) != 0U); + localB->msg_set_motor_config.has_hall_sens = + ((rtu_pck_input->PAYLOAD.ARG[1] & 2U) != 0U); + localB->msg_set_motor_config.has_torque_sens = + ((rtu_pck_input->PAYLOAD.ARG[1] & 4U) != 0U); + localB->msg_set_motor_config.use_index = + ((rtu_pck_input->PAYLOAD.ARG[1] & 8U) != 0U); + localB->msg_set_motor_config.has_speed_quadrature_encoder = + ((rtu_pck_input->PAYLOAD.ARG[1] & 16U) != 0U); + localB->msg_set_motor_config.enable_verbosity = + ((rtu_pck_input->PAYLOAD.ARG[1] & 32U) != 0U); + localB->msg_set_motor_config.rotor_encoder_resolution = + can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[2]), static_cast + (rtu_pck_input->PAYLOAD.ARG[3])); + localB->msg_set_motor_config.rotor_index_offset = + can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[4]), static_cast + (rtu_pck_input->PAYLOAD.ARG[5])); + localB->msg_set_motor_config.number_poles = + rtu_pck_input->PAYLOAD.ARG[6]; + localB->msg_set_motor_config.encoder_tolerance = + rtu_pck_input->PAYLOAD.ARG[7]; + localDW->cmd_processed = static_cast + (localDW->cmd_processed + 1); + localDW->ev_set_motor_configEventCounter++; + } else { + localDW->sfEvent = ca_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + } + } + } else { + localDW->sfEvent = ca_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_d_event_ev_error_pck_not4us; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + } + } + + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(rtu_pck_available, localB, localDW); + } + } + + // End of Chart: '/Decoding Logic' + if (localDW->ev_errorEventCounter > 0U) { + localB->ev_error = !localB->ev_error; + localDW->ev_errorEventCounter--; + } + + if (localDW->ev_set_control_modeEventCounter > 0U) { + localB->ev_set_control_mode = !localB->ev_set_control_mode; + localDW->ev_set_control_modeEventCounter--; + } + + if (localDW->ev_set_current_limitEventCounte > 0U) { + localB->ev_set_current_limit = !localB->ev_set_current_limit; + localDW->ev_set_current_limitEventCounte--; + } + + if (localDW->ev_desired_targetsEventCounter > 0U) { + localB->ev_desired_targets = !localB->ev_desired_targets; + localDW->ev_desired_targetsEventCounter--; + } + + if (localDW->ev_set_current_pidEventCounter > 0U) { + localB->ev_set_current_pid = !localB->ev_set_current_pid; + localDW->ev_set_current_pidEventCounter--; + } + + if (localDW->ev_set_velocity_pidEventCounter > 0U) { + localB->ev_set_velocity_pid = !localB->ev_set_velocity_pid; + localDW->ev_set_velocity_pidEventCounter--; + } + + if (localDW->ev_set_motor_configEventCounter > 0U) { + localB->ev_set_motor_config = !localB->ev_set_motor_config; + localDW->ev_set_motor_configEventCounter--; + } +} + +// Function for MATLAB Function: '/RAW2STRUCT Decoding Logic' +static CANClassTypes c_convert_to_enum_CANClassTypes(int32_T input) +{ + CANClassTypes output; + + // Initialize output value to default value for CANClassTypes (Motor_Control_Command) + output = CANClassTypes_Motor_Control_Command; + if (((input >= 0) && (input <= 2)) || ((input >= 4) && (input <= 7))) { + // Set output value to input value if it is a member of CANClassTypes + output = static_cast(input); + } + + return output; +} + +// System initialize for referenced model: 'can_decoder' +void can_decoder_Init(void) +{ + static const BUS_CAN_RX tmp = { false,// available + { { CANClassTypes_Motor_Control_Command,// CLS + 0U, // SRC + 0U // DST_TYP + }, // ID + + { 0U, // LEN + { false, // M + 0U // OPC + }, // CMD + + { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U }// ARG + } // PAYLOAD + } // packet + }; + + // SystemInitialize for Iterator SubSystem: '/Cycling Decoder' + for (int32_T ForEach_itr = 0; ForEach_itr < CAN_MAX_NUM_PACKETS; ForEach_itr++) + { + // SystemInitialize for Atomic SubSystem: '/CAN_RX_RAW2STRUCT' + // SystemInitialize for MATLAB Function: '/RAW2STRUCT Decoding Logic' + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct = tmp; + + // End of SystemInitialize for SubSystem: '/CAN_RX_RAW2STRUCT' + + // SystemInitialize for Atomic SubSystem: '/CAN_Decoder' + // SystemInitialize for Chart: '/Decoding Logic' + can_decoder_DecodingLogic_Init(&can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic, &can_decoder_DW.CoreSubsys[ForEach_itr].sf_DecodingLogic); + + // End of SystemInitialize for SubSystem: '/CAN_Decoder' + } + + // End of SystemInitialize for SubSystem: '/Cycling Decoder' +} + +// Output and update for referenced model: 'can_decoder' +void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const + ConfigurationParameters *rtu_ConfigurationParameters, + BUS_MESSAGES_RX_MULTIPLE *rty_messages_rx, + BUS_STATUS_RX_MULTIPLE *rty_status_rx, + BUS_CAN_RX_ERRORS_MULTIPLE *rty_errors_rx) +{ + // Outputs for Iterator SubSystem: '/Cycling Decoder' incorporates: + // ForEach: '/For Each' + + for (int32_T ForEach_itr = 0; ForEach_itr < CAN_MAX_NUM_PACKETS; ForEach_itr++) + { + uint8_T minval; + uint8_T x_idx_1; + + // Outputs for Atomic SubSystem: '/CAN_RX_RAW2STRUCT' + // MATLAB Function: '/RAW2STRUCT Decoding Logic' incorporates: + // ForEachSliceSelector generated from: '/pck_rx_raw' + + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.available = + rtu_pck_rx_raw->packets[ForEach_itr].available; + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.CLS = + c_convert_to_enum_CANClassTypes(static_cast(static_cast + (static_cast(rtu_pck_rx_raw->packets[ForEach_itr].packet.ID & + 1792) >> 8))); + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.SRC = + static_cast(static_cast(rtu_pck_rx_raw-> + packets[ForEach_itr].packet.ID & 240) >> 4); + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.DST_TYP = + static_cast(rtu_pck_rx_raw->packets[ForEach_itr].packet.ID & 15); + x_idx_1 = rtu_pck_rx_raw->packets[ForEach_itr].length; + minval = 8U; + if (x_idx_1 < 8) { + minval = x_idx_1; + } + + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = 0U; + if (minval > 0) { + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = + minval; + } + + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.M = + ((rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[0] & 128U) != 0U); + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.OPC = + static_cast(rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[0] + & 127); + for (int32_T i = 0; i < 8; i++) { + can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.ARG[i] = + rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[i]; + } + + boolean_T rtb_FixPtRelationalOperator; + boolean_T rtb_FixPtRelationalOperator_a; + boolean_T rtb_FixPtRelationalOperator_d; + boolean_T rtb_FixPtRelationalOperator_e; + boolean_T rtb_FixPtRelationalOperator_i; + boolean_T rtb_FixPtRelationalOperator_m; + boolean_T rtb_FixPtRelationalOperator_p; + + // End of MATLAB Function: '/RAW2STRUCT Decoding Logic' + // End of Outputs for SubSystem: '/CAN_RX_RAW2STRUCT' + + // Outputs for Atomic SubSystem: '/CAN_Decoder' + // Chart: '/Decoding Logic' incorporates: + // Constant: '/Constant' + // Constant: '/Constant1' + // Constant: '/Constant2' + + can_decoder_DecodingLogic(can_decoder_B.CoreSubsys[ForEach_itr]. + pck_rx_struct.available, &can_decoder_B.CoreSubsys[ForEach_itr]. + pck_rx_struct.packet, rtu_ConfigurationParameters, CAN_ID_AMC, 5, 10.0F, + &can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic, + &can_decoder_DW.CoreSubsys[ForEach_itr].sf_DecodingLogic); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator = (can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_set_control_mode != + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator_d = (can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_set_current_limit != + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_n); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator_a = (can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_desired_targets != + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_nk); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator_p = (can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_set_current_pid != + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_h); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator_m = (can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_set_velocity_pid != + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_l); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator_e = (can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_set_motor_config != + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_d); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator_i = (can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_error != can_decoder_DW.CoreSubsys[ForEach_itr]. + DelayInput1_DSTATE_j); + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_control_mode; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_n = + can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_set_current_limit; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_nk = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_desired_targets; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_h = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_current_pid; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_l = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_velocity_pid; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_d = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_motor_config; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_j = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_error; + + // ForEachSliceAssignment generated from: '/messages_rx' incorporates: + // BusCreator: '/Bus Creator2' + + rty_messages_rx->messages[ForEach_itr].control_mode = + can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.msg_set_control_mode; + rty_messages_rx->messages[ForEach_itr].current_limit = + can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.msg_set_current_limit; + rty_messages_rx->messages[ForEach_itr].desired_targets = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_desired_targets; + rty_messages_rx->messages[ForEach_itr].pid = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_set_pid; + rty_messages_rx->messages[ForEach_itr].motor_config = + can_decoder_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic.msg_set_motor_config; + + // ForEachSliceAssignment generated from: '/events_rx' incorporates: + // BusCreator: '/Bus Creator1' + + rty_status_rx->status[ForEach_itr].control_mode = + rtb_FixPtRelationalOperator; + rty_status_rx->status[ForEach_itr].current_limit = + rtb_FixPtRelationalOperator_d; + rty_status_rx->status[ForEach_itr].desired_targets = + rtb_FixPtRelationalOperator_a; + rty_status_rx->status[ForEach_itr].current_pid = + rtb_FixPtRelationalOperator_p; + rty_status_rx->status[ForEach_itr].velocity_pid = + rtb_FixPtRelationalOperator_m; + rty_status_rx->status[ForEach_itr].motor_config = + rtb_FixPtRelationalOperator_e; + + // ForEachSliceAssignment generated from: '/errors_rx' incorporates: + // BusCreator: '/Bus Creator3' + + rty_errors_rx->errors[ForEach_itr].event = rtb_FixPtRelationalOperator_i; + rty_errors_rx->errors[ForEach_itr].type = + can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.error_type; + + // End of Outputs for SubSystem: '/CAN_Decoder' + } + + // End of Outputs for SubSystem: '/Cycling Decoder' +} + +// Model initialize function +void can_decoder_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_can_decoder_T *const can_decoder_M = &(can_decoder_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(can_decoder_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.h index 621110bbad..d59d2ab304 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder.h @@ -1,223 +1,223 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_decoder.h -// -// Code generated for Simulink model 'can_decoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:53 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_can_decoder_h_ -#define RTW_HEADER_can_decoder_h_ -#include "rtwtypes.h" -#include "can_decoder_types.h" -#include "rtw_defines.h" - -// Block signals for system '/Decoding Logic' -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -struct B_DecodingLogic_can_decoder_T { - BUS_MSG_PID msg_set_pid; // '/Decoding Logic' - BUS_MSG_MOTOR_CONFIG msg_set_motor_config;// '/Decoding Logic' - BUS_MSG_DESIRED_TARGETS msg_desired_targets;// '/Decoding Logic' - BUS_MSG_CURRENT_LIMIT msg_set_current_limit;// '/Decoding Logic' - BUS_MSG_CONTROL_MODE msg_set_control_mode;// '/Decoding Logic' - CANErrorTypes error_type; // '/Decoding Logic' - boolean_T ev_error; // '/Decoding Logic' - boolean_T ev_set_control_mode; // '/Decoding Logic' - boolean_T ev_set_current_limit; // '/Decoding Logic' - boolean_T ev_desired_targets; // '/Decoding Logic' - boolean_T ev_set_current_pid; // '/Decoding Logic' - boolean_T ev_set_velocity_pid; // '/Decoding Logic' - boolean_T ev_set_motor_config; // '/Decoding Logic' -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -// Block states (default storage) for system '/Decoding Logic' -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -struct DW_DecodingLogic_can_decoder_T { - int32_T sfEvent; // '/Decoding Logic' - uint32_T ev_errorEventCounter; // '/Decoding Logic' - uint32_T ev_set_control_modeEventCounter;// '/Decoding Logic' - uint32_T ev_set_current_limitEventCounte;// '/Decoding Logic' - uint32_T ev_desired_targetsEventCounter;// '/Decoding Logic' - uint32_T ev_set_current_pidEventCounter;// '/Decoding Logic' - uint32_T ev_set_velocity_pidEventCounter;// '/Decoding Logic' - uint32_T ev_set_motor_configEventCounter;// '/Decoding Logic' - uint16_T cmd_processed; // '/Decoding Logic' - uint8_T is_SET_CONTROL_MODE; // '/Decoding Logic' - uint8_T is_DESIRED_TARGETS; // '/Decoding Logic' - uint8_T is_SET_OPTIONS; // '/Decoding Logic' - uint8_T is_SET_MOTOR_CONFIG; // '/Decoding Logic' - uint8_T is_ERROR_HANDLING; // '/Decoding Logic' - uint8_T is_active_c3_can_decoder; // '/Decoding Logic' - uint8_T is_active_SET_CONTROL_MODE; // '/Decoding Logic' - uint8_T is_active_DESIRED_TARGETS; // '/Decoding Logic' - uint8_T is_active_SET_OPTIONS; // '/Decoding Logic' - uint8_T is_active_SET_MOTOR_CONFIG; // '/Decoding Logic' - uint8_T is_active_ERROR_HANDLING; // '/Decoding Logic' - boolean_T ev_async; // '/Decoding Logic' -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -// Block signals for system '/Cycling Decoder' -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -struct B_CoreSubsys_can_decoder_T { - BUS_CAN_RX pck_rx_struct; // '/RAW2STRUCT Decoding Logic' - B_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -// Block states (default storage) for system '/Cycling Decoder' -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -struct DW_CoreSubsys_can_decoder_T { - boolean_T DelayInput1_DSTATE; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_n; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_nk; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_h; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_l; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_d; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_j; // '/Delay Input1' - DW_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -// Block signals for model 'can_decoder' -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -struct B_can_decoder_c_T { - B_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -// Block states (default storage) for model 'can_decoder' -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -struct DW_can_decoder_f_T { - DW_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_can_decoder_T { - const char_T **errorStatus; -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_can_decoder_T { - RT_MODEL_can_decoder_T rtm; -}; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -// -// Exported Global Parameters -// -// Note: Exported global parameters are tunable parameters with an exported -// global storage class designation. Code generation will declare the memory for -// these parameters and exports their symbols. -// - -extern real32_T CAN_ANGLE_ICUB2DEG; // Variable: CAN_ANGLE_ICUB2DEG - // Referenced by: '/Decoding Logic' - // 360/2^16 - -extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC - // Referenced by: '/Constant' - // 4 bits defining the ID of the AMC_BLDC board. - -extern void can_decoder_Init(void); -extern void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const - ConfigurationParameters *rtu_ConfigurationParameters, BUS_MESSAGES_RX_MULTIPLE - *rty_messages_rx, BUS_STATUS_RX_MULTIPLE *rty_status_rx, - BUS_CAN_RX_ERRORS_MULTIPLE *rty_errors_rx); - -// Model reference registration function -extern void can_decoder_initialize(const char_T **rt_errorStatus); - -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -extern void can_decoder_DecodingLogic_Init(B_DecodingLogic_can_decoder_T *localB, - DW_DecodingLogic_can_decoder_T *localDW); -extern void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const - BUS_CAN_PACKET_RX *rtu_pck_input, const ConfigurationParameters - *rtu_ConfigurationParameters, uint8_T rtu_CAN_ID_DST, uint8_T - rtu_CAN_VOLT_REF_SHIFT, real32_T rtu_CAN_VOLT_REF_GAIN, - B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW); - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_can_decoder_T can_decoder_MdlrefDW; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -#ifndef can_decoder_MDLREF_HIDE_CHILD_ - -// Block signals (default storage) -extern B_can_decoder_c_T can_decoder_B; - -// Block states (default storage) -extern DW_can_decoder_f_T can_decoder_DW; - -#endif //can_decoder_MDLREF_HIDE_CHILD_ - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'can_decoder' -// '' : 'can_decoder/Cycling Decoder' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder' -// '' : 'can_decoder/Cycling Decoder/CAN_RX_RAW2STRUCT' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Decoding Logic' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change1' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change2' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change3' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change4' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change5' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change6' -// '' : 'can_decoder/Cycling Decoder/CAN_RX_RAW2STRUCT/RAW2STRUCT Decoding Logic' - -#endif // RTW_HEADER_can_decoder_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_decoder.h +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:41 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_decoder_h_ +#define RTW_HEADER_can_decoder_h_ +#include "rtwtypes.h" +#include "can_decoder_types.h" +#include "rtw_defines.h" + +// Block signals for system '/Decoding Logic' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct B_DecodingLogic_can_decoder_T { + BUS_MSG_PID msg_set_pid; // '/Decoding Logic' + BUS_MSG_MOTOR_CONFIG msg_set_motor_config;// '/Decoding Logic' + BUS_MSG_DESIRED_TARGETS msg_desired_targets;// '/Decoding Logic' + BUS_MSG_CURRENT_LIMIT msg_set_current_limit;// '/Decoding Logic' + BUS_MSG_CONTROL_MODE msg_set_control_mode;// '/Decoding Logic' + CANErrorTypes error_type; // '/Decoding Logic' + boolean_T ev_error; // '/Decoding Logic' + boolean_T ev_set_control_mode; // '/Decoding Logic' + boolean_T ev_set_current_limit; // '/Decoding Logic' + boolean_T ev_desired_targets; // '/Decoding Logic' + boolean_T ev_set_current_pid; // '/Decoding Logic' + boolean_T ev_set_velocity_pid; // '/Decoding Logic' + boolean_T ev_set_motor_config; // '/Decoding Logic' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for system '/Decoding Logic' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct DW_DecodingLogic_can_decoder_T { + int32_T sfEvent; // '/Decoding Logic' + uint32_T ev_errorEventCounter; // '/Decoding Logic' + uint32_T ev_set_control_modeEventCounter;// '/Decoding Logic' + uint32_T ev_set_current_limitEventCounte;// '/Decoding Logic' + uint32_T ev_desired_targetsEventCounter;// '/Decoding Logic' + uint32_T ev_set_current_pidEventCounter;// '/Decoding Logic' + uint32_T ev_set_velocity_pidEventCounter;// '/Decoding Logic' + uint32_T ev_set_motor_configEventCounter;// '/Decoding Logic' + uint16_T cmd_processed; // '/Decoding Logic' + uint8_T is_SET_CONTROL_MODE; // '/Decoding Logic' + uint8_T is_DESIRED_TARGETS; // '/Decoding Logic' + uint8_T is_SET_OPTIONS; // '/Decoding Logic' + uint8_T is_SET_MOTOR_CONFIG; // '/Decoding Logic' + uint8_T is_ERROR_HANDLING; // '/Decoding Logic' + uint8_T is_active_c3_can_decoder; // '/Decoding Logic' + uint8_T is_active_SET_CONTROL_MODE; // '/Decoding Logic' + uint8_T is_active_DESIRED_TARGETS; // '/Decoding Logic' + uint8_T is_active_SET_OPTIONS; // '/Decoding Logic' + uint8_T is_active_SET_MOTOR_CONFIG; // '/Decoding Logic' + uint8_T is_active_ERROR_HANDLING; // '/Decoding Logic' + boolean_T ev_async; // '/Decoding Logic' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block signals for system '/Cycling Decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct B_CoreSubsys_can_decoder_T { + BUS_CAN_RX pck_rx_struct; // '/RAW2STRUCT Decoding Logic' + B_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for system '/Cycling Decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct DW_CoreSubsys_can_decoder_T { + boolean_T DelayInput1_DSTATE; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_n; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_nk; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_h; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_l; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_d; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_j; // '/Delay Input1' + DW_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block signals for model 'can_decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct B_can_decoder_c_T { + B_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for model 'can_decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct DW_can_decoder_f_T { + DW_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_can_decoder_T { + const char_T **errorStatus; +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_can_decoder_T { + RT_MODEL_can_decoder_T rtm; +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// +// Exported Global Parameters +// +// Note: Exported global parameters are tunable parameters with an exported +// global storage class designation. Code generation will declare the memory for +// these parameters and exports their symbols. +// + +extern real32_T CAN_ANGLE_ICUB2DEG; // Variable: CAN_ANGLE_ICUB2DEG + // Referenced by: '/Decoding Logic' + // 360/2^16 + +extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC + // Referenced by: '/Constant' + // 4 bits defining the ID of the AMC_BLDC board. + +extern void can_decoder_Init(void); +extern void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const + ConfigurationParameters *rtu_ConfigurationParameters, BUS_MESSAGES_RX_MULTIPLE + *rty_messages_rx, BUS_STATUS_RX_MULTIPLE *rty_status_rx, + BUS_CAN_RX_ERRORS_MULTIPLE *rty_errors_rx); + +// Model reference registration function +extern void can_decoder_initialize(const char_T **rt_errorStatus); + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +extern void can_decoder_DecodingLogic_Init(B_DecodingLogic_can_decoder_T *localB, + DW_DecodingLogic_can_decoder_T *localDW); +extern void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const + BUS_CAN_PACKET_RX *rtu_pck_input, const ConfigurationParameters + *rtu_ConfigurationParameters, uint8_T rtu_CAN_ID_DST, uint8_T + rtu_CAN_VOLT_REF_SHIFT, real32_T rtu_CAN_VOLT_REF_GAIN, + B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW); + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_can_decoder_T can_decoder_MdlrefDW; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +// Block signals (default storage) +extern B_can_decoder_c_T can_decoder_B; + +// Block states (default storage) +extern DW_can_decoder_f_T can_decoder_DW; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'can_decoder' +// '' : 'can_decoder/Cycling Decoder' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder' +// '' : 'can_decoder/Cycling Decoder/CAN_RX_RAW2STRUCT' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Decoding Logic' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change1' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change2' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change3' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change4' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change5' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change6' +// '' : 'can_decoder/Cycling Decoder/CAN_RX_RAW2STRUCT/RAW2STRUCT Decoding Logic' + +#endif // RTW_HEADER_can_decoder_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_private.h index 269f675a91..54253ae4ca 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_private.h @@ -1,46 +1,46 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_decoder_private.h -// -// Code generated for Simulink model 'can_decoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:53 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_can_decoder_private_h_ -#define RTW_HEADER_can_decoder_private_h_ -#include "rtwtypes.h" -#include "can_decoder_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif -#endif // RTW_HEADER_can_decoder_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_decoder_private.h +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:41 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_decoder_private_h_ +#define RTW_HEADER_can_decoder_private_h_ +#include "rtwtypes.h" +#include "can_decoder_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif +#endif // RTW_HEADER_can_decoder_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_types.h index b1f1fdce46..bb6770fd43 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-decoder/can_decoder_types.h @@ -1,545 +1,545 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_decoder_types.h -// -// Code generated for Simulink model 'can_decoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:53 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_can_decoder_types_h_ -#define RTW_HEADER_can_decoder_types_h_ -#include "rtwtypes.h" -#include "can_decoder_types.h" - -// Includes for objects with custom storage classes -#include "rtw_defines.h" - -// -// Constraints for division operations in dimension variants - -#if (1 == 0) || ((CAN_MAX_NUM_PACKETS % 1) != 0) -# error "The preprocessor definition '1' must not be equal to zero and the division of 'CAN_MAX_NUM_PACKETS' by '1' must not have a remainder." -#endif - -// -// Registered constraints for dimension variants - -#if CAN_MAX_NUM_PACKETS <= 0 -# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be greater than '0'" -#endif - -#if CAN_MAX_NUM_PACKETS >= 16 -# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be less than '16'" -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ - -// Fields of a transmitted CAN packet. -struct BUS_CAN_PACKET -{ - // ID of the CAN packet. - uint16_T ID; - - // PAYLOAD of the CAN packet. - uint8_T PAYLOAD[8]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_ - -struct BUS_CAN -{ - // If true, the packet is available to be processed. - boolean_T available; - uint8_T length; - BUS_CAN_PACKET packet; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ - -struct BUS_CAN_MULTIPLE -{ - BUS_CAN packets[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ -#define DEFINED_TYPEDEF_FOR_MCControlModes_ - -typedef enum { - MCControlModes_Idle = 0, // Default value - MCControlModes_OpenLoop = 80, - MCControlModes_SpeedVoltage = 10, - MCControlModes_SpeedCurrent = 11, - MCControlModes_Current = 6, - MCControlModes_NotConfigured = 176, - MCControlModes_HWFault = 160 -} MCControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ - -// Fields of a CONTROL_MODE message. -struct BUS_MSG_CONTROL_MODE -{ - // Motor selector. - boolean_T motor; - - // Control mode. - MCControlModes mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ - -// Fields of a CURRENT_LIMIT message. -struct BUS_MSG_CURRENT_LIMIT -{ - // Motor selector. - boolean_T motor; - - // Nominal current in A. - real32_T nominal; - - // Peak current in A. - real32_T peak; - - // Overload current in A. - real32_T overload; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ - -// Fields of a DESIRED_TARGETS message. -struct BUS_MSG_DESIRED_TARGETS -{ - // Target current in A. - real32_T current; - - // Target voltage in %. - real32_T voltage; - - // Target veocity in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ - -// Fields of a CURRENT_PID message. -struct BUS_MSG_PID -{ - // Motor selector. - boolean_T motor; - - // Proportional gain. - real32_T Kp; - - // Integral gain. - real32_T Ki; - - // Derivative gain. - real32_T Kd; - - // Shift factor. - uint8_T Ks; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ - -struct BUS_MSG_MOTOR_CONFIG -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - - // Number of polese of the motor. - uint8_T number_poles; - - // Encoder tolerance. - uint8_T encoder_tolerance; - - // Resolution of rotor encoder. - int16_T rotor_encoder_resolution; - - // Offset of the rotor encoder. - int16_T rotor_index_offset; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ - -// Aggregate of all CAN received messages. -struct BUS_MESSAGES_RX -{ - BUS_MSG_CONTROL_MODE control_mode; - BUS_MSG_CURRENT_LIMIT current_limit; - BUS_MSG_DESIRED_TARGETS desired_targets; - BUS_MSG_PID pid; - BUS_MSG_MOTOR_CONFIG motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ - -struct BUS_MESSAGES_RX_MULTIPLE -{ - BUS_MESSAGES_RX messages[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ - -// Aggregate of all events specifying types of received messages. -struct BUS_STATUS_RX -{ - boolean_T control_mode; - boolean_T current_limit; - boolean_T desired_targets; - boolean_T current_pid; - boolean_T velocity_pid; - boolean_T motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ - -struct BUS_STATUS_RX_MULTIPLE -{ - BUS_STATUS_RX status[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ -#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ - -typedef enum { - CANErrorTypes_No_Error = 0, // Default value - CANErrorTypes_Packet_Not4Us, - CANErrorTypes_Packet_Unrecognized, - CANErrorTypes_Packet_Malformed, - CANErrorTypes_Packet_MultiFunctionsDetected, - CANErrorTypes_Mode_Unrecognized -} CANErrorTypes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ - -// Specifies the CAN error types. -struct BUS_CAN_RX_ERRORS -{ - // True if an error has been detected. - boolean_T event; - CANErrorTypes type; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ - -struct BUS_CAN_RX_ERRORS_MULTIPLE -{ - BUS_CAN_RX_ERRORS errors[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_CANClassTypes_ -#define DEFINED_TYPEDEF_FOR_CANClassTypes_ - -typedef enum { - CANClassTypes_Motor_Control_Command = 0,// Default value - CANClassTypes_Motor_Control_Streaming = 1, - CANClassTypes_Analog_Sensors_Command = 2, - CANClassTypes_Skin_Sensor_Streaming = 4, - CANClassTypes_Inertial_Sensor_Streaming = 5, - CANClassTypes_Future_Use = 6, - CANClassTypes_Management_Bootloader = 7 -} CANClassTypes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ - -struct BUS_CAN_ID_RX -{ - // 3 bits defining the message class type. - CANClassTypes CLS; - - // 4 bits defining the source ID. - uint8_T SRC; - - // 4 bits definint the destination ID or the message sub-type. - uint8_T DST_TYP; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ - -struct BUS_CAN_CMD -{ - // 1 bits for motor selector. - boolean_T M; - - // 7 bits defining the operational code of the command. - uint8_T OPC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ - -struct BUS_CAN_PAYLOAD_RX -{ - // Actual length of the total PAYLOAD field. - uint8_T LEN; - BUS_CAN_CMD CMD; - - // 8 bytes for the command argument in order to account also message of type streaming. - uint8_T ARG[8]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ - -// Fields of a received CAN packet. -struct BUS_CAN_PACKET_RX -{ - // ID of the CAN packet. - BUS_CAN_ID_RX ID; - - // PAYLOAD of the CAN packet. - BUS_CAN_PAYLOAD_RX PAYLOAD; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ - -// Specifies the CAN input. -struct BUS_CAN_RX -{ - // If true, the packet is available to be processed. - boolean_T available; - BUS_CAN_PACKET_RX packet; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCOPC_ -#define DEFINED_TYPEDEF_FOR_MCOPC_ - -typedef enum { - MCOPC_Set_Control_Mode = 9, // Default value - MCOPC_Set_Current_Limit = 72, - MCOPC_Set_Current_PID = 101, - MCOPC_Set_Velocity_PID = 105, - MCOPC_Set_Motor_Config = 119 -} MCOPC; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCStreaming_ -#define DEFINED_TYPEDEF_FOR_MCStreaming_ - -typedef enum { - MCStreaming_Desired_Targets = 15, // Default value - MCStreaming_FOC = 0 -} MCStreaming; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_can_decoder_T RT_MODEL_can_decoder_T; - -#endif // RTW_HEADER_can_decoder_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_decoder_types.h +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:41 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_decoder_types_h_ +#define RTW_HEADER_can_decoder_types_h_ +#include "rtwtypes.h" +#include "can_decoder_types.h" + +// Includes for objects with custom storage classes +#include "rtw_defines.h" + +// +// Constraints for division operations in dimension variants + +#if (1 == 0) || ((CAN_MAX_NUM_PACKETS % 1) != 0) +# error "The preprocessor definition '1' must not be equal to zero and the division of 'CAN_MAX_NUM_PACKETS' by '1' must not have a remainder." +#endif + +// +// Registered constraints for dimension variants + +#if CAN_MAX_NUM_PACKETS <= 0 +# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be greater than '0'" +#endif + +#if CAN_MAX_NUM_PACKETS >= 16 +# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be less than '16'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ + +// Fields of a transmitted CAN packet. +struct BUS_CAN_PACKET +{ + // ID of the CAN packet. + uint16_T ID; + + // PAYLOAD of the CAN packet. + uint8_T PAYLOAD[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ + +struct BUS_CAN +{ + // If true, the packet is available to be processed. + boolean_T available; + uint8_T length; + BUS_CAN_PACKET packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ + +struct BUS_CAN_MULTIPLE +{ + BUS_CAN packets[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ +#define DEFINED_TYPEDEF_FOR_MCControlModes_ + +typedef enum { + MCControlModes_Idle = 0, // Default value + MCControlModes_OpenLoop = 80, + MCControlModes_SpeedVoltage = 10, + MCControlModes_SpeedCurrent = 11, + MCControlModes_Current = 6, + MCControlModes_NotConfigured = 176, + MCControlModes_HWFault = 160 +} MCControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ + +// Fields of a CONTROL_MODE message. +struct BUS_MSG_CONTROL_MODE +{ + // Motor selector. + boolean_T motor; + + // Control mode. + MCControlModes mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ + +// Fields of a CURRENT_LIMIT message. +struct BUS_MSG_CURRENT_LIMIT +{ + // Motor selector. + boolean_T motor; + + // Nominal current in A. + real32_T nominal; + + // Peak current in A. + real32_T peak; + + // Overload current in A. + real32_T overload; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ + +// Fields of a DESIRED_TARGETS message. +struct BUS_MSG_DESIRED_TARGETS +{ + // Target current in A. + real32_T current; + + // Target voltage in %. + real32_T voltage; + + // Target veocity in deg/s. + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ + +// Fields of a CURRENT_PID message. +struct BUS_MSG_PID +{ + // Motor selector. + boolean_T motor; + + // Proportional gain. + real32_T Kp; + + // Integral gain. + real32_T Ki; + + // Derivative gain. + real32_T Kd; + + // Shift factor. + uint8_T Ks; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ + +struct BUS_MSG_MOTOR_CONFIG +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + + // Number of polese of the motor. + uint8_T number_poles; + + // Encoder tolerance. + uint8_T encoder_tolerance; + + // Resolution of rotor encoder. + int16_T rotor_encoder_resolution; + + // Offset of the rotor encoder. + int16_T rotor_index_offset; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ + +// Aggregate of all CAN received messages. +struct BUS_MESSAGES_RX +{ + BUS_MSG_CONTROL_MODE control_mode; + BUS_MSG_CURRENT_LIMIT current_limit; + BUS_MSG_DESIRED_TARGETS desired_targets; + BUS_MSG_PID pid; + BUS_MSG_MOTOR_CONFIG motor_config; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ + +struct BUS_MESSAGES_RX_MULTIPLE +{ + BUS_MESSAGES_RX messages[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ + +// Aggregate of all events specifying types of received messages. +struct BUS_STATUS_RX +{ + boolean_T control_mode; + boolean_T current_limit; + boolean_T desired_targets; + boolean_T current_pid; + boolean_T velocity_pid; + boolean_T motor_config; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ + +struct BUS_STATUS_RX_MULTIPLE +{ + BUS_STATUS_RX status[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ +#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ + +typedef enum { + CANErrorTypes_No_Error = 0, // Default value + CANErrorTypes_Packet_Not4Us, + CANErrorTypes_Packet_Unrecognized, + CANErrorTypes_Packet_Malformed, + CANErrorTypes_Packet_MultiFunctionsDetected, + CANErrorTypes_Mode_Unrecognized +} CANErrorTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ + +// Specifies the CAN error types. +struct BUS_CAN_RX_ERRORS +{ + // True if an error has been detected. + boolean_T event; + CANErrorTypes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ + +struct BUS_CAN_RX_ERRORS_MULTIPLE +{ + BUS_CAN_RX_ERRORS errors[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_CANClassTypes_ +#define DEFINED_TYPEDEF_FOR_CANClassTypes_ + +typedef enum { + CANClassTypes_Motor_Control_Command = 0,// Default value + CANClassTypes_Motor_Control_Streaming = 1, + CANClassTypes_Analog_Sensors_Command = 2, + CANClassTypes_Skin_Sensor_Streaming = 4, + CANClassTypes_Inertial_Sensor_Streaming = 5, + CANClassTypes_Future_Use = 6, + CANClassTypes_Management_Bootloader = 7 +} CANClassTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ + +struct BUS_CAN_ID_RX +{ + // 3 bits defining the message class type. + CANClassTypes CLS; + + // 4 bits defining the source ID. + uint8_T SRC; + + // 4 bits definint the destination ID or the message sub-type. + uint8_T DST_TYP; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ + +struct BUS_CAN_CMD +{ + // 1 bits for motor selector. + boolean_T M; + + // 7 bits defining the operational code of the command. + uint8_T OPC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ + +struct BUS_CAN_PAYLOAD_RX +{ + // Actual length of the total PAYLOAD field. + uint8_T LEN; + BUS_CAN_CMD CMD; + + // 8 bytes for the command argument in order to account also message of type streaming. + uint8_T ARG[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ + +// Fields of a received CAN packet. +struct BUS_CAN_PACKET_RX +{ + // ID of the CAN packet. + BUS_CAN_ID_RX ID; + + // PAYLOAD of the CAN packet. + BUS_CAN_PAYLOAD_RX PAYLOAD; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ + +// Specifies the CAN input. +struct BUS_CAN_RX +{ + // If true, the packet is available to be processed. + boolean_T available; + BUS_CAN_PACKET_RX packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCOPC_ +#define DEFINED_TYPEDEF_FOR_MCOPC_ + +typedef enum { + MCOPC_Set_Control_Mode = 9, // Default value + MCOPC_Set_Current_Limit = 72, + MCOPC_Set_Current_PID = 101, + MCOPC_Set_Velocity_PID = 105, + MCOPC_Set_Motor_Config = 119 +} MCOPC; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCStreaming_ +#define DEFINED_TYPEDEF_FOR_MCStreaming_ + +typedef enum { + MCStreaming_Desired_Targets = 15, // Default value + MCStreaming_FOC = 0 +} MCStreaming; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_can_decoder_T RT_MODEL_can_decoder_T; + +#endif // RTW_HEADER_can_decoder_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.cpp index eb32e209fb..93f76bb3fe 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.cpp @@ -1,229 +1,229 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_encoder.cpp -// -// Code generated for Simulink model 'can_encoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:59 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "can_encoder.h" -#include "rtwtypes.h" -#include "can_encoder_types.h" -#include -#include -#include "can_encoder_private.h" -#include "rtw_defines.h" - -MdlrefDW_can_encoder_T can_encoder_MdlrefDW; - -// -// Output and update for atomic system: -// '/format_can_id' -// '/format_can_id' -// -void can_encoder_format_can_id(uint8_T rtu_class, uint8_T rtu_can_id_amc, - uint8_T rtu_dst_typ, uint16_T *rty_pkt_id) -{ - *rty_pkt_id = static_cast(rtu_class << 8); - *rty_pkt_id = static_cast(rtu_can_id_amc << 4 | *rty_pkt_id); - *rty_pkt_id = static_cast(rtu_dst_typ | *rty_pkt_id); -} - -// Output and update for referenced model: 'can_encoder' -void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const BUS_STATUS_TX - *rtu_status_tx, const ConfigurationParameters - *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx) -{ - BUS_CAN rtb_BusCreator_n_0[4]; - BUS_CAN rtb_BusCreator_hs; - BUS_CAN rtb_BusCreator_m; - BUS_CAN rtb_BusCreator_n; - int32_T i; - real32_T tmp; - uint32_T qY; - int16_T rtb_DataTypeConversion1_0; - int16_T rtb_DataTypeConversion_k; - uint8_T b_tmp[4]; - uint8_T tmp2[2]; - - // Outputs for Atomic SubSystem: '/CAN_Encoder' - // MATLAB Function: '/format_status_pck' incorporates: - // BusCreator: '/Bus Creator' - - rtb_BusCreator_m.packet.PAYLOAD[0] = static_cast - (rtu_messages_tx->status.control_mode); - rtb_BusCreator_m.packet.PAYLOAD[1] = 0U; - qY = 5U - rtu_ConfigurationParameters->CurLoopPID.shift_factor; - if (5U - rtu_ConfigurationParameters->CurLoopPID.shift_factor > 5U) { - qY = 0U; - } - - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Constant: '/length1' - // Product: '/Product' - - tmp = rtu_messages_tx->status.pwm_fbk * 10.0F; - if (tmp < 32768.0F) { - if (tmp >= -32768.0F) { - rtb_DataTypeConversion_k = static_cast(tmp); - } else { - rtb_DataTypeConversion_k = MIN_int16_T; - } - } else { - rtb_DataTypeConversion_k = MAX_int16_T; - } - - // MATLAB Function: '/format_status_pck' incorporates: - // BusCreator: '/Bus Creator' - // DataTypeConversion: '/Data Type Conversion' - - rtb_DataTypeConversion_k = static_cast(rtb_DataTypeConversion_k << - static_cast(qY)); - std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion_k, (size_t)2 * - sizeof(uint8_T)); - rtb_BusCreator_m.packet.PAYLOAD[2] = tmp2[0]; - rtb_BusCreator_m.packet.PAYLOAD[3] = tmp2[1]; - rtb_BusCreator_m.packet.PAYLOAD[4] = - rtu_messages_tx->status.flags.ExternalFaultAsserted; - rtb_BusCreator_m.packet.PAYLOAD[5] = 0U; - rtb_BusCreator_m.packet.PAYLOAD[6] = 0U; - rtb_BusCreator_m.packet.PAYLOAD[7] = 0U; - - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Gain: '/Gain2' - - tmp = 1000.0F * rtu_messages_tx->foc.current; - if (tmp < 32768.0F) { - if (tmp >= -32768.0F) { - rtb_DataTypeConversion_k = static_cast(tmp); - } else { - rtb_DataTypeConversion_k = MIN_int16_T; - } - } else { - rtb_DataTypeConversion_k = MAX_int16_T; - } - - // DataTypeConversion: '/Data Type Conversion1' incorporates: - // Gain: '/Gain1' - - tmp = CAN_ANGLE_DEG2ICUB / 1000.0F * rtu_messages_tx->foc.velocity; - if (tmp < 32768.0F) { - if (tmp >= -32768.0F) { - rtb_DataTypeConversion1_0 = static_cast(tmp); - } else { - rtb_DataTypeConversion1_0 = MIN_int16_T; - } - } else { - rtb_DataTypeConversion1_0 = MAX_int16_T; - } - - // DataTypeConversion: '/Data Type Conversion2' incorporates: - // Gain: '/Gain3' - - tmp = CAN_ANGLE_DEG2ICUB * rtu_messages_tx->foc.position; - if (tmp < 2.14748365E+9F) { - if (tmp >= -2.14748365E+9F) { - i = static_cast(tmp); - } else { - i = MIN_int32_T; - } - } else { - i = MAX_int32_T; - } - - // MATLAB Function: '/format_foc_pck' incorporates: - // BusCreator: '/Bus Creator' - // DataTypeConversion: '/Data Type Conversion' - // DataTypeConversion: '/Data Type Conversion1' - // DataTypeConversion: '/Data Type Conversion2' - - std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion_k, (size_t)2 * - sizeof(uint8_T)); - rtb_BusCreator_n.packet.PAYLOAD[0] = tmp2[0]; - rtb_BusCreator_n.packet.PAYLOAD[1] = tmp2[1]; - std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion1_0, (size_t)2 * - sizeof(uint8_T)); - rtb_BusCreator_n.packet.PAYLOAD[2] = tmp2[0]; - rtb_BusCreator_n.packet.PAYLOAD[3] = tmp2[1]; - std::memcpy((void *)&b_tmp[0], (void *)&i, (size_t)4 * sizeof(uint8_T)); - rtb_BusCreator_n.packet.PAYLOAD[4] = b_tmp[0]; - rtb_BusCreator_n.packet.PAYLOAD[5] = b_tmp[1]; - rtb_BusCreator_n.packet.PAYLOAD[6] = b_tmp[2]; - rtb_BusCreator_n.packet.PAYLOAD[7] = b_tmp[3]; - - // BusCreator: '/Bus Creator' incorporates: - // Constant: '/Constant' - // Constant: '/Motor Control Streaming' - // Constant: '/TYPESTATUS' - // Constant: '/length' - // MATLAB Function: '/format_can_id' - - can_encoder_format_can_id(1, CAN_ID_AMC, 3, &rtb_BusCreator_m.packet.ID); - rtb_BusCreator_m.available = rtu_status_tx->status; - rtb_BusCreator_m.length = 8U; - - // BusCreator: '/Bus Creator' incorporates: - // Constant: '/Constant' - // Constant: '/Motor Control Streaming' - // Constant: '/TYPE2FOC' - // Constant: '/length' - // MATLAB Function: '/format_can_id' - - can_encoder_format_can_id(1, CAN_ID_AMC, 0, &rtb_BusCreator_n.packet.ID); - rtb_BusCreator_n.available = rtu_status_tx->foc; - rtb_BusCreator_n.length = 8U; - - // BusCreator: '/Bus Creator' incorporates: - // BusCreator: '/Bus Creator6' - // Constant: '/ID_raw' - // Constant: '/available' - // Constant: '/length' - - rtb_BusCreator_hs.available = false; - rtb_BusCreator_hs.length = 0U; - rtb_BusCreator_hs.packet.ID = 0U; - for (i = 0; i < 8; i++) { - rtb_BusCreator_hs.packet.PAYLOAD[i] = 0U; - } - - // End of BusCreator: '/Bus Creator' - - // Concatenate: '/Vector Concatenate' - rtb_BusCreator_n_0[0] = rtb_BusCreator_m; - rtb_BusCreator_n_0[1] = rtb_BusCreator_n; - rtb_BusCreator_n_0[2] = rtb_BusCreator_hs; - rtb_BusCreator_n_0[3] = rtb_BusCreator_hs; - for (i = 0; i < CAN_MAX_NUM_PACKETS; i++) { - rty_pck_tx->packets[i] = rtb_BusCreator_n_0[i]; - } - - // End of Concatenate: '/Vector Concatenate' - // End of Outputs for SubSystem: '/CAN_Encoder' -} - -// Model initialize function -void can_encoder_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_can_encoder_T *const can_encoder_M = &(can_encoder_MdlrefDW.rtm); - - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(can_encoder_M, rt_errorStatus); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_encoder.cpp +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:46 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "can_encoder.h" +#include "rtwtypes.h" +#include "can_encoder_types.h" +#include +#include +#include "can_encoder_private.h" +#include "rtw_defines.h" + +MdlrefDW_can_encoder_T can_encoder_MdlrefDW; + +// +// Output and update for atomic system: +// '/format_can_id' +// '/format_can_id' +// +void can_encoder_format_can_id(uint8_T rtu_class, uint8_T rtu_can_id_amc, + uint8_T rtu_dst_typ, uint16_T *rty_pkt_id) +{ + *rty_pkt_id = static_cast(rtu_class << 8); + *rty_pkt_id = static_cast(rtu_can_id_amc << 4 | *rty_pkt_id); + *rty_pkt_id = static_cast(rtu_dst_typ | *rty_pkt_id); +} + +// Output and update for referenced model: 'can_encoder' +void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const BUS_STATUS_TX + *rtu_status_tx, const ConfigurationParameters + *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx) +{ + BUS_CAN rtb_BusCreator_n_0[4]; + BUS_CAN rtb_BusCreator_hs; + BUS_CAN rtb_BusCreator_m; + BUS_CAN rtb_BusCreator_n; + int32_T i; + real32_T tmp; + uint32_T qY; + int16_T rtb_DataTypeConversion1_0; + int16_T rtb_DataTypeConversion_k; + uint8_T b_tmp[4]; + uint8_T tmp2[2]; + + // Outputs for Atomic SubSystem: '/CAN_Encoder' + // MATLAB Function: '/format_status_pck' incorporates: + // BusCreator: '/Bus Creator' + + rtb_BusCreator_m.packet.PAYLOAD[0] = static_cast + (rtu_messages_tx->status.control_mode); + rtb_BusCreator_m.packet.PAYLOAD[1] = 0U; + qY = 5U - rtu_ConfigurationParameters->CurLoopPID.shift_factor; + if (5U - rtu_ConfigurationParameters->CurLoopPID.shift_factor > 5U) { + qY = 0U; + } + + // DataTypeConversion: '/Data Type Conversion' incorporates: + // Constant: '/length1' + // Product: '/Product' + + tmp = rtu_messages_tx->status.pwm_fbk * 10.0F; + if (tmp < 32768.0F) { + if (tmp >= -32768.0F) { + rtb_DataTypeConversion_k = static_cast(tmp); + } else { + rtb_DataTypeConversion_k = MIN_int16_T; + } + } else { + rtb_DataTypeConversion_k = MAX_int16_T; + } + + // MATLAB Function: '/format_status_pck' incorporates: + // BusCreator: '/Bus Creator' + // DataTypeConversion: '/Data Type Conversion' + + rtb_DataTypeConversion_k = static_cast(rtb_DataTypeConversion_k << + static_cast(qY)); + std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion_k, (size_t)2 * + sizeof(uint8_T)); + rtb_BusCreator_m.packet.PAYLOAD[2] = tmp2[0]; + rtb_BusCreator_m.packet.PAYLOAD[3] = tmp2[1]; + rtb_BusCreator_m.packet.PAYLOAD[4] = + rtu_messages_tx->status.flags.ExternalFaultAsserted; + rtb_BusCreator_m.packet.PAYLOAD[5] = 0U; + rtb_BusCreator_m.packet.PAYLOAD[6] = 0U; + rtb_BusCreator_m.packet.PAYLOAD[7] = 0U; + + // DataTypeConversion: '/Data Type Conversion' incorporates: + // Gain: '/Gain2' + + tmp = 1000.0F * rtu_messages_tx->foc.current; + if (tmp < 32768.0F) { + if (tmp >= -32768.0F) { + rtb_DataTypeConversion_k = static_cast(tmp); + } else { + rtb_DataTypeConversion_k = MIN_int16_T; + } + } else { + rtb_DataTypeConversion_k = MAX_int16_T; + } + + // DataTypeConversion: '/Data Type Conversion1' incorporates: + // Gain: '/Gain1' + + tmp = CAN_ANGLE_DEG2ICUB / 1000.0F * rtu_messages_tx->foc.velocity; + if (tmp < 32768.0F) { + if (tmp >= -32768.0F) { + rtb_DataTypeConversion1_0 = static_cast(tmp); + } else { + rtb_DataTypeConversion1_0 = MIN_int16_T; + } + } else { + rtb_DataTypeConversion1_0 = MAX_int16_T; + } + + // DataTypeConversion: '/Data Type Conversion2' incorporates: + // Gain: '/Gain3' + + tmp = CAN_ANGLE_DEG2ICUB * rtu_messages_tx->foc.position; + if (tmp < 2.14748365E+9F) { + if (tmp >= -2.14748365E+9F) { + i = static_cast(tmp); + } else { + i = MIN_int32_T; + } + } else { + i = MAX_int32_T; + } + + // MATLAB Function: '/format_foc_pck' incorporates: + // BusCreator: '/Bus Creator' + // DataTypeConversion: '/Data Type Conversion' + // DataTypeConversion: '/Data Type Conversion1' + // DataTypeConversion: '/Data Type Conversion2' + + std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion_k, (size_t)2 * + sizeof(uint8_T)); + rtb_BusCreator_n.packet.PAYLOAD[0] = tmp2[0]; + rtb_BusCreator_n.packet.PAYLOAD[1] = tmp2[1]; + std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion1_0, (size_t)2 * + sizeof(uint8_T)); + rtb_BusCreator_n.packet.PAYLOAD[2] = tmp2[0]; + rtb_BusCreator_n.packet.PAYLOAD[3] = tmp2[1]; + std::memcpy((void *)&b_tmp[0], (void *)&i, (size_t)4 * sizeof(uint8_T)); + rtb_BusCreator_n.packet.PAYLOAD[4] = b_tmp[0]; + rtb_BusCreator_n.packet.PAYLOAD[5] = b_tmp[1]; + rtb_BusCreator_n.packet.PAYLOAD[6] = b_tmp[2]; + rtb_BusCreator_n.packet.PAYLOAD[7] = b_tmp[3]; + + // BusCreator: '/Bus Creator' incorporates: + // Constant: '/Constant' + // Constant: '/Motor Control Streaming' + // Constant: '/TYPESTATUS' + // Constant: '/length' + // MATLAB Function: '/format_can_id' + + can_encoder_format_can_id(1, CAN_ID_AMC, 3, &rtb_BusCreator_m.packet.ID); + rtb_BusCreator_m.available = rtu_status_tx->status; + rtb_BusCreator_m.length = 8U; + + // BusCreator: '/Bus Creator' incorporates: + // Constant: '/Constant' + // Constant: '/Motor Control Streaming' + // Constant: '/TYPE2FOC' + // Constant: '/length' + // MATLAB Function: '/format_can_id' + + can_encoder_format_can_id(1, CAN_ID_AMC, 0, &rtb_BusCreator_n.packet.ID); + rtb_BusCreator_n.available = rtu_status_tx->foc; + rtb_BusCreator_n.length = 8U; + + // BusCreator: '/Bus Creator' incorporates: + // BusCreator: '/Bus Creator6' + // Constant: '/ID_raw' + // Constant: '/available' + // Constant: '/length' + + rtb_BusCreator_hs.available = false; + rtb_BusCreator_hs.length = 0U; + rtb_BusCreator_hs.packet.ID = 0U; + for (i = 0; i < 8; i++) { + rtb_BusCreator_hs.packet.PAYLOAD[i] = 0U; + } + + // End of BusCreator: '/Bus Creator' + + // Concatenate: '/Vector Concatenate' + rtb_BusCreator_n_0[0] = rtb_BusCreator_m; + rtb_BusCreator_n_0[1] = rtb_BusCreator_n; + rtb_BusCreator_n_0[2] = rtb_BusCreator_hs; + rtb_BusCreator_n_0[3] = rtb_BusCreator_hs; + for (i = 0; i < CAN_MAX_NUM_PACKETS; i++) { + rty_pck_tx->packets[i] = rtb_BusCreator_n_0[i]; + } + + // End of Concatenate: '/Vector Concatenate' + // End of Outputs for SubSystem: '/CAN_Encoder' +} + +// Model initialize function +void can_encoder_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_can_encoder_T *const can_encoder_M = &(can_encoder_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(can_encoder_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.h index 2e6b7147b3..c47b9a5a5c 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder.h @@ -1,112 +1,112 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_encoder.h -// -// Code generated for Simulink model 'can_encoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:59 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_can_encoder_h_ -#define RTW_HEADER_can_encoder_h_ -#include "rtwtypes.h" -#include "can_encoder_types.h" -#ifndef can_encoder_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_can_encoder_T { - const char_T **errorStatus; -}; - -#endif //can_encoder_MDLREF_HIDE_CHILD_ - -#ifndef can_encoder_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_can_encoder_T { - RT_MODEL_can_encoder_T rtm; -}; - -#endif //can_encoder_MDLREF_HIDE_CHILD_ - -// -// Exported Global Parameters -// -// Note: Exported global parameters are tunable parameters with an exported -// global storage class designation. Code generation will declare the memory for -// these parameters and exports their symbols. -// - -extern real32_T CAN_ANGLE_DEG2ICUB; // Variable: CAN_ANGLE_DEG2ICUB - // Referenced by: - // '/Gain1' - // '/Gain3' - // 2^16/360 - -extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC - // Referenced by: - // '/Constant' - // '/Constant' - // 4 bits defining the ID of the AMC_BLDC board. - -extern void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const - BUS_STATUS_TX *rtu_status_tx, const ConfigurationParameters - *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx); - -// Model reference registration function -extern void can_encoder_initialize(const char_T **rt_errorStatus); - -#ifndef can_encoder_MDLREF_HIDE_CHILD_ - -extern void can_encoder_format_can_id(uint8_T rtu_class, uint8_T rtu_can_id_amc, - uint8_T rtu_dst_typ, uint16_T *rty_pkt_id); - -#endif //can_encoder_MDLREF_HIDE_CHILD_ - -#ifndef can_encoder_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_can_encoder_T can_encoder_MdlrefDW; - -#endif //can_encoder_MDLREF_HIDE_CHILD_ - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'can_encoder' -// '' : 'can_encoder/CAN_Encoder' -// '' : 'can_encoder/CAN_Encoder/FOC' -// '' : 'can_encoder/CAN_Encoder/NotUsed' -// '' : 'can_encoder/CAN_Encoder/STATUS' -// '' : 'can_encoder/CAN_Encoder/FOC/format_can_id' -// '' : 'can_encoder/CAN_Encoder/FOC/format_foc_pck' -// '' : 'can_encoder/CAN_Encoder/FOC/format_can_id/format_can_id' -// '' : 'can_encoder/CAN_Encoder/STATUS/format_can_id' -// '' : 'can_encoder/CAN_Encoder/STATUS/format_status_pck' -// '' : 'can_encoder/CAN_Encoder/STATUS/format_can_id/format_can_id' - -#endif // RTW_HEADER_can_encoder_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_encoder.h +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:46 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_encoder_h_ +#define RTW_HEADER_can_encoder_h_ +#include "rtwtypes.h" +#include "can_encoder_types.h" +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_can_encoder_T { + const char_T **errorStatus; +}; + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_can_encoder_T { + RT_MODEL_can_encoder_T rtm; +}; + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +// +// Exported Global Parameters +// +// Note: Exported global parameters are tunable parameters with an exported +// global storage class designation. Code generation will declare the memory for +// these parameters and exports their symbols. +// + +extern real32_T CAN_ANGLE_DEG2ICUB; // Variable: CAN_ANGLE_DEG2ICUB + // Referenced by: + // '/Gain1' + // '/Gain3' + // 2^16/360 + +extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC + // Referenced by: + // '/Constant' + // '/Constant' + // 4 bits defining the ID of the AMC_BLDC board. + +extern void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const + BUS_STATUS_TX *rtu_status_tx, const ConfigurationParameters + *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx); + +// Model reference registration function +extern void can_encoder_initialize(const char_T **rt_errorStatus); + +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +extern void can_encoder_format_can_id(uint8_T rtu_class, uint8_T rtu_can_id_amc, + uint8_T rtu_dst_typ, uint16_T *rty_pkt_id); + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_can_encoder_T can_encoder_MdlrefDW; + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'can_encoder' +// '' : 'can_encoder/CAN_Encoder' +// '' : 'can_encoder/CAN_Encoder/FOC' +// '' : 'can_encoder/CAN_Encoder/NotUsed' +// '' : 'can_encoder/CAN_Encoder/STATUS' +// '' : 'can_encoder/CAN_Encoder/FOC/format_can_id' +// '' : 'can_encoder/CAN_Encoder/FOC/format_foc_pck' +// '' : 'can_encoder/CAN_Encoder/FOC/format_can_id/format_can_id' +// '' : 'can_encoder/CAN_Encoder/STATUS/format_can_id' +// '' : 'can_encoder/CAN_Encoder/STATUS/format_status_pck' +// '' : 'can_encoder/CAN_Encoder/STATUS/format_can_id/format_can_id' + +#endif // RTW_HEADER_can_encoder_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_private.h index f9cf84421e..7cd6e539d2 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_private.h @@ -1,46 +1,46 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_encoder_private.h -// -// Code generated for Simulink model 'can_encoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:59 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_can_encoder_private_h_ -#define RTW_HEADER_can_encoder_private_h_ -#include "rtwtypes.h" -#include "can_encoder_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif -#endif // RTW_HEADER_can_encoder_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_encoder_private.h +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:46 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_encoder_private_h_ +#define RTW_HEADER_can_encoder_private_h_ +#include "rtwtypes.h" +#include "can_encoder_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif +#endif // RTW_HEADER_can_encoder_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_types.h index 48f5160353..6bd0dac5b2 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/can-encoder/can_encoder_types.h @@ -1,331 +1,331 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: can_encoder_types.h -// -// Code generated for Simulink model 'can_encoder'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:59 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_can_encoder_types_h_ -#define RTW_HEADER_can_encoder_types_h_ -#include "rtwtypes.h" -#include "can_encoder_types.h" - -// Includes for objects with custom storage classes -#include "rtw_defines.h" - -// -// Registered constraints for dimension variants - -// Constraint 'CAN_MAX_NUM_PACKETS == 4' registered by: -// '/Vector Concatenate' - -#if CAN_MAX_NUM_PACKETS != 4 -# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be equal to '4'" -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ - -// Fields of a FOC message. -struct BUS_MSG_FOC -{ - // Current feedback in A. - real32_T current; - - // Position feedback in deg. - real32_T position; - - // Velocity feedback in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ -#define DEFINED_TYPEDEF_FOR_MCControlModes_ - -typedef enum { - MCControlModes_Idle = 0, // Default value - MCControlModes_OpenLoop = 80, - MCControlModes_SpeedVoltage = 10, - MCControlModes_SpeedCurrent = 11, - MCControlModes_Current = 6, - MCControlModes_NotConfigured = 176, - MCControlModes_HWFault = 160 -} MCControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ - -struct BUS_FLAGS_TX -{ - boolean_T dirty; - boolean_T stuck; - boolean_T index_broken; - boolean_T phase_broken; - real32_T not_calibrated; - boolean_T ExternalFaultAsserted; - boolean_T UnderVoltageFailure; - boolean_T OverVoltageFailure; - boolean_T OverCurrentFailure; - boolean_T DHESInvalidValue; - boolean_T AS5045CSumError; - boolean_T DHESInvalidSequence; - boolean_T CANInvalidProtocol; - boolean_T CAN_BufferOverRun; - boolean_T SetpointExpired; - boolean_T CAN_TXIsPasv; - boolean_T CAN_RXIsPasv; - boolean_T CAN_IsWarnTX; - boolean_T CAN_IsWarnRX; - boolean_T OverHeating; - boolean_T ADCCalFailure; - boolean_T I2TFailure; - boolean_T EMUROMFault; - boolean_T EMUROMCRCFault; - boolean_T EncoderFault; - boolean_T FirmwareSPITimingError; - boolean_T AS5045CalcError; - boolean_T FirmwarePWMFatalError; - boolean_T CAN_TXWasPasv; - boolean_T CAN_RXWasPasv; - boolean_T CAN_RTRFlagActive; - boolean_T CAN_WasWarn; - boolean_T CAN_DLCError; - boolean_T SiliconRevisionFault; - boolean_T PositionLimitUpper; - boolean_T PositionLimitLower; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ - -struct BUS_MSG_STATUS -{ - MCControlModes control_mode; - real32_T pwm_fbk; - BUS_FLAGS_TX flags; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ - -// Aggregate of all CAN transmitted messages. -struct BUS_MESSAGES_TX -{ - BUS_MSG_FOC foc; - BUS_MSG_STATUS status; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ - -// Aggregate of all events specifying types of transmitted messages. -struct BUS_STATUS_TX -{ - boolean_T foc; - boolean_T status; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ - -// Fields of a transmitted CAN packet. -struct BUS_CAN_PACKET -{ - // ID of the CAN packet. - uint16_T ID; - - // PAYLOAD of the CAN packet. - uint8_T PAYLOAD[8]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_ - -struct BUS_CAN -{ - // If true, the packet is available to be processed. - boolean_T available; - uint8_T length; - BUS_CAN_PACKET packet; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ - -struct BUS_CAN_MULTIPLE -{ - BUS_CAN packets[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_can_encoder_T RT_MODEL_can_encoder_T; - -#endif // RTW_HEADER_can_encoder_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_encoder_types.h +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:46 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_encoder_types_h_ +#define RTW_HEADER_can_encoder_types_h_ +#include "rtwtypes.h" +#include "can_encoder_types.h" + +// Includes for objects with custom storage classes +#include "rtw_defines.h" + +// +// Registered constraints for dimension variants + +// Constraint 'CAN_MAX_NUM_PACKETS == 4' registered by: +// '/Vector Concatenate' + +#if CAN_MAX_NUM_PACKETS != 4 +# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be equal to '4'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ + +// Fields of a FOC message. +struct BUS_MSG_FOC +{ + // Current feedback in A. + real32_T current; + + // Position feedback in deg. + real32_T position; + + // Velocity feedback in deg/s. + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ +#define DEFINED_TYPEDEF_FOR_MCControlModes_ + +typedef enum { + MCControlModes_Idle = 0, // Default value + MCControlModes_OpenLoop = 80, + MCControlModes_SpeedVoltage = 10, + MCControlModes_SpeedCurrent = 11, + MCControlModes_Current = 6, + MCControlModes_NotConfigured = 176, + MCControlModes_HWFault = 160 +} MCControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ + +struct BUS_FLAGS_TX +{ + boolean_T dirty; + boolean_T stuck; + boolean_T index_broken; + boolean_T phase_broken; + real32_T not_calibrated; + boolean_T ExternalFaultAsserted; + boolean_T UnderVoltageFailure; + boolean_T OverVoltageFailure; + boolean_T OverCurrentFailure; + boolean_T DHESInvalidValue; + boolean_T AS5045CSumError; + boolean_T DHESInvalidSequence; + boolean_T CANInvalidProtocol; + boolean_T CAN_BufferOverRun; + boolean_T SetpointExpired; + boolean_T CAN_TXIsPasv; + boolean_T CAN_RXIsPasv; + boolean_T CAN_IsWarnTX; + boolean_T CAN_IsWarnRX; + boolean_T OverHeating; + boolean_T ADCCalFailure; + boolean_T I2TFailure; + boolean_T EMUROMFault; + boolean_T EMUROMCRCFault; + boolean_T EncoderFault; + boolean_T FirmwareSPITimingError; + boolean_T AS5045CalcError; + boolean_T FirmwarePWMFatalError; + boolean_T CAN_TXWasPasv; + boolean_T CAN_RXWasPasv; + boolean_T CAN_RTRFlagActive; + boolean_T CAN_WasWarn; + boolean_T CAN_DLCError; + boolean_T SiliconRevisionFault; + boolean_T PositionLimitUpper; + boolean_T PositionLimitLower; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ + +struct BUS_MSG_STATUS +{ + MCControlModes control_mode; + real32_T pwm_fbk; + BUS_FLAGS_TX flags; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ + +// Aggregate of all CAN transmitted messages. +struct BUS_MESSAGES_TX +{ + BUS_MSG_FOC foc; + BUS_MSG_STATUS status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ + +// Aggregate of all events specifying types of transmitted messages. +struct BUS_STATUS_TX +{ + boolean_T foc; + boolean_T status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ + +// Fields of a transmitted CAN packet. +struct BUS_CAN_PACKET +{ + // ID of the CAN packet. + uint16_T ID; + + // PAYLOAD of the CAN packet. + uint8_T PAYLOAD[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ + +struct BUS_CAN +{ + // If true, the packet is available to be processed. + boolean_T available; + uint8_T length; + BUS_CAN_PACKET packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ + +struct BUS_CAN_MULTIPLE +{ + BUS_CAN packets[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_can_encoder_T RT_MODEL_can_encoder_T; + +#endif // RTW_HEADER_can_encoder_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.cpp index 2886e95b95..c630c0ad87 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.cpp @@ -1,616 +1,616 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: FOCInnerLoop.cpp -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:08 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "control_foc_types.h" -#include "FOCInnerLoop.h" -#include "control_foc.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#include "arm_math.h" -#include -#include "control_foc_private.h" -#include "rtwtypes.h" -#include "zero_crossing_types.h" - -// System initialize for atomic system: '/FOC inner loop' -void FOCInnerLoop_Init(void) -{ - // InitializeConditions for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - control_foc_DW.DelayInput1_DSTATE = ControlModes_Idle; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - control_foc_DW.Integrator_PrevResetState = 2; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - control_foc_DW.Integrator_PrevResetState_k = 2; -} - -// Output and update for atomic system: '/FOC inner loop' -void FOCInnerLoop(const Flags *rtu_Flags, const ConfigurationParameters - *rtu_ConfigurationParameters, const SensorsData *rtu_Sensors, - const EstimatedData *rtu_Estimates, const Targets *rtu_Targets, - const ControlOuterOutputs *rtu_OuterOutputs, ControlOutputs - *rty_FOCOutputs) -{ - real32_T rtb_IaIbIc0[2]; - real32_T rtb_Add; - real32_T rtb_DProdOut; - real32_T rtb_FilterDifferentiatorTF; - real32_T rtb_FilterDifferentiatorTF_f; - real32_T rtb_PProdOut_k; - real32_T rtb_Product; - real32_T rtb_SinCos_o1; - real32_T rtb_SinCos_o2; - real32_T rtb_Unary_Minus; - real32_T rtb_algDD_o1_p; - real32_T rtb_algDD_o2_n; - real32_T rtb_sum_beta; - int8_T tmp; - int8_T tmp_0; - boolean_T rtb_FixPtRelationalOperator; - - // Sum: '/Add' incorporates: - // Product: '/Product1' - // Product: '/Product2' - - rtb_Add = rtu_OuterOutputs->motorcurrent.current * - rtu_ConfigurationParameters->motorconfig.Rphase + - rtu_Estimates->jointvelocities.velocity * - rtu_ConfigurationParameters->motorconfig.Kbemf; - - // MinMax: '/Min' - if ((rtu_ConfigurationParameters->motorconfig.Vcc <= - rtu_ConfigurationParameters->motorconfig.Vmax) || rtIsNaNF - (rtu_ConfigurationParameters->motorconfig.Vmax)) { - rtb_Unary_Minus = rtu_ConfigurationParameters->motorconfig.Vcc; - } else { - rtb_Unary_Minus = rtu_ConfigurationParameters->motorconfig.Vmax; - } - - // Product: '/Product' incorporates: - // Gain: '/Gain4' - // MinMax: '/Min' - - rtb_Product = 0.5F * rtb_Unary_Minus * control_foc_ConstB.Sum5; - - // Gain: '/Ia+Ib+Ic=0' - for (int32_T i = 0; i < 2; i++) { - rtb_IaIbIc0[i] = 0.0F; - rtb_IaIbIc0[i] += rtCP_IaIbIc0_Gain[i] * rtu_Sensors->motorsensors.Iabc[0]; - rtb_IaIbIc0[i] += rtCP_IaIbIc0_Gain[i + 2] * rtu_Sensors->motorsensors.Iabc - [1]; - rtb_IaIbIc0[i] += rtCP_IaIbIc0_Gain[i + 4] * rtu_Sensors->motorsensors.Iabc - [2]; - } - - // End of Gain: '/Ia+Ib+Ic=0' - - // Outputs for Atomic SubSystem: '/Clarke Transform' - // AlgorithmDescriptorDelegate generated from: '/a16' - arm_clarke_f32(rtb_IaIbIc0[0], rtb_IaIbIc0[1], &rtb_algDD_o1_p, - &rtb_algDD_o2_n); - - // End of Outputs for SubSystem: '/Clarke Transform' - - // Gain: '/deg2rad' - rtb_Unary_Minus = 0.0174532924F * rtu_Sensors->motorsensors.angle; - - // Trigonometry: '/SinCos' - rtb_SinCos_o1 = std::sin(rtb_Unary_Minus); - rtb_SinCos_o2 = std::cos(rtb_Unary_Minus); - - // Outputs for Atomic SubSystem: '/Park Transform' - // Switch: '/Switch' incorporates: - // Product: '/acos' - // Product: '/asin' - // Product: '/bcos' - // Product: '/bsin' - // Sum: '/sum_Ds' - // Sum: '/sum_Qs' - // UnaryMinus: '/Unary_Minus' - - rtb_IaIbIc0[0] = -(rtb_algDD_o2_n * rtb_SinCos_o2 - rtb_algDD_o1_p * - rtb_SinCos_o1); - rtb_IaIbIc0[1] = rtb_algDD_o1_p * rtb_SinCos_o2 + rtb_algDD_o2_n * - rtb_SinCos_o1; - - // Sum: '/Sum' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - - rtb_Unary_Minus = rtu_OuterOutputs->motorcurrent.current - rtb_IaIbIc0[1]; - - // End of Outputs for SubSystem: '/Park Transform' - - // Product: '/PProd Out' - rtb_algDD_o1_p = rtb_Unary_Minus * rtu_ConfigurationParameters->CurLoopPID.P; - - // SampleTimeMath: '/Tsamp' incorporates: - // SampleTimeMath: '/Tsamp' - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - - rtb_FilterDifferentiatorTF_f = rtu_ConfigurationParameters->CurLoopPID.N * - 1.82857148E-5F; - - // Math: '/Reciprocal' incorporates: - // Constant: '/Filter Den Constant' - // Math: '/Reciprocal' - // SampleTimeMath: '/Tsamp' - // Sum: '/SumDen' - // - // About '/Reciprocal': - // Operator: reciprocal - // - // About '/Reciprocal': - // Operator: reciprocal - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - - rtb_sum_beta = 1.0F / (rtb_FilterDifferentiatorTF_f + 1.0F); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator = (rtu_Flags->control_mode != - control_foc_DW.DelayInput1_DSTATE); - - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // Constant: '/Filter Den Constant' - // Math: '/Reciprocal' - // Product: '/DProd Out' - // Product: '/Divide' - // SampleTimeMath: '/Tsamp' - // Sum: '/SumNum' - // - // About '/Reciprocal': - // Operator: reciprocal - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - - if (rtb_FixPtRelationalOperator && - (control_foc_PrevZCX.FilterDifferentiatorTF_Reset_ZC != POS_ZCSIG)) { - control_foc_DW.FilterDifferentiatorTF_states = 0.0F; - } - - control_foc_PrevZCX.FilterDifferentiatorTF_Reset_ZC = - rtb_FixPtRelationalOperator; - control_foc_DW.FilterDifferentiatorTF_tmp = rtb_Unary_Minus * - rtu_ConfigurationParameters->CurLoopPID.D - (rtb_FilterDifferentiatorTF_f - - 1.0F) * rtb_sum_beta * control_foc_DW.FilterDifferentiatorTF_states; - - // Product: '/NProd Out' incorporates: - // DiscreteTransferFcn: '/Filter Differentiator TF' - // Math: '/Reciprocal' - // Product: '/DenCoefOut' - // - // About '/Reciprocal': - // Operator: reciprocal - - rtb_algDD_o2_n = (control_foc_DW.FilterDifferentiatorTF_tmp - - control_foc_DW.FilterDifferentiatorTF_states) * rtb_sum_beta - * rtu_ConfigurationParameters->CurLoopPID.N; - - // Sum: '/SumI1' incorporates: - // Product: '/IProd Out' - // Sum: '/Sum Fdbk' - // Sum: '/SumI3' - // UnitDelay: '/Unit Delay' - - rtb_FilterDifferentiatorTF = (control_foc_DW.UnitDelay_DSTATE - - ((rtb_algDD_o1_p + control_foc_DW.Integrator_DSTATE) + rtb_algDD_o2_n)) + - rtb_Unary_Minus * rtu_ConfigurationParameters->CurLoopPID.I; - - // DiscreteIntegrator: '/Integrator' - if (rtb_FixPtRelationalOperator && (control_foc_DW.Integrator_PrevResetState <= - 0)) { - control_foc_DW.Integrator_DSTATE = 0.0F; - } - - // DiscreteIntegrator: '/Integrator' - rtb_DProdOut = 1.82857148E-5F * rtb_FilterDifferentiatorTF + - control_foc_DW.Integrator_DSTATE; - - // Switch: '/Switch1' incorporates: - // Gain: '/Gain6' - // Product: '/Divide2' - // Sum: '/Sum' - // Sum: '/Sum2' - // Sum: '/Sum6' - - if (rtu_OuterOutputs->cur_en) { - rtb_algDD_o2_n = ((rtb_algDD_o1_p + rtb_DProdOut) + rtb_algDD_o2_n) + - rtb_Add; - } else { - rtb_algDD_o2_n = rtu_Targets->motorvoltage.voltage * rtb_Product * 0.01F + - rtu_OuterOutputs->current_limiter; - } - - // End of Switch: '/Switch1' - - // Switch: '/Switch2' incorporates: - // Gain: '/Gain2' - // RelationalOperator: '/LowerRelop1' - // RelationalOperator: '/UpperRelop' - // Switch: '/Switch' - - if (rtb_algDD_o2_n > rtb_Product) { - rtb_algDD_o1_p = rtb_Product; - } else if (rtb_algDD_o2_n < -rtb_Product) { - // Switch: '/Switch' incorporates: - // Gain: '/Gain2' - - rtb_algDD_o1_p = -rtb_Product; - } else { - rtb_algDD_o1_p = rtb_algDD_o2_n; - } - - // End of Switch: '/Switch2' - - // Gain: '/Gain3' incorporates: - // Product: '/Divide1' - - rtb_algDD_o2_n = rtb_algDD_o1_p / rtb_Product * 100.0F; - - // Outputs for Atomic SubSystem: '/Park Transform' - // Product: '/PProd Out' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - // Gain: '/Gain' - - rtb_PProdOut_k = -rtb_IaIbIc0[0] * rtu_ConfigurationParameters->CurLoopPID.P; - - // End of Outputs for SubSystem: '/Park Transform' - - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - // Constant: '/Filter Den Constant' - // Gain: '/Gain' - // Product: '/DProd Out' - // Product: '/Divide' - // Sum: '/SumNum' - - if (rtb_FixPtRelationalOperator && - (control_foc_PrevZCX.FilterDifferentiatorTF_Reset__o != POS_ZCSIG)) { - control_foc_DW.FilterDifferentiatorTF_states_k = 0.0F; - } - - control_foc_PrevZCX.FilterDifferentiatorTF_Reset__o = - rtb_FixPtRelationalOperator; - - // Outputs for Atomic SubSystem: '/Park Transform' - control_foc_DW.FilterDifferentiatorTF_tmp_c = -rtb_IaIbIc0[0] * - rtu_ConfigurationParameters->CurLoopPID.D - (rtb_FilterDifferentiatorTF_f - - 1.0F) * rtb_sum_beta * control_foc_DW.FilterDifferentiatorTF_states_k; - - // End of Outputs for SubSystem: '/Park Transform' - - // Product: '/NProd Out' incorporates: - // DiscreteTransferFcn: '/Filter Differentiator TF' - // Product: '/DenCoefOut' - - rtb_sum_beta = (control_foc_DW.FilterDifferentiatorTF_tmp_c - - control_foc_DW.FilterDifferentiatorTF_states_k) * rtb_sum_beta - * rtu_ConfigurationParameters->CurLoopPID.N; - - // Sum: '/Sum Fdbk' - rtb_FilterDifferentiatorTF_f = (rtb_PProdOut_k + - control_foc_DW.Integrator_DSTATE_o) + rtb_sum_beta; - - // Switch: '/Switch' incorporates: - // Gain: '/Gain2' - // RelationalOperator: '/u_GTE_up' - // RelationalOperator: '/u_GT_lo' - // Switch: '/Switch1' - - if (rtb_FilterDifferentiatorTF_f >= rtb_Product) { - rtb_Unary_Minus = rtb_Product; - } else if (rtb_FilterDifferentiatorTF_f > -rtb_Product) { - // Switch: '/Switch1' - rtb_Unary_Minus = rtb_FilterDifferentiatorTF_f; - } else { - rtb_Unary_Minus = -rtb_Product; - } - - // Sum: '/Diff' incorporates: - // Switch: '/Switch' - - rtb_FilterDifferentiatorTF_f -= rtb_Unary_Minus; - - // Outputs for Atomic SubSystem: '/Park Transform' - // Product: '/IProd Out' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - // Gain: '/Gain' - - rtb_Unary_Minus = -rtb_IaIbIc0[0] * rtu_ConfigurationParameters->CurLoopPID.I; - - // End of Outputs for SubSystem: '/Park Transform' - - // Switch: '/Switch1' incorporates: - // Constant: '/Clamping_zero' - // Constant: '/Constant' - // Constant: '/Constant2' - // RelationalOperator: '/fix for DT propagation issue' - - if (rtb_FilterDifferentiatorTF_f > 0.0F) { - tmp = 1; - } else { - tmp = -1; - } - - // Switch: '/Switch2' incorporates: - // Constant: '/Clamping_zero' - // Constant: '/Constant3' - // Constant: '/Constant4' - // RelationalOperator: '/fix for DT propagation issue1' - - if (rtb_Unary_Minus > 0.0F) { - tmp_0 = 1; - } else { - tmp_0 = -1; - } - - // Switch: '/Switch' incorporates: - // Constant: '/Clamping_zero' - // Constant: '/Constant1' - // Logic: '/AND3' - // RelationalOperator: '/Equal1' - // RelationalOperator: '/Relational Operator' - // Switch: '/Switch1' - // Switch: '/Switch2' - - if ((rtb_FilterDifferentiatorTF_f != 0.0F) && (tmp == tmp_0)) { - rtb_FilterDifferentiatorTF_f = 0.0F; - } else { - rtb_FilterDifferentiatorTF_f = rtb_Unary_Minus; - } - - // End of Switch: '/Switch' - - // DiscreteIntegrator: '/Integrator' - if (rtb_FixPtRelationalOperator && (control_foc_DW.Integrator_PrevResetState_k - <= 0)) { - control_foc_DW.Integrator_DSTATE_o = 0.0F; - } - - // DiscreteIntegrator: '/Integrator' - rtb_Unary_Minus = 1.82857148E-5F * rtb_FilterDifferentiatorTF_f + - control_foc_DW.Integrator_DSTATE_o; - - // Sum: '/Sum' - rtb_PProdOut_k = (rtb_PProdOut_k + rtb_Unary_Minus) + rtb_sum_beta; - - // Switch: '/Switch2' incorporates: - // RelationalOperator: '/LowerRelop1' - - if (!(rtb_PProdOut_k > rtb_Product)) { - // Switch: '/Switch' incorporates: - // Gain: '/Gain2' - // RelationalOperator: '/UpperRelop' - - if (rtb_PProdOut_k < -rtb_Product) { - rtb_Product = -rtb_Product; - } else { - rtb_Product = rtb_PProdOut_k; - } - - // End of Switch: '/Switch' - } - - // End of Switch: '/Switch2' - - // Outputs for Atomic SubSystem: '/Inverse Park Transform' - // Switch: '/Switch' incorporates: - // Product: '/dsin' - // Product: '/qcos' - // Sum: '/sum_beta' - - rtb_IaIbIc0[0] = rtb_algDD_o1_p * rtb_SinCos_o2 + rtb_Product * rtb_SinCos_o1; - - // End of Outputs for SubSystem: '/Inverse Park Transform' - - // Switch: '/Switch2' incorporates: - // Constant: '/Constant1' - - if (rtu_OuterOutputs->out_en) { - // Outputs for Atomic SubSystem: '/Inverse Park Transform' - // Gain: '/sqrt3_by_two' incorporates: - // Product: '/dcos' - // Product: '/qsin' - // Sum: '/sum_alpha' - // UnaryMinus: '/Unary_Minus' - - rtb_SinCos_o2 = -(rtb_Product * rtb_SinCos_o2 - rtb_algDD_o1_p * - rtb_SinCos_o1) * 0.866025388F; - - // Gain: '/one_by_two' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - - rtb_Product = 0.5F * rtb_IaIbIc0[0]; - - // End of Outputs for SubSystem: '/Inverse Park Transform' - - // Sum: '/add_c' - rtb_SinCos_o1 = (0.0F - rtb_Product) - rtb_SinCos_o2; - - // Sum: '/add_b' - rtb_Product = rtb_SinCos_o2 - rtb_Product; - - // Outputs for Atomic SubSystem: '/Inverse Park Transform' - // MinMax: '/Min1' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - - if ((rtb_IaIbIc0[0] <= rtb_Product) || rtIsNaNF(rtb_Product)) { - rtb_SinCos_o2 = rtb_IaIbIc0[0]; - } else { - rtb_SinCos_o2 = rtb_Product; - } - - // End of Outputs for SubSystem: '/Inverse Park Transform' - if ((!(rtb_SinCos_o2 <= rtb_SinCos_o1)) && (!rtIsNaNF(rtb_SinCos_o1))) { - rtb_SinCos_o2 = rtb_SinCos_o1; - } - - // Outputs for Atomic SubSystem: '/Inverse Park Transform' - // Sum: '/Sum1' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - // Constant: '/Constant2' - // Gain: '/Gain1' - // MinMax: '/Min1' - // Product: '/Divide' - // Sum: '/Sum4' - - rtb_PProdOut_k = (rtb_IaIbIc0[0] - rtb_SinCos_o2) / - rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; - - // End of Outputs for SubSystem: '/Inverse Park Transform' - - // Saturate: '/Saturation' - if (rtb_PProdOut_k > 100.0F) { - rty_FOCOutputs->Vabc[0] = 100.0F; - } else if (rtb_PProdOut_k < 0.0F) { - rty_FOCOutputs->Vabc[0] = 0.0F; - } else { - rty_FOCOutputs->Vabc[0] = rtb_PProdOut_k; - } - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant2' - // Gain: '/Gain1' - // MinMax: '/Min1' - // Product: '/Divide' - // Sum: '/Sum4' - - rtb_PProdOut_k = (rtb_Product - rtb_SinCos_o2) / - rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; - - // Saturate: '/Saturation' - if (rtb_PProdOut_k > 100.0F) { - rty_FOCOutputs->Vabc[1] = 100.0F; - } else if (rtb_PProdOut_k < 0.0F) { - rty_FOCOutputs->Vabc[1] = 0.0F; - } else { - rty_FOCOutputs->Vabc[1] = rtb_PProdOut_k; - } - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant2' - // Gain: '/Gain1' - // MinMax: '/Min1' - // Product: '/Divide' - // Sum: '/Sum4' - - rtb_PProdOut_k = (rtb_SinCos_o1 - rtb_SinCos_o2) / - rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; - - // Saturate: '/Saturation' - if (rtb_PProdOut_k > 100.0F) { - rty_FOCOutputs->Vabc[2] = 100.0F; - } else if (rtb_PProdOut_k < 0.0F) { - rty_FOCOutputs->Vabc[2] = 0.0F; - } else { - rty_FOCOutputs->Vabc[2] = rtb_PProdOut_k; - } - } else { - rty_FOCOutputs->Vabc[0] = 0.0F; - rty_FOCOutputs->Vabc[1] = 0.0F; - rty_FOCOutputs->Vabc[2] = 0.0F; - } - - // End of Switch: '/Switch2' - - // Outputs for Atomic SubSystem: '/Park Transform' - // BusCreator: '/Bus Creator1' incorporates: - // AlgorithmDescriptorDelegate generated from: '/a16' - - rty_FOCOutputs->Iq_fbk.current = rtb_IaIbIc0[1]; - - // End of Outputs for SubSystem: '/Park Transform' - - // Saturate: '/Saturation1' - if (rtb_algDD_o2_n > 100.0F) { - // BusCreator: '/Bus Creator' - rty_FOCOutputs->Vq = 100.0F; - } else if (rtb_algDD_o2_n < -100.0F) { - // BusCreator: '/Bus Creator' - rty_FOCOutputs->Vq = -100.0F; - } else { - // BusCreator: '/Bus Creator' - rty_FOCOutputs->Vq = rtb_algDD_o2_n; - } - - // End of Saturate: '/Saturation1' - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - control_foc_DW.DelayInput1_DSTATE = rtu_Flags->control_mode; - - // Update for DiscreteTransferFcn: '/Filter Differentiator TF' - control_foc_DW.FilterDifferentiatorTF_states = - control_foc_DW.FilterDifferentiatorTF_tmp; - - // Update for UnitDelay: '/Unit Delay' incorporates: - // Sum: '/Sum3' - - control_foc_DW.UnitDelay_DSTATE = rtb_algDD_o1_p - rtb_Add; - - // Update for DiscreteIntegrator: '/Integrator' - control_foc_DW.Integrator_DSTATE = 1.82857148E-5F * rtb_FilterDifferentiatorTF - + rtb_DProdOut; - control_foc_DW.Integrator_PrevResetState = static_cast - (rtb_FixPtRelationalOperator); - - // Update for DiscreteTransferFcn: '/Filter Differentiator TF' - control_foc_DW.FilterDifferentiatorTF_states_k = - control_foc_DW.FilterDifferentiatorTF_tmp_c; - - // Update for DiscreteIntegrator: '/Integrator' - control_foc_DW.Integrator_DSTATE_o = 1.82857148E-5F * - rtb_FilterDifferentiatorTF_f + rtb_Unary_Minus; - control_foc_DW.Integrator_PrevResetState_k = static_cast - (rtb_FixPtRelationalOperator); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: FOCInnerLoop.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "control_foc_types.h" +#include "FOCInnerLoop.h" +#include "control_foc.h" + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include "arm_math.h" +#include +#include "control_foc_private.h" +#include "rtwtypes.h" +#include "zero_crossing_types.h" + +// System initialize for atomic system: '/FOC inner loop' +void FOCInnerLoop_Init(void) +{ + // InitializeConditions for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + control_foc_DW.DelayInput1_DSTATE = ControlModes_Idle; + + // InitializeConditions for DiscreteIntegrator: '/Integrator' + control_foc_DW.Integrator_PrevResetState = 2; + + // InitializeConditions for DiscreteIntegrator: '/Integrator' + control_foc_DW.Integrator_PrevResetState_k = 2; +} + +// Output and update for atomic system: '/FOC inner loop' +void FOCInnerLoop(const Flags *rtu_Flags, const ConfigurationParameters + *rtu_ConfigurationParameters, const SensorsData *rtu_Sensors, + const EstimatedData *rtu_Estimates, const Targets *rtu_Targets, + const ControlOuterOutputs *rtu_OuterOutputs, ControlOutputs + *rty_FOCOutputs) +{ + real32_T rtb_IaIbIc0[2]; + real32_T rtb_Add; + real32_T rtb_DProdOut; + real32_T rtb_FilterDifferentiatorTF; + real32_T rtb_FilterDifferentiatorTF_f; + real32_T rtb_PProdOut_k; + real32_T rtb_Product; + real32_T rtb_SinCos_o1; + real32_T rtb_SinCos_o2; + real32_T rtb_Unary_Minus; + real32_T rtb_algDD_o1_p; + real32_T rtb_algDD_o2_n; + real32_T rtb_sum_beta; + int8_T tmp; + int8_T tmp_0; + boolean_T rtb_FixPtRelationalOperator; + + // Sum: '/Add' incorporates: + // Product: '/Product1' + // Product: '/Product2' + + rtb_Add = rtu_OuterOutputs->motorcurrent.current * + rtu_ConfigurationParameters->motorconfig.Rphase + + rtu_Estimates->jointvelocities.velocity * + rtu_ConfigurationParameters->motorconfig.Kbemf; + + // MinMax: '/Min' + if ((rtu_ConfigurationParameters->motorconfig.Vcc <= + rtu_ConfigurationParameters->motorconfig.Vmax) || rtIsNaNF + (rtu_ConfigurationParameters->motorconfig.Vmax)) { + rtb_Unary_Minus = rtu_ConfigurationParameters->motorconfig.Vcc; + } else { + rtb_Unary_Minus = rtu_ConfigurationParameters->motorconfig.Vmax; + } + + // Product: '/Product' incorporates: + // Gain: '/Gain4' + // MinMax: '/Min' + + rtb_Product = 0.5F * rtb_Unary_Minus * control_foc_ConstB.Sum5; + + // Gain: '/Ia+Ib+Ic=0' + for (int32_T i = 0; i < 2; i++) { + rtb_IaIbIc0[i] = 0.0F; + rtb_IaIbIc0[i] += rtCP_IaIbIc0_Gain[i] * rtu_Sensors->motorsensors.Iabc[0]; + rtb_IaIbIc0[i] += rtCP_IaIbIc0_Gain[i + 2] * rtu_Sensors->motorsensors.Iabc + [1]; + rtb_IaIbIc0[i] += rtCP_IaIbIc0_Gain[i + 4] * rtu_Sensors->motorsensors.Iabc + [2]; + } + + // End of Gain: '/Ia+Ib+Ic=0' + + // Outputs for Atomic SubSystem: '/Clarke Transform' + // AlgorithmDescriptorDelegate generated from: '/a16' + arm_clarke_f32(rtb_IaIbIc0[0], rtb_IaIbIc0[1], &rtb_algDD_o1_p, + &rtb_algDD_o2_n); + + // End of Outputs for SubSystem: '/Clarke Transform' + + // Gain: '/deg2rad' + rtb_Unary_Minus = 0.0174532924F * rtu_Sensors->motorsensors.angle; + + // Trigonometry: '/SinCos' + rtb_SinCos_o1 = std::sin(rtb_Unary_Minus); + rtb_SinCos_o2 = std::cos(rtb_Unary_Minus); + + // Outputs for Atomic SubSystem: '/Park Transform' + // Switch: '/Switch' incorporates: + // Product: '/acos' + // Product: '/asin' + // Product: '/bcos' + // Product: '/bsin' + // Sum: '/sum_Ds' + // Sum: '/sum_Qs' + // UnaryMinus: '/Unary_Minus' + + rtb_IaIbIc0[0] = -(rtb_algDD_o2_n * rtb_SinCos_o2 - rtb_algDD_o1_p * + rtb_SinCos_o1); + rtb_IaIbIc0[1] = rtb_algDD_o1_p * rtb_SinCos_o2 + rtb_algDD_o2_n * + rtb_SinCos_o1; + + // Sum: '/Sum' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + + rtb_Unary_Minus = rtu_OuterOutputs->motorcurrent.current - rtb_IaIbIc0[1]; + + // End of Outputs for SubSystem: '/Park Transform' + + // Product: '/PProd Out' + rtb_algDD_o1_p = rtb_Unary_Minus * rtu_ConfigurationParameters->CurLoopPID.P; + + // SampleTimeMath: '/Tsamp' incorporates: + // SampleTimeMath: '/Tsamp' + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_FilterDifferentiatorTF_f = rtu_ConfigurationParameters->CurLoopPID.N * + 1.82857148E-5F; + + // Math: '/Reciprocal' incorporates: + // Constant: '/Filter Den Constant' + // Math: '/Reciprocal' + // SampleTimeMath: '/Tsamp' + // Sum: '/SumDen' + // + // About '/Reciprocal': + // Operator: reciprocal + // + // About '/Reciprocal': + // Operator: reciprocal + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_sum_beta = 1.0F / (rtb_FilterDifferentiatorTF_f + 1.0F); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator = (rtu_Flags->control_mode != + control_foc_DW.DelayInput1_DSTATE); + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // Constant: '/Filter Den Constant' + // Math: '/Reciprocal' + // Product: '/DProd Out' + // Product: '/Divide' + // SampleTimeMath: '/Tsamp' + // Sum: '/SumNum' + // + // About '/Reciprocal': + // Operator: reciprocal + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + if (rtb_FixPtRelationalOperator && + (control_foc_PrevZCX.FilterDifferentiatorTF_Reset_ZC != POS_ZCSIG)) { + control_foc_DW.FilterDifferentiatorTF_states = 0.0F; + } + + control_foc_PrevZCX.FilterDifferentiatorTF_Reset_ZC = + rtb_FixPtRelationalOperator; + control_foc_DW.FilterDifferentiatorTF_tmp = rtb_Unary_Minus * + rtu_ConfigurationParameters->CurLoopPID.D - (rtb_FilterDifferentiatorTF_f - + 1.0F) * rtb_sum_beta * control_foc_DW.FilterDifferentiatorTF_states; + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Math: '/Reciprocal' + // Product: '/DenCoefOut' + // + // About '/Reciprocal': + // Operator: reciprocal + + rtb_algDD_o2_n = (control_foc_DW.FilterDifferentiatorTF_tmp - + control_foc_DW.FilterDifferentiatorTF_states) * rtb_sum_beta + * rtu_ConfigurationParameters->CurLoopPID.N; + + // Sum: '/SumI1' incorporates: + // Product: '/IProd Out' + // Sum: '/Sum Fdbk' + // Sum: '/SumI3' + // UnitDelay: '/Unit Delay' + + rtb_FilterDifferentiatorTF = (control_foc_DW.UnitDelay_DSTATE - + ((rtb_algDD_o1_p + control_foc_DW.Integrator_DSTATE) + rtb_algDD_o2_n)) + + rtb_Unary_Minus * rtu_ConfigurationParameters->CurLoopPID.I; + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && (control_foc_DW.Integrator_PrevResetState <= + 0)) { + control_foc_DW.Integrator_DSTATE = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_DProdOut = 1.82857148E-5F * rtb_FilterDifferentiatorTF + + control_foc_DW.Integrator_DSTATE; + + // Switch: '/Switch1' incorporates: + // Gain: '/Gain6' + // Product: '/Divide2' + // Sum: '/Sum' + // Sum: '/Sum2' + // Sum: '/Sum6' + + if (rtu_OuterOutputs->cur_en) { + rtb_algDD_o2_n = ((rtb_algDD_o1_p + rtb_DProdOut) + rtb_algDD_o2_n) + + rtb_Add; + } else { + rtb_algDD_o2_n = rtu_Targets->motorvoltage.voltage * rtb_Product * 0.01F + + rtu_OuterOutputs->current_limiter; + } + + // End of Switch: '/Switch1' + + // Switch: '/Switch2' incorporates: + // Gain: '/Gain2' + // RelationalOperator: '/LowerRelop1' + // RelationalOperator: '/UpperRelop' + // Switch: '/Switch' + + if (rtb_algDD_o2_n > rtb_Product) { + rtb_algDD_o1_p = rtb_Product; + } else if (rtb_algDD_o2_n < -rtb_Product) { + // Switch: '/Switch' incorporates: + // Gain: '/Gain2' + + rtb_algDD_o1_p = -rtb_Product; + } else { + rtb_algDD_o1_p = rtb_algDD_o2_n; + } + + // End of Switch: '/Switch2' + + // Gain: '/Gain3' incorporates: + // Product: '/Divide1' + + rtb_algDD_o2_n = rtb_algDD_o1_p / rtb_Product * 100.0F; + + // Outputs for Atomic SubSystem: '/Park Transform' + // Product: '/PProd Out' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + // Gain: '/Gain' + + rtb_PProdOut_k = -rtb_IaIbIc0[0] * rtu_ConfigurationParameters->CurLoopPID.P; + + // End of Outputs for SubSystem: '/Park Transform' + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + // Constant: '/Filter Den Constant' + // Gain: '/Gain' + // Product: '/DProd Out' + // Product: '/Divide' + // Sum: '/SumNum' + + if (rtb_FixPtRelationalOperator && + (control_foc_PrevZCX.FilterDifferentiatorTF_Reset__o != POS_ZCSIG)) { + control_foc_DW.FilterDifferentiatorTF_states_k = 0.0F; + } + + control_foc_PrevZCX.FilterDifferentiatorTF_Reset__o = + rtb_FixPtRelationalOperator; + + // Outputs for Atomic SubSystem: '/Park Transform' + control_foc_DW.FilterDifferentiatorTF_tmp_c = -rtb_IaIbIc0[0] * + rtu_ConfigurationParameters->CurLoopPID.D - (rtb_FilterDifferentiatorTF_f - + 1.0F) * rtb_sum_beta * control_foc_DW.FilterDifferentiatorTF_states_k; + + // End of Outputs for SubSystem: '/Park Transform' + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DenCoefOut' + + rtb_sum_beta = (control_foc_DW.FilterDifferentiatorTF_tmp_c - + control_foc_DW.FilterDifferentiatorTF_states_k) * rtb_sum_beta + * rtu_ConfigurationParameters->CurLoopPID.N; + + // Sum: '/Sum Fdbk' + rtb_FilterDifferentiatorTF_f = (rtb_PProdOut_k + + control_foc_DW.Integrator_DSTATE_o) + rtb_sum_beta; + + // Switch: '/Switch' incorporates: + // Gain: '/Gain2' + // RelationalOperator: '/u_GTE_up' + // RelationalOperator: '/u_GT_lo' + // Switch: '/Switch1' + + if (rtb_FilterDifferentiatorTF_f >= rtb_Product) { + rtb_Unary_Minus = rtb_Product; + } else if (rtb_FilterDifferentiatorTF_f > -rtb_Product) { + // Switch: '/Switch1' + rtb_Unary_Minus = rtb_FilterDifferentiatorTF_f; + } else { + rtb_Unary_Minus = -rtb_Product; + } + + // Sum: '/Diff' incorporates: + // Switch: '/Switch' + + rtb_FilterDifferentiatorTF_f -= rtb_Unary_Minus; + + // Outputs for Atomic SubSystem: '/Park Transform' + // Product: '/IProd Out' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + // Gain: '/Gain' + + rtb_Unary_Minus = -rtb_IaIbIc0[0] * rtu_ConfigurationParameters->CurLoopPID.I; + + // End of Outputs for SubSystem: '/Park Transform' + + // Switch: '/Switch1' incorporates: + // Constant: '/Clamping_zero' + // Constant: '/Constant' + // Constant: '/Constant2' + // RelationalOperator: '/fix for DT propagation issue' + + if (rtb_FilterDifferentiatorTF_f > 0.0F) { + tmp = 1; + } else { + tmp = -1; + } + + // Switch: '/Switch2' incorporates: + // Constant: '/Clamping_zero' + // Constant: '/Constant3' + // Constant: '/Constant4' + // RelationalOperator: '/fix for DT propagation issue1' + + if (rtb_Unary_Minus > 0.0F) { + tmp_0 = 1; + } else { + tmp_0 = -1; + } + + // Switch: '/Switch' incorporates: + // Constant: '/Clamping_zero' + // Constant: '/Constant1' + // Logic: '/AND3' + // RelationalOperator: '/Equal1' + // RelationalOperator: '/Relational Operator' + // Switch: '/Switch1' + // Switch: '/Switch2' + + if ((rtb_FilterDifferentiatorTF_f != 0.0F) && (tmp == tmp_0)) { + rtb_FilterDifferentiatorTF_f = 0.0F; + } else { + rtb_FilterDifferentiatorTF_f = rtb_Unary_Minus; + } + + // End of Switch: '/Switch' + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && (control_foc_DW.Integrator_PrevResetState_k + <= 0)) { + control_foc_DW.Integrator_DSTATE_o = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_Unary_Minus = 1.82857148E-5F * rtb_FilterDifferentiatorTF_f + + control_foc_DW.Integrator_DSTATE_o; + + // Sum: '/Sum' + rtb_PProdOut_k = (rtb_PProdOut_k + rtb_Unary_Minus) + rtb_sum_beta; + + // Switch: '/Switch2' incorporates: + // RelationalOperator: '/LowerRelop1' + + if (!(rtb_PProdOut_k > rtb_Product)) { + // Switch: '/Switch' incorporates: + // Gain: '/Gain2' + // RelationalOperator: '/UpperRelop' + + if (rtb_PProdOut_k < -rtb_Product) { + rtb_Product = -rtb_Product; + } else { + rtb_Product = rtb_PProdOut_k; + } + + // End of Switch: '/Switch' + } + + // End of Switch: '/Switch2' + + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // Switch: '/Switch' incorporates: + // Product: '/dsin' + // Product: '/qcos' + // Sum: '/sum_beta' + + rtb_IaIbIc0[0] = rtb_algDD_o1_p * rtb_SinCos_o2 + rtb_Product * rtb_SinCos_o1; + + // End of Outputs for SubSystem: '/Inverse Park Transform' + + // Switch: '/Switch2' incorporates: + // Constant: '/Constant1' + + if (rtu_OuterOutputs->out_en) { + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // Gain: '/sqrt3_by_two' incorporates: + // Product: '/dcos' + // Product: '/qsin' + // Sum: '/sum_alpha' + // UnaryMinus: '/Unary_Minus' + + rtb_SinCos_o2 = -(rtb_Product * rtb_SinCos_o2 - rtb_algDD_o1_p * + rtb_SinCos_o1) * 0.866025388F; + + // Gain: '/one_by_two' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + + rtb_Product = 0.5F * rtb_IaIbIc0[0]; + + // End of Outputs for SubSystem: '/Inverse Park Transform' + + // Sum: '/add_c' + rtb_SinCos_o1 = (0.0F - rtb_Product) - rtb_SinCos_o2; + + // Sum: '/add_b' + rtb_Product = rtb_SinCos_o2 - rtb_Product; + + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // MinMax: '/Min1' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + + if ((rtb_IaIbIc0[0] <= rtb_Product) || rtIsNaNF(rtb_Product)) { + rtb_SinCos_o2 = rtb_IaIbIc0[0]; + } else { + rtb_SinCos_o2 = rtb_Product; + } + + // End of Outputs for SubSystem: '/Inverse Park Transform' + if ((!(rtb_SinCos_o2 <= rtb_SinCos_o1)) && (!rtIsNaNF(rtb_SinCos_o1))) { + rtb_SinCos_o2 = rtb_SinCos_o1; + } + + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // Sum: '/Sum1' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + // Constant: '/Constant2' + // Gain: '/Gain1' + // MinMax: '/Min1' + // Product: '/Divide' + // Sum: '/Sum4' + + rtb_PProdOut_k = (rtb_IaIbIc0[0] - rtb_SinCos_o2) / + rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; + + // End of Outputs for SubSystem: '/Inverse Park Transform' + + // Saturate: '/Saturation' + if (rtb_PProdOut_k > 100.0F) { + rty_FOCOutputs->Vabc[0] = 100.0F; + } else if (rtb_PProdOut_k < 0.0F) { + rty_FOCOutputs->Vabc[0] = 0.0F; + } else { + rty_FOCOutputs->Vabc[0] = rtb_PProdOut_k; + } + + // Sum: '/Sum1' incorporates: + // Constant: '/Constant2' + // Gain: '/Gain1' + // MinMax: '/Min1' + // Product: '/Divide' + // Sum: '/Sum4' + + rtb_PProdOut_k = (rtb_Product - rtb_SinCos_o2) / + rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; + + // Saturate: '/Saturation' + if (rtb_PProdOut_k > 100.0F) { + rty_FOCOutputs->Vabc[1] = 100.0F; + } else if (rtb_PProdOut_k < 0.0F) { + rty_FOCOutputs->Vabc[1] = 0.0F; + } else { + rty_FOCOutputs->Vabc[1] = rtb_PProdOut_k; + } + + // Sum: '/Sum1' incorporates: + // Constant: '/Constant2' + // Gain: '/Gain1' + // MinMax: '/Min1' + // Product: '/Divide' + // Sum: '/Sum4' + + rtb_PProdOut_k = (rtb_SinCos_o1 - rtb_SinCos_o2) / + rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; + + // Saturate: '/Saturation' + if (rtb_PProdOut_k > 100.0F) { + rty_FOCOutputs->Vabc[2] = 100.0F; + } else if (rtb_PProdOut_k < 0.0F) { + rty_FOCOutputs->Vabc[2] = 0.0F; + } else { + rty_FOCOutputs->Vabc[2] = rtb_PProdOut_k; + } + } else { + rty_FOCOutputs->Vabc[0] = 0.0F; + rty_FOCOutputs->Vabc[1] = 0.0F; + rty_FOCOutputs->Vabc[2] = 0.0F; + } + + // End of Switch: '/Switch2' + + // Outputs for Atomic SubSystem: '/Park Transform' + // BusCreator: '/Bus Creator1' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + + rty_FOCOutputs->Iq_fbk.current = rtb_IaIbIc0[1]; + + // End of Outputs for SubSystem: '/Park Transform' + + // Saturate: '/Saturation1' + if (rtb_algDD_o2_n > 100.0F) { + // BusCreator: '/Bus Creator' + rty_FOCOutputs->Vq = 100.0F; + } else if (rtb_algDD_o2_n < -100.0F) { + // BusCreator: '/Bus Creator' + rty_FOCOutputs->Vq = -100.0F; + } else { + // BusCreator: '/Bus Creator' + rty_FOCOutputs->Vq = rtb_algDD_o2_n; + } + + // End of Saturate: '/Saturation1' + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + control_foc_DW.DelayInput1_DSTATE = rtu_Flags->control_mode; + + // Update for DiscreteTransferFcn: '/Filter Differentiator TF' + control_foc_DW.FilterDifferentiatorTF_states = + control_foc_DW.FilterDifferentiatorTF_tmp; + + // Update for UnitDelay: '/Unit Delay' incorporates: + // Sum: '/Sum3' + + control_foc_DW.UnitDelay_DSTATE = rtb_algDD_o1_p - rtb_Add; + + // Update for DiscreteIntegrator: '/Integrator' + control_foc_DW.Integrator_DSTATE = 1.82857148E-5F * rtb_FilterDifferentiatorTF + + rtb_DProdOut; + control_foc_DW.Integrator_PrevResetState = static_cast + (rtb_FixPtRelationalOperator); + + // Update for DiscreteTransferFcn: '/Filter Differentiator TF' + control_foc_DW.FilterDifferentiatorTF_states_k = + control_foc_DW.FilterDifferentiatorTF_tmp_c; + + // Update for DiscreteIntegrator: '/Integrator' + control_foc_DW.Integrator_DSTATE_o = 1.82857148E-5F * + rtb_FilterDifferentiatorTF_f + rtb_Unary_Minus; + control_foc_DW.Integrator_PrevResetState_k = static_cast + (rtb_FixPtRelationalOperator); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.h index f3aafa7167..519f7934d0 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/FOCInnerLoop.h @@ -1,38 +1,38 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: FOCInnerLoop.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:08 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_FOCInnerLoop_h_ -#define RTW_HEADER_FOCInnerLoop_h_ -#include "rtwtypes.h" -#include "control_foc_types.h" -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -extern void FOCInnerLoop_Init(void); -extern void FOCInnerLoop(const Flags *rtu_Flags, const ConfigurationParameters - *rtu_ConfigurationParameters, const SensorsData *rtu_Sensors, const - EstimatedData *rtu_Estimates, const Targets *rtu_Targets, const - ControlOuterOutputs *rtu_OuterOutputs, ControlOutputs *rty_FOCOutputs); - -#endif //control_foc_MDLREF_HIDE_CHILD_ -#endif // RTW_HEADER_FOCInnerLoop_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: FOCInnerLoop.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_FOCInnerLoop_h_ +#define RTW_HEADER_FOCInnerLoop_h_ +#include "rtwtypes.h" +#include "control_foc_types.h" +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +extern void FOCInnerLoop_Init(void); +extern void FOCInnerLoop(const Flags *rtu_Flags, const ConfigurationParameters + *rtu_ConfigurationParameters, const SensorsData *rtu_Sensors, const + EstimatedData *rtu_Estimates, const Targets *rtu_Targets, const + ControlOuterOutputs *rtu_OuterOutputs, ControlOutputs *rty_FOCOutputs); + +#endif //control_foc_MDLREF_HIDE_CHILD_ +#endif // RTW_HEADER_FOCInnerLoop_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.cpp index 8ea249c044..82f477e338 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.cpp @@ -1,81 +1,81 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_foc.cpp -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:08 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "control_foc.h" -#include "control_foc_types.h" -#include "FOCInnerLoop.h" -#include "control_foc_private.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -MdlrefDW_control_foc_T control_foc_MdlrefDW; - -// Block states (default storage) -DW_control_foc_f_T control_foc_DW; - -// Previous zero-crossings (trigger) states -ZCE_control_foc_T control_foc_PrevZCX; - -// System initialize for referenced model: 'control_foc' -void control_foc_Init(void) -{ - // SystemInitialize for Atomic SubSystem: '/FOC inner loop' - FOCInnerLoop_Init(); - - // End of SystemInitialize for SubSystem: '/FOC inner loop' -} - -// Output and update for referenced model: 'control_foc' -void control_foc(const SensorsData *rtu_Sensors, const FOCSlowInputs - *rtu_FOCSlowInputs, ControlOutputs *rty_FOCOutputs) -{ - // Outputs for Atomic SubSystem: '/FOC inner loop' - FOCInnerLoop(&rtu_FOCSlowInputs->flags, - &rtu_FOCSlowInputs->configurationparameters, rtu_Sensors, - &rtu_FOCSlowInputs->estimateddata, &rtu_FOCSlowInputs->targets, - &rtu_FOCSlowInputs->controlouteroutputs, rty_FOCOutputs); - - // End of Outputs for SubSystem: '/FOC inner loop' -} - -// Model initialize function -void control_foc_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_control_foc_T *const control_foc_M = &(control_foc_MdlrefDW.rtm); - - // Registration code - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // initialize error status - rtmSetErrorStatusPointer(control_foc_M, rt_errorStatus); - control_foc_PrevZCX.FilterDifferentiatorTF_Reset_ZC = POS_ZCSIG; - control_foc_PrevZCX.FilterDifferentiatorTF_Reset__o = POS_ZCSIG; -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_foc.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "control_foc.h" +#include "control_foc_types.h" +#include "FOCInnerLoop.h" +#include "control_foc_private.h" + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +MdlrefDW_control_foc_T control_foc_MdlrefDW; + +// Block states (default storage) +DW_control_foc_f_T control_foc_DW; + +// Previous zero-crossings (trigger) states +ZCE_control_foc_T control_foc_PrevZCX; + +// System initialize for referenced model: 'control_foc' +void control_foc_Init(void) +{ + // SystemInitialize for Atomic SubSystem: '/FOC inner loop' + FOCInnerLoop_Init(); + + // End of SystemInitialize for SubSystem: '/FOC inner loop' +} + +// Output and update for referenced model: 'control_foc' +void control_foc(const SensorsData *rtu_Sensors, const FOCSlowInputs + *rtu_FOCSlowInputs, ControlOutputs *rty_FOCOutputs) +{ + // Outputs for Atomic SubSystem: '/FOC inner loop' + FOCInnerLoop(&rtu_FOCSlowInputs->flags, + &rtu_FOCSlowInputs->configurationparameters, rtu_Sensors, + &rtu_FOCSlowInputs->estimateddata, &rtu_FOCSlowInputs->targets, + &rtu_FOCSlowInputs->controlouteroutputs, rty_FOCOutputs); + + // End of Outputs for SubSystem: '/FOC inner loop' +} + +// Model initialize function +void control_foc_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_control_foc_T *const control_foc_M = &(control_foc_MdlrefDW.rtm); + + // Registration code + + // initialize non-finites + rt_InitInfAndNaN(sizeof(real_T)); + + // initialize error status + rtmSetErrorStatusPointer(control_foc_M, rt_errorStatus); + control_foc_PrevZCX.FilterDifferentiatorTF_Reset_ZC = POS_ZCSIG; + control_foc_PrevZCX.FilterDifferentiatorTF_Reset__o = POS_ZCSIG; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.h index c0706ed326..89ba123206 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc.h @@ -1,280 +1,286 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_foc.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:08 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_control_foc_h_ -#define RTW_HEADER_control_foc_h_ -#include "rtwtypes.h" -#include "control_foc_types.h" -#include "rtGetInf.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#include "zero_crossing_types.h" - -// Block states (default storage) for model 'control_foc' -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -struct DW_control_foc_f_T { - real32_T FilterDifferentiatorTF_states;// '/Filter Differentiator TF' - real32_T UnitDelay_DSTATE; // '/Unit Delay' - real32_T Integrator_DSTATE; // '/Integrator' - real32_T FilterDifferentiatorTF_states_k;// '/Filter Differentiator TF' - real32_T Integrator_DSTATE_o; // '/Integrator' - ControlModes DelayInput1_DSTATE; // '/Delay Input1' - real32_T FilterDifferentiatorTF_tmp; // '/Filter Differentiator TF' - real32_T FilterDifferentiatorTF_tmp_c;// '/Filter Differentiator TF' - int8_T Integrator_PrevResetState; // '/Integrator' - int8_T Integrator_PrevResetState_k; // '/Integrator' -}; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -// Zero-crossing (trigger) state for model 'control_foc' -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -struct ZCV_control_foc_g_T { - real_T FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' - real_T FilterDifferentiatorTF_Reset__c;// '/Filter Differentiator TF' -}; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -// Zero-crossing (trigger) state for model 'control_foc' -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -struct ZCE_control_foc_T { - ZCSigState FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' - ZCSigState FilterDifferentiatorTF_Reset__o;// '/Filter Differentiator TF' -}; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -// Invariant block signals for model 'control_foc' -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -struct ConstB_control_foc_h_T { - real32_T Gain5; // '/Gain5' - real32_T Sum5; // '/Sum5' -}; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_control_foc_T { - const char_T **errorStatus; -}; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_control_foc_T { - RT_MODEL_control_foc_T rtm; -}; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -extern void control_foc_Init(void); -extern void control_foc(const SensorsData *rtu_Sensors, const FOCSlowInputs - *rtu_FOCSlowInputs, ControlOutputs *rty_FOCOutputs); - -// Model reference registration function -extern void control_foc_initialize(const char_T **rt_errorStatus); - -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_control_foc_T control_foc_MdlrefDW; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -#ifndef control_foc_MDLREF_HIDE_CHILD_ - -// Block states (default storage) -extern DW_control_foc_f_T control_foc_DW; - -// Previous zero-crossings (trigger) states -extern ZCE_control_foc_T control_foc_PrevZCX; - -#endif //control_foc_MDLREF_HIDE_CHILD_ - -//- -// These blocks were eliminated from the model due to optimizations: -// -// Block '/Data Type Duplicate' : Unused code path elimination -// Block '/Direct Lookup Table (n-D)1' : 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 Duplicate' : Unused code path elimination -// Block '/Data Type Duplicate1' : Unused code path elimination -// Block '/Data Type Duplicate' : Unused code path elimination -// Block '/Data Type Duplicate1' : Unused code path elimination -// Block '/Data Type Duplicate' : Unused code path elimination -// Block '/Data Type Propagation' : Unused code path elimination -// Block '/Passthrough for tuning' : Eliminate redundant data type conversion -// Block '/Passthrough for tuning' : Eliminate redundant data type conversion -// Block '/Kt' : Eliminated nontunable gain of 1 -// Block '/Offset' : Unused code path elimination -// Block '/Offset' : Unused code path elimination - - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'control_foc' -// '' : 'control_foc/FOC inner loop' -// '' : 'control_foc/FOC inner loop/Clarke Transform' -// '' : 'control_foc/FOC inner loop/Detect Change' -// '' : 'control_foc/FOC inner loop/Id PID control' -// '' : 'control_foc/FOC inner loop/Inverse Clarke Transform' -// '' : 'control_foc/FOC inner loop/Inverse Park Transform' -// '' : 'control_foc/FOC inner loop/Iq PID control' -// '' : 'control_foc/FOC inner loop/Park Transform' -// '' : 'control_foc/FOC inner loop/Saturation Dynamic' -// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup' -// '' : 'control_foc/FOC inner loop/Id PID control/D Gain' -// '' : 'control_foc/FOC inner loop/Id PID control/Filter' -// '' : 'control_foc/FOC inner loop/Id PID control/Filter ICs' -// '' : 'control_foc/FOC inner loop/Id PID control/I Gain' -// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain' -// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain Fdbk' -// '' : 'control_foc/FOC inner loop/Id PID control/Integrator' -// '' : 'control_foc/FOC inner loop/Id PID control/Integrator ICs' -// '' : 'control_foc/FOC inner loop/Id PID control/N Copy' -// '' : 'control_foc/FOC inner loop/Id PID control/N Gain' -// '' : 'control_foc/FOC inner loop/Id PID control/P Copy' -// '' : 'control_foc/FOC inner loop/Id PID control/Parallel P Gain' -// '' : 'control_foc/FOC inner loop/Id PID control/Reset Signal' -// '' : 'control_foc/FOC inner loop/Id PID control/Saturation' -// '' : 'control_foc/FOC inner loop/Id PID control/Saturation Fdbk' -// '' : 'control_foc/FOC inner loop/Id PID control/Sum' -// '' : 'control_foc/FOC inner loop/Id PID control/Sum Fdbk' -// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode' -// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode Sum' -// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Integral' -// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Ngain' -// '' : 'control_foc/FOC inner loop/Id PID control/postSat Signal' -// '' : 'control_foc/FOC inner loop/Id PID control/preSat Signal' -// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel' -// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone' -// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone/External' -// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone/External/Dead Zone Dynamic' -// '' : 'control_foc/FOC inner loop/Id PID control/D Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter' -// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter/Tsamp' -// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' -// '' : 'control_foc/FOC inner loop/Id PID control/Filter ICs/Internal IC - Filter' -// '' : 'control_foc/FOC inner loop/Id PID control/I Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain/Passthrough' -// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain Fdbk/Passthrough' -// '' : 'control_foc/FOC inner loop/Id PID control/Integrator/Discrete' -// '' : 'control_foc/FOC inner loop/Id PID control/Integrator ICs/Internal IC' -// '' : 'control_foc/FOC inner loop/Id PID control/N Copy/External Parameters' -// '' : 'control_foc/FOC inner loop/Id PID control/N Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Id PID control/P Copy/Disabled' -// '' : 'control_foc/FOC inner loop/Id PID control/Parallel P Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Id PID control/Reset Signal/External Reset' -// '' : 'control_foc/FOC inner loop/Id PID control/Saturation/External' -// '' : 'control_foc/FOC inner loop/Id PID control/Saturation/External/Saturation Dynamic' -// '' : 'control_foc/FOC inner loop/Id PID control/Saturation Fdbk/Passthrough' -// '' : 'control_foc/FOC inner loop/Id PID control/Sum/Sum_PID' -// '' : 'control_foc/FOC inner loop/Id PID control/Sum Fdbk/Enabled' -// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode/Disabled' -// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode Sum/Passthrough' -// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Integral/Passthrough' -// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Ngain/Passthrough' -// '' : 'control_foc/FOC inner loop/Id PID control/postSat Signal/Feedback_Path' -// '' : 'control_foc/FOC inner loop/Id PID control/preSat Signal/Feedback_Path' -// '' : 'control_foc/FOC inner loop/Inverse Park Transform/Switch_Axis' -// '' : 'control_foc/FOC inner loop/Iq PID control/Anti-windup' -// '' : 'control_foc/FOC inner loop/Iq PID control/D Gain' -// '' : 'control_foc/FOC inner loop/Iq PID control/Filter' -// '' : 'control_foc/FOC inner loop/Iq PID control/Filter ICs' -// '' : 'control_foc/FOC inner loop/Iq PID control/I Gain' -// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain' -// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain Fdbk' -// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator' -// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator ICs' -// '' : 'control_foc/FOC inner loop/Iq PID control/N Copy' -// '' : 'control_foc/FOC inner loop/Iq PID control/N Gain' -// '' : 'control_foc/FOC inner loop/Iq PID control/P Copy' -// '' : 'control_foc/FOC inner loop/Iq PID control/Parallel P Gain' -// '' : 'control_foc/FOC inner loop/Iq PID control/Reset Signal' -// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation' -// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation Fdbk' -// '' : 'control_foc/FOC inner loop/Iq PID control/Sum' -// '' : 'control_foc/FOC inner loop/Iq PID control/Sum Fdbk' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode Sum' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Integral' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Ngain' -// '' : 'control_foc/FOC inner loop/Iq PID control/postSat Signal' -// '' : 'control_foc/FOC inner loop/Iq PID control/preSat Signal' -// '' : 'control_foc/FOC inner loop/Iq PID control/Anti-windup/Passthrough' -// '' : 'control_foc/FOC inner loop/Iq PID control/D Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter' -// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter/Tsamp' -// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' -// '' : 'control_foc/FOC inner loop/Iq PID control/Filter ICs/Internal IC - Filter' -// '' : 'control_foc/FOC inner loop/Iq PID control/I Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain/Passthrough' -// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain Fdbk/Passthrough' -// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator/Discrete' -// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator ICs/Internal IC' -// '' : 'control_foc/FOC inner loop/Iq PID control/N Copy/External Parameters' -// '' : 'control_foc/FOC inner loop/Iq PID control/N Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Iq PID control/P Copy/Disabled' -// '' : 'control_foc/FOC inner loop/Iq PID control/Parallel P Gain/External Parameters' -// '' : 'control_foc/FOC inner loop/Iq PID control/Reset Signal/External Reset' -// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation/Passthrough' -// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation Fdbk/Passthrough' -// '' : 'control_foc/FOC inner loop/Iq PID control/Sum/Sum_PID' -// '' : 'control_foc/FOC inner loop/Iq PID control/Sum Fdbk/Enabled' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode/Enabled' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode Sum/Tracking Mode' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Integral/Passthrough' -// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Ngain/Passthrough' -// '' : 'control_foc/FOC inner loop/Iq PID control/postSat Signal/Feedback_Path' -// '' : 'control_foc/FOC inner loop/Iq PID control/preSat Signal/Feedback_Path' -// '' : 'control_foc/FOC inner loop/Park Transform/Switch_Axis' - -#endif // RTW_HEADER_control_foc_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_foc.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_foc_h_ +#define RTW_HEADER_control_foc_h_ +#include "rtwtypes.h" +#include "control_foc_types.h" + +extern "C" +{ + +#include "rtGetInf.h" + +} + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include "zero_crossing_types.h" + +// Block states (default storage) for model 'control_foc' +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +struct DW_control_foc_f_T { + real32_T FilterDifferentiatorTF_states;// '/Filter Differentiator TF' + real32_T UnitDelay_DSTATE; // '/Unit Delay' + real32_T Integrator_DSTATE; // '/Integrator' + real32_T FilterDifferentiatorTF_states_k;// '/Filter Differentiator TF' + real32_T Integrator_DSTATE_o; // '/Integrator' + ControlModes DelayInput1_DSTATE; // '/Delay Input1' + real32_T FilterDifferentiatorTF_tmp; // '/Filter Differentiator TF' + real32_T FilterDifferentiatorTF_tmp_c;// '/Filter Differentiator TF' + int8_T Integrator_PrevResetState; // '/Integrator' + int8_T Integrator_PrevResetState_k; // '/Integrator' +}; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +// Zero-crossing (trigger) state for model 'control_foc' +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +struct ZCV_control_foc_g_T { + real_T FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' + real_T FilterDifferentiatorTF_Reset__c;// '/Filter Differentiator TF' +}; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +// Zero-crossing (trigger) state for model 'control_foc' +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +struct ZCE_control_foc_T { + ZCSigState FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' + ZCSigState FilterDifferentiatorTF_Reset__o;// '/Filter Differentiator TF' +}; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +// Invariant block signals for model 'control_foc' +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +struct ConstB_control_foc_h_T { + real32_T Gain5; // '/Gain5' + real32_T Sum5; // '/Sum5' +}; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_control_foc_T { + const char_T **errorStatus; +}; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_control_foc_T { + RT_MODEL_control_foc_T rtm; +}; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +extern void control_foc_Init(void); +extern void control_foc(const SensorsData *rtu_Sensors, const FOCSlowInputs + *rtu_FOCSlowInputs, ControlOutputs *rty_FOCOutputs); + +// Model reference registration function +extern void control_foc_initialize(const char_T **rt_errorStatus); + +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_control_foc_T control_foc_MdlrefDW; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +#ifndef control_foc_MDLREF_HIDE_CHILD_ + +// Block states (default storage) +extern DW_control_foc_f_T control_foc_DW; + +// Previous zero-crossings (trigger) states +extern ZCE_control_foc_T control_foc_PrevZCX; + +#endif //control_foc_MDLREF_HIDE_CHILD_ + +//- +// These blocks were eliminated from the model due to optimizations: +// +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Direct Lookup Table (n-D)1' : 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 Duplicate' : Unused code path elimination +// Block '/Data Type Duplicate1' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Duplicate1' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Propagation' : Unused code path elimination +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 +// Block '/Offset' : Unused code path elimination +// Block '/Offset' : Unused code path elimination + + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'control_foc' +// '' : 'control_foc/FOC inner loop' +// '' : 'control_foc/FOC inner loop/Clarke Transform' +// '' : 'control_foc/FOC inner loop/Detect Change' +// '' : 'control_foc/FOC inner loop/Id PID control' +// '' : 'control_foc/FOC inner loop/Inverse Clarke Transform' +// '' : 'control_foc/FOC inner loop/Inverse Park Transform' +// '' : 'control_foc/FOC inner loop/Iq PID control' +// '' : 'control_foc/FOC inner loop/Park Transform' +// '' : 'control_foc/FOC inner loop/Saturation Dynamic' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup' +// '' : 'control_foc/FOC inner loop/Id PID control/D Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter ICs' +// '' : 'control_foc/FOC inner loop/Id PID control/I Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain Fdbk' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator ICs' +// '' : 'control_foc/FOC inner loop/Id PID control/N Copy' +// '' : 'control_foc/FOC inner loop/Id PID control/N Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/P Copy' +// '' : 'control_foc/FOC inner loop/Id PID control/Parallel P Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Reset Signal' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation Fdbk' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum Fdbk' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode Sum' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Integral' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Ngain' +// '' : 'control_foc/FOC inner loop/Id PID control/postSat Signal' +// '' : 'control_foc/FOC inner loop/Id PID control/preSat Signal' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone/External' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone/External/Dead Zone Dynamic' +// '' : 'control_foc/FOC inner loop/Id PID control/D Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter ICs/Internal IC - Filter' +// '' : 'control_foc/FOC inner loop/Id PID control/I Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator/Discrete' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator ICs/Internal IC' +// '' : 'control_foc/FOC inner loop/Id PID control/N Copy/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/N Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/P Copy/Disabled' +// '' : 'control_foc/FOC inner loop/Id PID control/Parallel P Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/Reset Signal/External Reset' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation/External' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation/External/Saturation Dynamic' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum/Sum_PID' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum Fdbk/Enabled' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode/Disabled' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode Sum/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Integral/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Ngain/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/postSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Id PID control/preSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Inverse Park Transform/Switch_Axis' +// '' : 'control_foc/FOC inner loop/Iq PID control/Anti-windup' +// '' : 'control_foc/FOC inner loop/Iq PID control/D Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter ICs' +// '' : 'control_foc/FOC inner loop/Iq PID control/I Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain Fdbk' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator ICs' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Copy' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/P Copy' +// '' : 'control_foc/FOC inner loop/Iq PID control/Parallel P Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Reset Signal' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation Fdbk' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum Fdbk' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode Sum' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Integral' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Ngain' +// '' : 'control_foc/FOC inner loop/Iq PID control/postSat Signal' +// '' : 'control_foc/FOC inner loop/Iq PID control/preSat Signal' +// '' : 'control_foc/FOC inner loop/Iq PID control/Anti-windup/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/D Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter ICs/Internal IC - Filter' +// '' : 'control_foc/FOC inner loop/Iq PID control/I Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator/Discrete' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator ICs/Internal IC' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Copy/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/P Copy/Disabled' +// '' : 'control_foc/FOC inner loop/Iq PID control/Parallel P Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/Reset Signal/External Reset' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum/Sum_PID' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum Fdbk/Enabled' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode/Enabled' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode Sum/Tracking Mode' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Integral/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Ngain/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/postSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Iq PID control/preSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Park Transform/Switch_Axis' + +#endif // RTW_HEADER_control_foc_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_data.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_data.cpp index 4c1ded76b6..91bd5cb602 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_data.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_data.cpp @@ -1,33 +1,33 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_foc_data.cpp -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:08 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "control_foc_private.h" - -// Invariant block signals (default storage) -const ConstB_control_foc_h_T control_foc_ConstB = { - 0.0249999985F - , // '/Gain5' - 0.975F - // '/Sum5' -}; - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_foc_data.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "control_foc_private.h" + +// Invariant block signals (default storage) +const ConstB_control_foc_h_T control_foc_ConstB = { + 0.0249999985F + , // '/Gain5' + 0.975F + // '/Sum5' +}; + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_private.h index d27414eee8..b067d3c1df 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_private.h @@ -1,58 +1,58 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_foc_private.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:08 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_control_foc_private_h_ -#define RTW_HEADER_control_foc_private_h_ -#include "rtwtypes.h" -#include "zero_crossing_types.h" -#include "control_foc_types.h" -#include "control_foc.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -extern const real32_T rtCP_pooled_IgamRjjg0YgF[6]; - -#define rtCP_IaIbIc0_Gain rtCP_pooled_IgamRjjg0YgF // Computed Parameter: rtCP_IaIbIc0_Gain - // Referenced by: '/Ia+Ib+Ic=0' - - -// Invariant block signals (default storage) -extern const ConstB_control_foc_h_T control_foc_ConstB; - -#endif // RTW_HEADER_control_foc_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_foc_private.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_foc_private_h_ +#define RTW_HEADER_control_foc_private_h_ +#include "rtwtypes.h" +#include "zero_crossing_types.h" +#include "control_foc_types.h" +#include "control_foc.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +extern const real32_T rtCP_pooled_IgamRjjg0YgF[6]; + +#define rtCP_IaIbIc0_Gain rtCP_pooled_IgamRjjg0YgF // Computed Parameter: rtCP_IaIbIc0_Gain + // Referenced by: '/Ia+Ib+Ic=0' + + +// Invariant block signals (default storage) +extern const ConstB_control_foc_h_T control_foc_ConstB; + +#endif // RTW_HEADER_control_foc_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_types.h index 95dbfdcf70..19fe7862a4 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-foc/control_foc_types.h @@ -1,337 +1,337 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_foc_types.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:08 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_control_foc_types_h_ -#define RTW_HEADER_control_foc_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ -#define DEFINED_TYPEDEF_FOR_ControlModes_ - -typedef enum { - ControlModes_NotConfigured = 0, // Default value - ControlModes_Idle, - ControlModes_Position, - ControlModes_PositionDirect, - ControlModes_Current, - ControlModes_Velocity, - ControlModes_Voltage, - ControlModes_Torque, - ControlModes_HwFaultCM -} ControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ - -struct SensorsData -{ - // position encoders - JointPositions jointpositions; - MotorSensors motorsensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ - -struct Flags -{ - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocities - JointVelocities jointvelocities; - MotorCurrent Iq_filtered; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ - -struct ControlOuterOutputs -{ - boolean_T vel_en; - boolean_T cur_en; - boolean_T out_en; - MotorCurrent motorcurrent; - real32_T current_limiter; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_FOCSlowInputs_ -#define DEFINED_TYPEDEF_FOR_FOCSlowInputs_ - -struct FOCSlowInputs -{ - Flags flags; - ConfigurationParameters configurationparameters; - EstimatedData estimateddata; - Targets targets; - ControlOuterOutputs controlouteroutputs; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs -{ - // control effort (quadrature) - real32_T Vq; - - // control effort (3-phases) - real32_T Vabc[3]; - - // quadrature current - MotorCurrent Iq_fbk; -}; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_control_foc_T RT_MODEL_control_foc_T; - -#endif // RTW_HEADER_control_foc_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_foc_types.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_foc_types_h_ +#define RTW_HEADER_control_foc_types_h_ +#include "rtwtypes.h" +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_Torque, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ +#define DEFINED_TYPEDEF_FOR_JointPositions_ + +struct JointPositions +{ + // joint positions + real32_T position; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + // control mode + ControlModes control_mode; + boolean_T enable_sending_msg_status; + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ +#define DEFINED_TYPEDEF_FOR_JointVelocities_ + +struct JointVelocities +{ + // joint velocities + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ +#define DEFINED_TYPEDEF_FOR_MotorCurrent_ + +struct MotorCurrent +{ + // motor current + real32_T current; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocities + JointVelocities jointvelocities; + MotorCurrent Iq_filtered; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ +#define DEFINED_TYPEDEF_FOR_MotorVoltage_ + +struct MotorVoltage +{ + // motor voltage + real32_T voltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + JointPositions jointpositions; + JointVelocities jointvelocities; + MotorCurrent motorcurrent; + MotorVoltage motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ + +struct ControlOuterOutputs +{ + boolean_T vel_en; + boolean_T cur_en; + boolean_T out_en; + MotorCurrent motorcurrent; + real32_T current_limiter; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_FOCSlowInputs_ +#define DEFINED_TYPEDEF_FOR_FOCSlowInputs_ + +struct FOCSlowInputs +{ + Flags flags; + ConfigurationParameters configurationparameters; + EstimatedData estimateddata; + Targets targets; + ControlOuterOutputs controlouteroutputs; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOutputs_ + +struct ControlOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + MotorCurrent Iq_fbk; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_control_foc_T RT_MODEL_control_foc_T; + +#endif // RTW_HEADER_control_foc_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.cpp index f86c5507cb..934c41c586 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.cpp @@ -1,563 +1,563 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_outer.cpp -// -// Code generated for Simulink model 'control_outer'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:17 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "control_outer.h" -#include "control_outer_types.h" -#include -#include "rtwtypes.h" -#include "control_outer_private.h" -#include "zero_crossing_types.h" - -MdlrefDW_control_outer_T control_outer_MdlrefDW; - -// Block signals (default storage) -B_control_outer_c_T control_outer_B; - -// Block states (default storage) -DW_control_outer_f_T control_outer_DW; - -// Previous zero-crossings (trigger) states -ZCE_control_outer_T control_outer_PrevZCX; - -// System initialize for referenced model: 'control_outer' -void control_outer_Init(void) -{ - // InitializeConditions for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - control_outer_DW.DelayInput1_DSTATE = ControlModes_Idle; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - control_outer_DW.Integrator_PrevResetState = 2; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - control_outer_DW.Integrator_PrevResetState_n = 2; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - control_outer_DW.Integrator_PrevResetState_c = 2; -} - -// Enable for referenced model: 'control_outer' -void control_outer_Enable(void) -{ - // Enable for DiscreteIntegrator: '/Discrete-Time Integrator' - control_outer_DW.DiscreteTimeIntegrator_SYSTEM_E = 1U; -} - -// Disable for referenced model: 'control_outer' -void control_outer_Disable(void) -{ - // Disable for DiscreteIntegrator: '/Discrete-Time Integrator' - control_outer_DW.DiscreteTimeIntegrator_DSTATE = - control_outer_B.DiscreteTimeIntegrator; -} - -// Output and update for referenced model: 'control_outer' -void control_outer(const Flags *rtu_Flags, const ConfigurationParameters - *rtu_ConfigurationParameters, const Targets *rtu_Targets, - const SensorsData *rtu_Sensors, const EstimatedData - *rtu_Estimates, ControlOuterOutputs *rty_OuterOutputs) -{ - int32_T rowIdx; - int32_T tmp; - real32_T rtb_Abs; - real32_T rtb_DProdOut; - real32_T rtb_DProdOut_f; - real32_T rtb_DenCoefOut; - real32_T rtb_FilterDifferentiatorTF; - real32_T rtb_FilterDifferentiatorTF_n; - real32_T rtb_Gain1_h; - real32_T rtb_PProdOut; - real32_T rtb_PProdOut_o; - real32_T rtb_Product; - real32_T rtb_Switch2_f; - boolean_T rtb_Compare; - boolean_T rtb_Compare_m; - boolean_T rtb_FixPtRelationalOperator; - - // Abs: '/Abs' - rtb_Abs = std::abs(rtu_ConfigurationParameters->thresholds.jntVelMax); - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare_m = (rtu_Flags->control_mode != ControlModes_Current); - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rty_OuterOutputs->cur_en = (rtu_Flags->control_mode != ControlModes_Voltage); - - // Logic: '/NOR' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - - rty_OuterOutputs->out_en = ((rtu_Flags->control_mode != - ControlModes_NotConfigured) && (rtu_Flags->control_mode != ControlModes_Idle) - && (rtu_Flags->control_mode != ControlModes_HwFaultCM)); - - // Sum: '/Sum3' - rtb_DenCoefOut = rtu_Targets->jointpositions.position - - rtu_Sensors->jointpositions.position; - - // Product: '/PProd Out' - rtb_PProdOut = rtb_DenCoefOut * rtu_ConfigurationParameters->PosLoopPID.P; - - // SampleTimeMath: '/Tsamp' - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - - rtb_Product = rtu_ConfigurationParameters->PosLoopPID.N * 0.0005F; - - // Math: '/Reciprocal' incorporates: - // Constant: '/Filter Den Constant' - // Sum: '/SumDen' - // - // About '/Reciprocal': - // Operator: reciprocal - - rtb_Switch2_f = 1.0F / (rtb_Product + 1.0F); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator = (rtu_Flags->control_mode != - control_outer_DW.DelayInput1_DSTATE); - - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // Constant: '/Filter Den Constant' - // Product: '/DProd Out' - // Product: '/Divide' - // Sum: '/SumNum' - - if (rtb_FixPtRelationalOperator && - (control_outer_PrevZCX.FilterDifferentiatorTF_Reset_ZC != POS_ZCSIG)) { - control_outer_DW.FilterDifferentiatorTF_states = 0.0F; - } - - control_outer_PrevZCX.FilterDifferentiatorTF_Reset_ZC = - rtb_FixPtRelationalOperator; - control_outer_DW.FilterDifferentiatorTF_tmp = rtb_DenCoefOut * - rtu_ConfigurationParameters->PosLoopPID.D - (rtb_Product - 1.0F) * - rtb_Switch2_f * control_outer_DW.FilterDifferentiatorTF_states; - - // Product: '/NProd Out' incorporates: - // DiscreteTransferFcn: '/Filter Differentiator TF' - // Product: '/DenCoefOut' - - rtb_Switch2_f = (control_outer_DW.FilterDifferentiatorTF_tmp - - control_outer_DW.FilterDifferentiatorTF_states) * - rtb_Switch2_f * rtu_ConfigurationParameters->PosLoopPID.N; - - // Sum: '/SumI1' incorporates: - // Product: '/IProd Out' - // Sum: '/Sum Fdbk' - // Sum: '/SumI3' - // UnitDelay: '/Unit Delay' - - rtb_DProdOut = (control_outer_DW.UnitDelay_DSTATE - ((rtb_PProdOut + - control_outer_DW.Integrator_DSTATE) + rtb_Switch2_f)) + rtb_DenCoefOut * - rtu_ConfigurationParameters->PosLoopPID.I; - - // DiscreteIntegrator: '/Integrator' - if (rtb_FixPtRelationalOperator && (control_outer_DW.Integrator_PrevResetState - <= 0)) { - control_outer_DW.Integrator_DSTATE = 0.0F; - } - - // DiscreteIntegrator: '/Integrator' - rtb_FilterDifferentiatorTF = 0.0005F * rtb_DProdOut + - control_outer_DW.Integrator_DSTATE; - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare = (rtu_Flags->control_mode == ControlModes_Position); - - // Product: '/PProd Out' - rtb_PProdOut_o = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.P; - - // Product: '/IProd Out' - rtb_Product = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.I; - - // Product: '/DProd Out' - rtb_DProdOut_f = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.D; - - // SampleTimeMath: '/Tsamp' - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - - rtb_Gain1_h = rtu_ConfigurationParameters->DirLoopPID.N * 0.0005F; - - // Math: '/Reciprocal' incorporates: - // Constant: '/Filter Den Constant' - // Sum: '/SumDen' - // - // About '/Reciprocal': - // Operator: reciprocal - - rtb_DenCoefOut = 1.0F / (rtb_Gain1_h + 1.0F); - - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // Constant: '/Filter Den Constant' - // DiscreteTransferFcn: '/Filter Differentiator TF' - // Product: '/Divide' - // Sum: '/SumNum' - - if (rtb_FixPtRelationalOperator && - (control_outer_PrevZCX.FilterDifferentiatorTF_Reset__m != POS_ZCSIG)) { - control_outer_DW.FilterDifferentiatorTF_states_i = 0.0F; - } - - control_outer_PrevZCX.FilterDifferentiatorTF_Reset__m = - rtb_FixPtRelationalOperator; - control_outer_DW.FilterDifferentiatorTF_tmp_m = rtb_DProdOut_f - (rtb_Gain1_h - - 1.0F) * rtb_DenCoefOut * control_outer_DW.FilterDifferentiatorTF_states_i; - - // Product: '/NProd Out' incorporates: - // DiscreteTransferFcn: '/Filter Differentiator TF' - // Product: '/DenCoefOut' - - rtb_DenCoefOut = (control_outer_DW.FilterDifferentiatorTF_tmp_m - - control_outer_DW.FilterDifferentiatorTF_states_i) * - rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.N; - - // Sum: '/SumI1' incorporates: - // Sum: '/Sum Fdbk' - // Sum: '/SumI3' - // UnitDelay: '/Unit Delay' - - rtb_FilterDifferentiatorTF_n = (control_outer_DW.UnitDelay_DSTATE - - ((rtb_PProdOut_o + control_outer_DW.Integrator_DSTATE_i) + rtb_DenCoefOut)) - + rtb_Product; - - // DiscreteIntegrator: '/Integrator' - if (rtb_FixPtRelationalOperator && - (control_outer_DW.Integrator_PrevResetState_n <= 0)) { - control_outer_DW.Integrator_DSTATE_i = 0.0F; - } - - // DiscreteIntegrator: '/Integrator' - rtb_DProdOut_f = 0.0005F * rtb_FilterDifferentiatorTF_n + - control_outer_DW.Integrator_DSTATE_i; - - // Switch: '/Switch3' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Constant: '/Constant' - // Logic: '/OR' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - - if (rtb_Compare || (rtu_Flags->control_mode == ControlModes_PositionDirect) || - (rtu_Flags->control_mode == ControlModes_Velocity)) { - // Switch: '/Switch5' incorporates: - // Sum: '/Sum' - // Sum: '/Sum' - - if (rtb_Compare) { - rtb_DenCoefOut = (rtb_PProdOut + rtb_FilterDifferentiatorTF) + - rtb_Switch2_f; - } else { - rtb_DenCoefOut += rtb_PProdOut_o + rtb_DProdOut_f; - } - - // End of Switch: '/Switch5' - } else { - rtb_DenCoefOut = 0.0F; - } - - // Sum: '/Sum2' incorporates: - // Switch: '/Switch3' - - rtb_Switch2_f = rtb_DenCoefOut + rtu_Targets->jointvelocities.velocity; - - // Switch: '/Switch2' incorporates: - // Gain: '/Gain' - // RelationalOperator: '/LowerRelop1' - // RelationalOperator: '/UpperRelop' - // Switch: '/Switch' - - if (rtb_Switch2_f > rtb_Abs) { - rtb_Switch2_f = rtb_Abs; - } else if (rtb_Switch2_f < -rtb_Abs) { - // Switch: '/Switch' incorporates: - // Gain: '/Gain' - - rtb_Switch2_f = -rtb_Abs; - } - - // End of Switch: '/Switch2' - - // Sum: '/Sum1' - rtb_Product = rtb_Switch2_f - rtu_Estimates->jointvelocities.velocity; - - // Product: '/PProd Out' - rtb_Abs = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.P; - - // Product: '/IProd Out' - rtb_Gain1_h = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.I; - - // Product: '/DProd Out' - rtb_PProdOut = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.D; - - // SampleTimeMath: '/Tsamp' - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - - rtb_Product = rtu_ConfigurationParameters->VelLoopPID.N * 0.0005F; - - // Math: '/Reciprocal' incorporates: - // Constant: '/Filter Den Constant' - // Sum: '/SumDen' - // - // About '/Reciprocal': - // Operator: reciprocal - - rtb_DenCoefOut = 1.0F / (rtb_Product + 1.0F); - - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // Constant: '/Filter Den Constant' - // DiscreteTransferFcn: '/Filter Differentiator TF' - // Product: '/Divide' - // Sum: '/SumNum' - - if (rtb_FixPtRelationalOperator && - (control_outer_PrevZCX.FilterDifferentiatorTF_Reset__e != POS_ZCSIG)) { - control_outer_DW.FilterDifferentiatorTF_states_c = 0.0F; - } - - control_outer_PrevZCX.FilterDifferentiatorTF_Reset__e = - rtb_FixPtRelationalOperator; - control_outer_DW.FilterDifferentiatorTF_tmp_p = rtb_PProdOut - (rtb_Product - - 1.0F) * rtb_DenCoefOut * control_outer_DW.FilterDifferentiatorTF_states_c; - - // Product: '/NProd Out' incorporates: - // DiscreteTransferFcn: '/Filter Differentiator TF' - // Product: '/DenCoefOut' - - rtb_DenCoefOut = (control_outer_DW.FilterDifferentiatorTF_tmp_p - - control_outer_DW.FilterDifferentiatorTF_states_c) * - rtb_DenCoefOut * rtu_ConfigurationParameters->VelLoopPID.N; - - // Sum: '/SumI1' incorporates: - // Sum: '/Sum Fdbk' - // Sum: '/SumI3' - // UnitDelay: '/Unit Delay1' - - rtb_PProdOut = (control_outer_DW.UnitDelay1_DSTATE - ((rtb_Abs + - control_outer_DW.Integrator_DSTATE_b) + rtb_DenCoefOut)) + rtb_Gain1_h; - - // DiscreteIntegrator: '/Integrator' - if (rtb_FixPtRelationalOperator && - (control_outer_DW.Integrator_PrevResetState_c <= 0)) { - control_outer_DW.Integrator_DSTATE_b = 0.0F; - } - - // DiscreteIntegrator: '/Integrator' - rtb_Product = 0.0005F * rtb_PProdOut + control_outer_DW.Integrator_DSTATE_b; - - // Switch: '/Switch1' incorporates: - // Sum: '/Sum' - - if (rtb_Compare_m) { - rtb_Abs = (rtb_Abs + rtb_Product) + rtb_DenCoefOut; - } else { - rtb_Abs = rtu_Targets->motorcurrent.current; - } - - // End of Switch: '/Switch1' - - // Switch: '/Switch2' incorporates: - // Gain: '/Gain1' - // RelationalOperator: '/LowerRelop1' - // RelationalOperator: '/UpperRelop' - // Switch: '/Switch' - - if (rtb_Abs > rtu_ConfigurationParameters->thresholds.motorPeakCurrents) { - rtb_Abs = rtu_ConfigurationParameters->thresholds.motorPeakCurrents; - } else if (rtb_Abs < - -rtu_ConfigurationParameters->thresholds.motorPeakCurrents) { - // Switch: '/Switch' incorporates: - // Gain: '/Gain1' - - rtb_Abs = -rtu_ConfigurationParameters->thresholds.motorPeakCurrents; - } - - // End of Switch: '/Switch2' - - // BusCreator: '/Bus Creator2' - rty_OuterOutputs->motorcurrent.current = rtb_Abs; - - // Sum: '/Sum' incorporates: - // Abs: '/Abs1' - - rtb_DenCoefOut = rtu_ConfigurationParameters->thresholds.motorPeakCurrents - - std::abs(rtu_Estimates->Iq_filtered.current); - - // CombinatorialLogic: '/Logic' incorporates: - // Constant: '/Constant' - // Gain: '/Gain1' - // Logic: '/Logical Operator' - // Memory: '/Memory' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Compare' - - rowIdx = static_cast(((((rtb_DenCoefOut > 0.1F * - rtu_ConfigurationParameters->thresholds.motorPeakCurrents) || - rtb_FixPtRelationalOperator) + (static_cast(rtb_DenCoefOut <= 0.0F) - << 1)) << 1) + control_outer_DW.Memory_PreviousInput); - rtb_Compare = rtCP_Logic_table[static_cast(rowIdx) + 8U]; - - // DiscreteIntegrator: '/Discrete-Time Integrator' - if (control_outer_DW.DiscreteTimeIntegrator_SYSTEM_E != 0) { - // DiscreteIntegrator: '/Discrete-Time Integrator' - control_outer_B.DiscreteTimeIntegrator = - control_outer_DW.DiscreteTimeIntegrator_DSTATE; - } else if (rtb_Compare || (control_outer_DW.DiscreteTimeIntegrator_PrevRese != - 0)) { - // DiscreteIntegrator: '/Discrete-Time Integrator' - control_outer_B.DiscreteTimeIntegrator = 0.0F; - } else { - // DiscreteIntegrator: '/Discrete-Time Integrator' - control_outer_B.DiscreteTimeIntegrator = 0.0005F * rtb_DenCoefOut + - control_outer_DW.DiscreteTimeIntegrator_DSTATE; - } - - // End of DiscreteIntegrator: '/Discrete-Time Integrator' - - // BusCreator: '/Bus Creator1' - rty_OuterOutputs->vel_en = rtb_Compare_m; - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant' - // RelationalOperator: '/Compare' - - if (rtu_Estimates->Iq_filtered.current < 0.0F) { - tmp = -1; - } else { - tmp = 1; - } - - // BusCreator: '/Bus Creator1' incorporates: - // Product: '/Product' - // Switch: '/Switch1' - - rty_OuterOutputs->current_limiter = static_cast(tmp) * - control_outer_B.DiscreteTimeIntegrator * - rtu_ConfigurationParameters->CurLoopPID.I; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - control_outer_DW.DelayInput1_DSTATE = rtu_Flags->control_mode; - - // Update for DiscreteTransferFcn: '/Filter Differentiator TF' - control_outer_DW.FilterDifferentiatorTF_states = - control_outer_DW.FilterDifferentiatorTF_tmp; - - // Update for UnitDelay: '/Unit Delay' incorporates: - // Sum: '/Sum3' - - control_outer_DW.UnitDelay_DSTATE = rtb_Switch2_f - - rtu_Targets->jointvelocities.velocity; - - // Update for DiscreteIntegrator: '/Integrator' - control_outer_DW.Integrator_DSTATE = 0.0005F * rtb_DProdOut + - rtb_FilterDifferentiatorTF; - control_outer_DW.Integrator_PrevResetState = static_cast - (rtb_FixPtRelationalOperator); - - // Update for DiscreteTransferFcn: '/Filter Differentiator TF' - control_outer_DW.FilterDifferentiatorTF_states_i = - control_outer_DW.FilterDifferentiatorTF_tmp_m; - - // Update for DiscreteIntegrator: '/Integrator' incorporates: - // DiscreteIntegrator: '/Integrator' - - control_outer_DW.Integrator_DSTATE_i = 0.0005F * rtb_FilterDifferentiatorTF_n - + rtb_DProdOut_f; - control_outer_DW.Integrator_PrevResetState_n = static_cast - (rtb_FixPtRelationalOperator); - - // Update for DiscreteTransferFcn: '/Filter Differentiator TF' - control_outer_DW.FilterDifferentiatorTF_states_c = - control_outer_DW.FilterDifferentiatorTF_tmp_p; - - // Update for UnitDelay: '/Unit Delay1' - control_outer_DW.UnitDelay1_DSTATE = rtb_Abs; - - // Update for DiscreteIntegrator: '/Integrator' incorporates: - // DiscreteIntegrator: '/Integrator' - - control_outer_DW.Integrator_DSTATE_b = 0.0005F * rtb_PProdOut + rtb_Product; - control_outer_DW.Integrator_PrevResetState_c = static_cast - (rtb_FixPtRelationalOperator); - - // Update for Memory: '/Memory' incorporates: - // CombinatorialLogic: '/Logic' - - control_outer_DW.Memory_PreviousInput = rtCP_Logic_table[static_cast - (rowIdx)]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - control_outer_DW.DiscreteTimeIntegrator_SYSTEM_E = 0U; - control_outer_DW.DiscreteTimeIntegrator_DSTATE = 0.0005F * rtb_DenCoefOut + - control_outer_B.DiscreteTimeIntegrator; - control_outer_DW.DiscreteTimeIntegrator_PrevRese = static_cast - (rtb_Compare); -} - -// Model initialize function -void control_outer_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_control_outer_T *const control_outer_M = &(control_outer_MdlrefDW.rtm); - - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(control_outer_M, rt_errorStatus); - control_outer_PrevZCX.FilterDifferentiatorTF_Reset_ZC = POS_ZCSIG; - control_outer_PrevZCX.FilterDifferentiatorTF_Reset__m = POS_ZCSIG; - control_outer_PrevZCX.FilterDifferentiatorTF_Reset__e = POS_ZCSIG; -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_outer.cpp +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:58 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "control_outer.h" +#include "control_outer_types.h" +#include +#include "rtwtypes.h" +#include "control_outer_private.h" +#include "zero_crossing_types.h" + +MdlrefDW_control_outer_T control_outer_MdlrefDW; + +// Block signals (default storage) +B_control_outer_c_T control_outer_B; + +// Block states (default storage) +DW_control_outer_f_T control_outer_DW; + +// Previous zero-crossings (trigger) states +ZCE_control_outer_T control_outer_PrevZCX; + +// System initialize for referenced model: 'control_outer' +void control_outer_Init(void) +{ + // InitializeConditions for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + control_outer_DW.DelayInput1_DSTATE = ControlModes_Idle; + + // InitializeConditions for DiscreteIntegrator: '/Integrator' + control_outer_DW.Integrator_PrevResetState = 2; + + // InitializeConditions for DiscreteIntegrator: '/Integrator' + control_outer_DW.Integrator_PrevResetState_n = 2; + + // InitializeConditions for DiscreteIntegrator: '/Integrator' + control_outer_DW.Integrator_PrevResetState_c = 2; +} + +// Enable for referenced model: 'control_outer' +void control_outer_Enable(void) +{ + // Enable for DiscreteIntegrator: '/Discrete-Time Integrator' + control_outer_DW.DiscreteTimeIntegrator_SYSTEM_E = 1U; +} + +// Disable for referenced model: 'control_outer' +void control_outer_Disable(void) +{ + // Disable for DiscreteIntegrator: '/Discrete-Time Integrator' + control_outer_DW.DiscreteTimeIntegrator_DSTATE = + control_outer_B.DiscreteTimeIntegrator; +} + +// Output and update for referenced model: 'control_outer' +void control_outer(const Flags *rtu_Flags, const ConfigurationParameters + *rtu_ConfigurationParameters, const Targets *rtu_Targets, + const SensorsData *rtu_Sensors, const EstimatedData + *rtu_Estimates, ControlOuterOutputs *rty_OuterOutputs) +{ + int32_T rowIdx; + int32_T tmp; + real32_T rtb_Abs; + real32_T rtb_DProdOut; + real32_T rtb_DProdOut_f; + real32_T rtb_DenCoefOut; + real32_T rtb_FilterDifferentiatorTF; + real32_T rtb_FilterDifferentiatorTF_n; + real32_T rtb_Gain1_h; + real32_T rtb_PProdOut; + real32_T rtb_PProdOut_o; + real32_T rtb_Product; + real32_T rtb_Switch2_f; + boolean_T rtb_Compare; + boolean_T rtb_Compare_m; + boolean_T rtb_FixPtRelationalOperator; + + // Abs: '/Abs' + rtb_Abs = std::abs(rtu_ConfigurationParameters->thresholds.jntVelMax); + + // RelationalOperator: '/Compare' incorporates: + // Constant: '/Constant' + + rtb_Compare_m = (rtu_Flags->control_mode != ControlModes_Current); + + // RelationalOperator: '/Compare' incorporates: + // Constant: '/Constant' + + rty_OuterOutputs->cur_en = (rtu_Flags->control_mode != ControlModes_Voltage); + + // Logic: '/NOR' incorporates: + // Constant: '/Constant' + // Constant: '/Constant' + // Constant: '/Constant' + // RelationalOperator: '/Compare' + // RelationalOperator: '/Compare' + // RelationalOperator: '/Compare' + + rty_OuterOutputs->out_en = ((rtu_Flags->control_mode != + ControlModes_NotConfigured) && (rtu_Flags->control_mode != ControlModes_Idle) + && (rtu_Flags->control_mode != ControlModes_HwFaultCM)); + + // Sum: '/Sum3' + rtb_DenCoefOut = rtu_Targets->jointpositions.position - + rtu_Sensors->jointpositions.position; + + // Product: '/PProd Out' + rtb_PProdOut = rtb_DenCoefOut * rtu_ConfigurationParameters->PosLoopPID.P; + + // SampleTimeMath: '/Tsamp' + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_Product = rtu_ConfigurationParameters->PosLoopPID.N * 0.0005F; + + // Math: '/Reciprocal' incorporates: + // Constant: '/Filter Den Constant' + // Sum: '/SumDen' + // + // About '/Reciprocal': + // Operator: reciprocal + + rtb_Switch2_f = 1.0F / (rtb_Product + 1.0F); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator = (rtu_Flags->control_mode != + control_outer_DW.DelayInput1_DSTATE); + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // Constant: '/Filter Den Constant' + // Product: '/DProd Out' + // Product: '/Divide' + // Sum: '/SumNum' + + if (rtb_FixPtRelationalOperator && + (control_outer_PrevZCX.FilterDifferentiatorTF_Reset_ZC != POS_ZCSIG)) { + control_outer_DW.FilterDifferentiatorTF_states = 0.0F; + } + + control_outer_PrevZCX.FilterDifferentiatorTF_Reset_ZC = + rtb_FixPtRelationalOperator; + control_outer_DW.FilterDifferentiatorTF_tmp = rtb_DenCoefOut * + rtu_ConfigurationParameters->PosLoopPID.D - (rtb_Product - 1.0F) * + rtb_Switch2_f * control_outer_DW.FilterDifferentiatorTF_states; + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DenCoefOut' + + rtb_Switch2_f = (control_outer_DW.FilterDifferentiatorTF_tmp - + control_outer_DW.FilterDifferentiatorTF_states) * + rtb_Switch2_f * rtu_ConfigurationParameters->PosLoopPID.N; + + // Sum: '/SumI1' incorporates: + // Product: '/IProd Out' + // Sum: '/Sum Fdbk' + // Sum: '/SumI3' + // UnitDelay: '/Unit Delay' + + rtb_DProdOut = (control_outer_DW.UnitDelay_DSTATE - ((rtb_PProdOut + + control_outer_DW.Integrator_DSTATE) + rtb_Switch2_f)) + rtb_DenCoefOut * + rtu_ConfigurationParameters->PosLoopPID.I; + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && (control_outer_DW.Integrator_PrevResetState + <= 0)) { + control_outer_DW.Integrator_DSTATE = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_FilterDifferentiatorTF = 0.0005F * rtb_DProdOut + + control_outer_DW.Integrator_DSTATE; + + // RelationalOperator: '/Compare' incorporates: + // Constant: '/Constant' + + rtb_Compare = (rtu_Flags->control_mode == ControlModes_Position); + + // Product: '/PProd Out' + rtb_PProdOut_o = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.P; + + // Product: '/IProd Out' + rtb_Product = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.I; + + // Product: '/DProd Out' + rtb_DProdOut_f = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.D; + + // SampleTimeMath: '/Tsamp' + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_Gain1_h = rtu_ConfigurationParameters->DirLoopPID.N * 0.0005F; + + // Math: '/Reciprocal' incorporates: + // Constant: '/Filter Den Constant' + // Sum: '/SumDen' + // + // About '/Reciprocal': + // Operator: reciprocal + + rtb_DenCoefOut = 1.0F / (rtb_Gain1_h + 1.0F); + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // Constant: '/Filter Den Constant' + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/Divide' + // Sum: '/SumNum' + + if (rtb_FixPtRelationalOperator && + (control_outer_PrevZCX.FilterDifferentiatorTF_Reset__m != POS_ZCSIG)) { + control_outer_DW.FilterDifferentiatorTF_states_i = 0.0F; + } + + control_outer_PrevZCX.FilterDifferentiatorTF_Reset__m = + rtb_FixPtRelationalOperator; + control_outer_DW.FilterDifferentiatorTF_tmp_m = rtb_DProdOut_f - (rtb_Gain1_h + - 1.0F) * rtb_DenCoefOut * control_outer_DW.FilterDifferentiatorTF_states_i; + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DenCoefOut' + + rtb_DenCoefOut = (control_outer_DW.FilterDifferentiatorTF_tmp_m - + control_outer_DW.FilterDifferentiatorTF_states_i) * + rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.N; + + // Sum: '/SumI1' incorporates: + // Sum: '/Sum Fdbk' + // Sum: '/SumI3' + // UnitDelay: '/Unit Delay' + + rtb_FilterDifferentiatorTF_n = (control_outer_DW.UnitDelay_DSTATE - + ((rtb_PProdOut_o + control_outer_DW.Integrator_DSTATE_i) + rtb_DenCoefOut)) + + rtb_Product; + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && + (control_outer_DW.Integrator_PrevResetState_n <= 0)) { + control_outer_DW.Integrator_DSTATE_i = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_DProdOut_f = 0.0005F * rtb_FilterDifferentiatorTF_n + + control_outer_DW.Integrator_DSTATE_i; + + // Switch: '/Switch3' incorporates: + // Constant: '/Constant1' + // Constant: '/Constant' + // Constant: '/Constant' + // Logic: '/OR' + // RelationalOperator: '/Compare' + // RelationalOperator: '/Compare' + + if (rtb_Compare || (rtu_Flags->control_mode == ControlModes_PositionDirect) || + (rtu_Flags->control_mode == ControlModes_Velocity)) { + // Switch: '/Switch5' incorporates: + // Sum: '/Sum' + // Sum: '/Sum' + + if (rtb_Compare) { + rtb_DenCoefOut = (rtb_PProdOut + rtb_FilterDifferentiatorTF) + + rtb_Switch2_f; + } else { + rtb_DenCoefOut += rtb_PProdOut_o + rtb_DProdOut_f; + } + + // End of Switch: '/Switch5' + } else { + rtb_DenCoefOut = 0.0F; + } + + // Sum: '/Sum2' incorporates: + // Switch: '/Switch3' + + rtb_Switch2_f = rtb_DenCoefOut + rtu_Targets->jointvelocities.velocity; + + // Switch: '/Switch2' incorporates: + // Gain: '/Gain' + // RelationalOperator: '/LowerRelop1' + // RelationalOperator: '/UpperRelop' + // Switch: '/Switch' + + if (rtb_Switch2_f > rtb_Abs) { + rtb_Switch2_f = rtb_Abs; + } else if (rtb_Switch2_f < -rtb_Abs) { + // Switch: '/Switch' incorporates: + // Gain: '/Gain' + + rtb_Switch2_f = -rtb_Abs; + } + + // End of Switch: '/Switch2' + + // Sum: '/Sum1' + rtb_Product = rtb_Switch2_f - rtu_Estimates->jointvelocities.velocity; + + // Product: '/PProd Out' + rtb_Abs = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.P; + + // Product: '/IProd Out' + rtb_Gain1_h = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.I; + + // Product: '/DProd Out' + rtb_PProdOut = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.D; + + // SampleTimeMath: '/Tsamp' + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_Product = rtu_ConfigurationParameters->VelLoopPID.N * 0.0005F; + + // Math: '/Reciprocal' incorporates: + // Constant: '/Filter Den Constant' + // Sum: '/SumDen' + // + // About '/Reciprocal': + // Operator: reciprocal + + rtb_DenCoefOut = 1.0F / (rtb_Product + 1.0F); + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // Constant: '/Filter Den Constant' + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/Divide' + // Sum: '/SumNum' + + if (rtb_FixPtRelationalOperator && + (control_outer_PrevZCX.FilterDifferentiatorTF_Reset__e != POS_ZCSIG)) { + control_outer_DW.FilterDifferentiatorTF_states_c = 0.0F; + } + + control_outer_PrevZCX.FilterDifferentiatorTF_Reset__e = + rtb_FixPtRelationalOperator; + control_outer_DW.FilterDifferentiatorTF_tmp_p = rtb_PProdOut - (rtb_Product - + 1.0F) * rtb_DenCoefOut * control_outer_DW.FilterDifferentiatorTF_states_c; + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DenCoefOut' + + rtb_DenCoefOut = (control_outer_DW.FilterDifferentiatorTF_tmp_p - + control_outer_DW.FilterDifferentiatorTF_states_c) * + rtb_DenCoefOut * rtu_ConfigurationParameters->VelLoopPID.N; + + // Sum: '/SumI1' incorporates: + // Sum: '/Sum Fdbk' + // Sum: '/SumI3' + // UnitDelay: '/Unit Delay1' + + rtb_PProdOut = (control_outer_DW.UnitDelay1_DSTATE - ((rtb_Abs + + control_outer_DW.Integrator_DSTATE_b) + rtb_DenCoefOut)) + rtb_Gain1_h; + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && + (control_outer_DW.Integrator_PrevResetState_c <= 0)) { + control_outer_DW.Integrator_DSTATE_b = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_Product = 0.0005F * rtb_PProdOut + control_outer_DW.Integrator_DSTATE_b; + + // Switch: '/Switch1' incorporates: + // Sum: '/Sum' + + if (rtb_Compare_m) { + rtb_Abs = (rtb_Abs + rtb_Product) + rtb_DenCoefOut; + } else { + rtb_Abs = rtu_Targets->motorcurrent.current; + } + + // End of Switch: '/Switch1' + + // Switch: '/Switch2' incorporates: + // Gain: '/Gain1' + // RelationalOperator: '/LowerRelop1' + // RelationalOperator: '/UpperRelop' + // Switch: '/Switch' + + if (rtb_Abs > rtu_ConfigurationParameters->thresholds.motorPeakCurrents) { + rtb_Abs = rtu_ConfigurationParameters->thresholds.motorPeakCurrents; + } else if (rtb_Abs < + -rtu_ConfigurationParameters->thresholds.motorPeakCurrents) { + // Switch: '/Switch' incorporates: + // Gain: '/Gain1' + + rtb_Abs = -rtu_ConfigurationParameters->thresholds.motorPeakCurrents; + } + + // End of Switch: '/Switch2' + + // BusCreator: '/Bus Creator2' + rty_OuterOutputs->motorcurrent.current = rtb_Abs; + + // Sum: '/Sum' incorporates: + // Abs: '/Abs1' + + rtb_DenCoefOut = rtu_ConfigurationParameters->thresholds.motorPeakCurrents - + std::abs(rtu_Estimates->Iq_filtered.current); + + // CombinatorialLogic: '/Logic' incorporates: + // Constant: '/Constant' + // Gain: '/Gain1' + // Logic: '/Logical Operator' + // Memory: '/Memory' + // RelationalOperator: '/Relational Operator' + // RelationalOperator: '/Compare' + + rowIdx = static_cast(((((rtb_DenCoefOut > 0.1F * + rtu_ConfigurationParameters->thresholds.motorPeakCurrents) || + rtb_FixPtRelationalOperator) + (static_cast(rtb_DenCoefOut <= 0.0F) + << 1)) << 1) + control_outer_DW.Memory_PreviousInput); + rtb_Compare = rtCP_Logic_table[static_cast(rowIdx) + 8U]; + + // DiscreteIntegrator: '/Discrete-Time Integrator' + if (control_outer_DW.DiscreteTimeIntegrator_SYSTEM_E != 0) { + // DiscreteIntegrator: '/Discrete-Time Integrator' + control_outer_B.DiscreteTimeIntegrator = + control_outer_DW.DiscreteTimeIntegrator_DSTATE; + } else if (rtb_Compare || (control_outer_DW.DiscreteTimeIntegrator_PrevRese != + 0)) { + // DiscreteIntegrator: '/Discrete-Time Integrator' + control_outer_B.DiscreteTimeIntegrator = 0.0F; + } else { + // DiscreteIntegrator: '/Discrete-Time Integrator' + control_outer_B.DiscreteTimeIntegrator = 0.0005F * rtb_DenCoefOut + + control_outer_DW.DiscreteTimeIntegrator_DSTATE; + } + + // End of DiscreteIntegrator: '/Discrete-Time Integrator' + + // BusCreator: '/Bus Creator1' + rty_OuterOutputs->vel_en = rtb_Compare_m; + + // Switch: '/Switch1' incorporates: + // Constant: '/Constant1' + // Constant: '/Constant2' + // Constant: '/Constant' + // RelationalOperator: '/Compare' + + if (rtu_Estimates->Iq_filtered.current < 0.0F) { + tmp = -1; + } else { + tmp = 1; + } + + // BusCreator: '/Bus Creator1' incorporates: + // Product: '/Product' + // Switch: '/Switch1' + + rty_OuterOutputs->current_limiter = static_cast(tmp) * + control_outer_B.DiscreteTimeIntegrator * + rtu_ConfigurationParameters->CurLoopPID.I; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + control_outer_DW.DelayInput1_DSTATE = rtu_Flags->control_mode; + + // Update for DiscreteTransferFcn: '/Filter Differentiator TF' + control_outer_DW.FilterDifferentiatorTF_states = + control_outer_DW.FilterDifferentiatorTF_tmp; + + // Update for UnitDelay: '/Unit Delay' incorporates: + // Sum: '/Sum3' + + control_outer_DW.UnitDelay_DSTATE = rtb_Switch2_f - + rtu_Targets->jointvelocities.velocity; + + // Update for DiscreteIntegrator: '/Integrator' + control_outer_DW.Integrator_DSTATE = 0.0005F * rtb_DProdOut + + rtb_FilterDifferentiatorTF; + control_outer_DW.Integrator_PrevResetState = static_cast + (rtb_FixPtRelationalOperator); + + // Update for DiscreteTransferFcn: '/Filter Differentiator TF' + control_outer_DW.FilterDifferentiatorTF_states_i = + control_outer_DW.FilterDifferentiatorTF_tmp_m; + + // Update for DiscreteIntegrator: '/Integrator' incorporates: + // DiscreteIntegrator: '/Integrator' + + control_outer_DW.Integrator_DSTATE_i = 0.0005F * rtb_FilterDifferentiatorTF_n + + rtb_DProdOut_f; + control_outer_DW.Integrator_PrevResetState_n = static_cast + (rtb_FixPtRelationalOperator); + + // Update for DiscreteTransferFcn: '/Filter Differentiator TF' + control_outer_DW.FilterDifferentiatorTF_states_c = + control_outer_DW.FilterDifferentiatorTF_tmp_p; + + // Update for UnitDelay: '/Unit Delay1' + control_outer_DW.UnitDelay1_DSTATE = rtb_Abs; + + // Update for DiscreteIntegrator: '/Integrator' incorporates: + // DiscreteIntegrator: '/Integrator' + + control_outer_DW.Integrator_DSTATE_b = 0.0005F * rtb_PProdOut + rtb_Product; + control_outer_DW.Integrator_PrevResetState_c = static_cast + (rtb_FixPtRelationalOperator); + + // Update for Memory: '/Memory' incorporates: + // CombinatorialLogic: '/Logic' + + control_outer_DW.Memory_PreviousInput = rtCP_Logic_table[static_cast + (rowIdx)]; + + // Update for DiscreteIntegrator: '/Discrete-Time Integrator' + control_outer_DW.DiscreteTimeIntegrator_SYSTEM_E = 0U; + control_outer_DW.DiscreteTimeIntegrator_DSTATE = 0.0005F * rtb_DenCoefOut + + control_outer_B.DiscreteTimeIntegrator; + control_outer_DW.DiscreteTimeIntegrator_PrevRese = static_cast + (rtb_Compare); +} + +// Model initialize function +void control_outer_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_control_outer_T *const control_outer_M = &(control_outer_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(control_outer_M, rt_errorStatus); + control_outer_PrevZCX.FilterDifferentiatorTF_Reset_ZC = POS_ZCSIG; + control_outer_PrevZCX.FilterDifferentiatorTF_Reset__m = POS_ZCSIG; + control_outer_PrevZCX.FilterDifferentiatorTF_Reset__e = POS_ZCSIG; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.h index 1fba1b2b6c..5d4f4588cc 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer.h @@ -1,337 +1,337 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_outer.h -// -// Code generated for Simulink model 'control_outer'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:17 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_control_outer_h_ -#define RTW_HEADER_control_outer_h_ -#include "rtwtypes.h" -#include "control_outer_types.h" -#include "zero_crossing_types.h" - -// Block signals for model 'control_outer' -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -struct B_control_outer_c_T { - real32_T DiscreteTimeIntegrator; // '/Discrete-Time Integrator' -}; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -// Block states (default storage) for model 'control_outer' -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -struct DW_control_outer_f_T { - real32_T FilterDifferentiatorTF_states;// '/Filter Differentiator TF' - real32_T UnitDelay_DSTATE; // '/Unit Delay' - real32_T Integrator_DSTATE; // '/Integrator' - real32_T FilterDifferentiatorTF_states_i;// '/Filter Differentiator TF' - real32_T Integrator_DSTATE_i; // '/Integrator' - real32_T FilterDifferentiatorTF_states_c;// '/Filter Differentiator TF' - real32_T UnitDelay1_DSTATE; // '/Unit Delay1' - real32_T Integrator_DSTATE_b; // '/Integrator' - real32_T DiscreteTimeIntegrator_DSTATE;// '/Discrete-Time Integrator' - ControlModes DelayInput1_DSTATE; // '/Delay Input1' - real32_T FilterDifferentiatorTF_tmp; // '/Filter Differentiator TF' - real32_T FilterDifferentiatorTF_tmp_m;// '/Filter Differentiator TF' - real32_T FilterDifferentiatorTF_tmp_p;// '/Filter Differentiator TF' - int8_T Integrator_PrevResetState; // '/Integrator' - int8_T Integrator_PrevResetState_n; // '/Integrator' - int8_T Integrator_PrevResetState_c; // '/Integrator' - int8_T DiscreteTimeIntegrator_PrevRese;// '/Discrete-Time Integrator' - uint8_T DiscreteTimeIntegrator_SYSTEM_E;// '/Discrete-Time Integrator' - boolean_T Memory_PreviousInput; // '/Memory' -}; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -// Zero-crossing (trigger) state for model 'control_outer' -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -struct ZCV_control_outer_g_T { - real_T FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' - real_T FilterDifferentiatorTF_Reset__f;// '/Filter Differentiator TF' - real_T FilterDifferentiatorTF_Reset__m;// '/Filter Differentiator TF' -}; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -// Zero-crossing (trigger) state for model 'control_outer' -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -struct ZCE_control_outer_T { - ZCSigState FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' - ZCSigState FilterDifferentiatorTF_Reset__m;// '/Filter Differentiator TF' - ZCSigState FilterDifferentiatorTF_Reset__e;// '/Filter Differentiator TF' -}; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_control_outer_T { - const char_T **errorStatus; -}; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_control_outer_T { - RT_MODEL_control_outer_T rtm; -}; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -extern void control_outer_Init(void); -extern void control_outer_Enable(void); -extern void control_outer_Disable(void); -extern void control_outer(const Flags *rtu_Flags, const ConfigurationParameters * - rtu_ConfigurationParameters, const Targets *rtu_Targets, const SensorsData - *rtu_Sensors, const EstimatedData *rtu_Estimates, ControlOuterOutputs - *rty_OuterOutputs); - -// Model reference registration function -extern void control_outer_initialize(const char_T **rt_errorStatus); - -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_control_outer_T control_outer_MdlrefDW; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -#ifndef control_outer_MDLREF_HIDE_CHILD_ - -// Block signals (default storage) -extern B_control_outer_c_T control_outer_B; - -// Block states (default storage) -extern DW_control_outer_f_T control_outer_DW; - -// Previous zero-crossings (trigger) states -extern ZCE_control_outer_T control_outer_PrevZCX; - -#endif //control_outer_MDLREF_HIDE_CHILD_ - -//- -// These blocks were eliminated from the model due to optimizations: -// -// 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 '/Passthrough for tuning' : Eliminate redundant data type conversion -// Block '/Kt' : Eliminated nontunable gain of 1 -// Block '/Passthrough for tuning' : Eliminate redundant data type conversion -// Block '/Kt' : Eliminated nontunable gain of 1 -// Block '/Passthrough for tuning' : Eliminate redundant data type conversion -// Block '/Kt' : Eliminated nontunable gain of 1 - - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'control_outer' -// '' : 'control_outer/Current Limiter' -// '' : 'control_outer/Detect Change' -// '' : 'control_outer/Enabling Logic' -// '' : 'control_outer/Position PID control' -// '' : 'control_outer/Saturation Dynamic' -// '' : 'control_outer/Saturation Dynamic1' -// '' : 'control_outer/Velocity PID control' -// '' : 'control_outer/Current Limiter/Compare To Zero' -// '' : 'control_outer/Current Limiter/Compare To Zero1' -// '' : 'control_outer/Current Limiter/S-R Flip-Flop' -// '' : 'control_outer/Enabling Logic/Compare To Constant' -// '' : 'control_outer/Enabling Logic/Compare To Constant1' -// '' : 'control_outer/Enabling Logic/Compare To Constant2' -// '' : 'control_outer/Enabling Logic/Compare To Constant3' -// '' : 'control_outer/Enabling Logic/Compare To Constant4' -// '' : 'control_outer/Enabling Logic/Compare To Constant5' -// '' : 'control_outer/Enabling Logic/Compare To Constant6' -// '' : 'control_outer/Enabling Logic/Compare To Constant7' -// '' : 'control_outer/Position PID control/Position PID' -// '' : 'control_outer/Position PID control/Position-Direct PID' -// '' : 'control_outer/Position PID control/Position PID/Anti-windup' -// '' : 'control_outer/Position PID control/Position PID/D Gain' -// '' : 'control_outer/Position PID control/Position PID/Filter' -// '' : 'control_outer/Position PID control/Position PID/Filter ICs' -// '' : 'control_outer/Position PID control/Position PID/I Gain' -// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain' -// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain Fdbk' -// '' : 'control_outer/Position PID control/Position PID/Integrator' -// '' : 'control_outer/Position PID control/Position PID/Integrator ICs' -// '' : 'control_outer/Position PID control/Position PID/N Copy' -// '' : 'control_outer/Position PID control/Position PID/N Gain' -// '' : 'control_outer/Position PID control/Position PID/P Copy' -// '' : 'control_outer/Position PID control/Position PID/Parallel P Gain' -// '' : 'control_outer/Position PID control/Position PID/Reset Signal' -// '' : 'control_outer/Position PID control/Position PID/Saturation' -// '' : 'control_outer/Position PID control/Position PID/Saturation Fdbk' -// '' : 'control_outer/Position PID control/Position PID/Sum' -// '' : 'control_outer/Position PID control/Position PID/Sum Fdbk' -// '' : 'control_outer/Position PID control/Position PID/Tracking Mode' -// '' : 'control_outer/Position PID control/Position PID/Tracking Mode Sum' -// '' : 'control_outer/Position PID control/Position PID/Tsamp - Integral' -// '' : 'control_outer/Position PID control/Position PID/Tsamp - Ngain' -// '' : 'control_outer/Position PID control/Position PID/postSat Signal' -// '' : 'control_outer/Position PID control/Position PID/preSat Signal' -// '' : 'control_outer/Position PID control/Position PID/Anti-windup/Passthrough' -// '' : 'control_outer/Position PID control/Position PID/D Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter' -// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter/Tsamp' -// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' -// '' : 'control_outer/Position PID control/Position PID/Filter ICs/Internal IC - Filter' -// '' : 'control_outer/Position PID control/Position PID/I Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain/Passthrough' -// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain Fdbk/Passthrough' -// '' : 'control_outer/Position PID control/Position PID/Integrator/Discrete' -// '' : 'control_outer/Position PID control/Position PID/Integrator ICs/Internal IC' -// '' : 'control_outer/Position PID control/Position PID/N Copy/External Parameters' -// '' : 'control_outer/Position PID control/Position PID/N Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position PID/P Copy/Disabled' -// '' : 'control_outer/Position PID control/Position PID/Parallel P Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position PID/Reset Signal/External Reset' -// '' : 'control_outer/Position PID control/Position PID/Saturation/Passthrough' -// '' : 'control_outer/Position PID control/Position PID/Saturation Fdbk/Passthrough' -// '' : 'control_outer/Position PID control/Position PID/Sum/Sum_PID' -// '' : 'control_outer/Position PID control/Position PID/Sum Fdbk/Enabled' -// '' : 'control_outer/Position PID control/Position PID/Tracking Mode/Enabled' -// '' : 'control_outer/Position PID control/Position PID/Tracking Mode Sum/Tracking Mode' -// '' : 'control_outer/Position PID control/Position PID/Tsamp - Integral/Passthrough' -// '' : 'control_outer/Position PID control/Position PID/Tsamp - Ngain/Passthrough' -// '' : 'control_outer/Position PID control/Position PID/postSat Signal/Feedback_Path' -// '' : 'control_outer/Position PID control/Position PID/preSat Signal/Feedback_Path' -// '' : 'control_outer/Position PID control/Position-Direct PID/Anti-windup' -// '' : 'control_outer/Position PID control/Position-Direct PID/D Gain' -// '' : 'control_outer/Position PID control/Position-Direct PID/Filter' -// '' : 'control_outer/Position PID control/Position-Direct PID/Filter ICs' -// '' : 'control_outer/Position PID control/Position-Direct PID/I Gain' -// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain' -// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain Fdbk' -// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator' -// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator ICs' -// '' : 'control_outer/Position PID control/Position-Direct PID/N Copy' -// '' : 'control_outer/Position PID control/Position-Direct PID/N Gain' -// '' : 'control_outer/Position PID control/Position-Direct PID/P Copy' -// '' : 'control_outer/Position PID control/Position-Direct PID/Parallel P Gain' -// '' : 'control_outer/Position PID control/Position-Direct PID/Reset Signal' -// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation' -// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation Fdbk' -// '' : 'control_outer/Position PID control/Position-Direct PID/Sum' -// '' : 'control_outer/Position PID control/Position-Direct PID/Sum Fdbk' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode Sum' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Integral' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Ngain' -// '' : 'control_outer/Position PID control/Position-Direct PID/postSat Signal' -// '' : 'control_outer/Position PID control/Position-Direct PID/preSat Signal' -// '' : 'control_outer/Position PID control/Position-Direct PID/Anti-windup/Passthrough' -// '' : 'control_outer/Position PID control/Position-Direct PID/D Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter' -// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter/Tsamp' -// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' -// '' : 'control_outer/Position PID control/Position-Direct PID/Filter ICs/Internal IC - Filter' -// '' : 'control_outer/Position PID control/Position-Direct PID/I Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain/Passthrough' -// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain Fdbk/Passthrough' -// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator/Discrete' -// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator ICs/Internal IC' -// '' : 'control_outer/Position PID control/Position-Direct PID/N Copy/External Parameters' -// '' : 'control_outer/Position PID control/Position-Direct PID/N Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position-Direct PID/P Copy/Disabled' -// '' : 'control_outer/Position PID control/Position-Direct PID/Parallel P Gain/External Parameters' -// '' : 'control_outer/Position PID control/Position-Direct PID/Reset Signal/External Reset' -// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation/Passthrough' -// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation Fdbk/Passthrough' -// '' : 'control_outer/Position PID control/Position-Direct PID/Sum/Sum_PID' -// '' : 'control_outer/Position PID control/Position-Direct PID/Sum Fdbk/Enabled' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode/Enabled' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode Sum/Tracking Mode' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Integral/Passthrough' -// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Ngain/Passthrough' -// '' : 'control_outer/Position PID control/Position-Direct PID/postSat Signal/Feedback_Path' -// '' : 'control_outer/Position PID control/Position-Direct PID/preSat Signal/Feedback_Path' -// '' : 'control_outer/Velocity PID control/Anti-windup' -// '' : 'control_outer/Velocity PID control/D Gain' -// '' : 'control_outer/Velocity PID control/Filter' -// '' : 'control_outer/Velocity PID control/Filter ICs' -// '' : 'control_outer/Velocity PID control/I Gain' -// '' : 'control_outer/Velocity PID control/Ideal P Gain' -// '' : 'control_outer/Velocity PID control/Ideal P Gain Fdbk' -// '' : 'control_outer/Velocity PID control/Integrator' -// '' : 'control_outer/Velocity PID control/Integrator ICs' -// '' : 'control_outer/Velocity PID control/N Copy' -// '' : 'control_outer/Velocity PID control/N Gain' -// '' : 'control_outer/Velocity PID control/P Copy' -// '' : 'control_outer/Velocity PID control/Parallel P Gain' -// '' : 'control_outer/Velocity PID control/Reset Signal' -// '' : 'control_outer/Velocity PID control/Saturation' -// '' : 'control_outer/Velocity PID control/Saturation Fdbk' -// '' : 'control_outer/Velocity PID control/Sum' -// '' : 'control_outer/Velocity PID control/Sum Fdbk' -// '' : 'control_outer/Velocity PID control/Tracking Mode' -// '' : 'control_outer/Velocity PID control/Tracking Mode Sum' -// '' : 'control_outer/Velocity PID control/Tsamp - Integral' -// '' : 'control_outer/Velocity PID control/Tsamp - Ngain' -// '' : 'control_outer/Velocity PID control/postSat Signal' -// '' : 'control_outer/Velocity PID control/preSat Signal' -// '' : 'control_outer/Velocity PID control/Anti-windup/Passthrough' -// '' : 'control_outer/Velocity PID control/D Gain/External Parameters' -// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter' -// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter/Tsamp' -// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' -// '' : 'control_outer/Velocity PID control/Filter ICs/Internal IC - Filter' -// '' : 'control_outer/Velocity PID control/I Gain/External Parameters' -// '' : 'control_outer/Velocity PID control/Ideal P Gain/Passthrough' -// '' : 'control_outer/Velocity PID control/Ideal P Gain Fdbk/Passthrough' -// '' : 'control_outer/Velocity PID control/Integrator/Discrete' -// '' : 'control_outer/Velocity PID control/Integrator ICs/Internal IC' -// '' : 'control_outer/Velocity PID control/N Copy/External Parameters' -// '' : 'control_outer/Velocity PID control/N Gain/External Parameters' -// '' : 'control_outer/Velocity PID control/P Copy/Disabled' -// '' : 'control_outer/Velocity PID control/Parallel P Gain/External Parameters' -// '' : 'control_outer/Velocity PID control/Reset Signal/External Reset' -// '' : 'control_outer/Velocity PID control/Saturation/Passthrough' -// '' : 'control_outer/Velocity PID control/Saturation Fdbk/Passthrough' -// '' : 'control_outer/Velocity PID control/Sum/Sum_PID' -// '' : 'control_outer/Velocity PID control/Sum Fdbk/Enabled' -// '' : 'control_outer/Velocity PID control/Tracking Mode/Enabled' -// '' : 'control_outer/Velocity PID control/Tracking Mode Sum/Tracking Mode' -// '' : 'control_outer/Velocity PID control/Tsamp - Integral/Passthrough' -// '' : 'control_outer/Velocity PID control/Tsamp - Ngain/Passthrough' -// '' : 'control_outer/Velocity PID control/postSat Signal/Feedback_Path' -// '' : 'control_outer/Velocity PID control/preSat Signal/Feedback_Path' - -#endif // RTW_HEADER_control_outer_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_outer.h +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:58 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_outer_h_ +#define RTW_HEADER_control_outer_h_ +#include "rtwtypes.h" +#include "control_outer_types.h" +#include "zero_crossing_types.h" + +// Block signals for model 'control_outer' +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +struct B_control_outer_c_T { + real32_T DiscreteTimeIntegrator; // '/Discrete-Time Integrator' +}; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for model 'control_outer' +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +struct DW_control_outer_f_T { + real32_T FilterDifferentiatorTF_states;// '/Filter Differentiator TF' + real32_T UnitDelay_DSTATE; // '/Unit Delay' + real32_T Integrator_DSTATE; // '/Integrator' + real32_T FilterDifferentiatorTF_states_i;// '/Filter Differentiator TF' + real32_T Integrator_DSTATE_i; // '/Integrator' + real32_T FilterDifferentiatorTF_states_c;// '/Filter Differentiator TF' + real32_T UnitDelay1_DSTATE; // '/Unit Delay1' + real32_T Integrator_DSTATE_b; // '/Integrator' + real32_T DiscreteTimeIntegrator_DSTATE;// '/Discrete-Time Integrator' + ControlModes DelayInput1_DSTATE; // '/Delay Input1' + real32_T FilterDifferentiatorTF_tmp; // '/Filter Differentiator TF' + real32_T FilterDifferentiatorTF_tmp_m;// '/Filter Differentiator TF' + real32_T FilterDifferentiatorTF_tmp_p;// '/Filter Differentiator TF' + int8_T Integrator_PrevResetState; // '/Integrator' + int8_T Integrator_PrevResetState_n; // '/Integrator' + int8_T Integrator_PrevResetState_c; // '/Integrator' + int8_T DiscreteTimeIntegrator_PrevRese;// '/Discrete-Time Integrator' + uint8_T DiscreteTimeIntegrator_SYSTEM_E;// '/Discrete-Time Integrator' + boolean_T Memory_PreviousInput; // '/Memory' +}; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +// Zero-crossing (trigger) state for model 'control_outer' +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +struct ZCV_control_outer_g_T { + real_T FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' + real_T FilterDifferentiatorTF_Reset__f;// '/Filter Differentiator TF' + real_T FilterDifferentiatorTF_Reset__m;// '/Filter Differentiator TF' +}; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +// Zero-crossing (trigger) state for model 'control_outer' +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +struct ZCE_control_outer_T { + ZCSigState FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' + ZCSigState FilterDifferentiatorTF_Reset__m;// '/Filter Differentiator TF' + ZCSigState FilterDifferentiatorTF_Reset__e;// '/Filter Differentiator TF' +}; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_control_outer_T { + const char_T **errorStatus; +}; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_control_outer_T { + RT_MODEL_control_outer_T rtm; +}; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +extern void control_outer_Init(void); +extern void control_outer_Enable(void); +extern void control_outer_Disable(void); +extern void control_outer(const Flags *rtu_Flags, const ConfigurationParameters * + rtu_ConfigurationParameters, const Targets *rtu_Targets, const SensorsData + *rtu_Sensors, const EstimatedData *rtu_Estimates, ControlOuterOutputs + *rty_OuterOutputs); + +// Model reference registration function +extern void control_outer_initialize(const char_T **rt_errorStatus); + +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_control_outer_T control_outer_MdlrefDW; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +#ifndef control_outer_MDLREF_HIDE_CHILD_ + +// Block signals (default storage) +extern B_control_outer_c_T control_outer_B; + +// Block states (default storage) +extern DW_control_outer_f_T control_outer_DW; + +// Previous zero-crossings (trigger) states +extern ZCE_control_outer_T control_outer_PrevZCX; + +#endif //control_outer_MDLREF_HIDE_CHILD_ + +//- +// These blocks were eliminated from the model due to optimizations: +// +// 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 '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 + + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'control_outer' +// '' : 'control_outer/Current Limiter' +// '' : 'control_outer/Detect Change' +// '' : 'control_outer/Enabling Logic' +// '' : 'control_outer/Position PID control' +// '' : 'control_outer/Saturation Dynamic' +// '' : 'control_outer/Saturation Dynamic1' +// '' : 'control_outer/Velocity PID control' +// '' : 'control_outer/Current Limiter/Compare To Zero' +// '' : 'control_outer/Current Limiter/Compare To Zero1' +// '' : 'control_outer/Current Limiter/S-R Flip-Flop' +// '' : 'control_outer/Enabling Logic/Compare To Constant' +// '' : 'control_outer/Enabling Logic/Compare To Constant1' +// '' : 'control_outer/Enabling Logic/Compare To Constant2' +// '' : 'control_outer/Enabling Logic/Compare To Constant3' +// '' : 'control_outer/Enabling Logic/Compare To Constant4' +// '' : 'control_outer/Enabling Logic/Compare To Constant5' +// '' : 'control_outer/Enabling Logic/Compare To Constant6' +// '' : 'control_outer/Enabling Logic/Compare To Constant7' +// '' : 'control_outer/Position PID control/Position PID' +// '' : 'control_outer/Position PID control/Position-Direct PID' +// '' : 'control_outer/Position PID control/Position PID/Anti-windup' +// '' : 'control_outer/Position PID control/Position PID/D Gain' +// '' : 'control_outer/Position PID control/Position PID/Filter' +// '' : 'control_outer/Position PID control/Position PID/Filter ICs' +// '' : 'control_outer/Position PID control/Position PID/I Gain' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain Fdbk' +// '' : 'control_outer/Position PID control/Position PID/Integrator' +// '' : 'control_outer/Position PID control/Position PID/Integrator ICs' +// '' : 'control_outer/Position PID control/Position PID/N Copy' +// '' : 'control_outer/Position PID control/Position PID/N Gain' +// '' : 'control_outer/Position PID control/Position PID/P Copy' +// '' : 'control_outer/Position PID control/Position PID/Parallel P Gain' +// '' : 'control_outer/Position PID control/Position PID/Reset Signal' +// '' : 'control_outer/Position PID control/Position PID/Saturation' +// '' : 'control_outer/Position PID control/Position PID/Saturation Fdbk' +// '' : 'control_outer/Position PID control/Position PID/Sum' +// '' : 'control_outer/Position PID control/Position PID/Sum Fdbk' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode Sum' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Integral' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Ngain' +// '' : 'control_outer/Position PID control/Position PID/postSat Signal' +// '' : 'control_outer/Position PID control/Position PID/preSat Signal' +// '' : 'control_outer/Position PID control/Position PID/Anti-windup/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/D Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter' +// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_outer/Position PID control/Position PID/Filter ICs/Internal IC - Filter' +// '' : 'control_outer/Position PID control/Position PID/I Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Integrator/Discrete' +// '' : 'control_outer/Position PID control/Position PID/Integrator ICs/Internal IC' +// '' : 'control_outer/Position PID control/Position PID/N Copy/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/N Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/P Copy/Disabled' +// '' : 'control_outer/Position PID control/Position PID/Parallel P Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/Reset Signal/External Reset' +// '' : 'control_outer/Position PID control/Position PID/Saturation/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Saturation Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Sum/Sum_PID' +// '' : 'control_outer/Position PID control/Position PID/Sum Fdbk/Enabled' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode/Enabled' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode Sum/Tracking Mode' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Integral/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Ngain/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/postSat Signal/Feedback_Path' +// '' : 'control_outer/Position PID control/Position PID/preSat Signal/Feedback_Path' +// '' : 'control_outer/Position PID control/Position-Direct PID/Anti-windup' +// '' : 'control_outer/Position PID control/Position-Direct PID/D Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter ICs' +// '' : 'control_outer/Position PID control/Position-Direct PID/I Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain Fdbk' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator ICs' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Copy' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/P Copy' +// '' : 'control_outer/Position PID control/Position-Direct PID/Parallel P Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Reset Signal' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation Fdbk' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum Fdbk' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode Sum' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Integral' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Ngain' +// '' : 'control_outer/Position PID control/Position-Direct PID/postSat Signal' +// '' : 'control_outer/Position PID control/Position-Direct PID/preSat Signal' +// '' : 'control_outer/Position PID control/Position-Direct PID/Anti-windup/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/D Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter ICs/Internal IC - Filter' +// '' : 'control_outer/Position PID control/Position-Direct PID/I Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator/Discrete' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator ICs/Internal IC' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Copy/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/P Copy/Disabled' +// '' : 'control_outer/Position PID control/Position-Direct PID/Parallel P Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/Reset Signal/External Reset' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum/Sum_PID' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum Fdbk/Enabled' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode/Enabled' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode Sum/Tracking Mode' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Integral/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Ngain/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/postSat Signal/Feedback_Path' +// '' : 'control_outer/Position PID control/Position-Direct PID/preSat Signal/Feedback_Path' +// '' : 'control_outer/Velocity PID control/Anti-windup' +// '' : 'control_outer/Velocity PID control/D Gain' +// '' : 'control_outer/Velocity PID control/Filter' +// '' : 'control_outer/Velocity PID control/Filter ICs' +// '' : 'control_outer/Velocity PID control/I Gain' +// '' : 'control_outer/Velocity PID control/Ideal P Gain' +// '' : 'control_outer/Velocity PID control/Ideal P Gain Fdbk' +// '' : 'control_outer/Velocity PID control/Integrator' +// '' : 'control_outer/Velocity PID control/Integrator ICs' +// '' : 'control_outer/Velocity PID control/N Copy' +// '' : 'control_outer/Velocity PID control/N Gain' +// '' : 'control_outer/Velocity PID control/P Copy' +// '' : 'control_outer/Velocity PID control/Parallel P Gain' +// '' : 'control_outer/Velocity PID control/Reset Signal' +// '' : 'control_outer/Velocity PID control/Saturation' +// '' : 'control_outer/Velocity PID control/Saturation Fdbk' +// '' : 'control_outer/Velocity PID control/Sum' +// '' : 'control_outer/Velocity PID control/Sum Fdbk' +// '' : 'control_outer/Velocity PID control/Tracking Mode' +// '' : 'control_outer/Velocity PID control/Tracking Mode Sum' +// '' : 'control_outer/Velocity PID control/Tsamp - Integral' +// '' : 'control_outer/Velocity PID control/Tsamp - Ngain' +// '' : 'control_outer/Velocity PID control/postSat Signal' +// '' : 'control_outer/Velocity PID control/preSat Signal' +// '' : 'control_outer/Velocity PID control/Anti-windup/Passthrough' +// '' : 'control_outer/Velocity PID control/D Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter' +// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_outer/Velocity PID control/Filter ICs/Internal IC - Filter' +// '' : 'control_outer/Velocity PID control/I Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/Ideal P Gain/Passthrough' +// '' : 'control_outer/Velocity PID control/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_outer/Velocity PID control/Integrator/Discrete' +// '' : 'control_outer/Velocity PID control/Integrator ICs/Internal IC' +// '' : 'control_outer/Velocity PID control/N Copy/External Parameters' +// '' : 'control_outer/Velocity PID control/N Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/P Copy/Disabled' +// '' : 'control_outer/Velocity PID control/Parallel P Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/Reset Signal/External Reset' +// '' : 'control_outer/Velocity PID control/Saturation/Passthrough' +// '' : 'control_outer/Velocity PID control/Saturation Fdbk/Passthrough' +// '' : 'control_outer/Velocity PID control/Sum/Sum_PID' +// '' : 'control_outer/Velocity PID control/Sum Fdbk/Enabled' +// '' : 'control_outer/Velocity PID control/Tracking Mode/Enabled' +// '' : 'control_outer/Velocity PID control/Tracking Mode Sum/Tracking Mode' +// '' : 'control_outer/Velocity PID control/Tsamp - Integral/Passthrough' +// '' : 'control_outer/Velocity PID control/Tsamp - Ngain/Passthrough' +// '' : 'control_outer/Velocity PID control/postSat Signal/Feedback_Path' +// '' : 'control_outer/Velocity PID control/preSat Signal/Feedback_Path' + +#endif // RTW_HEADER_control_outer_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_private.h index 387b9000ce..a7077489b7 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_private.h @@ -1,53 +1,53 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_outer_private.h -// -// Code generated for Simulink model 'control_outer'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:17 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_control_outer_private_h_ -#define RTW_HEADER_control_outer_private_h_ -#include "rtwtypes.h" -#include "zero_crossing_types.h" -#include "control_outer_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -extern const boolean_T rtCP_pooled_kUC6nmgO8rex[16]; - -#define rtCP_Logic_table rtCP_pooled_kUC6nmgO8rex // Computed Parameter: rtCP_Logic_table - // Referenced by: '/Logic' - -#endif // RTW_HEADER_control_outer_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_outer_private.h +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:58 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_outer_private_h_ +#define RTW_HEADER_control_outer_private_h_ +#include "rtwtypes.h" +#include "zero_crossing_types.h" +#include "control_outer_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +extern const boolean_T rtCP_pooled_kUC6nmgO8rex[16]; + +#define rtCP_Logic_table rtCP_pooled_kUC6nmgO8rex // Computed Parameter: rtCP_Logic_table + // Referenced by: '/Logic' + +#endif // RTW_HEADER_control_outer_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_types.h index e48bedad8b..52b92c7b33 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/control-outer/control_outer_types.h @@ -1,306 +1,306 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_outer_types.h -// -// Code generated for Simulink model 'control_outer'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:17 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_control_outer_types_h_ -#define RTW_HEADER_control_outer_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ -#define DEFINED_TYPEDEF_FOR_ControlModes_ - -typedef enum { - ControlModes_NotConfigured = 0, // Default value - ControlModes_Idle, - ControlModes_Position, - ControlModes_PositionDirect, - ControlModes_Current, - ControlModes_Velocity, - ControlModes_Voltage, - ControlModes_Torque, - ControlModes_HwFaultCM -} ControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ - -struct Flags -{ - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ - -struct SensorsData -{ - // position encoders - JointPositions jointpositions; - MotorSensors motorsensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocities - JointVelocities jointvelocities; - MotorCurrent Iq_filtered; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ - -struct ControlOuterOutputs -{ - boolean_T vel_en; - boolean_T cur_en; - boolean_T out_en; - MotorCurrent motorcurrent; - real32_T current_limiter; -}; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_control_outer_T RT_MODEL_control_outer_T; - -#endif // RTW_HEADER_control_outer_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: control_outer_types.h +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:58 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_outer_types_h_ +#define RTW_HEADER_control_outer_types_h_ +#include "rtwtypes.h" +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_Torque, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + // control mode + ControlModes control_mode; + boolean_T enable_sending_msg_status; + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ +#define DEFINED_TYPEDEF_FOR_JointPositions_ + +struct JointPositions +{ + // joint positions + real32_T position; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ +#define DEFINED_TYPEDEF_FOR_JointVelocities_ + +struct JointVelocities +{ + // joint velocities + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ +#define DEFINED_TYPEDEF_FOR_MotorCurrent_ + +struct MotorCurrent +{ + // motor current + real32_T current; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ +#define DEFINED_TYPEDEF_FOR_MotorVoltage_ + +struct MotorVoltage +{ + // motor voltage + real32_T voltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + JointPositions jointpositions; + JointVelocities jointvelocities; + MotorCurrent motorcurrent; + MotorVoltage motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocities + JointVelocities jointvelocities; + MotorCurrent Iq_filtered; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ + +struct ControlOuterOutputs +{ + boolean_T vel_en; + boolean_T cur_en; + boolean_T out_en; + MotorCurrent motorcurrent; + real32_T current_limiter; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_control_outer_T RT_MODEL_control_outer_T; + +#endif // RTW_HEADER_control_outer_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.cpp index 9fd23136c3..6b75ee8763 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.cpp @@ -1,498 +1,498 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: estimation_velocity.cpp -// -// Code generated for Simulink model 'estimation_velocity'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:27 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "estimation_velocity.h" -#include "estimation_velocity_types.h" -#include "rtwtypes.h" -#include -#include -#include "mw_cmsis.h" -#include "rt_hypotf_snf.h" -#include "estimation_velocity_private.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -MdlrefDW_estimation_velocity_T estimation_velocity_MdlrefDW; - -// Block states (default storage) -DW_estimation_velocity_f_T estimation_velocity_DW; - -// Forward declaration for local functions -static real32_T estimation_velocity_xnrm2(int32_T n, const real32_T x[32], - int32_T ix0); -static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], - real32_T tau[2], int32_T jpvt[2]); -static real32_T estimation_velocity_xnrm2(int32_T n, const real32_T x[32], - int32_T ix0) -{ - real32_T absxk; - real32_T y; - y = 0.0F; - if (n < 1) { - } else if (n == 1) { - y = std::abs(x[ix0 - 1]); - } else { - int32_T kend; - real32_T scale; - scale = 1.29246971E-26F; - kend = ix0 + n; - for (int32_T k = ix0; k < kend; k++) { - absxk = std::abs(x[k - 1]); - if (absxk > scale) { - real32_T t; - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - real32_T t; - t = absxk / scale; - y += t * t; - } - } - - mw_arm_sqrt_f32(y, &absxk); - y = scale * absxk; - } - - return y; -} - -static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], - real32_T tau[2], int32_T jpvt[2]) -{ - int32_T ix; - int32_T lastv; - real32_T vn1[2]; - real32_T vn2[2]; - real32_T work[2]; - real32_T absxk; - real32_T b_atmp; - real32_T scale; - real32_T t; - for (int32_T b_k = 0; b_k < 2; b_k++) { - jpvt[b_k] = b_k + 1; - } - - tau[0] = 0.0F; - tau[1] = 0.0F; - std::memcpy(&b_A[0], &A[0], sizeof(real32_T) << 5U); - work[0] = 0.0F; - work[1] = 0.0F; - for (int32_T b_k = 0; b_k < 2; b_k++) { - ix = b_k << 4; - b_atmp = 0.0F; - scale = 1.29246971E-26F; - for (lastv = ix + 1; lastv <= ix + 16; lastv++) { - absxk = std::abs(A[lastv - 1]); - if (absxk > scale) { - t = scale / absxk; - b_atmp = b_atmp * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - b_atmp += t * t; - } - } - - mw_arm_sqrt_f32(b_atmp, &b_atmp); - b_atmp *= scale; - vn2[b_k] = b_atmp; - vn1[b_k] = b_atmp; - } - - for (int32_T b_k = 0; b_k < 2; b_k++) { - int32_T i; - int32_T jy; - int32_T kend; - int32_T kend_tmp; - int32_T knt; - int32_T nmip1; - i = b_k + 1; - kend_tmp = b_k << 4; - kend = kend_tmp + b_k; - nmip1 = 2 - b_k; - knt = 0; - if (2 - b_k > 1) { - scale = vn1[b_k]; - for (lastv = 2; lastv <= nmip1; lastv++) { - absxk = vn1[b_k + 1]; - if (absxk > scale) { - knt = 1; - scale = absxk; - } - } - } - - nmip1 = b_k + knt; - if (nmip1 + 1 != b_k + 1) { - ix = nmip1 << 4; - for (lastv = 0; lastv < 16; lastv++) { - knt = ix + lastv; - scale = b_A[knt]; - jy = kend_tmp + lastv; - b_A[knt] = b_A[jy]; - b_A[jy] = scale; - } - - ix = jpvt[nmip1]; - jpvt[nmip1] = jpvt[b_k]; - jpvt[b_k] = ix; - vn1[nmip1] = vn1[b_k]; - vn2[nmip1] = vn2[b_k]; - } - - ix = kend + 2; - b_atmp = b_A[kend]; - tau[b_k] = 0.0F; - scale = estimation_velocity_xnrm2(15 - b_k, b_A, kend + 2); - if (scale != 0.0F) { - scale = rt_hypotf_snf(b_A[kend], scale); - if (b_A[kend] >= 0.0F) { - scale = -scale; - } - - if (std::abs(scale) < 9.86076132E-32F) { - knt = -1; - do { - knt++; - nmip1 = (kend - b_k) - 1; - for (lastv = ix; lastv <= nmip1 + 17; lastv++) { - b_A[lastv - 1] *= 1.01412048E+31F; - } - - scale *= 1.01412048E+31F; - b_atmp *= 1.01412048E+31F; - } while ((std::abs(scale) < 9.86076132E-32F) && (knt + 1 < 20)); - - scale = rt_hypotf_snf(b_atmp, estimation_velocity_xnrm2(15 - b_k, b_A, - kend + 2)); - if (b_atmp >= 0.0F) { - scale = -scale; - } - - tau[b_k] = (scale - b_atmp) / scale; - b_atmp = 1.0F / (b_atmp - scale); - nmip1 = (kend - b_k) - 1; - for (lastv = ix; lastv <= nmip1 + 17; lastv++) { - b_A[lastv - 1] *= b_atmp; - } - - for (lastv = 0; lastv <= knt; lastv++) { - scale *= 9.86076132E-32F; - } - - b_atmp = scale; - } else { - tau[b_k] = (scale - b_A[kend]) / scale; - b_atmp = 1.0F / (b_A[kend] - scale); - nmip1 = (kend - b_k) - 1; - for (lastv = ix; lastv <= nmip1 + 17; lastv++) { - b_A[lastv - 1] *= b_atmp; - } - - b_atmp = scale; - } - } - - b_A[kend] = b_atmp; - if (b_k + 1 < 2) { - b_atmp = b_A[kend]; - b_A[kend] = 1.0F; - kend_tmp = kend + 17; - if (tau[0] != 0.0F) { - boolean_T exitg2; - lastv = 16; - ix = kend - 1; - while ((lastv > 0) && (b_A[ix + 16] == 0.0F)) { - lastv--; - ix--; - } - - knt = 0; - exitg2 = false; - while ((!exitg2) && (knt + 1 > 0)) { - int32_T exitg1; - ix = (knt << 4) + kend; - jy = ix + 17; - do { - exitg1 = 0; - if (jy <= (ix + lastv) + 16) { - if (b_A[jy - 1] != 0.0F) { - exitg1 = 1; - } else { - jy++; - } - } else { - knt--; - exitg1 = 2; - } - } while (exitg1 == 0); - - if (exitg1 == 1) { - exitg2 = true; - } - } - } else { - lastv = 0; - knt = -1; - } - - if (lastv > 0) { - int32_T d; - if (knt + 1 != 0) { - if (knt >= 0) { - std::memset(&work[0], 0, static_cast(knt + 1) * sizeof - (real32_T)); - } - - nmip1 = (knt << 4) + kend; - for (ix = kend_tmp; ix <= nmip1 + 17; ix += 16) { - scale = 0.0F; - d = ix + lastv; - for (jy = ix; jy < d; jy++) { - scale += b_A[(kend + jy) - ix] * b_A[jy - 1]; - } - - jy = ((ix - kend) - 17) >> 4; - work[jy] += scale; - } - } - - if (!(-tau[0] == 0.0F)) { - jy = kend; - for (kend_tmp = 0; kend_tmp <= knt; kend_tmp++) { - scale = work[kend_tmp]; - if (scale != 0.0F) { - scale *= -tau[0]; - nmip1 = jy + 17; - ix = (lastv + jy) + 16; - for (d = nmip1; d <= ix; d++) { - b_A[d - 1] += b_A[((kend + d) - jy) - 17] * scale; - } - } - - jy += 16; - } - } - } - - b_A[kend] = b_atmp; - } - - for (nmip1 = i + 1; nmip1 < 3; nmip1++) { - absxk = vn2[1]; - b_atmp = vn1[1]; - if (vn1[1] != 0.0F) { - b_atmp = std::abs(b_A[b_k + 16]) / vn1[1]; - b_atmp = 1.0F - b_atmp * b_atmp; - if (b_atmp < 0.0F) { - b_atmp = 0.0F; - } - - scale = vn1[1] / vn2[1]; - scale = scale * scale * b_atmp; - if (scale <= 0.000345266977F) { - ix = b_k + 17; - b_atmp = 0.0F; - scale = 1.29246971E-26F; - for (lastv = ix + 1; lastv < 33; lastv++) { - absxk = std::abs(b_A[lastv - 1]); - if (absxk > scale) { - t = scale / absxk; - b_atmp = b_atmp * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - b_atmp += t * t; - } - } - - mw_arm_sqrt_f32(b_atmp, &b_atmp); - b_atmp *= scale; - absxk = b_atmp; - } else { - mw_arm_sqrt_f32(b_atmp, &b_atmp); - b_atmp *= vn1[1]; - } - } - - vn1[1] = b_atmp; - vn2[1] = absxk; - } - } -} - -// System initialize for referenced model: 'estimation_velocity' -void estimation_velocity_Init(void) -{ - // Start for MATLABSystem: '/QR Solver' - estimation_velocity_DW.objisempty = true; -} - -// Output and update for referenced model: 'estimation_velocity' -void estimation_velocity(const SensorsData *rtu_SensorsData, const - ConfigurationParameters *rtu_ConfigurationParameters, JointVelocities - *rty_EstimatedVelocity) -{ - int32_T jpvt[2]; - int32_T i; - int32_T rankA; - real32_T b_A[32]; - real32_T rtb_DelayLine[16]; - real32_T rtb_QRSolver_0[2]; - real32_T tau[2]; - real32_T tol; - - // S-Function (sdspsreg2): '/Delay Line' - for (rankA = 0; rankA < 15 - estimation_velocity_DW.DelayLine_BUFF_OFFSET; - rankA++) { - rtb_DelayLine[rankA] = - estimation_velocity_DW.DelayLine_Buff[estimation_velocity_DW.DelayLine_BUFF_OFFSET - + rankA]; - } - - for (rankA = 0; rankA < estimation_velocity_DW.DelayLine_BUFF_OFFSET; rankA++) - { - rtb_DelayLine[(rankA - estimation_velocity_DW.DelayLine_BUFF_OFFSET) + 15] = - estimation_velocity_DW.DelayLine_Buff[rankA]; - } - - rtb_DelayLine[15] = rtu_SensorsData->jointpositions.position; - - // MATLABSystem: '/QR Solver' incorporates: - // Constant: '/Constant' - - estimation_velocity_xgeqp3(rtCP_Constant_Value_c, b_A, tau, jpvt); - rankA = 0; - tol = 1.90734863E-5F * std::abs(b_A[0]); - while ((rankA < 2) && (!(std::abs(b_A[(rankA << 4) + rankA]) <= tol))) { - rankA++; - } - - for (int32_T i_0 = 0; i_0 < 2; i_0++) { - real32_T tau_0; - tau_0 = tau[i_0]; - rtb_QRSolver_0[i_0] = 0.0F; - if (tau_0 != 0.0F) { - tol = rtb_DelayLine[i_0]; - for (i = i_0 + 2; i < 17; i++) { - tol += b_A[((i_0 << 4) + i) - 1] * rtb_DelayLine[i - 1]; - } - - tol *= tau_0; - if (tol != 0.0F) { - rtb_DelayLine[i_0] -= tol; - for (i = i_0 + 2; i < 17; i++) { - rtb_DelayLine[i - 1] -= b_A[((i_0 << 4) + i) - 1] * tol; - } - } - } - } - - for (i = 0; i < rankA; i++) { - rtb_QRSolver_0[jpvt[i] - 1] = rtb_DelayLine[i]; - } - - for (int32_T i_0 = rankA; i_0 >= 1; i_0--) { - int32_T rtb_QRSolver_tmp; - int32_T rtb_QRSolver_tmp_0; - rtb_QRSolver_tmp = jpvt[i_0 - 1] - 1; - rtb_QRSolver_tmp_0 = (i_0 - 1) << 4; - rtb_QRSolver_0[rtb_QRSolver_tmp] /= b_A[(rtb_QRSolver_tmp_0 + i_0) - 1]; - i = i_0 - 2; - for (int32_T bIndx = 0; bIndx <= i; bIndx++) { - rtb_QRSolver_0[jpvt[0] - 1] -= rtb_QRSolver_0[rtb_QRSolver_tmp] * - b_A[rtb_QRSolver_tmp_0]; - } - } - - // MultiPortSwitch: '/Index Vector' incorporates: - // Constant: '/Constant' - // Delay: '/Delay' - // Gain: '/Gain' - // MATLABSystem: '/QR Solver' - // S-Function (sdspsreg2): '/Delay Line' - // Sum: '/Sum' - - switch (rtu_ConfigurationParameters->estimationconfig.velocity_mode) { - case EstimationVelocityModes_Disabled: - rty_EstimatedVelocity->velocity = 0.0F; - break; - - case EstimationVelocityModes_MovingAverage: - rty_EstimatedVelocity->velocity = (rtu_SensorsData->jointpositions.position - - estimation_velocity_DW.Delay_DSTATE[estimation_velocity_DW.CircBufIdx]) * - 62.5F; - break; - - default: - rty_EstimatedVelocity->velocity = rtb_QRSolver_0[0]; - break; - } - - // End of MultiPortSwitch: '/Index Vector' - - // Update for S-Function (sdspsreg2): '/Delay Line' - estimation_velocity_DW.DelayLine_Buff[estimation_velocity_DW.DelayLine_BUFF_OFFSET] - = rtu_SensorsData->jointpositions.position; - estimation_velocity_DW.DelayLine_BUFF_OFFSET++; - while (estimation_velocity_DW.DelayLine_BUFF_OFFSET >= 15) { - estimation_velocity_DW.DelayLine_BUFF_OFFSET -= 15; - } - - // End of Update for S-Function (sdspsreg2): '/Delay Line' - - // Update for Delay: '/Delay' incorporates: - // S-Function (sdspsreg2): '/Delay Line' - - estimation_velocity_DW.Delay_DSTATE[estimation_velocity_DW.CircBufIdx] = - rtu_SensorsData->jointpositions.position; - if (estimation_velocity_DW.CircBufIdx < 15U) { - estimation_velocity_DW.CircBufIdx++; - } else { - estimation_velocity_DW.CircBufIdx = 0U; - } - - // End of Update for Delay: '/Delay' -} - -// Model initialize function -void estimation_velocity_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_estimation_velocity_T *const estimation_velocity_M = - &(estimation_velocity_MdlrefDW.rtm); - - // Registration code - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // initialize error status - rtmSetErrorStatusPointer(estimation_velocity_M, rt_errorStatus); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: estimation_velocity.cpp +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:07 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "estimation_velocity.h" +#include "estimation_velocity_types.h" +#include "rtwtypes.h" +#include +#include +#include "mw_cmsis.h" +#include "rt_hypotf_snf.h" +#include "estimation_velocity_private.h" + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +MdlrefDW_estimation_velocity_T estimation_velocity_MdlrefDW; + +// Block states (default storage) +DW_estimation_velocity_f_T estimation_velocity_DW; + +// Forward declaration for local functions +static real32_T estimation_velocity_xnrm2(int32_T n, const real32_T x[32], + int32_T ix0); +static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], + real32_T tau[2], int32_T jpvt[2]); +static real32_T estimation_velocity_xnrm2(int32_T n, const real32_T x[32], + int32_T ix0) +{ + real32_T absxk; + real32_T y; + y = 0.0F; + if (n < 1) { + } else if (n == 1) { + y = std::abs(x[ix0 - 1]); + } else { + int32_T kend; + real32_T scale; + scale = 1.29246971E-26F; + kend = ix0 + n; + for (int32_T k = ix0; k < kend; k++) { + absxk = std::abs(x[k - 1]); + if (absxk > scale) { + real32_T t; + t = scale / absxk; + y = y * t * t + 1.0F; + scale = absxk; + } else { + real32_T t; + t = absxk / scale; + y += t * t; + } + } + + mw_arm_sqrt_f32(y, &absxk); + y = scale * absxk; + } + + return y; +} + +static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], + real32_T tau[2], int32_T jpvt[2]) +{ + int32_T ix; + int32_T lastv; + real32_T vn1[2]; + real32_T vn2[2]; + real32_T work[2]; + real32_T absxk; + real32_T b_atmp; + real32_T scale; + real32_T t; + for (int32_T b_k = 0; b_k < 2; b_k++) { + jpvt[b_k] = b_k + 1; + } + + tau[0] = 0.0F; + tau[1] = 0.0F; + std::memcpy(&b_A[0], &A[0], sizeof(real32_T) << 5U); + work[0] = 0.0F; + work[1] = 0.0F; + for (int32_T b_k = 0; b_k < 2; b_k++) { + ix = b_k << 4; + b_atmp = 0.0F; + scale = 1.29246971E-26F; + for (lastv = ix + 1; lastv <= ix + 16; lastv++) { + absxk = std::abs(A[lastv - 1]); + if (absxk > scale) { + t = scale / absxk; + b_atmp = b_atmp * t * t + 1.0F; + scale = absxk; + } else { + t = absxk / scale; + b_atmp += t * t; + } + } + + mw_arm_sqrt_f32(b_atmp, &b_atmp); + b_atmp *= scale; + vn2[b_k] = b_atmp; + vn1[b_k] = b_atmp; + } + + for (int32_T b_k = 0; b_k < 2; b_k++) { + int32_T i; + int32_T jy; + int32_T kend; + int32_T kend_tmp; + int32_T knt; + int32_T nmip1; + i = b_k + 1; + kend_tmp = b_k << 4; + kend = kend_tmp + b_k; + nmip1 = 2 - b_k; + knt = 0; + if (2 - b_k > 1) { + scale = vn1[b_k]; + for (lastv = 2; lastv <= nmip1; lastv++) { + absxk = vn1[b_k + 1]; + if (absxk > scale) { + knt = 1; + scale = absxk; + } + } + } + + nmip1 = b_k + knt; + if (nmip1 + 1 != b_k + 1) { + ix = nmip1 << 4; + for (lastv = 0; lastv < 16; lastv++) { + knt = ix + lastv; + scale = b_A[knt]; + jy = kend_tmp + lastv; + b_A[knt] = b_A[jy]; + b_A[jy] = scale; + } + + ix = jpvt[nmip1]; + jpvt[nmip1] = jpvt[b_k]; + jpvt[b_k] = ix; + vn1[nmip1] = vn1[b_k]; + vn2[nmip1] = vn2[b_k]; + } + + ix = kend + 2; + b_atmp = b_A[kend]; + tau[b_k] = 0.0F; + scale = estimation_velocity_xnrm2(15 - b_k, b_A, kend + 2); + if (scale != 0.0F) { + scale = rt_hypotf_snf(b_A[kend], scale); + if (b_A[kend] >= 0.0F) { + scale = -scale; + } + + if (std::abs(scale) < 9.86076132E-32F) { + knt = -1; + do { + knt++; + nmip1 = (kend - b_k) - 1; + for (lastv = ix; lastv <= nmip1 + 17; lastv++) { + b_A[lastv - 1] *= 1.01412048E+31F; + } + + scale *= 1.01412048E+31F; + b_atmp *= 1.01412048E+31F; + } while ((std::abs(scale) < 9.86076132E-32F) && (knt + 1 < 20)); + + scale = rt_hypotf_snf(b_atmp, estimation_velocity_xnrm2(15 - b_k, b_A, + kend + 2)); + if (b_atmp >= 0.0F) { + scale = -scale; + } + + tau[b_k] = (scale - b_atmp) / scale; + b_atmp = 1.0F / (b_atmp - scale); + nmip1 = (kend - b_k) - 1; + for (lastv = ix; lastv <= nmip1 + 17; lastv++) { + b_A[lastv - 1] *= b_atmp; + } + + for (lastv = 0; lastv <= knt; lastv++) { + scale *= 9.86076132E-32F; + } + + b_atmp = scale; + } else { + tau[b_k] = (scale - b_A[kend]) / scale; + b_atmp = 1.0F / (b_A[kend] - scale); + nmip1 = (kend - b_k) - 1; + for (lastv = ix; lastv <= nmip1 + 17; lastv++) { + b_A[lastv - 1] *= b_atmp; + } + + b_atmp = scale; + } + } + + b_A[kend] = b_atmp; + if (b_k + 1 < 2) { + b_atmp = b_A[kend]; + b_A[kend] = 1.0F; + kend_tmp = kend + 17; + if (tau[0] != 0.0F) { + boolean_T exitg2; + lastv = 16; + ix = kend - 1; + while ((lastv > 0) && (b_A[ix + 16] == 0.0F)) { + lastv--; + ix--; + } + + knt = 0; + exitg2 = false; + while ((!exitg2) && (knt + 1 > 0)) { + int32_T exitg1; + ix = (knt << 4) + kend; + jy = ix + 17; + do { + exitg1 = 0; + if (jy <= (ix + lastv) + 16) { + if (b_A[jy - 1] != 0.0F) { + exitg1 = 1; + } else { + jy++; + } + } else { + knt--; + exitg1 = 2; + } + } while (exitg1 == 0); + + if (exitg1 == 1) { + exitg2 = true; + } + } + } else { + lastv = 0; + knt = -1; + } + + if (lastv > 0) { + int32_T d; + if (knt + 1 != 0) { + if (knt >= 0) { + std::memset(&work[0], 0, static_cast(knt + 1) * sizeof + (real32_T)); + } + + nmip1 = (knt << 4) + kend; + for (ix = kend_tmp; ix <= nmip1 + 17; ix += 16) { + scale = 0.0F; + d = ix + lastv; + for (jy = ix; jy < d; jy++) { + scale += b_A[(kend + jy) - ix] * b_A[jy - 1]; + } + + jy = ((ix - kend) - 17) >> 4; + work[jy] += scale; + } + } + + if (!(-tau[0] == 0.0F)) { + jy = kend; + for (kend_tmp = 0; kend_tmp <= knt; kend_tmp++) { + scale = work[kend_tmp]; + if (scale != 0.0F) { + scale *= -tau[0]; + nmip1 = jy + 17; + ix = (lastv + jy) + 16; + for (d = nmip1; d <= ix; d++) { + b_A[d - 1] += b_A[((kend + d) - jy) - 17] * scale; + } + } + + jy += 16; + } + } + } + + b_A[kend] = b_atmp; + } + + for (nmip1 = i + 1; nmip1 < 3; nmip1++) { + absxk = vn2[1]; + b_atmp = vn1[1]; + if (vn1[1] != 0.0F) { + b_atmp = std::abs(b_A[b_k + 16]) / vn1[1]; + b_atmp = 1.0F - b_atmp * b_atmp; + if (b_atmp < 0.0F) { + b_atmp = 0.0F; + } + + scale = vn1[1] / vn2[1]; + scale = scale * scale * b_atmp; + if (scale <= 0.000345266977F) { + ix = b_k + 17; + b_atmp = 0.0F; + scale = 1.29246971E-26F; + for (lastv = ix + 1; lastv < 33; lastv++) { + absxk = std::abs(b_A[lastv - 1]); + if (absxk > scale) { + t = scale / absxk; + b_atmp = b_atmp * t * t + 1.0F; + scale = absxk; + } else { + t = absxk / scale; + b_atmp += t * t; + } + } + + mw_arm_sqrt_f32(b_atmp, &b_atmp); + b_atmp *= scale; + absxk = b_atmp; + } else { + mw_arm_sqrt_f32(b_atmp, &b_atmp); + b_atmp *= vn1[1]; + } + } + + vn1[1] = b_atmp; + vn2[1] = absxk; + } + } +} + +// System initialize for referenced model: 'estimation_velocity' +void estimation_velocity_Init(void) +{ + // Start for MATLABSystem: '/QR Solver' + estimation_velocity_DW.objisempty = true; +} + +// Output and update for referenced model: 'estimation_velocity' +void estimation_velocity(const SensorsData *rtu_SensorsData, const + ConfigurationParameters *rtu_ConfigurationParameters, JointVelocities + *rty_EstimatedVelocity) +{ + int32_T jpvt[2]; + int32_T i; + int32_T rankA; + real32_T b_A[32]; + real32_T rtb_DelayLine[16]; + real32_T rtb_QRSolver_0[2]; + real32_T tau[2]; + real32_T tol; + + // S-Function (sdspsreg2): '/Delay Line' + for (rankA = 0; rankA < 15 - estimation_velocity_DW.DelayLine_BUFF_OFFSET; + rankA++) { + rtb_DelayLine[rankA] = + estimation_velocity_DW.DelayLine_Buff[estimation_velocity_DW.DelayLine_BUFF_OFFSET + + rankA]; + } + + for (rankA = 0; rankA < estimation_velocity_DW.DelayLine_BUFF_OFFSET; rankA++) + { + rtb_DelayLine[(rankA - estimation_velocity_DW.DelayLine_BUFF_OFFSET) + 15] = + estimation_velocity_DW.DelayLine_Buff[rankA]; + } + + rtb_DelayLine[15] = rtu_SensorsData->jointpositions.position; + + // MATLABSystem: '/QR Solver' incorporates: + // Constant: '/Constant' + + estimation_velocity_xgeqp3(rtCP_Constant_Value_c, b_A, tau, jpvt); + rankA = 0; + tol = 1.90734863E-5F * std::abs(b_A[0]); + while ((rankA < 2) && (!(std::abs(b_A[(rankA << 4) + rankA]) <= tol))) { + rankA++; + } + + for (int32_T i_0 = 0; i_0 < 2; i_0++) { + real32_T tau_0; + tau_0 = tau[i_0]; + rtb_QRSolver_0[i_0] = 0.0F; + if (tau_0 != 0.0F) { + tol = rtb_DelayLine[i_0]; + for (i = i_0 + 2; i < 17; i++) { + tol += b_A[((i_0 << 4) + i) - 1] * rtb_DelayLine[i - 1]; + } + + tol *= tau_0; + if (tol != 0.0F) { + rtb_DelayLine[i_0] -= tol; + for (i = i_0 + 2; i < 17; i++) { + rtb_DelayLine[i - 1] -= b_A[((i_0 << 4) + i) - 1] * tol; + } + } + } + } + + for (i = 0; i < rankA; i++) { + rtb_QRSolver_0[jpvt[i] - 1] = rtb_DelayLine[i]; + } + + for (int32_T i_0 = rankA; i_0 >= 1; i_0--) { + int32_T rtb_QRSolver_tmp; + int32_T rtb_QRSolver_tmp_0; + rtb_QRSolver_tmp = jpvt[i_0 - 1] - 1; + rtb_QRSolver_tmp_0 = (i_0 - 1) << 4; + rtb_QRSolver_0[rtb_QRSolver_tmp] /= b_A[(rtb_QRSolver_tmp_0 + i_0) - 1]; + i = i_0 - 2; + for (int32_T bIndx = 0; bIndx <= i; bIndx++) { + rtb_QRSolver_0[jpvt[0] - 1] -= rtb_QRSolver_0[rtb_QRSolver_tmp] * + b_A[rtb_QRSolver_tmp_0]; + } + } + + // MultiPortSwitch: '/Index Vector' incorporates: + // Constant: '/Constant' + // Delay: '/Delay' + // Gain: '/Gain' + // MATLABSystem: '/QR Solver' + // S-Function (sdspsreg2): '/Delay Line' + // Sum: '/Sum' + + switch (rtu_ConfigurationParameters->estimationconfig.velocity_mode) { + case EstimationVelocityModes_Disabled: + rty_EstimatedVelocity->velocity = 0.0F; + break; + + case EstimationVelocityModes_MovingAverage: + rty_EstimatedVelocity->velocity = (rtu_SensorsData->jointpositions.position + - estimation_velocity_DW.Delay_DSTATE[estimation_velocity_DW.CircBufIdx]) * + 62.5F; + break; + + default: + rty_EstimatedVelocity->velocity = rtb_QRSolver_0[0]; + break; + } + + // End of MultiPortSwitch: '/Index Vector' + + // Update for S-Function (sdspsreg2): '/Delay Line' + estimation_velocity_DW.DelayLine_Buff[estimation_velocity_DW.DelayLine_BUFF_OFFSET] + = rtu_SensorsData->jointpositions.position; + estimation_velocity_DW.DelayLine_BUFF_OFFSET++; + while (estimation_velocity_DW.DelayLine_BUFF_OFFSET >= 15) { + estimation_velocity_DW.DelayLine_BUFF_OFFSET -= 15; + } + + // End of Update for S-Function (sdspsreg2): '/Delay Line' + + // Update for Delay: '/Delay' incorporates: + // S-Function (sdspsreg2): '/Delay Line' + + estimation_velocity_DW.Delay_DSTATE[estimation_velocity_DW.CircBufIdx] = + rtu_SensorsData->jointpositions.position; + if (estimation_velocity_DW.CircBufIdx < 15U) { + estimation_velocity_DW.CircBufIdx++; + } else { + estimation_velocity_DW.CircBufIdx = 0U; + } + + // End of Update for Delay: '/Delay' +} + +// Model initialize function +void estimation_velocity_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_estimation_velocity_T *const estimation_velocity_M = + &(estimation_velocity_MdlrefDW.rtm); + + // Registration code + + // initialize non-finites + rt_InitInfAndNaN(sizeof(real_T)); + + // initialize error status + rtmSetErrorStatusPointer(estimation_velocity_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.h index 3c9abbd6ae..5b57c99298 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity.h @@ -1,118 +1,118 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: estimation_velocity.h -// -// Code generated for Simulink model 'estimation_velocity'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:27 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_estimation_velocity_h_ -#define RTW_HEADER_estimation_velocity_h_ -#include "rtwtypes.h" -#include "estimation_velocity_types.h" -#include "rtGetInf.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -// Block states (default storage) for model 'estimation_velocity' -#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ - -struct DW_estimation_velocity_f_T { - real32_T DelayLine_Buff[15]; // '/Delay Line' - real32_T Delay_DSTATE[16]; // '/Delay' - int32_T DelayLine_BUFF_OFFSET; // '/Delay Line' - uint32_T CircBufIdx; // '/Delay' - dsp_simulink_QRSolver_estimat_T obj; // '/QR Solver' - boolean_T objisempty; // '/QR Solver' -}; - -#endif //estimation_velocity_MDLREF_HIDE_CHILD_ - -#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_estimation_velocity_T { - const char_T **errorStatus; -}; - -#endif //estimation_velocity_MDLREF_HIDE_CHILD_ - -#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_estimation_velocity_T { - RT_MODEL_estimation_velocity_T rtm; -}; - -#endif //estimation_velocity_MDLREF_HIDE_CHILD_ - -extern void estimation_velocity_Init(void); -extern void estimation_velocity(const SensorsData *rtu_SensorsData, const - ConfigurationParameters *rtu_ConfigurationParameters, JointVelocities - *rty_EstimatedVelocity); - -// Model reference registration function -extern void estimation_velocity_initialize(const char_T **rt_errorStatus); - -#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_estimation_velocity_T estimation_velocity_MdlrefDW; - -#endif //estimation_velocity_MDLREF_HIDE_CHILD_ - -#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ - -// Block states (default storage) -extern DW_estimation_velocity_f_T estimation_velocity_DW; - -#endif //estimation_velocity_MDLREF_HIDE_CHILD_ - -//- -// These blocks were eliminated from the model due to optimizations: -// -// Block '/Check Signal Attributes' : Unused code path elimination -// Block '/Check Signal Attributes' : Unused code path elimination -// Block '/Check Signal Attributes' : Unused code path elimination - - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'estimation_velocity' -// '' : 'estimation_velocity/Least Squares Polynomial Fit' -// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes' -// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes1' -// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes2' - -#endif // RTW_HEADER_estimation_velocity_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: estimation_velocity.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:07 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_estimation_velocity_h_ +#define RTW_HEADER_estimation_velocity_h_ +#include "rtwtypes.h" +#include "estimation_velocity_types.h" +#include "rtGetInf.h" + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +// Block states (default storage) for model 'estimation_velocity' +#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ + +struct DW_estimation_velocity_f_T { + real32_T DelayLine_Buff[15]; // '/Delay Line' + real32_T Delay_DSTATE[16]; // '/Delay' + int32_T DelayLine_BUFF_OFFSET; // '/Delay Line' + uint32_T CircBufIdx; // '/Delay' + dsp_simulink_QRSolver_estimat_T obj; // '/QR Solver' + boolean_T objisempty; // '/QR Solver' +}; + +#endif //estimation_velocity_MDLREF_HIDE_CHILD_ + +#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_estimation_velocity_T { + const char_T **errorStatus; +}; + +#endif //estimation_velocity_MDLREF_HIDE_CHILD_ + +#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_estimation_velocity_T { + RT_MODEL_estimation_velocity_T rtm; +}; + +#endif //estimation_velocity_MDLREF_HIDE_CHILD_ + +extern void estimation_velocity_Init(void); +extern void estimation_velocity(const SensorsData *rtu_SensorsData, const + ConfigurationParameters *rtu_ConfigurationParameters, JointVelocities + *rty_EstimatedVelocity); + +// Model reference registration function +extern void estimation_velocity_initialize(const char_T **rt_errorStatus); + +#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_estimation_velocity_T estimation_velocity_MdlrefDW; + +#endif //estimation_velocity_MDLREF_HIDE_CHILD_ + +#ifndef estimation_velocity_MDLREF_HIDE_CHILD_ + +// Block states (default storage) +extern DW_estimation_velocity_f_T estimation_velocity_DW; + +#endif //estimation_velocity_MDLREF_HIDE_CHILD_ + +//- +// These blocks were eliminated from the model due to optimizations: +// +// Block '/Check Signal Attributes' : Unused code path elimination +// Block '/Check Signal Attributes' : Unused code path elimination +// Block '/Check Signal Attributes' : Unused code path elimination + + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'estimation_velocity' +// '' : 'estimation_velocity/Least Squares Polynomial Fit' +// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes' +// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes1' +// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes2' + +#endif // RTW_HEADER_estimation_velocity_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_private.h index 0372dc1bcb..98fcd74d6b 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_private.h @@ -1,52 +1,52 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: estimation_velocity_private.h -// -// Code generated for Simulink model 'estimation_velocity'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:27 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_estimation_velocity_private_h_ -#define RTW_HEADER_estimation_velocity_private_h_ -#include "rtwtypes.h" -#include "estimation_velocity_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -extern const real32_T rtCP_pooled_Az3IVI54Pn7X[32]; - -#define rtCP_Constant_Value_c rtCP_pooled_Az3IVI54Pn7X // Computed Parameter: rtCP_Constant_Value_c - // Referenced by: '/Constant' - -#endif // RTW_HEADER_estimation_velocity_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: estimation_velocity_private.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:07 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_estimation_velocity_private_h_ +#define RTW_HEADER_estimation_velocity_private_h_ +#include "rtwtypes.h" +#include "estimation_velocity_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +extern const real32_T rtCP_pooled_Az3IVI54Pn7X[32]; + +#define rtCP_Constant_Value_c rtCP_pooled_Az3IVI54Pn7X // Computed Parameter: rtCP_Constant_Value_c + // Referenced by: '/Constant' + +#endif // RTW_HEADER_estimation_velocity_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_types.h index 2a46cd1705..842159f48e 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/estimator/estimation_velocity_types.h @@ -1,225 +1,225 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: estimation_velocity_types.h -// -// Code generated for Simulink model 'estimation_velocity'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:27 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_estimation_velocity_types_h_ -#define RTW_HEADER_estimation_velocity_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ - -struct SensorsData -{ - // position encoders - JointPositions jointpositions; - MotorSensors motorsensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef struct_dsp_simulink_QRSolver_estimat_T -#define struct_dsp_simulink_QRSolver_estimat_T - -struct dsp_simulink_QRSolver_estimat_T -{ - int32_T isInitialized; -}; - -#endif // struct_dsp_simulink_QRSolver_estimat_T - -// Forward declaration for rtModel -typedef struct tag_RTM_estimation_velocity_T RT_MODEL_estimation_velocity_T; - -#endif // RTW_HEADER_estimation_velocity_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: estimation_velocity_types.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:07 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_estimation_velocity_types_h_ +#define RTW_HEADER_estimation_velocity_types_h_ +#include "rtwtypes.h" +#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ +#define DEFINED_TYPEDEF_FOR_JointPositions_ + +struct JointPositions +{ + // joint positions + real32_T position; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ +#define DEFINED_TYPEDEF_FOR_JointVelocities_ + +struct JointVelocities +{ + // joint velocities + real32_T velocity; +}; + +#endif + +#ifndef struct_dsp_simulink_QRSolver_estimat_T +#define struct_dsp_simulink_QRSolver_estimat_T + +struct dsp_simulink_QRSolver_estimat_T +{ + int32_T isInitialized; +}; + +#endif // struct_dsp_simulink_QRSolver_estimat_T + +// Forward declaration for rtModel +typedef struct tag_RTM_estimation_velocity_T RT_MODEL_estimation_velocity_T; + +#endif // RTW_HEADER_estimation_velocity_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.cpp index 3181371c4b..6e320aea47 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.cpp @@ -1,372 +1,372 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: filter_current.cpp -// -// Code generated for Simulink model 'filter_current'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:34 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "filter_current.h" -#include "filter_current_types.h" -#include "rtwtypes.h" -#include -#include -#include "filter_current_private.h" - -MdlrefDW_filter_current_T filter_current_MdlrefDW; - -// Block states (default storage) -DW_filter_current_f_T filter_current_DW; - -// Forward declaration for local functions -static void filter_MedianFilterCG_resetImpl(c_dsp_internal_MedianFilterCG_T *obj); -static void f_MedianFilterCG_trickleDownMax(c_dsp_internal_MedianFilterCG_T *obj, - real32_T i); -static void f_MedianFilterCG_trickleDownMin(c_dsp_internal_MedianFilterCG_T *obj, - real32_T i); -static void filter_MedianFilterCG_resetImpl(c_dsp_internal_MedianFilterCG_T *obj) -{ - real32_T cnt1; - real32_T cnt2; - std::memset(&obj->pBuf[0], 0, sizeof(real32_T) << 5U); - std::memset(&obj->pPos[0], 0, sizeof(real32_T) << 5U); - std::memset(&obj->pHeap[0], 0, sizeof(real32_T) << 5U); - obj->pWinLen = 32.0F; - obj->pIdx = obj->pWinLen; - obj->pMidHeap = std::ceil((obj->pWinLen + 1.0F) / 2.0F); - cnt1 = (obj->pWinLen - 1.0F) / 2.0F; - if (cnt1 < 0.0F) { - obj->pMinHeapLength = std::ceil(cnt1); - } else { - obj->pMinHeapLength = std::floor(cnt1); - } - - cnt1 = obj->pWinLen / 2.0F; - if (cnt1 < 0.0F) { - obj->pMaxHeapLength = std::ceil(cnt1); - } else { - obj->pMaxHeapLength = std::floor(cnt1); - } - - cnt1 = 1.0F; - cnt2 = obj->pWinLen; - for (int32_T i = 0; i < 32; i++) { - if (static_cast(std::fmod(32.0F - static_cast(i), 2.0F)) == - 0) { - obj->pPos[31 - i] = cnt1; - cnt1++; - } else { - obj->pPos[31 - i] = cnt2; - cnt2--; - } - - obj->pHeap[static_cast(obj->pPos[31 - i]) - 1] = 32.0F - - static_cast(i); - } -} - -static void f_MedianFilterCG_trickleDownMax(c_dsp_internal_MedianFilterCG_T *obj, - real32_T i) -{ - boolean_T exitg1; - exitg1 = false; - while ((!exitg1) && (i >= -obj->pMaxHeapLength)) { - real32_T ind2; - real32_T temp_tmp; - real32_T tmp; - real32_T u_tmp; - if ((i < -1.0F) && (i > -obj->pMaxHeapLength) && (obj->pBuf - [static_cast(obj->pHeap[static_cast(i + obj->pMidHeap) - - 1]) - 1] < obj->pBuf[static_cast(obj->pHeap - [static_cast((i - 1.0F) + obj->pMidHeap) - 1]) - 1])) { - i--; - } - - u_tmp = i / 2.0F; - if (u_tmp < 0.0F) { - temp_tmp = std::ceil(u_tmp); - } else { - temp_tmp = std::floor(u_tmp); - } - - ind2 = i + obj->pMidHeap; - tmp = obj->pHeap[static_cast(ind2) - 1]; - if (!(obj->pBuf[static_cast(obj->pHeap[static_cast - (temp_tmp + obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast - (tmp) - 1])) { - exitg1 = true; - } else { - if (u_tmp < 0.0F) { - temp_tmp = std::ceil(u_tmp); - } else { - temp_tmp = std::floor(u_tmp); - } - - u_tmp = temp_tmp + obj->pMidHeap; - temp_tmp = obj->pHeap[static_cast(u_tmp) - 1]; - obj->pHeap[static_cast(u_tmp) - 1] = tmp; - obj->pHeap[static_cast(ind2) - 1] = temp_tmp; - obj->pPos[static_cast(obj->pHeap[static_cast(u_tmp) - 1]) - - 1] = u_tmp; - obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - 1]) - - 1] = ind2; - i *= 2.0F; - } - } -} - -static void f_MedianFilterCG_trickleDownMin(c_dsp_internal_MedianFilterCG_T *obj, - real32_T i) -{ - boolean_T exitg1; - exitg1 = false; - while ((!exitg1) && (i <= obj->pMinHeapLength)) { - real32_T ind1; - real32_T temp; - real32_T tmp; - real32_T u_tmp; - if ((i > 1.0F) && (i < obj->pMinHeapLength) && (obj->pBuf - [static_cast(obj->pHeap[static_cast((i + 1.0F) + - obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast(obj-> - pHeap[static_cast(i + obj->pMidHeap) - 1]) - 1])) { - i++; - } - - u_tmp = i / 2.0F; - if (u_tmp < 0.0F) { - temp = std::ceil(u_tmp); - } else { - temp = std::floor(u_tmp); - } - - ind1 = i + obj->pMidHeap; - tmp = obj->pHeap[static_cast(ind1) - 1]; - if (!(obj->pBuf[static_cast(tmp) - 1] < obj->pBuf - [static_cast(obj->pHeap[static_cast(temp + - obj->pMidHeap) - 1]) - 1])) { - exitg1 = true; - } else { - if (u_tmp < 0.0F) { - temp = std::ceil(u_tmp); - } else { - temp = std::floor(u_tmp); - } - - u_tmp = temp + obj->pMidHeap; - obj->pHeap[static_cast(ind1) - 1] = obj->pHeap - [static_cast(u_tmp) - 1]; - obj->pHeap[static_cast(u_tmp) - 1] = tmp; - obj->pPos[static_cast(obj->pHeap[static_cast(ind1) - 1]) - - 1] = ind1; - obj->pPos[static_cast(obj->pHeap[static_cast(u_tmp) - 1]) - - 1] = u_tmp; - i *= 2.0F; - } - } -} - -// System initialize for referenced model: 'filter_current' -void filter_current_Init(void) -{ - // Start for MATLABSystem: '/Median Filter' - filter_current_DW.obj.matlabCodegenIsDeleted = false; - filter_current_DW.objisempty = true; - filter_current_DW.obj.isInitialized = 1; - filter_current_DW.obj.NumChannels = 1; - filter_current_DW.obj.pMID.isInitialized = 0; - filter_current_DW.obj.isSetupComplete = true; - - // InitializeConditions for MATLABSystem: '/Median Filter' - if (filter_current_DW.obj.pMID.isInitialized == 1) { - filter_MedianFilterCG_resetImpl(&filter_current_DW.obj.pMID); - } - - // End of InitializeConditions for MATLABSystem: '/Median Filter' -} - -// Output and update for referenced model: 'filter_current' -void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent - *rty_FilteredCurrent) -{ - c_dsp_internal_MedianFilterCG_T *obj; - real32_T p; - real32_T vprev; - - // MATLABSystem: '/Median Filter' - obj = &filter_current_DW.obj.pMID; - if (filter_current_DW.obj.pMID.isInitialized != 1) { - filter_current_DW.obj.pMID.isInitialized = 1; - filter_current_DW.obj.pMID.isSetupComplete = true; - filter_MedianFilterCG_resetImpl(&filter_current_DW.obj.pMID); - } - - vprev = filter_current_DW.obj.pMID.pBuf[static_cast - (filter_current_DW.obj.pMID.pIdx) - 1]; - filter_current_DW.obj.pMID.pBuf[static_cast - (filter_current_DW.obj.pMID.pIdx) - 1] = rtu_ControlOutputs->Iq_fbk.current; - p = filter_current_DW.obj.pMID.pPos[static_cast - (filter_current_DW.obj.pMID.pIdx) - 1]; - filter_current_DW.obj.pMID.pIdx++; - if (filter_current_DW.obj.pMID.pWinLen + 1.0F == - filter_current_DW.obj.pMID.pIdx) { - filter_current_DW.obj.pMID.pIdx = 1.0F; - } - - if (p > filter_current_DW.obj.pMID.pMidHeap) { - if (vprev < rtu_ControlOutputs->Iq_fbk.current) { - vprev = p - filter_current_DW.obj.pMID.pMidHeap; - f_MedianFilterCG_trickleDownMin(&filter_current_DW.obj.pMID, vprev * 2.0F); - } else { - boolean_T exitg1; - vprev = p - filter_current_DW.obj.pMID.pMidHeap; - exitg1 = false; - while ((!exitg1) && (vprev > 0.0F)) { - boolean_T flag; - flag = (obj->pBuf[static_cast(obj->pHeap[static_cast - (vprev + obj->pMidHeap) - 1]) - 1] < obj->pBuf - [static_cast(obj->pHeap[static_cast(std::floor - (vprev / 2.0F) + obj->pMidHeap) - 1]) - 1]); - if (!flag) { - exitg1 = true; - } else { - real32_T ind2; - real32_T temp; - p = vprev + obj->pMidHeap; - ind2 = std::floor(vprev / 2.0F) + obj->pMidHeap; - temp = obj->pHeap[static_cast(p) - 1]; - obj->pHeap[static_cast(p) - 1] = obj->pHeap - [static_cast(ind2) - 1]; - obj->pHeap[static_cast(ind2) - 1] = temp; - obj->pPos[static_cast(obj->pHeap[static_cast(p) - 1]) - - 1] = p; - obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - - 1]) - 1] = ind2; - vprev = std::floor(vprev / 2.0F); - } - } - - if (vprev == 0.0F) { - f_MedianFilterCG_trickleDownMax(&filter_current_DW.obj.pMID, -1.0F); - } - } - } else if (p < filter_current_DW.obj.pMID.pMidHeap) { - if (rtu_ControlOutputs->Iq_fbk.current < vprev) { - vprev = p - filter_current_DW.obj.pMID.pMidHeap; - f_MedianFilterCG_trickleDownMax(&filter_current_DW.obj.pMID, vprev * 2.0F); - } else { - boolean_T exitg1; - vprev = p - filter_current_DW.obj.pMID.pMidHeap; - exitg1 = false; - while ((!exitg1) && (vprev < 0.0F)) { - real32_T u_tmp; - boolean_T flag; - u_tmp = vprev / 2.0F; - if (u_tmp < 0.0F) { - p = std::ceil(u_tmp); - } else { - p = -0.0F; - } - - flag = (obj->pBuf[static_cast(obj->pHeap[static_cast(p - + obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast - (obj->pHeap[static_cast(vprev + obj->pMidHeap) - 1]) - - 1]); - if (!flag) { - exitg1 = true; - } else { - real32_T ind2; - real32_T temp; - if (u_tmp < 0.0F) { - p = std::ceil(u_tmp); - } else { - p = -0.0F; - } - - p += obj->pMidHeap; - ind2 = vprev + obj->pMidHeap; - temp = obj->pHeap[static_cast(p) - 1]; - obj->pHeap[static_cast(p) - 1] = obj->pHeap - [static_cast(ind2) - 1]; - obj->pHeap[static_cast(ind2) - 1] = temp; - obj->pPos[static_cast(obj->pHeap[static_cast(p) - 1]) - - 1] = p; - obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - - 1]) - 1] = ind2; - if (u_tmp < 0.0F) { - vprev = std::ceil(u_tmp); - } else { - vprev = -0.0F; - } - } - } - - if (vprev == 0.0F) { - f_MedianFilterCG_trickleDownMin(&filter_current_DW.obj.pMID, 1.0F); - } - } - } else { - if (filter_current_DW.obj.pMID.pMaxHeapLength != 0.0F) { - f_MedianFilterCG_trickleDownMax(&filter_current_DW.obj.pMID, -1.0F); - } - - if (filter_current_DW.obj.pMID.pMinHeapLength > 0.0F) { - f_MedianFilterCG_trickleDownMin(&filter_current_DW.obj.pMID, 1.0F); - } - } - - vprev = filter_current_DW.obj.pMID.pBuf[static_cast - (filter_current_DW.obj.pMID.pHeap[static_cast - (filter_current_DW.obj.pMID.pMidHeap) - 1]) - 1]; - vprev += filter_current_DW.obj.pMID.pBuf[static_cast - (filter_current_DW.obj.pMID.pHeap[static_cast - (filter_current_DW.obj.pMID.pMidHeap - 1.0F) - 1]) - 1]; - rty_FilteredCurrent->current = vprev / 2.0F; - - // End of MATLABSystem: '/Median Filter' -} - -// Termination for referenced model: 'filter_current' -void filter_current_Term(void) -{ - // Terminate for MATLABSystem: '/Median Filter' - if (!filter_current_DW.obj.matlabCodegenIsDeleted) { - filter_current_DW.obj.matlabCodegenIsDeleted = true; - if ((filter_current_DW.obj.isInitialized == 1) && - filter_current_DW.obj.isSetupComplete) { - filter_current_DW.obj.NumChannels = -1; - if (filter_current_DW.obj.pMID.isInitialized == 1) { - filter_current_DW.obj.pMID.isInitialized = 2; - } - } - } - - // End of Terminate for MATLABSystem: '/Median Filter' -} - -// Model initialize function -void filter_current_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_filter_current_T *const filter_current_M = - &(filter_current_MdlrefDW.rtm); - - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(filter_current_M, rt_errorStatus); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: filter_current.cpp +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:17 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "filter_current.h" +#include "filter_current_types.h" +#include "rtwtypes.h" +#include +#include +#include "filter_current_private.h" + +MdlrefDW_filter_current_T filter_current_MdlrefDW; + +// Block states (default storage) +DW_filter_current_f_T filter_current_DW; + +// Forward declaration for local functions +static void filter_MedianFilterCG_resetImpl(c_dsp_internal_MedianFilterCG_T *obj); +static void f_MedianFilterCG_trickleDownMax(c_dsp_internal_MedianFilterCG_T *obj, + real32_T i); +static void f_MedianFilterCG_trickleDownMin(c_dsp_internal_MedianFilterCG_T *obj, + real32_T i); +static void filter_MedianFilterCG_resetImpl(c_dsp_internal_MedianFilterCG_T *obj) +{ + real32_T cnt1; + real32_T cnt2; + std::memset(&obj->pBuf[0], 0, sizeof(real32_T) << 5U); + std::memset(&obj->pPos[0], 0, sizeof(real32_T) << 5U); + std::memset(&obj->pHeap[0], 0, sizeof(real32_T) << 5U); + obj->pWinLen = 32.0F; + obj->pIdx = obj->pWinLen; + obj->pMidHeap = std::ceil((obj->pWinLen + 1.0F) / 2.0F); + cnt1 = (obj->pWinLen - 1.0F) / 2.0F; + if (cnt1 < 0.0F) { + obj->pMinHeapLength = std::ceil(cnt1); + } else { + obj->pMinHeapLength = std::floor(cnt1); + } + + cnt1 = obj->pWinLen / 2.0F; + if (cnt1 < 0.0F) { + obj->pMaxHeapLength = std::ceil(cnt1); + } else { + obj->pMaxHeapLength = std::floor(cnt1); + } + + cnt1 = 1.0F; + cnt2 = obj->pWinLen; + for (int32_T i = 0; i < 32; i++) { + if (static_cast(std::fmod(32.0F - static_cast(i), 2.0F)) == + 0) { + obj->pPos[31 - i] = cnt1; + cnt1++; + } else { + obj->pPos[31 - i] = cnt2; + cnt2--; + } + + obj->pHeap[static_cast(obj->pPos[31 - i]) - 1] = 32.0F - + static_cast(i); + } +} + +static void f_MedianFilterCG_trickleDownMax(c_dsp_internal_MedianFilterCG_T *obj, + real32_T i) +{ + boolean_T exitg1; + exitg1 = false; + while ((!exitg1) && (i >= -obj->pMaxHeapLength)) { + real32_T ind2; + real32_T temp_tmp; + real32_T tmp; + real32_T u_tmp; + if ((i < -1.0F) && (i > -obj->pMaxHeapLength) && (obj->pBuf + [static_cast(obj->pHeap[static_cast(i + obj->pMidHeap) + - 1]) - 1] < obj->pBuf[static_cast(obj->pHeap + [static_cast((i - 1.0F) + obj->pMidHeap) - 1]) - 1])) { + i--; + } + + u_tmp = i / 2.0F; + if (u_tmp < 0.0F) { + temp_tmp = std::ceil(u_tmp); + } else { + temp_tmp = std::floor(u_tmp); + } + + ind2 = i + obj->pMidHeap; + tmp = obj->pHeap[static_cast(ind2) - 1]; + if (!(obj->pBuf[static_cast(obj->pHeap[static_cast + (temp_tmp + obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast + (tmp) - 1])) { + exitg1 = true; + } else { + if (u_tmp < 0.0F) { + temp_tmp = std::ceil(u_tmp); + } else { + temp_tmp = std::floor(u_tmp); + } + + u_tmp = temp_tmp + obj->pMidHeap; + temp_tmp = obj->pHeap[static_cast(u_tmp) - 1]; + obj->pHeap[static_cast(u_tmp) - 1] = tmp; + obj->pHeap[static_cast(ind2) - 1] = temp_tmp; + obj->pPos[static_cast(obj->pHeap[static_cast(u_tmp) - 1]) + - 1] = u_tmp; + obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - 1]) + - 1] = ind2; + i *= 2.0F; + } + } +} + +static void f_MedianFilterCG_trickleDownMin(c_dsp_internal_MedianFilterCG_T *obj, + real32_T i) +{ + boolean_T exitg1; + exitg1 = false; + while ((!exitg1) && (i <= obj->pMinHeapLength)) { + real32_T ind1; + real32_T temp; + real32_T tmp; + real32_T u_tmp; + if ((i > 1.0F) && (i < obj->pMinHeapLength) && (obj->pBuf + [static_cast(obj->pHeap[static_cast((i + 1.0F) + + obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast(obj-> + pHeap[static_cast(i + obj->pMidHeap) - 1]) - 1])) { + i++; + } + + u_tmp = i / 2.0F; + if (u_tmp < 0.0F) { + temp = std::ceil(u_tmp); + } else { + temp = std::floor(u_tmp); + } + + ind1 = i + obj->pMidHeap; + tmp = obj->pHeap[static_cast(ind1) - 1]; + if (!(obj->pBuf[static_cast(tmp) - 1] < obj->pBuf + [static_cast(obj->pHeap[static_cast(temp + + obj->pMidHeap) - 1]) - 1])) { + exitg1 = true; + } else { + if (u_tmp < 0.0F) { + temp = std::ceil(u_tmp); + } else { + temp = std::floor(u_tmp); + } + + u_tmp = temp + obj->pMidHeap; + obj->pHeap[static_cast(ind1) - 1] = obj->pHeap + [static_cast(u_tmp) - 1]; + obj->pHeap[static_cast(u_tmp) - 1] = tmp; + obj->pPos[static_cast(obj->pHeap[static_cast(ind1) - 1]) + - 1] = ind1; + obj->pPos[static_cast(obj->pHeap[static_cast(u_tmp) - 1]) + - 1] = u_tmp; + i *= 2.0F; + } + } +} + +// System initialize for referenced model: 'filter_current' +void filter_current_Init(void) +{ + // Start for MATLABSystem: '/Median Filter' + filter_current_DW.obj.matlabCodegenIsDeleted = false; + filter_current_DW.objisempty = true; + filter_current_DW.obj.isInitialized = 1; + filter_current_DW.obj.NumChannels = 1; + filter_current_DW.obj.pMID.isInitialized = 0; + filter_current_DW.obj.isSetupComplete = true; + + // InitializeConditions for MATLABSystem: '/Median Filter' + if (filter_current_DW.obj.pMID.isInitialized == 1) { + filter_MedianFilterCG_resetImpl(&filter_current_DW.obj.pMID); + } + + // End of InitializeConditions for MATLABSystem: '/Median Filter' +} + +// Output and update for referenced model: 'filter_current' +void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent + *rty_FilteredCurrent) +{ + c_dsp_internal_MedianFilterCG_T *obj; + real32_T p; + real32_T vprev; + + // MATLABSystem: '/Median Filter' + obj = &filter_current_DW.obj.pMID; + if (filter_current_DW.obj.pMID.isInitialized != 1) { + filter_current_DW.obj.pMID.isInitialized = 1; + filter_current_DW.obj.pMID.isSetupComplete = true; + filter_MedianFilterCG_resetImpl(&filter_current_DW.obj.pMID); + } + + vprev = filter_current_DW.obj.pMID.pBuf[static_cast + (filter_current_DW.obj.pMID.pIdx) - 1]; + filter_current_DW.obj.pMID.pBuf[static_cast + (filter_current_DW.obj.pMID.pIdx) - 1] = rtu_ControlOutputs->Iq_fbk.current; + p = filter_current_DW.obj.pMID.pPos[static_cast + (filter_current_DW.obj.pMID.pIdx) - 1]; + filter_current_DW.obj.pMID.pIdx++; + if (filter_current_DW.obj.pMID.pWinLen + 1.0F == + filter_current_DW.obj.pMID.pIdx) { + filter_current_DW.obj.pMID.pIdx = 1.0F; + } + + if (p > filter_current_DW.obj.pMID.pMidHeap) { + if (vprev < rtu_ControlOutputs->Iq_fbk.current) { + vprev = p - filter_current_DW.obj.pMID.pMidHeap; + f_MedianFilterCG_trickleDownMin(&filter_current_DW.obj.pMID, vprev * 2.0F); + } else { + boolean_T exitg1; + vprev = p - filter_current_DW.obj.pMID.pMidHeap; + exitg1 = false; + while ((!exitg1) && (vprev > 0.0F)) { + boolean_T flag; + flag = (obj->pBuf[static_cast(obj->pHeap[static_cast + (vprev + obj->pMidHeap) - 1]) - 1] < obj->pBuf + [static_cast(obj->pHeap[static_cast(std::floor + (vprev / 2.0F) + obj->pMidHeap) - 1]) - 1]); + if (!flag) { + exitg1 = true; + } else { + real32_T ind2; + real32_T temp; + p = vprev + obj->pMidHeap; + ind2 = std::floor(vprev / 2.0F) + obj->pMidHeap; + temp = obj->pHeap[static_cast(p) - 1]; + obj->pHeap[static_cast(p) - 1] = obj->pHeap + [static_cast(ind2) - 1]; + obj->pHeap[static_cast(ind2) - 1] = temp; + obj->pPos[static_cast(obj->pHeap[static_cast(p) - 1]) + - 1] = p; + obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - + 1]) - 1] = ind2; + vprev = std::floor(vprev / 2.0F); + } + } + + if (vprev == 0.0F) { + f_MedianFilterCG_trickleDownMax(&filter_current_DW.obj.pMID, -1.0F); + } + } + } else if (p < filter_current_DW.obj.pMID.pMidHeap) { + if (rtu_ControlOutputs->Iq_fbk.current < vprev) { + vprev = p - filter_current_DW.obj.pMID.pMidHeap; + f_MedianFilterCG_trickleDownMax(&filter_current_DW.obj.pMID, vprev * 2.0F); + } else { + boolean_T exitg1; + vprev = p - filter_current_DW.obj.pMID.pMidHeap; + exitg1 = false; + while ((!exitg1) && (vprev < 0.0F)) { + real32_T u_tmp; + boolean_T flag; + u_tmp = vprev / 2.0F; + if (u_tmp < 0.0F) { + p = std::ceil(u_tmp); + } else { + p = -0.0F; + } + + flag = (obj->pBuf[static_cast(obj->pHeap[static_cast(p + + obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast + (obj->pHeap[static_cast(vprev + obj->pMidHeap) - 1]) - + 1]); + if (!flag) { + exitg1 = true; + } else { + real32_T ind2; + real32_T temp; + if (u_tmp < 0.0F) { + p = std::ceil(u_tmp); + } else { + p = -0.0F; + } + + p += obj->pMidHeap; + ind2 = vprev + obj->pMidHeap; + temp = obj->pHeap[static_cast(p) - 1]; + obj->pHeap[static_cast(p) - 1] = obj->pHeap + [static_cast(ind2) - 1]; + obj->pHeap[static_cast(ind2) - 1] = temp; + obj->pPos[static_cast(obj->pHeap[static_cast(p) - 1]) + - 1] = p; + obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - + 1]) - 1] = ind2; + if (u_tmp < 0.0F) { + vprev = std::ceil(u_tmp); + } else { + vprev = -0.0F; + } + } + } + + if (vprev == 0.0F) { + f_MedianFilterCG_trickleDownMin(&filter_current_DW.obj.pMID, 1.0F); + } + } + } else { + if (filter_current_DW.obj.pMID.pMaxHeapLength != 0.0F) { + f_MedianFilterCG_trickleDownMax(&filter_current_DW.obj.pMID, -1.0F); + } + + if (filter_current_DW.obj.pMID.pMinHeapLength > 0.0F) { + f_MedianFilterCG_trickleDownMin(&filter_current_DW.obj.pMID, 1.0F); + } + } + + vprev = filter_current_DW.obj.pMID.pBuf[static_cast + (filter_current_DW.obj.pMID.pHeap[static_cast + (filter_current_DW.obj.pMID.pMidHeap) - 1]) - 1]; + vprev += filter_current_DW.obj.pMID.pBuf[static_cast + (filter_current_DW.obj.pMID.pHeap[static_cast + (filter_current_DW.obj.pMID.pMidHeap - 1.0F) - 1]) - 1]; + rty_FilteredCurrent->current = vprev / 2.0F; + + // End of MATLABSystem: '/Median Filter' +} + +// Termination for referenced model: 'filter_current' +void filter_current_Term(void) +{ + // Terminate for MATLABSystem: '/Median Filter' + if (!filter_current_DW.obj.matlabCodegenIsDeleted) { + filter_current_DW.obj.matlabCodegenIsDeleted = true; + if ((filter_current_DW.obj.isInitialized == 1) && + filter_current_DW.obj.isSetupComplete) { + filter_current_DW.obj.NumChannels = -1; + if (filter_current_DW.obj.pMID.isInitialized == 1) { + filter_current_DW.obj.pMID.isInitialized = 2; + } + } + } + + // End of Terminate for MATLABSystem: '/Median Filter' +} + +// Model initialize function +void filter_current_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_filter_current_T *const filter_current_M = + &(filter_current_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(filter_current_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.h index 04ae05c04f..dd6fa898cf 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current.h @@ -1,94 +1,94 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: filter_current.h -// -// Code generated for Simulink model 'filter_current'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:34 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_filter_current_h_ -#define RTW_HEADER_filter_current_h_ -#include "rtwtypes.h" -#include "filter_current_types.h" - -// Block states (default storage) for model 'filter_current' -#ifndef filter_current_MDLREF_HIDE_CHILD_ - -struct DW_filter_current_f_T { - dsp_simulink_MedianFilter_fil_T obj; // '/Median Filter' - boolean_T objisempty; // '/Median Filter' -}; - -#endif //filter_current_MDLREF_HIDE_CHILD_ - -#ifndef filter_current_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_filter_current_T { - const char_T **errorStatus; -}; - -#endif //filter_current_MDLREF_HIDE_CHILD_ - -#ifndef filter_current_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_filter_current_T { - RT_MODEL_filter_current_T rtm; -}; - -#endif //filter_current_MDLREF_HIDE_CHILD_ - -extern void filter_current_Init(void); -extern void filter_current(const ControlOutputs *rtu_ControlOutputs, - MotorCurrent *rty_FilteredCurrent); -extern void filter_current_Term(void); - -// Model reference registration function -extern void filter_current_initialize(const char_T **rt_errorStatus); - -#ifndef filter_current_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_filter_current_T filter_current_MdlrefDW; - -#endif //filter_current_MDLREF_HIDE_CHILD_ - -#ifndef filter_current_MDLREF_HIDE_CHILD_ - -// Block states (default storage) -extern DW_filter_current_f_T filter_current_DW; - -#endif //filter_current_MDLREF_HIDE_CHILD_ - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'filter_current' - -#endif // RTW_HEADER_filter_current_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: filter_current.h +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:17 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_filter_current_h_ +#define RTW_HEADER_filter_current_h_ +#include "rtwtypes.h" +#include "filter_current_types.h" + +// Block states (default storage) for model 'filter_current' +#ifndef filter_current_MDLREF_HIDE_CHILD_ + +struct DW_filter_current_f_T { + dsp_simulink_MedianFilter_fil_T obj; // '/Median Filter' + boolean_T objisempty; // '/Median Filter' +}; + +#endif //filter_current_MDLREF_HIDE_CHILD_ + +#ifndef filter_current_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_filter_current_T { + const char_T **errorStatus; +}; + +#endif //filter_current_MDLREF_HIDE_CHILD_ + +#ifndef filter_current_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_filter_current_T { + RT_MODEL_filter_current_T rtm; +}; + +#endif //filter_current_MDLREF_HIDE_CHILD_ + +extern void filter_current_Init(void); +extern void filter_current(const ControlOutputs *rtu_ControlOutputs, + MotorCurrent *rty_FilteredCurrent); +extern void filter_current_Term(void); + +// Model reference registration function +extern void filter_current_initialize(const char_T **rt_errorStatus); + +#ifndef filter_current_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_filter_current_T filter_current_MdlrefDW; + +#endif //filter_current_MDLREF_HIDE_CHILD_ + +#ifndef filter_current_MDLREF_HIDE_CHILD_ + +// Block states (default storage) +extern DW_filter_current_f_T filter_current_DW; + +#endif //filter_current_MDLREF_HIDE_CHILD_ + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'filter_current' + +#endif // RTW_HEADER_filter_current_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_private.h index 270af9cb5d..063954a735 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_private.h @@ -1,46 +1,46 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: filter_current_private.h -// -// Code generated for Simulink model 'filter_current'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:34 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_filter_current_private_h_ -#define RTW_HEADER_filter_current_private_h_ -#include "rtwtypes.h" -#include "filter_current_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif -#endif // RTW_HEADER_filter_current_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: filter_current_private.h +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:17 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_filter_current_private_h_ +#define RTW_HEADER_filter_current_private_h_ +#include "rtwtypes.h" +#include "filter_current_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif +#endif // RTW_HEADER_filter_current_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_types.h index 8600a05b8b..442658758a 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/filter-current/filter_current_types.h @@ -1,103 +1,103 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: filter_current_types.h -// -// Code generated for Simulink model 'filter_current'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:23:34 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_filter_current_types_h_ -#define RTW_HEADER_filter_current_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs -{ - // control effort (quadrature) - real32_T Vq; - - // control effort (3-phases) - real32_T Vabc[3]; - - // quadrature current - MotorCurrent Iq_fbk; -}; - -#endif - -#ifndef struct_c_dsp_internal_MedianFilterCG_T -#define struct_c_dsp_internal_MedianFilterCG_T - -struct c_dsp_internal_MedianFilterCG_T -{ - int32_T isInitialized; - boolean_T isSetupComplete; - real32_T pWinLen; - real32_T pBuf[32]; - real32_T pHeap[32]; - real32_T pMidHeap; - real32_T pIdx; - real32_T pPos[32]; - real32_T pMinHeapLength; - real32_T pMaxHeapLength; -}; - -#endif // struct_c_dsp_internal_MedianFilterCG_T - -#ifndef struct_cell_wrap_filter_current_T -#define struct_cell_wrap_filter_current_T - -struct cell_wrap_filter_current_T -{ - uint32_T f1[8]; -}; - -#endif // struct_cell_wrap_filter_current_T - -#ifndef struct_dsp_simulink_MedianFilter_fil_T -#define struct_dsp_simulink_MedianFilter_fil_T - -struct dsp_simulink_MedianFilter_fil_T -{ - boolean_T matlabCodegenIsDeleted; - int32_T isInitialized; - boolean_T isSetupComplete; - cell_wrap_filter_current_T inputVarSize; - int32_T NumChannels; - c_dsp_internal_MedianFilterCG_T pMID; -}; - -#endif // struct_dsp_simulink_MedianFilter_fil_T - -// Forward declaration for rtModel -typedef struct tag_RTM_filter_current_T RT_MODEL_filter_current_T; - -#endif // RTW_HEADER_filter_current_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: filter_current_types.h +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:17 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_filter_current_types_h_ +#define RTW_HEADER_filter_current_types_h_ +#include "rtwtypes.h" +#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ +#define DEFINED_TYPEDEF_FOR_MotorCurrent_ + +struct MotorCurrent +{ + // motor current + real32_T current; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOutputs_ + +struct ControlOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + MotorCurrent Iq_fbk; +}; + +#endif + +#ifndef struct_c_dsp_internal_MedianFilterCG_T +#define struct_c_dsp_internal_MedianFilterCG_T + +struct c_dsp_internal_MedianFilterCG_T +{ + int32_T isInitialized; + boolean_T isSetupComplete; + real32_T pWinLen; + real32_T pBuf[32]; + real32_T pHeap[32]; + real32_T pMidHeap; + real32_T pIdx; + real32_T pPos[32]; + real32_T pMinHeapLength; + real32_T pMaxHeapLength; +}; + +#endif // struct_c_dsp_internal_MedianFilterCG_T + +#ifndef struct_cell_wrap_filter_current_T +#define struct_cell_wrap_filter_current_T + +struct cell_wrap_filter_current_T +{ + uint32_T f1[8]; +}; + +#endif // struct_cell_wrap_filter_current_T + +#ifndef struct_dsp_simulink_MedianFilter_fil_T +#define struct_dsp_simulink_MedianFilter_fil_T + +struct dsp_simulink_MedianFilter_fil_T +{ + boolean_T matlabCodegenIsDeleted; + int32_T isInitialized; + boolean_T isSetupComplete; + cell_wrap_filter_current_T inputVarSize; + int32_T NumChannels; + c_dsp_internal_MedianFilterCG_T pMID; +}; + +#endif // struct_dsp_simulink_MedianFilter_fil_T + +// Forward declaration for rtModel +typedef struct tag_RTM_filter_current_T RT_MODEL_filter_current_T; + +#endif // RTW_HEADER_filter_current_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/const_params.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/const_params.cpp index e049dd5b0b..9816ae9f9d 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/const_params.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/const_params.cpp @@ -1,28 +1,28 @@ -// -// const_params.cpp -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// Code generation for model "estimation_velocity". -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C++ source code generated on : Mon Sep 26 16:38:11 2022 - -#include "rtwtypes.h" - -extern const real32_T rtCP_pooled_Az3IVI54Pn7X[32]; -const real32_T rtCP_pooled_Az3IVI54Pn7X[32] = { 0.0F, 0.001F, 0.002F, 0.003F, - 0.004F, 0.005F, 0.006F, 0.007F, 0.008F, 0.009F, 0.01F, 0.011F, 0.012F, 0.013F, - 0.014F, 0.015F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F } ; - -extern const real32_T rtCP_pooled_IgamRjjg0YgF[6]; -const real32_T rtCP_pooled_IgamRjjg0YgF[6] = { 0.666666687F, -0.333333343F, - -0.333333343F, 0.666666687F, -0.333333343F, -0.333333343F } ; - -extern const boolean_T rtCP_pooled_kUC6nmgO8rex[16]; -const boolean_T rtCP_pooled_kUC6nmgO8rex[16] = { false, true, false, false, true, - true, false, false, true, false, true, true, false, false, false, false } ; +// +// const_params.cpp +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// Code generation for model "estimation_velocity". +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C++ source code generated on : Fri Feb 10 13:58:07 2023 + +#include "rtwtypes.h" + +extern const real32_T rtCP_pooled_Az3IVI54Pn7X[32]; +const real32_T rtCP_pooled_Az3IVI54Pn7X[32] = { 0.0F, 0.001F, 0.002F, 0.003F, + 0.004F, 0.005F, 0.006F, 0.007F, 0.008F, 0.009F, 0.01F, 0.011F, 0.012F, 0.013F, + 0.014F, 0.015F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, + 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F } ; + +extern const real32_T rtCP_pooled_IgamRjjg0YgF[6]; +const real32_T rtCP_pooled_IgamRjjg0YgF[6] = { 0.666666687F, -0.333333343F, + -0.333333343F, 0.666666687F, -0.333333343F, -0.333333343F } ; + +extern const boolean_T rtCP_pooled_kUC6nmgO8rex[16]; +const boolean_T rtCP_pooled_kUC6nmgO8rex[16] = { false, true, false, false, true, + true, false, false, true, false, true, true, false, false, false, false } ; diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/mw_cmsis.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/mw_cmsis.h index 5ab7118a5c..89fae226bf 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/mw_cmsis.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/mw_cmsis.h @@ -1,75 +1,75 @@ -/* Copyright 2015-2021 The MathWorks, Inc. */ - -/**************************************************** -* * -* Wrapper functions for CMSIS functions * -* * -****************************************************/ - -#ifndef MW_CMSIS_H -#define MW_CMSIS_H - -#include "arm_math.h" -#include "rtwtypes.h" - -#define mw_arm_abs_f32(pSrc, pDst, blockSize) arm_abs_f32((float32_t *)pSrc, (float32_t *)pDst, blockSize) -#define mw_arm_abs_q7(pSrc, pDst, blockSize) arm_abs_q7((q7_t *)pSrc, (q7_t *)pDst, blockSize) -#define mw_arm_abs_q15(pSrc, pDst, blockSize) arm_abs_q15((q15_t *)pSrc, (q15_t *)pDst, blockSize) -#define mw_arm_abs_q31(pSrc, pDst, blockSize) arm_abs_q31((q31_t *)pSrc, (q31_t *)pDst, blockSize) - -#define mw_arm_sqrt_q15(in, pOut) arm_sqrt_q15((q15_t)in,(q15_t *)pOut) -#define mw_arm_sqrt_q31(in, pOut) arm_sqrt_q31((q31_t)in,(q31_t *)pOut) -#define mw_arm_sqrt_f32(in, pOut) arm_sqrt_f32((float32_t)in,(float32_t *)pOut) - -#define mw_arm_float_to_q31(pSrc, pDst, blockSize) arm_float_to_q31((float32_t *)pSrc, (q31_t *)pDst, blockSize) -#define mw_arm_float_to_q15(pSrc, pDst, blockSize) arm_float_to_q15((float32_t *)pSrc, (q15_t *)pDst, blockSize) -#define mw_arm_float_to_q7(pSrc, pDst, blockSize) arm_float_to_q7((float32_t *)pSrc, (q7_t *)pDst, blockSize) - -#define mw_arm_q15_to_float(pSrc, pDst, blockSize) arm_q15_to_float((q15_t *)pSrc, (float32_t *)pDst, blockSize) -#define mw_arm_q15_to_q31(pSrc, pDst, blockSize) arm_q15_to_q31((q15_t *)pSrc, (q31_t *)pDst, blockSize) -#define mw_arm_q15_to_q7(pSrc, pDst, blockSize) arm_q15_to_q7((q15_t *)pSrc, (q7_t *)pDst, blockSize) - -#define mw_arm_q31_to_float(pSrc, pDst, blockSize) arm_q31_to_float((q31_t *)pSrc, (float32_t *)pDst, blockSize) -#define mw_arm_q31_to_q15(pSrc, pDst, blockSize) arm_q31_to_q15((q31_t *)pSrc, (q15_t *)pDst, blockSize) -#define mw_arm_q31_to_q7(pSrc, pDst, blockSize) arm_q31_to_q7((q31_t *)pSrc, (q7_t *)pDst, blockSize) - -#define mw_arm_q7_to_float(pSrc, pDst, blockSize) arm_q7_to_float((q7_t *)pSrc, (float32_t *)pDst, blockSize) -#define mw_arm_q7_to_q31(pSrc, pDst, blockSize) arm_q7_to_q31((q7_t *)pSrc, (q31_t *)pDst, blockSize) -#define mw_arm_q7_to_q15(pSrc, pDst, blockSize) arm_q7_to_q15((q7_t *)pSrc, (q15_t *)pDst, blockSize) - -#define mw_arm_add_f32(pSrcA, pSrcB, pDst, blockSize) arm_add_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) -#define mw_arm_add_q31(pSrcA, pSrcB, pDst, blockSize) arm_add_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) -#define mw_arm_add_q15(pSrcA, pSrcB, pDst, blockSize) arm_add_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) -#define mw_arm_add_q7(pSrcA, pSrcB, pDst, blockSize) arm_add_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) - -#define mw_arm_sub_f32(pSrcA, pSrcB, pDst, blockSize) arm_sub_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) -#define mw_arm_sub_q31(pSrcA, pSrcB, pDst, blockSize) arm_sub_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) -#define mw_arm_sub_q15(pSrcA, pSrcB, pDst, blockSize) arm_sub_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) -#define mw_arm_sub_q7(pSrcA, pSrcB, pDst, blockSize) arm_sub_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) - -#define mw_arm_mult_f32(pSrcA, pSrcB, pDst, blockSize) arm_mult_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) -#define mw_arm_mult_q31(pSrcA, pSrcB, pDst, blockSize) arm_mult_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) -#define mw_arm_mult_q15(pSrcA, pSrcB, pDst, blockSize) arm_mult_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) -#define mw_arm_mult_q7(pSrcA, pSrcB, pDst, blockSize) arm_mult_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) - -#define mw_arm_cmplx_conj_f32(pSrc, pDst, numSamples) arm_cmplx_conj_f32((float32_t *)pSrc, (float32_t *)pDst, numSamples) -#define mw_arm_cmplx_conj_q31(pSrc, pDst, numSamples) arm_cmplx_conj_q31((q31_t *)pSrc, (q31_t *)pDst, numSamples) -#define mw_arm_cmplx_conj_q15(pSrc, pDst, numSamples) arm_cmplx_conj_q15((q15_t *)pSrc, (q15_t *)pDst, numSamples) - -#define mw_arm_cmplx_mult_cmplx_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) -#define mw_arm_cmplx_mult_cmplx_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) -#define mw_arm_cmplx_mult_cmplx_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) - -#define mw_arm_cmplx_mult_real_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) -#define mw_arm_cmplx_mult_real_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) -#define mw_arm_cmplx_mult_real_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) - -#define mw_arm_rshift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, -(shiftBits),(q15_t *)pDst, blockSize) -#define mw_arm_rshift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, -(shiftBits), (q31_t *)pDst, blockSize) -#define mw_arm_rshift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, -(shiftBits), (q7_t *)pDst, blockSize) - -#define mw_arm_shift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, shiftBits,(q15_t *)pDst, blockSize) -#define mw_arm_shift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, shiftBits, (q31_t *)pDst, blockSize) -#define mw_arm_shift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, shiftBits, (q7_t *)pDst, blockSize) - -#endif +/* Copyright 2015-2021 The MathWorks, Inc. */ + +/**************************************************** +* * +* Wrapper functions for CMSIS functions * +* * +****************************************************/ + +#ifndef MW_CMSIS_H +#define MW_CMSIS_H + +#include "arm_math.h" +#include "rtwtypes.h" + +#define mw_arm_abs_f32(pSrc, pDst, blockSize) arm_abs_f32((float32_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_abs_q7(pSrc, pDst, blockSize) arm_abs_q7((q7_t *)pSrc, (q7_t *)pDst, blockSize) +#define mw_arm_abs_q15(pSrc, pDst, blockSize) arm_abs_q15((q15_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_abs_q31(pSrc, pDst, blockSize) arm_abs_q31((q31_t *)pSrc, (q31_t *)pDst, blockSize) + +#define mw_arm_sqrt_q15(in, pOut) arm_sqrt_q15((q15_t)in,(q15_t *)pOut) +#define mw_arm_sqrt_q31(in, pOut) arm_sqrt_q31((q31_t)in,(q31_t *)pOut) +#define mw_arm_sqrt_f32(in, pOut) arm_sqrt_f32((float32_t)in,(float32_t *)pOut) + +#define mw_arm_float_to_q31(pSrc, pDst, blockSize) arm_float_to_q31((float32_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_float_to_q15(pSrc, pDst, blockSize) arm_float_to_q15((float32_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_float_to_q7(pSrc, pDst, blockSize) arm_float_to_q7((float32_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q15_to_float(pSrc, pDst, blockSize) arm_q15_to_float((q15_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q15_to_q31(pSrc, pDst, blockSize) arm_q15_to_q31((q15_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_q15_to_q7(pSrc, pDst, blockSize) arm_q15_to_q7((q15_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q31_to_float(pSrc, pDst, blockSize) arm_q31_to_float((q31_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q31_to_q15(pSrc, pDst, blockSize) arm_q31_to_q15((q31_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_q31_to_q7(pSrc, pDst, blockSize) arm_q31_to_q7((q31_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q7_to_float(pSrc, pDst, blockSize) arm_q7_to_float((q7_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q7_to_q31(pSrc, pDst, blockSize) arm_q7_to_q31((q7_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_q7_to_q15(pSrc, pDst, blockSize) arm_q7_to_q15((q7_t *)pSrc, (q15_t *)pDst, blockSize) + +#define mw_arm_add_f32(pSrcA, pSrcB, pDst, blockSize) arm_add_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_add_q31(pSrcA, pSrcB, pDst, blockSize) arm_add_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_add_q15(pSrcA, pSrcB, pDst, blockSize) arm_add_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_add_q7(pSrcA, pSrcB, pDst, blockSize) arm_add_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_sub_f32(pSrcA, pSrcB, pDst, blockSize) arm_sub_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_sub_q31(pSrcA, pSrcB, pDst, blockSize) arm_sub_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_sub_q15(pSrcA, pSrcB, pDst, blockSize) arm_sub_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_sub_q7(pSrcA, pSrcB, pDst, blockSize) arm_sub_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_mult_f32(pSrcA, pSrcB, pDst, blockSize) arm_mult_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_mult_q31(pSrcA, pSrcB, pDst, blockSize) arm_mult_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_mult_q15(pSrcA, pSrcB, pDst, blockSize) arm_mult_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_mult_q7(pSrcA, pSrcB, pDst, blockSize) arm_mult_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_cmplx_conj_f32(pSrc, pDst, numSamples) arm_cmplx_conj_f32((float32_t *)pSrc, (float32_t *)pDst, numSamples) +#define mw_arm_cmplx_conj_q31(pSrc, pDst, numSamples) arm_cmplx_conj_q31((q31_t *)pSrc, (q31_t *)pDst, numSamples) +#define mw_arm_cmplx_conj_q15(pSrc, pDst, numSamples) arm_cmplx_conj_q15((q15_t *)pSrc, (q15_t *)pDst, numSamples) + +#define mw_arm_cmplx_mult_cmplx_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_cmplx_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_cmplx_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) + +#define mw_arm_cmplx_mult_real_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_real_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_real_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) + +#define mw_arm_rshift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, -(shiftBits),(q15_t *)pDst, blockSize) +#define mw_arm_rshift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, -(shiftBits), (q31_t *)pDst, blockSize) +#define mw_arm_rshift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, -(shiftBits), (q7_t *)pDst, blockSize) + +#define mw_arm_shift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, shiftBits,(q15_t *)pDst, blockSize) +#define mw_arm_shift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, shiftBits, (q31_t *)pDst, blockSize) +#define mw_arm_shift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, shiftBits, (q7_t *)pDst, blockSize) + +#endif diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.cpp index 40cae5b6b3..2b83bc7b5a 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.cpp @@ -1,111 +1,111 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rtGetInf.cpp -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:51 2022 -// -#include "rtwtypes.h" - -extern "C" -{ - -#include "rtGetInf.h" - -} - -#include - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#define NumBitsPerChar 8U - -extern "C" -{ - // - // Initialize rtInf needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetInf(void) - { - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - } - - return inf; - } - - // - // Initialize rtInfF needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetInfF(void) - { - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; - } - - // - // Initialize rtMinusInf needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetMinusInf(void) - { - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - } - - return minf; - } - - // - // Initialize rtMinusInfF needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetMinusInfF(void) - { - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtGetInf.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +#include "rtwtypes.h" + +extern "C" +{ + +#include "rtGetInf.h" + +} + +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#define NumBitsPerChar 8U + +extern "C" +{ + // + // Initialize rtInf needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real_T rtGetInf(void) + { + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T inf = 0.0; + if (bitsPerReal == 32U) { + inf = rtGetInfF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + } + + return inf; + } + + // + // Initialize rtInfF needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real32_T rtGetInfF(void) + { + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; + } + + // + // Initialize rtMinusInf needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real_T rtGetMinusInf(void) + { + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T minf = 0.0; + if (bitsPerReal == 32U) { + minf = rtGetMinusInfF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + } + + return minf; + } + + // + // Initialize rtMinusInfF needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real32_T rtGetMinusInfF(void) + { + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; + } +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.h index 1b85371e9c..bb5435f081 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetInf.h @@ -1,40 +1,40 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rtGetInf.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:51 2022 -// -#ifndef RTW_HEADER_rtGetInf_h_ -#define RTW_HEADER_rtGetInf_h_ -#include "rtwtypes.h" -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern real_T rtGetInf(void); - extern real32_T rtGetInfF(void); - extern real_T rtGetMinusInf(void); - extern real32_T rtGetMinusInfF(void); - -#ifdef __cplusplus - -} // extern "C" - -#endif -#endif // RTW_HEADER_rtGetInf_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtGetInf.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +#ifndef RTW_HEADER_rtGetInf_h_ +#define RTW_HEADER_rtGetInf_h_ +#include "rtwtypes.h" +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern real_T rtGetInf(void); + extern real32_T rtGetInfF(void); + extern real_T rtGetMinusInf(void); + extern real32_T rtGetMinusInfF(void); + +#ifdef __cplusplus + +} // extern "C" + +#endif +#endif // RTW_HEADER_rtGetInf_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.cpp index e808d5f4a9..5e41929476 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.cpp @@ -1,77 +1,77 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rtGetNaN.cpp -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:51 2022 -// -#include "rtwtypes.h" - -extern "C" -{ - -#include "rtGetNaN.h" - -} - -#include - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#define NumBitsPerChar 8U - -extern "C" -{ - // - // Initialize rtNaN needed by the generated code. - // NaN is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetNaN(void) - { - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - } - - return nan; - } - - // - // Initialize rtNaNF needed by the generated code. - // NaN is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetNaNF(void) - { - IEEESingle nanF = { { 0.0F } }; - - nanF.wordL.wordLuint = 0xFFC00000U; - return nanF.wordL.wordLreal; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtGetNaN.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +#include "rtwtypes.h" + +extern "C" +{ + +#include "rtGetNaN.h" + +} + +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#define NumBitsPerChar 8U + +extern "C" +{ + // + // Initialize rtNaN needed by the generated code. + // NaN is initialized as non-signaling. Assumes IEEE. + // + real_T rtGetNaN(void) + { + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T nan = 0.0; + if (bitsPerReal == 32U) { + nan = rtGetNaNF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + } + + return nan; + } + + // + // Initialize rtNaNF needed by the generated code. + // NaN is initialized as non-signaling. Assumes IEEE. + // + real32_T rtGetNaNF(void) + { + IEEESingle nanF = { { 0.0F } }; + + nanF.wordL.wordLuint = 0xFFC00000U; + return nanF.wordL.wordLreal; + } +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.h index 7d7567cda0..d9d18720ec 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtGetNaN.h @@ -1,38 +1,38 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rtGetNaN.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:51 2022 -// -#ifndef RTW_HEADER_rtGetNaN_h_ -#define RTW_HEADER_rtGetNaN_h_ -#include "rtwtypes.h" -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern real_T rtGetNaN(void); - extern real32_T rtGetNaNF(void); - -#ifdef __cplusplus - -} // extern "C" - -#endif -#endif // RTW_HEADER_rtGetNaN_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtGetNaN.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +#ifndef RTW_HEADER_rtGetNaN_h_ +#define RTW_HEADER_rtGetNaN_h_ +#include "rtwtypes.h" +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern real_T rtGetNaN(void); + extern real32_T rtGetNaNF(void); + +#ifdef __cplusplus + +} // extern "C" + +#endif +#endif // RTW_HEADER_rtGetNaN_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.cpp index a16fdd23c2..bd3539b79f 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.cpp @@ -1,57 +1,57 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rt_hypotf_snf.cpp -// -// Code generated for Simulink model 'estimation_velocity'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:38:11 2022 -// -#include "rtwtypes.h" -#include "rt_hypotf_snf.h" -#include - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#include "mw_cmsis.h" -#include "rtGetNaN.h" - -real32_T rt_hypotf_snf(real32_T u0, real32_T u1) -{ - real32_T a; - real32_T b; - real32_T tmp; - real32_T y; - a = std::abs(u0); - b = std::abs(u1); - if (a < b) { - a /= b; - mw_arm_sqrt_f32(a * a + 1.0F, &tmp); - y = tmp * b; - } else if (a > b) { - b /= a; - mw_arm_sqrt_f32(b * b + 1.0F, &tmp); - y = tmp * a; - } else if (rtIsNaNF(b)) { - y = (rtNaNF); - } else { - y = a * 1.41421354F; - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_hypotf_snf.cpp +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:07 2023 +// +#include "rtwtypes.h" +#include "rt_hypotf_snf.h" +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include "mw_cmsis.h" +#include "rtGetNaN.h" + +real32_T rt_hypotf_snf(real32_T u0, real32_T u1) +{ + real32_T a; + real32_T b; + real32_T tmp; + real32_T y; + a = std::abs(u0); + b = std::abs(u1); + if (a < b) { + a /= b; + mw_arm_sqrt_f32(a * a + 1.0F, &tmp); + y = tmp * b; + } else if (a > b) { + b /= a; + mw_arm_sqrt_f32(b * b + 1.0F, &tmp); + y = tmp * a; + } else if (rtIsNaNF(b)) { + y = (rtNaNF); + } else { + y = a * 1.41421354F; + } + + return y; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.h index c9770e270b..8a7dff47ab 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_hypotf_snf.h @@ -1,26 +1,26 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rt_hypotf_snf.h -// -// Code generated for Simulink model 'estimation_velocity'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:38:11 2022 -// -#ifndef RTW_HEADER_rt_hypotf_snf_h_ -#define RTW_HEADER_rt_hypotf_snf_h_ -#include "rtwtypes.h" - -extern real32_T rt_hypotf_snf(real32_T u0, real32_T u1); - -#endif // RTW_HEADER_rt_hypotf_snf_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_hypotf_snf.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:58:07 2023 +// +#ifndef RTW_HEADER_rt_hypotf_snf_h_ +#define RTW_HEADER_rt_hypotf_snf_h_ +#include "rtwtypes.h" + +extern real32_T rt_hypotf_snf(real32_T u0, real32_T u1); + +#endif // RTW_HEADER_rt_hypotf_snf_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.cpp index 6e4abe9df7..cef10a4989 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.cpp @@ -1,116 +1,116 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rt_nonfinite.cpp -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:51 2022 -// -extern "C" -{ - -#include "rtGetNaN.h" - -} - -extern "C" -{ - -#include "rtGetInf.h" - -} - -#include -#include "rtwtypes.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#define NumBitsPerChar 8U - -extern "C" -{ - real_T rtInf; - real_T rtMinusInf; - real_T rtNaN; - real32_T rtInfF; - real32_T rtMinusInfF; - real32_T rtNaNF; -} - -extern "C" -{ - // - // Initialize the rtInf, rtMinusInf, and rtNaN needed by the - // generated code. NaN is initialized as non-signaling. Assumes IEEE. - // - void rt_InitInfAndNaN(size_t realSize) - { - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); - } - - // Test if value is infinite - boolean_T rtIsInf(real_T value) - { - return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); - } - - // Test if single-precision value is infinite - boolean_T rtIsInfF(real32_T value) - { - return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); - } - - // Test if value is not a number - boolean_T rtIsNaN(real_T value) - { - boolean_T result = (boolean_T) 0; - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - if (bitsPerReal == 32U) { - result = rtIsNaNF((real32_T)value); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.fltVal = value; - result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == - 0x7FF00000 && - ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || - (tmpVal.bitVal.words.wordL != 0) )); - } - - return result; - } - - // Test if single-precision value is not a number - boolean_T rtIsNaNF(real32_T value) - { - IEEESingle tmp; - tmp.wordL.wordLreal = value; - return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && - (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_nonfinite.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +extern "C" +{ + +#include "rtGetNaN.h" + +} + +extern "C" +{ + +#include "rtGetInf.h" + +} + +#include +#include "rtwtypes.h" + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#define NumBitsPerChar 8U + +extern "C" +{ + real_T rtInf; + real_T rtMinusInf; + real_T rtNaN; + real32_T rtInfF; + real32_T rtMinusInfF; + real32_T rtNaNF; +} + +extern "C" +{ + // + // Initialize the rtInf, rtMinusInf, and rtNaN needed by the + // generated code. NaN is initialized as non-signaling. Assumes IEEE. + // + void rt_InitInfAndNaN(size_t realSize) + { + (void) (realSize); + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); + } + + // Test if value is infinite + boolean_T rtIsInf(real_T value) + { + return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); + } + + // Test if single-precision value is infinite + boolean_T rtIsInfF(real32_T value) + { + return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); + } + + // Test if value is not a number + boolean_T rtIsNaN(real_T value) + { + boolean_T result = (boolean_T) 0; + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + if (bitsPerReal == 32U) { + result = rtIsNaNF((real32_T)value); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.fltVal = value; + result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == + 0x7FF00000 && + ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || + (tmpVal.bitVal.words.wordL != 0) )); + } + + return result; + } + + // Test if single-precision value is not a number + boolean_T rtIsNaNF(real32_T value) + { + IEEESingle tmp; + tmp.wordL.wordLreal = value; + return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && + (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); + } +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.h index f6433f6dd5..305503dbff 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_nonfinite.h @@ -1,69 +1,69 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rt_nonfinite.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:51 2022 -// -#ifndef RTW_HEADER_rt_nonfinite_h_ -#define RTW_HEADER_rt_nonfinite_h_ -#include -#include "rtwtypes.h" -#define NOT_USING_NONFINITE_LITERALS 1 -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern real_T rtInf; - extern real_T rtMinusInf; - extern real_T rtNaN; - extern real32_T rtInfF; - extern real32_T rtMinusInfF; - extern real32_T rtNaNF; - extern void rt_InitInfAndNaN(size_t realSize); - extern boolean_T rtIsInf(real_T value); - extern boolean_T rtIsInfF(real32_T value); - extern boolean_T rtIsNaN(real_T value); - extern boolean_T rtIsNaNF(real32_T value); - struct BigEndianIEEEDouble { - struct { - uint32_T wordH; - uint32_T wordL; - } words; - }; - - struct LittleEndianIEEEDouble { - struct { - uint32_T wordL; - uint32_T wordH; - } words; - }; - - struct IEEESingle { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; - }; - -#ifdef __cplusplus - -} // extern "C" - -#endif -#endif // RTW_HEADER_rt_nonfinite_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_nonfinite.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +#ifndef RTW_HEADER_rt_nonfinite_h_ +#define RTW_HEADER_rt_nonfinite_h_ +#include +#include "rtwtypes.h" +#define NOT_USING_NONFINITE_LITERALS 1 +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern real_T rtInf; + extern real_T rtMinusInf; + extern real_T rtNaN; + extern real32_T rtInfF; + extern real32_T rtMinusInfF; + extern real32_T rtNaNF; + extern void rt_InitInfAndNaN(size_t realSize); + extern boolean_T rtIsInf(real_T value); + extern boolean_T rtIsInfF(real32_T value); + extern boolean_T rtIsNaN(real_T value); + extern boolean_T rtIsNaNF(real32_T value); + struct BigEndianIEEEDouble { + struct { + uint32_T wordH; + uint32_T wordL; + } words; + }; + + struct LittleEndianIEEEDouble { + struct { + uint32_T wordL; + uint32_T wordH; + } words; + }; + + struct IEEESingle { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; + }; + +#ifdef __cplusplus + +} // extern "C" + +#endif +#endif // RTW_HEADER_rt_nonfinite_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.cpp index 79d9135023..b36d2f1f5f 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.cpp @@ -1,40 +1,40 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rt_roundd_snf.cpp -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.2 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:04 2022 -// -#include "rtwtypes.h" -#include "rt_roundd_snf.h" -#include - -real_T rt_roundd_snf(real_T u) -{ - real_T y; - if (std::abs(u) < 4.503599627370496E+15) { - if (u >= 0.5) { - y = std::floor(u + 0.5); - } else if (u > -0.5) { - y = u * 0.0; - } else { - y = std::ceil(u - 0.5); - } - } else { - y = u; - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_roundd_snf.cpp +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// +#include "rtwtypes.h" +#include "rt_roundd_snf.h" +#include + +real_T rt_roundd_snf(real_T u) +{ + real_T y; + if (std::abs(u) < 4.503599627370496E+15) { + if (u >= 0.5) { + y = std::floor(u + 0.5); + } else if (u > -0.5) { + y = u * 0.0; + } else { + y = std::ceil(u - 0.5); + } + } else { + y = u; + } + + return y; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.h index 61347a7198..e88d68e10b 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rt_roundd_snf.h @@ -1,26 +1,26 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rt_roundd_snf.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.2 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:04 2022 -// -#ifndef RTW_HEADER_rt_roundd_snf_h_ -#define RTW_HEADER_rt_roundd_snf_h_ -#include "rtwtypes.h" - -extern real_T rt_roundd_snf(real_T u); - -#endif // RTW_HEADER_rt_roundd_snf_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_roundd_snf.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// +#ifndef RTW_HEADER_rt_roundd_snf_h_ +#define RTW_HEADER_rt_roundd_snf_h_ +#include "rtwtypes.h" + +extern real_T rt_roundd_snf(real_T u); + +#endif // RTW_HEADER_rt_roundd_snf_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_defines.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_defines.h index 4f2808b2b9..824b1c2856 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_defines.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_defines.h @@ -1,28 +1,28 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rtw_defines.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.2 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:04 2022 -// - -#ifndef RTW_HEADER_rtw_defines_h_ -#define RTW_HEADER_rtw_defines_h_ -#include "rtwtypes.h" - -// Exported data define -// Definition for custom storage class: Define -#define CAN_MAX_NUM_PACKETS 4 // Maximum number of TX/RX packets handled per time instance. -#endif // RTW_HEADER_rtw_defines_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtw_defines.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// + +#ifndef RTW_HEADER_rtw_defines_h_ +#define RTW_HEADER_rtw_defines_h_ +#include "rtwtypes.h" + +// Exported data define +// Definition for custom storage class: Define +#define CAN_MAX_NUM_PACKETS 4 // Maximum number of TX/RX packets handled per time instance. +#endif // RTW_HEADER_rtw_defines_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c index e3a9d831db..2c1c411ca2 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c @@ -25,11 +25,12 @@ #include "embot_hw_motor.h" #endif /* STM32HAL_BOARD_AMCBLDC */ -void rtw_configMotor(int16_t rotor_enc_resolution, uint8_t pole_pairs, uint8_t has_hall_sens, uint8_t swapBC, uint16_t hall_sens_offset) +void rtw_configMotor(uint8_t has_quad_enc, int16_t rotor_enc_resolution, uint8_t pole_pairs, uint8_t has_hall_sens, uint8_t swapBC, uint16_t hall_sens_offset) { #ifdef STM32HAL_BOARD_AMCBLDC embot::hw::motor::config( - embot::hw::MOTOR::one, + embot::hw::MOTOR::one, + has_quad_enc, rotor_enc_resolution, pole_pairs, has_hall_sens, diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.h index 84a5945f48..f7d9094710 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.h @@ -27,7 +27,7 @@ /*inizialize the motor encoder using the data read in the configuration files (yarprobotinterface) swapBC = 1 hall_sens_offset = 120 *65536/360 ; icubDeg*/ -void rtw_configMotor(int16_t rotor_enc_resolution, uint8_t pole_pairs, uint8_t has_hall_sens, uint8_t swapBC, uint16_t hall_sens_offset); +void rtw_configMotor(uint8_t has_quad_enc, int16_t rotor_enc_resolution, uint8_t pole_pairs, uint8_t has_hall_sens, uint8_t swapBC, uint16_t hall_sens_offset); #ifdef __cplusplus } /* extern "C" */ diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtwtypes.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtwtypes.h index 9c85b2506f..03d8ccb589 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtwtypes.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtwtypes.h @@ -1,154 +1,154 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: rtwtypes.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.2 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:04 2022 -// -#ifndef RTWTYPES_H -#define RTWTYPES_H - -// Logical type definitions -#if (!defined(__cplusplus)) -#ifndef false -#define false (0U) -#endif - -#ifndef true -#define true (1U) -#endif -#endif - -//=======================================================================* -// Target hardware information -// Device type: ARM Compatible->ARM Cortex-M -// Number of bits: char: 8 short: 16 int: 32 -// long: 32 -// native word size: 32 -// Byte ordering: LittleEndian -// Signed integer division rounds to: Zero -// Shift right on a signed integer as arithmetic shift: on -// ======================================================================= - -//=======================================================================* -// Fixed width word size data types: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// real32_T, real64_T - 32 and 64 bit floating point numbers * -// ======================================================================= -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -//===========================================================================* -// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * -// real_T, time_T, ulong_T. * -// =========================================================================== -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -//===========================================================================* -// Complex number type definitions * -// =========================================================================== -#define CREAL_T - -typedef struct { - real32_T re; - real32_T im; -} creal32_T; - -typedef struct { - real64_T re; - real64_T im; -} creal64_T; - -typedef struct { - real_T re; - real_T im; -} creal_T; - -#define CINT8_T - -typedef struct { - int8_T re; - int8_T im; -} cint8_T; - -#define CUINT8_T - -typedef struct { - uint8_T re; - uint8_T im; -} cuint8_T; - -#define CINT16_T - -typedef struct { - int16_T re; - int16_T im; -} cint16_T; - -#define CUINT16_T - -typedef struct { - uint16_T re; - uint16_T im; -} cuint16_T; - -#define CINT32_T - -typedef struct { - int32_T re; - int32_T im; -} cint32_T; - -#define CUINT32_T - -typedef struct { - uint32_T re; - uint32_T im; -} cuint32_T; - -//=======================================================================* -// Min and Max: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// ======================================================================= -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255U)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535U)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) - -// Block D-Work pointer type -typedef void * pointer_T; - -#endif // RTWTYPES_H - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtwtypes.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// +#ifndef RTWTYPES_H +#define RTWTYPES_H + +// Logical type definitions +#if (!defined(__cplusplus)) +#ifndef false +#define false (0U) +#endif + +#ifndef true +#define true (1U) +#endif +#endif + +//=======================================================================* +// Target hardware information +// Device type: ARM Compatible->ARM Cortex-M +// Number of bits: char: 8 short: 16 int: 32 +// long: 32 +// native word size: 32 +// Byte ordering: LittleEndian +// Signed integer division rounds to: Zero +// Shift right on a signed integer as arithmetic shift: on +// ======================================================================= + +//=======================================================================* +// Fixed width word size data types: * +// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * +// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * +// real32_T, real64_T - 32 and 64 bit floating point numbers * +// ======================================================================= +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +//===========================================================================* +// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * +// real_T, time_T, ulong_T. * +// =========================================================================== +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef unsigned char uchar_T; +typedef char_T byte_T; + +//===========================================================================* +// Complex number type definitions * +// =========================================================================== +#define CREAL_T + +typedef struct { + real32_T re; + real32_T im; +} creal32_T; + +typedef struct { + real64_T re; + real64_T im; +} creal64_T; + +typedef struct { + real_T re; + real_T im; +} creal_T; + +#define CINT8_T + +typedef struct { + int8_T re; + int8_T im; +} cint8_T; + +#define CUINT8_T + +typedef struct { + uint8_T re; + uint8_T im; +} cuint8_T; + +#define CINT16_T + +typedef struct { + int16_T re; + int16_T im; +} cint16_T; + +#define CUINT16_T + +typedef struct { + uint16_T re; + uint16_T im; +} cuint16_T; + +#define CINT32_T + +typedef struct { + int32_T re; + int32_T im; +} cint32_T; + +#define CUINT32_T + +typedef struct { + uint32_T re; + uint32_T im; +} cuint32_T; + +//=======================================================================* +// Min and Max: * +// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * +// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * +// ======================================================================= +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255U)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535U)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) + +// Block D-Work pointer type +typedef void * pointer_T; + +#endif // RTWTYPES_H + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/zero_crossing_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/zero_crossing_types.h index a11398e90f..9e46c90cb1 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/zero_crossing_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/zero_crossing_types.h @@ -1,44 +1,44 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: zero_crossing_types.h -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 4.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Mon Sep 26 16:37:51 2022 -// -#ifndef ZERO_CROSSING_TYPES_H -#define ZERO_CROSSING_TYPES_H -#include "rtwtypes.h" - -// Trigger directions: falling, either, and rising -typedef enum { - FALLING_ZERO_CROSSING = -1, - ANY_ZERO_CROSSING = 0, - RISING_ZERO_CROSSING = 1 -} ZCDirection; - -// Previous state of a trigger signal -typedef uint8_T ZCSigState; - -// Initial value of a trigger zero crossing signal -#define UNINITIALIZED_ZCSIG 0x03U -#define NEG_ZCSIG 0x02U -#define POS_ZCSIG 0x01U -#define ZERO_ZCSIG 0x00U - -// Current state of a trigger signal -typedef enum { FALLING_ZCEVENT = -1, NO_ZCEVENT = 0, RISING_ZCEVENT = 1 } - ZCEventType; - -#endif // ZERO_CROSSING_TYPES_H - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: zero_crossing_types.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 4.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:51 2023 +// +#ifndef ZERO_CROSSING_TYPES_H +#define ZERO_CROSSING_TYPES_H +#include "rtwtypes.h" + +// Trigger directions: falling, either, and rising +typedef enum { + FALLING_ZERO_CROSSING = -1, + ANY_ZERO_CROSSING = 0, + RISING_ZERO_CROSSING = 1 +} ZCDirection; + +// Previous state of a trigger signal +typedef uint8_T ZCSigState; + +// Initial value of a trigger zero crossing signal +#define UNINITIALIZED_ZCSIG 0x03U +#define NEG_ZCSIG 0x02U +#define POS_ZCSIG 0x01U +#define ZERO_ZCSIG 0x00U + +// Current state of a trigger signal +typedef enum { FALLING_ZCEVENT = -1, NO_ZCEVENT = 0, RISING_ZCEVENT = 1 } + ZCEventType; + +#endif // ZERO_CROSSING_TYPES_H + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.cpp index 8afc0f762d..343d5482a4 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.cpp @@ -1,1531 +1,1533 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX.cpp -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.6 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:29 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "SupervisorFSM_RX.h" -#include "rtwtypes.h" -#include "SupervisorFSM_RX_types.h" -#include -#include "rt_roundd_snf.h" -#include "rtw_defines.h" -#include "SupervisorFSM_RX_private.h" - -// Named constants for Chart: '/ControlMode_SM_motor0' -const uint8_T SupervisorFSM_RX_IN_Current = 1U; -const uint8_T SupervisorFSM_RX_IN_HWFault = 2U; -const uint8_T SupervisorFSM_RX_IN_Idle = 3U; -const uint8_T SupervisorFSM_RX_IN_Position = 5U; -const uint8_T SupervisorFSM_RX_IN_Velocity = 6U; -const uint8_T SupervisorFSM_RX_IN_Voltage = 7U; -const uint8_T SupervisorFSM__IN_NotConfigured = 4U; - -// Named constants for Chart: '/SupervisorRX State Handler' -const uint8_T Superviso_IN_LimitNonConfigured = 1U; -const uint8_T SupervisorFSM_RX_IN_Configured = 1U; -const uint8_T SupervisorFSM_RX_IN_Fault = 2U; -const uint8_T SupervisorFSM_RX_IN_NoFault = 2U; -const uint8_T SupervisorFSM_RX_IN_state1 = 1U; -const uint8_T SupervisorFSM__IN_ButtonPressed = 1U; -const uint8_T SupervisorFS_IN_NotConfigured_d = 3U; -const uint8_T SupervisorF_IN_OverCurrentFault = 3U; -MdlrefDW_SupervisorFSM_RX_T SupervisorFSM_RX_MdlrefDW; - -// Block signals (default storage) -B_SupervisorFSM_RX_c_T SupervisorFSM_RX_B; - -// Block states (default storage) -DW_SupervisorFSM_RX_f_T SupervisorFSM_RX_DW; - -// Forward declaration for local functions -static boolean_T Supervisor_CheckSetPointVoltage(void); -static boolean_T Supervisor_CheckSetPointCurrent(void); -static boolean_T Superviso_CheckSetPointVelocity(void); -static boolean_T Superviso_CheckSetPointPosition(void); - -// Forward declaration for local functions -static boolean_T SupervisorFSM_RX_IsBoardReady(void); -static boolean_T SupervisorFS_isConfigurationSet(void); -static boolean_T SupervisorFS_IsNewCtrl_Velocity(void); -static boolean_T SupervisorFSM_IsNewCtrl_Voltage(void); -static boolean_T SupervisorFSM_IsNewCtrl_Current(void); -static boolean_T SupervisorFS_IsNewCtrl_Position(void); -static boolean_T SupervisorFSM_RX_IsNewCtrl_Idle(void); -static void SupervisorFSM_RX_Voltage(void); - -// Forward declaration for local functions -static ControlModes SupervisorFSM_RX_convert(MCControlModes mccontrolmode); -static void SupervisorFSM_formatMotorConfig(const BUS_MESSAGES_RX *Selector2); -static void SupervisorF_hardwareConfigMotor(void); - -// Forward declaration for local functions -static boolean_T SupervisorFSM_isBoardConfigured(void); - -// Declare global variables for system: model 'SupervisorFSM_RX' -const SensorsData *SupervisorFSM_R_rtu_SensorsData;// '/SensorsData' -const ControlOutputs *SupervisorFS_rtu_ControlOutputs;// '/ControlOutputs' -const BUS_MESSAGES_RX_MULTIPLE *SupervisorFSM_RX_rtu_MessagesRx;// '/MessagesRx' -const BUS_STATUS_RX_MULTIPLE *SupervisorFSM_RX_rtu_StatusRx;// '/StatusRx' -const BUS_CAN_RX_ERRORS_MULTIPLE *SupervisorFSM_RX_rtu_ErrorsRx;// '/ErrorsRx' - -// Function for Chart: '/Chart1' -static boolean_T Supervisor_CheckSetPointVoltage(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Voltage; -} - -// Function for Chart: '/Chart1' -static boolean_T Supervisor_CheckSetPointCurrent(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Current; -} - -// Function for Chart: '/Chart1' -static boolean_T Superviso_CheckSetPointVelocity(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Velocity; -} - -// Function for Chart: '/Chart1' -static boolean_T Superviso_CheckSetPointPosition(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Position; -} - -// System initialize for function-call system: '/SetpointHandler' -void Supervisor_SetpointHandler_Init(void) -{ - // SystemInitialize for Chart: '/Chart1' - SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; - SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; - SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; - SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; -} - -// Output and update for function-call system: '/SetpointHandler' -void SupervisorFSM_R_SetpointHandler(void) -{ - // Chart: '/Chart1' - if (SupervisorFSM_RX_DW.is_active_c4_SupervisorFSM_RX == 0U) { - real32_T newSetpoint; - SupervisorFSM_RX_DW.is_active_c4_SupervisorFSM_RX = 1U; - if (SupervisorFSM_RX_B.receivedNewSetpoint) { - if (Supervisor_CheckSetPointVoltage()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.voltage; - } else if (Supervisor_CheckSetPointCurrent()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.current; - } else if (Superviso_CheckSetPointVelocity()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.velocity; - } else { - newSetpoint = 0.0F; - } - } else { - newSetpoint = SupervisorFSM_RX_B.newSetpoint_m; - } - - SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; - SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; - SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; - SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; - if (Supervisor_CheckSetPointCurrent()) { - SupervisorFSM_RX_B.targets.motorcurrent.current = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (Supervisor_CheckSetPointVoltage()) { - SupervisorFSM_RX_B.targets.motorvoltage.voltage = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (Superviso_CheckSetPointVelocity()) { - SupervisorFSM_RX_B.targets.jointvelocities.velocity = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (Superviso_CheckSetPointPosition()) { - SupervisorFSM_RX_B.targets.jointpositions.position = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } - } else { - real32_T newSetpoint; - if (SupervisorFSM_RX_B.receivedNewSetpoint) { - if (Supervisor_CheckSetPointVoltage()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.voltage; - } else if (Supervisor_CheckSetPointCurrent()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.current; - } else if (Superviso_CheckSetPointVelocity()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.velocity; - } else { - newSetpoint = 0.0F; - } - } else { - newSetpoint = SupervisorFSM_RX_B.newSetpoint_m; - } - - SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; - SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; - SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; - SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; - if (Supervisor_CheckSetPointCurrent()) { - SupervisorFSM_RX_B.targets.motorcurrent.current = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (Supervisor_CheckSetPointVoltage()) { - SupervisorFSM_RX_B.targets.motorvoltage.voltage = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (Superviso_CheckSetPointVelocity()) { - SupervisorFSM_RX_B.targets.jointvelocities.velocity = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (Superviso_CheckSetPointPosition()) { - SupervisorFSM_RX_B.targets.jointpositions.position = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } - } - - // End of Chart: '/Chart1' -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsBoardReady(void) -{ - return SupervisorFSM_RX_B.BoardSt == BoardState_Configured; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFS_isConfigurationSet(void) -{ - return SupervisorFSM_RX_B.areLimitsSet; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFS_IsNewCtrl_Velocity(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Velocity; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_IsNewCtrl_Voltage(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Voltage; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_IsNewCtrl_Current(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Current; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFS_IsNewCtrl_Position(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Position; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsNewCtrl_Idle(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Idle; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static void SupervisorFSM_RX_Voltage(void) -{ - boolean_T guard1 = false; - boolean_T guard2 = false; - boolean_T guard3 = false; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - guard1 = false; - guard2 = false; - guard3 = false; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard1 = true; - } else { - guard3 = true; - } - } else { - guard3 = true; - } - - if (guard3) { - if (SupervisorFS_IsNewCtrl_Velocity()) { - if ((!SupervisorFSM_IsNewCtrl_Voltage()) && - SupervisorFS_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_m = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard2 = true; - } - } else if (!SupervisorFSM_IsNewCtrl_Voltage()) { - if (SupervisorFSM_IsNewCtrl_Current()) { - // Chart: '/ControlMode_SM_motor0' - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFS_rtu_ControlOutputs->Iq_fbk.current; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFS_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - - // Chart: '/ControlMode_SM_motor0' - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFSM_R_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard1 = true; - } else { - guard2 = true; - } - } else { - guard2 = true; - } - } - - if (guard2) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard1) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } -} - -// Output and update for function-call system: '/Control Mode Handler Motor 0' -void Superv_ControlModeHandlerMotor0(void) -{ - // Chart: '/ControlMode_SM_motor0' - if (SupervisorFSM_RX_DW.is_active_c12_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c12_SupervisorFSM_RX = 1U; - if (SupervisorFSM_RX_IsBoardReady() && SupervisorFS_isConfigurationSet()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM__IN_NotConfigured; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; - rtw_disableMotor(); - } - } else { - boolean_T guard1 = false; - boolean_T guard10 = false; - boolean_T guard11 = false; - boolean_T guard2 = false; - boolean_T guard3 = false; - boolean_T guard4 = false; - boolean_T guard5 = false; - boolean_T guard6 = false; - boolean_T guard7 = false; - boolean_T guard8 = false; - boolean_T guard9 = false; - guard1 = false; - guard2 = false; - guard3 = false; - guard4 = false; - guard5 = false; - guard6 = false; - guard7 = false; - guard8 = false; - guard9 = false; - guard10 = false; - guard11 = false; - switch (SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX) { - case SupervisorFSM_RX_IN_Current: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard1 = true; - } else { - guard7 = true; - } - } else { - guard7 = true; - } - break; - - case SupervisorFSM_RX_IN_HWFault: - { - boolean_T tmp; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - tmp = !SupervisorFSM_RX_B.isInOverCurrent; - if (tmp && SupervisorFSM_RX_IsNewCtrl_Idle() && - (!SupervisorFS_isConfigurationSet())) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM__IN_NotConfigured; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; - rtw_disableMotor(); - } else if (tmp && SupervisorFSM_RX_IsNewCtrl_Idle()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - } - break; - - case SupervisorFSM_RX_IN_Idle: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard3 = true; - } else { - guard8 = true; - } - } else { - guard8 = true; - } - break; - - case SupervisorFSM__IN_NotConfigured: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; - if (SupervisorFSM_RX_IsBoardReady() && SupervisorFS_isConfigurationSet()) - { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - break; - - case SupervisorFSM_RX_IN_Position: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard4 = true; - } else { - guard11 = true; - } - } else { - guard11 = true; - } - break; - - case SupervisorFSM_RX_IN_Velocity: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard5 = true; - } else { - guard10 = true; - } - } else { - guard10 = true; - } - break; - - default: - // case IN_Voltage: - SupervisorFSM_RX_Voltage(); - break; - } - - if (guard11) { - if ((!SupervisorFSM_RX_IsNewCtrl_Idle()) && - (!SupervisorFS_IsNewCtrl_Position())) { - if (SupervisorFSM_IsNewCtrl_Current()) { - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFS_rtu_ControlOutputs->Iq_fbk.current; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_m = SupervisorFS_rtu_ControlOutputs->Vq; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFS_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_m = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard9 = true; - } - } else { - guard9 = true; - } - } - - if (guard10) { - if (!SupervisorFS_IsNewCtrl_Velocity()) { - if (SupervisorFSM_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_m = SupervisorFS_rtu_ControlOutputs->Vq; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_IsNewCtrl_Current()) { - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFS_rtu_ControlOutputs->Iq_fbk.current; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFS_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFSM_R_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard5 = true; - } else { - guard6 = true; - } - } else { - guard6 = true; - } - } - - if (guard9) { - if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard4 = true; - } - } - - if (guard8) { - if (!SupervisorFSM_RX_IsNewCtrl_Idle()) { - rtw_enableMotor(); - if (SupervisorFS_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFSM_R_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_IsNewCtrl_Current()) { - SupervisorFSM_RX_B.newSetpoint_m = 0.0F; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_m = 0.0F; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFS_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_m = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard3 = true; - } - } else { - guard3 = true; - } - } - - if (guard7) { - if ((!SupervisorFSM_RX_IsNewCtrl_Idle()) && - (!SupervisorFS_IsNewCtrl_Position())) { - if (!SupervisorFSM_IsNewCtrl_Current()) { - if (SupervisorFSM_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFS_rtu_ControlOutputs->Vq; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFS_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_m = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard2 = true; - } - } else { - guard2 = true; - } - } else if (!SupervisorFSM_IsNewCtrl_Current()) { - if (SupervisorFS_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - SupervisorFSM_RX_B.newSetpoint_m = - SupervisorFSM_R_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard1 = true; - } else { - guard2 = true; - } - } else { - guard2 = true; - } - } - - if (guard6) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_m = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard5) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard4) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard3) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard2) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard1) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - } - - // End of Chart: '/ControlMode_SM_motor0' -} - -// System initialize for function-call system: '/Limits Handler' -void SupervisorFS_LimitsHandler_Init(void) -{ - // SystemInitialize for Chart: '/Chart' - SupervisorFSM_RX_B.thresholds.jntPosMin = 0.0F; - SupervisorFSM_RX_B.thresholds.jntPosMax = 0.0F; - SupervisorFSM_RX_B.thresholds.hardwareJntPosMin = 0.0F; - SupervisorFSM_RX_B.thresholds.hardwareJntPosMax = 0.0F; - SupervisorFSM_RX_B.thresholds.rotorPosMin = 0.0F; - SupervisorFSM_RX_B.thresholds.rotorPosMax = 0.0F; - SupervisorFSM_RX_B.thresholds.jntVelMax = 0.0F; - SupervisorFSM_RX_B.thresholds.velocityTimeout = 0U; - SupervisorFSM_RX_B.thresholds.motorNominalCurrents = 0.0F; - SupervisorFSM_RX_B.thresholds.motorPeakCurrents = 0.0F; - SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = 0.0F; - SupervisorFSM_RX_B.thresholds.motorPwmLimit = 0U; -} - -// Output and update for function-call system: '/Limits Handler' -void SupervisorFSM_RX_LimitsHandler(void) -{ - // Chart: '/Chart' - if (SupervisorFSM_RX_DW.is_active_c14_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c14_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_B.thresholds = InitConfParams.thresholds; - if (SupervisorFSM_RX_B.newLimit.type == ControlModes_Current) { - SupervisorFSM_RX_B.thresholds.motorNominalCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.nominal); - SupervisorFSM_RX_B.thresholds.motorPeakCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.peak); - SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.overload); - if (!SupervisorFSM_RX_B.areLimitsSet) { - SupervisorFSM_RX_B.areLimitsSet = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - Superv_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - } - } else if (SupervisorFSM_RX_B.newLimit.type == ControlModes_Current) { - SupervisorFSM_RX_B.thresholds.motorNominalCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.nominal); - SupervisorFSM_RX_B.thresholds.motorPeakCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.peak); - SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.overload); - if (!SupervisorFSM_RX_B.areLimitsSet) { - SupervisorFSM_RX_B.areLimitsSet = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - Superv_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - } - - // End of Chart: '/Chart' -} - -// System initialize for function-call system: '/PID Handler' -void SupervisorFSM_R_PIDHandler_Init(void) -{ - // SystemInitialize for Chart: '/Chart' - SupervisorFSM_RX_B.CurrentPID.OutMax = 0.0F; - SupervisorFSM_RX_B.CurrentPID.OutMin = 0.0F; - SupervisorFSM_RX_B.CurrentPID.P = 0.0F; - SupervisorFSM_RX_B.CurrentPID.I = 0.0F; - SupervisorFSM_RX_B.CurrentPID.D = 0.0F; - SupervisorFSM_RX_B.CurrentPID.N = 0.0F; - SupervisorFSM_RX_B.CurrentPID.I0 = 0.0F; - SupervisorFSM_RX_B.CurrentPID.D0 = 0.0F; - SupervisorFSM_RX_B.CurrentPID.shift_factor = 0U; - SupervisorFSM_RX_B.VelocityPID.OutMax = 0.0F; - SupervisorFSM_RX_B.VelocityPID.OutMin = 0.0F; - SupervisorFSM_RX_B.VelocityPID.P = 0.0F; - SupervisorFSM_RX_B.VelocityPID.I = 0.0F; - SupervisorFSM_RX_B.VelocityPID.D = 0.0F; - SupervisorFSM_RX_B.VelocityPID.N = 0.0F; - SupervisorFSM_RX_B.VelocityPID.I0 = 0.0F; - SupervisorFSM_RX_B.VelocityPID.D0 = 0.0F; - SupervisorFSM_RX_B.VelocityPID.shift_factor = 0U; - SupervisorFSM_RX_B.PositionPID.OutMax = 0.0F; - SupervisorFSM_RX_B.PositionPID.OutMin = 0.0F; - SupervisorFSM_RX_B.PositionPID.P = 0.0F; - SupervisorFSM_RX_B.PositionPID.I = 0.0F; - SupervisorFSM_RX_B.PositionPID.D = 0.0F; - SupervisorFSM_RX_B.PositionPID.N = 0.0F; - SupervisorFSM_RX_B.PositionPID.I0 = 0.0F; - SupervisorFSM_RX_B.PositionPID.D0 = 0.0F; - SupervisorFSM_RX_B.PositionPID.shift_factor = 0U; - SupervisorFSM_RX_B.OpenLoopPID.OutMax = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.OutMin = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.P = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.I = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.D = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.N = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.I0 = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.D0 = 0.0F; - SupervisorFSM_RX_B.OpenLoopPID.shift_factor = 0U; -} - -// Output and update for function-call system: '/PID Handler' -void SupervisorFSM_RX_PIDHandler(void) -{ - // Chart: '/Chart' - if (SupervisorFSM_RX_DW.is_active_c3_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c3_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_B.PositionPID = InitConfParams.PosLoopPID; - SupervisorFSM_RX_B.CurrentPID = InitConfParams.CurLoopPID; - SupervisorFSM_RX_B.VelocityPID = InitConfParams.VelLoopPID; - switch (SupervisorFSM_RX_B.newPIDType) { - case ControlModes_Current: - SupervisorFSM_RX_B.CurrentPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.CurrentPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.CurrentPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.CurrentPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Position: - SupervisorFSM_RX_B.PositionPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.PositionPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.PositionPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.PositionPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Voltage: - SupervisorFSM_RX_B.OpenLoopPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.OpenLoopPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.OpenLoopPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.OpenLoopPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Velocity: - SupervisorFSM_RX_B.VelocityPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.VelocityPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.VelocityPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.VelocityPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - } - } else { - switch (SupervisorFSM_RX_B.newPIDType) { - case ControlModes_Current: - SupervisorFSM_RX_B.CurrentPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.CurrentPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.CurrentPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.CurrentPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Position: - SupervisorFSM_RX_B.PositionPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.PositionPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.PositionPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.PositionPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Voltage: - SupervisorFSM_RX_B.OpenLoopPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.OpenLoopPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.OpenLoopPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.OpenLoopPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Velocity: - SupervisorFSM_RX_B.VelocityPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.VelocityPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.VelocityPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.VelocityPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - } - } - - // End of Chart: '/Chart' -} - -// Function for Chart: '/CAN Event Dispatcher' -static ControlModes SupervisorFSM_RX_convert(MCControlModes mccontrolmode) -{ - ControlModes controlmode; - switch (mccontrolmode) { - case MCControlModes_Idle: - controlmode = ControlModes_Idle; - break; - - case MCControlModes_Current: - controlmode = ControlModes_Current; - break; - - case MCControlModes_SpeedVoltage: - controlmode = ControlModes_Velocity; - break; - - case MCControlModes_OpenLoop: - controlmode = ControlModes_Voltage; - break; - - default: - controlmode = ControlModes_Idle; - break; - } - - return controlmode; -} - -// Function for Chart: '/CAN Event Dispatcher' -static void SupervisorFSM_formatMotorConfig(const BUS_MESSAGES_RX *Selector2) -{ - SupervisorFSM_RX_B.motorConfig.use_index = Selector2->motor_config.use_index; - SupervisorFSM_RX_B.motorConfig.pole_pairs = static_cast(rt_roundd_snf - (static_cast(Selector2->motor_config.number_poles) / 2.0)); - SupervisorFSM_RX_B.motorConfig.encoder_tolerance = - Selector2->motor_config.encoder_tolerance; - SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution = - Selector2->motor_config.rotor_encoder_resolution; - SupervisorFSM_RX_B.motorConfig.rotor_index_offset = - Selector2->motor_config.rotor_index_offset; - SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder = - Selector2->motor_config.has_quadrature_encoder; - SupervisorFSM_RX_B.motorConfig.has_hall_sens = - Selector2->motor_config.has_hall_sens; - SupervisorFSM_RX_B.motorConfig.has_speed_quadrature_encoder = - Selector2->motor_config.has_speed_quadrature_encoder; - SupervisorFSM_RX_B.motorConfig.has_torque_sens = - Selector2->motor_config.has_torque_sens; - SupervisorFSM_RX_B.motorConfig.enable_verbosity = - Selector2->motor_config.enable_verbosity; -} - -// Function for Chart: '/CAN Event Dispatcher' -static void SupervisorF_hardwareConfigMotor(void) -{ - rtw_configMotor(SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution, - SupervisorFSM_RX_B.motorConfig.pole_pairs, static_cast - (SupervisorFSM_RX_B.motorConfig.has_hall_sens), 1U, 21845U); -} - -// System initialize for function-call system: '/CAN Message Handler' -void Supervis_CANMessageHandler_Init(void) -{ - // SystemInitialize for Chart: '/CAN Event Dispatcher' - SupervisorFSM_RX_B.newSetpoint.current = 0.0F; - SupervisorFSM_RX_B.newSetpoint.voltage = 0.0F; - SupervisorFSM_RX_B.newSetpoint.velocity = 0.0F; - SupervisorFSM_RX_B.newLimit.overload = 0.0F; - SupervisorFSM_RX_B.newLimit.peak = 0.0F; - SupervisorFSM_RX_B.newLimit.nominal = 0.0F; - SupervisorFSM_RX_B.newLimit.type = ControlModes_NotConfigured; - SupervisorFSM_RX_B.newPID.motor = false; - SupervisorFSM_RX_B.newPID.Kp = 0.0F; - SupervisorFSM_RX_B.newPID.Ki = 0.0F; - SupervisorFSM_RX_B.newPID.Kd = 0.0F; - SupervisorFSM_RX_B.newPID.Ks = 0U; - SupervisorFSM_RX_B.motorConfig.has_hall_sens = false; - SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder = false; - SupervisorFSM_RX_B.motorConfig.has_speed_quadrature_encoder = false; - SupervisorFSM_RX_B.motorConfig.has_torque_sens = false; - SupervisorFSM_RX_B.motorConfig.use_index = false; - SupervisorFSM_RX_B.motorConfig.enable_verbosity = false; - SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution = 0; - SupervisorFSM_RX_B.motorConfig.rotor_index_offset = 0; - SupervisorFSM_RX_B.motorConfig.encoder_tolerance = 0U; - SupervisorFSM_RX_B.motorConfig.pole_pairs = 0U; - SupervisorFSM_RX_B.motorConfig.Kbemf = 0.0F; - SupervisorFSM_RX_B.motorConfig.Rphase = 0.0F; - SupervisorFSM_RX_B.motorConfig.Imin = 0.0F; - SupervisorFSM_RX_B.motorConfig.Imax = 0.0F; - SupervisorFSM_RX_B.motorConfig.Vcc = 0.0F; - SupervisorFSM_RX_B.motorConfig.Vmax = 0.0F; - SupervisorFSM_RX_B.estimationConfig.velocity_mode = - EstimationVelocityModes_Disabled; - - // SystemInitialize for Function Call SubSystem: '/SetpointHandler' - Supervisor_SetpointHandler_Init(); - - // End of SystemInitialize for SubSystem: '/SetpointHandler' - - // SystemInitialize for Function Call SubSystem: '/Limits Handler' - SupervisorFS_LimitsHandler_Init(); - - // End of SystemInitialize for SubSystem: '/Limits Handler' - - // SystemInitialize for Function Call SubSystem: '/PID Handler' - SupervisorFSM_R_PIDHandler_Init(); - - // End of SystemInitialize for SubSystem: '/PID Handler' -} - -// Output and update for function-call system: '/CAN Message Handler' -void SupervisorFSM_CANMessageHandler(void) -{ - BUS_MESSAGES_RX Selector2; - - // Selector: '/Selector2' - Selector2 = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1]; - - // Chart: '/CAN Event Dispatcher' incorporates: - // Selector: '/Selector' - // Selector: '/Selector1' - // Selector: '/Selector2' - - if (SupervisorFSM_RX_DW.is_active_c11_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c11_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_B.motorConfig = InitConfParams.motorconfig; - SupervisorFSM_RX_B.estimationConfig = InitConfParams.estimationconfig; - if (!SupervisorFSM_RX_rtu_ErrorsRx->errors[SupervisorFSM_RX_B.messageIndex - - 1].event) { - if (SupervisorFSM_RX_rtu_StatusRx->status[SupervisorFSM_RX_B.messageIndex - - 1].control_mode) { - SupervisorFSM_RX_B.requiredControlMode = SupervisorFSM_RX_convert - (SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].control_mode.mode); - SupervisorFSM_RX_B.receivedNewSetpoint = false; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - Superv_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_limit) { - SupervisorFSM_RX_B.newLimit.type = ControlModes_Current; - SupervisorFSM_RX_B.newLimit.peak = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.peak; - SupervisorFSM_RX_B.newLimit.nominal = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.nominal; - SupervisorFSM_RX_B.newLimit.overload = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.overload; - - // Outputs for Function Call SubSystem: '/Limits Handler' - SupervisorFSM_RX_LimitsHandler(); - - // End of Outputs for SubSystem: '/Limits Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Current; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].velocity_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Velocity; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].motor_config) { - SupervisorFSM_formatMotorConfig(&Selector2); - SupervisorF_hardwareConfigMotor(); - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].desired_targets) { - SupervisorFSM_RX_B.newSetpoint = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].desired_targets; - SupervisorFSM_RX_B.enableSendingMsgStatus = true; - SupervisorFSM_RX_B.receivedNewSetpoint = true; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - } - } else { - SupervisorFSM_RX_B.enableSendingMsgStatus = - ((SupervisorFSM_RX_B.messageIndex != 1) && - SupervisorFSM_RX_B.enableSendingMsgStatus); - if (!SupervisorFSM_RX_rtu_ErrorsRx->errors[SupervisorFSM_RX_B.messageIndex - - 1].event) { - if (SupervisorFSM_RX_rtu_StatusRx->status[SupervisorFSM_RX_B.messageIndex - - 1].control_mode) { - SupervisorFSM_RX_B.requiredControlMode = SupervisorFSM_RX_convert - (SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].control_mode.mode); - SupervisorFSM_RX_B.receivedNewSetpoint = false; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - Superv_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_limit) { - SupervisorFSM_RX_B.newLimit.type = ControlModes_Current; - SupervisorFSM_RX_B.newLimit.peak = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.peak; - SupervisorFSM_RX_B.newLimit.nominal = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.nominal; - SupervisorFSM_RX_B.newLimit.overload = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.overload; - - // Outputs for Function Call SubSystem: '/Limits Handler' - SupervisorFSM_RX_LimitsHandler(); - - // End of Outputs for SubSystem: '/Limits Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Current; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].velocity_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Velocity; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].motor_config) { - SupervisorFSM_formatMotorConfig(&Selector2); - SupervisorF_hardwareConfigMotor(); - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].desired_targets) { - SupervisorFSM_RX_B.newSetpoint = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].desired_targets; - SupervisorFSM_RX_B.enableSendingMsgStatus = true; - SupervisorFSM_RX_B.receivedNewSetpoint = true; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_R_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - } - } - - // End of Chart: '/CAN Event Dispatcher' -} - -// Function for Chart: '/SupervisorRX State Handler' -static boolean_T SupervisorFSM_isBoardConfigured(void) -{ - return true; -} - -// System initialize for referenced model: 'SupervisorFSM_RX' -void SupervisorFSM_RX_Init(Flags *rty_Flags, ConfigurationParameters - *rty_ConfigurationParameters) -{ - // SystemInitialize for Chart: '/SupervisorRX State Handler' incorporates: - // SubSystem: '/CAN Message Handler' - - Supervis_CANMessageHandler_Init(); - - // SystemInitialize for BusCreator: '/Bus Creator' - rty_Flags->control_mode = SupervisorFSM_RX_B.controlModeDefined; - rty_Flags->enable_sending_msg_status = - SupervisorFSM_RX_B.enableSendingMsgStatus; - rty_Flags->fault_button = false; - - // SystemInitialize for BusCreator: '/Bus Creator1' - rty_ConfigurationParameters->motorconfig = SupervisorFSM_RX_B.motorConfig; - rty_ConfigurationParameters->estimationconfig = - SupervisorFSM_RX_B.estimationConfig; - rty_ConfigurationParameters->CurLoopPID = SupervisorFSM_RX_B.CurrentPID; - rty_ConfigurationParameters->PosLoopPID = SupervisorFSM_RX_B.PositionPID; - rty_ConfigurationParameters->VelLoopPID = SupervisorFSM_RX_B.VelocityPID; - rty_ConfigurationParameters->DirLoopPID = SupervisorFSM_RX_B.OpenLoopPID; - rty_ConfigurationParameters->thresholds = SupervisorFSM_RX_B.thresholds; -} - -// Output and update for referenced model: 'SupervisorFSM_RX' -void SupervisorFSM_RX(const SensorsData *rtu_SensorsData, const ExternalFlags - *rtu_ExternalFlags, const ControlOutputs - *rtu_ControlOutputs, const BUS_MESSAGES_RX_MULTIPLE - *rtu_MessagesRx, const BUS_STATUS_RX_MULTIPLE - *rtu_StatusRx, const BUS_CAN_RX_ERRORS_MULTIPLE - *rtu_ErrorsRx, Flags *rty_Flags, Targets *rty_Targets, - ConfigurationParameters *rty_ConfigurationParameters) -{ - real32_T rtb_UnitDelay_thresholds_motorO; - SupervisorFSM_R_rtu_SensorsData = rtu_SensorsData; - SupervisorFS_rtu_ControlOutputs = rtu_ControlOutputs; - SupervisorFSM_RX_rtu_MessagesRx = rtu_MessagesRx; - SupervisorFSM_RX_rtu_StatusRx = rtu_StatusRx; - SupervisorFSM_RX_rtu_ErrorsRx = rtu_ErrorsRx; - - // UnitDelay: '/Unit Delay' - rtb_UnitDelay_thresholds_motorO = - SupervisorFSM_RX_DW.UnitDelay_DSTATE.thresholds.motorOverloadCurrents; - - // Chart: '/SupervisorRX State Handler' incorporates: - // UnitDelay: '/Unit Delay' - - SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev = - SupervisorFSM_RX_DW.ExternalFlags_fault_button_star; - SupervisorFSM_RX_DW.ExternalFlags_fault_button_star = - rtu_ExternalFlags->fault_button; - if (SupervisorFSM_RX_DW.is_active_c2_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev = - rtu_ExternalFlags->fault_button; - SupervisorFSM_RX_DW.ExternalFlags_fault_button_star = - rtu_ExternalFlags->fault_button; - SupervisorFSM_RX_DW.is_active_c2_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_DW.is_active_STATE_HANDLER = 1U; - SupervisorFSM_RX_DW.is_STATE_HANDLER = SupervisorFS_IN_NotConfigured_d; - SupervisorFSM_RX_B.BoardSt = BoardState_NotConfigured; - SupervisorFSM_RX_DW.is_active_FAULT_HANDLER = 1U; - SupervisorFSM_RX_DW.is_active_FaultButtonPressed = 1U; - SupervisorFSM_RX_DW.is_FaultButtonPressed = SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isFaultButtonPressed = false; - SupervisorFSM_RX_DW.is_active_OverCurrent = 1U; - SupervisorFSM_RX_DW.is_OverCurrent = Superviso_IN_LimitNonConfigured; - SupervisorFSM_RX_DW.is_active_CAN_MESSAGES_FOR_LOOP = 1U; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - - // Outputs for Function Call SubSystem: '/Limits Handler' - SupervisorFSM_RX_LimitsHandler(); - - // End of Outputs for SubSystem: '/Limits Handler' - SupervisorFSM_RX_B.messageIndex = 1; - while (SupervisorFSM_RX_B.messageIndex <= CAN_MAX_NUM_PACKETS) { - // Outputs for Function Call SubSystem: '/CAN Message Handler' - SupervisorFSM_CANMessageHandler(); - - // End of Outputs for SubSystem: '/CAN Message Handler' - SupervisorFSM_RX_B.messageIndex++; - } - - SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP = SupervisorFSM_RX_IN_state1; - } else { - if (SupervisorFSM_RX_DW.is_active_STATE_HANDLER != 0U) { - switch (SupervisorFSM_RX_DW.is_STATE_HANDLER) { - case SupervisorFSM_RX_IN_Configured: - SupervisorFSM_RX_B.BoardSt = BoardState_Configured; - break; - - case SupervisorFSM_RX_IN_Fault: - SupervisorFSM_RX_B.BoardSt = BoardState_Fault; - break; - - case SupervisorFS_IN_NotConfigured_d: - SupervisorFSM_RX_B.BoardSt = BoardState_NotConfigured; - if (SupervisorFSM_isBoardConfigured()) { - SupervisorFSM_RX_DW.is_STATE_HANDLER = SupervisorFSM_RX_IN_Configured; - SupervisorFSM_RX_B.BoardSt = BoardState_Configured; - } - break; - } - } - - if (SupervisorFSM_RX_DW.is_active_FAULT_HANDLER != 0U) { - if (SupervisorFSM_RX_DW.is_active_FaultButtonPressed != 0U) { - switch (SupervisorFSM_RX_DW.is_FaultButtonPressed) { - case SupervisorFSM__IN_ButtonPressed: - SupervisorFSM_RX_B.isFaultButtonPressed = true; - if ((SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev != - SupervisorFSM_RX_DW.ExternalFlags_fault_button_star) && - (!SupervisorFSM_RX_DW.ExternalFlags_fault_button_star)) { - SupervisorFSM_RX_DW.is_FaultButtonPressed = - SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isFaultButtonPressed = false; - } - break; - - case SupervisorFSM_RX_IN_NoFault: - SupervisorFSM_RX_B.isFaultButtonPressed = false; - if ((SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev != - SupervisorFSM_RX_DW.ExternalFlags_fault_button_star) && - SupervisorFSM_RX_DW.ExternalFlags_fault_button_star) { - SupervisorFSM_RX_DW.is_FaultButtonPressed = - SupervisorFSM__IN_ButtonPressed; - SupervisorFSM_RX_B.isFaultButtonPressed = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - Superv_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - break; - } - } - - if (SupervisorFSM_RX_DW.is_active_OverCurrent != 0U) { - switch (SupervisorFSM_RX_DW.is_OverCurrent) { - case Superviso_IN_LimitNonConfigured: - if (SupervisorFSM_RX_B.areLimitsSet) { - SupervisorFSM_RX_DW.is_OverCurrent = SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isInOverCurrent = false; - - // MotorFaultFlags.overCurrent=0; - } - break; - - case SupervisorFSM_RX_IN_NoFault: - SupervisorFSM_RX_B.isInOverCurrent = false; - if (std::abs(rtu_ControlOutputs->Iq_fbk.current) >= - rtb_UnitDelay_thresholds_motorO) { - SupervisorFSM_RX_DW.is_OverCurrent = SupervisorF_IN_OverCurrentFault; - - // MotorFaultFlags.overCurrent=1; - SupervisorFSM_RX_B.isInOverCurrent = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - Superv_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - break; - - case SupervisorF_IN_OverCurrentFault: - SupervisorFSM_RX_B.isInOverCurrent = true; - if (std::abs(rtu_ControlOutputs->Iq_fbk.current) < - rtb_UnitDelay_thresholds_motorO) { - SupervisorFSM_RX_DW.is_OverCurrent = SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isInOverCurrent = false; - - // MotorFaultFlags.overCurrent=0; - } - break; - } - } - } - - if ((SupervisorFSM_RX_DW.is_active_CAN_MESSAGES_FOR_LOOP != 0U) && - (SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP == - SupervisorFSM_RX_IN_state1)) { - SupervisorFSM_RX_B.messageIndex = 1; - while (SupervisorFSM_RX_B.messageIndex <= CAN_MAX_NUM_PACKETS) { - // Outputs for Function Call SubSystem: '/CAN Message Handler' - SupervisorFSM_CANMessageHandler(); - - // End of Outputs for SubSystem: '/CAN Message Handler' - SupervisorFSM_RX_B.messageIndex++; - } - - SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP = SupervisorFSM_RX_IN_state1; - } - } - - // End of Chart: '/SupervisorRX State Handler' - - // BusCreator: '/Bus Creator' - rty_Flags->control_mode = SupervisorFSM_RX_B.controlModeDefined; - rty_Flags->enable_sending_msg_status = - SupervisorFSM_RX_B.enableSendingMsgStatus; - rty_Flags->fault_button = SupervisorFSM_RX_B.isFaultButtonPressed; - - // BusCreator: '/Bus Creator1' - rty_ConfigurationParameters->motorconfig = SupervisorFSM_RX_B.motorConfig; - rty_ConfigurationParameters->estimationconfig = - SupervisorFSM_RX_B.estimationConfig; - rty_ConfigurationParameters->CurLoopPID = SupervisorFSM_RX_B.CurrentPID; - rty_ConfigurationParameters->PosLoopPID = SupervisorFSM_RX_B.PositionPID; - rty_ConfigurationParameters->VelLoopPID = SupervisorFSM_RX_B.VelocityPID; - rty_ConfigurationParameters->DirLoopPID = SupervisorFSM_RX_B.OpenLoopPID; - rty_ConfigurationParameters->thresholds = SupervisorFSM_RX_B.thresholds; - - // Switch: '/Switch2' incorporates: - // BusCreator: '/Bus Creator2' - // BusCreator: '/Bus Creator3' - // BusCreator: '/Bus Creator4' - // BusCreator: '/Bus Creator5' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - - if (SupervisorFSM_RX_B.hasSetpointChanged) { - *rty_Targets = SupervisorFSM_RX_B.targets; - } else { - rty_Targets->jointpositions.position = 0.0F; - rty_Targets->jointvelocities.velocity = 0.0F; - rty_Targets->motorcurrent.current = 0.0F; - rty_Targets->motorvoltage.voltage = 0.0F; - } - - // End of Switch: '/Switch2' - - // Update for UnitDelay: '/Unit Delay' - SupervisorFSM_RX_DW.UnitDelay_DSTATE = *rty_ConfigurationParameters; -} - -// Model initialize function -void SupervisorFSM_RX_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_SupervisorFSM_RX_T *const SupervisorFSM_RX_M = - &(SupervisorFSM_RX_MdlrefDW.rtm); - - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(SupervisorFSM_RX_M, rt_errorStatus); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_RX.cpp +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "SupervisorFSM_RX.h" +#include "rtwtypes.h" +#include "SupervisorFSM_RX_types.h" +#include +#include "rt_roundd_snf.h" +#include "rtw_defines.h" +#include "SupervisorFSM_RX_private.h" + +// Named constants for Chart: '/ControlMode_SM_motor0' +const uint8_T SupervisorFSM_RX_IN_Current = 1U; +const uint8_T SupervisorFSM_RX_IN_HWFault = 2U; +const uint8_T SupervisorFSM_RX_IN_Idle = 3U; +const uint8_T SupervisorFSM_RX_IN_Position = 5U; +const uint8_T SupervisorFSM_RX_IN_Velocity = 6U; +const uint8_T SupervisorFSM_RX_IN_Voltage = 7U; +const uint8_T SupervisorFSM__IN_NotConfigured = 4U; + +// Named constants for Chart: '/SupervisorRX State Handler' +const uint8_T Superviso_IN_LimitNonConfigured = 1U; +const uint8_T SupervisorFSM_RX_IN_Configured = 1U; +const uint8_T SupervisorFSM_RX_IN_Fault = 2U; +const uint8_T SupervisorFSM_RX_IN_NoFault = 2U; +const uint8_T SupervisorFSM_RX_IN_state1 = 1U; +const uint8_T SupervisorFSM__IN_ButtonPressed = 1U; +const uint8_T SupervisorFS_IN_NotConfigured_d = 3U; +const uint8_T SupervisorF_IN_OverCurrentFault = 3U; +MdlrefDW_SupervisorFSM_RX_T SupervisorFSM_RX_MdlrefDW; + +// Block signals (default storage) +B_SupervisorFSM_RX_c_T SupervisorFSM_RX_B; + +// Block states (default storage) +DW_SupervisorFSM_RX_f_T SupervisorFSM_RX_DW; + +// Forward declaration for local functions +static boolean_T Supervisor_CheckSetPointVoltage(void); +static boolean_T Supervisor_CheckSetPointCurrent(void); +static boolean_T Superviso_CheckSetPointVelocity(void); +static boolean_T Superviso_CheckSetPointPosition(void); + +// Forward declaration for local functions +static boolean_T SupervisorFSM_RX_IsBoardReady(void); +static boolean_T SupervisorFS_isConfigurationSet(void); +static boolean_T SupervisorFS_IsNewCtrl_Velocity(void); +static boolean_T SupervisorFSM_IsNewCtrl_Voltage(void); +static boolean_T SupervisorFSM_IsNewCtrl_Current(void); +static boolean_T SupervisorFS_IsNewCtrl_Position(void); +static boolean_T SupervisorFSM_RX_IsNewCtrl_Idle(void); +static void SupervisorFSM_RX_Voltage(void); + +// Forward declaration for local functions +static ControlModes SupervisorFSM_RX_convert(MCControlModes mccontrolmode); +static void SupervisorFSM_formatMotorConfig(const BUS_MESSAGES_RX *Selector2); +static void SupervisorF_hardwareConfigMotor(void); + +// Forward declaration for local functions +static boolean_T SupervisorFSM_isBoardConfigured(void); + +// Declare global variables for system: model 'SupervisorFSM_RX' +const SensorsData *SupervisorFSM_R_rtu_SensorsData;// '/SensorsData' +const ControlOutputs *SupervisorFS_rtu_ControlOutputs;// '/ControlOutputs' +const BUS_MESSAGES_RX_MULTIPLE *SupervisorFSM_RX_rtu_MessagesRx;// '/MessagesRx' +const BUS_STATUS_RX_MULTIPLE *SupervisorFSM_RX_rtu_StatusRx;// '/StatusRx' +const BUS_CAN_RX_ERRORS_MULTIPLE *SupervisorFSM_RX_rtu_ErrorsRx;// '/ErrorsRx' + +// Function for Chart: '/Chart1' +static boolean_T Supervisor_CheckSetPointVoltage(void) +{ + return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Voltage; +} + +// Function for Chart: '/Chart1' +static boolean_T Supervisor_CheckSetPointCurrent(void) +{ + return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Current; +} + +// Function for Chart: '/Chart1' +static boolean_T Superviso_CheckSetPointVelocity(void) +{ + return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Velocity; +} + +// Function for Chart: '/Chart1' +static boolean_T Superviso_CheckSetPointPosition(void) +{ + return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Position; +} + +// System initialize for function-call system: '/SetpointHandler' +void Supervisor_SetpointHandler_Init(void) +{ + // SystemInitialize for Chart: '/Chart1' + SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; + SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; + SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; + SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; +} + +// Output and update for function-call system: '/SetpointHandler' +void SupervisorFSM_R_SetpointHandler(void) +{ + // Chart: '/Chart1' + if (SupervisorFSM_RX_DW.is_active_c4_SupervisorFSM_RX == 0U) { + real32_T newSetpoint; + SupervisorFSM_RX_DW.is_active_c4_SupervisorFSM_RX = 1U; + if (SupervisorFSM_RX_B.receivedNewSetpoint) { + if (Supervisor_CheckSetPointVoltage()) { + newSetpoint = SupervisorFSM_RX_B.newSetpoint.voltage; + } else if (Supervisor_CheckSetPointCurrent()) { + newSetpoint = SupervisorFSM_RX_B.newSetpoint.current; + } else if (Superviso_CheckSetPointVelocity()) { + newSetpoint = SupervisorFSM_RX_B.newSetpoint.velocity; + } else { + newSetpoint = 0.0F; + } + } else { + newSetpoint = SupervisorFSM_RX_B.newSetpoint_m; + } + + SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; + SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; + SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; + SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; + if (Supervisor_CheckSetPointCurrent()) { + SupervisorFSM_RX_B.targets.motorcurrent.current = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } else if (Supervisor_CheckSetPointVoltage()) { + SupervisorFSM_RX_B.targets.motorvoltage.voltage = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } else if (Superviso_CheckSetPointVelocity()) { + SupervisorFSM_RX_B.targets.jointvelocities.velocity = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } else if (Superviso_CheckSetPointPosition()) { + SupervisorFSM_RX_B.targets.jointpositions.position = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } + } else { + real32_T newSetpoint; + if (SupervisorFSM_RX_B.receivedNewSetpoint) { + if (Supervisor_CheckSetPointVoltage()) { + newSetpoint = SupervisorFSM_RX_B.newSetpoint.voltage; + } else if (Supervisor_CheckSetPointCurrent()) { + newSetpoint = SupervisorFSM_RX_B.newSetpoint.current; + } else if (Superviso_CheckSetPointVelocity()) { + newSetpoint = SupervisorFSM_RX_B.newSetpoint.velocity; + } else { + newSetpoint = 0.0F; + } + } else { + newSetpoint = SupervisorFSM_RX_B.newSetpoint_m; + } + + SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; + SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; + SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; + SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; + if (Supervisor_CheckSetPointCurrent()) { + SupervisorFSM_RX_B.targets.motorcurrent.current = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } else if (Supervisor_CheckSetPointVoltage()) { + SupervisorFSM_RX_B.targets.motorvoltage.voltage = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } else if (Superviso_CheckSetPointVelocity()) { + SupervisorFSM_RX_B.targets.jointvelocities.velocity = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } else if (Superviso_CheckSetPointPosition()) { + SupervisorFSM_RX_B.targets.jointpositions.position = newSetpoint; + SupervisorFSM_RX_B.hasSetpointChanged = true; + } + } + + // End of Chart: '/Chart1' +} + +// Function for Chart: '/ControlMode_SM_motor0' +static boolean_T SupervisorFSM_RX_IsBoardReady(void) +{ + return SupervisorFSM_RX_B.BoardSt == BoardState_Configured; +} + +// Function for Chart: '/ControlMode_SM_motor0' +static boolean_T SupervisorFS_isConfigurationSet(void) +{ + return SupervisorFSM_RX_B.areLimitsSet; +} + +// Function for Chart: '/ControlMode_SM_motor0' +static boolean_T SupervisorFS_IsNewCtrl_Velocity(void) +{ + return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Velocity; +} + +// Function for Chart: '/ControlMode_SM_motor0' +static boolean_T SupervisorFSM_IsNewCtrl_Voltage(void) +{ + return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Voltage; +} + +// Function for Chart: '/ControlMode_SM_motor0' +static boolean_T SupervisorFSM_IsNewCtrl_Current(void) +{ + return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Current; +} + +// Function for Chart: '/ControlMode_SM_motor0' +static boolean_T SupervisorFS_IsNewCtrl_Position(void) +{ + return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Position; +} + +// Function for Chart: '/ControlMode_SM_motor0' +static boolean_T SupervisorFSM_RX_IsNewCtrl_Idle(void) +{ + return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Idle; +} + +// Function for Chart: '/ControlMode_SM_motor0' +static void SupervisorFSM_RX_Voltage(void) +{ + boolean_T guard1 = false; + boolean_T guard2 = false; + boolean_T guard3 = false; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; + guard1 = false; + guard2 = false; + guard3 = false; + if (SupervisorFSM_RX_B.isInOverCurrent || + SupervisorFSM_RX_B.isFaultButtonPressed) { + if (SupervisorFSM_RX_B.isInOverCurrent) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_HWFault; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { + guard1 = true; + } else { + guard3 = true; + } + } else { + guard3 = true; + } + + if (guard3) { + if (SupervisorFS_IsNewCtrl_Velocity()) { + if ((!SupervisorFSM_IsNewCtrl_Voltage()) && + SupervisorFS_IsNewCtrl_Velocity()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Velocity; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; + SupervisorFSM_RX_B.newSetpoint_m = 0.0F; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else { + guard2 = true; + } + } else if (!SupervisorFSM_IsNewCtrl_Voltage()) { + if (SupervisorFSM_IsNewCtrl_Current()) { + // Chart: '/ControlMode_SM_motor0' + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFS_rtu_ControlOutputs->Iq_fbk.current; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Current; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFS_IsNewCtrl_Position()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Position; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; + + // Chart: '/ControlMode_SM_motor0' + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFSM_R_rtu_SensorsData->jointpositions.position; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { + guard1 = true; + } else { + guard2 = true; + } + } else { + guard2 = true; + } + } + + if (guard2) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Voltage; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + + if (guard1) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } +} + +// Output and update for function-call system: '/Control Mode Handler Motor 0' +void Superv_ControlModeHandlerMotor0(void) +{ + // Chart: '/ControlMode_SM_motor0' + if (SupervisorFSM_RX_DW.is_active_c12_SupervisorFSM_RX == 0U) { + SupervisorFSM_RX_DW.is_active_c12_SupervisorFSM_RX = 1U; + if (SupervisorFSM_RX_IsBoardReady() && SupervisorFS_isConfigurationSet()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM__IN_NotConfigured; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; + rtw_disableMotor(); + } + } else { + boolean_T guard1 = false; + boolean_T guard10 = false; + boolean_T guard11 = false; + boolean_T guard2 = false; + boolean_T guard3 = false; + boolean_T guard4 = false; + boolean_T guard5 = false; + boolean_T guard6 = false; + boolean_T guard7 = false; + boolean_T guard8 = false; + boolean_T guard9 = false; + guard1 = false; + guard2 = false; + guard3 = false; + guard4 = false; + guard5 = false; + guard6 = false; + guard7 = false; + guard8 = false; + guard9 = false; + guard10 = false; + guard11 = false; + switch (SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX) { + case SupervisorFSM_RX_IN_Current: + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; + if (SupervisorFSM_RX_B.isInOverCurrent || + SupervisorFSM_RX_B.isFaultButtonPressed) { + if (SupervisorFSM_RX_B.isInOverCurrent) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_HWFault; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { + guard1 = true; + } else { + guard7 = true; + } + } else { + guard7 = true; + } + break; + + case SupervisorFSM_RX_IN_HWFault: + { + boolean_T tmp; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; + tmp = !SupervisorFSM_RX_B.isInOverCurrent; + if (tmp && SupervisorFSM_RX_IsNewCtrl_Idle() && + (!SupervisorFS_isConfigurationSet())) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM__IN_NotConfigured; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; + rtw_disableMotor(); + } else if (tmp && SupervisorFSM_RX_IsNewCtrl_Idle()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + } + break; + + case SupervisorFSM_RX_IN_Idle: + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + if (SupervisorFSM_RX_B.isInOverCurrent || + SupervisorFSM_RX_B.isFaultButtonPressed) { + if (SupervisorFSM_RX_B.isInOverCurrent) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_HWFault; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { + guard3 = true; + } else { + guard8 = true; + } + } else { + guard8 = true; + } + break; + + case SupervisorFSM__IN_NotConfigured: + SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; + if (SupervisorFSM_RX_IsBoardReady() && SupervisorFS_isConfigurationSet()) + { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_B.isInOverCurrent) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_HWFault; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + break; + + case SupervisorFSM_RX_IN_Position: + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; + if (SupervisorFSM_RX_B.isInOverCurrent || + SupervisorFSM_RX_B.isFaultButtonPressed) { + if (SupervisorFSM_RX_B.isInOverCurrent) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_HWFault; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { + guard4 = true; + } else { + guard11 = true; + } + } else { + guard11 = true; + } + break; + + case SupervisorFSM_RX_IN_Velocity: + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; + if (SupervisorFSM_RX_B.isInOverCurrent || + SupervisorFSM_RX_B.isFaultButtonPressed) { + if (SupervisorFSM_RX_B.isInOverCurrent) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_HWFault; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { + guard5 = true; + } else { + guard10 = true; + } + } else { + guard10 = true; + } + break; + + default: + // case IN_Voltage: + SupervisorFSM_RX_Voltage(); + break; + } + + if (guard11) { + if ((!SupervisorFSM_RX_IsNewCtrl_Idle()) && + (!SupervisorFS_IsNewCtrl_Position())) { + if (SupervisorFSM_IsNewCtrl_Current()) { + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFS_rtu_ControlOutputs->Iq_fbk.current; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Current; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_IsNewCtrl_Voltage()) { + SupervisorFSM_RX_B.newSetpoint_m = SupervisorFS_rtu_ControlOutputs->Vq; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Voltage; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFS_IsNewCtrl_Velocity()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Velocity; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; + SupervisorFSM_RX_B.newSetpoint_m = 0.0F; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else { + guard9 = true; + } + } else { + guard9 = true; + } + } + + if (guard10) { + if (!SupervisorFS_IsNewCtrl_Velocity()) { + if (SupervisorFSM_IsNewCtrl_Voltage()) { + SupervisorFSM_RX_B.newSetpoint_m = SupervisorFS_rtu_ControlOutputs->Vq; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Voltage; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_IsNewCtrl_Current()) { + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFS_rtu_ControlOutputs->Iq_fbk.current; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Current; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFS_IsNewCtrl_Position()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Position; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFSM_R_rtu_SensorsData->jointpositions.position; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { + guard5 = true; + } else { + guard6 = true; + } + } else { + guard6 = true; + } + } + + if (guard9) { + if (SupervisorFSM_RX_IsNewCtrl_Idle()) { + guard4 = true; + } + } + + if (guard8) { + if (!SupervisorFSM_RX_IsNewCtrl_Idle()) { + rtw_enableMotor(); + if (SupervisorFS_IsNewCtrl_Position()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Position; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFSM_R_rtu_SensorsData->jointpositions.position; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_IsNewCtrl_Current()) { + SupervisorFSM_RX_B.newSetpoint_m = 0.0F; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Current; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_IsNewCtrl_Voltage()) { + SupervisorFSM_RX_B.newSetpoint_m = 0.0F; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Voltage; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFS_IsNewCtrl_Velocity()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Velocity; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; + SupervisorFSM_RX_B.newSetpoint_m = 0.0F; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else { + guard3 = true; + } + } else { + guard3 = true; + } + } + + if (guard7) { + if ((!SupervisorFSM_RX_IsNewCtrl_Idle()) && + (!SupervisorFS_IsNewCtrl_Position())) { + if (!SupervisorFSM_IsNewCtrl_Current()) { + if (SupervisorFSM_IsNewCtrl_Voltage()) { + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFS_rtu_ControlOutputs->Vq; + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Voltage; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFS_IsNewCtrl_Velocity()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Velocity; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; + SupervisorFSM_RX_B.newSetpoint_m = 0.0F; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else { + guard2 = true; + } + } else { + guard2 = true; + } + } else if (!SupervisorFSM_IsNewCtrl_Current()) { + if (SupervisorFS_IsNewCtrl_Position()) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = + SupervisorFSM_RX_IN_Position; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; + SupervisorFSM_RX_B.newSetpoint_m = + SupervisorFSM_R_rtu_SensorsData->jointpositions.position; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { + guard1 = true; + } else { + guard2 = true; + } + } else { + guard2 = true; + } + } + + if (guard6) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Velocity; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; + SupervisorFSM_RX_B.newSetpoint_m = 0.0F; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + + if (guard5) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + + if (guard4) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + + if (guard3) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + + if (guard2) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Current; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + + if (guard1) { + SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; + SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; + rtw_disableMotor(); + + // Outputs for Function Call SubSystem: '/SetpointHandler' + // this updates the targets value + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + } + + // End of Chart: '/ControlMode_SM_motor0' +} + +// System initialize for function-call system: '/Limits Handler' +void SupervisorFS_LimitsHandler_Init(void) +{ + // SystemInitialize for Chart: '/Chart' + SupervisorFSM_RX_B.thresholds.jntPosMin = 0.0F; + SupervisorFSM_RX_B.thresholds.jntPosMax = 0.0F; + SupervisorFSM_RX_B.thresholds.hardwareJntPosMin = 0.0F; + SupervisorFSM_RX_B.thresholds.hardwareJntPosMax = 0.0F; + SupervisorFSM_RX_B.thresholds.rotorPosMin = 0.0F; + SupervisorFSM_RX_B.thresholds.rotorPosMax = 0.0F; + SupervisorFSM_RX_B.thresholds.jntVelMax = 0.0F; + SupervisorFSM_RX_B.thresholds.velocityTimeout = 0U; + SupervisorFSM_RX_B.thresholds.motorNominalCurrents = 0.0F; + SupervisorFSM_RX_B.thresholds.motorPeakCurrents = 0.0F; + SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = 0.0F; + SupervisorFSM_RX_B.thresholds.motorPwmLimit = 0U; +} + +// Output and update for function-call system: '/Limits Handler' +void SupervisorFSM_RX_LimitsHandler(void) +{ + // Chart: '/Chart' + if (SupervisorFSM_RX_DW.is_active_c14_SupervisorFSM_RX == 0U) { + SupervisorFSM_RX_DW.is_active_c14_SupervisorFSM_RX = 1U; + SupervisorFSM_RX_B.thresholds = InitConfParams.thresholds; + if (SupervisorFSM_RX_B.newLimit.type == ControlModes_Current) { + SupervisorFSM_RX_B.thresholds.motorNominalCurrents = std::abs + (SupervisorFSM_RX_B.newLimit.nominal); + SupervisorFSM_RX_B.thresholds.motorPeakCurrents = std::abs + (SupervisorFSM_RX_B.newLimit.peak); + SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = std::abs + (SupervisorFSM_RX_B.newLimit.overload); + if (!SupervisorFSM_RX_B.areLimitsSet) { + SupervisorFSM_RX_B.areLimitsSet = true; + + // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' + Superv_ControlModeHandlerMotor0(); + + // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' + } + } + } else if (SupervisorFSM_RX_B.newLimit.type == ControlModes_Current) { + SupervisorFSM_RX_B.thresholds.motorNominalCurrents = std::abs + (SupervisorFSM_RX_B.newLimit.nominal); + SupervisorFSM_RX_B.thresholds.motorPeakCurrents = std::abs + (SupervisorFSM_RX_B.newLimit.peak); + SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = std::abs + (SupervisorFSM_RX_B.newLimit.overload); + if (!SupervisorFSM_RX_B.areLimitsSet) { + SupervisorFSM_RX_B.areLimitsSet = true; + + // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' + Superv_ControlModeHandlerMotor0(); + + // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' + } + } + + // End of Chart: '/Chart' +} + +// System initialize for function-call system: '/PID Handler' +void SupervisorFSM_R_PIDHandler_Init(void) +{ + // SystemInitialize for Chart: '/Chart' + SupervisorFSM_RX_B.CurrentPID.OutMax = 0.0F; + SupervisorFSM_RX_B.CurrentPID.OutMin = 0.0F; + SupervisorFSM_RX_B.CurrentPID.P = 0.0F; + SupervisorFSM_RX_B.CurrentPID.I = 0.0F; + SupervisorFSM_RX_B.CurrentPID.D = 0.0F; + SupervisorFSM_RX_B.CurrentPID.N = 0.0F; + SupervisorFSM_RX_B.CurrentPID.I0 = 0.0F; + SupervisorFSM_RX_B.CurrentPID.D0 = 0.0F; + SupervisorFSM_RX_B.CurrentPID.shift_factor = 0U; + SupervisorFSM_RX_B.VelocityPID.OutMax = 0.0F; + SupervisorFSM_RX_B.VelocityPID.OutMin = 0.0F; + SupervisorFSM_RX_B.VelocityPID.P = 0.0F; + SupervisorFSM_RX_B.VelocityPID.I = 0.0F; + SupervisorFSM_RX_B.VelocityPID.D = 0.0F; + SupervisorFSM_RX_B.VelocityPID.N = 0.0F; + SupervisorFSM_RX_B.VelocityPID.I0 = 0.0F; + SupervisorFSM_RX_B.VelocityPID.D0 = 0.0F; + SupervisorFSM_RX_B.VelocityPID.shift_factor = 0U; + SupervisorFSM_RX_B.PositionPID.OutMax = 0.0F; + SupervisorFSM_RX_B.PositionPID.OutMin = 0.0F; + SupervisorFSM_RX_B.PositionPID.P = 0.0F; + SupervisorFSM_RX_B.PositionPID.I = 0.0F; + SupervisorFSM_RX_B.PositionPID.D = 0.0F; + SupervisorFSM_RX_B.PositionPID.N = 0.0F; + SupervisorFSM_RX_B.PositionPID.I0 = 0.0F; + SupervisorFSM_RX_B.PositionPID.D0 = 0.0F; + SupervisorFSM_RX_B.PositionPID.shift_factor = 0U; + SupervisorFSM_RX_B.OpenLoopPID.OutMax = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.OutMin = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.P = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.I = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.D = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.N = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.I0 = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.D0 = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.shift_factor = 0U; +} + +// Output and update for function-call system: '/PID Handler' +void SupervisorFSM_RX_PIDHandler(void) +{ + // Chart: '/Chart' + if (SupervisorFSM_RX_DW.is_active_c3_SupervisorFSM_RX == 0U) { + SupervisorFSM_RX_DW.is_active_c3_SupervisorFSM_RX = 1U; + SupervisorFSM_RX_B.PositionPID = InitConfParams.PosLoopPID; + SupervisorFSM_RX_B.CurrentPID = InitConfParams.CurLoopPID; + SupervisorFSM_RX_B.VelocityPID = InitConfParams.VelLoopPID; + switch (SupervisorFSM_RX_B.newPIDType) { + case ControlModes_Current: + SupervisorFSM_RX_B.CurrentPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.CurrentPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.CurrentPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.CurrentPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + + case ControlModes_Position: + SupervisorFSM_RX_B.PositionPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.PositionPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.PositionPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.PositionPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + + case ControlModes_Voltage: + SupervisorFSM_RX_B.OpenLoopPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.OpenLoopPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.OpenLoopPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.OpenLoopPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + + case ControlModes_Velocity: + SupervisorFSM_RX_B.VelocityPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.VelocityPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.VelocityPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.VelocityPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + } + } else { + switch (SupervisorFSM_RX_B.newPIDType) { + case ControlModes_Current: + SupervisorFSM_RX_B.CurrentPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.CurrentPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.CurrentPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.CurrentPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + + case ControlModes_Position: + SupervisorFSM_RX_B.PositionPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.PositionPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.PositionPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.PositionPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + + case ControlModes_Voltage: + SupervisorFSM_RX_B.OpenLoopPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.OpenLoopPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.OpenLoopPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.OpenLoopPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + + case ControlModes_Velocity: + SupervisorFSM_RX_B.VelocityPID.P = SupervisorFSM_RX_B.newPID.Kp; + SupervisorFSM_RX_B.VelocityPID.I = SupervisorFSM_RX_B.newPID.Ki; + SupervisorFSM_RX_B.VelocityPID.D = SupervisorFSM_RX_B.newPID.Kd; + SupervisorFSM_RX_B.VelocityPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; + break; + } + } + + // End of Chart: '/Chart' +} + +// Function for Chart: '/CAN Event Dispatcher' +static ControlModes SupervisorFSM_RX_convert(MCControlModes mccontrolmode) +{ + ControlModes controlmode; + switch (mccontrolmode) { + case MCControlModes_Idle: + controlmode = ControlModes_Idle; + break; + + case MCControlModes_Current: + controlmode = ControlModes_Current; + break; + + case MCControlModes_SpeedVoltage: + controlmode = ControlModes_Velocity; + break; + + case MCControlModes_OpenLoop: + controlmode = ControlModes_Voltage; + break; + + default: + controlmode = ControlModes_Idle; + break; + } + + return controlmode; +} + +// Function for Chart: '/CAN Event Dispatcher' +static void SupervisorFSM_formatMotorConfig(const BUS_MESSAGES_RX *Selector2) +{ + SupervisorFSM_RX_B.motorConfig.use_index = Selector2->motor_config.use_index; + SupervisorFSM_RX_B.motorConfig.pole_pairs = static_cast(rt_roundd_snf + (static_cast(Selector2->motor_config.number_poles) / 2.0)); + SupervisorFSM_RX_B.motorConfig.encoder_tolerance = + Selector2->motor_config.encoder_tolerance; + SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution = + Selector2->motor_config.rotor_encoder_resolution; + SupervisorFSM_RX_B.motorConfig.rotor_index_offset = + Selector2->motor_config.rotor_index_offset; + SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder = + Selector2->motor_config.has_quadrature_encoder; + SupervisorFSM_RX_B.motorConfig.has_hall_sens = + Selector2->motor_config.has_hall_sens; + SupervisorFSM_RX_B.motorConfig.has_speed_quadrature_encoder = + Selector2->motor_config.has_speed_quadrature_encoder; + SupervisorFSM_RX_B.motorConfig.has_torque_sens = + Selector2->motor_config.has_torque_sens; + SupervisorFSM_RX_B.motorConfig.enable_verbosity = + Selector2->motor_config.enable_verbosity; +} + +// Function for Chart: '/CAN Event Dispatcher' +static void SupervisorF_hardwareConfigMotor(void) +{ + rtw_configMotor(static_cast + (SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder), + SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution, + SupervisorFSM_RX_B.motorConfig.pole_pairs, static_cast + (SupervisorFSM_RX_B.motorConfig.has_hall_sens), 1U, 21845U); +} + +// System initialize for function-call system: '/CAN Message Handler' +void Supervis_CANMessageHandler_Init(void) +{ + // SystemInitialize for Chart: '/CAN Event Dispatcher' + SupervisorFSM_RX_B.newSetpoint.current = 0.0F; + SupervisorFSM_RX_B.newSetpoint.voltage = 0.0F; + SupervisorFSM_RX_B.newSetpoint.velocity = 0.0F; + SupervisorFSM_RX_B.newLimit.overload = 0.0F; + SupervisorFSM_RX_B.newLimit.peak = 0.0F; + SupervisorFSM_RX_B.newLimit.nominal = 0.0F; + SupervisorFSM_RX_B.newLimit.type = ControlModes_NotConfigured; + SupervisorFSM_RX_B.newPID.motor = false; + SupervisorFSM_RX_B.newPID.Kp = 0.0F; + SupervisorFSM_RX_B.newPID.Ki = 0.0F; + SupervisorFSM_RX_B.newPID.Kd = 0.0F; + SupervisorFSM_RX_B.newPID.Ks = 0U; + SupervisorFSM_RX_B.motorConfig.has_hall_sens = false; + SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder = false; + SupervisorFSM_RX_B.motorConfig.has_speed_quadrature_encoder = false; + SupervisorFSM_RX_B.motorConfig.has_torque_sens = false; + SupervisorFSM_RX_B.motorConfig.use_index = false; + SupervisorFSM_RX_B.motorConfig.enable_verbosity = false; + SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution = 0; + SupervisorFSM_RX_B.motorConfig.rotor_index_offset = 0; + SupervisorFSM_RX_B.motorConfig.encoder_tolerance = 0U; + SupervisorFSM_RX_B.motorConfig.pole_pairs = 0U; + SupervisorFSM_RX_B.motorConfig.Kbemf = 0.0F; + SupervisorFSM_RX_B.motorConfig.Rphase = 0.0F; + SupervisorFSM_RX_B.motorConfig.Imin = 0.0F; + SupervisorFSM_RX_B.motorConfig.Imax = 0.0F; + SupervisorFSM_RX_B.motorConfig.Vcc = 0.0F; + SupervisorFSM_RX_B.motorConfig.Vmax = 0.0F; + SupervisorFSM_RX_B.estimationConfig.velocity_mode = + EstimationVelocityModes_Disabled; + + // SystemInitialize for Function Call SubSystem: '/SetpointHandler' + Supervisor_SetpointHandler_Init(); + + // End of SystemInitialize for SubSystem: '/SetpointHandler' + + // SystemInitialize for Function Call SubSystem: '/Limits Handler' + SupervisorFS_LimitsHandler_Init(); + + // End of SystemInitialize for SubSystem: '/Limits Handler' + + // SystemInitialize for Function Call SubSystem: '/PID Handler' + SupervisorFSM_R_PIDHandler_Init(); + + // End of SystemInitialize for SubSystem: '/PID Handler' +} + +// Output and update for function-call system: '/CAN Message Handler' +void SupervisorFSM_CANMessageHandler(void) +{ + BUS_MESSAGES_RX Selector2; + + // Selector: '/Selector2' + Selector2 = SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1]; + + // Chart: '/CAN Event Dispatcher' incorporates: + // Selector: '/Selector' + // Selector: '/Selector1' + // Selector: '/Selector2' + + if (SupervisorFSM_RX_DW.is_active_c11_SupervisorFSM_RX == 0U) { + SupervisorFSM_RX_DW.is_active_c11_SupervisorFSM_RX = 1U; + SupervisorFSM_RX_B.motorConfig = InitConfParams.motorconfig; + SupervisorFSM_RX_B.estimationConfig = InitConfParams.estimationconfig; + if (!SupervisorFSM_RX_rtu_ErrorsRx->errors[SupervisorFSM_RX_B.messageIndex - + 1].event) { + if (SupervisorFSM_RX_rtu_StatusRx->status[SupervisorFSM_RX_B.messageIndex + - 1].control_mode) { + SupervisorFSM_RX_B.requiredControlMode = SupervisorFSM_RX_convert + (SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].control_mode.mode); + SupervisorFSM_RX_B.receivedNewSetpoint = false; + + // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' + Superv_ControlModeHandlerMotor0(); + + // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].current_limit) { + SupervisorFSM_RX_B.newLimit.type = ControlModes_Current; + SupervisorFSM_RX_B.newLimit.peak = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.peak; + SupervisorFSM_RX_B.newLimit.nominal = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.nominal; + SupervisorFSM_RX_B.newLimit.overload = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.overload; + + // Outputs for Function Call SubSystem: '/Limits Handler' + SupervisorFSM_RX_LimitsHandler(); + + // End of Outputs for SubSystem: '/Limits Handler' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].current_pid) { + SupervisorFSM_RX_B.newPIDType = ControlModes_Current; + SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].pid; + + // Outputs for Function Call SubSystem: '/PID Handler' + SupervisorFSM_RX_PIDHandler(); + + // End of Outputs for SubSystem: '/PID Handler' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].velocity_pid) { + SupervisorFSM_RX_B.newPIDType = ControlModes_Velocity; + SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].pid; + + // Outputs for Function Call SubSystem: '/PID Handler' + SupervisorFSM_RX_PIDHandler(); + + // End of Outputs for SubSystem: '/PID Handler' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].motor_config) { + SupervisorFSM_formatMotorConfig(&Selector2); + SupervisorF_hardwareConfigMotor(); + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].desired_targets) { + SupervisorFSM_RX_B.newSetpoint = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].desired_targets; + SupervisorFSM_RX_B.enableSendingMsgStatus = true; + SupervisorFSM_RX_B.receivedNewSetpoint = true; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + } + } else { + SupervisorFSM_RX_B.enableSendingMsgStatus = + ((SupervisorFSM_RX_B.messageIndex != 1) && + SupervisorFSM_RX_B.enableSendingMsgStatus); + if (!SupervisorFSM_RX_rtu_ErrorsRx->errors[SupervisorFSM_RX_B.messageIndex - + 1].event) { + if (SupervisorFSM_RX_rtu_StatusRx->status[SupervisorFSM_RX_B.messageIndex + - 1].control_mode) { + SupervisorFSM_RX_B.requiredControlMode = SupervisorFSM_RX_convert + (SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].control_mode.mode); + SupervisorFSM_RX_B.receivedNewSetpoint = false; + + // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' + Superv_ControlModeHandlerMotor0(); + + // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].current_limit) { + SupervisorFSM_RX_B.newLimit.type = ControlModes_Current; + SupervisorFSM_RX_B.newLimit.peak = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.peak; + SupervisorFSM_RX_B.newLimit.nominal = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.nominal; + SupervisorFSM_RX_B.newLimit.overload = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.overload; + + // Outputs for Function Call SubSystem: '/Limits Handler' + SupervisorFSM_RX_LimitsHandler(); + + // End of Outputs for SubSystem: '/Limits Handler' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].current_pid) { + SupervisorFSM_RX_B.newPIDType = ControlModes_Current; + SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].pid; + + // Outputs for Function Call SubSystem: '/PID Handler' + SupervisorFSM_RX_PIDHandler(); + + // End of Outputs for SubSystem: '/PID Handler' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].velocity_pid) { + SupervisorFSM_RX_B.newPIDType = ControlModes_Velocity; + SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].pid; + + // Outputs for Function Call SubSystem: '/PID Handler' + SupervisorFSM_RX_PIDHandler(); + + // End of Outputs for SubSystem: '/PID Handler' + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].motor_config) { + SupervisorFSM_formatMotorConfig(&Selector2); + SupervisorF_hardwareConfigMotor(); + } else if (SupervisorFSM_RX_rtu_StatusRx-> + status[SupervisorFSM_RX_B.messageIndex - 1].desired_targets) { + SupervisorFSM_RX_B.newSetpoint = + SupervisorFSM_RX_rtu_MessagesRx-> + messages[SupervisorFSM_RX_B.messageIndex - 1].desired_targets; + SupervisorFSM_RX_B.enableSendingMsgStatus = true; + SupervisorFSM_RX_B.receivedNewSetpoint = true; + + // Outputs for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_R_SetpointHandler(); + + // End of Outputs for SubSystem: '/SetpointHandler' + } + } + } + + // End of Chart: '/CAN Event Dispatcher' +} + +// Function for Chart: '/SupervisorRX State Handler' +static boolean_T SupervisorFSM_isBoardConfigured(void) +{ + return true; +} + +// System initialize for referenced model: 'SupervisorFSM_RX' +void SupervisorFSM_RX_Init(Flags *rty_Flags, ConfigurationParameters + *rty_ConfigurationParameters) +{ + // SystemInitialize for Chart: '/SupervisorRX State Handler' incorporates: + // SubSystem: '/CAN Message Handler' + + Supervis_CANMessageHandler_Init(); + + // SystemInitialize for BusCreator: '/Bus Creator' + rty_Flags->control_mode = SupervisorFSM_RX_B.controlModeDefined; + rty_Flags->enable_sending_msg_status = + SupervisorFSM_RX_B.enableSendingMsgStatus; + rty_Flags->fault_button = false; + + // SystemInitialize for BusCreator: '/Bus Creator1' + rty_ConfigurationParameters->motorconfig = SupervisorFSM_RX_B.motorConfig; + rty_ConfigurationParameters->estimationconfig = + SupervisorFSM_RX_B.estimationConfig; + rty_ConfigurationParameters->CurLoopPID = SupervisorFSM_RX_B.CurrentPID; + rty_ConfigurationParameters->PosLoopPID = SupervisorFSM_RX_B.PositionPID; + rty_ConfigurationParameters->VelLoopPID = SupervisorFSM_RX_B.VelocityPID; + rty_ConfigurationParameters->DirLoopPID = SupervisorFSM_RX_B.OpenLoopPID; + rty_ConfigurationParameters->thresholds = SupervisorFSM_RX_B.thresholds; +} + +// Output and update for referenced model: 'SupervisorFSM_RX' +void SupervisorFSM_RX(const SensorsData *rtu_SensorsData, const ExternalFlags + *rtu_ExternalFlags, const ControlOutputs + *rtu_ControlOutputs, const BUS_MESSAGES_RX_MULTIPLE + *rtu_MessagesRx, const BUS_STATUS_RX_MULTIPLE + *rtu_StatusRx, const BUS_CAN_RX_ERRORS_MULTIPLE + *rtu_ErrorsRx, Flags *rty_Flags, Targets *rty_Targets, + ConfigurationParameters *rty_ConfigurationParameters) +{ + real32_T rtb_UnitDelay_thresholds_motorO; + SupervisorFSM_R_rtu_SensorsData = rtu_SensorsData; + SupervisorFS_rtu_ControlOutputs = rtu_ControlOutputs; + SupervisorFSM_RX_rtu_MessagesRx = rtu_MessagesRx; + SupervisorFSM_RX_rtu_StatusRx = rtu_StatusRx; + SupervisorFSM_RX_rtu_ErrorsRx = rtu_ErrorsRx; + + // UnitDelay: '/Unit Delay' + rtb_UnitDelay_thresholds_motorO = + SupervisorFSM_RX_DW.UnitDelay_DSTATE.thresholds.motorOverloadCurrents; + + // Chart: '/SupervisorRX State Handler' incorporates: + // UnitDelay: '/Unit Delay' + + SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev = + SupervisorFSM_RX_DW.ExternalFlags_fault_button_star; + SupervisorFSM_RX_DW.ExternalFlags_fault_button_star = + rtu_ExternalFlags->fault_button; + if (SupervisorFSM_RX_DW.is_active_c2_SupervisorFSM_RX == 0U) { + SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev = + rtu_ExternalFlags->fault_button; + SupervisorFSM_RX_DW.ExternalFlags_fault_button_star = + rtu_ExternalFlags->fault_button; + SupervisorFSM_RX_DW.is_active_c2_SupervisorFSM_RX = 1U; + SupervisorFSM_RX_DW.is_active_STATE_HANDLER = 1U; + SupervisorFSM_RX_DW.is_STATE_HANDLER = SupervisorFS_IN_NotConfigured_d; + SupervisorFSM_RX_B.BoardSt = BoardState_NotConfigured; + SupervisorFSM_RX_DW.is_active_FAULT_HANDLER = 1U; + SupervisorFSM_RX_DW.is_active_FaultButtonPressed = 1U; + SupervisorFSM_RX_DW.is_FaultButtonPressed = SupervisorFSM_RX_IN_NoFault; + SupervisorFSM_RX_B.isFaultButtonPressed = false; + SupervisorFSM_RX_DW.is_active_OverCurrent = 1U; + SupervisorFSM_RX_DW.is_OverCurrent = Superviso_IN_LimitNonConfigured; + SupervisorFSM_RX_DW.is_active_CAN_MESSAGES_FOR_LOOP = 1U; + + // Outputs for Function Call SubSystem: '/PID Handler' + SupervisorFSM_RX_PIDHandler(); + + // End of Outputs for SubSystem: '/PID Handler' + + // Outputs for Function Call SubSystem: '/Limits Handler' + SupervisorFSM_RX_LimitsHandler(); + + // End of Outputs for SubSystem: '/Limits Handler' + SupervisorFSM_RX_B.messageIndex = 1; + while (SupervisorFSM_RX_B.messageIndex <= CAN_MAX_NUM_PACKETS) { + // Outputs for Function Call SubSystem: '/CAN Message Handler' + SupervisorFSM_CANMessageHandler(); + + // End of Outputs for SubSystem: '/CAN Message Handler' + SupervisorFSM_RX_B.messageIndex++; + } + + SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP = SupervisorFSM_RX_IN_state1; + } else { + if (SupervisorFSM_RX_DW.is_active_STATE_HANDLER != 0U) { + switch (SupervisorFSM_RX_DW.is_STATE_HANDLER) { + case SupervisorFSM_RX_IN_Configured: + SupervisorFSM_RX_B.BoardSt = BoardState_Configured; + break; + + case SupervisorFSM_RX_IN_Fault: + SupervisorFSM_RX_B.BoardSt = BoardState_Fault; + break; + + case SupervisorFS_IN_NotConfigured_d: + SupervisorFSM_RX_B.BoardSt = BoardState_NotConfigured; + if (SupervisorFSM_isBoardConfigured()) { + SupervisorFSM_RX_DW.is_STATE_HANDLER = SupervisorFSM_RX_IN_Configured; + SupervisorFSM_RX_B.BoardSt = BoardState_Configured; + } + break; + } + } + + if (SupervisorFSM_RX_DW.is_active_FAULT_HANDLER != 0U) { + if (SupervisorFSM_RX_DW.is_active_FaultButtonPressed != 0U) { + switch (SupervisorFSM_RX_DW.is_FaultButtonPressed) { + case SupervisorFSM__IN_ButtonPressed: + SupervisorFSM_RX_B.isFaultButtonPressed = true; + if ((SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev != + SupervisorFSM_RX_DW.ExternalFlags_fault_button_star) && + (!SupervisorFSM_RX_DW.ExternalFlags_fault_button_star)) { + SupervisorFSM_RX_DW.is_FaultButtonPressed = + SupervisorFSM_RX_IN_NoFault; + SupervisorFSM_RX_B.isFaultButtonPressed = false; + } + break; + + case SupervisorFSM_RX_IN_NoFault: + SupervisorFSM_RX_B.isFaultButtonPressed = false; + if ((SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev != + SupervisorFSM_RX_DW.ExternalFlags_fault_button_star) && + SupervisorFSM_RX_DW.ExternalFlags_fault_button_star) { + SupervisorFSM_RX_DW.is_FaultButtonPressed = + SupervisorFSM__IN_ButtonPressed; + SupervisorFSM_RX_B.isFaultButtonPressed = true; + + // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' + Superv_ControlModeHandlerMotor0(); + + // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' + } + break; + } + } + + if (SupervisorFSM_RX_DW.is_active_OverCurrent != 0U) { + switch (SupervisorFSM_RX_DW.is_OverCurrent) { + case Superviso_IN_LimitNonConfigured: + if (SupervisorFSM_RX_B.areLimitsSet) { + SupervisorFSM_RX_DW.is_OverCurrent = SupervisorFSM_RX_IN_NoFault; + SupervisorFSM_RX_B.isInOverCurrent = false; + + // MotorFaultFlags.overCurrent=0; + } + break; + + case SupervisorFSM_RX_IN_NoFault: + SupervisorFSM_RX_B.isInOverCurrent = false; + if (std::abs(rtu_ControlOutputs->Iq_fbk.current) >= + rtb_UnitDelay_thresholds_motorO) { + SupervisorFSM_RX_DW.is_OverCurrent = SupervisorF_IN_OverCurrentFault; + + // MotorFaultFlags.overCurrent=1; + SupervisorFSM_RX_B.isInOverCurrent = true; + + // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' + Superv_ControlModeHandlerMotor0(); + + // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' + } + break; + + case SupervisorF_IN_OverCurrentFault: + SupervisorFSM_RX_B.isInOverCurrent = true; + if (std::abs(rtu_ControlOutputs->Iq_fbk.current) < + rtb_UnitDelay_thresholds_motorO) { + SupervisorFSM_RX_DW.is_OverCurrent = SupervisorFSM_RX_IN_NoFault; + SupervisorFSM_RX_B.isInOverCurrent = false; + + // MotorFaultFlags.overCurrent=0; + } + break; + } + } + } + + if ((SupervisorFSM_RX_DW.is_active_CAN_MESSAGES_FOR_LOOP != 0U) && + (SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP == + SupervisorFSM_RX_IN_state1)) { + SupervisorFSM_RX_B.messageIndex = 1; + while (SupervisorFSM_RX_B.messageIndex <= CAN_MAX_NUM_PACKETS) { + // Outputs for Function Call SubSystem: '/CAN Message Handler' + SupervisorFSM_CANMessageHandler(); + + // End of Outputs for SubSystem: '/CAN Message Handler' + SupervisorFSM_RX_B.messageIndex++; + } + + SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP = SupervisorFSM_RX_IN_state1; + } + } + + // End of Chart: '/SupervisorRX State Handler' + + // BusCreator: '/Bus Creator' + rty_Flags->control_mode = SupervisorFSM_RX_B.controlModeDefined; + rty_Flags->enable_sending_msg_status = + SupervisorFSM_RX_B.enableSendingMsgStatus; + rty_Flags->fault_button = SupervisorFSM_RX_B.isFaultButtonPressed; + + // BusCreator: '/Bus Creator1' + rty_ConfigurationParameters->motorconfig = SupervisorFSM_RX_B.motorConfig; + rty_ConfigurationParameters->estimationconfig = + SupervisorFSM_RX_B.estimationConfig; + rty_ConfigurationParameters->CurLoopPID = SupervisorFSM_RX_B.CurrentPID; + rty_ConfigurationParameters->PosLoopPID = SupervisorFSM_RX_B.PositionPID; + rty_ConfigurationParameters->VelLoopPID = SupervisorFSM_RX_B.VelocityPID; + rty_ConfigurationParameters->DirLoopPID = SupervisorFSM_RX_B.OpenLoopPID; + rty_ConfigurationParameters->thresholds = SupervisorFSM_RX_B.thresholds; + + // Switch: '/Switch2' incorporates: + // BusCreator: '/Bus Creator2' + // BusCreator: '/Bus Creator3' + // BusCreator: '/Bus Creator4' + // BusCreator: '/Bus Creator5' + // Constant: '/Constant' + // Constant: '/Constant1' + // Constant: '/Constant2' + // Constant: '/Constant3' + + if (SupervisorFSM_RX_B.hasSetpointChanged) { + *rty_Targets = SupervisorFSM_RX_B.targets; + } else { + rty_Targets->jointpositions.position = 0.0F; + rty_Targets->jointvelocities.velocity = 0.0F; + rty_Targets->motorcurrent.current = 0.0F; + rty_Targets->motorvoltage.voltage = 0.0F; + } + + // End of Switch: '/Switch2' + + // Update for UnitDelay: '/Unit Delay' + SupervisorFSM_RX_DW.UnitDelay_DSTATE = *rty_ConfigurationParameters; +} + +// Model initialize function +void SupervisorFSM_RX_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_SupervisorFSM_RX_T *const SupervisorFSM_RX_M = + &(SupervisorFSM_RX_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(SupervisorFSM_RX_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.h index e4112595db..811f7375f5 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.h @@ -1,195 +1,195 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.6 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:29 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_RX_h_ -#define RTW_HEADER_SupervisorFSM_RX_h_ -#include "rtwtypes.h" -#include "SupervisorFSM_RX_types.h" - -// Includes for objects with custom storage classes -#include "rtw_defines.h" - -// user code (top of header file) -#include "rtw_enable_disable_motors.h" -#include "rtw_motor_config.h" - -// Block signals for model 'SupervisorFSM_RX' -#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -struct B_SupervisorFSM_RX_c_T { - Thresholds thresholds; // '/Chart' - PIDConfig CurrentPID; // '/Chart' - PIDConfig VelocityPID; // '/Chart' - PIDConfig PositionPID; // '/Chart' - PIDConfig OpenLoopPID; // '/Chart' - MotorConfig motorConfig; // '/CAN Event Dispatcher' - Targets targets; // '/Chart1' - BUS_MSG_PID newPID; // '/CAN Event Dispatcher' - SV_Limits newLimit; // '/CAN Event Dispatcher' - BUS_MSG_DESIRED_TARGETS newSetpoint; // '/CAN Event Dispatcher' - EstimationConfig estimationConfig; // '/CAN Event Dispatcher' - real32_T newSetpoint_m; // '/ControlMode_SM_motor0' - int32_T messageIndex; // '/SupervisorRX State Handler' - ControlModes requiredControlMode; // '/CAN Event Dispatcher' - ControlModes newPIDType; // '/CAN Event Dispatcher' - ControlModes controlModeDefined; // '/ControlMode_SM_motor0' - BoardState BoardSt; // '/SupervisorRX State Handler' - boolean_T isInOverCurrent; // '/SupervisorRX State Handler' - boolean_T isFaultButtonPressed; // '/SupervisorRX State Handler' - boolean_T enableSendingMsgStatus; // '/CAN Event Dispatcher' - boolean_T receivedNewSetpoint; // '/CAN Event Dispatcher' - boolean_T areLimitsSet; // '/Chart' - boolean_T hasSetpointChanged; // '/Chart1' -}; - -#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -// Block states (default storage) for model 'SupervisorFSM_RX' -#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -struct DW_SupervisorFSM_RX_f_T { - ConfigurationParameters UnitDelay_DSTATE;// '/Unit Delay' - uint8_T is_STATE_HANDLER; // '/SupervisorRX State Handler' - uint8_T is_OverCurrent; // '/SupervisorRX State Handler' - uint8_T is_FaultButtonPressed; // '/SupervisorRX State Handler' - uint8_T is_CAN_MESSAGES_FOR_LOOP; // '/SupervisorRX State Handler' - uint8_T is_active_c2_SupervisorFSM_RX;// '/SupervisorRX State Handler' - uint8_T is_active_STATE_HANDLER; // '/SupervisorRX State Handler' - uint8_T is_active_FAULT_HANDLER; // '/SupervisorRX State Handler' - uint8_T is_active_FaultButtonPressed;// '/SupervisorRX State Handler' - uint8_T is_active_OverCurrent; // '/SupervisorRX State Handler' - uint8_T is_active_CAN_MESSAGES_FOR_LOOP;// '/SupervisorRX State Handler' - uint8_T is_active_c11_SupervisorFSM_RX;// '/CAN Event Dispatcher' - uint8_T is_active_c3_SupervisorFSM_RX;// '/Chart' - uint8_T is_active_c14_SupervisorFSM_RX;// '/Chart' - uint8_T is_c12_SupervisorFSM_RX; // '/ControlMode_SM_motor0' - uint8_T is_active_c12_SupervisorFSM_RX;// '/ControlMode_SM_motor0' - uint8_T is_active_c4_SupervisorFSM_RX;// '/Chart1' - boolean_T ExternalFlags_fault_button_prev;// '/SupervisorRX State Handler' - boolean_T ExternalFlags_fault_button_star;// '/SupervisorRX State Handler' -}; - -#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_SupervisorFSM_RX_T { - const char_T **errorStatus; -}; - -#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_SupervisorFSM_RX_T { - RT_MODEL_SupervisorFSM_RX_T rtm; -}; - -#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -// -// Exported Global Parameters -// -// Note: Exported global parameters are tunable parameters with an exported -// global storage class designation. Code generation will declare the memory for -// these parameters and exports their symbols. -// - -extern ConfigurationParameters InitConfParams;// Variable: InitConfParams - // Referenced by: - // '/CAN Event Dispatcher' - // '/Chart' - // '/Chart' - -extern void SupervisorFSM_RX_Init(Flags *rty_Flags, ConfigurationParameters - *rty_ConfigurationParameters); -extern void SupervisorFSM_RX(const SensorsData *rtu_SensorsData, const - ExternalFlags *rtu_ExternalFlags, const ControlOutputs *rtu_ControlOutputs, - const BUS_MESSAGES_RX_MULTIPLE *rtu_MessagesRx, const BUS_STATUS_RX_MULTIPLE - *rtu_StatusRx, const BUS_CAN_RX_ERRORS_MULTIPLE *rtu_ErrorsRx, Flags - *rty_Flags, Targets *rty_Targets, ConfigurationParameters - *rty_ConfigurationParameters); - -// Model reference registration function -extern void SupervisorFSM_RX_initialize(const char_T **rt_errorStatus); - -#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -extern void Supervisor_SetpointHandler_Init(void); -extern void SupervisorFSM_R_SetpointHandler(void); -extern void Superv_ControlModeHandlerMotor0(void); -extern void SupervisorFS_LimitsHandler_Init(void); -extern void SupervisorFSM_RX_LimitsHandler(void); -extern void SupervisorFSM_R_PIDHandler_Init(void); -extern void SupervisorFSM_RX_PIDHandler(void); -extern void Supervis_CANMessageHandler_Init(void); -extern void SupervisorFSM_CANMessageHandler(void); - -#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_SupervisorFSM_RX_T SupervisorFSM_RX_MdlrefDW; - -#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -// Block signals (default storage) -extern B_SupervisorFSM_RX_c_T SupervisorFSM_RX_B; - -// Block states (default storage) -extern DW_SupervisorFSM_RX_f_T SupervisorFSM_RX_DW; - -#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'SupervisorFSM_RX' -// '' : 'SupervisorFSM_RX/CAN Message Handler' -// '' : 'SupervisorFSM_RX/Control Mode Handler Motor 0' -// '' : 'SupervisorFSM_RX/Limits Handler' -// '' : 'SupervisorFSM_RX/PID Handler' -// '' : 'SupervisorFSM_RX/SetpointHandler' -// '' : 'SupervisorFSM_RX/SupervisorRX State Handler' -// '' : 'SupervisorFSM_RX/CAN Message Handler/CAN Event Dispatcher' -// '' : 'SupervisorFSM_RX/Control Mode Handler Motor 0/ControlMode_SM_motor0' -// '' : 'SupervisorFSM_RX/Limits Handler/Chart' -// '' : 'SupervisorFSM_RX/PID Handler/Chart' -// '' : 'SupervisorFSM_RX/SetpointHandler/Chart1' - -#endif // RTW_HEADER_SupervisorFSM_RX_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_RX.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_RX_h_ +#define RTW_HEADER_SupervisorFSM_RX_h_ +#include "rtwtypes.h" +#include "SupervisorFSM_RX_types.h" + +// Includes for objects with custom storage classes +#include "rtw_defines.h" + +// user code (top of header file) +#include "rtw_enable_disable_motors.h" +#include "rtw_motor_config.h" + +// Block signals for model 'SupervisorFSM_RX' +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +struct B_SupervisorFSM_RX_c_T { + Thresholds thresholds; // '/Chart' + PIDConfig CurrentPID; // '/Chart' + PIDConfig VelocityPID; // '/Chart' + PIDConfig PositionPID; // '/Chart' + PIDConfig OpenLoopPID; // '/Chart' + MotorConfig motorConfig; // '/CAN Event Dispatcher' + Targets targets; // '/Chart1' + BUS_MSG_PID newPID; // '/CAN Event Dispatcher' + SV_Limits newLimit; // '/CAN Event Dispatcher' + BUS_MSG_DESIRED_TARGETS newSetpoint; // '/CAN Event Dispatcher' + EstimationConfig estimationConfig; // '/CAN Event Dispatcher' + real32_T newSetpoint_m; // '/ControlMode_SM_motor0' + int32_T messageIndex; // '/SupervisorRX State Handler' + ControlModes requiredControlMode; // '/CAN Event Dispatcher' + ControlModes newPIDType; // '/CAN Event Dispatcher' + ControlModes controlModeDefined; // '/ControlMode_SM_motor0' + BoardState BoardSt; // '/SupervisorRX State Handler' + boolean_T isInOverCurrent; // '/SupervisorRX State Handler' + boolean_T isFaultButtonPressed; // '/SupervisorRX State Handler' + boolean_T enableSendingMsgStatus; // '/CAN Event Dispatcher' + boolean_T receivedNewSetpoint; // '/CAN Event Dispatcher' + boolean_T areLimitsSet; // '/Chart' + boolean_T hasSetpointChanged; // '/Chart1' +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for model 'SupervisorFSM_RX' +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +struct DW_SupervisorFSM_RX_f_T { + ConfigurationParameters UnitDelay_DSTATE;// '/Unit Delay' + uint8_T is_STATE_HANDLER; // '/SupervisorRX State Handler' + uint8_T is_OverCurrent; // '/SupervisorRX State Handler' + uint8_T is_FaultButtonPressed; // '/SupervisorRX State Handler' + uint8_T is_CAN_MESSAGES_FOR_LOOP; // '/SupervisorRX State Handler' + uint8_T is_active_c2_SupervisorFSM_RX;// '/SupervisorRX State Handler' + uint8_T is_active_STATE_HANDLER; // '/SupervisorRX State Handler' + uint8_T is_active_FAULT_HANDLER; // '/SupervisorRX State Handler' + uint8_T is_active_FaultButtonPressed;// '/SupervisorRX State Handler' + uint8_T is_active_OverCurrent; // '/SupervisorRX State Handler' + uint8_T is_active_CAN_MESSAGES_FOR_LOOP;// '/SupervisorRX State Handler' + uint8_T is_active_c11_SupervisorFSM_RX;// '/CAN Event Dispatcher' + uint8_T is_active_c3_SupervisorFSM_RX;// '/Chart' + uint8_T is_active_c14_SupervisorFSM_RX;// '/Chart' + uint8_T is_c12_SupervisorFSM_RX; // '/ControlMode_SM_motor0' + uint8_T is_active_c12_SupervisorFSM_RX;// '/ControlMode_SM_motor0' + uint8_T is_active_c4_SupervisorFSM_RX;// '/Chart1' + boolean_T ExternalFlags_fault_button_prev;// '/SupervisorRX State Handler' + boolean_T ExternalFlags_fault_button_star;// '/SupervisorRX State Handler' +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_SupervisorFSM_RX_T { + const char_T **errorStatus; +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_SupervisorFSM_RX_T { + RT_MODEL_SupervisorFSM_RX_T rtm; +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// +// Exported Global Parameters +// +// Note: Exported global parameters are tunable parameters with an exported +// global storage class designation. Code generation will declare the memory for +// these parameters and exports their symbols. +// + +extern ConfigurationParameters InitConfParams;// Variable: InitConfParams + // Referenced by: + // '/CAN Event Dispatcher' + // '/Chart' + // '/Chart' + +extern void SupervisorFSM_RX_Init(Flags *rty_Flags, ConfigurationParameters + *rty_ConfigurationParameters); +extern void SupervisorFSM_RX(const SensorsData *rtu_SensorsData, const + ExternalFlags *rtu_ExternalFlags, const ControlOutputs *rtu_ControlOutputs, + const BUS_MESSAGES_RX_MULTIPLE *rtu_MessagesRx, const BUS_STATUS_RX_MULTIPLE + *rtu_StatusRx, const BUS_CAN_RX_ERRORS_MULTIPLE *rtu_ErrorsRx, Flags + *rty_Flags, Targets *rty_Targets, ConfigurationParameters + *rty_ConfigurationParameters); + +// Model reference registration function +extern void SupervisorFSM_RX_initialize(const char_T **rt_errorStatus); + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +extern void Supervisor_SetpointHandler_Init(void); +extern void SupervisorFSM_R_SetpointHandler(void); +extern void Superv_ControlModeHandlerMotor0(void); +extern void SupervisorFS_LimitsHandler_Init(void); +extern void SupervisorFSM_RX_LimitsHandler(void); +extern void SupervisorFSM_R_PIDHandler_Init(void); +extern void SupervisorFSM_RX_PIDHandler(void); +extern void Supervis_CANMessageHandler_Init(void); +extern void SupervisorFSM_CANMessageHandler(void); + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_SupervisorFSM_RX_T SupervisorFSM_RX_MdlrefDW; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Block signals (default storage) +extern B_SupervisorFSM_RX_c_T SupervisorFSM_RX_B; + +// Block states (default storage) +extern DW_SupervisorFSM_RX_f_T SupervisorFSM_RX_DW; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'SupervisorFSM_RX' +// '' : 'SupervisorFSM_RX/CAN Message Handler' +// '' : 'SupervisorFSM_RX/Control Mode Handler Motor 0' +// '' : 'SupervisorFSM_RX/Limits Handler' +// '' : 'SupervisorFSM_RX/PID Handler' +// '' : 'SupervisorFSM_RX/SetpointHandler' +// '' : 'SupervisorFSM_RX/SupervisorRX State Handler' +// '' : 'SupervisorFSM_RX/CAN Message Handler/CAN Event Dispatcher' +// '' : 'SupervisorFSM_RX/Control Mode Handler Motor 0/ControlMode_SM_motor0' +// '' : 'SupervisorFSM_RX/Limits Handler/Chart' +// '' : 'SupervisorFSM_RX/PID Handler/Chart' +// '' : 'SupervisorFSM_RX/SetpointHandler/Chart1' + +#endif // RTW_HEADER_SupervisorFSM_RX_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_private.h index fbba29a2ea..1921af09d1 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_private.h @@ -1,46 +1,46 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX_private.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.6 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:29 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_RX_private_h_ -#define RTW_HEADER_SupervisorFSM_RX_private_h_ -#include "rtwtypes.h" -#include "SupervisorFSM_RX_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif -#endif // RTW_HEADER_SupervisorFSM_RX_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_RX_private.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_RX_private_h_ +#define RTW_HEADER_SupervisorFSM_RX_private_h_ +#include "rtwtypes.h" +#include "SupervisorFSM_RX_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif +#endif // RTW_HEADER_SupervisorFSM_RX_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_types.h index fb650b9ce1..d9b4cb70da 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_types.h @@ -1,552 +1,552 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX_types.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 5.6 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:29 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_RX_types_h_ -#define RTW_HEADER_SupervisorFSM_RX_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ - -struct SensorsData -{ - // position encoders - JointPositions jointpositions; - MotorSensors motorsensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ -#define DEFINED_TYPEDEF_FOR_ExternalFlags_ - -struct ExternalFlags -{ - // External Fault Button (1 == pressed) - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs -{ - // control effort (quadrature) - real32_T Vq; - - // control effort (3-phases) - real32_T Vabc[3]; - - // quadrature current - MotorCurrent Iq_fbk; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ -#define DEFINED_TYPEDEF_FOR_MCControlModes_ - -typedef enum { - MCControlModes_Idle = 0, // Default value - MCControlModes_OpenLoop = 80, - MCControlModes_SpeedVoltage = 10, - MCControlModes_SpeedCurrent = 11, - MCControlModes_Current = 6, - MCControlModes_NotConfigured = 176, - MCControlModes_HWFault = 160 -} MCControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ - -// Fields of a CONTROL_MODE message. -struct BUS_MSG_CONTROL_MODE -{ - // Motor selector. - boolean_T motor; - - // Control mode. - MCControlModes mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ - -// Fields of a CURRENT_LIMIT message. -struct BUS_MSG_CURRENT_LIMIT -{ - // Motor selector. - boolean_T motor; - - // Nominal current in A. - real32_T nominal; - - // Peak current in A. - real32_T peak; - - // Overload current in A. - real32_T overload; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ - -// Fields of a DESIRED_TARGETS message. -struct BUS_MSG_DESIRED_TARGETS -{ - // Target current in A. - real32_T current; - - // Target voltage in %. - real32_T voltage; - - // Target veocity in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ - -// Fields of a CURRENT_PID message. -struct BUS_MSG_PID -{ - // Motor selector. - boolean_T motor; - - // Proportional gain. - real32_T Kp; - - // Integral gain. - real32_T Ki; - - // Derivative gain. - real32_T Kd; - - // Shift factor. - uint8_T Ks; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ - -struct BUS_MSG_MOTOR_CONFIG -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - - // Number of polese of the motor. - uint8_T number_poles; - - // Encoder tolerance. - uint8_T encoder_tolerance; - - // Resolution of rotor encoder. - int16_T rotor_encoder_resolution; - - // Offset of the rotor encoder. - int16_T rotor_index_offset; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ - -// Aggregate of all CAN received messages. -struct BUS_MESSAGES_RX -{ - BUS_MSG_CONTROL_MODE control_mode; - BUS_MSG_CURRENT_LIMIT current_limit; - BUS_MSG_DESIRED_TARGETS desired_targets; - BUS_MSG_PID pid; - BUS_MSG_MOTOR_CONFIG motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ - -struct BUS_MESSAGES_RX_MULTIPLE -{ - BUS_MESSAGES_RX messages[4]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocities - JointVelocities jointvelocities; - MotorCurrent Iq_filtered; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ - -// Aggregate of all events specifying types of received messages. -struct BUS_STATUS_RX -{ - boolean_T control_mode; - boolean_T current_limit; - boolean_T desired_targets; - boolean_T current_pid; - boolean_T velocity_pid; - boolean_T motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ - -struct BUS_STATUS_RX_MULTIPLE -{ - BUS_STATUS_RX status[4]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ -#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ - -typedef enum { - CANErrorTypes_No_Error = 0, // Default value - CANErrorTypes_Packet_Not4Us, - CANErrorTypes_Packet_Unrecognized, - CANErrorTypes_Packet_Malformed, - CANErrorTypes_Packet_MultiFunctionsDetected, - CANErrorTypes_Mode_Unrecognized -} CANErrorTypes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ - -// Specifies the CAN error types. -struct BUS_CAN_RX_ERRORS -{ - // True if an error has been detected. - boolean_T event; - CANErrorTypes type; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ - -struct BUS_CAN_RX_ERRORS_MULTIPLE -{ - BUS_CAN_RX_ERRORS errors[4]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ -#define DEFINED_TYPEDEF_FOR_ControlModes_ - -typedef enum { - ControlModes_NotConfigured = 0, // Default value - ControlModes_Idle, - ControlModes_Position, - ControlModes_PositionDirect, - ControlModes_Current, - ControlModes_Velocity, - ControlModes_Voltage, - ControlModes_Torque, - ControlModes_HwFaultCM -} ControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ - -struct Flags -{ - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SV_Limits_ -#define DEFINED_TYPEDEF_FOR_SV_Limits_ - -struct SV_Limits -{ - real32_T overload; - real32_T peak; - real32_T nominal; - ControlModes type; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BoardState_ -#define DEFINED_TYPEDEF_FOR_BoardState_ - -typedef enum { - BoardState_NotConfigured = 0, // Default value - BoardState_Configured, - BoardState_Fault -} BoardState; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_SupervisorFSM_RX_T RT_MODEL_SupervisorFSM_RX_T; - -#endif // RTW_HEADER_SupervisorFSM_RX_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_RX_types.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 5.7 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:19 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_RX_types_h_ +#define RTW_HEADER_SupervisorFSM_RX_types_h_ +#include "rtwtypes.h" +#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ +#define DEFINED_TYPEDEF_FOR_JointPositions_ + +struct JointPositions +{ + // joint positions + real32_T position; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ +#define DEFINED_TYPEDEF_FOR_ExternalFlags_ + +struct ExternalFlags +{ + // External Fault Button (1 == pressed) + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ +#define DEFINED_TYPEDEF_FOR_MotorCurrent_ + +struct MotorCurrent +{ + // motor current + real32_T current; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOutputs_ + +struct ControlOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + MotorCurrent Iq_fbk; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ +#define DEFINED_TYPEDEF_FOR_MCControlModes_ + +typedef enum { + MCControlModes_Idle = 0, // Default value + MCControlModes_OpenLoop = 80, + MCControlModes_SpeedVoltage = 10, + MCControlModes_SpeedCurrent = 11, + MCControlModes_Current = 6, + MCControlModes_NotConfigured = 176, + MCControlModes_HWFault = 160 +} MCControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ + +// Fields of a CONTROL_MODE message. +struct BUS_MSG_CONTROL_MODE +{ + // Motor selector. + boolean_T motor; + + // Control mode. + MCControlModes mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ + +// Fields of a CURRENT_LIMIT message. +struct BUS_MSG_CURRENT_LIMIT +{ + // Motor selector. + boolean_T motor; + + // Nominal current in A. + real32_T nominal; + + // Peak current in A. + real32_T peak; + + // Overload current in A. + real32_T overload; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ + +// Fields of a DESIRED_TARGETS message. +struct BUS_MSG_DESIRED_TARGETS +{ + // Target current in A. + real32_T current; + + // Target voltage in %. + real32_T voltage; + + // Target veocity in deg/s. + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ + +// Fields of a CURRENT_PID message. +struct BUS_MSG_PID +{ + // Motor selector. + boolean_T motor; + + // Proportional gain. + real32_T Kp; + + // Integral gain. + real32_T Ki; + + // Derivative gain. + real32_T Kd; + + // Shift factor. + uint8_T Ks; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ + +struct BUS_MSG_MOTOR_CONFIG +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + + // Number of polese of the motor. + uint8_T number_poles; + + // Encoder tolerance. + uint8_T encoder_tolerance; + + // Resolution of rotor encoder. + int16_T rotor_encoder_resolution; + + // Offset of the rotor encoder. + int16_T rotor_index_offset; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ + +// Aggregate of all CAN received messages. +struct BUS_MESSAGES_RX +{ + BUS_MSG_CONTROL_MODE control_mode; + BUS_MSG_CURRENT_LIMIT current_limit; + BUS_MSG_DESIRED_TARGETS desired_targets; + BUS_MSG_PID pid; + BUS_MSG_MOTOR_CONFIG motor_config; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ + +struct BUS_MESSAGES_RX_MULTIPLE +{ + BUS_MESSAGES_RX messages[4]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ +#define DEFINED_TYPEDEF_FOR_JointVelocities_ + +struct JointVelocities +{ + // joint velocities + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocities + JointVelocities jointvelocities; + MotorCurrent Iq_filtered; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ + +// Aggregate of all events specifying types of received messages. +struct BUS_STATUS_RX +{ + boolean_T control_mode; + boolean_T current_limit; + boolean_T desired_targets; + boolean_T current_pid; + boolean_T velocity_pid; + boolean_T motor_config; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ + +struct BUS_STATUS_RX_MULTIPLE +{ + BUS_STATUS_RX status[4]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ +#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ + +typedef enum { + CANErrorTypes_No_Error = 0, // Default value + CANErrorTypes_Packet_Not4Us, + CANErrorTypes_Packet_Unrecognized, + CANErrorTypes_Packet_Malformed, + CANErrorTypes_Packet_MultiFunctionsDetected, + CANErrorTypes_Mode_Unrecognized +} CANErrorTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ + +// Specifies the CAN error types. +struct BUS_CAN_RX_ERRORS +{ + // True if an error has been detected. + boolean_T event; + CANErrorTypes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ + +struct BUS_CAN_RX_ERRORS_MULTIPLE +{ + BUS_CAN_RX_ERRORS errors[4]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_Torque, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + // control mode + ControlModes control_mode; + boolean_T enable_sending_msg_status; + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ +#define DEFINED_TYPEDEF_FOR_MotorVoltage_ + +struct MotorVoltage +{ + // motor voltage + real32_T voltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + JointPositions jointpositions; + JointVelocities jointvelocities; + MotorCurrent motorcurrent; + MotorVoltage motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SV_Limits_ +#define DEFINED_TYPEDEF_FOR_SV_Limits_ + +struct SV_Limits +{ + real32_T overload; + real32_T peak; + real32_T nominal; + ControlModes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BoardState_ +#define DEFINED_TYPEDEF_FOR_BoardState_ + +typedef enum { + BoardState_NotConfigured = 0, // Default value + BoardState_Configured, + BoardState_Fault +} BoardState; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_SupervisorFSM_RX_T RT_MODEL_SupervisorFSM_RX_T; + +#endif // RTW_HEADER_SupervisorFSM_RX_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.cpp index 4187188ab4..00ef6624b8 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.cpp @@ -1,200 +1,200 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_TX.cpp -// -// Code generated for Simulink model 'SupervisorFSM_TX'. -// -// Model version : 5.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:45 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "SupervisorFSM_TX.h" -#include "SupervisorFSM_TX_types.h" -#include "SupervisorFSM_TX_private.h" - -MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_MdlrefDW; - -// Block signals (default storage) -B_SupervisorFSM_TX_c_T SupervisorFSM_TX_B; - -// Block states (default storage) -DW_SupervisorFSM_TX_f_T SupervisorFSM_TX_DW; - -// Forward declaration for local functions -static MCControlModes SupervisorFSM_TX_convert(ControlModes controlmode); - -// Function for Chart: '/SupervisorFSM_TX' -static MCControlModes SupervisorFSM_TX_convert(ControlModes controlmode) -{ - MCControlModes mccontrolmode; - switch (controlmode) { - case ControlModes_Idle: - mccontrolmode = MCControlModes_Idle; - break; - - case ControlModes_Current: - mccontrolmode = MCControlModes_Current; - break; - - case ControlModes_Velocity: - mccontrolmode = MCControlModes_SpeedVoltage; - break; - - case ControlModes_Voltage: - mccontrolmode = MCControlModes_OpenLoop; - break; - - case ControlModes_HwFaultCM: - mccontrolmode = MCControlModes_HWFault; - break; - - default: - mccontrolmode = MCControlModes_Idle; - break; - } - - return mccontrolmode; -} - -// System initialize for referenced model: 'SupervisorFSM_TX' -void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx) -{ - // SystemInitialize for Chart: '/SupervisorFSM_TX' - rty_MessagesTx->foc.current = 0.0F; - rty_MessagesTx->foc.position = 0.0F; - rty_MessagesTx->foc.velocity = 0.0F; - rty_MessagesTx->status.control_mode = MCControlModes_Idle; - rty_MessagesTx->status.pwm_fbk = 0.0F; - rty_MessagesTx->status.flags.dirty = false; - rty_MessagesTx->status.flags.stuck = false; - rty_MessagesTx->status.flags.index_broken = false; - rty_MessagesTx->status.flags.phase_broken = false; - rty_MessagesTx->status.flags.not_calibrated = 0.0F; - rty_MessagesTx->status.flags.ExternalFaultAsserted = false; - rty_MessagesTx->status.flags.UnderVoltageFailure = false; - rty_MessagesTx->status.flags.OverVoltageFailure = false; - rty_MessagesTx->status.flags.OverCurrentFailure = false; - rty_MessagesTx->status.flags.DHESInvalidValue = false; - rty_MessagesTx->status.flags.AS5045CSumError = false; - rty_MessagesTx->status.flags.DHESInvalidSequence = false; - rty_MessagesTx->status.flags.CANInvalidProtocol = false; - rty_MessagesTx->status.flags.CAN_BufferOverRun = false; - rty_MessagesTx->status.flags.SetpointExpired = false; - rty_MessagesTx->status.flags.CAN_TXIsPasv = false; - rty_MessagesTx->status.flags.CAN_RXIsPasv = false; - rty_MessagesTx->status.flags.CAN_IsWarnTX = false; - rty_MessagesTx->status.flags.CAN_IsWarnRX = false; - rty_MessagesTx->status.flags.OverHeating = false; - rty_MessagesTx->status.flags.ADCCalFailure = false; - rty_MessagesTx->status.flags.I2TFailure = false; - rty_MessagesTx->status.flags.EMUROMFault = false; - rty_MessagesTx->status.flags.EMUROMCRCFault = false; - rty_MessagesTx->status.flags.EncoderFault = false; - rty_MessagesTx->status.flags.FirmwareSPITimingError = false; - rty_MessagesTx->status.flags.AS5045CalcError = false; - rty_MessagesTx->status.flags.FirmwarePWMFatalError = false; - rty_MessagesTx->status.flags.CAN_TXWasPasv = false; - rty_MessagesTx->status.flags.CAN_RXWasPasv = false; - rty_MessagesTx->status.flags.CAN_RTRFlagActive = false; - rty_MessagesTx->status.flags.CAN_WasWarn = false; - rty_MessagesTx->status.flags.CAN_DLCError = false; - rty_MessagesTx->status.flags.SiliconRevisionFault = false; - rty_MessagesTx->status.flags.PositionLimitUpper = false; - rty_MessagesTx->status.flags.PositionLimitLower = false; -} - -// Output and update for referenced model: 'SupervisorFSM_TX' -void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const EstimatedData - *rtu_EstimatedData, const Flags *rtu_Flags, const - ControlOutputs *rtu_ControlOutputs, BUS_MESSAGES_TX - *rty_MessagesTx, BUS_STATUS_TX *rty_StatusTx) -{ - // Chart: '/SupervisorFSM_TX' - if (SupervisorFSM_TX_DW.is_active_c3_SupervisorFSM_TX == 0U) { - SupervisorFSM_TX_DW.is_active_c3_SupervisorFSM_TX = 1U; - } else if (rtu_Flags->enable_sending_msg_status) { - rty_MessagesTx->foc.current = rtu_ControlOutputs->Iq_fbk.current; - rty_MessagesTx->foc.velocity = rtu_EstimatedData->jointvelocities.velocity; - rty_MessagesTx->foc.position = rtu_SensorsData->jointpositions.position; - SupervisorFSM_TX_DW.ev_focEventCounter++; - rty_MessagesTx->status.control_mode = SupervisorFSM_TX_convert - (rtu_Flags->control_mode); - rty_MessagesTx->status.pwm_fbk = rtu_ControlOutputs->Vq; - rty_MessagesTx->status.flags.ExternalFaultAsserted = rtu_Flags->fault_button; - SupervisorFSM_TX_DW.ev_statusEventCounter++; - } - - if (SupervisorFSM_TX_DW.ev_focEventCounter > 0U) { - SupervisorFSM_TX_B.ev_foc = !SupervisorFSM_TX_B.ev_foc; - SupervisorFSM_TX_DW.ev_focEventCounter--; - } - - if (SupervisorFSM_TX_DW.ev_statusEventCounter > 0U) { - SupervisorFSM_TX_B.ev_status = !SupervisorFSM_TX_B.ev_status; - SupervisorFSM_TX_DW.ev_statusEventCounter--; - } - - // End of Chart: '/SupervisorFSM_TX' - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rty_StatusTx->foc = (SupervisorFSM_TX_B.ev_foc != - SupervisorFSM_TX_DW.DelayInput1_DSTATE); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rty_StatusTx->status = (SupervisorFSM_TX_B.ev_status != - SupervisorFSM_TX_DW.DelayInput1_DSTATE_d); - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - SupervisorFSM_TX_DW.DelayInput1_DSTATE = SupervisorFSM_TX_B.ev_foc; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - SupervisorFSM_TX_DW.DelayInput1_DSTATE_d = SupervisorFSM_TX_B.ev_status; -} - -// Model initialize function -void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_SupervisorFSM_TX_T *const SupervisorFSM_TX_M = - &(SupervisorFSM_TX_MdlrefDW.rtm); - - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(SupervisorFSM_TX_M, rt_errorStatus); -} - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_TX.cpp +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 5.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "SupervisorFSM_TX.h" +#include "SupervisorFSM_TX_types.h" +#include "SupervisorFSM_TX_private.h" + +MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_MdlrefDW; + +// Block signals (default storage) +B_SupervisorFSM_TX_c_T SupervisorFSM_TX_B; + +// Block states (default storage) +DW_SupervisorFSM_TX_f_T SupervisorFSM_TX_DW; + +// Forward declaration for local functions +static MCControlModes SupervisorFSM_TX_convert(ControlModes controlmode); + +// Function for Chart: '/SupervisorFSM_TX' +static MCControlModes SupervisorFSM_TX_convert(ControlModes controlmode) +{ + MCControlModes mccontrolmode; + switch (controlmode) { + case ControlModes_Idle: + mccontrolmode = MCControlModes_Idle; + break; + + case ControlModes_Current: + mccontrolmode = MCControlModes_Current; + break; + + case ControlModes_Velocity: + mccontrolmode = MCControlModes_SpeedVoltage; + break; + + case ControlModes_Voltage: + mccontrolmode = MCControlModes_OpenLoop; + break; + + case ControlModes_HwFaultCM: + mccontrolmode = MCControlModes_HWFault; + break; + + default: + mccontrolmode = MCControlModes_Idle; + break; + } + + return mccontrolmode; +} + +// System initialize for referenced model: 'SupervisorFSM_TX' +void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx) +{ + // SystemInitialize for Chart: '/SupervisorFSM_TX' + rty_MessagesTx->foc.current = 0.0F; + rty_MessagesTx->foc.position = 0.0F; + rty_MessagesTx->foc.velocity = 0.0F; + rty_MessagesTx->status.control_mode = MCControlModes_Idle; + rty_MessagesTx->status.pwm_fbk = 0.0F; + rty_MessagesTx->status.flags.dirty = false; + rty_MessagesTx->status.flags.stuck = false; + rty_MessagesTx->status.flags.index_broken = false; + rty_MessagesTx->status.flags.phase_broken = false; + rty_MessagesTx->status.flags.not_calibrated = 0.0F; + rty_MessagesTx->status.flags.ExternalFaultAsserted = false; + rty_MessagesTx->status.flags.UnderVoltageFailure = false; + rty_MessagesTx->status.flags.OverVoltageFailure = false; + rty_MessagesTx->status.flags.OverCurrentFailure = false; + rty_MessagesTx->status.flags.DHESInvalidValue = false; + rty_MessagesTx->status.flags.AS5045CSumError = false; + rty_MessagesTx->status.flags.DHESInvalidSequence = false; + rty_MessagesTx->status.flags.CANInvalidProtocol = false; + rty_MessagesTx->status.flags.CAN_BufferOverRun = false; + rty_MessagesTx->status.flags.SetpointExpired = false; + rty_MessagesTx->status.flags.CAN_TXIsPasv = false; + rty_MessagesTx->status.flags.CAN_RXIsPasv = false; + rty_MessagesTx->status.flags.CAN_IsWarnTX = false; + rty_MessagesTx->status.flags.CAN_IsWarnRX = false; + rty_MessagesTx->status.flags.OverHeating = false; + rty_MessagesTx->status.flags.ADCCalFailure = false; + rty_MessagesTx->status.flags.I2TFailure = false; + rty_MessagesTx->status.flags.EMUROMFault = false; + rty_MessagesTx->status.flags.EMUROMCRCFault = false; + rty_MessagesTx->status.flags.EncoderFault = false; + rty_MessagesTx->status.flags.FirmwareSPITimingError = false; + rty_MessagesTx->status.flags.AS5045CalcError = false; + rty_MessagesTx->status.flags.FirmwarePWMFatalError = false; + rty_MessagesTx->status.flags.CAN_TXWasPasv = false; + rty_MessagesTx->status.flags.CAN_RXWasPasv = false; + rty_MessagesTx->status.flags.CAN_RTRFlagActive = false; + rty_MessagesTx->status.flags.CAN_WasWarn = false; + rty_MessagesTx->status.flags.CAN_DLCError = false; + rty_MessagesTx->status.flags.SiliconRevisionFault = false; + rty_MessagesTx->status.flags.PositionLimitUpper = false; + rty_MessagesTx->status.flags.PositionLimitLower = false; +} + +// Output and update for referenced model: 'SupervisorFSM_TX' +void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const EstimatedData + *rtu_EstimatedData, const Flags *rtu_Flags, const + ControlOutputs *rtu_ControlOutputs, BUS_MESSAGES_TX + *rty_MessagesTx, BUS_STATUS_TX *rty_StatusTx) +{ + // Chart: '/SupervisorFSM_TX' + if (SupervisorFSM_TX_DW.is_active_c3_SupervisorFSM_TX == 0U) { + SupervisorFSM_TX_DW.is_active_c3_SupervisorFSM_TX = 1U; + } else if (rtu_Flags->enable_sending_msg_status) { + rty_MessagesTx->foc.current = rtu_ControlOutputs->Iq_fbk.current; + rty_MessagesTx->foc.velocity = rtu_EstimatedData->jointvelocities.velocity; + rty_MessagesTx->foc.position = rtu_SensorsData->jointpositions.position; + SupervisorFSM_TX_DW.ev_focEventCounter++; + rty_MessagesTx->status.control_mode = SupervisorFSM_TX_convert + (rtu_Flags->control_mode); + rty_MessagesTx->status.pwm_fbk = rtu_ControlOutputs->Vq; + rty_MessagesTx->status.flags.ExternalFaultAsserted = rtu_Flags->fault_button; + SupervisorFSM_TX_DW.ev_statusEventCounter++; + } + + if (SupervisorFSM_TX_DW.ev_focEventCounter > 0U) { + SupervisorFSM_TX_B.ev_foc = !SupervisorFSM_TX_B.ev_foc; + SupervisorFSM_TX_DW.ev_focEventCounter--; + } + + if (SupervisorFSM_TX_DW.ev_statusEventCounter > 0U) { + SupervisorFSM_TX_B.ev_status = !SupervisorFSM_TX_B.ev_status; + SupervisorFSM_TX_DW.ev_statusEventCounter--; + } + + // End of Chart: '/SupervisorFSM_TX' + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rty_StatusTx->foc = (SupervisorFSM_TX_B.ev_foc != + SupervisorFSM_TX_DW.DelayInput1_DSTATE); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rty_StatusTx->status = (SupervisorFSM_TX_B.ev_status != + SupervisorFSM_TX_DW.DelayInput1_DSTATE_d); + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + SupervisorFSM_TX_DW.DelayInput1_DSTATE = SupervisorFSM_TX_B.ev_foc; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + SupervisorFSM_TX_DW.DelayInput1_DSTATE_d = SupervisorFSM_TX_B.ev_status; +} + +// Model initialize function +void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_SupervisorFSM_TX_T *const SupervisorFSM_TX_M = + &(SupervisorFSM_TX_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(SupervisorFSM_TX_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.h index efe7f9ef6a..74320a4dc5 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.h @@ -1,114 +1,114 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_TX.h -// -// Code generated for Simulink model 'SupervisorFSM_TX'. -// -// Model version : 5.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:45 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_TX_h_ -#define RTW_HEADER_SupervisorFSM_TX_h_ -#include "rtwtypes.h" -#include "SupervisorFSM_TX_types.h" - -// Block signals for model 'SupervisorFSM_TX' -#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -struct B_SupervisorFSM_TX_c_T { - boolean_T ev_foc; // '/SupervisorFSM_TX' - boolean_T ev_status; // '/SupervisorFSM_TX' -}; - -#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -// Block states (default storage) for model 'SupervisorFSM_TX' -#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -struct DW_SupervisorFSM_TX_f_T { - uint32_T ev_focEventCounter; // '/SupervisorFSM_TX' - uint32_T ev_statusEventCounter; // '/SupervisorFSM_TX' - boolean_T DelayInput1_DSTATE; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_d; // '/Delay Input1' - uint8_T is_active_c3_SupervisorFSM_TX;// '/SupervisorFSM_TX' -}; - -#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -// Real-time Model Data Structure -struct tag_RTM_SupervisorFSM_TX_T { - const char_T **errorStatus; -}; - -#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -struct MdlrefDW_SupervisorFSM_TX_T { - RT_MODEL_SupervisorFSM_TX_T rtm; -}; - -#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -extern void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx); -extern void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const - EstimatedData *rtu_EstimatedData, const Flags *rtu_Flags, const ControlOutputs - *rtu_ControlOutputs, BUS_MESSAGES_TX *rty_MessagesTx, BUS_STATUS_TX - *rty_StatusTx); - -// Model reference registration function -extern void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus); - -#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -extern MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_MdlrefDW; - -#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -// Block signals (default storage) -extern B_SupervisorFSM_TX_c_T SupervisorFSM_TX_B; - -// Block states (default storage) -extern DW_SupervisorFSM_TX_f_T SupervisorFSM_TX_DW; - -#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'SupervisorFSM_TX' -// '' : 'SupervisorFSM_TX/Detect Change' -// '' : 'SupervisorFSM_TX/Detect Change1' -// '' : 'SupervisorFSM_TX/SupervisorFSM_TX' - -#endif // RTW_HEADER_SupervisorFSM_TX_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_TX.h +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 5.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_TX_h_ +#define RTW_HEADER_SupervisorFSM_TX_h_ +#include "rtwtypes.h" +#include "SupervisorFSM_TX_types.h" + +// Block signals for model 'SupervisorFSM_TX' +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +struct B_SupervisorFSM_TX_c_T { + boolean_T ev_foc; // '/SupervisorFSM_TX' + boolean_T ev_status; // '/SupervisorFSM_TX' +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for model 'SupervisorFSM_TX' +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +struct DW_SupervisorFSM_TX_f_T { + uint32_T ev_focEventCounter; // '/SupervisorFSM_TX' + uint32_T ev_statusEventCounter; // '/SupervisorFSM_TX' + boolean_T DelayInput1_DSTATE; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_d; // '/Delay Input1' + uint8_T is_active_c3_SupervisorFSM_TX;// '/SupervisorFSM_TX' +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_SupervisorFSM_TX_T { + const char_T **errorStatus; +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_SupervisorFSM_TX_T { + RT_MODEL_SupervisorFSM_TX_T rtm; +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +extern void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx); +extern void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const + EstimatedData *rtu_EstimatedData, const Flags *rtu_Flags, const ControlOutputs + *rtu_ControlOutputs, BUS_MESSAGES_TX *rty_MessagesTx, BUS_STATUS_TX + *rty_StatusTx); + +// Model reference registration function +extern void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus); + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_MdlrefDW; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +// Block signals (default storage) +extern B_SupervisorFSM_TX_c_T SupervisorFSM_TX_B; + +// Block states (default storage) +extern DW_SupervisorFSM_TX_f_T SupervisorFSM_TX_DW; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'SupervisorFSM_TX' +// '' : 'SupervisorFSM_TX/Detect Change' +// '' : 'SupervisorFSM_TX/Detect Change1' +// '' : 'SupervisorFSM_TX/SupervisorFSM_TX' + +#endif // RTW_HEADER_SupervisorFSM_TX_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_private.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_private.h index 196742999e..e37c111bc6 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_private.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_private.h @@ -1,46 +1,46 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_TX_private.h -// -// Code generated for Simulink model 'SupervisorFSM_TX'. -// -// Model version : 5.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:45 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_TX_private_h_ -#define RTW_HEADER_SupervisorFSM_TX_private_h_ -#include "rtwtypes.h" -#include "SupervisorFSM_TX_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif -#endif // RTW_HEADER_SupervisorFSM_TX_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_TX_private.h +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 5.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_TX_private_h_ +#define RTW_HEADER_SupervisorFSM_TX_private_h_ +#include "rtwtypes.h" +#include "SupervisorFSM_TX_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif +#endif // RTW_HEADER_SupervisorFSM_TX_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_types.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_types.h index 32053ff7dc..2d30c62f08 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_types.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_types.h @@ -1,399 +1,399 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_TX_types.h -// -// Code generated for Simulink model 'SupervisorFSM_TX'. -// -// Model version : 5.0 -// Simulink Coder version : 9.8 (R2022b) 13-May-2022 -// C/C++ source code generated on : Wed Sep 28 09:22:45 2022 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_TX_types_h_ -#define RTW_HEADER_SupervisorFSM_TX_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ - -struct SensorsData -{ - // position encoders - JointPositions jointpositions; - MotorSensors motorsensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocities - JointVelocities jointvelocities; - MotorCurrent Iq_filtered; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ -#define DEFINED_TYPEDEF_FOR_ControlModes_ - -typedef enum { - ControlModes_NotConfigured = 0, // Default value - ControlModes_Idle, - ControlModes_Position, - ControlModes_PositionDirect, - ControlModes_Current, - ControlModes_Velocity, - ControlModes_Voltage, - ControlModes_Torque, - ControlModes_HwFaultCM -} ControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ - -struct Flags -{ - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs -{ - // control effort (quadrature) - real32_T Vq; - - // control effort (3-phases) - real32_T Vabc[3]; - - // quadrature current - MotorCurrent Iq_fbk; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vcc; - real32_T Vmax; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ - -// Aggregate of all events specifying types of transmitted messages. -struct BUS_STATUS_TX -{ - boolean_T foc; - boolean_T status; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ - -// Fields of a FOC message. -struct BUS_MSG_FOC -{ - // Current feedback in A. - real32_T current; - - // Position feedback in deg. - real32_T position; - - // Velocity feedback in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ -#define DEFINED_TYPEDEF_FOR_MCControlModes_ - -typedef enum { - MCControlModes_Idle = 0, // Default value - MCControlModes_OpenLoop = 80, - MCControlModes_SpeedVoltage = 10, - MCControlModes_SpeedCurrent = 11, - MCControlModes_Current = 6, - MCControlModes_NotConfigured = 176, - MCControlModes_HWFault = 160 -} MCControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ - -struct BUS_FLAGS_TX -{ - boolean_T dirty; - boolean_T stuck; - boolean_T index_broken; - boolean_T phase_broken; - real32_T not_calibrated; - boolean_T ExternalFaultAsserted; - boolean_T UnderVoltageFailure; - boolean_T OverVoltageFailure; - boolean_T OverCurrentFailure; - boolean_T DHESInvalidValue; - boolean_T AS5045CSumError; - boolean_T DHESInvalidSequence; - boolean_T CANInvalidProtocol; - boolean_T CAN_BufferOverRun; - boolean_T SetpointExpired; - boolean_T CAN_TXIsPasv; - boolean_T CAN_RXIsPasv; - boolean_T CAN_IsWarnTX; - boolean_T CAN_IsWarnRX; - boolean_T OverHeating; - boolean_T ADCCalFailure; - boolean_T I2TFailure; - boolean_T EMUROMFault; - boolean_T EMUROMCRCFault; - boolean_T EncoderFault; - boolean_T FirmwareSPITimingError; - boolean_T AS5045CalcError; - boolean_T FirmwarePWMFatalError; - boolean_T CAN_TXWasPasv; - boolean_T CAN_RXWasPasv; - boolean_T CAN_RTRFlagActive; - boolean_T CAN_WasWarn; - boolean_T CAN_DLCError; - boolean_T SiliconRevisionFault; - boolean_T PositionLimitUpper; - boolean_T PositionLimitLower; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ - -struct BUS_MSG_STATUS -{ - MCControlModes control_mode; - real32_T pwm_fbk; - BUS_FLAGS_TX flags; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ - -// Aggregate of all CAN transmitted messages. -struct BUS_MESSAGES_TX -{ - BUS_MSG_FOC foc; - BUS_MSG_STATUS status; -}; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_SupervisorFSM_TX_T RT_MODEL_SupervisorFSM_TX_T; - -#endif // RTW_HEADER_SupervisorFSM_TX_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: SupervisorFSM_TX_types.h +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 5.0 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Fri Feb 10 13:57:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_TX_types_h_ +#define RTW_HEADER_SupervisorFSM_TX_types_h_ +#include "rtwtypes.h" +#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ +#define DEFINED_TYPEDEF_FOR_JointPositions_ + +struct JointPositions +{ + // joint positions + real32_T position; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ +#define DEFINED_TYPEDEF_FOR_JointVelocities_ + +struct JointVelocities +{ + // joint velocities + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ +#define DEFINED_TYPEDEF_FOR_MotorCurrent_ + +struct MotorCurrent +{ + // motor current + real32_T current; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocities + JointVelocities jointvelocities; + MotorCurrent Iq_filtered; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_Torque, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + // control mode + ControlModes control_mode; + boolean_T enable_sending_msg_status; + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOutputs_ + +struct ControlOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + MotorCurrent Iq_fbk; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ +#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + +struct EstimationConfig +{ + EstimationVelocityModes velocity_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ +#define DEFINED_TYPEDEF_FOR_PIDConfig_ + +struct PIDConfig +{ + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // It shall be greater than hardwareJntPosMin + real32_T jntPosMin; + + // It shall be smaller than hardwareJntPosMax + real32_T jntPosMax; + + // Imposed by hardware constraint + real32_T hardwareJntPosMin; + + // Imposed by hardware constraint + real32_T hardwareJntPosMax; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMin; + + // If robotMin == rotorMax == 0, there's no check + real32_T rotorPosMax; + + // Can be only non-negative + real32_T jntVelMax; + + // Timeout on reception of velocity setpoint + // Can be only non-negative + uint32_T velocityTimeout; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ + +struct ConfigurationParameters +{ + MotorConfig motorconfig; + EstimationConfig estimationconfig; + PIDConfig CurLoopPID; + PIDConfig PosLoopPID; + PIDConfig VelLoopPID; + PIDConfig DirLoopPID; + Thresholds thresholds; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ + +// Aggregate of all events specifying types of transmitted messages. +struct BUS_STATUS_TX +{ + boolean_T foc; + boolean_T status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ + +// Fields of a FOC message. +struct BUS_MSG_FOC +{ + // Current feedback in A. + real32_T current; + + // Position feedback in deg. + real32_T position; + + // Velocity feedback in deg/s. + real32_T velocity; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ +#define DEFINED_TYPEDEF_FOR_MCControlModes_ + +typedef enum { + MCControlModes_Idle = 0, // Default value + MCControlModes_OpenLoop = 80, + MCControlModes_SpeedVoltage = 10, + MCControlModes_SpeedCurrent = 11, + MCControlModes_Current = 6, + MCControlModes_NotConfigured = 176, + MCControlModes_HWFault = 160 +} MCControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ + +struct BUS_FLAGS_TX +{ + boolean_T dirty; + boolean_T stuck; + boolean_T index_broken; + boolean_T phase_broken; + real32_T not_calibrated; + boolean_T ExternalFaultAsserted; + boolean_T UnderVoltageFailure; + boolean_T OverVoltageFailure; + boolean_T OverCurrentFailure; + boolean_T DHESInvalidValue; + boolean_T AS5045CSumError; + boolean_T DHESInvalidSequence; + boolean_T CANInvalidProtocol; + boolean_T CAN_BufferOverRun; + boolean_T SetpointExpired; + boolean_T CAN_TXIsPasv; + boolean_T CAN_RXIsPasv; + boolean_T CAN_IsWarnTX; + boolean_T CAN_IsWarnRX; + boolean_T OverHeating; + boolean_T ADCCalFailure; + boolean_T I2TFailure; + boolean_T EMUROMFault; + boolean_T EMUROMCRCFault; + boolean_T EncoderFault; + boolean_T FirmwareSPITimingError; + boolean_T AS5045CalcError; + boolean_T FirmwarePWMFatalError; + boolean_T CAN_TXWasPasv; + boolean_T CAN_RXWasPasv; + boolean_T CAN_RTRFlagActive; + boolean_T CAN_WasWarn; + boolean_T CAN_DLCError; + boolean_T SiliconRevisionFault; + boolean_T PositionLimitUpper; + boolean_T PositionLimitLower; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ + +struct BUS_MSG_STATUS +{ + MCControlModes control_mode; + real32_T pwm_fbk; + BUS_FLAGS_TX flags; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ + +// Aggregate of all CAN transmitted messages. +struct BUS_MESSAGES_TX +{ + BUS_MSG_FOC foc; + BUS_MSG_STATUS status; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_SupervisorFSM_TX_T RT_MODEL_SupervisorFSM_TX_T; + +#endif // RTW_HEADER_SupervisorFSM_TX_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.c b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.c index a21856c3a0..536b9aa9e0 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.c +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.c @@ -108,7 +108,7 @@ HAL_StatusTypeDef encoderDeinit(void) return HAL_OK; } -HAL_StatusTypeDef encoderConfig(int16_t resolution, uint8_t num_polar_couples, uint8_t has_hall_sens) +HAL_StatusTypeDef encoderConfig(uint8_t has_quad_enc, int16_t resolution, uint8_t num_polar_couples, uint8_t has_hall_sens) { TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; @@ -117,6 +117,16 @@ HAL_StatusTypeDef encoderConfig(int16_t resolution, uint8_t num_polar_couples, u MainConf.encoder.has_hall_sens = has_hall_sens; MainConf.pwm.num_polar_couples = num_polar_couples; + if(!has_quad_enc) + resolution=0; + + MainConf.encoder.resolution = resolution; + + if (resolution == 0) + { + return HAL_OK; + } + if (resolution < 0) { resolution = -resolution; @@ -127,12 +137,6 @@ HAL_StatusTypeDef encoderConfig(int16_t resolution, uint8_t num_polar_couples, u htim2.Init.CounterMode = TIM_COUNTERMODE_UP; } - MainConf.encoder.resolution = resolution; - - if (resolution == 0) - { - return HAL_OK; - } /* Forced, for now */ encoderConvFactor = 65536L*num_polar_couples/resolution; diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.h index a74aa4ad47..c4e81aaf0f 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.h @@ -46,7 +46,7 @@ typedef struct extern HAL_StatusTypeDef encoderInit(void); extern HAL_StatusTypeDef encoderDeinit(void); -extern HAL_StatusTypeDef encoderConfig(int16_t resolution, uint8_t num_polar_couples, uint8_t has_hall_sens); +extern HAL_StatusTypeDef encoderConfig(uint8_t has_quad_enc, int16_t resolution, uint8_t num_polar_couples, uint8_t has_hall_sens); extern uint32_t encoderGetCounter(void); extern void encoderReset(); diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp index ac6e5c0577..b5fd23eb9f 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp @@ -141,6 +141,7 @@ namespace embot { namespace hw { namespace motor { result_t s_hw_deinit(MOTOR h); result_t s_hw_configure( MOTOR h, + uint8_t has_quad_enc, int16_t enc_resolution, uint8_t pwm_num_polar_couples, uint8_t pwm_has_hall_sens, @@ -187,6 +188,7 @@ namespace embot { namespace hw { namespace motor { result_t config( MOTOR h, + uint8_t has_quad_enc, int16_t enc_resolution, uint8_t pwm_num_polar_couples, uint8_t pwm_has_hall_sens, @@ -194,6 +196,7 @@ namespace embot { namespace hw { namespace motor { uint16_t pwm_hall_offset) { return s_hw_configure(h, + has_quad_enc, enc_resolution, pwm_num_polar_couples, pwm_has_hall_sens, @@ -409,6 +412,7 @@ namespace embot { namespace hw { namespace motor { result_t s_hw_configure( MOTOR h, + uint8_t has_quad_enc, int16_t enc_resolution, uint8_t pwm_num_polar_couples, uint8_t pwm_has_hall_sens, @@ -421,7 +425,7 @@ namespace embot { namespace hw { namespace motor { // s_hw_init(h); - if (HAL_OK != encoderConfig(enc_resolution, pwm_num_polar_couples, pwm_has_hall_sens)) res = resNOK; + if (HAL_OK != encoderConfig(has_quad_enc, enc_resolution, pwm_num_polar_couples, pwm_has_hall_sens)) res = resNOK; if (HAL_OK != hallConfig(pwm_swapBC, pwm_hall_offset)) res = resNOK; return res; diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h index 1bdc5381bd..830900c294 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h @@ -53,7 +53,8 @@ namespace embot { namespace hw { namespace motor { result_t fault(MOTOR h, bool on); result_t init(embot::hw::MOTOR h, const Config &config); - result_t config(MOTOR h, + result_t config(MOTOR h, + uint8_t has_quad_enc, int16_t enc_resolution, uint8_t pwm_num_polar_couples, uint8_t pwm_has_hall_sens,