diff --git a/X7/hal_3axis_magn_wrapper.c b/X7/hal_3axis_magn_wrapper.c new file mode 100644 index 0000000..73fba54 --- /dev/null +++ b/X7/hal_3axis_magn_wrapper.c @@ -0,0 +1,131 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + +#include "hal_api.h" + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "spi_devs.h" +#endif +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int magn_inited[3]; + +void initialize_3axis_magn(uint8_t id) +{ + switch (id) + { + case 0: + if (!magn_inited[0]) + { +#ifdef HAL_IMPL + // TODO initialize 3axis-magn No.0 + magn_inited[0] = 1; +#endif + } + break; + + case 1: + if (!magn_inited[1]) + { +#ifdef HAL_IMPL + // TODO initialize 3axis-magn No.1 + magn_inited[1] = 1; +#endif + } + break; + + case 2: + if (!magn_inited[2]) + { +#ifdef HAL_IMPL + // TODO initialize 3axis-magn No.2 + magn_inited[2] = 1; +#endif + } + break; + + default: + break; + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_3axis_magn_Start_wrapper(const uint8_T *id, const int_T p_width0) +{ + /* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ + initialize_3axis_magn(id[0]); + /* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_3axis_magn_Outputs_wrapper(HAL_magn_mG_t *magn, + int32_T *ErrorCode, + const uint8_T *id, const int_T p_width0) +{ + /* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ + ErrorCode[0] = -1; + switch (id[0]) + { + case 0: + if (magn_inited[0]) + { +#ifdef HAL_IMPL + if (cps_rm3100.success) + { + magn->x = cps_rm3100.magx; + magn->y = cps_rm3100.magy; + magn->z = cps_rm3100.magz; + magn->seq = cps_rm3100.seq; + + ErrorCode[0] = 0; + } + else + ErrorCode[0] = -1; +#endif + } + break; + + case 1: + if (magn_inited[1]) + { +#ifdef HAL_IMPL + ErrorCode[0] = -1; +#endif + } + break; + case 2: + if (magn_inited[2]) + { +#ifdef HAL_IMPL + ErrorCode[0] = -1; +#endif + } + break; + default: + break; + } + /* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} diff --git a/X7/hal_6axis_imu_wrapper.c b/X7/hal_6axis_imu_wrapper.c new file mode 100644 index 0000000..20fab82 --- /dev/null +++ b/X7/hal_6axis_imu_wrapper.c @@ -0,0 +1,176 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + +#include "hal_api.h" + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "spi_devs.h" +#endif +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int imu_inited[3]; + +void initialize_6axis_imu(uint8_t id) +{ + switch (id) + { + case 0: + if (!imu_inited[0]) + { +#ifdef HAL_IMPL + // TODO initialize 6axis-IMU No.0 + imu_inited[0] = 1; +#endif + } + break; + + case 1: + if (!imu_inited[1]) + { +#ifdef HAL_IMPL + // TODO initialize 6axis-IMU No.1 + imu_inited[1] = 1; +#endif + } + break; + + case 2: + if (!imu_inited[2]) + { +#ifdef HAL_IMPL + // TODO initialize 6axis-IMU No.2 + imu_inited[2] = 1; +#endif + } + break; + + default: + break; + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_6axis_imu_Start_wrapper(const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +initialize_6axis_imu(id[0]); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_6axis_imu_Outputs_wrapper(HAL_acc_SI_t *acc, + HAL_gyro_SI_t *gyro, + int32_T *ErrorCode, + const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ + ErrorCode[0] = -1; + switch (id[0]) + { + case 0: + if (imu_inited[0]) + { +#ifdef HAL_IMPL + // read 6axis-IMU + if (imu_adis1647x.success) + { + acc->a[0] = imu_adis1647x.accel[0]; + acc->a[1] = imu_adis1647x.accel[1]; + acc->a[2] = imu_adis1647x.accel[2]; + gyro->v[0] = imu_adis1647x.gyro[0]; + gyro->v[1] = imu_adis1647x.gyro[1]; + gyro->v[2] = imu_adis1647x.gyro[2]; + acc->seq = imu_adis1647x.seq; + gyro->seq = imu_adis1647x.seq; + acc->temperature = imu_adis1647x.temp; + gyro->temperature = imu_adis1647x.temp; + + ErrorCode[0] = 0; + } + else if (imu_icm20689.success) + { + acc->a[0] = imu_icm20689.accel[0]; + acc->a[1] = imu_icm20689.accel[1]; + acc->a[2] = imu_icm20689.accel[2]; + gyro->v[0] = imu_icm20689.gyro[0]; + gyro->v[1] = imu_icm20689.gyro[1]; + gyro->v[2] = imu_icm20689.gyro[2]; + acc->seq = imu_icm20689.seq; + gyro->seq = imu_icm20689.seq; + acc->temperature = imu_icm20689.temp_degc; + gyro->temperature = imu_icm20689.temp_degc; + + ErrorCode[0] = 0; + } + else + ErrorCode[0] = -1; +#endif + } + break; + + case 1: + if (imu_inited[1]) + { +#ifdef HAL_IMPL + // read 6axis-IMU + if (imu_bmi088.success) + { + acc->a[0] = imu_bmi088.accel[0]; + acc->a[1] = imu_bmi088.accel[1]; + acc->a[2] = imu_bmi088.accel[2]; + gyro->v[0] = imu_bmi088.gyro[0]; + gyro->v[1] = imu_bmi088.gyro[1]; + gyro->v[2] = imu_bmi088.gyro[2]; + acc->seq = imu_bmi088.accel_seq; + gyro->seq = imu_bmi088.gyro_seq; + acc->temperature = imu_bmi088.temp_degc; + gyro->temperature = imu_bmi088.temp_degc; + + ErrorCode[0] = 0; + } + else + ErrorCode[0] = -1; +#endif + } + break; + + case 2: + if (imu_inited[2]) + { +#ifdef HAL_IMPL + // TODO read 6axis-IMU + ErrorCode[0] = -1; +#endif + } + break; + default: + break; + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_ai_wrapper.c b/X7/hal_ai_wrapper.c new file mode 100644 index 0000000..9f50b65 --- /dev/null +++ b/X7/hal_ai_wrapper.c @@ -0,0 +1,88 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "adc_devs.h" +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define u_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int is_analog_input_init = 0; + +void analog_input_init() +{ + if (!is_analog_input_init) + { +#ifdef HAL_IMPL + //TODO + is_analog_input_init = 1; +#endif + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_ai_Start_wrapper(void) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +analog_input_init(); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_ai_Outputs_wrapper(const uint16_T *start_idx, + uint16_T *AI, + int32_T *ErrorCode, + const int_T y_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + if (is_analog_input_init) + { +#ifdef HAL_IMPL + /* + * input *AI* from ADC channels from *start_idx* to *start_idx*+y_width-1. + * + * ErrorCode - return zero on success, otherwise nonzero on failures. + */ + for (int i=0; i= 0) + { + AI[i] = adc_values_v[ch]*1000; + } + else + { + AI[i] = 0; + } + } + ErrorCode[0] = 0; +#endif + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_ao_wrapper.c b/X7/hal_ao_wrapper.c new file mode 100644 index 0000000..71a5165 --- /dev/null +++ b/X7/hal_ao_wrapper.c @@ -0,0 +1,77 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +//#include "param_mgr.h" +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int is_analog_output_init = 0; + +void analog_output_init() +{ + if (!is_analog_output_init) + { +#ifdef HAL_IMPL + //TODO + is_analog_output_init = 1; +#endif + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_ao_Start_wrapper(void) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +analog_output_init(); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_ao_Outputs_wrapper(const uint16_T *AO, + const uint16_T *start_idx, + int32_T *ErrorCode, + const int_T u_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + if (is_analog_output_init) + { +#ifdef HAL_IMPL + /* + * output *AO* to analog output devices from *start_idx* to *start_idx*+u_width-1. + * + * ErrorCode - return zero on success, otherwise nonzero on failures. + */ + // TODO set AO to DAC channels + ErrorCode[0] = 0; +#endif + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_clock_wrapper.c b/X7/hal_clock_wrapper.c new file mode 100644 index 0000000..5230e56 --- /dev/null +++ b/X7/hal_clock_wrapper.c @@ -0,0 +1,77 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +/* TODO include files */ +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int is_init = 0; + +static void initialize_clock() +{ + if (!is_init) + { +#ifdef HAL_IMPL + /* TODO setup clock */ + is_init = 1; +#endif + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_clock_Start_wrapper(const uint8_T *typ, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +initialize_clock(); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_clock_Outputs_wrapper(uint32_T *boot_time, + const uint8_T *typ, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +if (is_init) +{ +#ifdef HAL_IMPL +//TODO get clock + // typ: 0 - msec; 1 - usec; + if(typ[0] == 0) + { + boot_time[0] = get_milliseconds(); + } + else if(typ[0] == 1) + { + boot_time[0] = get_mircoseconds(); + } +#endif +} +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_di_wrapper.c b/X7/hal_di_wrapper.c new file mode 100644 index 0000000..426ff0f --- /dev/null +++ b/X7/hal_di_wrapper.c @@ -0,0 +1,80 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "gpio_devs.h" +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define u_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int is_discrete_input_init = 0; + +void discrete_input_init() +{ + if (!is_discrete_input_init) + { +#ifdef HAL_IMPL + //TODO + is_discrete_input_init = 1; +#endif + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_di_Start_wrapper(void) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +discrete_input_init(); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_di_Outputs_wrapper(const uint16_T *start_idx, + boolean_T *DI, + int32_T *ErrorCode, + const int_T y_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + if (is_discrete_input_init) + { +#ifdef HAL_IMPL + /* + * input *DI* from DI devices from *start_idx* to *start_idx*+y_width-1. + * + * ErrorCode - return zero on success, otherwise nonzero on failures. + */ + for (int i=0; i +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int gps_inited[2]; + +void initialize_gps(uint8_t id) +{ + switch (id) + { + case 0: + if (!gps_inited[0]) + { +#ifdef HAL_IMPL + gps_inited[0] = 1; +#endif + } + break; + + case 1: + if (!gps_inited[1]) + { +#ifdef HAL_IMPL + gps_inited[1] = 1; +#endif + } + break; + + default: + break; + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_gps_Start_wrapper(const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +initialize_gps(id[0]); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_gps_Outputs_wrapper(HAL_GPS_SI_t *gps, + int32_T *ErrorCode, + const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + switch (id[0]) + { + case 0: + if (gps_inited[id[0]]) + { +#ifdef HAL_IMPL + gps->latitude = g_Ublox.LatDeg*1e-7; + gps->longitude = g_Ublox.LonDeg*1e-7; + gps->altitude = g_Ublox.msl*0.001f; + + gps->vel_north = g_Ublox.vn*0.001f; + gps->vel_east = g_Ublox.ve*0.001f; + gps->vel_down = g_Ublox.vd*0.001f; + + gps->heading = 0; ///< Unit is radian, valid when fixtype_att>1 + gps->pitch = 0; ///< Unit is radian, valid when fixtype_att>1 + + gps->h_acc = g_Ublox.hacc*0.001f; + gps->v_acc = g_Ublox.vacc*0.001f; + gps->vel_acc = g_Ublox.sacc*0.001f; + gps->att_acc = g_Ublox.cacc/57.3f; + + gps->TOW = g_Ublox.towms; + gps->WN = (g_Ublox.utc.year%100)*10000+g_Ublox.utc.month*100+g_Ublox.utc.day; + gps->seq = g_Ublox.seq; + gps->seq_att = 0; + gps->HDOP = g_Ublox.pdop*100; //RTK传过来的只有pdop + gps->VDOP = g_Ublox.pdop*100; //RTK传过来的只有pdop + if (g_Ublox.RtkType & 0x01) + { + gps->fixtype = g_Ublox.GpsType; + } + else + { + gps->fixtype = 0; + } + gps->fixtype_att = 0; //普通GPS没意义 赋值0 + gps->satnum = g_Ublox.SvNum; + + ErrorCode[0] = 0; +#endif + } + else + ErrorCode[0] = 1; + break; + + case 1: + if (gps_inited[1]) + { +#ifdef HAL_IMPL + ErrorCode[0] = 1; +#endif + } + break; + default: + break; + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_ins_wrapper.c b/X7/hal_ins_wrapper.c new file mode 100644 index 0000000..39fdb0c --- /dev/null +++ b/X7/hal_ins_wrapper.c @@ -0,0 +1,119 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + +#include "hal_api.h" + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +/* TODO include files */ +#include "fuckQvar.h" +uint32_t g_ins_seq = 0; + +#endif +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int ins_inited[1]; + +void initialize_ins(uint8_t id) +{ + switch (id) + { + case 0: + if (!ins_inited[0]) + { +#ifdef HAL_IMPL + // TODO initialize GPS No.0 + ins_inited[0] = 1; +#endif + } + break; + default: + break; + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_ins_Start_wrapper(const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +initialize_ins(id[0]); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_ins_Outputs_wrapper(HAL_INS_SI_t *ins, + int32_T *ErrorCode, + const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + switch (id[0]) + { + case 0: + if (ins_inited[0]) + { +#ifdef HAL_IMPL + // TODO read pressure + ins->latitude = Pack_EKF[EKF_for_FC].alldata.gps_latti; ///< Unit is degree + ins->longitude = Pack_EKF[EKF_for_FC].alldata.gps_longi; + ins->altitude = Pack_EKF[EKF_for_FC].alldata.gps_high; + + ins->vel_north = Pack_EKF[EKF_for_FC].xEKF[3]; ///< Unit is meter per second, in NED axes + ins->vel_east = Pack_EKF[EKF_for_FC].xEKF[4]; + ins->vel_down = Pack_EKF[EKF_for_FC].xEKF[5]; + + ins->roll = Pack_EKF[EKF_for_FC].alldata.true_roll/ deg; ///< Unit is radian + ins->pitch = Pack_EKF[EKF_for_FC].alldata.true_pitch/ deg; + ins->yaw = Pack_EKF[EKF_for_FC].alldata.true_yaw / deg; + + ins->seq = g_ins_seq; + + /** 下面这几项待实现 */ + ins->status = 0xff; ///< working status + ///< bit0 - attitude valid + ///< bit1 - speed valid + ///< bit2 - position valid + ///< bit7 - calibration + + ins->integration_status = 0xff; ///< integration method + ///< 0 - pure Inertial mode + ///< bit0(1) - GPS assist + ///< bit1(2) - baro assist + ///< bit2(4) - radar assist + ins->BIT = 0; ///< health status 0-health; otherwise-unhealth. + + + ErrorCode[0] = 0; +#endif + } + break; + default: + break; + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_led_set_wrapper.c b/X7/hal_led_set_wrapper.c new file mode 100644 index 0000000..96ed669 --- /dev/null +++ b/X7/hal_led_set_wrapper.c @@ -0,0 +1,91 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + +#include "hal_api.h" + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +//#include "param_mgr.h" +extern void DrvNuaaHalLedConfigSet(int TailRed,int TailGreen ,int TailBlue); +#endif +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define u_width 1 +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int is_led_init[1] = 0; + +void led_set_init(uint8_t id) +{ + switch (id) + { + case 0u: + if (!is_led_init[0]) + { +#ifdef HAL_IMPL + //TODO + is_led_init[0] = 1; +#endif + } + break; + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_led_set_Start_wrapper(const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +led_set_init(id[0]); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_led_set_Outputs_wrapper(const HAL_led_color_t *color, + int32_T *ErrorCode, + const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +/* + * set LED group *id* with *color* + * + * ErrorCode - return zero on success, otherwise nonzero on failures. + */ + ErrorCode[0] = -1; + switch (id[0]) + { + case 0u: + if (is_led_init[0]) + { +#ifdef HAL_IMPL + // TODO set led color + DrvNuaaHalLedConfigSet(color->r, color->g, color->b); + + ErrorCode[0] = 0; +#endif + } + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_pressure_wrapper.c b/X7/hal_pressure_wrapper.c new file mode 100644 index 0000000..2ff4112 --- /dev/null +++ b/X7/hal_pressure_wrapper.c @@ -0,0 +1,135 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + +#include "hal_api.h" + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "spi_devs.h" +#endif +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int pres_inited[3]; + +void initialize_pressure(uint8_t id) +{ + switch (id) + { + case 0: + if (!pres_inited[0]) + { +#ifdef HAL_IMPL + // TODO initialize pressure No.0 + pres_inited[0] = 1; +#endif + } + break; + + case 1: + if (!pres_inited[1]) + { +#ifdef HAL_IMPL + // TODO initialize pressure No.0 + pres_inited[1] = 1; +#endif + } + break; + + case 2: + if (!pres_inited[2]) + { +#ifdef HAL_IMPL + // TODO initialize pressure No.0 + pres_inited[2] = 1; +#endif + } + break; + + default: + break; + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_pressure_Start_wrapper(const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +initialize_pressure(id[0]); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_pressure_Outputs_wrapper(HAL_pressure_SI_t *pres, + int32_T *ErrorCode, + const uint8_T *id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + switch (id[0]) + { + case 0: + if (pres_inited[0]) + { +#ifdef HAL_IMPL + // TODO read pressure + ErrorCode[0] = -1; +#endif + } + break; + + case 1: + if (pres_inited[id[0]]) + { +#ifdef HAL_IMPL + pres->pressure = baro_ms5611_1.pressure; + pres->temperature = baro_ms5611_1.temp*0.01f; + pres->seq = baro_ms5611_1.seq; + + ErrorCode[0] = 0; +#endif + } + break; + + case 2: + if (pres_inited[id[0]]) + { +#ifdef HAL_IMPL + pres->pressure = baro_ms5611_2.pressure; + pres->temperature = baro_ms5611_2.temp*0.01f; + pres->seq = baro_ms5611_2.seq; + + ErrorCode[0] = 0; +#endif + } + break; + + default: + break; + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_pwm_in_wrapper.c b/X7/hal_pwm_in_wrapper.c new file mode 100644 index 0000000..044a1f7 --- /dev/null +++ b/X7/hal_pwm_in_wrapper.c @@ -0,0 +1,77 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +//#include "param_mgr.h" +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define u_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int is_pwm_in_init = 0; + +void pwm_in_init() +{ + if (!is_pwm_in_init) + { +#ifdef HAL_IMPL + //TODO + is_pwm_in_init = 1; +#endif + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_pwm_in_Start_wrapper(void) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +pwm_in_init(); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_pwm_in_Outputs_wrapper(const uint16_T *start_idx, + uint16_T *pwm, + int32_T *ErrorCode, + const int_T y_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + if (is_pwm_in_init) + { +#ifdef HAL_IMPL + /* + * input *pwm* from pwm devices from *start_idx* to *start_idx*+y_width-1. + * + * ErrorCode - return zero on success, otherwise nonzero on failures. + */ + // TODO set pwm outputs + ErrorCode[0] = 0; +#endif + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_pwm_out_wrapper.c b/X7/hal_pwm_out_wrapper.c new file mode 100644 index 0000000..5757d77 --- /dev/null +++ b/X7/hal_pwm_out_wrapper.c @@ -0,0 +1,84 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "tim_devs.h" +#endif +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#endif + +int is_pwm_out_init = 0; + +void pwm_out_init() +{ + if (!is_pwm_out_init) + { +#ifdef HAL_IMPL + //TODO + is_pwm_out_init = 1; +#endif + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_pwm_out_Start_wrapper(void) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +pwm_out_init(); +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_pwm_out_Outputs_wrapper(const uint16_T *pwm, + const uint16_T *start_idx, + int32_T *ErrorCode, + const int_T u_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +ErrorCode[0] = -1; + if (is_pwm_out_init) + { +#ifdef HAL_IMPL + /* + * output *pwm* to pwm devices from *start_idx* to *start_idx*+u_width-1. + * + * ErrorCode - return zero on success, othedrwise nonzero on failures. + */ + for(uint8_T i = 0; i < u_width; ++i) + { + set_pwm(start_idx[0]+i, pwm[i]); + } + ErrorCode[0] = 0; +#endif + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_rec_wrapper.c b/X7/hal_rec_wrapper.c new file mode 100644 index 0000000..44a1448 --- /dev/null +++ b/X7/hal_rec_wrapper.c @@ -0,0 +1,77 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "param_pre.h" +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define u_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +static void pack(uint8_T *pkg, uint16_T *len, const int_T y_width, uint8_T *ts, uint8_T *src, uint16_T length) +{ + uint8_T sum; + int16_T i,j; + sum = 0; + for (i = 0; i < 4; ++i) + { + pkg[i] = ts[i]; + sum += ts[i]; + } + if (length > y_width - 5) + { + length = y_width - 5; + } + for (i = 0, j = 4; i < length; ++i,++j) + { + pkg[j] = src[i]; + sum += src[i]; + } + pkg[j] = sum; + len[0] = length+4; +} + +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Output function + * + */ +void hal_rec_Outputs_wrapper(const uint8_T *id, + const uint32_T *time_stamp, + uint8_T *pkg, + uint16_T *len, + const int_T y_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ + switch (id[0]) + { +#ifdef HAL_IMPL + case 1: + pack(pkg, len, y_width, (uint8_T *)time_stamp, (uint8_T *)&rec_hi, sizeof(rec_hi)); + break; +#endif + default: + pack(pkg, len, y_width, (uint8_T *)time_stamp, NULL, 0u); + } + +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_sbus_in_wrapper.c b/X7/hal_sbus_in_wrapper.c new file mode 100644 index 0000000..ec7fe64 --- /dev/null +++ b/X7/hal_sbus_in_wrapper.c @@ -0,0 +1,99 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + +#include "hal_api.h" + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "uart_fifos.h" +#endif +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +int is_sbus_in_init[1]; + +void sbus_input_init(uint8_t id) +{ + switch (id) + { + case 0: + if (!is_sbus_in_init[0]) + { +#ifdef HAL_IMPL + //TODO config channels, update rate, etc. and bind properties + is_sbus_in_init[0] = 1; +#endif + } + } +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_sbus_in_Start_wrapper(const uint8_T *id, const int_T p_width0) +{ + /* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ + sbus_input_init(id[0]); + /* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_sbus_in_Outputs_wrapper(HAL_sbus_in_t *sbus, + int32_T *ErrorCode, + const uint8_T *id, const int_T p_width0) +{ + /* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ + ErrorCode[0] = -1; + /* + * input *AI* from ADC channels from *start_idx* to *start_idx*+y_width-1. + * + * ErrorCode - return zero on success, otherwise nonzero on failures. + */ + switch (id[0]) + { + case 0u: + if (is_sbus_in_init[0]) + { +#ifdef HAL_IMPL + // TODO set sbus outputs + sbus_in.valid; + if(sbus_in.valid) + { + for(uint8_T i = 0; i < 18; i++) + { + sbus->channels[i] = sbus_in.ch[i]; + } + sbus->seq = sbus_in.seq; + sbus->valid = 1; + ErrorCode[0] = 0; + } + else + { + ErrorCode[0] = 1; + sbus->valid = 0; + } +#endif + } + break; + } + /* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} diff --git a/X7/hal_sen_read_wrapper.c b/X7/hal_sen_read_wrapper.c new file mode 100644 index 0000000..46593b9 --- /dev/null +++ b/X7/hal_sen_read_wrapper.c @@ -0,0 +1,275 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +//#include "param_mgr.h" +#endif +#include +#include +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +typedef enum PROP_TYPE +{ + PROP_ARRAY_UINT8 = 1, /* 8-bit unsigned integer | */ + PROP_ARRAY_INT8 = 2, /* 8-bit signed integer | */ + PROP_ARRAY_UINT16 = 3, /* 16-bit unsigned integer | */ + PROP_ARRAY_INT16 = 4, /* 16-bit signed integer | */ + PROP_ARRAY_UINT32 = 5, /* 32-bit unsigned integer | */ + PROP_ARRAY_INT32 = 6, /* 32-bit signed integer | */ + PROP_ARRAY_REAL32 = 7, /* 32-bit floating-point | */ + PROP_FUNC_UINT8 = 8, /* 8-bit unsigned integer | */ + PROP_FUNC_INT8 = 9, /* 8-bit signed integer | */ + PROP_FUNC_UINT16 = 10, /* 16-bit unsigned integer | */ + PROP_FUNC_INT16 = 11, /* 16-bit signed integer | */ + PROP_FUNC_UINT32 = 12, /* 32-bit unsigned integer | */ + PROP_FUNC_INT32 = 13, /* 32-bit signed integer | */ + PROP_FUNC_REAL32 = 14, /* 32-bit floating-point | */ +} PROP_TYPE; + +typedef int8_t (*get_prop_b_func_ptr)(int index, void *paramter); +typedef uint8_t (*get_prop_B_func_ptr)(int index, void *paramter); +typedef int16_t (*get_prop_h_func_ptr)(int index, void *paramter); +typedef uint16_t (*get_prop_H_func_ptr)(int index, void *paramter); +typedef int32_t (*get_prop_i_func_ptr)(int index, void *paramter); +typedef uint32_t (*get_prop_I_func_ptr)(int index, void *paramter); +typedef float (*get_prop_f_func_ptr)(int index, void *paramter); + +float RPM0[4]; + +float RPM1(int index, void *paramter) +{ + (void *)paramter; + + return RPM0[index]; +} +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_sen_read_Start_wrapper(void **pW, + const uint8_T *name, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +/* initialize code to bind property; */ + int *valid = (int *)&pW[0]; /* 0-invalid; 1-valid; */ + PROP_TYPE *type = (PROP_TYPE *)&pW[1]; + int *len = (int *)&pW[2]; /* parameter length; */ + void **val_ptr = &pW[3]; /* pointer to value array */ + void **parameter = &pW[4]; /* parameter for function */ + + /* set default pW values */ + *valid = 0; + *type = 0; + *val_ptr = NULL; + *parameter = NULL; + *len = 0; + if (strncmp(name, "RPM0", p_width0)) + { + *valid = 1; + *type = PROP_ARRAY_REAL32; + *len = 4; + *val_ptr = &RPM0; + } + else if (strncmp(name, "RPM1", p_width0)) + { + *valid = 1; + *type = PROP_FUNC_REAL32; + *len = 4; + *val_ptr = &RPM1; + } +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_sen_read_Outputs_wrapper(real32_T *value, + int32_T *ErrorCode, + void **pW, + const uint8_T *name, const int_T p_width0, + const int_T y_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +/* get binded property value; */ + ErrorCode[0] = -1; + + int *valid = (int *)&pW[0]; + if (*valid) + { + int *len = (int *)&pW[2]; + if (y_width > *len) + { + ErrorCode[0] = -2; + } + else + { + int i; + PROP_TYPE *type = (PROP_TYPE *)&pW[1]; + switch (*type) + { + case PROP_ARRAY_UINT8: + { + uint8_t *val = *(uint8_t **)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)val[i]; + } + ErrorCode[0] = 0; + } + break; + case PROP_ARRAY_INT8: + { + int8_t *val = *(int8_t **)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)val[i]; + } + ErrorCode[0] = 0; + } + break; + case PROP_ARRAY_UINT16: + { + uint16_t *val = *(uint16_t **)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)val[i]; + } + ErrorCode[0] = 0; + } + break; + case PROP_ARRAY_INT16: + { + int16_t *val = *(int16_t **)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)val[i]; + } + ErrorCode[0] = 0; + } + break; + case PROP_ARRAY_UINT32: + { + uint32_t *val = *(uint32_t **)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)val[i]; + } + ErrorCode[0] = 0; + } + break; + case PROP_ARRAY_INT32: + { + int32_t *val = *(int32_t **)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)val[i]; + } + ErrorCode[0] = 0; + } + break; + case PROP_ARRAY_REAL32: + { + float *val = *(float **)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = val[i]; + } + ErrorCode[0] = 0; + } + break; + case PROP_FUNC_UINT8: + { + get_prop_B_func_ptr func = *(get_prop_B_func_ptr *)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)(*func)(i, pW[4]); + } + ErrorCode[0] = 0; + } + break; + case PROP_FUNC_INT8: + { + get_prop_b_func_ptr func = *(get_prop_b_func_ptr *)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)(*func)(i, pW[4]); + } + ErrorCode[0] = 0; + } + break; + case PROP_FUNC_UINT16: + { + get_prop_H_func_ptr func = *(get_prop_H_func_ptr *)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)(*func)(i, pW[4]); + } + ErrorCode[0] = 0; + } + break; + case PROP_FUNC_INT16: + { + get_prop_h_func_ptr func = *(get_prop_h_func_ptr *)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)(*func)(i, pW[4]); + } + ErrorCode[0] = 0; + } + break; + case PROP_FUNC_UINT32: + { + get_prop_I_func_ptr func = *(get_prop_I_func_ptr *)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)(*func)(i, pW[4]); + } + ErrorCode[0] = 0; + } + break; + case PROP_FUNC_INT32: + { + get_prop_i_func_ptr func = *(get_prop_i_func_ptr *)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (float)(*func)(i, pW[4]); + } + ErrorCode[0] = 0; + } + break; + case PROP_FUNC_REAL32: + { + get_prop_f_func_ptr func = *(get_prop_f_func_ptr *)&pW[3]; + for (i = 0; i < y_width; ++i) + { + value[i] = (*func)(i, pW[4]); + } + ErrorCode[0] = 0; + } + break; + } + } + } +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_uart_in_wrapper.c b/X7/hal_uart_in_wrapper.c new file mode 100644 index 0000000..3c04f9c --- /dev/null +++ b/X7/hal_uart_in_wrapper.c @@ -0,0 +1,95 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "uart_fifos.h" +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ + +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_uart_in_Start_wrapper(const uint8_T *uart_id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL + // initialize_uart_device(uart_id[0]); +#endif +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_uart_in_Outputs_wrapper(uint8_T *Data, + uint16_T *Data_len, + int32_T *ErrorCode, + const uint8_T *uart_id, const int_T p_width0, + const int_T y_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +/* ********************************************************** + * collect all data from this uart and push to Data buffer; + * + * parameter: + * uart_id - uart FIFO device to read + * + * outputs: + * Data - read all FIFO data and write to Data array + * Data_len[0] - the number of written data + * ErrorCode[0] - return zero for success, others for failail. + * + * constant: + * y_width - maximum length to write to Data array + ************************************************************/ +Data_len[0] = 0u; +ErrorCode[0] = -1; + switch (uart_id[0]) + { + case 0: + Data_len[0] = UART_RX_FIFO_read(&uart2_TELEM1_rx, Data, y_width); + + ErrorCode[0] = 0; + break; + case 1: + Data_len[0] = UART_RX_FIFO_read(&uart6_TELEM2_rx, Data, y_width); + + ErrorCode[0] = 0; + break; + case 2: + Data_len[0] = UART_RX_FIFO_read(&uart3_rx, Data, y_width); + + ErrorCode[0] = 0; + break; + } +#else +/* dummy function for generate mex file for sim*/ +Data_len[0] = 0u; +ErrorCode[0] = 0; +#endif +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + + diff --git a/X7/hal_uart_out_wrapper.c b/X7/hal_uart_out_wrapper.c new file mode 100644 index 0000000..d0dcf4a --- /dev/null +++ b/X7/hal_uart_out_wrapper.c @@ -0,0 +1,137 @@ + +/* + * Include Files + * + */ +#if defined(MATLAB_MEX_FILE) +#include "tmwtypes.h" +#include "simstruc_types.h" +#else +#include "rtwtypes.h" +#endif + + + +/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +#include "uart_fifos.h" +#endif +/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */ +#define y_width 1 + +/* + * Create external references here. + * + */ +/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */ +#define REC_ZONE_LEN (2000) +extern uint8_t rec_zone[2][REC_ZONE_LEN]; +extern int rec_sel; +extern int rec_head[2]; + +static int FAT_REC_write(uint8_t *buff, const uint16_t len) +{ + if (rec_head[rec_sel] < REC_ZONE_LEN) + { + for (int i=0u;i= REC_ZONE_LEN) + { + break; + } + } + } + return 0; +} + + +/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */ + +/* + * Start function + * + */ +void hal_uart_out_Start_wrapper(const uint8_T *uart_id, const int_T p_width0) +{ +/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL + //initialize_uart_device(uart_id[0]); +#endif +/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */ +} +/* + * Output function + * + */ +void hal_uart_out_Outputs_wrapper(uint8_T *buff, + const uint16_T *len, + const uint8_T *sep, + int32_T *ErrorCode, + const uint8_T *uart_id, const int_T p_width0, + const int_T u_width) +{ +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */ +#ifdef HAL_IMPL +/* *********************************************** + * push all data to this uart output queue; + * + * inputs: + * buff - data array to be sent + * len[0] - available length in the data array + * sep[0] - it means the data array updated when sep[0] changed, + * uart_id[0] - uart FIFO device which is required to send data + * + * output: + * ErrorCode[0] - return zero for success, others for failail. + * + * constant: + * u_width - the maximum size of buff, do not read buff out of range. + *************************************************/ +ErrorCode[0] = -1; +static uint8_T seq_last[16]; + switch (uart_id[0]) + { + case 0: + // TODO write all data to fifo + if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart2_TELEM1_tx, (uint8_t *)buff, len[0])) + { + seq_last[uart_id[0]] = sep[0]; + ErrorCode[0] = 0; + } + break; + case 1: + // TODO write all data to fifo + if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart6_TELEM2_tx, (uint8_t *)buff, len[0])) + { + seq_last[uart_id[0]] = sep[0]; + ErrorCode[0] = 0; + } + break; + case 2: + // TODO write all data to fifo + if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart3_tx, (uint8_t *)buff, len[0])) + { + seq_last[uart_id[0]] = sep[0]; + ErrorCode[0] = 0; + } + break; + case 4: + // TODO write all data to fifo + if (sep[0] != seq_last[uart_id[0]] && !FAT_REC_write((uint8_t *)buff, len[0])) + { + seq_last[uart_id[0]] = sep[0]; + ErrorCode[0] = 0; + } + break; + + + } +#else +/* dummy function for generate mex file for sim*/ +ErrorCode[0] = 0; +#endif +/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */ +} + +