real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
int32_T u0_0;
int32_T u1_0;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = (rtNaN);
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u0 > 0.0) {
u0_0 = 1;
} else {
u0_0 = -1;
}
if (u1 > 0.0) {
u1_0 = 1;
} else {
u1_0 = -1;
}
y = atan2(u0_0, u1_0);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
/* Function: rt_atan2_snf =======================================================
* Abstract:
* Calls ATAN2, with guards against domain error and non-finites
*/
real_T rt_atan2_snf(real_T a, real_T b)
{
real_T retValue;
if (rtIsNaN(a) || rtIsNaN(b)) {
retValue = (rtNaN);
} else {
if (rtIsInf(a) && rtIsInf(b)) {
if (b > 0.0) {
b = 1.0;
} else {
b = -1.0;
}
if (a > 0.0) {
a = 1.0;
} else {
a = -1.0;
}
retValue = atan2(a,b);
} else if (b == 0.0) {
if (a > 0.0) {
retValue = (RT_PI)/2.0;
} else if (a < 0.0) {
retValue = -(RT_PI)/2.0;
} else {
retValue = 0.0;
}
} else {
retValue = atan2(a,b);
}
}
return retValue;
} /* end rt_atan2_snf */
/* Function Definitions */
static real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
int32_T i4;
int32_T i5;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u1 > 0.0) {
i4 = 1;
} else {
i4 = -1;
}
if (u0 > 0.0) {
i5 = 1;
} else {
i5 = -1;
}
y = atan2((real_T)i5, (real_T)i4);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
double scalar_log2(double x)
{
double y;
int eint;
double fdbl;
if (x == 0.0) {
y = rtMinusInf;
} else if (x < 0.0) {
y = rtNaN;
} else if ((!rtIsInf(x)) && (!rtIsNaN(x))) {
if ((!rtIsInf(x)) && (!rtIsNaN(x))) {
fdbl = frexp(x, &eint);
} else {
fdbl = x;
eint = 0;
}
if (fdbl == 0.5) {
y = (double)eint - 1.0;
} else {
y = log(fdbl) / 0.69314718055994529 + (double)eint;
}
} else {
y = x;
}
return y;
}
/* Function: rt_hypot_snf =======================================================
* Abstract:
* hypot(a,b) = sqrt(a^2 + b^2)
*/
real_T rt_hypot_snf(real_T a, real_T b)
{
real_T t; /* scales a & b */
real_T retValue;
if (rtIsNaN(a) || rtIsNaN(b)) {
retValue = (rtNaN);
} else {
if (rtIsInf(a) && rtIsInf(b)) {
retValue = (rtInf);
} else {
if (fabs(a) > fabs(b)) { /* Case 1: a > b ==> t = b / a < 1.0 */
t = b/a;
retValue = (fabs(a)*sqrt(1.0 + t*t));
} else { /* Case 2: a <= b ==> t = a / b <= 1.0 */
if (b == 0.0) {
retValue = (0.0);
} else {
t = a/b;
retValue = (fabs(b)*sqrt(1.0 + t*t));
}
} /* one input is inf */
} /* one input is nan */
}
return retValue;
} /* end rt_hypot_snf */
/* Function Definitions */
static real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
int32_T i7;
int32_T i8;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u1 > 0.0) {
i7 = 1;
} else {
i7 = -1;
}
if (u0 > 0.0) {
i8 = 1;
} else {
i8 = -1;
}
y = atan2((real_T)i8, (real_T)i7);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
//
// Arguments : const emxArray_real_T *y
// double *miny
// double *maxy
// Return Type : void
//
void MinAndMaxNoNonFinites(const emxArray_real_T *y, double *miny, double *maxy)
{
int ny;
unsigned int k;
ny = y->size[0];
k = 1U;
while ((k <= (unsigned int)ny) && (!((!rtIsInf(y->data[(int)k - 1])) &&
(!rtIsNaN(y->data[(int)k - 1]))))) {
k++;
}
if (k > (unsigned int)y->size[0]) {
*miny = 0.0;
*maxy = 0.0;
} else {
*miny = y->data[(int)k - 1];
*maxy = y->data[(int)k - 1];
while (k <= (unsigned int)ny) {
if ((!rtIsInf(y->data[(int)k - 1])) && (!rtIsNaN(y->data[(int)k - 1]))) {
if (y->data[(int)k - 1] < *miny) {
*miny = y->data[(int)k - 1];
}
if (y->data[(int)k - 1] > *maxy) {
*maxy = y->data[(int)k - 1];
}
}
k++;
}
}
}
/*
* Arguments : double u0
* double u1
* Return Type : double
*/
static double rt_atan2d_snf(double u0, double u1)
{
double y;
int b_u0;
int b_u1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u0 > 0.0) {
b_u0 = 1;
} else {
b_u0 = -1;
}
if (u1 > 0.0) {
b_u1 = 1;
} else {
b_u1 = -1;
}
y = atan2(b_u0, b_u1);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(double)(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
/* Function: rt_rem_snf =======================================================
* Abstract:
* Calls double-precision version of REM, with guard against domain error
* and guards against non-finites
*/
real_T rt_rem_snf(const real_T xr, const real_T yr)
{
real_T zr, yrf, tr, trf, wr;
if (yr == 0.0 || rtIsInf(xr) || rtIsInf(yr) || rtIsNaN(xr) || rtIsNaN(yr)) {
zr = (rtNaN);
} else {
yrf = (yr < 0.0) ? ceil(yr) : floor(yr);
tr = xr/yr;
if (yr == yrf) {
/* Integer denominator. Use conventional formula.*/
trf = (tr < 0.0) ? ceil(tr) : floor(tr);
zr = xr - trf*yr;
} else {
/* Noninteger denominator. Check for nearly integer quotient.*/
wr = rt_round_snf(tr);
if (fabs(tr - wr) <= ((DBL_EPSILON)*fabs(tr))) {
zr = 0.0;
} else {
trf = (tr < 0.0) ? ceil(tr) : floor(tr);
zr = (tr - trf)*yr;
}
}
}
return zr;
} /* end rt_rem_snf */
/*
* Arguments : const double y[128]
* double iPk_data[]
* int iPk_size[1]
* double iInf_data[]
* int iInf_size[1]
* double iInflect_data[]
* int iInflect_size[1]
* Return Type : void
*/
static void getAllPeaks(const double y[128], double iPk_data[], int iPk_size[1],
double iInf_data[], int iInf_size[1], double iInflect_data[], int
iInflect_size[1])
{
boolean_T x[128];
int i;
unsigned char ii_data[128];
int ii;
boolean_T exitg1;
boolean_T guard1 = false;
double yTemp[128];
for (i = 0; i < 128; i++) {
x[i] = (rtIsInf(y[i]) && (y[i] > 0.0));
}
i = 0;
ii = 1;
exitg1 = false;
while ((!exitg1) && (ii < 129)) {
guard1 = false;
if (x[ii - 1]) {
i++;
ii_data[i - 1] = (unsigned char)ii;
if (i >= 128) {
exitg1 = true;
} else {
guard1 = true;
}
} else {
guard1 = true;
}
if (guard1) {
ii++;
}
}
if (1 > i) {
i = 0;
}
iInf_size[0] = i;
for (ii = 0; ii < i; ii++) {
iInf_data[ii] = ii_data[ii];
}
memcpy(&yTemp[0], &y[0], sizeof(double) << 7);
for (ii = 0; ii < i; ii++) {
ii_data[ii] = (unsigned char)iInf_data[ii];
}
for (ii = 0; ii < i; ii++) {
yTemp[ii_data[ii] - 1] = rtNaN;
}
findLocalMaxima(yTemp, iPk_data, iPk_size, iInflect_data, iInflect_size);
}
static real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
y = atan2(u0 > 0.0 ? 1.0 : -1.0, u1 > 0.0 ? 1.0 : -1.0);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
/* Function: rt_pow_snf =======================================================
* Abstract:
* Calls double-precision version of POW, with guard against domain error
* and guards against non-finites
*/
real_T rt_pow_snf(const real_T xr, const real_T yr)
{
real_T ret, axr, ayr;
if (rtIsNaN(xr) || rtIsNaN(yr)) {
ret = (rtNaN);
} else {
axr = fabs(xr);
ayr = fabs(yr);
if (rtIsInf(ayr)) {
if (axr == 1.0) {
ret = (rtNaN);
} else if (axr > 1.0) {
if (yr > 0.0) {
ret = (rtInf);
} else {
ret = 0.0;
}
} else {
if (yr > 0.0) {
ret = 0.0;
} else {
ret = (rtInf);
}
}
} else {
if (ayr == 0.0) {
ret = 1.0;
} else if (ayr == 1.0) {
if (yr > 0.0) {
ret = xr;
} else {
ret = 1.0/xr;
}
} else if (yr == 2.0) {
ret = xr*xr;
} else if (yr == 0.5 && xr >= 0.0) {
ret = sqrt(xr);
} else if ((xr < 0.0) && (yr > floor(yr))) {
ret = (rtNaN);
} else {
ret = pow(xr,yr);
}
}
}
return ret;
} /* end rt_pow_snf */
/* Function Definitions */
double rt_powd_snf(double u0, double u1)
{
double y;
double d0;
double d1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else {
d0 = std::abs(u0);
d1 = std::abs(u1);
if (rtIsInf(u1)) {
if (d0 == 1.0) {
y = 1.0;
} else if (d0 > 1.0) {
if (u1 > 0.0) {
y = rtInf;
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = rtInf;
}
} else if (d1 == 0.0) {
y = 1.0;
} else if (d1 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = std::sqrt(u0);
} else if ((u0 < 0.0) && (u1 > std::floor(u1))) {
y = rtNaN;
} else {
y = pow(u0, u1);
}
}
return y;
}
/* Function Definitions */
static real_T rt_powd_snf(real_T u0, real_T u1)
{
real_T y;
real_T d0;
real_T d1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else {
d0 = fabs(u0);
d1 = fabs(u1);
if (rtIsInf(u1)) {
if (d0 == 1.0) {
y = rtNaN;
} else if (d0 > 1.0) {
if (u1 > 0.0) {
y = rtInf;
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = rtInf;
}
} else if (d1 == 0.0) {
y = 1.0;
} else if (d1 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = sqrt(u0);
} else if ((u0 < 0.0) && (u1 > floor(u1))) {
y = rtNaN;
} else {
y = pow(u0, u1);
}
}
return y;
}
/* Function Definitions */
double rt_powd_snf(double u0, double u1)
{
double y;
double d20;
double d21;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else {
d20 = fabs(u0);
d21 = fabs(u1);
if (rtIsInf(u1)) {
if (d20 == 1.0) {
y = rtNaN;
} else if (d20 > 1.0) {
if (u1 > 0.0) {
y = rtInf;
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = rtInf;
}
} else if (d21 == 0.0) {
y = 1.0;
} else if (d21 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = sqrt(u0);
} else if ((u0 < 0.0) && (u1 > floor(u1))) {
y = rtNaN;
} else {
y = pow(u0, u1);
}
}
return y;
}
real_T rt_powd_snf(real_T u0, real_T u1)
{
real_T y;
real_T tmp;
real_T tmp_0;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = (rtNaN);
} else {
tmp = fabs(u0);
tmp_0 = fabs(u1);
if (rtIsInf(u1)) {
if (tmp == 1.0) {
y = (rtNaN);
} else if (tmp > 1.0) {
if (u1 > 0.0) {
y = (rtInf);
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = (rtInf);
}
} else if (tmp_0 == 0.0) {
y = 1.0;
} else if (tmp_0 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = sqrt(u0);
} else if ((u0 < 0.0) && (u1 > floor(u1))) {
y = (rtNaN);
} else {
y = pow(u0, u1);
}
}
return y;
}
/* Function Definitions */
void b_sqrt(creal_T *x)
{
real_T absxi;
real_T absxr;
if (x->im == 0.0) {
if (x->re < 0.0) {
absxi = 0.0;
absxr = sqrt(fabs(x->re));
} else {
absxi = sqrt(x->re);
absxr = 0.0;
}
} else if (x->re == 0.0) {
if (x->im < 0.0) {
absxi = sqrt(-x->im / 2.0);
absxr = -absxi;
} else {
absxi = sqrt(x->im / 2.0);
absxr = absxi;
}
} else if (rtIsNaN(x->re) || rtIsNaN(x->im)) {
absxi = rtNaN;
absxr = rtNaN;
} else if (rtIsInf(x->im)) {
absxi = rtInf;
absxr = x->im;
} else if (rtIsInf(x->re)) {
if (x->re < 0.0) {
absxi = 0.0;
absxr = rtInf;
} else {
absxi = rtInf;
absxr = 0.0;
}
} else {
absxr = fabs(x->re);
absxi = fabs(x->im);
if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
{
absxr *= 0.5;
absxi *= 0.5;
absxi = rt_hypotd_snf(absxr, absxi);
if (absxi > absxr) {
absxi = sqrt(absxi) * sqrt(1.0 + absxr / absxi);
} else {
absxi = sqrt(absxi) * 1.4142135623730951;
}
} else {
absxi = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
}
if (x->re > 0.0) {
absxr = 0.5 * (x->im / absxi);
} else {
if (x->im < 0.0) {
absxr = -absxi;
} else {
absxr = absxi;
}
absxi = 0.5 * (x->im / absxr);
}
}
x->re = absxi;
x->im = absxr;
}
//
// Arguments : const emxArray_real_T *Y
// double no[32]
// double xo[32]
// Return Type : void
//
void hist(const emxArray_real_T *Y, double no[32], double xo[32])
{
int k;
int b_Y[1];
emxArray_real_T c_Y;
double maxy;
double miny;
double edges[33];
int exponent;
int d_Y[1];
double nn[33];
k = Y->size[0];
if (k == 0) {
for (k = 0; k < 32; k++) {
xo[k] = 1.0 + (double)k;
no[k] = 0.0;
}
} else {
b_Y[0] = Y->size[0];
c_Y = *Y;
c_Y.size = (int *)&b_Y;
c_Y.numDimensions = 1;
MinAndMaxNoNonFinites(&c_Y, &miny, &maxy);
if (miny == maxy) {
miny = (miny - 16.0) - 0.5;
maxy = (maxy + 16.0) - 0.5;
}
linspace(miny, maxy, edges);
miny = (edges[1] - edges[0]) / 2.0;
for (k = 0; k < 32; k++) {
xo[k] = edges[k] + miny;
}
edges[0] = rtMinusInf;
edges[32] = rtInf;
for (k = 0; k < 31; k++) {
miny = std::abs(edges[k + 1]);
if ((!rtIsInf(miny)) && (!rtIsNaN(miny))) {
if (miny <= 2.2250738585072014E-308) {
miny = 4.94065645841247E-324;
} else {
frexp(miny, &exponent);
miny = std::ldexp(1.0, exponent - 53);
}
} else {
miny = rtNaN;
}
edges[k + 1] += miny;
}
d_Y[0] = Y->size[0];
c_Y = *Y;
c_Y.size = (int *)&d_Y;
c_Y.numDimensions = 1;
histc(&c_Y, edges, nn);
memcpy(&no[0], &nn[0], sizeof(double) << 5);
no[31] += nn[32];
}
}
/* 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' */
}
/* Core#0 */
void mcos_core0_thread(uint32_t stacd, uintptr_t exinf)
{ /* omit input channel bit vector */
/* CH__VEC(IN_0022,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0022,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0014,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0014,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0002,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0002,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0012,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0012,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0004,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0004,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0023,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0023,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0016,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0016,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0005,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0005,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0015,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0015,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0006,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0006,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0024,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0024,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0018,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0018,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0007,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0007,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0017,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0017,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0008,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0008,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0025,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0025,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0020,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0020,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0009,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0009,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0019,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0019,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0010,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0010,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0026,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0026,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0013,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0013,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0011,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0011,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0021,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0021,1); */
/* omit input channel bit vector */
/* CH__VEC(IN_0003,1); */
/* omit output channel bit vector */
/* CH__VEC(OUT_0003,1); */
/* Real-Time Model Object */
RT_MODEL_offset_rate210_T offset_rate210_M_;
RT_MODEL_offset_rate210_T * const offset_rate210_M = &offset_rate210_M_;
/* states */
struct {
/* Block: offset_rate210_Sqrt1 */
real_T UnitDelay2_DSTATE; /* task_0022 */
} offset_rate210_DW;
/* params */
struct {
/* Block: offset_rate210_Sqrt1 */
real_T UnitDelay2_InitialCondition;
/* Block: offset_rate210_Gain1 */
real_T Gain1_Gain;
/* Block: offset_rate210_Gain2 */
real_T Gain2_Gain;
/* Block: offset_rate210_Gain3 */
real_T Gain3_Gain;
/* Block: offset_rate210_Gain4 */
real_T Gain4_Gain;
/* Block: offset_rate210_Gain5 */
real_T Gain5_Gain;
/* Block: offset_rate210_Gain6 */
real_T Gain6_Gain;
/* Block: offset_rate210_Gain7 */
real_T Gain7_Gain;
/* Block: offset_rate210_Gain8 */
real_T Gain8_Gain;
/* Block: offset_rate210_Gain9 */
real_T Gain9_Gain;
/* Block: offset_rate210_Gain10 */
real_T Gain10_Gain;
} offset_rate210_P = {
1.0, /* Expression: 1
* Referenced by: '<Root>/UnitDelay2'
*/
2.0, /* Expression: 2
* Referenced by: '<Root>/Gain1'
*/
0.5, /* Expression: 0.5
* Referenced by: '<Root>/Gain2'
*/
2.0, /* Expression: 2
* Referenced by: '<Root>/Gain3'
*/
0.5, /* Expression: 0.5
* Referenced by: '<Root>/Gain4'
*/
2.0, /* Expression: 2
* Referenced by: '<Root>/Gain5'
*/
0.5, /* Expression: 0.5
* Referenced by: '<Root>/Gain6'
*/
2.0, /* Expression: 2
* Referenced by: '<Root>/Gain7'
*/
0.5, /* Expression: 0.5
* Referenced by: '<Root>/Gain8'
*/
2.0, /* Expression: 2
* Referenced by: '<Root>/Gain9'
*/
0.5, /* Expression: 0.5
* Referenced by: '<Root>/Gain10'
*/
};
/* internal variables */
/* Block: offset_rate210_Sqrt1 */
real_T offset_rate210_UnitDelay2_1;
/* input variables */
/* Block: offset_rate210_Sqrt1 */
real_T offset_rate210_Gain_1;
/* Block: offset_rate210_MathFunction2 */
real_T offset_rate210_Sqrt1_1;
/* Block: offset_rate210_Gain1 */
real_T offset_rate210_MathFunction2_1;
/* Block: offset_rate210_MathFunction1 */
real_T offset_rate210_Gain1_1;
/* Block: offset_rate210_Gain2 */
real_T offset_rate210_MathFunction1_1;
/* Block: offset_rate210_Sqrt2 */
real_T offset_rate210_Gain2_1;
/* Block: offset_rate210_MathFunction4 */
real_T offset_rate210_Sqrt2_1;
/* Block: offset_rate210_Gain3 */
real_T offset_rate210_MathFunction4_1;
/* Block: offset_rate210_MathFunction3 */
real_T offset_rate210_Gain3_1;
/* Block: offset_rate210_Gain4 */
real_T offset_rate210_MathFunction3_1;
/* Block: offset_rate210_Sqrt3 */
real_T offset_rate210_Gain4_1;
/* Block: offset_rate210_MathFunction6 */
real_T offset_rate210_Sqrt3_1;
/* Block: offset_rate210_Gain5 */
real_T offset_rate210_MathFunction6_1;
/* Block: offset_rate210_MathFunction5 */
real_T offset_rate210_Gain5_1;
/* Block: offset_rate210_Gain6 */
real_T offset_rate210_MathFunction5_1;
/* Block: offset_rate210_Sqrt4 */
real_T offset_rate210_Gain6_1;
/* Block: offset_rate210_MathFunction8 */
real_T offset_rate210_Sqrt4_1;
/* Block: offset_rate210_Gain7 */
real_T offset_rate210_MathFunction8_1;
/* Block: offset_rate210_MathFunction7 */
real_T offset_rate210_Gain7_1;
/* Block: offset_rate210_Gain8 */
real_T offset_rate210_MathFunction7_1;
/* Block: offset_rate210_Sqrt5 */
real_T offset_rate210_Gain8_1;
/* Block: offset_rate210_MathFunction10 */
real_T offset_rate210_Sqrt5_1;
/* Block: offset_rate210_Gain9 */
real_T offset_rate210_MathFunction10_1;
/* Block: offset_rate210_MathFunction9 */
real_T offset_rate210_Gain9_1;
/* Block: offset_rate210_Gain10 */
real_T offset_rate210_MathFunction9_1;
/* output variables */
/* Block: offset_rate210_Sqrt1 */
/* Block: offset_rate210_MathFunction2 */
/* Block: offset_rate210_Gain1 */
/* Block: offset_rate210_MathFunction1 */
/* Block: offset_rate210_Gain2 */
/* Block: offset_rate210_Sqrt2 */
/* Block: offset_rate210_MathFunction4 */
/* Block: offset_rate210_Gain3 */
/* Block: offset_rate210_MathFunction3 */
/* Block: offset_rate210_Gain4 */
/* Block: offset_rate210_Sqrt3 */
/* Block: offset_rate210_MathFunction6 */
/* Block: offset_rate210_Gain5 */
/* Block: offset_rate210_MathFunction5 */
/* Block: offset_rate210_Gain6 */
/* Block: offset_rate210_Sqrt4 */
/* Block: offset_rate210_MathFunction8 */
/* Block: offset_rate210_Gain7 */
/* Block: offset_rate210_MathFunction7 */
/* Block: offset_rate210_Gain8 */
/* Block: offset_rate210_Sqrt5 */
/* Block: offset_rate210_MathFunction10 */
/* Block: offset_rate210_Gain9 */
/* Block: offset_rate210_MathFunction9 */
/* Block: offset_rate210_Gain10 */
real_T offset_rate210_Gain10_1;
/* functions */
/* Block: offset_rate210_MathFunction1 */
real_T rt_powd_snf(real_T u0, real_T u1)
{
real_T y;
real_T tmp;
real_T tmp_0;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = (rtNaN);
} else {
tmp = fabs(u0);
tmp_0 = fabs(u1);
if (rtIsInf(u1)) {
if (tmp == 1.0) {
y = (rtNaN);
} else if (tmp > 1.0) {
if (u1 > 0.0) {
y = (rtInf);
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = (rtInf);
}
} else if (tmp_0 == 0.0) {
y = 1.0;
} else if (tmp_0 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = sqrt(u0);
} else if ((u0 < 0.0) && (u1 > floor(u1))) {
y = (rtNaN);
} else {
y = pow(u0, u1);
}
}
return y;
}
//
// sample script to create volumetric mesh from
// multiple levelsets of a binary segmented head image.
// Arguments : void
// Return Type : void
//
static void c_meshing()
{
double fid;
static double face[362368];
static double elem[565750];
static double node[100564];
static int idx[113150];
int k;
boolean_T p;
static int idx0[113150];
int i;
int i2;
int j;
int pEnd;
int nb;
int b_k;
int qEnd;
int kEnd;
emxArray_real_T *b;
double x;
int32_T exitg1;
int exponent;
emxArray_real_T *b_b;
// create volumetric tetrahedral mesh from the two-layer 3D images
// head surface element size bound
b_fprintf();
TCHAR pwd[MAX_PATH];
GetCurrentDirectory(MAX_PATH,pwd);
std::string str= pwd;
std::string const command=char(34)+str+"\\bin\\cgalmesh.exe" +char(34)+" pre_cgalmesh.inr post_cgalmesh.mesh 30 4 0.5 3 100 1648335518";
system(command.c_str());
b_readmedit(node, elem, face);
// node=node*double(vol(1,1,1));
for (k = 0; k < 113150; k++) {
idx[k] = k + 1;
}
for (k = 0; k < 113150; k += 2) {
if ((elem[452600 + k] <= elem[k + 452601]) || rtIsNaN(elem[k + 452601])) {
p = true;
} else {
p = false;
}
if (p) {
} else {
idx[k] = k + 2;
idx[k + 1] = k + 1;
}
}
for (i = 0; i < 113150; i++) {
idx0[i] = 1;
}
i = 2;
while (i < 113150) {
i2 = i << 1;
j = 1;
for (pEnd = 1 + i; pEnd < 113151; pEnd = qEnd + i) {
nb = j;
b_k = pEnd - 1;
qEnd = j + i2;
if (qEnd > 113151) {
qEnd = 113151;
}
k = 0;
kEnd = qEnd - j;
while (k + 1 <= kEnd) {
if ((elem[idx[nb - 1] + 452599] <= elem[idx[b_k] + 452599]) || rtIsNaN
(elem[idx[b_k] + 452599])) {
p = true;
} else {
p = false;
}
if (p) {
idx0[k] = idx[nb - 1];
nb++;
if (nb == pEnd) {
while (b_k + 1 < qEnd) {
k++;
idx0[k] = idx[b_k];
b_k++;
}
}
} else {
idx0[k] = idx[b_k];
b_k++;
if (b_k + 1 == qEnd) {
while (nb < pEnd) {
k++;
idx0[k] = idx[nb - 1];
nb++;
}
}
}
k++;
}
for (k = 0; k + 1 <= kEnd; k++) {
idx[(j + k) - 1] = idx0[k];
}
j = qEnd;
}
i = i2;
}
emxInit_real_T(&b, 1);
i2 = b->size[0];
b->size[0] = 113150;
emxEnsureCapacity((emxArray__common *)b, i2, (int)sizeof(double));
for (k = 0; k < 113150; k++) {
b->data[k] = elem[idx[k] + 452599];
}
k = 0;
while ((k + 1 <= 113150) && rtIsInf(b->data[k]) && (b->data[k] < 0.0)) {
k++;
}
b_k = k;
k = 113150;
while ((k >= 1) && rtIsNaN(b->data[k - 1])) {
k--;
}
pEnd = 113150 - k;
while ((k >= 1) && rtIsInf(b->data[k - 1]) && (b->data[k - 1] > 0.0)) {
k--;
}
i2 = 113150 - (k + pEnd);
nb = -1;
if (b_k > 0) {
nb = 0;
}
i = (b_k + k) - b_k;
while (b_k + 1 <= i) {
x = b->data[b_k];
do {
exitg1 = 0;
b_k++;
if (b_k + 1 > i) {
exitg1 = 1;
} else {
fid = fabs(x / 2.0);
if ((!rtIsInf(fid)) && (!rtIsNaN(fid))) {
if (fid <= 2.2250738585072014E-308) {
fid = 4.94065645841247E-324;
} else {
frexp(fid, &exponent);
fid = ldexp(1.0, exponent - 53);
}
} else {
fid = rtNaN;
}
if ((fabs(x - b->data[b_k]) < fid) || (rtIsInf(b->data[b_k]) && rtIsInf
(x) && ((b->data[b_k] > 0.0) == (x > 0.0)))) {
p = true;
} else {
p = false;
}
if (!p) {
exitg1 = 1;
}
}
} while (exitg1 == 0);
nb++;
b->data[nb] = x;
}
if (i2 > 0) {
nb++;
b->data[nb] = b->data[i];
}
b_k = i + i2;
for (j = 1; j <= pEnd; j++) {
nb++;
b->data[nb] = b->data[(b_k + j) - 1];
}
if (1 > nb + 1) {
i = -1;
} else {
i = nb;
}
emxInit_real_T(&b_b, 1);
i2 = b_b->size[0];
b_b->size[0] = i + 1;
emxEnsureCapacity((emxArray__common *)b_b, i2, (int)sizeof(double));
for (i2 = 0; i2 <= i; i2++) {
b_b->data[i2] = b->data[i2];
}
i2 = b->size[0];
b->size[0] = b_b->size[0];
emxEnsureCapacity((emxArray__common *)b, i2, (int)sizeof(double));
i = b_b->size[0];
for (i2 = 0; i2 < i; i2++) {
b->data[i2] = b_b->data[i2];
}
emxFree_real_T(&b_b);
c_fprintf((double)b->size[0]);
d_fprintf();
emxFree_real_T(&b);
}
static real_T c2_atan2(real_T c2_Y, real_T c2_X)
{
real_T c2_k;
real_T c2_b_k;
real_T c2_b_x;
real_T c2_xk;
real_T c2_yk;
real_T c2_c_x;
real_T c2_b_xk;
real_T c2_c_xk;
real_T c2_b_y;
real_T c2_d_x;
real_T c2_e_x;
boolean_T c2_b;
real_T c2_f_x;
boolean_T c2_b_b;
real_T c2_g_x;
real_T c2_r;
real_T c2_c_y;
real_T c2_b_r;
real_T c2_h_x;
boolean_T c2_c_b;
real_T c2_i_x;
boolean_T c2_d_b;
real_T c2_j_x;
real_T c2_c_r;
real_T c2_k_x;
real_T c2_d_r;
real_T c2_l_x;
real_T c2_e_r;
real_T c2_m_x;
real_T c2_f_r;
c2_k = 1.0;
c2_b_k = c2_k;
c2_b_x = c2_Y;
c2_xk = c2_b_x;
c2_yk = c2_xk;
c2_c_x = c2_X;
c2_b_xk = c2_c_x;
c2_c_xk = c2_b_xk;
_SFD_EML_ARRAY_BOUNDS_CHECK("R", (int32_T)_SFD_INTEGER_CHECK("k", c2_b_k), 1,
1, 1);
c2_b_y = c2_yk;
c2_d_x = c2_c_xk;
c2_e_x = c2_d_x;
c2_b = rtIsNaN(c2_e_x);
if(c2_b) {
goto label_1;
} else {
c2_f_x = c2_b_y;
c2_b_b = rtIsNaN(c2_f_x);
if(c2_b_b) {
goto label_1;
} else {
c2_h_x = c2_b_y;
c2_c_b = rtIsInf(c2_h_x);
if(c2_c_b) {
c2_i_x = c2_d_x;
c2_d_b = rtIsInf(c2_i_x);
if(c2_d_b) {
c2_j_x = atan2(c2_sign(c2_b_y), c2_sign(c2_d_x));
c2_c_r = c2_j_x;
c2_b_r = c2_c_r;
goto label_2;
}
}
}
}
if(c2_d_x == 0.0) {
if(c2_b_y > 0.0) {
c2_k_x = 1.5707963267948966E+000;
c2_d_r = c2_k_x;
c2_b_r = c2_d_r;
} else if(c2_b_y < 0.0) {
c2_l_x = -1.5707963267948966E+000;
c2_e_r = c2_l_x;
c2_b_r = c2_e_r;
} else {
c2_b_r = 0.0;
}
} else {
c2_m_x = atan2(c2_b_y, c2_d_x);
c2_f_r = c2_m_x;
c2_b_r = c2_f_r;
}
goto label_2;
label_1:;
c2_g_x = rtNaN;
c2_r = c2_g_x;
c2_c_y = c2_r;
c2_b_r = c2_c_y;
label_2:;
return c2_b_r;
}
/*
* Arguments : double m
* double f
* const emxArray_real_T *w
* emxArray_real_T *b
* Return Type : void
*/
void fkernel(double m, double f, const emxArray_real_T *w, emxArray_real_T *b)
{
int k;
int nm1d2;
double anew;
double y;
int n;
double apnd;
double ndbl;
double cdiff;
double absa;
double absb;
emxArray_real_T *b_m;
emxArray_int32_T *r0;
emxArray_real_T *b_y;
emxArray_real_T *x;
k = b->size[0] * b->size[1];
b->size[0] = 1;
b->size[1] = w->size[1];
emxEnsureCapacity((emxArray__common *)b, k, (int)sizeof(double));
nm1d2 = w->size[1];
for (k = 0; k < nm1d2; k++) {
b->data[k] = 0.0;
}
anew = -m / 2.0;
y = m / 2.0;
if (rtIsNaN(anew) || rtIsNaN(y)) {
n = 1;
anew = rtNaN;
apnd = y;
} else if (y < anew) {
n = 0;
apnd = y;
} else if (rtIsInf(anew) || rtIsInf(y)) {
n = 1;
anew = rtNaN;
apnd = y;
} else {
ndbl = floor((y - anew) + 0.5);
apnd = anew + ndbl;
cdiff = apnd - y;
absa = fabs(anew);
absb = fabs(y);
if ((absa >= absb) || rtIsNaN(absb)) {
absb = absa;
}
if (fabs(cdiff) < 4.4408920985006262E-16 * absb) {
ndbl++;
apnd = y;
} else if (cdiff > 0.0) {
apnd = anew + (ndbl - 1.0);
} else {
ndbl++;
}
if (ndbl >= 0.0) {
n = (int)ndbl;
} else {
n = 0;
}
}
emxInit_real_T(&b_m, 2);
k = b_m->size[0] * b_m->size[1];
b_m->size[0] = 1;
b_m->size[1] = n;
emxEnsureCapacity((emxArray__common *)b_m, k, (int)sizeof(double));
if (n > 0) {
b_m->data[0] = anew;
if (n > 1) {
b_m->data[n - 1] = apnd;
k = n - 1;
nm1d2 = k / 2;
for (k = 1; k < nm1d2; k++) {
b_m->data[k] = anew + (double)k;
b_m->data[(n - k) - 1] = apnd - (double)k;
}
if (nm1d2 << 1 == n - 1) {
b_m->data[nm1d2] = (anew + apnd) / 2.0;
} else {
b_m->data[nm1d2] = anew + (double)nm1d2;
b_m->data[nm1d2 + 1] = apnd - (double)nm1d2;
}
}
}
emxInit_int32_T(&r0, 2);
n = b_m->size[1] - 1;
nm1d2 = 0;
for (k = 0; k <= n; k++) {
if (b_m->data[k] == 0.0) {
nm1d2++;
}
}
k = r0->size[0] * r0->size[1];
r0->size[0] = 1;
r0->size[1] = nm1d2;
emxEnsureCapacity((emxArray__common *)r0, k, (int)sizeof(int));
nm1d2 = 0;
for (k = 0; k <= n; k++) {
if (b_m->data[k] == 0.0) {
r0->data[nm1d2] = k + 1;
nm1d2++;
}
}
nm1d2 = r0->size[0] * r0->size[1];
for (k = 0; k < nm1d2; k++) {
b->data[r0->data[k] - 1] = 6.2831853071795862 * f;
}
/* No division by zero */
n = b_m->size[1] - 1;
nm1d2 = 0;
for (k = 0; k <= n; k++) {
if (b_m->data[k] != 0.0) {
nm1d2++;
}
}
k = r0->size[0] * r0->size[1];
r0->size[0] = 1;
r0->size[1] = nm1d2;
emxEnsureCapacity((emxArray__common *)r0, k, (int)sizeof(int));
nm1d2 = 0;
for (k = 0; k <= n; k++) {
if (b_m->data[k] != 0.0) {
r0->data[nm1d2] = k + 1;
nm1d2++;
}
}
emxInit_real_T(&b_y, 2);
y = 6.2831853071795862 * f;
k = b_y->size[0] * b_y->size[1];
b_y->size[0] = 1;
b_y->size[1] = r0->size[1];
emxEnsureCapacity((emxArray__common *)b_y, k, (int)sizeof(double));
nm1d2 = r0->size[0] * r0->size[1];
for (k = 0; k < nm1d2; k++) {
b_y->data[k] = y * b_m->data[r0->data[k] - 1];
}
emxInit_real_T(&x, 2);
k = x->size[0] * x->size[1];
x->size[0] = 1;
x->size[1] = b_y->size[1];
emxEnsureCapacity((emxArray__common *)x, k, (int)sizeof(double));
nm1d2 = b_y->size[0] * b_y->size[1];
for (k = 0; k < nm1d2; k++) {
x->data[k] = b_y->data[k];
}
for (k = 0; k + 1 <= b_y->size[1]; k++) {
x->data[k] = sin(x->data[k]);
}
k = b_y->size[0] * b_y->size[1];
b_y->size[0] = 1;
b_y->size[1] = x->size[1];
emxEnsureCapacity((emxArray__common *)b_y, k, (int)sizeof(double));
nm1d2 = x->size[0] * x->size[1];
for (k = 0; k < nm1d2; k++) {
b_y->data[k] = x->data[k] / b_m->data[r0->data[k] - 1];
}
emxFree_real_T(&x);
emxFree_int32_T(&r0);
n = b_m->size[1];
nm1d2 = 0;
for (k = 0; k < n; k++) {
if (b_m->data[k] != 0.0) {
b->data[k] = b_y->data[nm1d2];
nm1d2++;
}
}
emxFree_real_T(&b_y);
emxFree_real_T(&b_m);
/* Sinc */
k = b->size[0] * b->size[1];
b->size[0] = 1;
emxEnsureCapacity((emxArray__common *)b, k, (int)sizeof(double));
nm1d2 = b->size[0];
k = b->size[1];
nm1d2 *= k;
for (k = 0; k < nm1d2; k++) {
b->data[k] *= w->data[k];
}
/* Window */
if (b->size[1] == 0) {
y = 0.0;
} else {
y = b->data[0];
for (k = 2; k <= b->size[1]; k++) {
y += b->data[k - 1];
}
}
k = b->size[0] * b->size[1];
b->size[0] = 1;
emxEnsureCapacity((emxArray__common *)b, k, (int)sizeof(double));
nm1d2 = b->size[0];
k = b->size[1];
nm1d2 *= k;
for (k = 0; k < nm1d2; k++) {
b->data[k] /= y;
}
/* Normalization to unity gain at DC */
}
/*
* CLCPMP Minimizing Hamiltonian: Co-States for soc and time
* Erstellungsdatum der ersten Version 19.08.2015 - Stephan Uebel
*
* Batterieleistungsgrenzen hinzugefügt am 13.12.2015
* ^^added battery power limit
*
* Massenaufschlag durch Trägheitsmoment herausgenommen
* ^^Mass increment removed by inertia
*
* % Inputdefinition
*
* engKinPre - Double(1,1) - kinetische Energie am Intervallanfang in J
* ^^ kinetic energy at start of interval (J)
* engKinAct - Double(1,1) - kinetische Energie am Intervallende in J
* ^^ kinetic energe at end of interval (J)
* gea - Double(1,1) - Gang
* ^^ gear
* slp - Double(1,1) - Steigung in rad
* ^^ slope in radians
* iceFlg - Boolean(1,1) - Flag für Motorzustand
* ^^ flag for motor condition
* batEng - Double(1,1) - Batterieenergie in J
* ^^ battery energy (J)
* psibatEng - Double(1,1) - Costate für Batterieenergie ohne Einheit
* ^^ costate for battery energy w/o unity
* psiTim - Double(1,1) - Costate für die Zeit ohne Einheit
* ^^ costate for time without unity
* batPwrAux - Double(1,1) - elektr. Nebenverbraucherleistung in W
* ^^ electric auxiliary power consumed (W)
* batEngStp - Double(1,1) - Drehmomentschritt
* ^^ torque step <- no, it's a battery step
* wayStp - Double(1,1) - Intervallschrittweite in m
* ^^ interval step distance (m)
* fzg_scalar - Struct(1,1) - Modelldaten - nur skalar
* fzg_array - Struct(1,1) - Modelldaten _ nur arrays
* Arguments : double engKinPre
* double engKinAct
* double gea
* double slp
* double batEng
* double psiBatEng
* double psiTim
* double batPwrAux
* double batEngStp
* double wayStp
* const struct2_T *fzg_scalar
* const struct4_T *fzg_array
* double *cosHamMin
* double *batFrcOut
* double *fulFrcOut
* Return Type : void
*/
void clcPMP_olyHyb_port(double engKinPre, double engKinAct, double gea, double
slp, double batEng, double psiBatEng, double psiTim, double batPwrAux, double
batEngStp, double wayStp, const struct2_T *fzg_scalar, const struct4_T
*fzg_array, double *cosHamMin, double *batFrcOut, double *fulFrcOut)
{
double vehVel;
double b_engKinPre[2];
double crsSpdVec[2];
int k;
boolean_T y;
boolean_T exitg3;
boolean_T exitg2;
double crsSpd;
double whlTrq;
double crsTrq;
double iceTrqMax;
double iceTrqMin;
double b_fzg_array[100];
double emoTrqMaxPos;
double emoTrqMinPos;
double emoTrqMax;
double emoTrqMin;
double batPwrMax;
double batPwrMin;
double batOcv;
double batEngDltMin;
double batEngDltMax;
double batEngDltMinInx;
double batEngDltMaxInx;
int batEngDltInx;
double batEngDlt;
double fulFrc;
double batFrc;
double b_batFrc;
double batPwr;
double emoTrq;
double iceTrq;
double fulPwr;
int ixstart;
double mtmp;
int itmp;
int ix;
boolean_T exitg1;
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Ausgabewert des Minimums der Hamiltonfunktion */
/* output for minimizing the hamiltonian */
*cosHamMin = rtInf;
/* Batterieladungsänderung im Wegschritt beir minimaler Hamiltonfunktion */
/* battery change in path_idx step with the minial hamiltonian */
*batFrcOut = rtInf;
/* Kraftstoffkraftänderung im Wegschritt bei minimaler Hamiltonfunktion */
/* fuel change in path_idx step with the minimal hamiltonian */
*fulFrcOut = 0.0;
/* % Initialisieren der persistent Größen */
/* initialize the persistance variables */
/* Diese werden die nur einmal für die Funktion berechnet */
/* only calculated once for the function */
if (!crsSpdHybMax_not_empty) {
/* maximale Drehzahl Elektrommotor */
/* maximum electric motor rotational speed */
/* maximale Drehzahl der Kurbelwelle */
/* maximum crankshaft rotational speed */
if ((fzg_array->iceSpdMgd[14850] <= fzg_array->emoSpdMgd[14850]) || rtIsNaN
(fzg_array->emoSpdMgd[14850])) {
crsSpdHybMax = fzg_array->iceSpdMgd[14850];
} else {
crsSpdHybMax = fzg_array->emoSpdMgd[14850];
}
crsSpdHybMax_not_empty = true;
/* minimale Drehzahl der Kurbelwelle */
/* minimum crankshaft rotational speed */
crsSpdHybMin = fzg_array->iceSpdMgd[0];
}
/* % Initialisieren der allgemein benötigten Kenngrößen */
/* initializing the commonly required parameters */
/* mittlere kinetische Energie im Wegschritt berechnen */
/* define the average kinetic energy at path_idx step - is just previous KE */
/* mittlere Geschwindigkeit im Wegschritt berechnen */
/* define the average speed at path_idx step */
vehVel = sqrt(2.0 * engKinPre / fzg_scalar->vehMas);
// printf("vehVel: %4.3f\n", vehVel);
/* % vorzeitiger Funktionsabbruch? */
/* premature function termination? */
/* Drehzahl der Kurbelwelle und Grenzen */
/* crankshaft speed and limits */
/* Aus den kinetischen Energien des Fahrzeugs wird über die Raddrehzahl */
/* und die übersetzung vom Getriebe die Kurbelwellendrehzahl berechnet. */
/* Zeilenrichtung entspricht den Gängen. (Zeilenvektor) */
/* from the vehicle's kinetic energy, the crankshaft speed is calculated */
/* by the speed and gearbox translation. Line direction corresponding to */
/* the aisles (row rector). EQUATION 1 */
// fzg_scalar->geaRat[] is returning 0!
b_engKinPre[0] = engKinPre;
b_engKinPre[1] = engKinAct;
for (k = 0; k < 2; k++) {
crsSpdVec[k] = fzg_array->geaRat[(int)gea - 1] * sqrt(2.0 * b_engKinPre[k] /
fzg_scalar->vehMas) / fzg_scalar->whlDrr;
// printf("crsSpdVec[%d]: %4.3f\n", k, crsSpdVec[k]);
// printf("(int)gea: %d\n", (int)gea);
// printf("(int)gea - 1: %d\n", (int)gea - 1);
// printf("fzg_array->geaRat[(int)gea - 1]: %4.3f\n", fzg_array->geaRat[(int)gea - 1]);
// printf("fzg_array->geaRat[5]: %4.3f\n", fzg_array->geaRat[5]);
}
/* Abbruch, wenn die Drehzahlen der Kurbelwelle zu hoch im hybridischen */
/* Modus */
/* stop if the crankshaft rotatoinal speed is too high in hybrid mode */
y = false;
k = 0;
exitg3 = false;
while ((!exitg3) && (k < 2)) {
// printf("crsSpdVec[%d]: %4.3f\n", k, crsSpdVec[k]);
// printf("crsSpdHybMax: %4.3f\n", crsSpdHybMax);
if (!!(crsSpdVec[k] > crsSpdHybMax)) {
y = true;
exitg3 = true;
} else {
k++;
}
}
// printf("STATUS OF Y(1): %d\n", y);
// if (!y)
// printf("11111111111111111111111111111111111111111111111111111111111\n");
if (y) {
} else {
/* Falls die Drehzahl des Verbrennungsmotors niedriger als die */
/* Leerlaufdrehzahl ist, */
/* stop if crankhaft rotional speed is lower than the idling speed */
y = false;
k = 0;
exitg2 = false;
while ((!exitg2) && (k < 2)) {
// printf("crsSpdVec[%d]: %4.3f\n", k, crsSpdVec[k]);
// printf("crsSpdHybMin: %4.3f\n", crsSpdHybMin);
if (!!(crsSpdVec[k] < crsSpdHybMin)) {
y = true;
exitg2 = true;
} else {
k++;
}
}
// printf("STATUS OF Y(2): %d\n", y);
// if (!y)
// printf("2222222222222222222222222222222222222222222222222222222222\n");
if (y) {
} else {
/* Prüfen, ob die Drehzahlgrenze des Elektromotors eingehalten wird */
/* check if electric motor speed limit is maintained */
/* mittlere Kurbelwellendrehzahlen berechnen */
/* calculate average crankshaft rotational speed */
/* - really just selecting the previous path_idx KE crankshaft speed */
crsSpd = crsSpdVec[0];
/* % Längsdynamik berechnen */
/* calculate longitundinal dynamics */
/* Es wird eine konstante Beschleunigung angenommen, die im Wegschritt */
/* wayStp das Fahrzeug von velPre auf velAct beschleunigt. */
/* constant acceleration assumed when transitioning from velPre to velAct */
/* for the selected wayStp path_idx step distance */
/* Berechnen der konstanten Beschleunigung */
/* calculate the constant acceleration */
/* Aus der mittleren kinetischen Energie im Intervall, der mittleren */
/* Steigung und dem Gang lässt sich über die Fahrwiderstandsgleichung */
/* die nötige Fahrwiderstandskraft berechnen, die aufgebracht werden */
/* muss, um diese zu realisieren. */
/* from the (avg) kinetic energy in the interval, the (avg) slope and */
/* transition can calculate the necessary traction force on the driving */
/* resistance equation (PART OF EQUATION 5) */
/* Steigungskraft aus der mittleren Steigung berechnen (Skalar) */
/* gradiant force from the calculated (average) gradient */
/* Rollreibungskraft berechnen (Skalar) */
/* calculated rolling friction force - not included in EQ 5??? */
/* Luftwiderstandskraft berechnen (2*c_a/m * E_kin) (Skalar) */
/* calculated air resistance force */
/* % Berechnung der minimalen kosten der Hamiltonfunktion */
/* Calculating the minimum cost of the Hamiltonian */
/* % Berechnen der Kraft am Rad für Antriebsstrangmodus */
/* calculate the force on the wheel for the drivetrain mode */
/* % dynamische Fahrzeugmasse bei Fahrzeugmotor an berechnen. Das */
/* % heißt es werden Trägheitsmoment von Verbrennungsmotor, */
/* % Elektromotor und Rädern mit einbezogen. */
/* calculate dynamic vehicle mass with the vehicle engine (with the moment */
/* of intertia of the ICE, electric motor, and wheels) */
/* vehMasDyn = (par.iceMoi_geaRat(gea) +... */
/* par.emoGeaMoi_geaRat(gea) + par.whlMoi)/par.whlDrr^2 ... */
/* + par.vehMas; */
/* Radkraft berechnen (Beschleunigungskraft + Steigungskraft + */
/* Rollwiderstandskraft + Luftwiderstandskraft) */
/* caluclating wheel forces (accerlation force + gradient force + rolling */
/* resistance + air resistance) EQUATION 5 */
/* % Getriebeübersetzung und -verlust */
/* gear ratio and loss */
/* Das Drehmoment des Rades ergibt sich über den Radhalbmesser aus */
/* der Fahrwiderstandskraft. */
/* the weel torque is obtained from the wheel radius of the rolling */
/* resistance force (torque = force * distance (in this case, radius) */
whlTrq = ((((engKinAct - engKinPre) / (fzg_scalar->vehMas * wayStp) *
fzg_scalar->vehMas + fzg_scalar->vehMas * 9.81 * sin(slp)) +
fzg_scalar->whlRosResCof * fzg_scalar->vehMas * 9.81 * cos(slp))
+ 2.0 * fzg_scalar->drgCof / fzg_scalar->vehMas * engKinPre) *
fzg_scalar->whlDrr;
// printf("whlTrq: %4.3f\n", whlTrq);
/* Berechnung des Kurbelwellenmoments */
/* Hier muss unterschieden werden, ob das Radmoment positiv oder */
/* negativ ist, da nur ein einfacher Wirkungsgrad für das Getriebe */
/* genutzt wird */
/* it's important to determine sign of crankshaft torque (positive or */
/* negative), since only a simple efficiency is used for the transmission */
/* PART OF EQ4 <- perhaps reversed? not too sure */
if (whlTrq < 0.0) {
crsTrq = whlTrq / fzg_array->geaRat[(int)gea - 1] * fzg_scalar->geaEfy;
} else {
crsTrq = whlTrq / fzg_array->geaRat[(int)gea - 1] / fzg_scalar->geaEfy;
}
// printf("crsTrq: %4.3f\n", crsTrq);
/* % Verbrennungsmotor */
/* internal combustion engine */
/* maximales Moment des Verbrennungsmotors berechnen */
/* calculate max torque of the engine (quadratic based on rotation speed) */
iceTrqMax = (fzg_array->iceTrqMaxCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
fzg_array->iceTrqMaxCof[1] * crsSpdVec[0]) +
fzg_array->iceTrqMaxCof[2];
/* minimales Moment des Verbrennungsmotors berechnen */
/* calculating mimimum ICE moment */
iceTrqMin = (fzg_array->iceTrqMinCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
fzg_array->iceTrqMinCof[1] * crsSpdVec[0]) +
fzg_array->iceTrqMinCof[2];
// printf("iceTrqMax: %4.3f\n", iceTrqMax);
// printf("iceTrqMin: %4.3f\n", iceTrqMin);
/* % Elektromotor */
/* electric motor */
/* maximales Moment, dass die E-Maschine liefern kann */
/* max torque that the electric motor can provide - from interpolation */
/* emoTrqMaxPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMax_emoSpd,crsSpd); */
// for (k = 0; k < 100; k++) {
// b_fzg_array[k] = fzg_array->emoSpdMgd[150 * k];
//// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
// }
for (k = 0; k < 100; k++) {
b_fzg_array[k] = fzg_array->emoSpdMgd[100 * k + k];
// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
}
emoTrqMaxPos = interp1q(b_fzg_array, fzg_array->emoTrqMax_emoSpd,
crsSpdVec[0]);
// printf("emoTrqMaxPos: %4.3f\n", emoTrqMaxPos);
/* Die gültigen Kurbelwellenmomente müssen kleiner sein als das */
/* Gesamtmoment von E-Motor und Verbrennungsmotor */
/* The valid crankshaft moments must be less than the total moment of the */
/* electric motor and the ICE.Otherwise, leave the function */
if (crsTrq > iceTrqMax + emoTrqMaxPos) {
// printf("\nWHOOPS! crsTrq is > iceTrqMax + emoTrqMaxPos\n");
} else {
/* % %% Optimaler Momentensplit - Minimierung der Hamiltonfunktion */
/* optimum torque split - minimizing the Hamiltonian */
/* Die Vorgehensweise ist ähnlich wie bei der ECMS. Es wird ein Vektor der */
/* möglichen Batterieenergieänderungen aufgestellt. Aus diesen lässt sich */
/* eine Batterieklemmleistung berechnen. Aus der über das */
/* Kurbelwellenmoment, ein Elektromotormoment berechnet werden kann. */
/* Über das geforderte Kurbelwellenmoment, kann für jedes Moment des */
/* Elektromotors ein Moment des Verbrennungsmotors gefunden werden. Für */
/* jedes Momentenpaar kann die Hamiltonfunktion berechnet werden. */
/* Ausgegeben wird der minimale Wert der Hamiltonfunktion und die */
/* durch das dabei verwendete Elektromotormoment verursachte */
/* Batterieladungsänderung. */
/* The procedure is similar to ECMS. It's based on a vector of possible */
/* battery energy changes, from which a battery terminal power can be */
/* calculated. */
/* From the crankshaft torque, an electric motor torque can be */
/* calculated, and an engine torque can be found for every electric motor */
/* moment. */
/* For every moment-pair the Hamiltonian can be calculated. It */
/* outputs the minimum Hamilotnian value and the battery charge change */
/* caused by the electric motor torque used. */
/* % Elektromotor - Aufstellen des Batterienergievektors */
/* electric motor - positioning the battery energy vectors */
if (batEngStp > 0.0) {
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* FUNCTION CALL */
/* Skalar - Wegschrittweite */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque allowed */
/* Skalar - max EM torque possible */
/* Skalar - Wegschrittweite */
/* Skalar - Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* Skalar - max EM torque possible */
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* BatEngDltClc Calculates the change in battery energy */
/* */
/* Erstellungsdatum der ersten Version 17.11.2015 - Stephan Uebel */
/* Berechnung der Verluste des Elektromotors bei voller rein elektrischer */
/* Fahrt (voller Lastpunktabsenkung) und bei voller Lastpunktanhebung */
/* Calculations of loss of electric motor at purely full electric */
/* Driving (full load point lowering) and at full load point raising */
/* */
/* Version vom 17.02.2016: Keine Einbeziehung von Rotationsmassen */
/* ^^ No inclusion of rotational masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* ^^ Zero-Order-Hold (no average velocities) */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing the function output (delta battery_energy min and max) */
/* % Elektromotor */
/* minimales Moment, dass die E-Maschine liefern kann */
/* minimum moment that the EM can provide (max is an input to function) */
/* emoTrqMinPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMin_emoSpd,crsSpd); */
// for (k = 0; k < 100; k++) {
// b_fzg_array[k] = fzg_array->emoSpdMgd[150 * k];
// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
// }
// for (k = 0; k < 100; k++)
// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
emoTrqMinPos = interp1q(b_fzg_array, fzg_array->emoTrqMin_emoSpd,
crsSpdVec[0]);
// printf("emoTrqMinPos: %4.3f\n", emoTrqMinPos);
/* % Verbrennungsmotor berechnen */
/* Durch EM zu lieferndes Kurbelwellenmoment */
/* crankshaft torque to be delivered by the electric motor (min and max) */
emoTrqMax = crsTrq - iceTrqMin;
emoTrqMin = crsTrq - iceTrqMax;
// printf("emoTrqMax: %4.3f\n", emoTrqMax);
// printf("emoTrqMin: %4.3f\n", emoTrqMin);
/* % Elektromotor berechnen */
/* calculate the electric motor */
if (emoTrqMaxPos < emoTrqMax) {
/* Moment des Elektromotors bei maximaler Entladung der Batterie */
/* EM torque at max battery discharge */
emoTrqMax = emoTrqMaxPos;
}
if (emoTrqMaxPos < emoTrqMin) {
/* Moment des Elektromotors bei minimaler Entladung der Batterie */
/* EM torque at min battery discharge */
emoTrqMin = emoTrqMaxPos;
}
if ((emoTrqMinPos >= emoTrqMax) || rtIsNaN(emoTrqMax)) {
emoTrqMax = emoTrqMinPos;
}
if ((emoTrqMinPos >= emoTrqMin) || rtIsNaN(emoTrqMin)) {
emoTrqMin = emoTrqMinPos;
}
/* % Berechnung der änderung der Batterieladung */
/* calculating the change in battery charge */
/* Interpolation der benötigten Batterieklemmleistung f�r das */
/* EM-Moment. Stellen die nicht definiert sind, werden mit inf */
/* ausgegeben. Positive Werte entsprechen entladen der Batterie. */
/* interpolating the required battery terminal power for the EM torque. */
/* Assign undefined values to inf. Positive values coresspond with battery */
/* discharge. */
/* batPwrMax = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMax) + batPwrAux; */
/* */
/* batPwrMin = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMin) + batPwrAux; */
batPwrMax = codegen_interp2(fzg_array->emoSpdMgd, fzg_array->emoTrqMgd,
fzg_array->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMax) +
batPwrAux;
// for (k = 0; k < 15000; k++)
// printf(" fzg_array->emoSpdMgd[%d]: %4.3f\n", k, fzg_array->emoSpdMgd[k]);
//
// for (k = 0; k < 15000; k++)
// printf(" fzg_array->emoTrqMgd[%d]: %4.3f\n", k, fzg_array->emoTrqMgd[k]);
//
// for (k = 0; k < 15000; k++)
// printf(" fzg_array->emoPwr_emoSpd_emoTrq[%d]: %4.3f\n", k, fzg_array->emoPwr_emoSpd_emoTrq[k]);
// printf(" crsSpdVec[0]: %4.3f\n", crsSpdVec[0]);
// printf(" emoTrqMax: %4.3f\n", emoTrqMax);
// printf("\n batPwrMax: %4.3f\n", batPwrMax);
batPwrMin = codegen_interp2(fzg_array->emoSpdMgd, fzg_array->emoTrqMgd,
fzg_array->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMin) +
batPwrAux;
// printf(" batPwrMin: %4.3f\n", batPwrMin);
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery max power is not above bat max bounds */
if (batPwrMax > fzg_scalar->batPwrMax) {
batPwrMax = fzg_scalar->batPwrMax;
}
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery min power is not below bat min bounds */
if (batPwrMin > fzg_scalar->batPwrMax) {
batPwrMin = fzg_scalar->batPwrMax;
}
/* Es kann vorkommen, dass mehr Leistung gespeist werden soll, als */
/* m�glich ist. */
/* double check that the max and min still remain within the other bounds */
if (batPwrMax < fzg_scalar->batPwrMin) {
batPwrMax = fzg_scalar->batPwrMin;
}
if (batPwrMin < fzg_scalar->batPwrMin) {
batPwrMin = fzg_scalar->batPwrMin;
}
/* Batteriespannung aus Kennkurve berechnen */
/* calculating battery voltage of characteristic curve - eq?-------------- */
batOcv = batEng * fzg_array->batOcvCof_batEng[0] +
fzg_array->batOcvCof_batEng[1];
/* FUNCTION CALL - min delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMin = batFrcClc_port(batPwrMax, vehVel, fzg_scalar->batRstDch,
fzg_scalar->batRstChr, batOcv) * wayStp;
/* <-multiply by delta_S */
/* FUNCTION CALL - max delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMax = batFrcClc_port(batPwrMin, vehVel, fzg_scalar->batRstDch,
fzg_scalar->batRstChr, batOcv) * wayStp;
/* Es werden 2 Fälle unterschieden: */
/* there are 2 different cases */
if ((batEngDltMin > 0.0) && (batEngDltMax > 0.0)) {
/* %% konventionelles Bremsen + Rekuperieren */
/* conventional brakes + recuperation */
/* */
/* set change in energy to discretized integer values w/ ceil and */
/* floor. This also helps for easy looping */
/* Konventionelles Bremsen wird ebenfalls untersucht. */
/* Hier liegt eventuell noch Beschleunigungspotential, da diese */
/* Zustandswechsel u.U. ausgeschlossen werden können. */
/* NOTE: u.U. = unter Ümständen = circumstances permitting */
/* convetional breaks also investigated. An accelerating potential */
/* is still possible, as these states can be excluded */
/* (circumstances permitting) <- ??? not sure what above means */
/* */
/* so if both min and max changes in battery energy are */
/* increasing, then set the delta min to zero */
batEngDltMinInx = 0.0;
batEngDltMaxInx = floor(batEngDltMax / batEngStp);
} else {
batEngDltMinInx = ceil(batEngDltMin / batEngStp);
batEngDltMaxInx = floor(batEngDltMax / batEngStp);
}
} else {
batEngDltMinInx = 0.0;
batEngDltMaxInx = 0.0;
}
/* you got a larger min chnage and a max change, you're out of bounds. Leave */
/* the function */
if (batEngDltMaxInx < batEngDltMinInx) {
} else {
/* % Schleife über alle Elektromotormomente */
/* now loop through all the electric-motor torques */
k = (int)(batEngDltMaxInx + (1.0 - batEngDltMinInx));
for (batEngDltInx = 0; batEngDltInx < k; batEngDltInx++) {
batEngDlt = (batEngDltMinInx + (double)batEngDltInx) * batEngStp;
/* open circuit voltage over each iteration */
batOcv = (batEng + batEngDlt) * fzg_array->batOcvCof_batEng[0] +
fzg_array->batOcvCof_batEng[1];
/* Skalar für die Batterieleistung in W */
/* Skalar Krafstoffkraft in N */
/* FUNCTION CALL */
/* Skalar für die Wegschrittweite in m, */
/* Skalar - vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergie�nderung, */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar f�r die Wegschrittweite in m */
/* vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergieänderung */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar f�r die Batterieleistung */
/* Skalar Kraftstoffkraft */
/* */
/* FULENGCLC Calculating fuel consumption */
/* Erstellungsdatum der ersten Version 04.09.2015 - Stephan Uebel */
/* */
/* Diese Funktion berechnet den Kraftstoffverbrauch für einen gegebenen */
/* Wegschritt der kinetischen Energie, der Batterieenergie und des */
/* Antriebsstrangzustands über dem Weg. */
/* this function calculates fuel consumption for a given route step of */
/* KE, the battery energy, and drivetrain state of the current path_idx */
/* */
/* Version vom 17.02.2016 : Rotationsmassen vernachlässigt */
/* ^^ neglected rotatary masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* */
/* version from 1.06.2016 - removed crsTrq calulations - they are already */
/* done in parent function (clcPMP_olHyb_tmp) and do not change w/ each */
/* iteration, making the caluclation here unnecessary */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Skalar - electric battery power (W) */
fulFrc = rtInf;
/* Skalar - fuel force intialization (N) */
/* % Batterie */
/* Batterieenergieänderung über dem Weg (Batteriekraft) */
/* Change in battery energy over the path_idx way (ie battery power) */
batFrc = batEngDlt / wayStp;
/* Fallunterscheidung, ob Batterie geladen oder entladen wird */
/* Case analysis - check if battery is charging or discharging. Set */
/* resistance accordingly */
/* elektrische Leistung des Elektromotors */
/* electric power from electric motor - DERIVATION? dunno */
/* innere Batterieleistung / internal batt power */
/* dissipat. Leist. / dissipated */
if (batFrc < 0.0) {
b_batFrc = fzg_scalar->batRstDch;
} else {
b_batFrc = fzg_scalar->batRstChr;
}
batPwr = (-batFrc * vehVel - batFrc * batFrc * (vehVel * vehVel) /
(batOcv * batOcv) * b_batFrc) - batPwrAux;
/* Nebenverbrauchlast / auxiliary power */
/* % Elektromotor */
/* Ermitteln des Kurbelwellenmoments durch EM aus Batterieleistung */
/* determine crankshaft torque cauesd by EM's battery power */
/* switching out codegen_interp2 for lininterp2-just might work! */
/* */
emoTrq = codegen_interp2(fzg_array->emoSpdMgd, fzg_array->emoPwrMgd,
fzg_array->emoTrq_emoSpd_emoPwr, crsSpd, batPwr);
/* emoTrq = lininterp2(par.emoSpdMgd(1,:), par.emoPwrMgd(:,1),... */
/* par.emoTrq_emoSpd_emoPwr',crsSpd,emoPwrEle); */
if (rtIsInf(emoTrq)) {
} else {
/* Grenzen des Elektromotors müssen hier nicht überprüft werden, da die */
/* Ladungsdeltas schon so gewählt wurden, dass das maximale Motormoment */
/* nicht überstiegen wird. */
/* Electric motor limits need not be checked here, since the charge deltas */
/* have been chosen so that the max torque is not exceeded. */
/* % Verbrennungsmotor */
/* Ermitteln des Kurbelwellenmoments durch den Verbrennungsmotor */
/* Determining the crankshaft moment from the ICE */
iceTrq = crsTrq - emoTrq;
/* Wenn das Verbrennungsmotormoment kleiner als das minimale */
/* Moment ist, ist dieser in der Schubabschaltung. Das */
/* verbleibende Moment liefern die Bremsen */
/* If engine torque is less than the min torque, fuel is cut. The */
/* remaining torque is deliver the brakes. */
if (iceTrq < iceTrqMin) {
fulPwr = 0.0;
} else if (iceTrq > iceTrqMax) {
fulPwr = rtInf;
} else {
/* replacing another coden_interp2 */
fulPwr = codegen_interp2(fzg_array->iceSpdMgd,
fzg_array->iceTrqMgd, fzg_array->iceFulPwr_iceSpd_iceTrq,
crsSpd, iceTrq);
/* fulPwr = lininterp2(par.iceSpdMgd(1,:), par.iceTrqMgd(:,1), ... */
/* par.iceFulPwr_iceSpd_iceTrq', crsSpd, iceTrq); */
}
/* Berechnen der Kraftstoffkraft */
/* calculate fuel force */
fulFrc = fulPwr / vehVel;
/* Berechnen der Kraftstoffvolumenänderung */
/* caluclate change in fuel volume - energy, no? */
/* % Ende der Funktion */
}
/* FUNCTION CALL */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open circuit voltage */
batFrc = batFrcClc_port(batPwr, vehVel, fzg_scalar->batRstDch,
fzg_scalar->batRstChr, batOcv);
/* %% Hamiltonfunktion bestimmen */
/* determine the hamiltonian */
/* Eq (29b) */
crsSpdVec[0] = (fulFrc + psiBatEng * batFrc) + psiTim / vehVel;
ixstart = 1;
mtmp = crsSpdVec[0];
itmp = 1;
if (rtIsNaN(crsSpdVec[0])) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 3)) {
ixstart = 2;
if (!rtIsNaN(*cosHamMin)) {
mtmp = *cosHamMin;
itmp = 2;
exitg1 = true;
} else {
ix = 3;
}
}
}
if ((ixstart < 2) && (*cosHamMin < mtmp)) {
mtmp = *cosHamMin;
itmp = 2;
}
*cosHamMin = mtmp;
/* Wenn der aktuelle Punkt besser ist, als der in cosHamMin */
/* gespeicherte Wert, werden die Ausgabegrößen neu beschrieben. */
/* if the current point is better than the stored cosHamMin value, */
/* then the output values are rewritten (selected prev. value is = 2) */
if (itmp == 1) {
*batFrcOut = batFrc;
*fulFrcOut = fulFrc;
}
}
}
}
}
}
// printf(" *cosHamMin (func): %4.3f\n", *cosHamMin);
// printf(" *batFrcOut (func): %4.3f\n", *batFrcOut);
// printf(" *fulFrcOut (func): %4.3f\n", *fulFrcOut);
/* end of function */
}
/* Model output function */
void udpRead_output(void)
{
char_T *sErr;
int32_T samplesRead;
boolean_T rtb_Compare;
int32_T s5_iter;
real_T tmp;
int32_T exitg1;
int32_T exitg2;
/* Reset subsysRan breadcrumbs */
srClearBC(udpRead_DW.ForIteratorSubsystem_SubsysRanB);
/* Reset subsysRan breadcrumbs */
srClearBC(udpRead_DW.ForIteratorSubsystem1_SubsysRan);
/* Reset subsysRan breadcrumbs */
srClearBC(udpRead_DW.EnabledSubsystem_SubsysRanBC);
/* S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
sErr = GetErrorBuffer(&udpRead_DW.UDPReceive_NetworkLib[0U]);
samplesRead = 31;
LibOutputs_Network(&udpRead_DW.UDPReceive_NetworkLib[0U],
&udpRead_B.UDPReceive_o1[0U], &samplesRead);
if (*sErr != 0) {
rtmSetErrorStatus(udpRead_M, sErr);
rtmSetStopRequested(udpRead_M, 1);
}
udpRead_B.UDPReceive_o2 = (uint16_T)samplesRead;
/* End of S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
/* RelationalOperator: '<S1>/Compare' incorporates:
* Constant: '<S1>/Constant'
*/
rtb_Compare = (udpRead_B.UDPReceive_o2 > udpRead_P.Constant_Value_p);
/* Outputs for Enabled SubSystem: '<Root>/Enabled Subsystem' incorporates:
* EnablePort: '<S2>/Enable'
*/
if (rtb_Compare) {
if (!udpRead_DW.EnabledSubsystem_MODE) {
udpRead_DW.EnabledSubsystem_MODE = true;
}
/* Outputs for Iterator SubSystem: '<S2>/For Iterator Subsystem1' incorporates:
* ForIterator: '<S5>/For Iterator'
*/
/* Selector: '<S5>/Selector' incorporates:
* Selector: '<S5>/Selector1'
* Selector: '<S5>/Selector2'
*/
s5_iter = 24;
do {
exitg2 = 0;
if (udpRead_P.Constant1_Value < 0.0) {
tmp = ceil(udpRead_P.Constant1_Value);
} else {
tmp = floor(udpRead_P.Constant1_Value);
}
if (rtIsNaN(tmp) || rtIsInf(tmp)) {
tmp = 0.0;
} else {
tmp = fmod(tmp, 4.294967296E+9);
}
if (s5_iter - 23 <= (tmp < 0.0 ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp)) {
udpRead_B.Selector = udpRead_B.UDPReceive_o1[s5_iter];
udpRead_B.Selector1 = udpRead_B.UDPReceive_o1[s5_iter + 2];
udpRead_B.Selector2 = udpRead_B.UDPReceive_o1[s5_iter + 4];
srUpdateBC(udpRead_DW.ForIteratorSubsystem1_SubsysRan);
s5_iter++;
} else {
exitg2 = 1;
}
} while (exitg2 == 0);
/* End of Outputs for SubSystem: '<S2>/For Iterator Subsystem1' */
/* Outputs for Iterator SubSystem: '<S2>/For Iterator Subsystem' incorporates:
* ForIterator: '<S4>/For Iterator'
*/
s5_iter = 1;
do {
exitg1 = 0;
if (udpRead_P.Constant_Value < 0.0) {
tmp = ceil(udpRead_P.Constant_Value);
} else {
tmp = floor(udpRead_P.Constant_Value);
}
if (rtIsNaN(tmp) || rtIsInf(tmp)) {
tmp = 0.0;
} else {
tmp = fmod(tmp, 4.294967296E+9);
}
if (s5_iter <= (tmp < 0.0 ? -(int32_T)(uint32_T)-tmp : (int32_T)(uint32_T)
tmp)) {
udpRead_B.Selector_d = udpRead_B.UDPReceive_o1[s5_iter - 1];
udpRead_B.Selector1_f = udpRead_B.UDPReceive_o1[s5_iter + 3];
udpRead_B.Selector2_f = udpRead_B.UDPReceive_o1[s5_iter + 7];
udpRead_B.Compare = (s5_iter == udpRead_P.CompareToConstant_const);
udpRead_B.Selector3 = udpRead_B.UDPReceive_o1[s5_iter + 11];
udpRead_B.Selector4 = udpRead_B.UDPReceive_o1[s5_iter + 15];
udpRead_B.Selector5 = udpRead_B.UDPReceive_o1[s5_iter + 19];
srUpdateBC(udpRead_DW.ForIteratorSubsystem_SubsysRanB);
s5_iter++;
} else {
exitg1 = 1;
}
} while (exitg1 == 0);
/* End of Outputs for SubSystem: '<S2>/For Iterator Subsystem' */
srUpdateBC(udpRead_DW.EnabledSubsystem_SubsysRanBC);
} else {
if (udpRead_DW.EnabledSubsystem_MODE) {
udpRead_DW.EnabledSubsystem_MODE = false;
}
}
/* End of Outputs for SubSystem: '<Root>/Enabled Subsystem' */
/* Switch: '<S3>/Init' incorporates:
* Constant: '<S3>/Initial Condition'
* Logic: '<S3>/FixPt Logical Operator'
* UnitDelay: '<S3>/FixPt Unit Delay1'
* UnitDelay: '<S3>/FixPt Unit Delay2'
*/
if (rtb_Compare || (udpRead_DW.FixPtUnitDelay2_DSTATE != 0)) {
udpRead_B.Init = udpRead_P.UnitDelayResettable_vinit;
} else {
udpRead_B.Init = udpRead_DW.FixPtUnitDelay1_DSTATE;
}
/* End of Switch: '<S3>/Init' */
/* Switch: '<S3>/Reset' incorporates:
* Constant: '<S3>/Initial Condition'
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Sum'
*/
if (rtb_Compare) {
udpRead_B.Xnew = udpRead_P.UnitDelayResettable_vinit;
} else {
udpRead_B.Xnew = 1.0F + udpRead_B.Init;
}
/* End of Switch: '<S3>/Reset' */
}
/* Model output function */
void testArduino_send_serial_output(void)
{
uint16_T rtb_AnalogInput_0;
uint16_T rtb_AnalogInput1_0;
uint16_T rtb_AnalogInput2_0;
uint8_T tmp[3];
real_T u;
real_T v;
if (testArduino_send_serial_M->Timing.TaskCounters.TID[1] == 0) {
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input' */
rtb_AnalogInput_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput_p1);
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input1' */
rtb_AnalogInput1_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput1_p1);
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input2' */
rtb_AnalogInput2_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput2_p1);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput_0 -
testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[0] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input1'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput1_0
- testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[1] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input2'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput2_0
- testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[2] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
Serial_write(testArduino_send_serial_P.SerialTransmit_portNumber, tmp, 3UL);
}
}
/* Function Definitions */
static boolean_T b_eml_relop(real_T a, const creal_T b, boolean_T safe_eq)
{
boolean_T p;
real_T x;
real_T b_a;
real_T b_b;
boolean_T guard1 = FALSE;
boolean_T guard2 = FALSE;
boolean_T guard3 = FALSE;
int32_T exponent;
int32_T b_exponent;
int32_T c_exponent;
if ((fabs(a) > 8.9884656743115785E+307) || (fabs(b.re) >
8.9884656743115785E+307) || (fabs(b.im) > 8.9884656743115785E+307)) {
x = fabs(a) / 2.0;
b_a = fabs(b.re / 2.0);
b_b = fabs(b.im / 2.0);
if (b_a < b_b) {
b_a /= b_b;
b_b *= sqrt(b_a * b_a + 1.0);
} else if (b_a > b_b) {
b_b /= b_a;
b_b = sqrt(b_b * b_b + 1.0) * b_a;
} else if (rtIsNaN(b_b)) {
} else {
b_b = b_a * 1.4142135623730951;
}
} else {
x = fabs(a);
b_a = fabs(b.re);
b_b = fabs(b.im);
if (b_a < b_b) {
b_a /= b_b;
b_b *= sqrt(b_a * b_a + 1.0);
} else if (b_a > b_b) {
b_b /= b_a;
b_b = sqrt(b_b * b_b + 1.0) * b_a;
} else if (rtIsNaN(b_b)) {
} else {
b_b = b_a * 1.4142135623730951;
}
}
guard1 = FALSE;
guard2 = FALSE;
guard3 = FALSE;
if ((!safe_eq) && (x == b_b)) {
guard3 = TRUE;
} else {
if (safe_eq) {
b_a = fabs(b_b / 2.0);
if ((!rtIsInf(b_a)) && (!rtIsNaN(b_a))) {
if (b_a <= 2.2250738585072014E-308) {
b_a = 4.94065645841247E-324;
} else {
frexp(b_a, &exponent);
b_a = ldexp(1.0, exponent - 53);
}
} else {
b_a = rtNaN;
}
if ((fabs(b_b - x) < b_a) || (rtIsInf(x) && rtIsInf(b_b) && ((x > 0.0) ==
(b_b > 0.0)))) {
p = TRUE;
} else {
p = FALSE;
}
if (p) {
guard3 = TRUE;
}
}
}
if (guard3 == TRUE) {
x = rt_atan2d_snf(0.0, a);
b_b = rt_atan2d_snf(b.im, b.re);
if ((!safe_eq) && (x == b_b)) {
guard2 = TRUE;
} else {
if (safe_eq) {
b_a = fabs(b_b / 2.0);
if ((!rtIsInf(b_a)) && (!rtIsNaN(b_a))) {
if (b_a <= 2.2250738585072014E-308) {
b_a = 4.94065645841247E-324;
} else {
frexp(b_a, &b_exponent);
b_a = ldexp(1.0, b_exponent - 53);
}
} else {
b_a = rtNaN;
}
if ((fabs(b_b - x) < b_a) || (rtIsInf(x) && rtIsInf(b_b) && ((x > 0.0) ==
(b_b > 0.0)))) {
p = TRUE;
} else {
p = FALSE;
}
if (p) {
guard2 = TRUE;
}
}
}
}
if (guard2 == TRUE) {
x = fabs(a);
b_b = fabs(b.re);
if ((!safe_eq) && (x == b_b)) {
guard1 = TRUE;
} else {
if (safe_eq) {
b_a = b_b / 2.0;
if ((!rtIsInf(b_a)) && (!rtIsNaN(b_a))) {
if (b_a <= 2.2250738585072014E-308) {
b_a = 4.94065645841247E-324;
} else {
frexp(b_a, &c_exponent);
b_a = ldexp(1.0, c_exponent - 53);
}
} else {
b_a = rtNaN;
}
if ((fabs(b_b - x) < b_a) || (rtIsInf(x) && rtIsInf(b_b) && ((x > 0.0) ==
(b_b > 0.0)))) {
p = TRUE;
} else {
p = FALSE;
}
if (p) {
guard1 = TRUE;
}
}
}
}
if (guard1 == TRUE) {
x = 0.0;
b_b = 0.0;
}
return x < b_b;
}
