/* Function Definitions */
static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
{
real32_T y;
int32_T b_u0;
int32_T b_u1;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else if (rtIsInfF(u0) && rtIsInfF(u1)) {
if (u0 > 0.0F) {
b_u0 = 1;
} else {
b_u0 = -1;
}
if (u1 > 0.0F) {
b_u1 = 1;
} else {
b_u1 = -1;
}
y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
} else if (u1 == 0.0F) {
if (u0 > 0.0F) {
y = RT_PIF / 2.0F;
} else if (u0 < 0.0F) {
y = -(RT_PIF / 2.0F);
} else {
y = 0.0F;
}
} else {
y = (real32_T)atan2(u0, u1);
}
return y;
}
/*
* Arguments : float u0
* float u1
* Return Type : float
*/
static float rt_atan2f_snf(float u0, float u1)
{
float y;
int b_u0;
int b_u1;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else if (rtIsInfF(u0) && rtIsInfF(u1)) {
if (u0 > 0.0F) {
b_u0 = 1;
} else {
b_u0 = -1;
}
if (u1 > 0.0F) {
b_u1 = 1;
} else {
b_u1 = -1;
}
y = (real32_T)atan2((float)b_u0, (float)b_u1);
} else if (u1 == 0.0F) {
if (u0 > 0.0F) {
y = RT_PIF / 2.0F;
} else if (u0 < 0.0F) {
y = -(float)(RT_PIF / 2.0F);
} else {
y = 0.0F;
}
} else {
y = (real32_T)atan2(u0, u1);
}
return y;
}
/* Function Definitions */
static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
{
real32_T y;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else if (rtIsInfF(u0) && rtIsInfF(u1)) {
y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F);
} else if (u1 == 0.0F) {
if (u0 > 0.0F) {
y = RT_PIF / 2.0F;
} else if (u0 < 0.0F) {
y = -(RT_PIF / 2.0F);
} else {
y = 0.0F;
}
} else {
y = (real32_T)atan2f(u0, u1);
}
return y;
}
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
/* Output and update for atomic system: '<Root>/Sensor_Data_Adapter' */
void AUAV_V_Sensor_Data_Adapter(void)
{
/* local block i/o variables */
real_T rtb_DataTypeConversion_n;
real_T rtb_DiscreteZeroPole_i;
boolean_T rtb_LogicalOperator_df;
int16_T rtb_Switch_d[13];
real32_T rtb_Sum_o;
uint8_T rtb_Compare_gl;
real32_T rtb_u001maxDynPress;
real32_T rtb_Sum_d;
int16_T rtb_DataTypeConversion1_ax;
int16_T rtb_DataTypeConversion2_c;
int16_T i;
real_T tmp;
real_T tmp_0;
/* Outputs for Enabled SubSystem: '<S16>/Raw HIL Readings' incorporates:
* EnablePort: '<S580>/Enable'
*/
if (AUAV_V3_TestSensors_B.DataStoreRead > 0.0) {
/* Outputs for Enabled SubSystem: '<S580>/Enabled Subsystem' incorporates:
* EnablePort: '<S582>/Enable'
*/
/* DataStoreRead: '<S580>/Data Store Read' */
if (AUAV_V3_TestSensors_DWork.SIX_DOF_HIL_FLAG > 0.0) {
/* S-Function (MCHP_C_function_Call): '<S582>/Data from HIL [hil.c]2' */
hilRead(
&AUAV_V3_TestSensors_B.DatafromHILhilc2[0]
);
/* S-Function (MCHP_C_function_Call): '<S582>/HIL Messages Parser//Decoder [hil.c]1' */
protDecodeHil(
&AUAV_V3_TestSensors_B.DatafromHILhilc2[0]
);
/* S-Function (MCHP_C_function_Call): '<S582>/HIL Raw Readings [hil.c]1' */
hil_getRawRead(
&AUAV_V3_TestSensors_B.HILRawReadingshilc1_k[0]
);
}
/* End of DataStoreRead: '<S580>/Data Store Read' */
/* End of Outputs for SubSystem: '<S580>/Enabled Subsystem' */
/* Outputs for Enabled SubSystem: '<S580>/Enabled Subsystem1' incorporates:
* EnablePort: '<S583>/Enable'
*/
/* DataStoreRead: '<S580>/Data Store Read1' */
if (AUAV_V3_TestSensors_DWork.X_PLANE_HIL_FLAG > 0.0) {
/* S-Function (MCHP_C_function_Call): '<S583>/Data from HIL X-Plane' */
HILSIM_set_gplane(
);
/* S-Function (MCHP_C_function_Call): '<S583>/Data from HIL X-Plane2' */
HILSIM_set_omegagyro(
);
/* S-Function (MCHP_C_function_Call): '<S583>/HIL Raw Readings [hil.c]1' */
hil_getRawRead(
&AUAV_V3_TestSensors_B.HILRawReadingshilc1[0]
);
/* MATLAB Function: '<S583>/Embedded MATLAB Function1' incorporates:
* Constant: '<S583>/Constant1'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_m);
/* DataTypeConversion: '<S583>/Data Type Conversion1' */
tmp_0 = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_m.y);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 65536.0);
}
/* MATLAB Function: '<S583>/Embedded MATLAB Function2' incorporates:
* Constant: '<S583>/Constant2'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_i);
/* DataTypeConversion: '<S583>/Data Type Conversion' incorporates:
* DataStoreRead: '<S583>/Get mlAirData1'
*/
rtb_Sum_o = (real32_T)floor(mlAirData.press_abs);
if (rtIsNaNF(rtb_Sum_o) || rtIsInfF(rtb_Sum_o)) {
rtb_Sum_o = 0.0F;
} else {
rtb_Sum_o = (real32_T)fmod(rtb_Sum_o, 65536.0F);
}
/* DataTypeConversion: '<S583>/Data Type Conversion2' */
tmp = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_i.y);
if (rtIsNaN(tmp) || rtIsInf(tmp)) {
tmp = 0.0;
} else {
tmp = fmod(tmp, 65536.0);
}
/* MATLAB Function: '<S583>/myMux Fun' incorporates:
* DataStoreRead: '<S583>/Get mlAirData2'
* DataTypeConversion: '<S583>/Data Type Conversion'
* DataTypeConversion: '<S583>/Data Type Conversion1'
* DataTypeConversion: '<S583>/Data Type Conversion2'
*/
AUAV_V3_TestSenso_myMuxFun(rtb_Sum_o < 0.0F ? -(int16_T)(uint16_T)
-rtb_Sum_o : (int16_T)(uint16_T)rtb_Sum_o, tmp_0 < 0.0 ? -(int16_T)
(uint16_T)-tmp_0 : (int16_T)(uint16_T)tmp_0, tmp < 0.0 ? -(int16_T)
(uint16_T)-tmp : (int16_T)(uint16_T)tmp, mlAirData.temperature,
&AUAV_V3_TestSensors_B.sf_myMuxFun_h);
/* S-Function (MCHP_C_function_Call): '<S583>/Update State AP ADC Data [updateSensorMcuState.c]1' */
updateRawADCData(
&AUAV_V3_TestSensors_B.sf_myMuxFun_h.y[0]
);
}
/* End of Outputs for SubSystem: '<S580>/Enabled Subsystem1' */
/* Switch: '<S580>/Switch' incorporates:
* DataStoreRead: '<S580>/Data Store Read1'
*/
for (i = 0; i < 13; i++) {
if (AUAV_V3_TestSensors_DWork.X_PLANE_HIL_FLAG != 0.0) {
AUAV_V3_TestSensors_B.Switch_i[i] =
AUAV_V3_TestSensors_B.HILRawReadingshilc1[i];
} else {
AUAV_V3_TestSensors_B.Switch_i[i] =
AUAV_V3_TestSensors_B.HILRawReadingshilc1_k[i];
}
}
/* End of Switch: '<S580>/Switch' */
}
/* End of Outputs for SubSystem: '<S16>/Raw HIL Readings' */
/* Logic: '<S581>/Logical Operator' */
rtb_LogicalOperator_df = !(AUAV_V3_TestSensors_B.DataStoreRead != 0.0);
/* Outputs for Enabled SubSystem: '<S581>/If no HIL then Read all the Sensors' incorporates:
* EnablePort: '<S591>/Enable'
*/
if (rtb_LogicalOperator_df) {
/* DataTypeConversion: '<S591>/Data Type Conversion' incorporates:
* DataStoreRead: '<S591>/Get mlAirData1'
*/
rtb_Sum_o = (real32_T)floor(mlAirData.press_abs);
if (rtIsNaNF(rtb_Sum_o) || rtIsInfF(rtb_Sum_o)) {
rtb_Sum_o = 0.0F;
} else {
rtb_Sum_o = (real32_T)fmod(rtb_Sum_o, 65536.0F);
}
i = rtb_Sum_o < 0.0F ? -(int16_T)(uint16_T)-rtb_Sum_o : (int16_T)(uint16_T)
rtb_Sum_o;
/* End of DataTypeConversion: '<S591>/Data Type Conversion' */
/* MATLAB Function: '<S591>/Embedded MATLAB Function1' incorporates:
* Constant: '<S591>/Constant1'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_ib);
/* DataTypeConversion: '<S591>/Data Type Conversion1' */
tmp_0 = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_ib.y);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 65536.0);
}
rtb_DataTypeConversion1_ax = tmp_0 < 0.0 ? -(int16_T)(uint16_T)-tmp_0 :
(int16_T)(uint16_T)tmp_0;
/* End of DataTypeConversion: '<S591>/Data Type Conversion1' */
/* MATLAB Function: '<S591>/Embedded MATLAB Function2' incorporates:
* Constant: '<S591>/Constant2'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_p);
/* DataTypeConversion: '<S591>/Data Type Conversion2' */
tmp_0 = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_p.y);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 65536.0);
}
rtb_DataTypeConversion2_c = tmp_0 < 0.0 ? -(int16_T)(uint16_T)-tmp_0 :
(int16_T)(uint16_T)tmp_0;
/* End of DataTypeConversion: '<S591>/Data Type Conversion2' */
/* MATLAB Function: '<S591>/myMux Fun' incorporates:
* DataStoreRead: '<S591>/Get mlAirData2'
*/
AUAV_V3_TestSenso_myMuxFun(i, rtb_DataTypeConversion1_ax,
rtb_DataTypeConversion2_c, mlAirData.temperature,
&AUAV_V3_TestSensors_B.sf_myMuxFun_n);
/* S-Function (MCHP_C_function_Call): '<S591>/Update State AP ADC Data [updateSensorMcuState.c]1' */
updateRawADCData(
&AUAV_V3_TestSensors_B.sf_myMuxFun_n.y[0]
);
/* S-Function (MCHP_C_function_Call): '<S591>/Read the Cube Data [adisCube16405.c]1' */
getCubeData(
&AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[0]
);
/* MATLAB Function: '<S591>/myMux Fun4' incorporates:
* DataStoreRead: '<S591>/Get mlAirData2'
*/
/* MATLAB Function 'Sensor_Data_Adapter/Sensor Suite/If no HIL then Read all the Sensors/myMux Fun4': '<S621>:1' */
/* This block supports an embeddable subset of the MATLAB language. */
/* See the help menu for details. */
/* '<S621>:1:5' */
AUAV_V3_TestSensors_B.y_l[0] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[0];
AUAV_V3_TestSensors_B.y_l[1] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[1];
AUAV_V3_TestSensors_B.y_l[2] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[2];
AUAV_V3_TestSensors_B.y_l[3] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[3];
AUAV_V3_TestSensors_B.y_l[4] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[4];
AUAV_V3_TestSensors_B.y_l[5] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[5];
AUAV_V3_TestSensors_B.y_l[6] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[6];
AUAV_V3_TestSensors_B.y_l[7] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[7];
AUAV_V3_TestSensors_B.y_l[8] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[8];
AUAV_V3_TestSensors_B.y_l[9] = i;
AUAV_V3_TestSensors_B.y_l[10] = rtb_DataTypeConversion1_ax;
AUAV_V3_TestSensors_B.y_l[11] = rtb_DataTypeConversion2_c;
AUAV_V3_TestSensors_B.y_l[12] = mlAirData.temperature;
/* S-Function (MCHP_C_function_Call): '<S591>/Is the GPS Novatel or Ublox? [gpsPort.c]1' */
AUAV_V3_TestSensors_B.IstheGPSNovatelorUbloxgpsPortc1 = isGPSNovatel(
);
/* Outputs for Enabled SubSystem: '<S591>/if GPS is Ublox' incorporates:
* EnablePort: '<S619>/Enable'
*/
/* Logic: '<S591>/Logical Operator' */
if (!(AUAV_V3_TestSensors_B.IstheGPSNovatelorUbloxgpsPortc1 != 0)) {
/* S-Function (MCHP_C_function_Call): '<S619>/Produce the GPS Main Data and update the AP State (lat lon hei cog sog) [gpsUblox.c]1' */
getGpsUbloxMainData(
&AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdat_a[
0]
);
}
/* End of Logic: '<S591>/Logical Operator' */
/* End of Outputs for SubSystem: '<S591>/if GPS is Ublox' */
}
/* End of Outputs for SubSystem: '<S581>/If no HIL then Read all the Sensors' */
/* Switch: '<S581>/Switch' */
for (i = 0; i < 13; i++) {
if (AUAV_V3_TestSensors_B.DataStoreRead != 0.0) {
rtb_Switch_d[i] = AUAV_V3_TestSensors_B.Switch_i[i];
} else {
rtb_Switch_d[i] = AUAV_V3_TestSensors_B.y_l[i];
}
}
/* End of Switch: '<S581>/Switch' */
/* MATLAB Function: '<S629>/Embedded MATLAB Function' incorporates:
* Constant: '<S629>/Constant'
* Constant: '<S629>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion5'
*/
A_EmbeddedMATLABFunction_f((real_T)rtb_Switch_d[12], 0.01, 0.02,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_g,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_g);
/* Sum: '<S628>/Sum' incorporates:
* Constant: '<S628>/Bias'
* Product: '<S628>/Divide'
*/
rtb_Sum_o = (real32_T)(1.5112853050231934 *
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_g.y) + -1605.28198F;
/* RelationalOperator: '<S641>/Compare' incorporates:
* Constant: '<S641>/Constant'
*/
rtb_Compare_gl = (uint8_T)(rtb_Sum_o < -130.0F);
/* Outputs for Enabled SubSystem: '<S626>/Hi Temp Compensation2' incorporates:
* EnablePort: '<S642>/Enable'
*/
/* Logic: '<S626>/Logical Operator' */
if (!(rtb_Compare_gl != 0)) {
/* Sum: '<S642>/Sum2' incorporates:
* Constant: '<S642>/Mean Temperature for Calibration'
* Constant: '<S642>/gains'
* Product: '<S642>/Divide1'
* Sum: '<S642>/Sum1'
*/
AUAV_V3_TestSensors_B.Merge = (real32_T)rtb_Switch_d[10] - (rtb_Sum_o -
293.053F) * -0.0950433F;
}
/* End of Logic: '<S626>/Logical Operator' */
/* End of Outputs for SubSystem: '<S626>/Hi Temp Compensation2' */
/* Outputs for Enabled SubSystem: '<S626>/Lo Temp Compensation' incorporates:
* EnablePort: '<S643>/Enable'
*/
if (rtb_Compare_gl > 0) {
/* Sum: '<S643>/Add' incorporates:
* Constant: '<S643>/Constant'
* Constant: '<S643>/Mean Temperature for Calibration'
* Constant: '<S643>/gains'
* Product: '<S643>/Divide1'
* Sum: '<S643>/Sum1'
* Sum: '<S643>/Sum2'
*/
AUAV_V3_TestSensors_B.Merge = ((real32_T)rtb_Switch_d[10] - (rtb_Sum_o -
-202.93F) * -0.0552923F) + -41.0F;
}
/* End of Outputs for SubSystem: '<S626>/Lo Temp Compensation' */
/* MATLAB Function: '<S631>/Embedded MATLAB Function' incorporates:
* Constant: '<S631>/Constant'
* Constant: '<S631>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion3'
*/
A_EmbeddedMATLABFunction_f((real_T)AUAV_V3_TestSensors_B.Merge, 0.01, 4.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_lw,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_lw);
/* Sum: '<S624>/Sum' incorporates:
* Constant: '<S624>/Bias'
* Constant: '<S624>/Gains'
* DataTypeConversion: '<S593>/Data Type Conversion4'
* Product: '<S624>/Divide'
*/
rtb_u001maxDynPress = 1.05137849F * (real32_T)
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_lw.y + -1005.87872F;
/* Saturate: '<S593>/[0.001 maxDynPress]' */
if (rtb_u001maxDynPress > 3000.0F) {
rtb_u001maxDynPress = 3000.0F;
} else {
if (rtb_u001maxDynPress < 0.001F) {
rtb_u001maxDynPress = 0.001F;
}
}
/* End of Saturate: '<S593>/[0.001 maxDynPress]' */
/* RelationalOperator: '<S644>/Compare' incorporates:
* Constant: '<S644>/Constant'
*/
rtb_Compare_gl = (uint8_T)(rtb_Sum_o < -50.0F);
/* Outputs for Enabled SubSystem: '<S627>/Hi Temp Compensation' incorporates:
* EnablePort: '<S645>/Enable'
*/
/* Logic: '<S627>/Logical Operator' */
if (!(rtb_Compare_gl != 0)) {
/* Sum: '<S645>/Add' incorporates:
* Constant: '<S645>/Constant'
* Constant: '<S645>/Mean Temperature for Calibration'
* Constant: '<S645>/gains'
* Product: '<S645>/Divide1'
* Sum: '<S645>/Sum1'
* Sum: '<S645>/Sum2'
*/
AUAV_V3_TestSensors_B.Merge_j = ((real32_T)rtb_Switch_d[9] - (rtb_Sum_o -
347.23F) * 0.0207608F) + -6.0F;
}
/* End of Logic: '<S627>/Logical Operator' */
/* End of Outputs for SubSystem: '<S627>/Hi Temp Compensation' */
/* Outputs for Enabled SubSystem: '<S627>/Lo Temp Compensation' incorporates:
* EnablePort: '<S646>/Enable'
*/
if (rtb_Compare_gl > 0) {
/* Sum: '<S646>/Sum2' incorporates:
* Constant: '<S646>/Mean Temperature for Calibration'
* Constant: '<S646>/gains'
* Product: '<S646>/Divide1'
* Sum: '<S646>/Sum1'
*/
AUAV_V3_TestSensors_B.Merge_j = (real32_T)rtb_Switch_d[9] - (rtb_Sum_o -
-161.3F) * -0.0102663F;
}
/* End of Outputs for SubSystem: '<S627>/Lo Temp Compensation' */
/* MATLAB Function: '<S632>/Embedded MATLAB Function' incorporates:
* Constant: '<S632>/Constant'
* Constant: '<S632>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion19'
*/
A_EmbeddedMATLABFunction_f((real_T)AUAV_V3_TestSensors_B.Merge_j, 0.01, 4.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_cn,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_cn);
/* Sum: '<S623>/Sum' incorporates:
* Constant: '<S623>/Bias'
* Constant: '<S623>/Gains'
* DataTypeConversion: '<S593>/Data Type Conversion1'
* Product: '<S623>/Divide'
*/
rtb_Sum_d = 27.127F * (real32_T)
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_cn.y + 9444.44434F;
/* MATLAB Function: '<S581>/myMux Fun' */
AUAV_V3_TestSe_myMuxFun1_e(rtb_u001maxDynPress, rtb_Sum_d, rtb_Sum_o,
&AUAV_V3_TestSensors_B.sf_myMuxFun);
/* Outputs for Enabled SubSystem: '<S581>/If no HIL then update Air Data' incorporates:
* EnablePort: '<S592>/Enable'
*/
/* Inport: '<S592>/AirData' */
AUAV_V3_TestSensors_B.AirData[0] = AUAV_V3_TestSensors_B.sf_myMuxFun.y[0];
AUAV_V3_TestSensors_B.AirData[1] = AUAV_V3_TestSensors_B.sf_myMuxFun.y[1];
AUAV_V3_TestSensors_B.AirData[2] = AUAV_V3_TestSensors_B.sf_myMuxFun.y[2];
/* S-Function (MCHP_C_function_Call): '<S592>/Update the Air Calibrated Data [updateSensorMcuState.c]1' */
updateAirData(
&AUAV_V3_TestSensors_B.AirData[0]
);
/* End of Outputs for SubSystem: '<S581>/If no HIL then update Air Data' */
/* MATLAB Function: '<S581>/myMux Fun1' */
/* MATLAB Function 'Sensor_Data_Adapter/Sensor Suite/myMux Fun1': '<S595>:1' */
/* This block supports an embeddable subset of the MATLAB language. */
/* See the help menu for details. */
/* '<S595>:1:5' */
AUAV_V3_TestSensors_B.y_o[0] = rtb_u001maxDynPress;
AUAV_V3_TestSensors_B.y_o[1] = AUAV_V3_TestSensors_B.AirData[0];
AUAV_V3_TestSensors_B.y_o[2] = 0.0F;
AUAV_V3_TestSensors_B.y_o[3] = 0.0F;
/* S-Function (MCHP_C_function_Call): '<S581>/Sensor DSC Diag [updateSensorMcuState.c]1' */
updateSensorDiag(
&AUAV_V3_TestSensors_B.y_o[0]
);
/* S-Function "MCHP_MCU_LOAD" Block: <S590>/Calculus Time Step1 */
AUAV_V3_TestSensors_B.CalculusTimeStep1 = MCHP_MCULoadResult[0];
/* S-Function "MCHP_MCU_OVERLOAD" Block: <S590>/Calculus Time Step2 */
{
uint16_T register tmp = MCHP_MCU_Overload.val;
MCHP_MCU_Overload.val ^= tmp; /* Multi Tasking: potential simultaneous access ==> using xor to protect from potential miss */
AUAV_V3_TestSensors_B.CalculusTimeStep2 = tmp;
}
/* DataTypeConversion: '<S590>/Data Type Conversion12' incorporates:
* DataTypeConversion: '<S590>/Data Type Conversion1'
* DataTypeConversion: '<S590>/Data Type Conversion2'
* Gain: '<S590>/Gain'
* Product: '<S590>/Divide'
* Rounding: '<S590>/Rounding Function'
*/
tmp_0 = floor((real_T)AUAV_V3_TestSensors_B.CalculusTimeStep1 / (real_T)
AUAV_V3_TestSensors_B.CalculusTimeStep2 * 100.0);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 256.0);
}
AUAV_V3_TestSensors_B.DataTypeConversion12 = (uint8_T)(tmp_0 < 0.0 ? (int16_T)
(uint8_T)-(int8_T)(uint8_T)-tmp_0 : (int16_T)(uint8_T)tmp_0);
/* End of DataTypeConversion: '<S590>/Data Type Conversion12' */
/* MATLAB Function: '<S630>/Embedded MATLAB Function' incorporates:
* Constant: '<S630>/Constant'
* Constant: '<S630>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion6'
*/
A_EmbeddedMATLABFunction_f((real_T)rtb_Switch_d[11], 0.01, 0.02,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_np,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_np);
/* DataTypeConversion: '<S593>/Data Type Conversion8' incorporates:
* Constant: '<S625>/Bias'
* Product: '<S625>/Divide'
* Sum: '<S625>/Sum'
*/
rtb_Sum_o = (real32_T)floor((real32_T)(3.1760616302490234 *
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_np.y) + 911.698242F);
if (rtIsNaNF(rtb_Sum_o) || rtIsInfF(rtb_Sum_o)) {
rtb_Sum_o = 0.0F;
} else {
rtb_Sum_o = (real32_T)fmod(rtb_Sum_o, 65536.0F);
}
AUAV_V3_TestSensors_B.DataTypeConversion8 = rtb_Sum_o < 0.0F ? (uint16_T)
-(int16_T)(uint16_T)-rtb_Sum_o : (uint16_T)rtb_Sum_o;
/* End of DataTypeConversion: '<S593>/Data Type Conversion8' */
/* S-Function (MCHP_C_function_Call): '<S581>/Update the Load and Power Data [updateSensorMcuState.c]1' */
updateLoadData(
AUAV_V3_TestSensors_B.DataTypeConversion12
, AUAV_V3_TestSensors_B.DataTypeConversion8
);
/* S-Function (MCHP_C_function_Call): '<S588>/Produce the GPS Main Data and update the AP State (lat lon hei cog sog) [gpsUblox.c]1' */
getGpsUbloxData(
&AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdatet[0]
);
/* MATLAB Function: '<S596>/Embedded MATLAB Function' incorporates:
* Constant: '<S589>/Gyro Gains'
* Constant: '<S596>/Constant'
* Constant: '<S596>/Constant1'
* DataTypeConversion: '<S589>/Data Type Conversion2'
* Gain: '<S599>/[ -1 1 -1]'
* Product: '<S589>/Divide'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[0] *
0.000266462477F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_f,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_f);
/* MATLAB Function: '<S596>/Embedded MATLAB Function1' incorporates:
* Constant: '<S589>/Gyro Gains'
* Constant: '<S596>/Constant2'
* Constant: '<S596>/Constant3'
* DataTypeConversion: '<S589>/Data Type Conversion2'
* Product: '<S589>/Divide'
*/
A_EmbeddedMATLABFunction_f((real_T)((real32_T)rtb_Switch_d[1] *
0.000266462477F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_i,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction1_i);
/* MATLAB Function: '<S596>/Embedded MATLAB Function2' incorporates:
* Constant: '<S589>/Gyro Gains'
* Constant: '<S596>/Constant4'
* Constant: '<S596>/Constant5'
* DataTypeConversion: '<S589>/Data Type Conversion2'
* Gain: '<S599>/[ -1 1 -1]'
* Product: '<S589>/Divide'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[2] *
0.000266462477F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_h,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction2_h);
/* MATLAB Function: '<S596>/myMux Fun' */
AUAV_V3_TestSen_myMuxFun_i(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_f.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_i.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_h.y,
&AUAV_V3_TestSensors_B.sf_myMuxFun_i);
/* DataTypeConversion: '<S589>/Data Type Conversion7' */
AUAV_V3_TestSensors_B.DataTypeConversion7[0] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_i.y[0];
AUAV_V3_TestSensors_B.DataTypeConversion7[1] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_i.y[1];
AUAV_V3_TestSensors_B.DataTypeConversion7[2] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_i.y[2];
/* MATLAB Function: '<S597>/Embedded MATLAB Function' incorporates:
* Constant: '<S589>/Gyro Gains1'
* Constant: '<S597>/Constant'
* Constant: '<S597>/Constant1'
* DataTypeConversion: '<S589>/Data Type Conversion1'
* Gain: '<S600>/[ -1 1 -1]'
* Product: '<S589>/Divide1'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[3] *
0.000599060033F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_kq,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_kq);
/* MATLAB Function: '<S597>/Embedded MATLAB Function1' incorporates:
* Constant: '<S589>/Gyro Gains1'
* Constant: '<S597>/Constant2'
* Constant: '<S597>/Constant3'
* DataTypeConversion: '<S589>/Data Type Conversion1'
* Product: '<S589>/Divide1'
*/
A_EmbeddedMATLABFunction_f((real_T)((real32_T)rtb_Switch_d[4] *
0.000599060033F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_f,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction1_f);
/* MATLAB Function: '<S597>/Embedded MATLAB Function2' incorporates:
* Constant: '<S589>/Gyro Gains1'
* Constant: '<S597>/Constant4'
* Constant: '<S597>/Constant5'
* DataTypeConversion: '<S589>/Data Type Conversion1'
* Gain: '<S600>/[ -1 1 -1]'
* Product: '<S589>/Divide1'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[5] *
0.000599060033F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_f,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction2_f);
/* MATLAB Function: '<S597>/myMux Fun' */
AUAV_V3_TestSen_myMuxFun_i
(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_kq.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_f.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_f.y,
&AUAV_V3_TestSensors_B.sf_myMuxFun_m);
/* DataTypeConversion: '<S589>/Data Type Conversion3' */
AUAV_V3_TestSensors_B.DataTypeConversion3[0] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_m.y[0];
AUAV_V3_TestSensors_B.DataTypeConversion3[1] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_m.y[1];
AUAV_V3_TestSensors_B.DataTypeConversion3[2] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_m.y[2];
/* MATLAB Function: '<S598>/Embedded MATLAB Function' incorporates:
* Constant: '<S589>/Gyro Gains2'
* Constant: '<S598>/Constant'
* Constant: '<S598>/Constant1'
* DataTypeConversion: '<S589>/Data Type Conversion5'
* Product: '<S589>/Divide2'
*/
A_EmbeddedMATLABFunction_f((real_T)((real32_T)rtb_Switch_d[6] * 0.8F), 0.01,
40.0, &AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_ft,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_ft);
/* MATLAB Function: '<S598>/Embedded MATLAB Function1' incorporates:
* Constant: '<S589>/Gyro Gains2'
* Constant: '<S598>/Constant2'
* Constant: '<S598>/Constant3'
* DataTypeConversion: '<S589>/Data Type Conversion5'
* Gain: '<S601>/[ 1 -1 -1]'
* Product: '<S589>/Divide2'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[8] * 0.8F), 0.01,
40.0, &AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_in,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction1_in);
/* MATLAB Function: '<S598>/Embedded MATLAB Function2' incorporates:
* Constant: '<S589>/Gyro Gains2'
* Constant: '<S598>/Constant4'
* Constant: '<S598>/Constant5'
* DataTypeConversion: '<S589>/Data Type Conversion5'
* Gain: '<S601>/[ 1 -1 -1]'
* Product: '<S589>/Divide2'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[7] * 0.8F), 0.01,
40.0, &AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_fu,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction2_fu);
/* MATLAB Function: '<S598>/myMux Fun' */
AUAV_V3_TestSen_myMuxFun_i
(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_ft.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_in.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_fu.y,
&AUAV_V3_TestSensors_B.sf_myMuxFun_a);
/* DataTypeConversion: '<S589>/Data Type Conversion6' */
AUAV_V3_TestSensors_B.DataTypeConversion6[0] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_a.y[0];
AUAV_V3_TestSensors_B.DataTypeConversion6[1] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_a.y[1];
AUAV_V3_TestSensors_B.DataTypeConversion6[2] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_a.y[2];
/* MATLAB Function: '<S622>/Enables//Disables the Computation of initial Baro Bias' */
EnablesDisablestheComputat
(&AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b,
&AUAV_V3_TestSensors_DWork.sf_EnablesDisablestheComputat_b);
/* Outputs for Enabled SubSystem: '<S622>/Initial Baro Bias' incorporates:
* EnablePort: '<S637>/Enable'
*/
if (AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b.tOut > 0.0) {
/* DataTypeConversion: '<S637>/Data Type Conversion' */
rtb_DataTypeConversion_n = rtb_Sum_d;
/* DiscreteZeroPole: '<S640>/Discrete Zero-Pole' */
{
rtb_DiscreteZeroPole_i = 0.014778325123152709*rtb_DataTypeConversion_n;
rtb_DiscreteZeroPole_i += 0.029119852459414206*
AUAV_V3_TestSensors_DWork.DiscreteZeroPole_DSTATE;
}
/* Saturate: '<S637>/[80k - 120k]' incorporates:
* DataTypeConversion: '<S637>/Data Type Conversion1'
*/
if ((real32_T)rtb_DiscreteZeroPole_i > 120000.0F) {
AUAV_V3_TestSensors_B.u0k120k = 120000.0F;
} else if ((real32_T)rtb_DiscreteZeroPole_i < 80000.0F) {
AUAV_V3_TestSensors_B.u0k120k = 80000.0F;
} else {
AUAV_V3_TestSensors_B.u0k120k = (real32_T)rtb_DiscreteZeroPole_i;
}
/* End of Saturate: '<S637>/[80k - 120k]' */
/* Update for DiscreteZeroPole: '<S640>/Discrete Zero-Pole' */
{
AUAV_V3_TestSensors_DWork.DiscreteZeroPole_DSTATE =
rtb_DataTypeConversion_n + 0.97044334975369462*
AUAV_V3_TestSensors_DWork.DiscreteZeroPole_DSTATE;
}
}
/* End of Outputs for SubSystem: '<S622>/Initial Baro Bias' */
/* Product: '<S633>/Divide' incorporates:
* Sum: '<S633>/Sum2'
*/
rtb_Sum_o = (rtb_Sum_d - AUAV_V3_TestSensors_B.u0k120k) /
AUAV_V3_TestSensors_B.u0k120k;
/* S-Function (MCHP_C_function_Call): '<S636>/Get the GS Location [updateSensorMCUState.c]1' */
getGSLocation(
&AUAV_V3_TestSensors_B.GettheGSLocationupdateSensorMCU[0]
);
/* Sum: '<S633>/Sum1' incorporates:
* Constant: '<S633>/Constant2'
* Constant: '<S633>/Constant3'
* Constant: '<S633>/Constant4'
* Constant: '<S633>/Constant5'
* Gain: '<S639>/Unit Conversion'
* Product: '<S633>/Divide1'
* Product: '<S633>/Divide2'
* Product: '<S633>/Divide3'
* Product: '<S633>/Divide4'
* Sum: '<S633>/Sum3'
*/
rtb_Sum_o = ((rtb_Sum_o * rtb_Sum_o * 0.093502529F + rtb_Sum_o * -0.188893303F)
+ 2.18031291E-5F) * 145473.5F * 0.3048F +
AUAV_V3_TestSensors_B.GettheGSLocationupdateSensorMCU[0];
/* Outputs for Enabled SubSystem: '<S622>/Zero Out Height' incorporates:
* EnablePort: '<S638>/Enable'
*/
if (AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b.tOut > 0.0) {
/* Sum: '<S638>/Sum' incorporates:
* Delay: '<S638>/Integer Delay'
*/
AUAV_V3_TestSensors_B.Sum =
AUAV_V3_TestSensors_B.GettheGSLocationupdateSensorMCU[0] -
AUAV_V3_TestSensors_DWork.IntegerDelay_DSTATE;
/* Update for Delay: '<S638>/Integer Delay' */
AUAV_V3_TestSensors_DWork.IntegerDelay_DSTATE = rtb_Sum_o;
}
/* End of Outputs for SubSystem: '<S622>/Zero Out Height' */
/* Outputs for Enabled SubSystem: '<S622>/Enabled Subsystem' */
/* Logic: '<S622>/Logical Operator' incorporates:
* Sum: '<S622>/Sum1'
*/
AUAV_V3_T_EnabledSubsystem
(!(AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b.tOut != 0.0),
AUAV_V3_TestSensors_B.Sum + rtb_Sum_o,
&AUAV_V3_TestSensors_B.EnabledSubsystem_m);
/* End of Outputs for SubSystem: '<S622>/Enabled Subsystem' */
/* Switch: '<S581>/Switch1' */
for (i = 0; i < 5; i++) {
if (rtb_LogicalOperator_df) {
AUAV_V3_TestSensors_B.Switch1[i] =
AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdat_a[i];
} else {
AUAV_V3_TestSensors_B.Switch1[i] =
AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdatet[i];
}
}
/* End of Switch: '<S581>/Switch1' */
/* S-Function (MCHP_C_function_Call): '<S16>/Checks if FixType is 3 [updateSensorMCUState.c]1' */
AUAV_V3_TestSensors_B.ChecksifFixTypeis3updateSensorM = isFixValid(
);
/* Outputs for Enabled SubSystem: '<S16>/Initialize GS Location' incorporates:
* EnablePort: '<S579>/Enable'
*/
/* Logic: '<S16>/Logical Operator' incorporates:
* DataStoreRead: '<S16>/Data Store Read'
*/
if ((AUAV_V3_TestSensors_B.ChecksifFixTypeis3updateSensorM != 0) &&
(AUAV_V3_TestSensors_DWork.GS_INIT_FLAG != 0.0)) {
/* DataStoreWrite: '<S579>/Data Store Write' incorporates:
* Constant: '<S579>/Constant'
*/
AUAV_V3_TestSensors_DWork.GS_INIT_FLAG = 0.0;
/* DataStoreWrite: '<S579>/Data Store Write1' incorporates:
* DataStoreRead: '<S579>/Data Store Read1'
* DataTypeConversion: '<S579>/Data Type Conversion'
* DataTypeConversion: '<S579>/Data Type Conversion1'
* DataTypeConversion: '<S579>/Data Type Conversion2'
*/
mlGSLocationFloat.lat = (real32_T)mlGpsData.lat;
mlGSLocationFloat.lon = (real32_T)mlGpsData.lon;
mlGSLocationFloat.alt = (real32_T)mlGpsData.alt;
}
/* End of Logic: '<S16>/Logical Operator' */
/* End of Outputs for SubSystem: '<S16>/Initialize GS Location' */
}
/* Model step function */
void test_udp_step(void)
{
/* local block i/o variables */
real32_T rtb_ARDrone_Acc_Y;
real32_T rtb_ARDrone_Acc_Z;
real32_T rtb_ARDrone_Acc_X;
real32_T rtb_ARDrone_Magneto_X;
int32_T rtb_UDPM_Receive_Int32;
int32_T rtb_UDPM_Receive_Int32_h;
int32_T rtb_UDPM_Receive_Int32_n;
int32_T rtb_UDPM_Receive_Int32_a;
int32_T rtb_UDPM_Receive_Int32_i;
int32_T rtb_UDPM_Receive_Int32_l;
int32_T rtb_UDPM_Receive_Int32_ac;
int32_T rtb_DataTypeConversion2;
int32_T rtb_DataTypeConversion3;
int32_T rtb_DataTypeConversion12;
int32_T rtb_DataTypeConversion11;
int32_T rtb_DataTypeConversion9;
int32_T rtb_DataTypeConversion1;
int16_T rtb_Height;
int8_T rtb_DataTypeConversion3_h;
real32_T tmp;
/* S-Function (UDPM_Receive_Int32): '<S10>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2);
/* DataTypeConversion: '<S4>/Data Type Conversion' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)test_udp_P.ARDrone_LED_p1);
/* S-Function (UDPM_Receive_Int32): '<S12>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_d);
/* DataTypeConversion: '<S4>/Data Type Conversion1' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED1' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)
test_udp_P.ARDrone_LED1_p1);
/* S-Function (UDPM_Receive_Int32): '<S13>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_f);
/* DataTypeConversion: '<S4>/Data Type Conversion2' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED2' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)
test_udp_P.ARDrone_LED2_p1);
/* S-Function (UDPM_Receive_Int32): '<S14>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_p);
/* DataTypeConversion: '<S4>/Data Type Conversion3' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED3' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)
test_udp_P.ARDrone_LED3_p1);
/* S-Function (UDPM_Receive_Int32): '<S7>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_j);
/* DataTypeConversion: '<S6>/Data Type Conversion' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor_p1);
/* DataTypeConversion: '<S6>/Data Type Conversion1' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor1' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor1_p1);
/* DataTypeConversion: '<S6>/Data Type Conversion2' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor2' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor2_p1);
/* DataTypeConversion: '<S6>/Data Type Conversion3' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor3' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor3_p1);
/* S-Function (ARDrone_Acc_X): '<S1>/ARDrone_Acc_X' */
rtb_ARDrone_Acc_X = Accelero_Get_X();
/* S-Function (ARDrone_Acc_Y): '<S1>/ARDrone_Acc_Y' */
rtb_ARDrone_Acc_Y = Accelero_Get_Y();
/* S-Function (ARDrone_Acc_Z): '<S1>/ARDrone_Acc_Z' */
rtb_ARDrone_Acc_Z = Accelero_Get_Z();
/* S-Function (ARDrone_Gyro_X): '<S3>/ARDrone_Gyro_X' */
rtb_ARDrone_Magneto_X = Gyro_Get_X();
/* DataTypeConversion: '<Root>/Data Type Conversion1' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion1 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion1' */
/* S-Function (ARDrone_Gyro_Y): '<S3>/ARDrone_Gyro_Y' */
rtb_ARDrone_Magneto_X = Gyro_Get_Y();
/* DataTypeConversion: '<Root>/Data Type Conversion2' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion2 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion2' */
/* S-Function (ARDrone_Gyro_Z): '<S3>/ARDrone_Gyro_Z' */
rtb_ARDrone_Magneto_X = Gyro_Get_Z();
/* DataTypeConversion: '<Root>/Data Type Conversion3' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion3 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion3' */
/* DataTypeConversion: '<Root>/Data Type Conversion4' */
tmp = (real32_T)floor(rtb_ARDrone_Acc_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion12 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion4' */
/* DataTypeConversion: '<Root>/Data Type Conversion5' */
tmp = (real32_T)floor(rtb_ARDrone_Acc_Y);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion11 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion5' */
/* DataTypeConversion: '<Root>/Data Type Conversion6' */
tmp = (real32_T)floor(rtb_ARDrone_Acc_Z);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion9 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion6' */
/* S-Function (UDPM_Send_Int32): '<S7>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion11, (int8_T)
test_udp_P.UDPM_Send_Int32_p2);
/* S-Function (UDPM_Receive_Int32): '<S8>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_m);
/* S-Function (UDPM_Send_Int32): '<S8>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion9, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_j);
/* S-Function (UDPM_Send_Int32): '<S10>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion1, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_h);
/* S-Function (UDPM_Send_Int32): '<S12>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion2, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_p);
/* S-Function (UDPM_Send_Int32): '<S13>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion3, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_i);
/* S-Function (UDPM_Send_Int32): '<S14>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion12, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_js);
/* S-Function (UDPM_Receive_Int32): '<S9>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_h = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_g);
/* S-Function (UDPM_Send_Int32): '<S9>/UDPM_Send_Int32' incorporates:
* S-Function (ARDrone_Temperature): '<S2>/ARDrone_Temperature'
*/
udp_send_int32( (int32_T)Barometer_Get_Temperature(), (int8_T)
test_udp_P.UDPM_Send_Int32_p2_k);
/* S-Function (UDPM_Receive_Int32): '<S18>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_n = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_c);
/* S-Function (UDPM_Send_Int32): '<S18>/UDPM_Send_Int32' incorporates:
* S-Function (ARDrone_Pressure): '<S2>/ARDrone_Pressure'
*/
udp_send_int32( (int32_T)Barometer_Get_Pressure(), (int8_T)
test_udp_P.UDPM_Send_Int32_p2_o);
/* S-Function (ARDrone_Magneto_Z): '<S5>/ARDrone_Magneto_Z' */
rtb_ARDrone_Magneto_X = Magneto_Get_Z();
/* DataTypeConversion: '<Root>/Data Type Conversion10' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion9 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion10' */
/* S-Function (ARDrone_Magneto_Y): '<S5>/ARDrone_Magneto_Y' */
rtb_ARDrone_Magneto_X = Magneto_Get_Y();
/* DataTypeConversion: '<Root>/Data Type Conversion11' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion11 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion11' */
/* S-Function (ARDrone_Magneto_X): '<S5>/ARDrone_Magneto_X' */
rtb_ARDrone_Magneto_X = Magneto_Get_X();
/* DataTypeConversion: '<Root>/Data Type Conversion12' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion12 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion12' */
/* S-Function (UDPM_Receive_Int32): '<S11>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_a = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_a);
/* S-Function (UDPM_Send_Int32): '<S11>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion12, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_oz);
/* S-Function (UDPM_Receive_Int32): '<S15>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_i = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_gb);
/* S-Function (UDPM_Send_Int32): '<S15>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion11, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_n);
/* S-Function (UDPM_Receive_Int32): '<S16>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_l = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_dn);
/* S-Function (UDPM_Send_Int32): '<S16>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion9, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_d);
/* S-Function (ARDrone_Height): '<Root>/Height' */
rtb_Height = Height_Get();
/* DataTypeConversion: '<Root>/Data Type Conversion9' */
rtb_DataTypeConversion9 = rtb_Height;
/* S-Function (UDPM_Receive_Int32): '<S17>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_ac = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_pv);
/* S-Function (UDPM_Send_Int32): '<S17>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion9, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_kb);
}
