无人驾驶车辆模型预测控制代码

/* Include files */ #include <stddef.h> #include "blas.h" #include "mpclib_sfun.h" #include "c3_mpclib.h" #define CHARTINSTANCE_CHARTNUMBER (chartInstance->chartNumber) #define CHARTINSTANCE_INSTANCENUMBER (chartInstance->instanceNumber) #include "mpclib_sfun_debug_macros.h" #define _SF_MEX_LISTEN_FOR_CTRL_C(S) sf_mex_listen_for_ctrl_c(sfGlobalDebugInstanceStruct,S); /* Type Definitions */ /* Named Constants */ #define CALL_EVENT (-1) #define c3_b_p (5.0) #define c3_b_nu (3.0) #define c3_b_isQP (TRUE) #define c3_b_ny (2.0) #define c3_b_degrees (7.0) #define c3_b_maxiter (120.0) #define c3_b_nxQP (11.0) #define c3_b_openloopflag (FALSE) #define c3_b_lims_inport (0.0) #define c3_b_no_umin (1.0) #define c3_b_no_umax (1.0) #define c3_b_no_ymin (1.0) #define c3_b_no_ymax (1.0) #define c3_b_switch_inport (0.0) #define c3_b_no_switch (1.0) #define c3_b_enable_value (0.0) #define c3_b_return_cost (0.0) #define c3_b_return_sequence (0.0) #define c3_b_no_ywt (1.0) #define c3_b_no_duwt (1.0) #define c3_b_no_rhoeps (1.0) #define c3_b_Wy (0.0) #define c3_b_Wdu (0.0) #define c3_b_Jm (0.0) #define c3_b_SuJm (0.0) #define c3_b_I2JmWuI2Jm (0.0) #define c3_b_Su1 (0.0) #define c3_b_I1WuI2Jm (0.0) #define c3_b_Sx (0.0) #define c3_b_Hv (0.0) #define c3_b_Wu (0.0) #define c3_b_I1 (0.0) /* Variable Declarations */ /* Variable Definitions */ static const char * c3_debug_family_names[71] = { "DataType", "isQP", "nu", "ny", "degrees", "Hinv", "Kx", "Ku1", "Kut", "Kr", "Kv", "Mlim", "Mx", "Mu1", "Mv", "z_degrees", "utarget", "p", "uoff", "yoff", "maxiter", "nxQP", "openloopflag", "lims_inport", "no_umin", "no_umax", "no_ymin", "no_ymax", "switch_inport", "no_switch", "enable_value", "return_cost", "H", "return_sequence", "blocking_moves", "Linv", "Ac", "no_ywt", "no_duwt", "no_rhoeps", "Wy", "Wdu", "Jm", "SuJm", "I2JmWuI2Jm", "Su1", "I1WuI2Jm", "Sx", "Hv", "Wu", "I1", "nargin", "nargout", "rseq", "vseq", "x", "old_u", "iA", "umin", "umax", "ymin", "ymax", "switch_in", "ywt", "duwt", "rhoeps", "u", "cost", "useq", "status", "iAout" }; /* Function Declarations */ static void initialize_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void initialize_params_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void enable_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void disable_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void c3_update_debugger_state_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static const mxArray *get_sim_state_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void set_sim_state_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_st); static void finalize_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void sf_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void c3_chartstep_c3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void initSimStructsc3_mpclib(SFc3_mpclibInstanceStruct *chartInstance); static void init_script_number_translation(uint32_T c3_machineNumber, uint32_T c3_chartNumber); static const mxArray *c3_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static void c3_sf_marshallIn(void *chartInstanceVoid, const mxArray *c3_mxArrayInData, const char_T *c3_varName, void *c3_outData); static const mxArray *c3_b_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static void c3_b_sf_marshallIn(void *chartInstanceVoid, const mxArray *c3_mxArrayInData, const char_T *c3_varName, void *c3_outData); static const mxArray *c3_c_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static void c3_c_sf_marshallIn(void *chartInstanceVoid, const mxArray *c3_mxArrayInData, const char_T *c3_varName, void *c3_outData); static const mxArray *c3_d_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static void c3_d_sf_marshallIn(void *chartInstanceVoid, const mxArray *c3_mxArrayInData, const char_T *c3_varName, void *c3_outData); static const mxArray *c3_e_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_f_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_g_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_h_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_i_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_j_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_k_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_l_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_m_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_n_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_o_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_p_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_q_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_r_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_s_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_t_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_u_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_v_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_w_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static const mxArray *c3_x_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static void c3_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_u, const char_T *c3_identifier, real_T c3_y[3]); static void c3_b_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_u, const emlrtMsgIdentifier *c3_parentId, real_T c3_y[3]); static real_T c3_c_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_cost, const char_T *c3_identifier); static real_T c3_d_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_u, const emlrtMsgIdentifier *c3_parentId); static void c3_e_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_useq, const char_T *c3_identifier, real_T c3_y[15]); static void c3_f_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_u, const emlrtMsgIdentifier *c3_parentId, real_T c3_y[15]); static void c3_g_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_iAout, const char_T *c3_identifier, boolean_T c3_y[25]); static void c3_h_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_u, const emlrtMsgIdentifier *c3_parentId, boolean_T c3_y[25]); static const mxArray *c3_y_sf_marshallOut(void *chartInstanceVoid, void *c3_inData); static int32_T c3_i_emlrt_marshallIn(SFc3_mpclibInstanceStruct *chartInstance, const mxArray *c3_u, const emlrtMsgIdentifier *c3_parentId); static void c3_e_sf_marshallIn(void *chartInstanceVoid, const mxArray *c3_mxArrayInData,