static void sf_c2_MigrationOWBG_Proto4_1DLinear2DNonLinear
(SFc2_MigrationOWBG_Proto4_1DLinear2DNonLinearInstanceStruct *chartInstance)
{
real_T c2_hoistedGlobal;
real_T c2_b_hoistedGlobal;
real_T c2_x;
real_T c2_y;
uint32_T c2_debug_family_var_map[5];
real_T c2_nargin = 2.0;
real_T c2_nargout = 1.0;
real_T c2_z;
real_T c2_b_x;
real_T c2_c_x;
real_T *c2_d_x;
real_T *c2_b_y;
real_T *c2_b_z;
c2_b_z = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c2_b_y = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c2_d_x = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 0U, chartInstance->c2_sfEvent);
_SFD_DATA_RANGE_CHECK(*c2_d_x, 0U);
_SFD_DATA_RANGE_CHECK(*c2_b_y, 1U);
_SFD_DATA_RANGE_CHECK(*c2_b_z, 2U);
chartInstance->c2_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 0U, chartInstance->c2_sfEvent);
c2_hoistedGlobal = *c2_d_x;
c2_b_hoistedGlobal = *c2_b_y;
c2_x = c2_hoistedGlobal;
c2_y = c2_b_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 5U, 5U, c2_debug_family_names,
c2_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c2_nargin, 0U, c2_sf_marshallOut,
c2_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c2_nargout, 1U, c2_sf_marshallOut,
c2_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c2_x, 2U, c2_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c2_y, 3U, c2_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c2_z, 4U, c2_sf_marshallOut,
c2_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 3);
c2_b_x = (45.96 - c2_mpower(chartInstance, c2_y)) - c2_mpower(chartInstance,
c2_x);
c2_z = c2_b_x;
if (c2_z < 0.0) {
c2_eml_error(chartInstance);
}
c2_c_x = c2_z;
c2_z = c2_c_x;
c2_z = muDoubleScalarSqrt(c2_z);
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, -3);
_SFD_SYMBOL_SCOPE_POP();
*c2_b_z = c2_z;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 0U, chartInstance->c2_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY
(_MigrationOWBG_Proto4_1DLinear2DNonLinearMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static real_T c3_norm(SFc3_ErdMondInstanceStruct *chartInstance, real_T c3_x[2])
{
real_T c3_y;
real_T c3_scale;
int32_T c3_k;
int32_T c3_b_k;
real_T c3_b_x;
real_T c3_c_x;
real_T c3_absxk;
real_T c3_t;
c3_y = 0.0;
c3_scale = 2.2250738585072014E-308;
c3_eml_int_forloop_overflow_check(chartInstance);
for (c3_k = 1; c3_k < 3; c3_k++) {
c3_b_k = c3_k;
c3_b_x = c3_x[_SFD_EML_ARRAY_BOUNDS_CHECK("", (int32_T)_SFD_INTEGER_CHECK("",
(real_T)c3_b_k), 1, 2, 1, 0) - 1];
c3_c_x = c3_b_x;
c3_absxk = muDoubleScalarAbs(c3_c_x);
if (c3_absxk > c3_scale) {
c3_t = c3_scale / c3_absxk;
c3_y = 1.0 + c3_y * c3_t * c3_t;
c3_scale = c3_absxk;
} else {
c3_t = c3_absxk / c3_scale;
c3_y += c3_t * c3_t;
}
}
return c3_scale * muDoubleScalarSqrt(c3_y);
}
void c_sqrt(const emlrtStack *sp, real_T *x)
{
emlrtStack st;
st.prev = sp;
st.tls = sp->tls;
if (*x < 0.0) {
st.site = &f_emlrtRSI;
eml_error(&st);
}
*x = muDoubleScalarSqrt(*x);
}
void b_sqrt(real_T x[1000000])
{
int32_T k;
for (k = 0; k < 1000000; k++) {
if (x[k] < 0.0) {
emlrtPushRtStackR2012b(&y_emlrtRSI, emlrtRootTLSGlobal);
eml_error();
emlrtPopRtStackR2012b(&y_emlrtRSI, emlrtRootTLSGlobal);
}
}
for (k = 0; k < 1000000; k++) {
x[k] = muDoubleScalarSqrt(x[k]);
}
}
void c_sqrt(const emlrtStack *sp, emxArray_real_T *x)
{
boolean_T p;
int32_T nx;
int32_T k;
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
p = false;
nx = x->size[0] * x->size[1];
for (k = 0; k < nx; k++) {
if (p || (x->data[k] < 0.0)) {
p = true;
} else {
p = false;
}
}
if (p) {
st.site = &h_emlrtRSI;
error(&st);
}
st.site = &i_emlrtRSI;
nx = x->size[0] * x->size[1];
b_st.site = &j_emlrtRSI;
if ((!(1 > nx)) && (nx > 2147483646)) {
c_st.site = &k_emlrtRSI;
check_forloop_overflow_error(&c_st);
}
for (k = 0; k + 1 <= nx; k++) {
x->data[k] = muDoubleScalarSqrt(x->data[k]);
}
}
/* Function Definitions */
real_T norm(const real_T x[3])
{
real_T y;
real_T scale;
int32_T k;
real_T absxk;
real_T t;
y = 0.0;
scale = 3.3121686421112381E-170;
for (k = 0; k < 3; k++) {
absxk = muDoubleScalarAbs(x[k]);
if (absxk > scale) {
t = scale / absxk;
y = 1.0 + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
}
return scale * muDoubleScalarSqrt(y);
}
/* Function Definitions */
real_T norm(const real_T x[4])
{
real_T y;
real_T scale;
int32_T k;
real_T absxk;
real_T t;
y = 0.0;
scale = 2.2250738585072014E-308;
for (k = 0; k < 4; k++) {
absxk = muDoubleScalarAbs(x[k]);
if (absxk > scale) {
t = scale / absxk;
y = 1.0 + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
}
return scale * muDoubleScalarSqrt(y);
}
/* Function Definitions */
void MechanicalPointForce(const emlrtStack *sp, const emxArray_real_T
*particlePosition, const emxArray_real_T *pointSourcePosition, real_T
forceDirection, real_T forceMagnitude, real_T cutoff, emxArray_real_T *force)
{
uint32_T sz[2];
int32_T ix;
emxArray_real_T *forceTemp;
int32_T loop_ub;
emxArray_real_T *forceMag;
int32_T vlen;
int32_T sIdx;
emxArray_real_T *forceDir;
emxArray_real_T *distToSource;
emxArray_int32_T *r0;
emxArray_boolean_T *r1;
emxArray_int32_T *r2;
emxArray_real_T *x;
emxArray_real_T *b_x;
emxArray_real_T *r3;
emxArray_real_T *r4;
emxArray_real_T *b_pointSourcePosition;
emxArray_real_T *b_forceDir;
emxArray_real_T *c_forceDir;
int32_T k;
int32_T vstride;
int32_T iy;
int32_T ixstart;
boolean_T overflow;
real_T s;
boolean_T b0;
uint32_T varargin_2[2];
boolean_T p;
boolean_T exitg1;
int32_T iv0[1];
int32_T iv1[2];
int32_T b_force[2];
int32_T iv2[1];
int32_T b_iy;
int32_T c_iy;
int32_T b_forceTemp[2];
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
emlrtStack e_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &c_st;
d_st.tls = c_st.tls;
e_st.prev = &d_st;
e_st.tls = d_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
/* apply mechanical (push or pull) force on particles */
/* mechanicalForce is a logical flag */
/* particlPosition is a N by 3 vector of particle position */
/* pointSourcePosition is the position of force sources */
/* forceDirection is either -1 for 'in' or 1 for 'out' */
/* forceMagnitude is a positive number between 0 and 1 */
/* cutoff is the maximal direction the force operates on particle relative */
/* to the pointSourcePosition */
/* the output is a vector of N by 3 of delta position to th */
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)particlePosition->size[ix];
}
emxInit_real_T(sp, &forceTemp, 2, &c_emlrtRTEI, true);
ix = forceTemp->size[0] * forceTemp->size[1];
forceTemp->size[0] = (int32_T)sz[0];
emxEnsureCapacity(sp, (emxArray__common *)forceTemp, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
ix = forceTemp->size[0] * forceTemp->size[1];
forceTemp->size[1] = (int32_T)sz[1];
emxEnsureCapacity(sp, (emxArray__common *)forceTemp, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = (int32_T)sz[0] * (int32_T)sz[1];
for (ix = 0; ix < loop_ub; ix++) {
forceTemp->data[ix] = 0.0;
}
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)particlePosition->size[ix];
}
ix = force->size[0] * force->size[1];
force->size[0] = (int32_T)sz[0];
emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
ix = force->size[0] * force->size[1];
force->size[1] = (int32_T)sz[1];
emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = (int32_T)sz[0] * (int32_T)sz[1];
for (ix = 0; ix < loop_ub; ix++) {
force->data[ix] = 0.0;
}
emxInit_real_T(sp, &forceMag, 2, &d_emlrtRTEI, true);
vlen = particlePosition->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
vlen = particlePosition->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[1] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = particlePosition->size[0] * particlePosition->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceMag->data[ix] = 0.0;
}
sIdx = 0;
emxInit_real_T(sp, &forceDir, 2, &e_emlrtRTEI, true);
b_emxInit_real_T(sp, &distToSource, 1, &f_emlrtRTEI, true);
emxInit_int32_T(sp, &r0, 1, &emlrtRTEI, true);
emxInit_boolean_T(sp, &r1, 2, &emlrtRTEI, true);
emxInit_int32_T(sp, &r2, 1, &emlrtRTEI, true);
emxInit_real_T(sp, &x, 2, &emlrtRTEI, true);
b_emxInit_real_T(sp, &b_x, 1, &emlrtRTEI, true);
b_emxInit_real_T(sp, &r3, 1, &emlrtRTEI, true);
b_emxInit_real_T(sp, &r4, 1, &emlrtRTEI, true);
emxInit_real_T(sp, &b_pointSourcePosition, 2, &emlrtRTEI, true);
b_emxInit_real_T(sp, &b_forceDir, 1, &emlrtRTEI, true);
emxInit_real_T(sp, &c_forceDir, 2, &emlrtRTEI, true);
while (sIdx <= pointSourcePosition->size[0] - 1) {
loop_ub = pointSourcePosition->size[1];
ix = pointSourcePosition->size[0];
if ((sIdx + 1 >= 1) && (sIdx + 1 < ix)) {
vlen = sIdx + 1;
} else {
vlen = emlrtDynamicBoundsCheckR2012b(sIdx + 1, 1, ix, (emlrtBCInfo *)
&e_emlrtBCI, sp);
}
ix = b_pointSourcePosition->size[0] * b_pointSourcePosition->size[1];
b_pointSourcePosition->size[0] = 1;
b_pointSourcePosition->size[1] = loop_ub;
emxEnsureCapacity(sp, (emxArray__common *)b_pointSourcePosition, ix,
(int32_T)sizeof(real_T), &emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
b_pointSourcePosition->data[b_pointSourcePosition->size[0] * ix] =
pointSourcePosition->data[(vlen + pointSourcePosition->size[0] * ix) - 1];
}
st.site = &emlrtRSI;
bsxfun(&st, particlePosition, b_pointSourcePosition, forceDir);
/* Find the distance between the particles and the source */
st.site = &b_emlrtRSI;
b_st.site = &h_emlrtRSI;
c_st.site = &i_emlrtRSI;
d_st.site = &j_emlrtRSI;
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)forceDir->size[ix];
}
ix = x->size[0] * x->size[1];
x->size[0] = (int32_T)sz[0];
x->size[1] = (int32_T)sz[1];
emxEnsureCapacity(&d_st, (emxArray__common *)x, ix, (int32_T)sizeof(real_T),
&b_emlrtRTEI);
if (dimagree(x, forceDir)) {
} else {
emlrtErrorWithMessageIdR2012b(&d_st, &b_emlrtRTEI, "MATLAB:dimagree", 0);
}
ix = (int32_T)sz[0] * (int32_T)sz[1];
for (k = 0; k < ix; k++) {
x->data[k] = forceDir->data[k] * forceDir->data[k];
}
st.site = &b_emlrtRSI;
b_st.site = &k_emlrtRSI;
c_st.site = &l_emlrtRSI;
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)x->size[ix];
}
ix = b_x->size[0];
b_x->size[0] = (int32_T)sz[0];
emxEnsureCapacity(&c_st, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
if ((x->size[0] == 0) || (x->size[1] == 0)) {
ix = b_x->size[0];
b_x->size[0] = (int32_T)sz[0];
emxEnsureCapacity(&c_st, (emxArray__common *)b_x, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = (int32_T)sz[0];
for (ix = 0; ix < loop_ub; ix++) {
b_x->data[ix] = 0.0;
}
} else {
vlen = x->size[1];
vstride = x->size[0];
iy = -1;
ixstart = -1;
d_st.site = &m_emlrtRSI;
overflow = (x->size[0] > 2147483646);
if (overflow) {
e_st.site = &g_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (loop_ub = 1; loop_ub <= vstride; loop_ub++) {
ixstart++;
ix = ixstart;
s = x->data[ixstart];
d_st.site = &n_emlrtRSI;
if (2 > vlen) {
b0 = false;
} else {
b0 = (vlen > 2147483646);
}
if (b0) {
e_st.site = &g_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (k = 2; k <= vlen; k++) {
ix += vstride;
s += x->data[ix];
}
iy++;
b_x->data[iy] = s;
}
}
st.site = &b_emlrtRSI;
ix = distToSource->size[0];
distToSource->size[0] = b_x->size[0];
emxEnsureCapacity(&st, (emxArray__common *)distToSource, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
distToSource->data[ix] = b_x->data[ix];
}
for (k = 0; k < b_x->size[0]; k++) {
if (b_x->data[k] < 0.0) {
b_st.site = &o_emlrtRSI;
eml_error(&b_st);
}
}
for (k = 0; k < b_x->size[0]; k++) {
distToSource->data[k] = muDoubleScalarSqrt(distToSource->data[k]);
}
/* Normalize the forceDirection */
iy = 0;
while (iy < 3) {
loop_ub = forceDir->size[0];
ix = r2->size[0];
r2->size[0] = loop_ub;
emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
r2->data[ix] = ix;
}
ix = forceDir->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&c_emlrtBCI,
sp);
st.site = &c_emlrtRSI;
ix = forceDir->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&d_emlrtBCI,
&st);
ix = forceDir->size[0];
sz[0] = (uint32_T)ix;
sz[1] = 1U;
varargin_2[0] = (uint32_T)distToSource->size[0];
varargin_2[1] = 1U;
overflow = false;
p = true;
k = 0;
exitg1 = false;
while ((!exitg1) && (k < 2)) {
if (!((int32_T)sz[k] == (int32_T)varargin_2[k])) {
p = false;
exitg1 = true;
} else {
k++;
}
}
if (!p) {
} else {
overflow = true;
}
if (overflow) {
} else {
emlrtErrorWithMessageIdR2012b(&st, &l_emlrtRTEI, "MATLAB:dimagree", 0);
}
loop_ub = forceDir->size[0];
ix = b_x->size[0];
b_x->size[0] = loop_ub;
emxEnsureCapacity(&st, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
b_x->data[ix] = forceDir->data[ix + forceDir->size[0] * iy] /
distToSource->data[ix];
}
iv0[0] = r2->size[0];
emlrtSubAssignSizeCheckR2012b(iv0, 1, *(int32_T (*)[1])b_x->size, 1,
(emlrtECInfo *)&d_emlrtECI, sp);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceDir->data[r2->data[ix] + forceDir->size[0] * iy] = b_x->data[ix];
}
/* bsxfun(@rdivide,forceDir,distToSource); */
iy++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
/* Multiply the */
if (forceDirection == -1.0) {
ix = r4->size[0];
r4->size[0] = distToSource->size[0];
emxEnsureCapacity(sp, (emxArray__common *)r4, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = distToSource->size[0];
for (ix = 0; ix < loop_ub; ix++) {
r4->data[ix] = 1.0 + distToSource->data[ix];
}
rdivide(sp, forceMagnitude, r4, b_x);
vlen = b_x->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceMag->data[ix] = 1.0 - b_x->data[ix];
}
} else {
if (forceDirection == 1.0) {
ix = r3->size[0];
r3->size[0] = distToSource->size[0];
emxEnsureCapacity(sp, (emxArray__common *)r3, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = distToSource->size[0];
for (ix = 0; ix < loop_ub; ix++) {
r3->data[ix] = 1.0 + distToSource->data[ix];
}
rdivide(sp, forceMagnitude, r3, b_x);
vlen = b_x->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceMag->data[ix] = b_x->data[ix];
}
}
}
iy = 0;
while (iy < 3) {
ix = forceDir->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&b_emlrtBCI,
sp);
ix = forceDir->size[0];
iv1[0] = ix;
iv1[1] = 1;
for (ix = 0; ix < 2; ix++) {
b_force[ix] = forceMag->size[ix];
}
if ((iv1[0] != b_force[0]) || (1 != b_force[1])) {
emlrtSizeEqCheckNDR2012b(iv1, b_force, (emlrtECInfo *)&c_emlrtECI, sp);
}
loop_ub = forceTemp->size[0];
ix = r2->size[0];
r2->size[0] = loop_ub;
emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
r2->data[ix] = ix;
}
ix = forceTemp->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&emlrtBCI, sp);
loop_ub = forceDir->size[0];
vlen = forceDir->size[0];
vstride = forceDir->size[0];
ix = b_forceDir->size[0];
b_forceDir->size[0] = vstride;
emxEnsureCapacity(sp, (emxArray__common *)b_forceDir, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
for (ix = 0; ix < vstride; ix++) {
b_forceDir->data[ix] = forceDir->data[ix + forceDir->size[0] * iy];
}
ix = c_forceDir->size[0] * c_forceDir->size[1];
c_forceDir->size[0] = loop_ub;
c_forceDir->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)c_forceDir, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
c_forceDir->data[ix] = b_forceDir->data[ix];
}
ix = b_x->size[0];
b_x->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
for (ix = 0; ix < vlen; ix++) {
b_x->data[ix] = c_forceDir->data[ix] * forceMag->data[ix];
}
iv2[0] = r2->size[0];
emlrtSubAssignSizeCheckR2012b(iv2, 1, *(int32_T (*)[1])b_x->size, 1,
(emlrtECInfo *)&b_emlrtECI, sp);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceTemp->data[r2->data[ix] + forceTemp->size[0] * iy] = b_x->data[ix];
}
/* bsxfun(@times,forceDir,forceTemp); */
iy++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
iy = distToSource->size[0] - 1;
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (distToSource->data[vstride] > cutoff) {
vlen++;
}
}
ix = r2->size[0];
r2->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (distToSource->data[vstride] > cutoff) {
r2->data[vlen] = vstride + 1;
vlen++;
}
}
loop_ub = forceTemp->size[1];
vstride = forceTemp->size[0];
vlen = r2->size[0];
for (ix = 0; ix < loop_ub; ix++) {
for (ixstart = 0; ixstart < vlen; ixstart++) {
iy = r2->data[ixstart];
if ((iy >= 1) && (iy < vstride)) {
b_iy = iy;
} else {
b_iy = emlrtDynamicBoundsCheckR2012b(iy, 1, vstride, (emlrtBCInfo *)
&f_emlrtBCI, sp);
}
forceTemp->data[(b_iy + forceTemp->size[0] * ix) - 1] = 0.0;
}
}
ix = r1->size[0] * r1->size[1];
r1->size[0] = forceTemp->size[0];
r1->size[1] = forceTemp->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r1, ix, (int32_T)sizeof(boolean_T),
&emlrtRTEI);
loop_ub = forceTemp->size[0] * forceTemp->size[1];
for (ix = 0; ix < loop_ub; ix++) {
r1->data[ix] = muDoubleScalarIsNaN(forceTemp->data[ix]);
}
iy = r1->size[0] * r1->size[1] - 1;
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (r1->data[vstride]) {
vlen++;
}
}
ix = r0->size[0];
r0->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)r0, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (r1->data[vstride]) {
r0->data[vlen] = vstride + 1;
vlen++;
}
}
vstride = forceTemp->size[0];
vlen = forceTemp->size[1];
loop_ub = r0->size[0];
for (ix = 0; ix < loop_ub; ix++) {
ixstart = vstride * vlen;
iy = r0->data[ix];
if ((iy >= 1) && (iy < ixstart)) {
c_iy = iy;
} else {
c_iy = emlrtDynamicBoundsCheckR2012b(iy, 1, ixstart, (emlrtBCInfo *)
&g_emlrtBCI, sp);
}
forceTemp->data[c_iy - 1] = 0.0;
}
for (ix = 0; ix < 2; ix++) {
b_force[ix] = force->size[ix];
}
for (ix = 0; ix < 2; ix++) {
b_forceTemp[ix] = forceTemp->size[ix];
}
if ((b_force[0] != b_forceTemp[0]) || (b_force[1] != b_forceTemp[1])) {
emlrtSizeEqCheckNDR2012b(b_force, b_forceTemp, (emlrtECInfo *)&emlrtECI,
sp);
}
ix = force->size[0] * force->size[1];
emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
vlen = force->size[0];
vstride = force->size[1];
loop_ub = vlen * vstride;
for (ix = 0; ix < loop_ub; ix++) {
force->data[ix] += forceTemp->data[ix];
}
sIdx++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
emxFree_real_T(&c_forceDir);
emxFree_real_T(&b_forceDir);
emxFree_real_T(&b_pointSourcePosition);
emxFree_real_T(&r4);
emxFree_real_T(&r3);
emxFree_real_T(&b_x);
emxFree_real_T(&x);
emxFree_int32_T(&r2);
emxFree_boolean_T(&r1);
emxFree_int32_T(&r0);
emxFree_real_T(&distToSource);
emxFree_real_T(&forceDir);
emxFree_real_T(&forceMag);
emxFree_real_T(&forceTemp);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
static void sf_c7_adcs_v15_integral_Power(void)
{
int32_T c7_i3;
int32_T c7_i4;
int32_T c7_previousEvent;
int32_T c7_i5;
real_T c7_x[7];
real_T c7_nargout = 1.0;
real_T c7_nargin = 1.0;
real_T c7_q[4];
real_T c7_xN[7];
int32_T c7_i6;
int32_T c7_i7;
real_T c7_A[4];
int32_T c7_i8;
real_T c7_a[4];
int32_T c7_i9;
real_T c7_b[4];
int32_T c7_i10;
real_T c7_b_a[4];
int32_T c7_i11;
real_T c7_b_b[4];
int32_T c7_i12;
real_T c7_b_x[4];
int32_T c7_i13;
real_T c7_y[4];
int32_T c7_i14;
real_T c7_c_x[4];
int32_T c7_i15;
real_T c7_b_y[4];
int32_T c7_i16;
real_T c7_d_x[4];
int32_T c7_i17;
real_T c7_c_y[4];
int32_T c7_i18;
real_T c7_e_x[4];
int32_T c7_i19;
real_T c7_d_y[4];
real_T c7_f_x;
real_T c7_B;
real_T c7_g_x;
int32_T c7_i20;
real_T c7_h_x[4];
real_T c7_e_y;
int32_T c7_i21;
real_T c7_i_x[4];
real_T c7_f_y;
int32_T c7_i22;
real_T c7_j_x[4];
real_T c7_g_y;
int32_T c7_i23;
int32_T c7_i24;
int32_T c7_i25;
int32_T c7_i26;
real_T (*c7_b_xN)[7];
real_T (*c7_k_x)[7];
c7_b_xN = (real_T (*)[7])ssGetOutputPortSignal(chartInstance.S, 1);
c7_k_x = (real_T (*)[7])ssGetInputPortSignal(chartInstance.S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance.S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG,5);
for (c7_i3 = 0; c7_i3 < 7; c7_i3 = c7_i3 + 1) {
_SFD_DATA_RANGE_CHECK((*c7_k_x)[c7_i3], 0U);
}
for (c7_i4 = 0; c7_i4 < 7; c7_i4 = c7_i4 + 1) {
_SFD_DATA_RANGE_CHECK((*c7_b_xN)[c7_i4], 1U);
}
c7_previousEvent = _sfEvent_;
_sfEvent_ = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG,5);
for (c7_i5 = 0; c7_i5 < 7; c7_i5 = c7_i5 + 1) {
c7_x[c7_i5] = (*c7_k_x)[c7_i5];
}
sf_debug_symbol_scope_push(5U, 0U);
sf_debug_symbol_scope_add("nargout", &c7_nargout, c7_c_sf_marshall);
sf_debug_symbol_scope_add("nargin", &c7_nargin, c7_c_sf_marshall);
sf_debug_symbol_scope_add("q", &c7_q, c7_b_sf_marshall);
sf_debug_symbol_scope_add("xN", &c7_xN, c7_sf_marshall);
sf_debug_symbol_scope_add("x", &c7_x, c7_sf_marshall);
CV_EML_FCN(0, 0);
/* This block gives DCM for converting from ECI to ORBIT frames */
/* inputs: state vector in ECI */
/* output: DCM */
_SFD_EML_CALL(0,6);
for (c7_i6 = 0; c7_i6 < 4; c7_i6 = c7_i6 + 1) {
c7_q[c7_i6] = c7_x[c7_i6];
}
_SFD_EML_CALL(0,7);
for (c7_i7 = 0; c7_i7 < 4; c7_i7 = c7_i7 + 1) {
c7_A[c7_i7] = c7_q[c7_i7];
}
for (c7_i8 = 0; c7_i8 < 4; c7_i8 = c7_i8 + 1) {
c7_a[c7_i8] = c7_q[c7_i8];
}
for (c7_i9 = 0; c7_i9 < 4; c7_i9 = c7_i9 + 1) {
c7_b[c7_i9] = c7_q[c7_i9];
}
c7_eml_scalar_eg();
for (c7_i10 = 0; c7_i10 < 4; c7_i10 = c7_i10 + 1) {
c7_b_a[c7_i10] = c7_a[c7_i10];
}
for (c7_i11 = 0; c7_i11 < 4; c7_i11 = c7_i11 + 1) {
c7_b_b[c7_i11] = c7_b[c7_i11];
}
for (c7_i12 = 0; c7_i12 < 4; c7_i12 = c7_i12 + 1) {
c7_b_x[c7_i12] = c7_b_a[c7_i12];
}
for (c7_i13 = 0; c7_i13 < 4; c7_i13 = c7_i13 + 1) {
c7_y[c7_i13] = c7_b_b[c7_i13];
}
for (c7_i14 = 0; c7_i14 < 4; c7_i14 = c7_i14 + 1) {
c7_c_x[c7_i14] = c7_b_x[c7_i14];
}
for (c7_i15 = 0; c7_i15 < 4; c7_i15 = c7_i15 + 1) {
c7_b_y[c7_i15] = c7_y[c7_i15];
}
for (c7_i16 = 0; c7_i16 < 4; c7_i16 = c7_i16 + 1) {
c7_d_x[c7_i16] = c7_c_x[c7_i16];
}
for (c7_i17 = 0; c7_i17 < 4; c7_i17 = c7_i17 + 1) {
c7_c_y[c7_i17] = c7_b_y[c7_i17];
}
for (c7_i18 = 0; c7_i18 < 4; c7_i18 = c7_i18 + 1) {
c7_e_x[c7_i18] = c7_d_x[c7_i18];
}
for (c7_i19 = 0; c7_i19 < 4; c7_i19 = c7_i19 + 1) {
c7_d_y[c7_i19] = c7_c_y[c7_i19];
}
c7_f_x = c7_ceval_xdot(4, c7_e_x, 1, 1, c7_d_y, 1, 1);
c7_B = c7_f_x;
if (c7_B < 0.0) {
c7_eml_error();
}
c7_g_x = c7_B;
c7_B = c7_g_x;
c7_B = muDoubleScalarSqrt(c7_B);
for (c7_i20 = 0; c7_i20 < 4; c7_i20 = c7_i20 + 1) {
c7_h_x[c7_i20] = c7_A[c7_i20];
}
c7_e_y = c7_B;
if (c7_e_y == 0.0) {
c7_eml_warning();
}
for (c7_i21 = 0; c7_i21 < 4; c7_i21 = c7_i21 + 1) {
c7_i_x[c7_i21] = c7_h_x[c7_i21];
}
c7_f_y = c7_e_y;
for (c7_i22 = 0; c7_i22 < 4; c7_i22 = c7_i22 + 1) {
c7_j_x[c7_i22] = c7_i_x[c7_i22];
}
c7_g_y = c7_f_y;
for (c7_i23 = 0; c7_i23 < 4; c7_i23 = c7_i23 + 1) {
c7_q[c7_i23] = c7_j_x[c7_i23] / c7_g_y;
}
_SFD_EML_CALL(0,8);
for (c7_i24 = 0; c7_i24 < 4; c7_i24 = c7_i24 + 1) {
c7_xN[c7_i24] = c7_q[c7_i24];
}
for (c7_i25 = 0; c7_i25 < 3; c7_i25 = c7_i25 + 1) {
c7_xN[c7_i25 + 4] = c7_x[c7_i25 + 4];
}
_SFD_EML_CALL(0,-8);
sf_debug_symbol_scope_pop();
for (c7_i26 = 0; c7_i26 < 7; c7_i26 = c7_i26 + 1) {
(*c7_b_xN)[c7_i26] = c7_xN[c7_i26];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG,5);
_sfEvent_ = c7_previousEvent;
sf_debug_check_for_state_inconsistency(_adcs_v15_integral_PowerMachineNumber_,
chartInstance.chartNumber, chartInstance.
instanceNumber);
}
static void sf_c4_MigrationBGOW_Proto6_MultiSwarm
(SFc4_MigrationBGOW_Proto6_MultiSwarmInstanceStruct *chartInstance)
{
real_T c4_hoistedGlobal;
real_T c4_b_hoistedGlobal;
real_T c4_x;
real_T c4_y;
uint32_T c4_debug_family_var_map[5];
real_T c4_nargin = 2.0;
real_T c4_nargout = 1.0;
real_T c4_z;
real_T c4_b_x;
real_T c4_c_x;
real_T *c4_d_x;
real_T *c4_b_y;
real_T *c4_b_z;
c4_b_z = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c4_b_y = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c4_d_x = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 3U, chartInstance->c4_sfEvent);
_SFD_DATA_RANGE_CHECK(*c4_d_x, 0U);
_SFD_DATA_RANGE_CHECK(*c4_b_y, 1U);
_SFD_DATA_RANGE_CHECK(*c4_b_z, 2U);
chartInstance->c4_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 3U, chartInstance->c4_sfEvent);
c4_hoistedGlobal = *c4_d_x;
c4_b_hoistedGlobal = *c4_b_y;
c4_x = c4_hoistedGlobal;
c4_y = c4_b_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 5U, 5U, c4_debug_family_names,
c4_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c4_nargin, 0U, c4_sf_marshallOut,
c4_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c4_nargout, 1U, c4_sf_marshallOut,
c4_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c4_x, 2U, c4_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c4_y, 3U, c4_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c4_z, 4U, c4_sf_marshallOut,
c4_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c4_sfEvent, 3);
c4_b_x = (26.4196 - c4_mpower(chartInstance, c4_x - 1.44)) - c4_mpower
(chartInstance, c4_y - 0.3677);
c4_z = c4_b_x;
if (c4_z < 0.0) {
c4_eml_error(chartInstance);
}
c4_c_x = c4_z;
c4_z = c4_c_x;
c4_z = muDoubleScalarSqrt(c4_z);
_SFD_EML_CALL(0U, chartInstance->c4_sfEvent, 4);
c4_z += 1.6;
_SFD_EML_CALL(0U, chartInstance->c4_sfEvent, -4);
_SFD_SYMBOL_SCOPE_POP();
*c4_b_z = c4_z;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 3U, chartInstance->c4_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY
(_MigrationBGOW_Proto6_MultiSwarmMachineNumber_, chartInstance->chartNumber,
chartInstance->instanceNumber);
}
static void sf_c1_my_demo_ref(SFc1_my_demo_refInstanceStruct *chartInstance)
{
int32_T c1_i0;
real_T c1_hoistedGlobal;
real_T c1_b_hoistedGlobal;
real_T c1_x;
real_T c1_y;
int32_T c1_i1;
real_T c1_fpath[170];
uint32_T c1_debug_family_var_map[13];
real_T c1_dmin;
real_T c1_xx[85];
real_T c1_yy[85];
real_T c1_d;
real_T c1_nargin = 3.0;
real_T c1_nargout = 3.0;
real_T c1_x_g;
real_T c1_y_g;
real_T c1_stop;
int32_T c1_i2;
int32_T c1_i3;
real_T c1_c_hoistedGlobal;
real_T c1_d_hoistedGlobal;
real_T c1_b_x;
real_T c1_c_x;
real_T *c1_b_x_g;
real_T *c1_b_y_g;
real_T *c1_d_x;
real_T *c1_b_y;
real_T *c1_b_stop;
real_T (*c1_b_fpath)[170];
boolean_T guard1 = FALSE;
boolean_T guard2 = FALSE;
c1_b_stop = (real_T *)ssGetOutputPortSignal(chartInstance->S, 3);
c1_b_fpath = (real_T (*)[170])ssGetInputPortSignal(chartInstance->S, 2);
c1_b_y = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c1_d_x = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
c1_b_y_g = (real_T *)ssGetOutputPortSignal(chartInstance->S, 2);
c1_b_x_g = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 0U, chartInstance->c1_sfEvent);
_SFD_DATA_RANGE_CHECK(*c1_b_x_g, 0U);
_SFD_DATA_RANGE_CHECK(*c1_b_y_g, 1U);
_SFD_DATA_RANGE_CHECK(*c1_d_x, 2U);
_SFD_DATA_RANGE_CHECK(*c1_b_y, 3U);
for (c1_i0 = 0; c1_i0 < 170; c1_i0++) {
_SFD_DATA_RANGE_CHECK((*c1_b_fpath)[c1_i0], 4U);
}
_SFD_DATA_RANGE_CHECK(*c1_b_stop, 5U);
chartInstance->c1_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 0U, chartInstance->c1_sfEvent);
c1_hoistedGlobal = *c1_d_x;
c1_b_hoistedGlobal = *c1_b_y;
c1_x = c1_hoistedGlobal;
c1_y = c1_b_hoistedGlobal;
for (c1_i1 = 0; c1_i1 < 170; c1_i1++) {
c1_fpath[c1_i1] = (*c1_b_fpath)[c1_i1];
}
sf_debug_symbol_scope_push_eml(0U, 13U, 13U, c1_debug_family_names,
c1_debug_family_var_map);
sf_debug_symbol_scope_add_eml_importable(&c1_dmin, 0U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(c1_xx, 1U, c1_d_sf_marshallOut,
c1_c_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(c1_yy, 2U, c1_d_sf_marshallOut,
c1_c_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c1_d, 3U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c1_nargin, 4U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c1_nargout, 5U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml(&c1_x, 6U, c1_b_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c1_y, 7U, c1_b_sf_marshallOut);
sf_debug_symbol_scope_add_eml(c1_fpath, 8U, c1_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(&c1_x_g, 9U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c1_y_g, 10U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c1_stop, 11U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&chartInstance->c1_i, 12U,
c1_sf_marshallOut, c1_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 6);
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 7);
if (CV_EML_IF(0, 1, 0, !chartInstance->c1_i_not_empty)) {
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 8);
chartInstance->c1_i = 1.0;
chartInstance->c1_i_not_empty = TRUE;
}
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 12);
c1_stop = 0.0;
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 13);
c1_dmin = 0.1;
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 26);
for (c1_i2 = 0; c1_i2 < 85; c1_i2++) {
c1_xx[c1_i2] = c1_fpath[c1_i2];
}
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 27);
for (c1_i3 = 0; c1_i3 < 85; c1_i3++) {
c1_yy[c1_i3] = c1_fpath[c1_i3 + 85];
}
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 29);
c1_c_hoistedGlobal = chartInstance->c1_i;
c1_d_hoistedGlobal = chartInstance->c1_i;
c1_b_x = c1_mpower(chartInstance, c1_x - c1_xx[_SFD_EML_ARRAY_BOUNDS_CHECK(
"xx", (int32_T)_SFD_INTEGER_CHECK("i", c1_c_hoistedGlobal), 1, 85, 1, 0) - 1])
+ c1_mpower(chartInstance, c1_y - c1_yy[_SFD_EML_ARRAY_BOUNDS_CHECK("yy",
(int32_T)_SFD_INTEGER_CHECK("i", c1_d_hoistedGlobal), 1, 85, 1, 0) - 1]);
c1_d = c1_b_x;
if (c1_d < 0.0) {
c1_eml_error(chartInstance);
}
c1_c_x = c1_d;
c1_d = c1_c_x;
c1_d = muDoubleScalarSqrt(c1_d);
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 31);
guard2 = FALSE;
if (CV_EML_COND(0, 1, 0, c1_d < c1_dmin)) {
if (CV_EML_COND(0, 1, 1, chartInstance->c1_i < 85.0)) {
CV_EML_MCDC(0, 1, 0, TRUE);
CV_EML_IF(0, 1, 1, TRUE);
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 32);
chartInstance->c1_i++;
} else {
guard2 = TRUE;
}
} else {
guard2 = TRUE;
}
if (guard2 == TRUE) {
CV_EML_MCDC(0, 1, 0, FALSE);
CV_EML_IF(0, 1, 1, FALSE);
}
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 36);
c1_x_g = c1_xx[_SFD_EML_ARRAY_BOUNDS_CHECK("xx", (int32_T)_SFD_INTEGER_CHECK(
"i", chartInstance->c1_i), 1, 85, 1, 0) - 1];
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 37);
c1_y_g = c1_yy[_SFD_EML_ARRAY_BOUNDS_CHECK("yy", (int32_T)_SFD_INTEGER_CHECK(
"i", chartInstance->c1_i), 1, 85, 1, 0) - 1];
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 40);
guard1 = FALSE;
if (CV_EML_COND(0, 1, 2, c1_d < c1_dmin)) {
if (CV_EML_COND(0, 1, 3, chartInstance->c1_i == 85.0)) {
CV_EML_MCDC(0, 1, 1, TRUE);
CV_EML_IF(0, 1, 2, TRUE);
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 41);
c1_stop = 1.0;
} else {
guard1 = TRUE;
}
} else {
guard1 = TRUE;
}
if (guard1 == TRUE) {
CV_EML_MCDC(0, 1, 1, FALSE);
CV_EML_IF(0, 1, 2, FALSE);
}
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, -41);
sf_debug_symbol_scope_pop();
*c1_b_x_g = c1_x_g;
*c1_b_y_g = c1_y_g;
*c1_b_stop = c1_stop;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 0U, chartInstance->c1_sfEvent);
sf_debug_check_for_state_inconsistency(_my_demo_refMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
void b_Acoeff(const emlrtStack *sp, real_T ksi, real_T j, const emxArray_real_T *
x, real_T t, const emxArray_real_T *gridT, emxArray_real_T *vals)
{
emxArray_real_T *b;
emxArray_real_T *r8;
int32_T b_x;
int32_T i;
emxArray_boolean_T *b_t;
real_T c_x;
emxArray_boolean_T *c_t;
emxArray_real_T *z0;
emxArray_real_T *d_x;
emxArray_real_T *e_x;
emxArray_real_T *r9;
int32_T b_b[2];
int32_T f_x[2];
emxArray_real_T *g_x;
emxArray_real_T *r10;
const mxArray *y;
static const int32_T iv16[2] = { 1, 45 };
const mxArray *m6;
char_T cv18[45];
static const char_T cv19[45] = { 'C', 'o', 'd', 'e', 'r', ':', 't', 'o', 'o',
'l', 'b', 'o', 'x', ':', 'm', 't', 'i', 'm', 'e', 's', '_', 'n', 'o', 'D',
'y', 'n', 'a', 'm', 'i', 'c', 'S', 'c', 'a', 'l', 'a', 'r', 'E', 'x', 'p',
'a', 'n', 's', 'i', 'o', 'n' };
const mxArray *b_y;
static const int32_T iv17[2] = { 1, 21 };
char_T cv20[21];
static const char_T cv21[21] = { 'C', 'o', 'd', 'e', 'r', ':', 'M', 'A', 'T',
'L', 'A', 'B', ':', 'i', 'n', 'n', 'e', 'r', 'd', 'i', 'm' };
emxArray_boolean_T *d_t;
real_T h_x;
emxArray_boolean_T *e_t;
emxArray_real_T *i_x;
emxArray_real_T *r11;
emxArray_real_T *j_x;
emxArray_real_T *r12;
emxArray_real_T *z1;
int32_T b_z0[2];
emxArray_real_T *c_z0;
emxArray_real_T *k_x;
const mxArray *c_y;
static const int32_T iv18[2] = { 1, 45 };
const mxArray *d_y;
static const int32_T iv19[2] = { 1, 21 };
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &b_st;
d_st.tls = b_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
emxInit_real_T(sp, &b, 2, &bb_emlrtRTEI, true);
emxInit_real_T(sp, &r8, 2, &bb_emlrtRTEI, true);
/* evaluate the coefficient A at the boundary ksi=0 or ksi=1; */
/* for the index j which describes the time steps timePoints_j, at time t and space */
/* point x */
/* timePoints is a vector describing the time descritized domain */
b_x = gridT->size[1];
i = (int32_T)emlrtIntegerCheckFastR2012b(j, &emlrtDCI, sp);
if (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &d_emlrtBCI,
sp) - 1]) {
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof(real_T),
&bb_emlrtRTEI);
vals->data[0] = 0.0;
} else {
emxInit_boolean_T(sp, &b_t, 2, &bb_emlrtRTEI, true);
b_x = b_t->size[0] * b_t->size[1];
b_t->size[0] = 1;
b_t->size[1] = 2 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)b_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)j;
b_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&e_emlrtBCI, sp) - 1]);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
b_t->data[b_t->size[0]] = (t <= gridT->
data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &f_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
b_t->data[b_t->size[0] * (b_x + 2)] = (x->data[x->size[0] * b_x] == ksi);
}
st.site = &pe_emlrtRSI;
if (all(&st, b_t)) {
b_x = gridT->size[1];
i = (int32_T)j;
emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &c_emlrtBCI, sp);
c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931;
st.site = &emlrtRSI;
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
vals->data[0] = muDoubleScalarSqrt(c_x);
} else {
emxInit_boolean_T(sp, &c_t, 2, &bb_emlrtRTEI, true);
b_x = c_t->size[0] * c_t->size[1];
c_t->size[0] = 1;
c_t->size[1] = 2 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)c_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)j;
c_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1,
b_x, &g_emlrtBCI, sp) - 1]);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
c_t->data[c_t->size[0]] = (t <= gridT->
data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &h_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
c_t->data[c_t->size[0] * (b_x + 2)] = (x->data[x->size[0] * b_x] != ksi);
}
emxInit_real_T(sp, &z0, 2, &cb_emlrtRTEI, true);
emxInit_real_T(sp, &d_x, 2, &bb_emlrtRTEI, true);
st.site = &qe_emlrtRSI;
if (all(&st, c_t)) {
st.site = &b_emlrtRSI;
b_x = gridT->size[1];
i = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&m_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
emxInit_real_T(&st, &e_x, 2, &bb_emlrtRTEI, true);
b_x = e_x->size[0] * e_x->size[1];
e_x->size[0] = 1;
e_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)e_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
e_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r9, 2, &bb_emlrtRTEI, true);
b_abs(sp, e_x, r9);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r9->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r9->size[0] * r9->size[1];
emxFree_real_T(&e_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r9->data[b_x];
}
emxFree_real_T(&r9);
rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z0);
st.site = &re_emlrtRSI;
mpower(&st, z0, d_x);
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] = -d_x->data[b_x];
}
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
b->size[1] = d_x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)b, b_x, (int32_T)sizeof(real_T),
&bb_emlrtRTEI);
i = d_x->size[0] * d_x->size[1];
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] = d_x->data[b_x];
}
for (i = 0; i < d_x->size[1]; i++) {
b->data[i] = muDoubleScalarExp(b->data[i]);
}
st.site = &re_emlrtRSI;
b_mrdivide(&st, b, z0);
for (b_x = 0; b_x < 2; b_x++) {
i = d_x->size[0] * d_x->size[1];
d_x->size[b_x] = z0->size[b_x];
emxEnsureCapacity(sp, (emxArray__common *)d_x, i, (int32_T)sizeof
(real_T), &ab_emlrtRTEI);
}
for (i = 0; i < z0->size[1]; i++) {
d_x->data[i] = scalar_erf(z0->data[i]);
}
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] *= 1.7724538509055159;
}
for (b_x = 0; b_x < 2; b_x++) {
b_b[b_x] = b->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
f_x[b_x] = d_x->size[b_x];
}
emxInit_real_T(sp, &g_x, 2, &bb_emlrtRTEI, true);
emlrtSizeEqCheck2DFastR2012b(b_b, f_x, &o_emlrtECI, sp);
b_x = g_x->size[0] * g_x->size[1];
g_x->size[0] = 1;
g_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)g_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
g_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r10, 2, &bb_emlrtRTEI, true);
b_abs(sp, g_x, r10);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r10->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r10->size[0] * r10->size[1];
emxFree_real_T(&g_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r10->data[b_x];
}
emxFree_real_T(&r10);
rdivide(sp, r8, 3.5449077018110318, vals);
st.site = &re_emlrtRSI;
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
emxEnsureCapacity(&st, (emxArray__common *)b, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = b->size[0];
b_x = b->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] -= d_x->data[b_x];
}
b_st.site = &he_emlrtRSI;
if (!(vals->size[1] == 1)) {
if ((vals->size[1] == 1) || (b->size[1] == 1)) {
y = NULL;
m6 = emlrtCreateCharArray(2, iv16);
for (i = 0; i < 45; i++) {
cv18[i] = cv19[i];
}
emlrtInitCharArrayR2013a(&b_st, 45, m6, cv18);
emlrtAssign(&y, m6);
c_st.site = &fh_emlrtRSI;
d_st.site = &vg_emlrtRSI;
b_error(&c_st, message(&d_st, y, &j_emlrtMCI), &k_emlrtMCI);
} else {
b_y = NULL;
m6 = emlrtCreateCharArray(2, iv17);
for (i = 0; i < 21; i++) {
cv20[i] = cv21[i];
}
emlrtInitCharArrayR2013a(&b_st, 21, m6, cv20);
emlrtAssign(&b_y, m6);
c_st.site = &gh_emlrtRSI;
d_st.site = &wg_emlrtRSI;
b_error(&c_st, message(&d_st, b_y, &l_emlrtMCI), &m_emlrtMCI);
}
}
c_x = vals->data[0];
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = b->size[1];
emxEnsureCapacity(&st, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = b->size[1];
for (b_x = 0; b_x < i; b_x++) {
vals->data[vals->size[0] * b_x] = c_x * b->data[b->size[0] * b_x];
}
} else {
emxInit_boolean_T(sp, &d_t, 2, &bb_emlrtRTEI, true);
b_x = d_t->size[0] * d_t->size[1];
d_t->size[0] = 1;
d_t->size[1] = 1 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)d_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
d_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1,
b_x, &i_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
d_t->data[d_t->size[0] * (b_x + 1)] = (x->data[x->size[0] * b_x] ==
ksi);
}
st.site = &se_emlrtRSI;
if (all(&st, d_t)) {
b_x = gridT->size[1];
i = (int32_T)j;
emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &b_emlrtBCI, sp);
c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931;
b_x = gridT->size[1];
i = (int32_T)(j + 1.0);
emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &emlrtBCI, sp);
h_x = (t - gridT->data[(int32_T)(j + 1.0) - 1]) / 3.1415926535897931;
st.site = &c_emlrtRSI;
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
st.site = &c_emlrtRSI;
if (h_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
vals->data[0] = muDoubleScalarSqrt(c_x) - muDoubleScalarSqrt(h_x);
} else {
emxInit_boolean_T(sp, &e_t, 2, &bb_emlrtRTEI, true);
b_x = e_t->size[0] * e_t->size[1];
e_t->size[0] = 1;
e_t->size[1] = 1 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)e_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
e_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1,
b_x, &j_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
e_t->data[e_t->size[0] * (b_x + 1)] = (x->data[x->size[0] * b_x] !=
ksi);
}
st.site = &te_emlrtRSI;
if (all(&st, e_t)) {
st.site = &d_emlrtRSI;
b_x = gridT->size[1];
i = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&k_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
emxInit_real_T(&st, &i_x, 2, &bb_emlrtRTEI, true);
b_x = i_x->size[0] * i_x->size[1];
i_x->size[0] = 1;
i_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)i_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
i_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r11, 2, &bb_emlrtRTEI, true);
b_abs(sp, i_x, r11);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r11->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r11->size[0] * r11->size[1];
emxFree_real_T(&i_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r11->data[b_x];
}
emxFree_real_T(&r11);
rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z0);
st.site = &e_emlrtRSI;
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&l_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
emxInit_real_T(&st, &j_x, 2, &bb_emlrtRTEI, true);
b_x = j_x->size[0] * j_x->size[1];
j_x->size[0] = 1;
j_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)j_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
j_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r12, 2, &bb_emlrtRTEI, true);
b_abs(sp, j_x, r12);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r12->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r12->size[0] * r12->size[1];
emxFree_real_T(&j_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r12->data[b_x];
}
emxFree_real_T(&r12);
emxInit_real_T(sp, &z1, 2, &db_emlrtRTEI, true);
rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z1);
st.site = &ue_emlrtRSI;
mpower(&st, z0, d_x);
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] = -d_x->data[b_x];
}
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
b->size[1] = d_x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)b, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0] * d_x->size[1];
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] = d_x->data[b_x];
}
for (i = 0; i < d_x->size[1]; i++) {
b->data[i] = muDoubleScalarExp(b->data[i]);
}
st.site = &ue_emlrtRSI;
b_mrdivide(&st, b, z0);
st.site = &ue_emlrtRSI;
mpower(&st, z1, d_x);
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] = -d_x->data[b_x];
}
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = d_x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0] * d_x->size[1];
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = d_x->data[b_x];
}
for (i = 0; i < d_x->size[1]; i++) {
r8->data[i] = muDoubleScalarExp(r8->data[i]);
}
st.site = &ue_emlrtRSI;
b_mrdivide(&st, r8, z1);
for (b_x = 0; b_x < 2; b_x++) {
b_b[b_x] = b->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
b_z0[b_x] = r8->size[b_x];
}
emxInit_real_T(sp, &c_z0, 2, &bb_emlrtRTEI, true);
emlrtSizeEqCheck2DFastR2012b(b_b, b_z0, &m_emlrtECI, sp);
b_x = c_z0->size[0] * c_z0->size[1];
c_z0->size[0] = 1;
c_z0->size[1] = z0->size[1];
emxEnsureCapacity(sp, (emxArray__common *)c_z0, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = z0->size[0] * z0->size[1];
for (b_x = 0; b_x < i; b_x++) {
c_z0->data[b_x] = z0->data[b_x];
}
b_erf(sp, c_z0, z0);
b_erf(sp, z1, d_x);
emxFree_real_T(&c_z0);
emxFree_real_T(&z1);
for (b_x = 0; b_x < 2; b_x++) {
b_z0[b_x] = z0->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
f_x[b_x] = d_x->size[b_x];
}
emlrtSizeEqCheck2DFastR2012b(b_z0, f_x, &n_emlrtECI, sp);
b_x = z0->size[0] * z0->size[1];
z0->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)z0, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = z0->size[0];
b_x = z0->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
z0->data[b_x] = 1.7724538509055159 * (z0->data[b_x] - d_x->
data[b_x]);
}
for (b_x = 0; b_x < 2; b_x++) {
b_b[b_x] = b->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
b_z0[b_x] = z0->size[b_x];
}
emxInit_real_T(sp, &k_x, 2, &bb_emlrtRTEI, true);
emlrtSizeEqCheck2DFastR2012b(b_b, b_z0, &m_emlrtECI, sp);
b_x = k_x->size[0] * k_x->size[1];
k_x->size[0] = 1;
k_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)k_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
k_x->data[b_x] = x->data[b_x] - ksi;
}
b_abs(sp, k_x, d_x);
rdivide(sp, d_x, 3.5449077018110318, vals);
st.site = &ue_emlrtRSI;
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
emxEnsureCapacity(&st, (emxArray__common *)b, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = b->size[0];
b_x = b->size[1];
i *= b_x;
emxFree_real_T(&k_x);
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] = (b->data[b_x] - r8->data[b_x]) + z0->data[b_x];
}
b_st.site = &he_emlrtRSI;
if (!(vals->size[1] == 1)) {
if ((vals->size[1] == 1) || (b->size[1] == 1)) {
c_y = NULL;
m6 = emlrtCreateCharArray(2, iv18);
for (i = 0; i < 45; i++) {
cv18[i] = cv19[i];
}
emlrtInitCharArrayR2013a(&b_st, 45, m6, cv18);
emlrtAssign(&c_y, m6);
c_st.site = &fh_emlrtRSI;
d_st.site = &vg_emlrtRSI;
b_error(&c_st, message(&d_st, c_y, &j_emlrtMCI), &k_emlrtMCI);
} else {
d_y = NULL;
m6 = emlrtCreateCharArray(2, iv19);
for (i = 0; i < 21; i++) {
cv20[i] = cv21[i];
}
emlrtInitCharArrayR2013a(&b_st, 21, m6, cv20);
emlrtAssign(&d_y, m6);
c_st.site = &gh_emlrtRSI;
d_st.site = &wg_emlrtRSI;
b_error(&c_st, message(&d_st, d_y, &l_emlrtMCI), &m_emlrtMCI);
}
}
c_x = vals->data[0];
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = b->size[1];
emxEnsureCapacity(&st, (emxArray__common *)vals, b_x, (int32_T)
sizeof(real_T), &bb_emlrtRTEI);
i = b->size[1];
for (b_x = 0; b_x < i; b_x++) {
vals->data[vals->size[0] * b_x] = c_x * b->data[b->size[0] * b_x];
}
} else {
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
vals->data[0] = 0.0;
}
emxFree_boolean_T(&e_t);
}
emxFree_boolean_T(&d_t);
}
emxFree_boolean_T(&c_t);
emxFree_real_T(&d_x);
emxFree_real_T(&z0);
}
emxFree_boolean_T(&b_t);
}
emxFree_real_T(&r8);
emxFree_real_T(&b);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
/* Function Definitions */
real_T Bcoeff(const emlrtStack *sp, real_T ksi, real_T j, real_T x, real_T t,
const emxArray_real_T *gridT)
{
real_T vals;
int32_T k;
int32_T i1;
boolean_T b_x[3];
boolean_T y;
boolean_T exitg2;
boolean_T exitg1;
real_T c_x;
real_T d_x;
emlrtStack st;
emlrtStack b_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
/* evaluate the coefficient B at the boundary ksi=0 or ksi=1; */
/* for the index j which describes the time steps timePoints_j, at time t and space */
/* point x */
/* timePoints is a vector describing the time descritized domain */
k = gridT->size[1];
i1 = (int32_T)emlrtIntegerCheckFastR2012b(j, &b_emlrtDCI, sp);
if (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k, &n_emlrtBCI,
sp) - 1]) {
vals = 0.0;
} else {
k = gridT->size[1];
i1 = (int32_T)j;
b_x[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&o_emlrtBCI, sp) - 1]);
k = gridT->size[1];
i1 = (int32_T)((uint32_T)j + 1U);
b_x[1] = (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&p_emlrtBCI, sp) - 1]);
b_x[2] = (x == ksi);
y = true;
k = 0;
exitg2 = false;
while ((!exitg2) && (k < 3)) {
if (b_x[k] == 0) {
y = false;
exitg2 = true;
} else {
k++;
}
}
if (y) {
vals = 0.0;
} else {
k = gridT->size[1];
i1 = (int32_T)j;
b_x[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&q_emlrtBCI, sp) - 1]);
k = gridT->size[1];
i1 = (int32_T)((uint32_T)j + 1U);
b_x[1] = (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&r_emlrtBCI, sp) - 1]);
b_x[2] = (x != ksi);
y = true;
k = 0;
exitg1 = false;
while ((!exitg1) && (k < 3)) {
if (b_x[k] == 0) {
y = false;
exitg1 = true;
} else {
k++;
}
}
if (y) {
st.site = &g_emlrtRSI;
k = gridT->size[1];
i1 = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&v_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
vals = -scalar_erf(muDoubleScalarAbs(x - ksi) / (2.0 *
muDoubleScalarSqrt(c_x))) / 2.0;
} else {
k = gridT->size[1];
i1 = (int32_T)((uint32_T)j + 1U);
if (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&s_emlrtBCI, sp) - 1]) {
st.site = &h_emlrtRSI;
k = gridT->size[1];
i1 = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&t_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
st.site = &i_emlrtRSI;
k = gridT->size[1];
i1 = (int32_T)((uint32_T)j + 1U);
d_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i1, 1, k,
&u_emlrtBCI, &st) - 1];
if (d_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
vals = (b_scalar_erf(muDoubleScalarAbs(x - ksi) / (2.0 *
muDoubleScalarSqrt(c_x))) - b_scalar_erf(muDoubleScalarAbs(x
- ksi) / (2.0 * muDoubleScalarSqrt(d_x)))) / 2.0;
} else {
vals = 0.0;
}
}
}
}
return vals;
}
/* Function Definitions */
void clcPMP_olyHyb_tmp(const emlrtStack *sp, real_T engKinPre, real_T engKinAct,
real_T gea, real_T slp, real_T batEng, real_T psiBatEng, real_T psiTim, real_T
batPwrAux, real_T batEngStp, real_T wayStp, const struct0_T *par, real_T
*cosHamMin, real_T *batFrcOut, real_T *fulFrcOut)
{
real_T mtmp;
real_T vehVel;
real_T b_engKinPre[2];
real_T crsSpdVec[2];
int32_T i18;
int32_T k;
boolean_T y;
boolean_T exitg3;
boolean_T exitg2;
real_T crsSpd;
real_T whlTrq;
real_T crsTrq;
real_T iceTrqMax;
real_T iceTrqMin;
real_T b_par[100];
real_T emoTrqMaxPos;
real_T emoTrqMinPos;
real_T emoTrqMax;
real_T emoTrqMin;
real_T batPwrMax;
real_T batPwrMin;
real_T batOcv;
real_T batEngDltMin;
real_T batEngDltMax;
real_T batEngDltMinInx;
real_T batEngDltMaxInx;
real_T batEngDlt;
real_T fulFrc;
real_T batFrc;
real_T b_batFrc;
real_T batPwr;
real_T emoTrq;
real_T iceTrq;
real_T fulPwr;
int32_T ixstart;
int32_T itmp;
int32_T ix;
boolean_T exitg1;
emlrtStack st;
emlrtStack b_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
/* 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) */
/* par - Struct(1,1) - Modelldaten */
/* ^^ model data */
/* % 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 */
crsSpdHybMax = muDoubleScalarMin(par->iceSpdMgd[14850], par->emoSpdMgd[14850]);
crsSpdHybMax_not_empty = true;
/* minimale Drehzahl der Kurbelwelle */
/* minimum crankshaft rotational speed */
crsSpdHybMin = par->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 */
mtmp = 2.0 * engKinPre / par->vehMas;
st.site = &g_emlrtRSI;
if (mtmp < 0.0) {
b_st.site = &h_emlrtRSI;
eml_error(&b_st);
}
vehVel = muDoubleScalarSqrt(mtmp);
/* % 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 */
b_engKinPre[0] = engKinPre;
b_engKinPre[1] = engKinAct;
for (i18 = 0; i18 < 2; i18++) {
crsSpdVec[i18] = 2.0 * b_engKinPre[i18] / par->vehMas;
}
st.site = &f_emlrtRSI;
for (k = 0; k < 2; k++) {
if (crsSpdVec[k] < 0.0) {
b_st.site = &h_emlrtRSI;
eml_error(&b_st);
}
}
for (k = 0; k < 2; k++) {
crsSpdVec[k] = muDoubleScalarSqrt(crsSpdVec[k]);
}
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &mb_emlrtBCI, sp);
mtmp = par->geaRat->data[(int32_T)gea - 1];
for (i18 = 0; i18 < 2; i18++) {
crsSpdVec[i18] = mtmp * crsSpdVec[i18] / par->whlDrr;
}
/* 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)) {
if (!!(crsSpdVec[k] > crsSpdHybMax)) {
y = true;
exitg3 = true;
} else {
k++;
}
}
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)) {
if (!!(crsSpdVec[k] < crsSpdHybMin)) {
y = true;
exitg2 = true;
} else {
k++;
}
}
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) / (par->vehMas * wayStp) * par->vehMas
+ par->vehMas * 9.81 * muDoubleScalarSin(slp)) +
par->whlRolResCof * par->vehMas * 9.81 * muDoubleScalarCos(slp))
+ 2.0 * par->drgCof / par->vehMas * engKinPre) * par->whlDrr;
/* 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) {
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &nb_emlrtBCI, sp);
crsTrq = whlTrq / par->geaRat->data[(int32_T)gea - 1] * par->geaEfy;
} else {
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &ob_emlrtBCI, sp);
crsTrq = whlTrq / par->geaRat->data[(int32_T)gea - 1] / par->geaEfy;
}
/* % Verbrennungsmotor */
/* internal combustion engine */
/* maximales Moment des Verbrennungsmotors berechnen */
/* calculate max torque of the engine (quadratic based on rotation speed) */
iceTrqMax = (par->iceTrqMaxCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
par->iceTrqMaxCof[1] * crsSpdVec[0]) + par->iceTrqMaxCof[2];
/* minimales Moment des Verbrennungsmotors berechnen */
/* calculating mimimum ICE moment */
iceTrqMin = (par->iceTrqMinCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
par->iceTrqMinCof[1] * crsSpdVec[0]) + par->iceTrqMinCof[2];
/* % 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 (i18 = 0; i18 < 100; i18++) {
b_par[i18] = par->emoSpdMgd[150 * i18];
}
emoTrqMaxPos = interp1q(b_par, par->emoTrqMax_emoSpd, crsSpdVec[0]);
/* 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) {
} 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 */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque allowed */
/* Skalar - max EM torque possible */
st.site = &e_emlrtRSI;
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* Skalar - Wegschrittweite */
/* Skalar - Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* Skalar - max EM torque possible */
/* 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 (i18 = 0; i18 < 100; i18++) {
b_par[i18] = par->emoSpdMgd[150 * i18];
}
emoTrqMinPos = interp1q(b_par, par->emoTrqMin_emoSpd, crsSpdVec[0]);
/* % 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;
/* % 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;
}
emoTrqMax = muDoubleScalarMax(emoTrqMinPos, emoTrqMax);
emoTrqMin = muDoubleScalarMax(emoTrqMinPos, emoTrqMin);
/* % 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; */
b_st.site = &i_emlrtRSI;
batPwrMax = codegen_interp2(&b_st, par->emoSpdMgd, par->emoTrqMgd,
par->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMax) + batPwrAux;
b_st.site = &j_emlrtRSI;
batPwrMin = codegen_interp2(&b_st, par->emoSpdMgd, par->emoTrqMgd,
par->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMin) + batPwrAux;
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery max power is not above bat max bounds */
if (batPwrMax > par->batPwrMax) {
batPwrMax = par->batPwrMax;
}
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery min power is not below bat min bounds */
if (batPwrMin > par->batPwrMax) {
batPwrMin = par->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 < par->batPwrMin) {
batPwrMax = par->batPwrMin;
}
if (batPwrMin < par->batPwrMin) {
batPwrMin = par->batPwrMin;
}
/* Batteriespannung aus Kennkurve berechnen */
/* calculating battery voltage of characteristic curve - eq?-------------- */
batOcv = batEng * par->batOcvCof_batEng[0] + par->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_tmp(batPwrMax, vehVel, par->batRstDch,
par->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_tmp(batPwrMin, vehVel, par->batRstDch,
par->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 = muDoubleScalarFloor(batEngDltMax / batEngStp);
} else {
batEngDltMinInx = muDoubleScalarCeil(batEngDltMin / batEngStp);
batEngDltMaxInx = muDoubleScalarFloor(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 */
i18 = (int32_T)(batEngDltMaxInx + (1.0 - batEngDltMinInx));
emlrtForLoopVectorCheckR2012b(batEngDltMinInx, 1.0, batEngDltMaxInx,
mxDOUBLE_CLASS, i18, &o_emlrtRTEI, sp);
k = 0;
while (k <= i18 - 1) {
batEngDlt = (batEngDltMinInx + (real_T)k) * batEngStp;
/* open circuit voltage over each iteration */
batOcv = (batEng + batEngDlt) * par->batOcvCof_batEng[0] +
par->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 der Fahrzeugparameter */
st.site = &d_emlrtRSI;
/* Skalar für die Batterieleistung */
/* Skalar Kraftstoffkraft */
/* 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 der Fahrzeugparameter */
/* */
/* 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 = par->batRstDch;
} else {
b_batFrc = par->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! */
/* */
b_st.site = &k_emlrtRSI;
emoTrq = codegen_interp2(&b_st, par->emoSpdMgd, par->emoPwrMgd,
par->emoTrq_emoSpd_emoPwr, crsSpd, batPwr);
/* emoTrq = lininterp2(par.emoSpdMgd(1,:), par.emoPwrMgd(:,1),... */
/* par.emoTrq_emoSpd_emoPwr',crsSpd,emoPwrEle); */
if (muDoubleScalarIsInf(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 */
b_st.site = &l_emlrtRSI;
fulPwr = codegen_interp2(&b_st, par->iceSpdMgd, par->iceTrqMgd,
par->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_tmp(batPwr, vehVel, par->batRstDch,
par->batRstChr, batOcv);
/* %% Hamiltonfunktion bestimmen */
/* determine the hamiltonian */
/* Eq (29b) */
crsSpdVec[0] = (fulFrc + psiBatEng * batFrc) + psiTim / vehVel;
ixstart = 1;
mtmp = crsSpdVec[0];
itmp = 1;
if (muDoubleScalarIsNaN(crsSpdVec[0])) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 3)) {
ixstart = 2;
if (!muDoubleScalarIsNaN(*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;
}
k++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
}
}
}
}
/* end of function */
}
/* Function Definitions */
void b_sqrt(real_T *x)
{
*x = muDoubleScalarSqrt(*x);
}
static void c_eml_qrsolve(const emlrtStack *sp, const emxArray_real_T *A,
emxArray_real_T *B, emxArray_real_T *Y)
{
emxArray_real_T *b_A;
emxArray_real_T *work;
int32_T mn;
int32_T i51;
int32_T ix;
emxArray_real_T *tau;
emxArray_int32_T *jpvt;
int32_T m;
int32_T n;
int32_T b_mn;
emxArray_real_T *vn1;
emxArray_real_T *vn2;
int32_T k;
boolean_T overflow;
boolean_T b12;
int32_T i;
int32_T i_i;
int32_T nmi;
int32_T mmi;
int32_T pvt;
int32_T iy;
boolean_T b13;
real_T xnorm;
int32_T i52;
real_T atmp;
real_T d16;
boolean_T b14;
boolean_T b_i;
ptrdiff_t n_t;
ptrdiff_t incx_t;
double * xix0_t;
boolean_T exitg1;
const mxArray *y;
static const int32_T iv78[2] = { 1, 8 };
const mxArray *m14;
char_T cv76[8];
static const char_T cv77[8] = { '%', '%', '%', 'd', '.', '%', 'd', 'e' };
char_T cv78[14];
uint32_T unnamed_idx_0;
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
emlrtStack e_st;
emlrtStack f_st;
emlrtStack g_st;
emlrtStack h_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &c_st;
d_st.tls = c_st.tls;
e_st.prev = &d_st;
e_st.tls = d_st.tls;
f_st.prev = &e_st;
f_st.tls = e_st.tls;
g_st.prev = &f_st;
g_st.tls = f_st.tls;
h_st.prev = &g_st;
h_st.tls = g_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
emxInit_real_T(sp, &b_A, 2, &m_emlrtRTEI, true);
b_emxInit_real_T(sp, &work, 1, &rb_emlrtRTEI, true);
mn = (int32_T)muDoubleScalarMin(A->size[0], A->size[1]);
st.site = &mc_emlrtRSI;
b_st.site = &nc_emlrtRSI;
c_st.site = &oc_emlrtRSI;
i51 = b_A->size[0] * b_A->size[1];
b_A->size[0] = A->size[0];
b_A->size[1] = A->size[1];
emxEnsureCapacity(&c_st, (emxArray__common *)b_A, i51, (int32_T)sizeof(real_T),
&m_emlrtRTEI);
ix = A->size[0] * A->size[1];
for (i51 = 0; i51 < ix; i51++) {
b_A->data[i51] = A->data[i51];
}
b_emxInit_real_T(&c_st, &tau, 1, &m_emlrtRTEI, true);
b_emxInit_int32_T(&c_st, &jpvt, 2, &m_emlrtRTEI, true);
m = b_A->size[0];
n = b_A->size[1];
b_mn = muIntScalarMin_sint32(b_A->size[0], b_A->size[1]);
i51 = tau->size[0];
tau->size[0] = b_mn;
emxEnsureCapacity(&c_st, (emxArray__common *)tau, i51, (int32_T)sizeof(real_T),
&n_emlrtRTEI);
d_st.site = &mf_emlrtRSI;
e_st.site = &rb_emlrtRSI;
f_st.site = &sb_emlrtRSI;
g_st.site = &tb_emlrtRSI;
eml_signed_integer_colon(&g_st, b_A->size[1], jpvt);
if ((b_A->size[0] == 0) || (b_A->size[1] == 0)) {
} else {
ix = b_A->size[1];
i51 = work->size[0];
work->size[0] = ix;
emxEnsureCapacity(&c_st, (emxArray__common *)work, i51, (int32_T)sizeof
(real_T), &m_emlrtRTEI);
for (i51 = 0; i51 < ix; i51++) {
work->data[i51] = 0.0;
}
b_emxInit_real_T(&c_st, &vn1, 1, &pb_emlrtRTEI, true);
b_emxInit_real_T(&c_st, &vn2, 1, &qb_emlrtRTEI, true);
d_st.site = &tc_emlrtRSI;
ix = b_A->size[1];
i51 = vn1->size[0];
vn1->size[0] = ix;
emxEnsureCapacity(&c_st, (emxArray__common *)vn1, i51, (int32_T)sizeof
(real_T), &pb_emlrtRTEI);
i51 = vn2->size[0];
vn2->size[0] = ix;
emxEnsureCapacity(&c_st, (emxArray__common *)vn2, i51, (int32_T)sizeof
(real_T), &qb_emlrtRTEI);
k = 1;
d_st.site = &nf_emlrtRSI;
overflow = (b_A->size[1] > 2147483646);
if (overflow) {
e_st.site = &db_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (ix = 0; ix + 1 <= b_A->size[1]; ix++) {
d_st.site = &sc_emlrtRSI;
vn1->data[ix] = b_eml_xnrm2(&d_st, b_A->size[0], b_A, k);
vn2->data[ix] = vn1->data[ix];
k += b_A->size[0];
}
d_st.site = &rc_emlrtRSI;
if (1 > b_mn) {
b12 = false;
} else {
b12 = (b_mn > 2147483646);
}
if (b12) {
e_st.site = &db_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (i = 1; i <= b_mn; i++) {
i_i = (i + (i - 1) * m) - 1;
nmi = n - i;
mmi = m - i;
d_st.site = &of_emlrtRSI;
ix = eml_ixamax(&d_st, 1 + nmi, vn1, i);
pvt = (i + ix) - 2;
if (pvt + 1 != i) {
d_st.site = &pf_emlrtRSI;
e_st.site = &bc_emlrtRSI;
f_st.site = &cc_emlrtRSI;
ix = 1 + m * pvt;
iy = 1 + m * (i - 1);
g_st.site = &dc_emlrtRSI;
if (1 > m) {
b13 = false;
} else {
b13 = (m > 2147483646);
}
if (b13) {
h_st.site = &db_emlrtRSI;
check_forloop_overflow_error(&h_st);
}
for (k = 1; k <= m; k++) {
i51 = b_A->size[0] * b_A->size[1];
xnorm = b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i51,
&le_emlrtBCI, &f_st) - 1];
i51 = b_A->size[0] * b_A->size[1];
i52 = b_A->size[0] * b_A->size[1];
b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i51, &le_emlrtBCI,
&f_st) - 1] = b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i52,
&le_emlrtBCI, &f_st) - 1];
i51 = b_A->size[0] * b_A->size[1];
b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i51, &le_emlrtBCI,
&f_st) - 1] = xnorm;
ix++;
iy++;
}
ix = jpvt->data[pvt];
jpvt->data[pvt] = jpvt->data[i - 1];
jpvt->data[i - 1] = ix;
vn1->data[pvt] = vn1->data[i - 1];
vn2->data[pvt] = vn2->data[i - 1];
}
if (i < m) {
d_st.site = &qc_emlrtRSI;
atmp = b_A->data[i_i];
d16 = 0.0;
if (1 + mmi <= 0) {
} else {
e_st.site = &wc_emlrtRSI;
xnorm = b_eml_xnrm2(&e_st, mmi, b_A, i_i + 2);
if (xnorm != 0.0) {
xnorm = muDoubleScalarHypot(b_A->data[i_i], xnorm);
if (b_A->data[i_i] >= 0.0) {
xnorm = -xnorm;
}
if (muDoubleScalarAbs(xnorm) < 1.0020841800044864E-292) {
ix = 0;
do {
ix++;
e_st.site = &xc_emlrtRSI;
b_eml_xscal(&e_st, mmi, 9.9792015476736E+291, b_A, i_i + 2);
xnorm *= 9.9792015476736E+291;
atmp *= 9.9792015476736E+291;
} while (!(muDoubleScalarAbs(xnorm) >= 1.0020841800044864E-292));
e_st.site = &yc_emlrtRSI;
xnorm = b_eml_xnrm2(&e_st, mmi, b_A, i_i + 2);
xnorm = muDoubleScalarHypot(atmp, xnorm);
if (atmp >= 0.0) {
xnorm = -xnorm;
}
d16 = (xnorm - atmp) / xnorm;
e_st.site = &ad_emlrtRSI;
b_eml_xscal(&e_st, mmi, 1.0 / (atmp - xnorm), b_A, i_i + 2);
e_st.site = &bd_emlrtRSI;
if (1 > ix) {
b14 = false;
} else {
b14 = (ix > 2147483646);
}
if (b14) {
f_st.site = &db_emlrtRSI;
check_forloop_overflow_error(&f_st);
}
for (k = 1; k <= ix; k++) {
xnorm *= 1.0020841800044864E-292;
}
atmp = xnorm;
} else {
d16 = (xnorm - b_A->data[i_i]) / xnorm;
atmp = 1.0 / (b_A->data[i_i] - xnorm);
e_st.site = &cd_emlrtRSI;
b_eml_xscal(&e_st, mmi, atmp, b_A, i_i + 2);
atmp = xnorm;
}
}
}
tau->data[i - 1] = d16;
} else {
atmp = b_A->data[i_i];
d_st.site = &pc_emlrtRSI;
tau->data[i - 1] = eml_matlab_zlarfg();
}
b_A->data[i_i] = atmp;
if (i < n) {
atmp = b_A->data[i_i];
b_A->data[i_i] = 1.0;
d_st.site = &qf_emlrtRSI;
eml_matlab_zlarf(&d_st, mmi + 1, nmi, i_i + 1, tau->data[i - 1], b_A, i
+ i * m, m, work);
b_A->data[i_i] = atmp;
}
d_st.site = &rf_emlrtRSI;
if (i + 1 > n) {
b_i = false;
} else {
b_i = (n > 2147483646);
}
if (b_i) {
e_st.site = &db_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (ix = i; ix + 1 <= n; ix++) {
if (vn1->data[ix] != 0.0) {
xnorm = muDoubleScalarAbs(b_A->data[(i + b_A->size[0] * ix) - 1]) /
vn1->data[ix];
xnorm = 1.0 - xnorm * xnorm;
if (xnorm < 0.0) {
xnorm = 0.0;
}
atmp = vn1->data[ix] / vn2->data[ix];
atmp = xnorm * (atmp * atmp);
if (atmp <= 1.4901161193847656E-8) {
if (i < m) {
d_st.site = &sf_emlrtRSI;
e_st.site = &uc_emlrtRSI;
if (mmi < 1) {
xnorm = 0.0;
} else {
f_st.site = &vc_emlrtRSI;
g_st.site = &vc_emlrtRSI;
n_t = (ptrdiff_t)(mmi);
g_st.site = &vc_emlrtRSI;
incx_t = (ptrdiff_t)(1);
i51 = b_A->size[0] * b_A->size[1];
i52 = (i + m * ix) + 1;
xix0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b
(i52, 1, i51, &vb_emlrtBCI, &f_st) - 1]);
xnorm = dnrm2(&n_t, xix0_t, &incx_t);
}
vn1->data[ix] = xnorm;
vn2->data[ix] = vn1->data[ix];
} else {
vn1->data[ix] = 0.0;
vn2->data[ix] = 0.0;
}
} else {
d_st.site = &tf_emlrtRSI;
vn1->data[ix] *= muDoubleScalarSqrt(xnorm);
}
}
}
}
emxFree_real_T(&vn2);
emxFree_real_T(&vn1);
}
atmp = 0.0;
if (mn > 0) {
xnorm = muDoubleScalarMax(A->size[0], A->size[1]) * muDoubleScalarAbs
(b_A->data[0]) * 2.2204460492503131E-16;
k = 0;
exitg1 = false;
while ((!exitg1) && (k <= mn - 1)) {
if (muDoubleScalarAbs(b_A->data[k + b_A->size[0] * k]) <= xnorm) {
st.site = &lc_emlrtRSI;
y = NULL;
m14 = emlrtCreateCharArray(2, iv78);
for (i = 0; i < 8; i++) {
cv76[i] = cv77[i];
}
emlrtInitCharArrayR2013a(&st, 8, m14, cv76);
emlrtAssign(&y, m14);
b_st.site = &tg_emlrtRSI;
emlrt_marshallIn(&b_st, c_sprintf(&b_st, b_sprintf(&b_st, y,
emlrt_marshallOut(14.0), emlrt_marshallOut(6.0), &o_emlrtMCI),
emlrt_marshallOut(xnorm), &p_emlrtMCI), "sprintf", cv78);
st.site = &kc_emlrtRSI;
b_eml_warning(&st, atmp, cv78);
exitg1 = true;
} else {
atmp++;
k++;
}
}
}
unnamed_idx_0 = (uint32_T)A->size[1];
i51 = Y->size[0];
Y->size[0] = (int32_T)unnamed_idx_0;
emxEnsureCapacity(sp, (emxArray__common *)Y, i51, (int32_T)sizeof(real_T),
&m_emlrtRTEI);
ix = (int32_T)unnamed_idx_0;
for (i51 = 0; i51 < ix; i51++) {
Y->data[i51] = 0.0;
}
for (ix = 0; ix < mn; ix++) {
if (tau->data[ix] != 0.0) {
xnorm = B->data[ix];
i51 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
emlrtForLoopVectorCheckR2012b((1.0 + (real_T)ix) + 1.0, 1.0, A->size[0],
mxDOUBLE_CLASS, i51, &ac_emlrtRTEI, sp);
for (i = 0; i < i51; i++) {
unnamed_idx_0 = ((uint32_T)ix + i) + 2U;
xnorm += b_A->data[((int32_T)unnamed_idx_0 + b_A->size[0] * ix) - 1] *
B->data[(int32_T)unnamed_idx_0 - 1];
}
xnorm *= tau->data[ix];
if (xnorm != 0.0) {
B->data[ix] -= xnorm;
i51 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
emlrtForLoopVectorCheckR2012b((1.0 + (real_T)ix) + 1.0, 1.0, A->size[0],
mxDOUBLE_CLASS, i51, &yb_emlrtRTEI, sp);
for (i = 0; i < i51; i++) {
unnamed_idx_0 = ((uint32_T)ix + i) + 2U;
B->data[(int32_T)unnamed_idx_0 - 1] -= b_A->data[((int32_T)
unnamed_idx_0 + b_A->size[0] * ix) - 1] * xnorm;
}
}
}
}
emxFree_real_T(&tau);
emlrtForLoopVectorCheckR2012b(1.0, 1.0, atmp, mxDOUBLE_CLASS, (int32_T)atmp,
&xb_emlrtRTEI, sp);
for (i = 0; i < (int32_T)atmp; i++) {
Y->data[jpvt->data[i] - 1] = B->data[i];
}
emlrtForLoopVectorCheckR2012b(atmp, -1.0, 1.0, mxDOUBLE_CLASS, (int32_T)-(1.0
+ (-1.0 - atmp)), &wb_emlrtRTEI, sp);
for (ix = 0; ix < (int32_T)-(1.0 + (-1.0 - atmp)); ix++) {
xnorm = atmp + -(real_T)ix;
Y->data[jpvt->data[(int32_T)xnorm - 1] - 1] = eml_div(Y->data[jpvt->data
[(int32_T)xnorm - 1] - 1], b_A->data[((int32_T)xnorm + b_A->size[0] *
((int32_T)xnorm - 1)) - 1]);
for (i = 0; i < (int32_T)(xnorm - 1.0); i++) {
Y->data[jpvt->data[i] - 1] -= Y->data[jpvt->data[(int32_T)xnorm - 1] - 1] *
b_A->data[i + b_A->size[0] * ((int32_T)xnorm - 1)];
}
}
emxFree_int32_T(&jpvt);
emxFree_real_T(&work);
emxFree_real_T(&b_A);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
/* Function Definitions */
void rsf2csf(const emxArray_real_T *Ur, const emxArray_real_T *Tr,
emxArray_creal_T *U, emxArray_creal_T *T)
{
int32_T y;
int32_T loop_ub;
int16_T varargin_1[2];
int32_T mtmp;
int32_T m;
real_T c;
real_T b;
real_T temp;
real_T p;
real_T bcmax;
real_T scale;
real_T bb;
real_T b_p;
real_T cs;
int32_T b_scale;
real_T b_c;
real_T mu1_re;
emlrtPushRtStackR2012b(&bh_emlrtRSI, emlrtRootTLSGlobal);
y = T->size[0] * T->size[1];
T->size[0] = Tr->size[0];
T->size[1] = Tr->size[1];
emxEnsureCapacity((emxArray__common *)T, y, (int32_T)sizeof(creal_T),
&v_emlrtRTEI);
loop_ub = Tr->size[0] * Tr->size[1];
for (y = 0; y < loop_ub; y++) {
T->data[y].re = Tr->data[y];
T->data[y].im = 0.0;
}
y = U->size[0] * U->size[1];
U->size[0] = Ur->size[0];
U->size[1] = Ur->size[1];
emxEnsureCapacity((emxArray__common *)U, y, (int32_T)sizeof(creal_T),
&v_emlrtRTEI);
loop_ub = Ur->size[0] * Ur->size[1];
for (y = 0; y < loop_ub; y++) {
U->data[y].re = Ur->data[y];
U->data[y].im = 0.0;
}
for (y = 0; y < 2; y++) {
varargin_1[y] = (int16_T)Tr->size[y];
}
mtmp = varargin_1[0];
if (varargin_1[1] < varargin_1[0]) {
mtmp = varargin_1[1];
}
for (y = 0; y < 2; y++) {
varargin_1[y] = (int16_T)Ur->size[y];
}
loop_ub = varargin_1[0];
if (varargin_1[1] < varargin_1[0]) {
loop_ub = varargin_1[1];
}
mtmp = (int32_T)muDoubleScalarMin(mtmp, loop_ub);
if (mtmp == 0) {
} else {
for (m = mtmp - 1; m + 1 >= 2; m--) {
if (Tr->data[m + Tr->size[0] * (m - 1)] != 0.0) {
emlrtPushRtStackR2012b(&ch_emlrtRSI, emlrtRootTLSGlobal);
c = Tr->data[m + Tr->size[0] * (m - 1)];
b = Tr->data[(m + Tr->size[0] * m) - 1];
temp = Tr->data[(m + Tr->size[0] * (m - 1)) - 1];
if (Tr->data[m + Tr->size[0] * (m - 1)] == 0.0) {
} else if (Tr->data[(m + Tr->size[0] * m) - 1] == 0.0) {
temp = Tr->data[m + Tr->size[0] * m];
b = -Tr->data[m + Tr->size[0] * (m - 1)];
c = 0.0;
} else if ((Tr->data[(m + Tr->size[0] * (m - 1)) - 1] - Tr->data[m +
Tr->size[0] * m] == 0.0) && ((Tr->data[(m + Tr->size[0] * m)
- 1] < 0.0) != (Tr->data[m + Tr->size[0] * (m - 1)] < 0.0))) {
} else {
temp = Tr->data[(m + Tr->size[0] * (m - 1)) - 1] - Tr->data[m +
Tr->size[0] * m];
p = 0.5 * temp;
bcmax = muDoubleScalarMax(muDoubleScalarAbs(Tr->data[(m + Tr->size[0] *
m) - 1]), muDoubleScalarAbs(Tr->data[m + Tr->size[0] * (m - 1)]));
if (!(Tr->data[(m + Tr->size[0] * m) - 1] < 0.0)) {
loop_ub = 1;
} else {
loop_ub = -1;
}
if (!(Tr->data[m + Tr->size[0] * (m - 1)] < 0.0)) {
y = 1;
} else {
y = -1;
}
scale = muDoubleScalarMax(muDoubleScalarAbs(p), bcmax);
bcmax = p / scale * p + bcmax / scale * (muDoubleScalarMin
(muDoubleScalarAbs(Tr->data[(m + Tr->size[0] * m) - 1]),
muDoubleScalarAbs(Tr->data[m + Tr->size[0] * (m - 1)])) * (real_T)
loop_ub * (real_T)y);
if (bcmax >= 8.8817841970012523E-16) {
emlrtPushRtStackR2012b(&gg_emlrtRSI, emlrtRootTLSGlobal);
bb = muDoubleScalarSqrt(scale) * muDoubleScalarSqrt(bcmax);
emlrtPopRtStackR2012b(&gg_emlrtRSI, emlrtRootTLSGlobal);
if (!(p < 0.0)) {
b_p = bb;
} else {
b_p = -bb;
}
temp = Tr->data[m + Tr->size[0] * m] + (p + b_p);
b = Tr->data[(m + Tr->size[0] * m) - 1] - Tr->data[m + Tr->size[0] *
(m - 1)];
c = 0.0;
} else {
scale = Tr->data[(m + Tr->size[0] * m) - 1] + Tr->data[m + Tr->size
[0] * (m - 1)];
bcmax = muDoubleScalarHypot(scale, temp);
emlrtPushRtStackR2012b(&hg_emlrtRSI, emlrtRootTLSGlobal);
bb = 0.5 * (1.0 + muDoubleScalarAbs(scale) / bcmax);
if (bb < 0.0) {
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
eml_error();
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
}
cs = muDoubleScalarSqrt(bb);
emlrtPopRtStackR2012b(&hg_emlrtRSI, emlrtRootTLSGlobal);
if (!(scale < 0.0)) {
b_scale = 1;
} else {
b_scale = -1;
}
bcmax = -(p / (bcmax * cs)) * (real_T)b_scale;
scale = Tr->data[(m + Tr->size[0] * (m - 1)) - 1] * cs + Tr->data[(m
+ Tr->size[0] * m) - 1] * bcmax;
bb = -Tr->data[(m + Tr->size[0] * (m - 1)) - 1] * bcmax + Tr->data
[(m + Tr->size[0] * m) - 1] * cs;
p = Tr->data[m + Tr->size[0] * (m - 1)] * cs + Tr->data[m + Tr->
size[0] * m] * bcmax;
temp = -Tr->data[m + Tr->size[0] * (m - 1)] * bcmax + Tr->data[m +
Tr->size[0] * m] * cs;
b = bb * cs + temp * bcmax;
c = -scale * bcmax + p * cs;
temp = 0.5 * ((scale * cs + p * bcmax) + (-bb * bcmax + temp * cs));
if (c != 0.0) {
if (b != 0.0) {
if ((b < 0.0) == (c < 0.0)) {
emlrtPushRtStackR2012b(&ig_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&ig_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&jg_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&jg_emlrtRSI, emlrtRootTLSGlobal);
bb = muDoubleScalarSqrt(muDoubleScalarAbs(b)) *
muDoubleScalarSqrt(muDoubleScalarAbs(c));
emlrtPushRtStackR2012b(&kg_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&kg_emlrtRSI, emlrtRootTLSGlobal);
if (!(c < 0.0)) {
b_c = bb;
} else {
b_c = -bb;
}
temp += b_c;
b -= c;
c = 0.0;
}
} else {
b = -c;
c = 0.0;
}
}
}
}
if (c == 0.0) {
bcmax = 0.0;
} else {
emlrtPushRtStackR2012b(&lg_emlrtRSI, emlrtRootTLSGlobal);
bcmax = muDoubleScalarSqrt(muDoubleScalarAbs(b)) * muDoubleScalarSqrt
(muDoubleScalarAbs(c));
emlrtPopRtStackR2012b(&lg_emlrtRSI, emlrtRootTLSGlobal);
}
emlrtPopRtStackR2012b(&ch_emlrtRSI, emlrtRootTLSGlobal);
mu1_re = temp - Tr->data[m + Tr->size[0] * m];
scale = muDoubleScalarHypot(muDoubleScalarHypot(mu1_re, bcmax), Tr->
data[m + Tr->size[0] * (m - 1)]);
if (bcmax == 0.0) {
mu1_re /= scale;
cs = 0.0;
} else if (mu1_re == 0.0) {
mu1_re = 0.0;
cs = bcmax / scale;
} else {
mu1_re /= scale;
cs = bcmax / scale;
}
c = Tr->data[m + Tr->size[0] * (m - 1)] / scale;
emlrtPushRtStackR2012b(&dh_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&dh_emlrtRSI, emlrtRootTLSGlobal);
for (loop_ub = m - 1; loop_ub + 1 <= mtmp; loop_ub++) {
b = T->data[(m + T->size[0] * loop_ub) - 1].re;
temp = T->data[(m + T->size[0] * loop_ub) - 1].im;
bb = T->data[(m + T->size[0] * loop_ub) - 1].re;
p = T->data[(m + T->size[0] * loop_ub) - 1].im;
bcmax = T->data[(m + T->size[0] * loop_ub) - 1].im;
scale = T->data[(m + T->size[0] * loop_ub) - 1].re;
T->data[(m + T->size[0] * loop_ub) - 1].re = (mu1_re * bb + cs * p) +
c * T->data[m + T->size[0] * loop_ub].re;
T->data[(m + T->size[0] * loop_ub) - 1].im = (mu1_re * bcmax - cs *
scale) + c * T->data[m + T->size[0] * loop_ub].im;
bcmax = mu1_re * T->data[m + T->size[0] * loop_ub].re - cs * T->data[m
+ T->size[0] * loop_ub].im;
scale = mu1_re * T->data[m + T->size[0] * loop_ub].im + cs * T->data[m
+ T->size[0] * loop_ub].re;
T->data[m + T->size[0] * loop_ub].re = bcmax - c * b;
T->data[m + T->size[0] * loop_ub].im = scale - c * temp;
}
emlrtPushRtStackR2012b(&eh_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&eh_emlrtRSI, emlrtRootTLSGlobal);
for (loop_ub = 0; loop_ub + 1 <= m + 1; loop_ub++) {
b = T->data[loop_ub + T->size[0] * (m - 1)].re;
temp = T->data[loop_ub + T->size[0] * (m - 1)].im;
bcmax = mu1_re * T->data[loop_ub + T->size[0] * (m - 1)].re - cs *
T->data[loop_ub + T->size[0] * (m - 1)].im;
scale = mu1_re * T->data[loop_ub + T->size[0] * (m - 1)].im + cs *
T->data[loop_ub + T->size[0] * (m - 1)].re;
bb = T->data[loop_ub + T->size[0] * m].re;
p = T->data[loop_ub + T->size[0] * m].im;
T->data[loop_ub + T->size[0] * (m - 1)].re = bcmax + c * bb;
T->data[loop_ub + T->size[0] * (m - 1)].im = scale + c * p;
bb = T->data[loop_ub + T->size[0] * m].re;
p = T->data[loop_ub + T->size[0] * m].im;
bcmax = T->data[loop_ub + T->size[0] * m].im;
scale = T->data[loop_ub + T->size[0] * m].re;
T->data[loop_ub + T->size[0] * m].re = (mu1_re * bb + cs * p) - c * b;
T->data[loop_ub + T->size[0] * m].im = (mu1_re * bcmax - cs * scale) -
c * temp;
}
emlrtPushRtStackR2012b(&fh_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&fh_emlrtRSI, emlrtRootTLSGlobal);
for (loop_ub = 0; loop_ub + 1 <= mtmp; loop_ub++) {
b = U->data[loop_ub + U->size[0] * (m - 1)].re;
temp = U->data[loop_ub + U->size[0] * (m - 1)].im;
bcmax = mu1_re * U->data[loop_ub + U->size[0] * (m - 1)].re - cs *
U->data[loop_ub + U->size[0] * (m - 1)].im;
scale = mu1_re * U->data[loop_ub + U->size[0] * (m - 1)].im + cs *
U->data[loop_ub + U->size[0] * (m - 1)].re;
bb = U->data[loop_ub + U->size[0] * m].re;
p = U->data[loop_ub + U->size[0] * m].im;
U->data[loop_ub + U->size[0] * (m - 1)].re = bcmax + c * bb;
U->data[loop_ub + U->size[0] * (m - 1)].im = scale + c * p;
bb = U->data[loop_ub + U->size[0] * m].re;
p = U->data[loop_ub + U->size[0] * m].im;
bcmax = U->data[loop_ub + U->size[0] * m].im;
scale = U->data[loop_ub + U->size[0] * m].re;
U->data[loop_ub + U->size[0] * m].re = (mu1_re * bb + cs * p) - c * b;
U->data[loop_ub + U->size[0] * m].im = (mu1_re * bcmax - cs * scale) -
c * temp;
}
T->data[m + T->size[0] * (m - 1)].re = 0.0;
T->data[m + T->size[0] * (m - 1)].im = 0.0;
}
}
}
emlrtPopRtStackR2012b(&bh_emlrtRSI, emlrtRootTLSGlobal);
}
/* Function Definitions */
real_T batFrcClc_tmp(real_T batPwr, real_T vel, real_T batRstDch, real_T
batRstChr, real_T batOcv)
{
real_T batFrc;
real_T batRst;
real_T batFrcCpl_re;
real_T yr;
real_T yi;
real_T ar;
real_T ai;
real_T br;
real_T batFrcCpl_im;
/* Skalar - Batteriekraft (E') */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
/* BATFRCCLC Calculating losses in battery */
/* Batteriekraft (E') aus Verlusten in der Batterie berechnen */
/* Calucluate battery power losses (E') */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* Zero-Order-Hold (average velocity is NOT used) */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing the function output */
batFrc = rtInf;
/* % Berechnung der Verluste in der Batterie */
/* calculating battery losses */
/* Berechnung der Batterieenergienderung im Wegschritt in externer Funktion */
/* Fallunterscheidung, ob Batterie geladen oder entladen wird */
/* */
/* calculate the change in battery energy along path_idx in exterior func, */
/* distinguished whether battery is charging or discharging. */
/* Assining battery resistance value */
if (batPwr > 0.0) {
batRst = batRstDch;
} else {
batRst = batRstChr;
}
/* Batterieenergieänderung über dem Weg berechnen. Herleitung der Formel */
/* kann zum Beispiel dem Paper mit Chalmers entnommen werden */
/* calculate battery power charge for path_idx. Formula derivation can be */
/* found from other papers (for example, Chalmers paper) */
batFrcCpl_re = 1.0 - 4.0 * batRst / (batOcv * batOcv) * batPwr;
if (batFrcCpl_re < 0.0) {
yr = 0.0;
yi = muDoubleScalarSqrt(muDoubleScalarAbs(batFrcCpl_re));
} else {
yr = muDoubleScalarSqrt(batFrcCpl_re);
yi = 0.0;
}
ar = batOcv * batOcv * (yr - 1.0);
ai = batOcv * batOcv * yi;
br = 2.0 * batRst * vel;
if (ai == 0.0) {
batFrcCpl_re = ar / br;
batFrcCpl_im = 0.0;
} else if (ar == 0.0) {
batFrcCpl_re = 0.0;
batFrcCpl_im = ai / br;
} else {
batFrcCpl_re = ar / br;
batFrcCpl_im = ai / br;
}
/* Sollte die physikalisch mögliche Batterieleistung überschritten werden, */
/* wird der Term unter der Wurzel negativ. In diesem Fall wird die Ausgabe */
/* ungültig geschrieben. */
/* check to make sure that the battery capacity is not exceeded (when the */
/* root becomes negative, the output is no longer valid) (Quadrants 3, 4) */
if (batFrcCpl_im != 0.0) {
} else {
batFrc = batFrcCpl_re;
}
return batFrc;
}
static void sf_c3_car_model(SFc3_car_modelInstanceStruct *chartInstance)
{
int32_T c3_i3;
int32_T c3_i4;
int32_T c3_i5;
int32_T c3_previousEvent;
int32_T c3_i6;
real_T c3_hoistedGlobal[4];
int32_T c3_i7;
real_T c3_b_hoistedGlobal[3];
int32_T c3_i8;
real_T c3_X[4];
int32_T c3_i9;
real_T c3_u[3];
uint32_T c3_debug_family_var_map[22];
real_T c3_l_F;
real_T c3_l_R;
real_T c3_Vx;
real_T c3_Vy;
real_T c3_r;
real_T c3_psi;
real_T c3_f_Fx;
real_T c3_f_Rx;
real_T c3_delta;
real_T c3_beta;
real_T c3_V;
real_T c3_s_Fy_num;
real_T c3_s_Fy_denum;
real_T c3_s_Fy;
real_T c3_s_Ry_num;
real_T c3_s_Ry_denum;
real_T c3_s_Ry;
real_T c3_nargin = 2.0;
real_T c3_nargout = 1.0;
real_T c3_s[2];
real_T c3_A;
real_T c3_B;
real_T c3_x;
real_T c3_y;
real_T c3_b_x;
real_T c3_b_y;
real_T c3_c_x;
real_T c3_c_y;
real_T c3_d_y;
real_T c3_d_x;
real_T c3_e_x;
real_T c3_f_x;
real_T c3_g_x;
real_T c3_h_x;
real_T c3_i_x;
real_T c3_j_x;
real_T c3_a;
real_T c3_b;
real_T c3_e_y;
real_T c3_b_a;
real_T c3_f_y;
real_T c3_k_x;
real_T c3_l_x;
real_T c3_m_x;
real_T c3_c_a;
real_T c3_b_b;
real_T c3_g_y;
real_T c3_n_x;
real_T c3_o_x;
real_T c3_p_x;
real_T c3_d_a;
real_T c3_c_b;
real_T c3_h_y;
real_T c3_e_a;
real_T c3_i_y;
real_T c3_q_x;
real_T c3_r_x;
real_T c3_s_x;
real_T c3_f_a;
real_T c3_d_b;
real_T c3_j_y;
real_T c3_b_A;
real_T c3_b_B;
real_T c3_t_x;
real_T c3_k_y;
real_T c3_u_x;
real_T c3_l_y;
real_T c3_v_x;
real_T c3_m_y;
real_T c3_w_x;
real_T c3_x_x;
real_T c3_y_x;
real_T c3_g_a;
real_T c3_e_b;
real_T c3_n_y;
real_T c3_h_a;
real_T c3_o_y;
real_T c3_ab_x;
real_T c3_bb_x;
real_T c3_cb_x;
real_T c3_i_a;
real_T c3_f_b;
real_T c3_c_A;
real_T c3_c_B;
real_T c3_db_x;
real_T c3_p_y;
real_T c3_eb_x;
real_T c3_q_y;
real_T c3_fb_x;
real_T c3_r_y;
int32_T c3_i10;
real_T (*c3_b_s)[2];
real_T (*c3_b_u)[3];
real_T (*c3_b_X)[4];
c3_b_u = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 1);
c3_b_s = (real_T (*)[2])ssGetOutputPortSignal(chartInstance->S, 1);
c3_b_X = (real_T (*)[4])ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG,2);
for (c3_i3 = 0; c3_i3 < 4; c3_i3 = c3_i3 + 1) {
_SFD_DATA_RANGE_CHECK((*c3_b_X)[c3_i3], 0U);
}
for (c3_i4 = 0; c3_i4 < 2; c3_i4 = c3_i4 + 1) {
_SFD_DATA_RANGE_CHECK((*c3_b_s)[c3_i4], 1U);
}
for (c3_i5 = 0; c3_i5 < 3; c3_i5 = c3_i5 + 1) {
_SFD_DATA_RANGE_CHECK((*c3_b_u)[c3_i5], 2U);
}
c3_previousEvent = _sfEvent_;
_sfEvent_ = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG,2);
for (c3_i6 = 0; c3_i6 < 4; c3_i6 = c3_i6 + 1) {
c3_hoistedGlobal[c3_i6] = (*c3_b_X)[c3_i6];
}
for (c3_i7 = 0; c3_i7 < 3; c3_i7 = c3_i7 + 1) {
c3_b_hoistedGlobal[c3_i7] = (*c3_b_u)[c3_i7];
}
for (c3_i8 = 0; c3_i8 < 4; c3_i8 = c3_i8 + 1) {
c3_X[c3_i8] = c3_hoistedGlobal[c3_i8];
}
for (c3_i9 = 0; c3_i9 < 3; c3_i9 = c3_i9 + 1) {
c3_u[c3_i9] = c3_b_hoistedGlobal[c3_i9];
}
sf_debug_symbol_scope_push_eml(0U, 22U, 22U, c3_debug_family_names,
c3_debug_family_var_map);
sf_debug_symbol_scope_add_eml(&c3_l_F, c3_d_sf_marshall, 0U);
sf_debug_symbol_scope_add_eml(&c3_l_R, c3_d_sf_marshall, 1U);
sf_debug_symbol_scope_add_eml(&c3_Vx, c3_d_sf_marshall, 2U);
sf_debug_symbol_scope_add_eml(&c3_Vy, c3_d_sf_marshall, 3U);
sf_debug_symbol_scope_add_eml(&c3_r, c3_d_sf_marshall, 4U);
sf_debug_symbol_scope_add_eml(&c3_psi, c3_d_sf_marshall, 5U);
sf_debug_symbol_scope_add_eml(&c3_f_Fx, c3_d_sf_marshall, 6U);
sf_debug_symbol_scope_add_eml(&c3_f_Rx, c3_d_sf_marshall, 7U);
sf_debug_symbol_scope_add_eml(&c3_delta, c3_d_sf_marshall, 8U);
sf_debug_symbol_scope_add_eml(&c3_beta, c3_d_sf_marshall, 9U);
sf_debug_symbol_scope_add_eml(&c3_V, c3_d_sf_marshall, 10U);
sf_debug_symbol_scope_add_eml(&c3_s_Fy_num, c3_d_sf_marshall, 11U);
sf_debug_symbol_scope_add_eml(&c3_s_Fy_denum, c3_d_sf_marshall, 12U);
sf_debug_symbol_scope_add_eml(&c3_s_Fy, c3_d_sf_marshall, 13U);
sf_debug_symbol_scope_add_eml(&c3_s_Ry_num, c3_d_sf_marshall, 14U);
sf_debug_symbol_scope_add_eml(&c3_s_Ry_denum, c3_d_sf_marshall, 15U);
sf_debug_symbol_scope_add_eml(&c3_s_Ry, c3_d_sf_marshall, 16U);
sf_debug_symbol_scope_add_eml(&c3_nargin, c3_d_sf_marshall, 17U);
sf_debug_symbol_scope_add_eml(&c3_nargout, c3_d_sf_marshall, 18U);
sf_debug_symbol_scope_add_eml(&c3_X, c3_c_sf_marshall, 19U);
sf_debug_symbol_scope_add_eml(&c3_u, c3_b_sf_marshall, 20U);
sf_debug_symbol_scope_add_eml(&c3_s, c3_sf_marshall, 21U);
CV_EML_FCN(0, 0);
/* l_F = param.l_F; */
/* l_R = param.l_R; */
_SFD_EML_CALL(0,7);
c3_l_F = 1.1;
_SFD_EML_CALL(0,8);
c3_l_R = 1.6;
_SFD_EML_CALL(0,10);
c3_Vx = c3_X[0];
_SFD_EML_CALL(0,11);
c3_Vy = c3_X[1];
_SFD_EML_CALL(0,12);
c3_r = c3_X[2];
_SFD_EML_CALL(0,13);
c3_psi = c3_X[3];
_SFD_EML_CALL(0,15);
c3_f_Fx = c3_u[0];
_SFD_EML_CALL(0,16);
c3_f_Rx = c3_u[1];
_SFD_EML_CALL(0,17);
c3_delta = c3_u[2];
_SFD_EML_CALL(0,20);
if (CV_EML_COND(0, 0, c3_Vx == 0.0)) {
if (CV_EML_COND(0, 1, c3_Vy == 0.0)) {
CV_EML_MCDC(0, 0, TRUE);
CV_EML_IF(0, 0, TRUE);
_SFD_EML_CALL(0,21);
c3_beta = 0.0;
goto label_1;
}
}
CV_EML_MCDC(0, 0, FALSE);
CV_EML_IF(0, 0, FALSE);
_SFD_EML_CALL(0,23);
c3_A = c3_Vy;
c3_B = c3_Vx;
c3_x = c3_A;
c3_y = c3_B;
c3_b_x = c3_x;
c3_b_y = c3_y;
c3_c_x = c3_b_x;
c3_c_y = c3_b_y;
c3_d_y = c3_c_x / c3_c_y;
c3_d_x = c3_d_y;
c3_beta = c3_d_x;
c3_e_x = c3_beta;
c3_beta = c3_e_x;
c3_beta = muDoubleScalarAtan(c3_beta);
label_1:
;
_SFD_EML_CALL(0,26);
c3_f_x = c3_mpower(chartInstance, c3_Vx) + c3_mpower(chartInstance, c3_Vy);
c3_V = c3_f_x;
if (c3_V < 0.0) {
c3_eml_error(chartInstance);
}
c3_g_x = c3_V;
c3_V = c3_g_x;
c3_V = muDoubleScalarSqrt(c3_V);
_SFD_EML_CALL(0,28);
c3_h_x = c3_beta - c3_delta;
c3_i_x = c3_h_x;
c3_j_x = c3_i_x;
c3_i_x = c3_j_x;
c3_i_x = muDoubleScalarSin(c3_i_x);
c3_a = c3_V;
c3_b = c3_i_x;
c3_e_y = c3_a * c3_b;
c3_b_a = c3_r;
c3_f_y = c3_b_a * 1.1;
c3_k_x = c3_delta;
c3_l_x = c3_k_x;
c3_m_x = c3_l_x;
c3_l_x = c3_m_x;
c3_l_x = muDoubleScalarCos(c3_l_x);
c3_c_a = c3_f_y;
c3_b_b = c3_l_x;
c3_g_y = c3_c_a * c3_b_b;
c3_s_Fy_num = c3_e_y + c3_g_y;
_SFD_EML_CALL(0,29);
c3_n_x = c3_beta - c3_delta;
c3_o_x = c3_n_x;
c3_p_x = c3_o_x;
c3_o_x = c3_p_x;
c3_o_x = muDoubleScalarCos(c3_o_x);
c3_d_a = c3_V;
c3_c_b = c3_o_x;
c3_h_y = c3_d_a * c3_c_b;
c3_e_a = c3_r;
c3_i_y = c3_e_a * 1.1;
c3_q_x = c3_delta;
c3_r_x = c3_q_x;
c3_s_x = c3_r_x;
c3_r_x = c3_s_x;
c3_r_x = muDoubleScalarSin(c3_r_x);
c3_f_a = c3_i_y;
c3_d_b = c3_r_x;
c3_j_y = c3_f_a * c3_d_b;
c3_s_Fy_denum = c3_h_y + c3_j_y;
_SFD_EML_CALL(0,31);
if (CV_EML_COND(0, 2, c3_s_Fy_num == 0.0)) {
if (CV_EML_COND(0, 3, c3_s_Fy_denum == 0.0)) {
CV_EML_MCDC(0, 1, TRUE);
CV_EML_IF(0, 1, TRUE);
_SFD_EML_CALL(0,32);
c3_s_Fy = 0.0;
goto label_2;
}
}
CV_EML_MCDC(0, 1, FALSE);
CV_EML_IF(0, 1, FALSE);
_SFD_EML_CALL(0,34);
c3_b_A = c3_s_Fy_num;
c3_b_B = c3_s_Fy_denum;
c3_t_x = c3_b_A;
c3_k_y = c3_b_B;
c3_u_x = c3_t_x;
c3_l_y = c3_k_y;
c3_v_x = c3_u_x;
c3_m_y = c3_l_y;
c3_s_Fy = c3_v_x / c3_m_y;
label_2:
;
_SFD_EML_CALL(0,37);
c3_w_x = c3_beta;
c3_x_x = c3_w_x;
c3_y_x = c3_x_x;
c3_x_x = c3_y_x;
c3_x_x = muDoubleScalarSin(c3_x_x);
c3_g_a = c3_V;
c3_e_b = c3_x_x;
c3_n_y = c3_g_a * c3_e_b;
c3_h_a = c3_r;
c3_o_y = c3_h_a * 1.6;
c3_s_Ry_num = c3_n_y - c3_o_y;
_SFD_EML_CALL(0,38);
c3_ab_x = c3_beta;
c3_bb_x = c3_ab_x;
c3_cb_x = c3_bb_x;
c3_bb_x = c3_cb_x;
c3_bb_x = muDoubleScalarCos(c3_bb_x);
c3_i_a = c3_V;
c3_f_b = c3_bb_x;
c3_s_Ry_denum = c3_i_a * c3_f_b;
_SFD_EML_CALL(0,40);
if (CV_EML_COND(0, 4, c3_s_Ry_num == 0.0)) {
if (CV_EML_COND(0, 5, c3_s_Ry_denum == 0.0)) {
CV_EML_MCDC(0, 2, TRUE);
CV_EML_IF(0, 2, TRUE);
_SFD_EML_CALL(0,41);
c3_s_Ry = 0.0;
goto label_3;
}
}
CV_EML_MCDC(0, 2, FALSE);
CV_EML_IF(0, 2, FALSE);
_SFD_EML_CALL(0,43);
c3_c_A = c3_s_Ry_num;
c3_c_B = c3_s_Ry_denum;
c3_db_x = c3_c_A;
c3_p_y = c3_c_B;
c3_eb_x = c3_db_x;
c3_q_y = c3_p_y;
c3_fb_x = c3_eb_x;
c3_r_y = c3_q_y;
c3_s_Ry = c3_fb_x / c3_r_y;
label_3:
;
_SFD_EML_CALL(0,46);
c3_s[0] = c3_s_Fy;
c3_s[1] = c3_s_Ry;
_SFD_EML_CALL(0,-46);
sf_debug_symbol_scope_pop();
for (c3_i10 = 0; c3_i10 < 2; c3_i10 = c3_i10 + 1) {
(*c3_b_s)[c3_i10] = c3_s[c3_i10];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG,2);
_sfEvent_ = c3_previousEvent;
sf_debug_check_for_state_inconsistency(_car_modelMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
/* Function Definitions */
real_T Acoeff(const emlrtStack *sp, real_T ksi, real_T j, real_T x, real_T t,
const emxArray_real_T *gridT)
{
real_T vals;
int32_T k;
int32_T i0;
boolean_T b_x[3];
boolean_T y;
boolean_T exitg4;
real_T c_x;
boolean_T exitg3;
real_T z0;
boolean_T d_x[2];
boolean_T exitg2;
real_T z1;
boolean_T exitg1;
emlrtStack st;
emlrtStack b_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
/* evaluate the coefficient A at the boundary ksi=0 or ksi=1; */
/* for the index j which describes the time steps timePoints_j, at time t and space */
/* point x */
/* timePoints is a vector describing the time descritized domain */
k = gridT->size[1];
i0 = (int32_T)emlrtIntegerCheckFastR2012b(j, &emlrtDCI, sp);
if (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k, &d_emlrtBCI,
sp) - 1]) {
vals = 0.0;
} else {
k = gridT->size[1];
i0 = (int32_T)j;
b_x[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&e_emlrtBCI, sp) - 1]);
k = gridT->size[1];
i0 = (int32_T)((uint32_T)j + 1U);
b_x[1] = (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&f_emlrtBCI, sp) - 1]);
b_x[2] = (x == ksi);
y = true;
k = 0;
exitg4 = false;
while ((!exitg4) && (k < 3)) {
if (b_x[k] == 0) {
y = false;
exitg4 = true;
} else {
k++;
}
}
if (y) {
k = gridT->size[1];
i0 = (int32_T)j;
emlrtDynamicBoundsCheckFastR2012b(i0, 1, k, &c_emlrtBCI, sp);
c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931;
st.site = &emlrtRSI;
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
vals = muDoubleScalarSqrt(c_x);
} else {
k = gridT->size[1];
i0 = (int32_T)j;
b_x[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&g_emlrtBCI, sp) - 1]);
k = gridT->size[1];
i0 = (int32_T)((uint32_T)j + 1U);
b_x[1] = (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&h_emlrtBCI, sp) - 1]);
b_x[2] = (x != ksi);
y = true;
k = 0;
exitg3 = false;
while ((!exitg3) && (k < 3)) {
if (b_x[k] == 0) {
y = false;
exitg3 = true;
} else {
k++;
}
}
if (y) {
st.site = &b_emlrtRSI;
k = gridT->size[1];
i0 = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&m_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
z0 = muDoubleScalarAbs(x - ksi) / (2.0 * muDoubleScalarSqrt(c_x));
vals = muDoubleScalarAbs(x - ksi) / 3.5449077018110318 *
(muDoubleScalarExp(-(z0 * z0)) / z0 - 1.7724538509055159 * scalar_erf
(z0));
} else {
k = gridT->size[1];
i0 = (int32_T)((uint32_T)j + 1U);
d_x[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&i_emlrtBCI, sp) - 1]);
d_x[1] = (x == ksi);
y = true;
k = 0;
exitg2 = false;
while ((!exitg2) && (k < 2)) {
if (d_x[k] == 0) {
y = false;
exitg2 = true;
} else {
k++;
}
}
if (y) {
k = gridT->size[1];
i0 = (int32_T)j;
emlrtDynamicBoundsCheckFastR2012b(i0, 1, k, &b_emlrtBCI, sp);
c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931;
k = gridT->size[1];
i0 = (int32_T)(j + 1.0);
emlrtDynamicBoundsCheckFastR2012b(i0, 1, k, &emlrtBCI, sp);
z1 = (t - gridT->data[(int32_T)(j + 1.0) - 1]) / 3.1415926535897931;
st.site = &c_emlrtRSI;
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
st.site = &c_emlrtRSI;
if (z1 < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
vals = muDoubleScalarSqrt(c_x) - muDoubleScalarSqrt(z1);
} else {
k = gridT->size[1];
i0 = (int32_T)((uint32_T)j + 1U);
d_x[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&j_emlrtBCI, sp) - 1]);
d_x[1] = (x != ksi);
y = true;
k = 0;
exitg1 = false;
while ((!exitg1) && (k < 2)) {
if (d_x[k] == 0) {
y = false;
exitg1 = true;
} else {
k++;
}
}
if (y) {
st.site = &d_emlrtRSI;
k = gridT->size[1];
i0 = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&k_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
z0 = muDoubleScalarAbs(x - ksi) / (2.0 * muDoubleScalarSqrt(c_x));
st.site = &e_emlrtRSI;
k = gridT->size[1];
i0 = (int32_T)((uint32_T)j + 1U);
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i0, 1, k,
&l_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
z1 = muDoubleScalarAbs(x - ksi) / (2.0 * muDoubleScalarSqrt(c_x));
vals = muDoubleScalarAbs(x - ksi) / 3.5449077018110318 *
((muDoubleScalarExp(-(z0 * z0)) / z0 - muDoubleScalarExp(-(z1 * z1))
/ z1) + 1.7724538509055159 * (b_scalar_erf(z0) - b_scalar_erf(z1)));
} else {
vals = 0.0;
}
}
}
}
}
return vals;
}