/* Function: rt_hypot_snf =======================================================
* Abstract:
* hypot(a,b) = sqrt(a^2 + b^2)
*/
real_T rt_hypot_snf(real_T a, real_T b)
{
real_T t; /* scales a & b */
real_T retValue;
if (rtIsNaN(a) || rtIsNaN(b)) {
retValue = (rtNaN);
} else {
if (rtIsInf(a) && rtIsInf(b)) {
retValue = (rtInf);
} else {
if (fabs(a) > fabs(b)) { /* Case 1: a > b ==> t = b / a < 1.0 */
t = b/a;
retValue = (fabs(a)*sqrt(1.0 + t*t));
} else { /* Case 2: a <= b ==> t = a / b <= 1.0 */
if (b == 0.0) {
retValue = (0.0);
} else {
t = a/b;
retValue = (fabs(b)*sqrt(1.0 + t*t));
}
} /* one input is inf */
} /* one input is nan */
}
return retValue;
} /* end rt_hypot_snf */
/* Function: rt_atan2_snf =======================================================
* Abstract:
* Calls ATAN2, with guards against domain error and non-finites
*/
real_T rt_atan2_snf(real_T a, real_T b)
{
real_T retValue;
if (rtIsNaN(a) || rtIsNaN(b)) {
retValue = (rtNaN);
} else {
if (rtIsInf(a) && rtIsInf(b)) {
if (b > 0.0) {
b = 1.0;
} else {
b = -1.0;
}
if (a > 0.0) {
a = 1.0;
} else {
a = -1.0;
}
retValue = atan2(a,b);
} else if (b == 0.0) {
if (a > 0.0) {
retValue = (RT_PI)/2.0;
} else if (a < 0.0) {
retValue = -(RT_PI)/2.0;
} else {
retValue = 0.0;
}
} else {
retValue = atan2(a,b);
}
}
return retValue;
} /* end rt_atan2_snf */
/* Function: rt_rem_snf =======================================================
* Abstract:
* Calls double-precision version of REM, with guard against domain error
* and guards against non-finites
*/
real_T rt_rem_snf(const real_T xr, const real_T yr)
{
real_T zr, yrf, tr, trf, wr;
if (yr == 0.0 || rtIsInf(xr) || rtIsInf(yr) || rtIsNaN(xr) || rtIsNaN(yr)) {
zr = (rtNaN);
} else {
yrf = (yr < 0.0) ? ceil(yr) : floor(yr);
tr = xr/yr;
if (yr == yrf) {
/* Integer denominator. Use conventional formula.*/
trf = (tr < 0.0) ? ceil(tr) : floor(tr);
zr = xr - trf*yr;
} else {
/* Noninteger denominator. Check for nearly integer quotient.*/
wr = rt_round_snf(tr);
if (fabs(tr - wr) <= ((DBL_EPSILON)*fabs(tr))) {
zr = 0.0;
} else {
trf = (tr < 0.0) ? ceil(tr) : floor(tr);
zr = (tr - trf)*yr;
}
}
}
return zr;
} /* end rt_rem_snf */
/* Function Definitions */
static real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
int32_T i7;
int32_T i8;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u1 > 0.0) {
i7 = 1;
} else {
i7 = -1;
}
if (u0 > 0.0) {
i8 = 1;
} else {
i8 = -1;
}
y = atan2((real_T)i8, (real_T)i7);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
//
// Arguments : const emxArray_real_T *y
// double *miny
// double *maxy
// Return Type : void
//
void MinAndMaxNoNonFinites(const emxArray_real_T *y, double *miny, double *maxy)
{
int ny;
unsigned int k;
ny = y->size[0];
k = 1U;
while ((k <= (unsigned int)ny) && (!((!rtIsInf(y->data[(int)k - 1])) &&
(!rtIsNaN(y->data[(int)k - 1]))))) {
k++;
}
if (k > (unsigned int)y->size[0]) {
*miny = 0.0;
*maxy = 0.0;
} else {
*miny = y->data[(int)k - 1];
*maxy = y->data[(int)k - 1];
while (k <= (unsigned int)ny) {
if ((!rtIsInf(y->data[(int)k - 1])) && (!rtIsNaN(y->data[(int)k - 1]))) {
if (y->data[(int)k - 1] < *miny) {
*miny = y->data[(int)k - 1];
}
if (y->data[(int)k - 1] > *maxy) {
*maxy = y->data[(int)k - 1];
}
}
k++;
}
}
}
/* Function Definitions */
static real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
int32_T i4;
int32_T i5;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u1 > 0.0) {
i4 = 1;
} else {
i4 = -1;
}
if (u0 > 0.0) {
i5 = 1;
} else {
i5 = -1;
}
y = atan2((real_T)i5, (real_T)i4);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
double scalar_log2(double x)
{
double y;
int eint;
double fdbl;
if (x == 0.0) {
y = rtMinusInf;
} else if (x < 0.0) {
y = rtNaN;
} else if ((!rtIsInf(x)) && (!rtIsNaN(x))) {
if ((!rtIsInf(x)) && (!rtIsNaN(x))) {
fdbl = frexp(x, &eint);
} else {
fdbl = x;
eint = 0;
}
if (fdbl == 0.5) {
y = (double)eint - 1.0;
} else {
y = log(fdbl) / 0.69314718055994529 + (double)eint;
}
} else {
y = x;
}
return y;
}
real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
int32_T u0_0;
int32_T u1_0;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = (rtNaN);
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u0 > 0.0) {
u0_0 = 1;
} else {
u0_0 = -1;
}
if (u1 > 0.0) {
u1_0 = 1;
} else {
u1_0 = -1;
}
y = atan2(u0_0, u1_0);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
/*
* Arguments : double u0
* double u1
* Return Type : double
*/
static double rt_atan2d_snf(double u0, double u1)
{
double y;
int b_u0;
int b_u1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u0 > 0.0) {
b_u0 = 1;
} else {
b_u0 = -1;
}
if (u1 > 0.0) {
b_u1 = 1;
} else {
b_u1 = -1;
}
y = atan2(b_u0, b_u1);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(double)(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
void c_log(creal_T *x)
{
real_T x_re;
real_T x_im;
real_T b_x_im;
real_T b_x_re;
if ((x->im == 0.0) && rtIsNaN(x->re)) {
} else if ((fabs(x->re) > 8.9884656743115785E+307) || (fabs(x->im) >
8.9884656743115785E+307)) {
x_re = x->re;
x_im = x->im;
b_x_im = x->im;
b_x_re = x->re;
x->re = log(rt_hypotd_snf(fabs(x_re / 2.0), fabs(x_im / 2.0))) +
0.69314718055994529;
x->im = rt_atan2d_snf(b_x_im, b_x_re);
} else {
x_re = x->re;
x_im = x->im;
b_x_im = x->im;
b_x_re = x->re;
x->re = log(rt_hypotd_snf(fabs(x_re), fabs(x_im)));
x->im = rt_atan2d_snf(b_x_im, b_x_re);
}
}
static real_T c2_sign(real_T c2_X)
{
real_T c2_S;
real_T c2_k;
real_T c2_b_k;
real_T c2_b_x;
real_T c2_c_x;
boolean_T c2_b;
real_T c2_b_y;
c2_S = 0.0;
c2_k = 1.0;
c2_b_k = c2_k;
_SFD_EML_ARRAY_BOUNDS_CHECK("X", (int32_T)_SFD_INTEGER_CHECK("k", c2_b_k), 1,
1, 1);
c2_b_x = c2_X;
c2_c_x = c2_b_x;
c2_b = rtIsNaN(c2_c_x);
if(c2_b) {
_SFD_EML_ARRAY_BOUNDS_CHECK("S", (int32_T)_SFD_INTEGER_CHECK("k", c2_b_k),
1, 1, 1);
c2_b_y = rtNaN;
return c2_b_y;
} else if(c2_b_x > 0.0) {
_SFD_EML_ARRAY_BOUNDS_CHECK("S", (int32_T)_SFD_INTEGER_CHECK("k", c2_b_k),
1, 1, 1);
return 1.0;
} else if(c2_b_x < 0.0) {
_SFD_EML_ARRAY_BOUNDS_CHECK("S", (int32_T)_SFD_INTEGER_CHECK("k", c2_b_k),
1, 1, 1);
return -1.0;
}
return c2_S;
}
/* Function: rt_pow_snf =======================================================
* Abstract:
* Calls double-precision version of POW, with guard against domain error
* and guards against non-finites
*/
real_T rt_pow_snf(const real_T xr, const real_T yr)
{
real_T ret, axr, ayr;
if (rtIsNaN(xr) || rtIsNaN(yr)) {
ret = (rtNaN);
} else {
axr = fabs(xr);
ayr = fabs(yr);
if (rtIsInf(ayr)) {
if (axr == 1.0) {
ret = (rtNaN);
} else if (axr > 1.0) {
if (yr > 0.0) {
ret = (rtInf);
} else {
ret = 0.0;
}
} else {
if (yr > 0.0) {
ret = 0.0;
} else {
ret = (rtInf);
}
}
} else {
if (ayr == 0.0) {
ret = 1.0;
} else if (ayr == 1.0) {
if (yr > 0.0) {
ret = xr;
} else {
ret = 1.0/xr;
}
} else if (yr == 2.0) {
ret = xr*xr;
} else if (yr == 0.5 && xr >= 0.0) {
ret = sqrt(xr);
} else if ((xr < 0.0) && (yr > floor(yr))) {
ret = (rtNaN);
} else {
ret = pow(xr,yr);
}
}
}
return ret;
} /* end rt_pow_snf */
/* Function Definitions */
double rt_powd_snf(double u0, double u1)
{
double y;
double d0;
double d1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else {
d0 = std::abs(u0);
d1 = std::abs(u1);
if (rtIsInf(u1)) {
if (d0 == 1.0) {
y = 1.0;
} else if (d0 > 1.0) {
if (u1 > 0.0) {
y = rtInf;
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = rtInf;
}
} else if (d1 == 0.0) {
y = 1.0;
} else if (d1 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = std::sqrt(u0);
} else if ((u0 < 0.0) && (u1 > std::floor(u1))) {
y = rtNaN;
} else {
y = pow(u0, u1);
}
}
return y;
}
/* Function Definitions */
static real_T rt_powd_snf(real_T u0, real_T u1)
{
real_T y;
real_T d0;
real_T d1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else {
d0 = fabs(u0);
d1 = fabs(u1);
if (rtIsInf(u1)) {
if (d0 == 1.0) {
y = rtNaN;
} else if (d0 > 1.0) {
if (u1 > 0.0) {
y = rtInf;
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = rtInf;
}
} else if (d1 == 0.0) {
y = 1.0;
} else if (d1 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = sqrt(u0);
} else if ((u0 < 0.0) && (u1 > floor(u1))) {
y = rtNaN;
} else {
y = pow(u0, u1);
}
}
return y;
}
real_T rt_powd_snf(real_T u0, real_T u1)
{
real_T y;
real_T tmp;
real_T tmp_0;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = (rtNaN);
} else {
tmp = fabs(u0);
tmp_0 = fabs(u1);
if (rtIsInf(u1)) {
if (tmp == 1.0) {
y = (rtNaN);
} else if (tmp > 1.0) {
if (u1 > 0.0) {
y = (rtInf);
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = (rtInf);
}
} else if (tmp_0 == 0.0) {
y = 1.0;
} else if (tmp_0 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = sqrt(u0);
} else if ((u0 < 0.0) && (u1 > floor(u1))) {
y = (rtNaN);
} else {
y = pow(u0, u1);
}
}
return y;
}
/* Function Definitions */
double rt_powd_snf(double u0, double u1)
{
double y;
double d20;
double d21;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else {
d20 = fabs(u0);
d21 = fabs(u1);
if (rtIsInf(u1)) {
if (d20 == 1.0) {
y = rtNaN;
} else if (d20 > 1.0) {
if (u1 > 0.0) {
y = rtInf;
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = rtInf;
}
} else if (d21 == 0.0) {
y = 1.0;
} else if (d21 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = sqrt(u0);
} else if ((u0 < 0.0) && (u1 > floor(u1))) {
y = rtNaN;
} else {
y = pow(u0, u1);
}
}
return y;
}
/*
* Arguments : const double X[15000]
* const double Y[15000]
* const double Z[15000]
* double xi
* double yi
* Return Type : double
*/
double codegen_interp2(const double X[15000], const double Y[15000], const
double Z[15000], double xi, double yi)
{
double Zi;
double idyi;
double idxi;
/* zi = codegen_interp2(X,Y,Z,xi,yi) gives the same result as */
/* interp2(X,Y,Z,xi,yi) */
/* Unlike interp2, codegen_interp2 is compatible with code generation */
/* Only linear interpolation is available */
/* Usage restrictions */
/* X and Y must have the same size as Z */
/* e.g., [X,Y] = meshgrid(x,y); */
idyi = (xi - X[0]) * (1.0 / (X[150] - X[0])) + 1.0;
idxi = (yi - Y[0]) * (1.0 / (Y[1] - Y[0])) + 1.0;
if ((idxi <= 1.0) || (idyi <= 1.0) || (idxi > 150.0) || (idyi > 100.0) ||
rtIsNaN(xi) || rtIsNaN(yi)) {
Zi = rtInf;
} else if ((idxi / ceil(idxi) != 1.0) && (idyi / ceil(idyi) != 1.0)) {
Zi = ((Z[((int)(double)(ceil(idxi) - 1.0) + 150 * ((int)(double)(ceil(idyi)
- 1.0) - 1)) - 1] * (ceil(idxi) - idxi) * (ceil(idyi) - idyi) + Z
[((int)(double)(ceil(idxi) - 1.0) + 150 * ((int)ceil(idyi) - 1)) - 1]
* (ceil(idxi) - idxi) * (1.0 - (ceil(idyi) - idyi))) + Z[((int)ceil
(idxi) + 150 * ((int)(double)(ceil(idyi) - 1.0) - 1)) - 1] * (1.0 -
(ceil(idxi) - idxi)) * (ceil(idyi) - idyi)) + Z[((int)ceil(idxi) +
150 * ((int)ceil(idyi) - 1)) - 1] * (1.0 - (ceil(idxi) - idxi)) * (1.0 -
(ceil(idyi) - idyi));
} else if ((idxi / ceil(idxi) != 1.0) && (idyi / ceil(idyi) == 1.0)) {
Zi = Z[((int)(double)(ceil(idxi) - 1.0) + 150 * ((int)idyi - 1)) - 1] *
(ceil(idxi) - idxi) + Z[((int)ceil(idxi) + 150 * ((int)idyi - 1)) - 1] *
(1.0 - (ceil(idxi) - idxi));
} else if ((idxi / ceil(idxi) == 1.0) && (idyi / ceil(idyi) != 1.0)) {
Zi = Z[((int)idxi + 150 * ((int)(double)(ceil(idyi) - 1.0) - 1)) - 1] *
(ceil(idyi) - idyi) + Z[((int)idxi + 150 * ((int)ceil(idyi) - 1)) - 1] *
(1.0 - (ceil(idyi) - idyi));
} else {
Zi = Z[((int)idxi + 150 * ((int)idyi - 1)) - 1];
}
return Zi;
}
static real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
y = atan2(u0 > 0.0 ? 1.0 : -1.0, u1 > 0.0 ? 1.0 : -1.0);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
// 由 VisionData 得到 M_otherMakers
void CHybidTrack::Get_M_otherMakers()
{
/// 最新的视觉数据
CVisionData_t& VisionData_t = VisionData.back();// 末尾 最新 的视觉数据
// 马克点的个数
int m_OtherMarkersN = VisionData_t.m_OtherMarkersN;
// 列表个数：当前采集的视觉数据 个数
int visualTimeN;
visualTimeN = VisionData.size()-1;
/// 转换为 MATLAB 识别的格式
// 将 VisionData_t 存到 M_otherMakers 的第 visualTimeN 个 （假定一次回调只会接收一个数据）
M_otherMakers[visualTimeN].frequency = m_V_Frequency;
// time 存取从开始采集到当前帧的时间
M_otherMakers[visualTimeN].time = VisionData_t.m_fLatency;
M_otherMakers[visualTimeN].inertial_k = VisionData_t.m_mappingInertial_k;
// 将视觉的视觉 减去 视觉起始采集 到 惯性起始采集时间
if (!rtIsNaN(m_fLatency_StartTwo))
{
M_otherMakers[visualTimeN].time = M_otherMakers[visualTimeN].time - m_fLatency_StartTwo;
}
M_otherMakers[visualTimeN].otherMakersN = m_OtherMarkersN;
double m_fTimestamp = VisionData_t.m_fTimestamp;
for (int i = 0; i < m_OtherMarkersN; i++)
{
// 将 m_OtherMarkersN 个马克点逐个导入 M_otherMakers[visualTimeN].Position
// M_otherMakers[visualTimeN].Position 为 [3*m_OtherMarkersN]
// 第一个马克点：M_otherMakers[visualTimeN].Position[:,i] 为一列：[0 + i * 3] [1 + i * 3] [2 + i * 3] 对应 m_OtherMarkersP[0].X Y Z
Point3D_t m_OtherMarkersP_i = VisionData_t.m_OtherMarkersP[i]; // 第一个点为 m_OtherMarkersP[0]
M_otherMakers[visualTimeN].Position[0 + i * 3] = m_OtherMarkersP_i.X;
M_otherMakers[visualTimeN].Position[1 + i * 3] = m_OtherMarkersP_i.Y;
M_otherMakers[visualTimeN].Position[2 + i * 3] = m_OtherMarkersP_i.Z;
}
}
real_T rt_hypotd_snf(real_T u0, real_T u1)
{
real_T y;
real_T a;
a = fabs(u0);
y = fabs(u1);
if (a < y) {
a /= y;
y *= sqrt(a * a + 1.0);
} else if (a > y) {
y /= a;
y = a * sqrt(y * y + 1.0);
} else if (rtIsNaN(y)) {
} else {
y = a * 1.4142135623730951;
}
return y;
}
/* Function: rt_round_snf =======================================================
* Abstract:
* Calls double-precision version of ROUND, with guard against domain error
* and guards against non-finites
*/
real_T rt_round_snf(const real_T xr)
{
real_T zr, axr;
if (rtIsNaN(xr)) {
zr = xr;
} else {
axr = fabs(xr);
if (axr < 4.5035996273704960E+015) {
if (xr < 0.0) {
zr = -floor(axr + 0.5);
} else {
zr = floor(axr + 0.5);
}
} else {
zr = xr;
}
}
return zr;
} /* end rt_round_snf */
/*
* Arguments : double u0
* double u1
* Return Type : double
*/
double rt_hypotd_snf(double u0, double u1)
{
double y;
double a;
double b;
a = fabs(u0);
b = fabs(u1);
if (a < b) {
a /= b;
y = b * sqrt(a * a + 1.0);
} else if (a > b) {
b /= a;
y = a * sqrt(b * b + 1.0);
} else if (rtIsNaN(b)) {
y = b;
} else {
y = a * 1.4142135623730951;
}
return y;
}
/*
* Arguments : const double Y[128]
* double iPk_data[]
* int iPk_size[1]
* Return Type : void
*/
static void removePeaksBelowThreshold(const double Y[128], double iPk_data[],
int iPk_size[1])
{
unsigned char unnamed_idx_0;
double base_data[128];
int k;
boolean_T Y_data[128];
int Y_size[1];
int i6;
int tmp_size[1];
int tmp_data[256];
unnamed_idx_0 = (unsigned char)iPk_size[0];
for (k = 0; k + 1 <= unnamed_idx_0; k++) {
if ((Y[(int)(iPk_data[k] - 1.0) - 1] >= Y[(int)(iPk_data[k] + 1.0) - 1]) ||
rtIsNaN(Y[(int)(iPk_data[k] + 1.0) - 1])) {
base_data[k] = Y[(int)(iPk_data[k] - 1.0) - 1];
} else {
base_data[k] = Y[(int)(iPk_data[k] + 1.0) - 1];
}
}
Y_size[0] = iPk_size[0];
k = iPk_size[0];
for (i6 = 0; i6 < k; i6++) {
Y_data[i6] = (Y[(int)iPk_data[i6] - 1] - base_data[i6] >= 0.0);
}
eml_li_find(Y_data, Y_size, tmp_data, tmp_size);
k = tmp_size[0];
for (i6 = 0; i6 < k; i6++) {
base_data[i6] = iPk_data[tmp_data[i6] - 1];
}
iPk_size[0] = tmp_size[0];
k = tmp_size[0];
for (i6 = 0; i6 < k; i6++) {
iPk_data[i6] = base_data[i6];
}
}
/*
* function offsetcost = equateoffsetcost(pathqi)
*/
void equateoffsetcost(const emxArray_real_T *pathqi, emxArray_real_T *offsetcost)
{
emxArray_real_T *b_pathqi;
int32_T n;
int32_T c_pathqi;
int32_T ixstart;
uint32_T uv0[2];
int32_T k;
emxArray_int32_T *r75;
int32_T exitg3;
real_T mtmp;
real_T b_mtmp;
boolean_T exitg2;
boolean_T exitg1;
b_emxInit_real_T(&b_pathqi, 2);
/* UNTITLED2 Summary of this function goes here */
/* Detailed explanation goes here */
/* 'equateoffsetcost:6' pathoffsetcost = abs(pathqi(end,:)); */
n = pathqi->size[1];
c_pathqi = pathqi->size[0];
ixstart = b_pathqi->size[0] * b_pathqi->size[1];
b_pathqi->size[0] = 1;
b_pathqi->size[1] = n;
emxEnsureCapacity((emxArray__common *)b_pathqi, ixstart, (int32_T)sizeof
(real_T));
ixstart = n - 1;
for (n = 0; n <= ixstart; n++) {
b_pathqi->data[b_pathqi->size[0] * n] = pathqi->data[(c_pathqi +
pathqi->size[0] * n) - 1];
}
for (n = 0; n < 2; n++) {
uv0[n] = (uint32_T)b_pathqi->size[n];
}
emxFree_real_T(&b_pathqi);
n = offsetcost->size[0] * offsetcost->size[1];
offsetcost->size[0] = 1;
offsetcost->size[1] = (int32_T)uv0[1];
emxEnsureCapacity((emxArray__common *)offsetcost, n, (int32_T)sizeof(real_T));
k = 0;
b_emxInit_int32_T(&r75, 1);
do {
exitg3 = 0;
n = pathqi->size[1];
ixstart = r75->size[0];
r75->size[0] = n;
emxEnsureCapacity((emxArray__common *)r75, ixstart, (int32_T)sizeof(int32_T));
ixstart = n - 1;
for (n = 0; n <= ixstart; n++) {
r75->data[n] = 1 + n;
}
if (k <= r75->size[0] - 1) {
c_pathqi = pathqi->size[0];
mtmp = pathqi->data[(c_pathqi + pathqi->size[0] * k) - 1];
offsetcost->data[k] = fabs(mtmp);
k++;
} else {
exitg3 = 1;
}
} while (exitg3 == 0U);
emxFree_int32_T(&r75);
/* 'equateoffsetcost:8' minoffsetcost = min(pathoffsetcost); */
ixstart = 1;
n = offsetcost->size[1];
b_mtmp = offsetcost->data[0];
if (n > 1) {
if (rtIsNaN(offsetcost->data[0])) {
c_pathqi = 2;
exitg2 = FALSE;
while ((exitg2 == 0U) && (c_pathqi <= n)) {
ixstart = c_pathqi;
if (!rtIsNaN(offsetcost->data[c_pathqi - 1])) {
b_mtmp = offsetcost->data[c_pathqi - 1];
exitg2 = TRUE;
} else {
c_pathqi++;
}
}
}
if (ixstart < n) {
while (ixstart + 1 <= n) {
if (offsetcost->data[ixstart] < b_mtmp) {
b_mtmp = offsetcost->data[ixstart];
}
ixstart++;
}
}
}
/* 'equateoffsetcost:9' maxoffsetcost = max(pathoffsetcost); */
ixstart = 1;
n = offsetcost->size[1];
mtmp = offsetcost->data[0];
if (n > 1) {
if (rtIsNaN(offsetcost->data[0])) {
c_pathqi = 2;
exitg1 = FALSE;
while ((exitg1 == 0U) && (c_pathqi <= n)) {
ixstart = c_pathqi;
if (!rtIsNaN(offsetcost->data[c_pathqi - 1])) {
mtmp = offsetcost->data[c_pathqi - 1];
exitg1 = TRUE;
} else {
c_pathqi++;
}
}
}
if (ixstart < n) {
while (ixstart + 1 <= n) {
if (offsetcost->data[ixstart] > mtmp) {
mtmp = offsetcost->data[ixstart];
}
ixstart++;
}
}
}
/* 'equateoffsetcost:10' offsetcost = (pathoffsetcost-minoffsetcost)*(1/(maxoffsetcost - minoffsetcost)); */
mtmp = 1.0 / (mtmp - b_mtmp);
n = offsetcost->size[0] * offsetcost->size[1];
offsetcost->size[0] = 1;
offsetcost->size[1] = offsetcost->size[1];
emxEnsureCapacity((emxArray__common *)offsetcost, n, (int32_T)sizeof(real_T));
ixstart = offsetcost->size[0];
n = offsetcost->size[1];
ixstart = ixstart * n - 1;
for (n = 0; n <= ixstart; n++) {
offsetcost->data[n] = (offsetcost->data[n] - b_mtmp) * mtmp;
}
}
/*
* CLCPMP Minimizing Hamiltonian: Co-States for soc and time
* Erstellungsdatum der ersten Version 19.08.2015 - Stephan Uebel
*
* Batterieleistungsgrenzen hinzugefügt am 13.12.2015
* ^^added battery power limit
*
* Massenaufschlag durch Trägheitsmoment herausgenommen
* ^^Mass increment removed by inertia
*
* % Inputdefinition
*
* engKinPre - Double(1,1) - kinetische Energie am Intervallanfang in J
* ^^ kinetic energy at start of interval (J)
* engKinAct - Double(1,1) - kinetische Energie am Intervallende in J
* ^^ kinetic energe at end of interval (J)
* gea - Double(1,1) - Gang
* ^^ gear
* slp - Double(1,1) - Steigung in rad
* ^^ slope in radians
* iceFlg - Boolean(1,1) - Flag für Motorzustand
* ^^ flag for motor condition
* batEng - Double(1,1) - Batterieenergie in J
* ^^ battery energy (J)
* psibatEng - Double(1,1) - Costate für Batterieenergie ohne Einheit
* ^^ costate for battery energy w/o unity
* psiTim - Double(1,1) - Costate für die Zeit ohne Einheit
* ^^ costate for time without unity
* batPwrAux - Double(1,1) - elektr. Nebenverbraucherleistung in W
* ^^ electric auxiliary power consumed (W)
* batEngStp - Double(1,1) - Drehmomentschritt
* ^^ torque step <- no, it's a battery step
* wayStp - Double(1,1) - Intervallschrittweite in m
* ^^ interval step distance (m)
* fzg_scalar - Struct(1,1) - Modelldaten - nur skalar
* fzg_array - Struct(1,1) - Modelldaten _ nur arrays
* Arguments : double engKinPre
* double engKinAct
* double gea
* double slp
* double batEng
* double psiBatEng
* double psiTim
* double batPwrAux
* double batEngStp
* double wayStp
* const struct2_T *fzg_scalar
* const struct4_T *fzg_array
* double *cosHamMin
* double *batFrcOut
* double *fulFrcOut
* Return Type : void
*/
void clcPMP_olyHyb_port(double engKinPre, double engKinAct, double gea, double
slp, double batEng, double psiBatEng, double psiTim, double batPwrAux, double
batEngStp, double wayStp, const struct2_T *fzg_scalar, const struct4_T
*fzg_array, double *cosHamMin, double *batFrcOut, double *fulFrcOut)
{
double vehVel;
double b_engKinPre[2];
double crsSpdVec[2];
int k;
boolean_T y;
boolean_T exitg3;
boolean_T exitg2;
double crsSpd;
double whlTrq;
double crsTrq;
double iceTrqMax;
double iceTrqMin;
double b_fzg_array[100];
double emoTrqMaxPos;
double emoTrqMinPos;
double emoTrqMax;
double emoTrqMin;
double batPwrMax;
double batPwrMin;
double batOcv;
double batEngDltMin;
double batEngDltMax;
double batEngDltMinInx;
double batEngDltMaxInx;
int batEngDltInx;
double batEngDlt;
double fulFrc;
double batFrc;
double b_batFrc;
double batPwr;
double emoTrq;
double iceTrq;
double fulPwr;
int ixstart;
double mtmp;
int itmp;
int ix;
boolean_T exitg1;
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Ausgabewert des Minimums der Hamiltonfunktion */
/* output for minimizing the hamiltonian */
*cosHamMin = rtInf;
/* Batterieladungsänderung im Wegschritt beir minimaler Hamiltonfunktion */
/* battery change in path_idx step with the minial hamiltonian */
*batFrcOut = rtInf;
/* Kraftstoffkraftänderung im Wegschritt bei minimaler Hamiltonfunktion */
/* fuel change in path_idx step with the minimal hamiltonian */
*fulFrcOut = 0.0;
/* % Initialisieren der persistent Größen */
/* initialize the persistance variables */
/* Diese werden die nur einmal für die Funktion berechnet */
/* only calculated once for the function */
if (!crsSpdHybMax_not_empty) {
/* maximale Drehzahl Elektrommotor */
/* maximum electric motor rotational speed */
/* maximale Drehzahl der Kurbelwelle */
/* maximum crankshaft rotational speed */
if ((fzg_array->iceSpdMgd[14850] <= fzg_array->emoSpdMgd[14850]) || rtIsNaN
(fzg_array->emoSpdMgd[14850])) {
crsSpdHybMax = fzg_array->iceSpdMgd[14850];
} else {
crsSpdHybMax = fzg_array->emoSpdMgd[14850];
}
crsSpdHybMax_not_empty = true;
/* minimale Drehzahl der Kurbelwelle */
/* minimum crankshaft rotational speed */
crsSpdHybMin = fzg_array->iceSpdMgd[0];
}
/* % Initialisieren der allgemein benötigten Kenngrößen */
/* initializing the commonly required parameters */
/* mittlere kinetische Energie im Wegschritt berechnen */
/* define the average kinetic energy at path_idx step - is just previous KE */
/* mittlere Geschwindigkeit im Wegschritt berechnen */
/* define the average speed at path_idx step */
vehVel = sqrt(2.0 * engKinPre / fzg_scalar->vehMas);
// printf("vehVel: %4.3f\n", vehVel);
/* % vorzeitiger Funktionsabbruch? */
/* premature function termination? */
/* Drehzahl der Kurbelwelle und Grenzen */
/* crankshaft speed and limits */
/* Aus den kinetischen Energien des Fahrzeugs wird über die Raddrehzahl */
/* und die übersetzung vom Getriebe die Kurbelwellendrehzahl berechnet. */
/* Zeilenrichtung entspricht den Gängen. (Zeilenvektor) */
/* from the vehicle's kinetic energy, the crankshaft speed is calculated */
/* by the speed and gearbox translation. Line direction corresponding to */
/* the aisles (row rector). EQUATION 1 */
// fzg_scalar->geaRat[] is returning 0!
b_engKinPre[0] = engKinPre;
b_engKinPre[1] = engKinAct;
for (k = 0; k < 2; k++) {
crsSpdVec[k] = fzg_array->geaRat[(int)gea - 1] * sqrt(2.0 * b_engKinPre[k] /
fzg_scalar->vehMas) / fzg_scalar->whlDrr;
// printf("crsSpdVec[%d]: %4.3f\n", k, crsSpdVec[k]);
// printf("(int)gea: %d\n", (int)gea);
// printf("(int)gea - 1: %d\n", (int)gea - 1);
// printf("fzg_array->geaRat[(int)gea - 1]: %4.3f\n", fzg_array->geaRat[(int)gea - 1]);
// printf("fzg_array->geaRat[5]: %4.3f\n", fzg_array->geaRat[5]);
}
/* Abbruch, wenn die Drehzahlen der Kurbelwelle zu hoch im hybridischen */
/* Modus */
/* stop if the crankshaft rotatoinal speed is too high in hybrid mode */
y = false;
k = 0;
exitg3 = false;
while ((!exitg3) && (k < 2)) {
// printf("crsSpdVec[%d]: %4.3f\n", k, crsSpdVec[k]);
// printf("crsSpdHybMax: %4.3f\n", crsSpdHybMax);
if (!!(crsSpdVec[k] > crsSpdHybMax)) {
y = true;
exitg3 = true;
} else {
k++;
}
}
// printf("STATUS OF Y(1): %d\n", y);
// if (!y)
// printf("11111111111111111111111111111111111111111111111111111111111\n");
if (y) {
} else {
/* Falls die Drehzahl des Verbrennungsmotors niedriger als die */
/* Leerlaufdrehzahl ist, */
/* stop if crankhaft rotional speed is lower than the idling speed */
y = false;
k = 0;
exitg2 = false;
while ((!exitg2) && (k < 2)) {
// printf("crsSpdVec[%d]: %4.3f\n", k, crsSpdVec[k]);
// printf("crsSpdHybMin: %4.3f\n", crsSpdHybMin);
if (!!(crsSpdVec[k] < crsSpdHybMin)) {
y = true;
exitg2 = true;
} else {
k++;
}
}
// printf("STATUS OF Y(2): %d\n", y);
// if (!y)
// printf("2222222222222222222222222222222222222222222222222222222222\n");
if (y) {
} else {
/* Prüfen, ob die Drehzahlgrenze des Elektromotors eingehalten wird */
/* check if electric motor speed limit is maintained */
/* mittlere Kurbelwellendrehzahlen berechnen */
/* calculate average crankshaft rotational speed */
/* - really just selecting the previous path_idx KE crankshaft speed */
crsSpd = crsSpdVec[0];
/* % Längsdynamik berechnen */
/* calculate longitundinal dynamics */
/* Es wird eine konstante Beschleunigung angenommen, die im Wegschritt */
/* wayStp das Fahrzeug von velPre auf velAct beschleunigt. */
/* constant acceleration assumed when transitioning from velPre to velAct */
/* for the selected wayStp path_idx step distance */
/* Berechnen der konstanten Beschleunigung */
/* calculate the constant acceleration */
/* Aus der mittleren kinetischen Energie im Intervall, der mittleren */
/* Steigung und dem Gang lässt sich über die Fahrwiderstandsgleichung */
/* die nötige Fahrwiderstandskraft berechnen, die aufgebracht werden */
/* muss, um diese zu realisieren. */
/* from the (avg) kinetic energy in the interval, the (avg) slope and */
/* transition can calculate the necessary traction force on the driving */
/* resistance equation (PART OF EQUATION 5) */
/* Steigungskraft aus der mittleren Steigung berechnen (Skalar) */
/* gradiant force from the calculated (average) gradient */
/* Rollreibungskraft berechnen (Skalar) */
/* calculated rolling friction force - not included in EQ 5??? */
/* Luftwiderstandskraft berechnen (2*c_a/m * E_kin) (Skalar) */
/* calculated air resistance force */
/* % Berechnung der minimalen kosten der Hamiltonfunktion */
/* Calculating the minimum cost of the Hamiltonian */
/* % Berechnen der Kraft am Rad für Antriebsstrangmodus */
/* calculate the force on the wheel for the drivetrain mode */
/* % dynamische Fahrzeugmasse bei Fahrzeugmotor an berechnen. Das */
/* % heißt es werden Trägheitsmoment von Verbrennungsmotor, */
/* % Elektromotor und Rädern mit einbezogen. */
/* calculate dynamic vehicle mass with the vehicle engine (with the moment */
/* of intertia of the ICE, electric motor, and wheels) */
/* vehMasDyn = (par.iceMoi_geaRat(gea) +... */
/* par.emoGeaMoi_geaRat(gea) + par.whlMoi)/par.whlDrr^2 ... */
/* + par.vehMas; */
/* Radkraft berechnen (Beschleunigungskraft + Steigungskraft + */
/* Rollwiderstandskraft + Luftwiderstandskraft) */
/* caluclating wheel forces (accerlation force + gradient force + rolling */
/* resistance + air resistance) EQUATION 5 */
/* % Getriebeübersetzung und -verlust */
/* gear ratio and loss */
/* Das Drehmoment des Rades ergibt sich über den Radhalbmesser aus */
/* der Fahrwiderstandskraft. */
/* the weel torque is obtained from the wheel radius of the rolling */
/* resistance force (torque = force * distance (in this case, radius) */
whlTrq = ((((engKinAct - engKinPre) / (fzg_scalar->vehMas * wayStp) *
fzg_scalar->vehMas + fzg_scalar->vehMas * 9.81 * sin(slp)) +
fzg_scalar->whlRosResCof * fzg_scalar->vehMas * 9.81 * cos(slp))
+ 2.0 * fzg_scalar->drgCof / fzg_scalar->vehMas * engKinPre) *
fzg_scalar->whlDrr;
// printf("whlTrq: %4.3f\n", whlTrq);
/* Berechnung des Kurbelwellenmoments */
/* Hier muss unterschieden werden, ob das Radmoment positiv oder */
/* negativ ist, da nur ein einfacher Wirkungsgrad für das Getriebe */
/* genutzt wird */
/* it's important to determine sign of crankshaft torque (positive or */
/* negative), since only a simple efficiency is used for the transmission */
/* PART OF EQ4 <- perhaps reversed? not too sure */
if (whlTrq < 0.0) {
crsTrq = whlTrq / fzg_array->geaRat[(int)gea - 1] * fzg_scalar->geaEfy;
} else {
crsTrq = whlTrq / fzg_array->geaRat[(int)gea - 1] / fzg_scalar->geaEfy;
}
// printf("crsTrq: %4.3f\n", crsTrq);
/* % Verbrennungsmotor */
/* internal combustion engine */
/* maximales Moment des Verbrennungsmotors berechnen */
/* calculate max torque of the engine (quadratic based on rotation speed) */
iceTrqMax = (fzg_array->iceTrqMaxCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
fzg_array->iceTrqMaxCof[1] * crsSpdVec[0]) +
fzg_array->iceTrqMaxCof[2];
/* minimales Moment des Verbrennungsmotors berechnen */
/* calculating mimimum ICE moment */
iceTrqMin = (fzg_array->iceTrqMinCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
fzg_array->iceTrqMinCof[1] * crsSpdVec[0]) +
fzg_array->iceTrqMinCof[2];
// printf("iceTrqMax: %4.3f\n", iceTrqMax);
// printf("iceTrqMin: %4.3f\n", iceTrqMin);
/* % Elektromotor */
/* electric motor */
/* maximales Moment, dass die E-Maschine liefern kann */
/* max torque that the electric motor can provide - from interpolation */
/* emoTrqMaxPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMax_emoSpd,crsSpd); */
// for (k = 0; k < 100; k++) {
// b_fzg_array[k] = fzg_array->emoSpdMgd[150 * k];
//// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
// }
for (k = 0; k < 100; k++) {
b_fzg_array[k] = fzg_array->emoSpdMgd[100 * k + k];
// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
}
emoTrqMaxPos = interp1q(b_fzg_array, fzg_array->emoTrqMax_emoSpd,
crsSpdVec[0]);
// printf("emoTrqMaxPos: %4.3f\n", emoTrqMaxPos);
/* Die gültigen Kurbelwellenmomente müssen kleiner sein als das */
/* Gesamtmoment von E-Motor und Verbrennungsmotor */
/* The valid crankshaft moments must be less than the total moment of the */
/* electric motor and the ICE.Otherwise, leave the function */
if (crsTrq > iceTrqMax + emoTrqMaxPos) {
// printf("\nWHOOPS! crsTrq is > iceTrqMax + emoTrqMaxPos\n");
} else {
/* % %% Optimaler Momentensplit - Minimierung der Hamiltonfunktion */
/* optimum torque split - minimizing the Hamiltonian */
/* Die Vorgehensweise ist ähnlich wie bei der ECMS. Es wird ein Vektor der */
/* möglichen Batterieenergieänderungen aufgestellt. Aus diesen lässt sich */
/* eine Batterieklemmleistung berechnen. Aus der über das */
/* Kurbelwellenmoment, ein Elektromotormoment berechnet werden kann. */
/* Über das geforderte Kurbelwellenmoment, kann für jedes Moment des */
/* Elektromotors ein Moment des Verbrennungsmotors gefunden werden. Für */
/* jedes Momentenpaar kann die Hamiltonfunktion berechnet werden. */
/* Ausgegeben wird der minimale Wert der Hamiltonfunktion und die */
/* durch das dabei verwendete Elektromotormoment verursachte */
/* Batterieladungsänderung. */
/* The procedure is similar to ECMS. It's based on a vector of possible */
/* battery energy changes, from which a battery terminal power can be */
/* calculated. */
/* From the crankshaft torque, an electric motor torque can be */
/* calculated, and an engine torque can be found for every electric motor */
/* moment. */
/* For every moment-pair the Hamiltonian can be calculated. It */
/* outputs the minimum Hamilotnian value and the battery charge change */
/* caused by the electric motor torque used. */
/* % Elektromotor - Aufstellen des Batterienergievektors */
/* electric motor - positioning the battery energy vectors */
if (batEngStp > 0.0) {
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* FUNCTION CALL */
/* Skalar - Wegschrittweite */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque allowed */
/* Skalar - max EM torque possible */
/* Skalar - Wegschrittweite */
/* Skalar - Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* Skalar - max EM torque possible */
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* BatEngDltClc Calculates the change in battery energy */
/* */
/* Erstellungsdatum der ersten Version 17.11.2015 - Stephan Uebel */
/* Berechnung der Verluste des Elektromotors bei voller rein elektrischer */
/* Fahrt (voller Lastpunktabsenkung) und bei voller Lastpunktanhebung */
/* Calculations of loss of electric motor at purely full electric */
/* Driving (full load point lowering) and at full load point raising */
/* */
/* Version vom 17.02.2016: Keine Einbeziehung von Rotationsmassen */
/* ^^ No inclusion of rotational masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* ^^ Zero-Order-Hold (no average velocities) */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing the function output (delta battery_energy min and max) */
/* % Elektromotor */
/* minimales Moment, dass die E-Maschine liefern kann */
/* minimum moment that the EM can provide (max is an input to function) */
/* emoTrqMinPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMin_emoSpd,crsSpd); */
// for (k = 0; k < 100; k++) {
// b_fzg_array[k] = fzg_array->emoSpdMgd[150 * k];
// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
// }
// for (k = 0; k < 100; k++)
// printf("b_fzg_array[%d]: %4.3f\n", k, b_fzg_array[k]);
emoTrqMinPos = interp1q(b_fzg_array, fzg_array->emoTrqMin_emoSpd,
crsSpdVec[0]);
// printf("emoTrqMinPos: %4.3f\n", emoTrqMinPos);
/* % Verbrennungsmotor berechnen */
/* Durch EM zu lieferndes Kurbelwellenmoment */
/* crankshaft torque to be delivered by the electric motor (min and max) */
emoTrqMax = crsTrq - iceTrqMin;
emoTrqMin = crsTrq - iceTrqMax;
// printf("emoTrqMax: %4.3f\n", emoTrqMax);
// printf("emoTrqMin: %4.3f\n", emoTrqMin);
/* % Elektromotor berechnen */
/* calculate the electric motor */
if (emoTrqMaxPos < emoTrqMax) {
/* Moment des Elektromotors bei maximaler Entladung der Batterie */
/* EM torque at max battery discharge */
emoTrqMax = emoTrqMaxPos;
}
if (emoTrqMaxPos < emoTrqMin) {
/* Moment des Elektromotors bei minimaler Entladung der Batterie */
/* EM torque at min battery discharge */
emoTrqMin = emoTrqMaxPos;
}
if ((emoTrqMinPos >= emoTrqMax) || rtIsNaN(emoTrqMax)) {
emoTrqMax = emoTrqMinPos;
}
if ((emoTrqMinPos >= emoTrqMin) || rtIsNaN(emoTrqMin)) {
emoTrqMin = emoTrqMinPos;
}
/* % Berechnung der änderung der Batterieladung */
/* calculating the change in battery charge */
/* Interpolation der benötigten Batterieklemmleistung f�r das */
/* EM-Moment. Stellen die nicht definiert sind, werden mit inf */
/* ausgegeben. Positive Werte entsprechen entladen der Batterie. */
/* interpolating the required battery terminal power for the EM torque. */
/* Assign undefined values to inf. Positive values coresspond with battery */
/* discharge. */
/* batPwrMax = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMax) + batPwrAux; */
/* */
/* batPwrMin = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMin) + batPwrAux; */
batPwrMax = codegen_interp2(fzg_array->emoSpdMgd, fzg_array->emoTrqMgd,
fzg_array->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMax) +
batPwrAux;
// for (k = 0; k < 15000; k++)
// printf(" fzg_array->emoSpdMgd[%d]: %4.3f\n", k, fzg_array->emoSpdMgd[k]);
//
// for (k = 0; k < 15000; k++)
// printf(" fzg_array->emoTrqMgd[%d]: %4.3f\n", k, fzg_array->emoTrqMgd[k]);
//
// for (k = 0; k < 15000; k++)
// printf(" fzg_array->emoPwr_emoSpd_emoTrq[%d]: %4.3f\n", k, fzg_array->emoPwr_emoSpd_emoTrq[k]);
// printf(" crsSpdVec[0]: %4.3f\n", crsSpdVec[0]);
// printf(" emoTrqMax: %4.3f\n", emoTrqMax);
// printf("\n batPwrMax: %4.3f\n", batPwrMax);
batPwrMin = codegen_interp2(fzg_array->emoSpdMgd, fzg_array->emoTrqMgd,
fzg_array->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMin) +
batPwrAux;
// printf(" batPwrMin: %4.3f\n", batPwrMin);
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery max power is not above bat max bounds */
if (batPwrMax > fzg_scalar->batPwrMax) {
batPwrMax = fzg_scalar->batPwrMax;
}
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery min power is not below bat min bounds */
if (batPwrMin > fzg_scalar->batPwrMax) {
batPwrMin = fzg_scalar->batPwrMax;
}
/* Es kann vorkommen, dass mehr Leistung gespeist werden soll, als */
/* m�glich ist. */
/* double check that the max and min still remain within the other bounds */
if (batPwrMax < fzg_scalar->batPwrMin) {
batPwrMax = fzg_scalar->batPwrMin;
}
if (batPwrMin < fzg_scalar->batPwrMin) {
batPwrMin = fzg_scalar->batPwrMin;
}
/* Batteriespannung aus Kennkurve berechnen */
/* calculating battery voltage of characteristic curve - eq?-------------- */
batOcv = batEng * fzg_array->batOcvCof_batEng[0] +
fzg_array->batOcvCof_batEng[1];
/* FUNCTION CALL - min delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMin = batFrcClc_port(batPwrMax, vehVel, fzg_scalar->batRstDch,
fzg_scalar->batRstChr, batOcv) * wayStp;
/* <-multiply by delta_S */
/* FUNCTION CALL - max delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMax = batFrcClc_port(batPwrMin, vehVel, fzg_scalar->batRstDch,
fzg_scalar->batRstChr, batOcv) * wayStp;
/* Es werden 2 Fälle unterschieden: */
/* there are 2 different cases */
if ((batEngDltMin > 0.0) && (batEngDltMax > 0.0)) {
/* %% konventionelles Bremsen + Rekuperieren */
/* conventional brakes + recuperation */
/* */
/* set change in energy to discretized integer values w/ ceil and */
/* floor. This also helps for easy looping */
/* Konventionelles Bremsen wird ebenfalls untersucht. */
/* Hier liegt eventuell noch Beschleunigungspotential, da diese */
/* Zustandswechsel u.U. ausgeschlossen werden können. */
/* NOTE: u.U. = unter Ümständen = circumstances permitting */
/* convetional breaks also investigated. An accelerating potential */
/* is still possible, as these states can be excluded */
/* (circumstances permitting) <- ??? not sure what above means */
/* */
/* so if both min and max changes in battery energy are */
/* increasing, then set the delta min to zero */
batEngDltMinInx = 0.0;
batEngDltMaxInx = floor(batEngDltMax / batEngStp);
} else {
batEngDltMinInx = ceil(batEngDltMin / batEngStp);
batEngDltMaxInx = floor(batEngDltMax / batEngStp);
}
} else {
batEngDltMinInx = 0.0;
batEngDltMaxInx = 0.0;
}
/* you got a larger min chnage and a max change, you're out of bounds. Leave */
/* the function */
if (batEngDltMaxInx < batEngDltMinInx) {
} else {
/* % Schleife über alle Elektromotormomente */
/* now loop through all the electric-motor torques */
k = (int)(batEngDltMaxInx + (1.0 - batEngDltMinInx));
for (batEngDltInx = 0; batEngDltInx < k; batEngDltInx++) {
batEngDlt = (batEngDltMinInx + (double)batEngDltInx) * batEngStp;
/* open circuit voltage over each iteration */
batOcv = (batEng + batEngDlt) * fzg_array->batOcvCof_batEng[0] +
fzg_array->batOcvCof_batEng[1];
/* Skalar für die Batterieleistung in W */
/* Skalar Krafstoffkraft in N */
/* FUNCTION CALL */
/* Skalar für die Wegschrittweite in m, */
/* Skalar - vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergie�nderung, */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar f�r die Wegschrittweite in m */
/* vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergieänderung */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct - Fahrzeugparameter - nur skalar */
/* struct - Fahrzeugparameter - nur array */
/* Skalar f�r die Batterieleistung */
/* Skalar Kraftstoffkraft */
/* */
/* FULENGCLC Calculating fuel consumption */
/* Erstellungsdatum der ersten Version 04.09.2015 - Stephan Uebel */
/* */
/* Diese Funktion berechnet den Kraftstoffverbrauch für einen gegebenen */
/* Wegschritt der kinetischen Energie, der Batterieenergie und des */
/* Antriebsstrangzustands über dem Weg. */
/* this function calculates fuel consumption for a given route step of */
/* KE, the battery energy, and drivetrain state of the current path_idx */
/* */
/* Version vom 17.02.2016 : Rotationsmassen vernachlässigt */
/* ^^ neglected rotatary masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* */
/* version from 1.06.2016 - removed crsTrq calulations - they are already */
/* done in parent function (clcPMP_olHyb_tmp) and do not change w/ each */
/* iteration, making the caluclation here unnecessary */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Skalar - electric battery power (W) */
fulFrc = rtInf;
/* Skalar - fuel force intialization (N) */
/* % Batterie */
/* Batterieenergieänderung über dem Weg (Batteriekraft) */
/* Change in battery energy over the path_idx way (ie battery power) */
batFrc = batEngDlt / wayStp;
/* Fallunterscheidung, ob Batterie geladen oder entladen wird */
/* Case analysis - check if battery is charging or discharging. Set */
/* resistance accordingly */
/* elektrische Leistung des Elektromotors */
/* electric power from electric motor - DERIVATION? dunno */
/* innere Batterieleistung / internal batt power */
/* dissipat. Leist. / dissipated */
if (batFrc < 0.0) {
b_batFrc = fzg_scalar->batRstDch;
} else {
b_batFrc = fzg_scalar->batRstChr;
}
batPwr = (-batFrc * vehVel - batFrc * batFrc * (vehVel * vehVel) /
(batOcv * batOcv) * b_batFrc) - batPwrAux;
/* Nebenverbrauchlast / auxiliary power */
/* % Elektromotor */
/* Ermitteln des Kurbelwellenmoments durch EM aus Batterieleistung */
/* determine crankshaft torque cauesd by EM's battery power */
/* switching out codegen_interp2 for lininterp2-just might work! */
/* */
emoTrq = codegen_interp2(fzg_array->emoSpdMgd, fzg_array->emoPwrMgd,
fzg_array->emoTrq_emoSpd_emoPwr, crsSpd, batPwr);
/* emoTrq = lininterp2(par.emoSpdMgd(1,:), par.emoPwrMgd(:,1),... */
/* par.emoTrq_emoSpd_emoPwr',crsSpd,emoPwrEle); */
if (rtIsInf(emoTrq)) {
} else {
/* Grenzen des Elektromotors müssen hier nicht überprüft werden, da die */
/* Ladungsdeltas schon so gewählt wurden, dass das maximale Motormoment */
/* nicht überstiegen wird. */
/* Electric motor limits need not be checked here, since the charge deltas */
/* have been chosen so that the max torque is not exceeded. */
/* % Verbrennungsmotor */
/* Ermitteln des Kurbelwellenmoments durch den Verbrennungsmotor */
/* Determining the crankshaft moment from the ICE */
iceTrq = crsTrq - emoTrq;
/* Wenn das Verbrennungsmotormoment kleiner als das minimale */
/* Moment ist, ist dieser in der Schubabschaltung. Das */
/* verbleibende Moment liefern die Bremsen */
/* If engine torque is less than the min torque, fuel is cut. The */
/* remaining torque is deliver the brakes. */
if (iceTrq < iceTrqMin) {
fulPwr = 0.0;
} else if (iceTrq > iceTrqMax) {
fulPwr = rtInf;
} else {
/* replacing another coden_interp2 */
fulPwr = codegen_interp2(fzg_array->iceSpdMgd,
fzg_array->iceTrqMgd, fzg_array->iceFulPwr_iceSpd_iceTrq,
crsSpd, iceTrq);
/* fulPwr = lininterp2(par.iceSpdMgd(1,:), par.iceTrqMgd(:,1), ... */
/* par.iceFulPwr_iceSpd_iceTrq', crsSpd, iceTrq); */
}
/* Berechnen der Kraftstoffkraft */
/* calculate fuel force */
fulFrc = fulPwr / vehVel;
/* Berechnen der Kraftstoffvolumenänderung */
/* caluclate change in fuel volume - energy, no? */
/* % Ende der Funktion */
}
/* FUNCTION CALL */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open circuit voltage */
batFrc = batFrcClc_port(batPwr, vehVel, fzg_scalar->batRstDch,
fzg_scalar->batRstChr, batOcv);
/* %% Hamiltonfunktion bestimmen */
/* determine the hamiltonian */
/* Eq (29b) */
crsSpdVec[0] = (fulFrc + psiBatEng * batFrc) + psiTim / vehVel;
ixstart = 1;
mtmp = crsSpdVec[0];
itmp = 1;
if (rtIsNaN(crsSpdVec[0])) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 3)) {
ixstart = 2;
if (!rtIsNaN(*cosHamMin)) {
mtmp = *cosHamMin;
itmp = 2;
exitg1 = true;
} else {
ix = 3;
}
}
}
if ((ixstart < 2) && (*cosHamMin < mtmp)) {
mtmp = *cosHamMin;
itmp = 2;
}
*cosHamMin = mtmp;
/* Wenn der aktuelle Punkt besser ist, als der in cosHamMin */
/* gespeicherte Wert, werden die Ausgabegrößen neu beschrieben. */
/* if the current point is better than the stored cosHamMin value, */
/* then the output values are rewritten (selected prev. value is = 2) */
if (itmp == 1) {
*batFrcOut = batFrc;
*fulFrcOut = fulFrc;
}
}
}
}
}
}
// printf(" *cosHamMin (func): %4.3f\n", *cosHamMin);
// printf(" *batFrcOut (func): %4.3f\n", *batFrcOut);
// printf(" *fulFrcOut (func): %4.3f\n", *fulFrcOut);
/* end of function */
}
/* Function Definitions */
void GetTotalStaticPT(double varargin_1, double varargin_2, const double
varargin_3[12], double varargin_4, double, double, double
varargin_7, double *p0, double *T0, double *Ts)
{
double err;
double rhoTemp;
double unusedUd;
double unusedUc;
double unusedUb;
double unusedUa;
double CpTemp;
double mtmp;
double cSq;
double p0TempNew;
double deltaT;
double T0TempNew;
double TsTempNew;
double p0Temp;
double T0Temp;
double TsTemp;
double T_sq;
double T_cb;
double T_qt;
int ixstart;
static const double b[12] = { 1.00794, 15.9994, 14.0067, 2.01588, 17.00734,
28.0101, 30.0061, 31.9988, 18.01528, 44.0095, 28.0134, 39.948 };
double a_CO2[7];
double a_CO[7];
double a_O2[7];
double a_H2[7];
double a_H2O[7];
double a_OH[7];
double a_O[7];
double a_N2[7];
double a_N[7];
double a_NO[7];
static const double dv79[7] = { 4.6365111, 0.0027414569, -9.9589759E-7,
1.6038666E-10, -9.1619857E-15, -49024.904, -1.9348955 };
static const double dv80[7] = { 3.0484859, 0.0013517281, -4.8579405E-7,
7.8853644E-11, -4.6980746E-15, -14266.117, 6.0170977 };
static const double dv81[7] = { 3.66096083, 0.000656365523, -1.41149485E-7,
2.05797658E-11, -1.29913248E-15, -1215.97725, 3.41536184 };
static const double dv82[7] = { 2.9328305, 0.00082659802, -1.4640057E-7,
1.5409851E-11, -6.8879615E-16, -813.05582, -1.0243164 };
static const double dv83[7] = { 2.6770389, 0.0029731816, -7.7376889E-7,
9.4433514E-11, -4.2689991E-15, -29885.894, 6.88255 };
static const double dv84[7] = { 2.83853033, 0.00110741289, -2.94000209E-7,
4.20698729E-11, -2.4228989E-15, 3697.80808, 5.84494652 };
static const double dv85[7] = { 2.54363697, -2.73162486E-5, -4.1902952E-9,
4.95481845E-12, -4.79553694E-16, 29226.012, 4.92229457 };
static const double dv86[7] = { 2.95257637, 0.0013969004, -4.92631603E-7,
7.86010195E-11, -4.60755204E-15, -923.948688, 5.87188762 };
static const double dv87[7] = { 2.4159429, 0.00017489065, -1.1902369E-7,
3.0226244E-11, -2.0360983E-15, 56133.775, 4.6496095 };
static const double dv88[7] = { 3.26071234, 0.00119101135, -4.29122646E-7,
6.94481463E-11, -4.03295681E-15, 9921.43132, 6.36900518 };
static const double dv89[7] = { 2.356813, 0.0089841299, -7.1220632E-6,
2.4573008E-9, -1.4288548E-13, -48371.971, 9.9009035 };
static const double dv90[7] = { 3.5795335, -0.00061035369, 1.0168143E-6,
9.0700586E-10, -9.0442449E-13, -14344.086, 3.5084093 };
static const double dv91[7] = { 3.78245636, -0.00299673415, 9.847302E-6,
-9.68129508E-9, 3.24372836E-12, -1063.94356, 3.65767573 };
static const double dv92[7] = { 2.3443029, 0.0079804248, -1.9477917E-5,
2.0156967E-8, -7.3760289E-12, -917.92413, 0.68300218 };
static const double dv93[7] = { 4.1986352, -0.0020364017, 6.5203416E-6,
-5.4879269E-9, 1.771968E-12, -30293.726, -0.84900901 };
static const double dv94[7] = { 3.99198424, -0.00240106655, 4.61664033E-6,
-3.87916306E-9, 1.36319502E-12, 3368.89836, -0.103998477 };
static const double dv95[7] = { 3.1682671, -0.00327931884, 6.64306396E-6,
-6.12806624E-9, 2.11265971E-12, 29122.2592, 2.05193346 };
static const double dv96[7] = { 3.53100528, -0.000123660988, -5.02999433E-7,
2.43530612E-9, -1.40881235E-12, -1046.97628, 2.96747038 };
static const double dv97[7] = { 2.5, 0.0, 0.0, 0.0, 0.0, 56104.638, 4.1939088
};
static const double dv98[7] = { 4.21859896, -0.00463988124, 1.10443049E-5,
-9.34055507E-9, 2.80554874E-12, 9845.09964, 2.28061001 };
double a_H[84];
double dv99[7];
static const double b_a_H[7] = { 2.5, 0.0, 0.0, 0.0, 0.0, 25473.66,
-0.44668285 };
static const double a_Ar[7] = { 2.5, 0.0, 0.0, 0.0, 0.0, -745.375, 4.3796749 };
double a_gas[7];
int ix;
double dv100[7];
double x[3];
double y[3];
boolean_T exitg1;
/* Calculates the total pressure, total and static temperature based from the */
/* measurements. */
/* Input */
/* - p : Pressure measurement (static) [Pa] */
/* - T : Temperature measurement [K] */
/* - x : Composition of the gas in mole fraction (12x1) */
/* - RF : Recovery factor for temperature measurement [K] */
/* - mDot : mass flow at the measurement [kg/s] */
/* - Area : Sectional area at the measurement [m2] */
/* - c : velocity of the gas at the measurement [m/s], optional */
/* Output */
/* - p0 : Total pressure [Pa] */
/* - T0 : Total temperature [K]; */
/* - Ts : Static temperature [K]; */
err = 1.0;
GetThermoDynProp(varargin_1, varargin_2, varargin_3, &mtmp, &CpTemp, &unusedUa,
&unusedUb, &unusedUc, &unusedUd, &rhoTemp);
cSq = varargin_7 * varargin_7;
p0TempNew = varargin_1 + 0.5 * rhoTemp * cSq;
deltaT = 0.5 * cSq / CpTemp;
T0TempNew = varargin_2 + (1.0 - varargin_4) * deltaT;
TsTempNew = varargin_2 - varargin_4 * deltaT;
while (err > 0.001) {
p0Temp = p0TempNew;
T0Temp = T0TempNew;
TsTemp = TsTempNew;
/* Calculate the specific thermodynamic properties of the gas based on the */
/* pressure, temperature and the composition based on the 7-coefficient NASA */
/* polynomials. Valid in the temperature range 300~6000K. */
/* Input */
/* P : Pressure in Pa */
/* T : Temperature in Kelvin */
/* x : Composition of the gas */
/* x(1) = x_H; x(2) = x_O; x(3) = x_N; x(4) = x_H2; */
/* x(5) = x_OH; x(6) = x_CO; x(7) = x_NO; x(8) = x_O2; */
/* x(9) = x_H2O; x(10) = x_CO2;x(11) = x_N2; x(12) = x_Ar */
/* Output */
/* R : Gas constant */
/* Cp : Specific heat capacity at constant pressure (J/kg/K) */
/* Cv : Specific heat capacity at constant volume (J/kg/K) */
/* u : Specific internal energy (J/kg) */
/* h : Specific enthalpy (J/kg) */
/* s : Specific entropy (J/kg/T) */
/* rho : Density of the gas (kg/m3) */
/* */
/* Atomic weight calculation from NIST atomic weights */
/* Ref : http://physics.nist.gov/cgi-bin/Compositions/stand_alone.pl */
/* */
/* The NASA 7-coefficients are obtained from the reference: */
/* Burcat, A., B. Ruscic, et al. (2005). Third millenium ideal gas and */
/* condensed phase thermochemical database for combustion with updates */
/* from active thermochemical tables, Argonne National Laboratory */
/* Argonne, IL. */
/* */
/* Created by Kevin Koosup Yum on 29 June */
/* % */
T_sq = TsTempNew * TsTempNew;
T_cb = rt_powd_snf(TsTempNew, 3.0);
T_qt = rt_powd_snf(TsTempNew, 4.0);
/* in J/kmol/K */
/* % Calculate R */
/* kg/kmol */
/* in kg/kmol */
mtmp = 0.0;
for (ixstart = 0; ixstart < 12; ixstart++) {
mtmp += varargin_3[ixstart] * b[ixstart];
}
unusedUa = 8314.4621 / mtmp;
/* in J/kg/K */
/* % */
if (TsTempNew >= 1000.0) {
/* Valid upto */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
/* 1000~6000K */
for (ixstart = 0; ixstart < 7; ixstart++) {
a_CO2[ixstart] = dv79[ixstart];
a_CO[ixstart] = dv80[ixstart];
a_O2[ixstart] = dv81[ixstart];
a_H2[ixstart] = dv82[ixstart];
a_H2O[ixstart] = dv83[ixstart];
a_OH[ixstart] = dv84[ixstart];
a_O[ixstart] = dv85[ixstart];
a_N2[ixstart] = dv86[ixstart];
a_N[ixstart] = dv87[ixstart];
a_NO[ixstart] = dv88[ixstart];
}
/* 1000~6000K */
} else {
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
/* 300~1000K */
for (ixstart = 0; ixstart < 7; ixstart++) {
a_CO2[ixstart] = dv89[ixstart];
a_CO[ixstart] = dv90[ixstart];
a_O2[ixstart] = dv91[ixstart];
a_H2[ixstart] = dv92[ixstart];
a_H2O[ixstart] = dv93[ixstart];
a_OH[ixstart] = dv94[ixstart];
a_O[ixstart] = dv95[ixstart];
a_N2[ixstart] = dv96[ixstart];
a_N[ixstart] = dv97[ixstart];
a_NO[ixstart] = dv98[ixstart];
}
/* 300~1000K */
}
/* % Calculate Cp & Cv */
/* k = x ./ M_mw'; */
/* a_gas_Cp = k*A; */
dv99[0] = 1.0;
dv99[1] = TsTempNew;
dv99[2] = T_sq;
dv99[3] = T_cb;
dv99[4] = T_qt;
dv99[5] = 0.0;
dv99[6] = 0.0;
for (ixstart = 0; ixstart < 7; ixstart++) {
a_H[12 * ixstart] = b_a_H[ixstart];
a_H[1 + 12 * ixstart] = a_O[ixstart];
a_H[2 + 12 * ixstart] = a_N[ixstart];
a_H[3 + 12 * ixstart] = a_H2[ixstart];
a_H[4 + 12 * ixstart] = a_OH[ixstart];
a_H[5 + 12 * ixstart] = a_CO[ixstart];
a_H[6 + 12 * ixstart] = a_NO[ixstart];
a_H[7 + 12 * ixstart] = a_O2[ixstart];
a_H[8 + 12 * ixstart] = a_H2O[ixstart];
a_H[9 + 12 * ixstart] = a_CO2[ixstart];
a_H[10 + 12 * ixstart] = a_N2[ixstart];
a_H[11 + 12 * ixstart] = a_Ar[ixstart];
a_gas[ixstart] = 0.0;
for (ix = 0; ix < 12; ix++) {
a_gas[ixstart] += varargin_3[ix] * a_H[ix + 12 * ixstart];
}
a_CO2[ixstart] = a_gas[ixstart] * dv99[ixstart];
}
unusedUb = a_CO2[0];
for (ixstart = 0; ixstart < 6; ixstart++) {
unusedUb += a_CO2[ixstart + 1];
}
/* J/kmol/K */
/* J/kmol/K */
/* % Calculate h and u */
/* x(1) = x_H; x(2) = x_O; x(3) = x_N; x(4) = x_H2; */
/* x(5) = x_OH; x(6) = x_CO; x(7) = x_NO; x(8) = x_O2; */
/* x(9) = x_H2O; x(10) = x_CO2;x(11) = x_N2; x(12) = x_Ar */
/* (J/kmol / J/kmol/K) */
/* J/kmol */
/* % Calculate s */
dv100[0] = log(TsTempNew);
dv100[1] = TsTempNew;
dv100[2] = T_sq / 2.0;
dv100[3] = T_cb / 3.0;
dv100[4] = T_qt / 4.0;
dv100[5] = 0.0;
dv100[6] = 1.0;
for (ixstart = 0; ixstart < 7; ixstart++) {
a_CO2[ixstart] = a_gas[ixstart] * dv100[ixstart];
}
mtmp = a_CO2[0];
for (ixstart = 0; ixstart < 6; ixstart++) {
mtmp += a_CO2[ixstart + 1];
}
/* % Calculate rho */
p0TempNew = varargin_1 + 0.5 * (unusedUa * mtmp) * cSq;
deltaT = 0.5 * cSq / (unusedUa * unusedUb);
T0TempNew = varargin_2 + (1.0 - varargin_4) * deltaT;
TsTempNew = varargin_2 - varargin_4 * deltaT;
x[0] = (p0TempNew - p0Temp) / p0Temp;
x[1] = (T0TempNew - T0Temp) / T0Temp;
x[2] = (TsTempNew - TsTemp) / TsTemp;
for (ixstart = 0; ixstart < 3; ixstart++) {
y[ixstart] = fabs(x[ixstart]);
}
ixstart = 1;
mtmp = y[0];
if (rtIsNaN(y[0])) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 4)) {
ixstart = ix;
if (!rtIsNaN(y[ix - 1])) {
mtmp = y[ix - 1];
exitg1 = true;
} else {
ix++;
}
}
}
if (ixstart < 3) {
while (ixstart + 1 < 4) {
if (y[ixstart] > mtmp) {
mtmp = y[ixstart];
}
ixstart++;
}
}
err = mtmp;
}
*p0 = p0TempNew;
*T0 = T0TempNew;
*Ts = TsTempNew;
}
/* Output and update for atomic system: '<Root>/Sensor_Data_Adapter' */
void AUAV_V_Sensor_Data_Adapter(void)
{
/* local block i/o variables */
real_T rtb_DataTypeConversion_n;
real_T rtb_DiscreteZeroPole_i;
boolean_T rtb_LogicalOperator_df;
int16_T rtb_Switch_d[13];
real32_T rtb_Sum_o;
uint8_T rtb_Compare_gl;
real32_T rtb_u001maxDynPress;
real32_T rtb_Sum_d;
int16_T rtb_DataTypeConversion1_ax;
int16_T rtb_DataTypeConversion2_c;
int16_T i;
real_T tmp;
real_T tmp_0;
/* Outputs for Enabled SubSystem: '<S16>/Raw HIL Readings' incorporates:
* EnablePort: '<S580>/Enable'
*/
if (AUAV_V3_TestSensors_B.DataStoreRead > 0.0) {
/* Outputs for Enabled SubSystem: '<S580>/Enabled Subsystem' incorporates:
* EnablePort: '<S582>/Enable'
*/
/* DataStoreRead: '<S580>/Data Store Read' */
if (AUAV_V3_TestSensors_DWork.SIX_DOF_HIL_FLAG > 0.0) {
/* S-Function (MCHP_C_function_Call): '<S582>/Data from HIL [hil.c]2' */
hilRead(
&AUAV_V3_TestSensors_B.DatafromHILhilc2[0]
);
/* S-Function (MCHP_C_function_Call): '<S582>/HIL Messages Parser//Decoder [hil.c]1' */
protDecodeHil(
&AUAV_V3_TestSensors_B.DatafromHILhilc2[0]
);
/* S-Function (MCHP_C_function_Call): '<S582>/HIL Raw Readings [hil.c]1' */
hil_getRawRead(
&AUAV_V3_TestSensors_B.HILRawReadingshilc1_k[0]
);
}
/* End of DataStoreRead: '<S580>/Data Store Read' */
/* End of Outputs for SubSystem: '<S580>/Enabled Subsystem' */
/* Outputs for Enabled SubSystem: '<S580>/Enabled Subsystem1' incorporates:
* EnablePort: '<S583>/Enable'
*/
/* DataStoreRead: '<S580>/Data Store Read1' */
if (AUAV_V3_TestSensors_DWork.X_PLANE_HIL_FLAG > 0.0) {
/* S-Function (MCHP_C_function_Call): '<S583>/Data from HIL X-Plane' */
HILSIM_set_gplane(
);
/* S-Function (MCHP_C_function_Call): '<S583>/Data from HIL X-Plane2' */
HILSIM_set_omegagyro(
);
/* S-Function (MCHP_C_function_Call): '<S583>/HIL Raw Readings [hil.c]1' */
hil_getRawRead(
&AUAV_V3_TestSensors_B.HILRawReadingshilc1[0]
);
/* MATLAB Function: '<S583>/Embedded MATLAB Function1' incorporates:
* Constant: '<S583>/Constant1'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_m);
/* DataTypeConversion: '<S583>/Data Type Conversion1' */
tmp_0 = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_m.y);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 65536.0);
}
/* MATLAB Function: '<S583>/Embedded MATLAB Function2' incorporates:
* Constant: '<S583>/Constant2'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_i);
/* DataTypeConversion: '<S583>/Data Type Conversion' incorporates:
* DataStoreRead: '<S583>/Get mlAirData1'
*/
rtb_Sum_o = (real32_T)floor(mlAirData.press_abs);
if (rtIsNaNF(rtb_Sum_o) || rtIsInfF(rtb_Sum_o)) {
rtb_Sum_o = 0.0F;
} else {
rtb_Sum_o = (real32_T)fmod(rtb_Sum_o, 65536.0F);
}
/* DataTypeConversion: '<S583>/Data Type Conversion2' */
tmp = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_i.y);
if (rtIsNaN(tmp) || rtIsInf(tmp)) {
tmp = 0.0;
} else {
tmp = fmod(tmp, 65536.0);
}
/* MATLAB Function: '<S583>/myMux Fun' incorporates:
* DataStoreRead: '<S583>/Get mlAirData2'
* DataTypeConversion: '<S583>/Data Type Conversion'
* DataTypeConversion: '<S583>/Data Type Conversion1'
* DataTypeConversion: '<S583>/Data Type Conversion2'
*/
AUAV_V3_TestSenso_myMuxFun(rtb_Sum_o < 0.0F ? -(int16_T)(uint16_T)
-rtb_Sum_o : (int16_T)(uint16_T)rtb_Sum_o, tmp_0 < 0.0 ? -(int16_T)
(uint16_T)-tmp_0 : (int16_T)(uint16_T)tmp_0, tmp < 0.0 ? -(int16_T)
(uint16_T)-tmp : (int16_T)(uint16_T)tmp, mlAirData.temperature,
&AUAV_V3_TestSensors_B.sf_myMuxFun_h);
/* S-Function (MCHP_C_function_Call): '<S583>/Update State AP ADC Data [updateSensorMcuState.c]1' */
updateRawADCData(
&AUAV_V3_TestSensors_B.sf_myMuxFun_h.y[0]
);
}
/* End of Outputs for SubSystem: '<S580>/Enabled Subsystem1' */
/* Switch: '<S580>/Switch' incorporates:
* DataStoreRead: '<S580>/Data Store Read1'
*/
for (i = 0; i < 13; i++) {
if (AUAV_V3_TestSensors_DWork.X_PLANE_HIL_FLAG != 0.0) {
AUAV_V3_TestSensors_B.Switch_i[i] =
AUAV_V3_TestSensors_B.HILRawReadingshilc1[i];
} else {
AUAV_V3_TestSensors_B.Switch_i[i] =
AUAV_V3_TestSensors_B.HILRawReadingshilc1_k[i];
}
}
/* End of Switch: '<S580>/Switch' */
}
/* End of Outputs for SubSystem: '<S16>/Raw HIL Readings' */
/* Logic: '<S581>/Logical Operator' */
rtb_LogicalOperator_df = !(AUAV_V3_TestSensors_B.DataStoreRead != 0.0);
/* Outputs for Enabled SubSystem: '<S581>/If no HIL then Read all the Sensors' incorporates:
* EnablePort: '<S591>/Enable'
*/
if (rtb_LogicalOperator_df) {
/* DataTypeConversion: '<S591>/Data Type Conversion' incorporates:
* DataStoreRead: '<S591>/Get mlAirData1'
*/
rtb_Sum_o = (real32_T)floor(mlAirData.press_abs);
if (rtIsNaNF(rtb_Sum_o) || rtIsInfF(rtb_Sum_o)) {
rtb_Sum_o = 0.0F;
} else {
rtb_Sum_o = (real32_T)fmod(rtb_Sum_o, 65536.0F);
}
i = rtb_Sum_o < 0.0F ? -(int16_T)(uint16_T)-rtb_Sum_o : (int16_T)(uint16_T)
rtb_Sum_o;
/* End of DataTypeConversion: '<S591>/Data Type Conversion' */
/* MATLAB Function: '<S591>/Embedded MATLAB Function1' incorporates:
* Constant: '<S591>/Constant1'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_ib);
/* DataTypeConversion: '<S591>/Data Type Conversion1' */
tmp_0 = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_ib.y);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 65536.0);
}
rtb_DataTypeConversion1_ax = tmp_0 < 0.0 ? -(int16_T)(uint16_T)-tmp_0 :
(int16_T)(uint16_T)tmp_0;
/* End of DataTypeConversion: '<S591>/Data Type Conversion1' */
/* MATLAB Function: '<S591>/Embedded MATLAB Function2' incorporates:
* Constant: '<S591>/Constant2'
*/
EmbeddedMATLABFunction1_m(AUAV_V3_TestSensors_ConstP.pooled16,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_p);
/* DataTypeConversion: '<S591>/Data Type Conversion2' */
tmp_0 = floor(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_p.y);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 65536.0);
}
rtb_DataTypeConversion2_c = tmp_0 < 0.0 ? -(int16_T)(uint16_T)-tmp_0 :
(int16_T)(uint16_T)tmp_0;
/* End of DataTypeConversion: '<S591>/Data Type Conversion2' */
/* MATLAB Function: '<S591>/myMux Fun' incorporates:
* DataStoreRead: '<S591>/Get mlAirData2'
*/
AUAV_V3_TestSenso_myMuxFun(i, rtb_DataTypeConversion1_ax,
rtb_DataTypeConversion2_c, mlAirData.temperature,
&AUAV_V3_TestSensors_B.sf_myMuxFun_n);
/* S-Function (MCHP_C_function_Call): '<S591>/Update State AP ADC Data [updateSensorMcuState.c]1' */
updateRawADCData(
&AUAV_V3_TestSensors_B.sf_myMuxFun_n.y[0]
);
/* S-Function (MCHP_C_function_Call): '<S591>/Read the Cube Data [adisCube16405.c]1' */
getCubeData(
&AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[0]
);
/* MATLAB Function: '<S591>/myMux Fun4' incorporates:
* DataStoreRead: '<S591>/Get mlAirData2'
*/
/* MATLAB Function 'Sensor_Data_Adapter/Sensor Suite/If no HIL then Read all the Sensors/myMux Fun4': '<S621>:1' */
/* This block supports an embeddable subset of the MATLAB language. */
/* See the help menu for details. */
/* '<S621>:1:5' */
AUAV_V3_TestSensors_B.y_l[0] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[0];
AUAV_V3_TestSensors_B.y_l[1] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[1];
AUAV_V3_TestSensors_B.y_l[2] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[2];
AUAV_V3_TestSensors_B.y_l[3] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[3];
AUAV_V3_TestSensors_B.y_l[4] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[4];
AUAV_V3_TestSensors_B.y_l[5] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[5];
AUAV_V3_TestSensors_B.y_l[6] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[6];
AUAV_V3_TestSensors_B.y_l[7] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[7];
AUAV_V3_TestSensors_B.y_l[8] =
AUAV_V3_TestSensors_B.ReadtheCubeDataadisCube16405c1[8];
AUAV_V3_TestSensors_B.y_l[9] = i;
AUAV_V3_TestSensors_B.y_l[10] = rtb_DataTypeConversion1_ax;
AUAV_V3_TestSensors_B.y_l[11] = rtb_DataTypeConversion2_c;
AUAV_V3_TestSensors_B.y_l[12] = mlAirData.temperature;
/* S-Function (MCHP_C_function_Call): '<S591>/Is the GPS Novatel or Ublox? [gpsPort.c]1' */
AUAV_V3_TestSensors_B.IstheGPSNovatelorUbloxgpsPortc1 = isGPSNovatel(
);
/* Outputs for Enabled SubSystem: '<S591>/if GPS is Ublox' incorporates:
* EnablePort: '<S619>/Enable'
*/
/* Logic: '<S591>/Logical Operator' */
if (!(AUAV_V3_TestSensors_B.IstheGPSNovatelorUbloxgpsPortc1 != 0)) {
/* S-Function (MCHP_C_function_Call): '<S619>/Produce the GPS Main Data and update the AP State (lat lon hei cog sog) [gpsUblox.c]1' */
getGpsUbloxMainData(
&AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdat_a[
0]
);
}
/* End of Logic: '<S591>/Logical Operator' */
/* End of Outputs for SubSystem: '<S591>/if GPS is Ublox' */
}
/* End of Outputs for SubSystem: '<S581>/If no HIL then Read all the Sensors' */
/* Switch: '<S581>/Switch' */
for (i = 0; i < 13; i++) {
if (AUAV_V3_TestSensors_B.DataStoreRead != 0.0) {
rtb_Switch_d[i] = AUAV_V3_TestSensors_B.Switch_i[i];
} else {
rtb_Switch_d[i] = AUAV_V3_TestSensors_B.y_l[i];
}
}
/* End of Switch: '<S581>/Switch' */
/* MATLAB Function: '<S629>/Embedded MATLAB Function' incorporates:
* Constant: '<S629>/Constant'
* Constant: '<S629>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion5'
*/
A_EmbeddedMATLABFunction_f((real_T)rtb_Switch_d[12], 0.01, 0.02,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_g,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_g);
/* Sum: '<S628>/Sum' incorporates:
* Constant: '<S628>/Bias'
* Product: '<S628>/Divide'
*/
rtb_Sum_o = (real32_T)(1.5112853050231934 *
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_g.y) + -1605.28198F;
/* RelationalOperator: '<S641>/Compare' incorporates:
* Constant: '<S641>/Constant'
*/
rtb_Compare_gl = (uint8_T)(rtb_Sum_o < -130.0F);
/* Outputs for Enabled SubSystem: '<S626>/Hi Temp Compensation2' incorporates:
* EnablePort: '<S642>/Enable'
*/
/* Logic: '<S626>/Logical Operator' */
if (!(rtb_Compare_gl != 0)) {
/* Sum: '<S642>/Sum2' incorporates:
* Constant: '<S642>/Mean Temperature for Calibration'
* Constant: '<S642>/gains'
* Product: '<S642>/Divide1'
* Sum: '<S642>/Sum1'
*/
AUAV_V3_TestSensors_B.Merge = (real32_T)rtb_Switch_d[10] - (rtb_Sum_o -
293.053F) * -0.0950433F;
}
/* End of Logic: '<S626>/Logical Operator' */
/* End of Outputs for SubSystem: '<S626>/Hi Temp Compensation2' */
/* Outputs for Enabled SubSystem: '<S626>/Lo Temp Compensation' incorporates:
* EnablePort: '<S643>/Enable'
*/
if (rtb_Compare_gl > 0) {
/* Sum: '<S643>/Add' incorporates:
* Constant: '<S643>/Constant'
* Constant: '<S643>/Mean Temperature for Calibration'
* Constant: '<S643>/gains'
* Product: '<S643>/Divide1'
* Sum: '<S643>/Sum1'
* Sum: '<S643>/Sum2'
*/
AUAV_V3_TestSensors_B.Merge = ((real32_T)rtb_Switch_d[10] - (rtb_Sum_o -
-202.93F) * -0.0552923F) + -41.0F;
}
/* End of Outputs for SubSystem: '<S626>/Lo Temp Compensation' */
/* MATLAB Function: '<S631>/Embedded MATLAB Function' incorporates:
* Constant: '<S631>/Constant'
* Constant: '<S631>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion3'
*/
A_EmbeddedMATLABFunction_f((real_T)AUAV_V3_TestSensors_B.Merge, 0.01, 4.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_lw,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_lw);
/* Sum: '<S624>/Sum' incorporates:
* Constant: '<S624>/Bias'
* Constant: '<S624>/Gains'
* DataTypeConversion: '<S593>/Data Type Conversion4'
* Product: '<S624>/Divide'
*/
rtb_u001maxDynPress = 1.05137849F * (real32_T)
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_lw.y + -1005.87872F;
/* Saturate: '<S593>/[0.001 maxDynPress]' */
if (rtb_u001maxDynPress > 3000.0F) {
rtb_u001maxDynPress = 3000.0F;
} else {
if (rtb_u001maxDynPress < 0.001F) {
rtb_u001maxDynPress = 0.001F;
}
}
/* End of Saturate: '<S593>/[0.001 maxDynPress]' */
/* RelationalOperator: '<S644>/Compare' incorporates:
* Constant: '<S644>/Constant'
*/
rtb_Compare_gl = (uint8_T)(rtb_Sum_o < -50.0F);
/* Outputs for Enabled SubSystem: '<S627>/Hi Temp Compensation' incorporates:
* EnablePort: '<S645>/Enable'
*/
/* Logic: '<S627>/Logical Operator' */
if (!(rtb_Compare_gl != 0)) {
/* Sum: '<S645>/Add' incorporates:
* Constant: '<S645>/Constant'
* Constant: '<S645>/Mean Temperature for Calibration'
* Constant: '<S645>/gains'
* Product: '<S645>/Divide1'
* Sum: '<S645>/Sum1'
* Sum: '<S645>/Sum2'
*/
AUAV_V3_TestSensors_B.Merge_j = ((real32_T)rtb_Switch_d[9] - (rtb_Sum_o -
347.23F) * 0.0207608F) + -6.0F;
}
/* End of Logic: '<S627>/Logical Operator' */
/* End of Outputs for SubSystem: '<S627>/Hi Temp Compensation' */
/* Outputs for Enabled SubSystem: '<S627>/Lo Temp Compensation' incorporates:
* EnablePort: '<S646>/Enable'
*/
if (rtb_Compare_gl > 0) {
/* Sum: '<S646>/Sum2' incorporates:
* Constant: '<S646>/Mean Temperature for Calibration'
* Constant: '<S646>/gains'
* Product: '<S646>/Divide1'
* Sum: '<S646>/Sum1'
*/
AUAV_V3_TestSensors_B.Merge_j = (real32_T)rtb_Switch_d[9] - (rtb_Sum_o -
-161.3F) * -0.0102663F;
}
/* End of Outputs for SubSystem: '<S627>/Lo Temp Compensation' */
/* MATLAB Function: '<S632>/Embedded MATLAB Function' incorporates:
* Constant: '<S632>/Constant'
* Constant: '<S632>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion19'
*/
A_EmbeddedMATLABFunction_f((real_T)AUAV_V3_TestSensors_B.Merge_j, 0.01, 4.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_cn,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_cn);
/* Sum: '<S623>/Sum' incorporates:
* Constant: '<S623>/Bias'
* Constant: '<S623>/Gains'
* DataTypeConversion: '<S593>/Data Type Conversion1'
* Product: '<S623>/Divide'
*/
rtb_Sum_d = 27.127F * (real32_T)
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_cn.y + 9444.44434F;
/* MATLAB Function: '<S581>/myMux Fun' */
AUAV_V3_TestSe_myMuxFun1_e(rtb_u001maxDynPress, rtb_Sum_d, rtb_Sum_o,
&AUAV_V3_TestSensors_B.sf_myMuxFun);
/* Outputs for Enabled SubSystem: '<S581>/If no HIL then update Air Data' incorporates:
* EnablePort: '<S592>/Enable'
*/
/* Inport: '<S592>/AirData' */
AUAV_V3_TestSensors_B.AirData[0] = AUAV_V3_TestSensors_B.sf_myMuxFun.y[0];
AUAV_V3_TestSensors_B.AirData[1] = AUAV_V3_TestSensors_B.sf_myMuxFun.y[1];
AUAV_V3_TestSensors_B.AirData[2] = AUAV_V3_TestSensors_B.sf_myMuxFun.y[2];
/* S-Function (MCHP_C_function_Call): '<S592>/Update the Air Calibrated Data [updateSensorMcuState.c]1' */
updateAirData(
&AUAV_V3_TestSensors_B.AirData[0]
);
/* End of Outputs for SubSystem: '<S581>/If no HIL then update Air Data' */
/* MATLAB Function: '<S581>/myMux Fun1' */
/* MATLAB Function 'Sensor_Data_Adapter/Sensor Suite/myMux Fun1': '<S595>:1' */
/* This block supports an embeddable subset of the MATLAB language. */
/* See the help menu for details. */
/* '<S595>:1:5' */
AUAV_V3_TestSensors_B.y_o[0] = rtb_u001maxDynPress;
AUAV_V3_TestSensors_B.y_o[1] = AUAV_V3_TestSensors_B.AirData[0];
AUAV_V3_TestSensors_B.y_o[2] = 0.0F;
AUAV_V3_TestSensors_B.y_o[3] = 0.0F;
/* S-Function (MCHP_C_function_Call): '<S581>/Sensor DSC Diag [updateSensorMcuState.c]1' */
updateSensorDiag(
&AUAV_V3_TestSensors_B.y_o[0]
);
/* S-Function "MCHP_MCU_LOAD" Block: <S590>/Calculus Time Step1 */
AUAV_V3_TestSensors_B.CalculusTimeStep1 = MCHP_MCULoadResult[0];
/* S-Function "MCHP_MCU_OVERLOAD" Block: <S590>/Calculus Time Step2 */
{
uint16_T register tmp = MCHP_MCU_Overload.val;
MCHP_MCU_Overload.val ^= tmp; /* Multi Tasking: potential simultaneous access ==> using xor to protect from potential miss */
AUAV_V3_TestSensors_B.CalculusTimeStep2 = tmp;
}
/* DataTypeConversion: '<S590>/Data Type Conversion12' incorporates:
* DataTypeConversion: '<S590>/Data Type Conversion1'
* DataTypeConversion: '<S590>/Data Type Conversion2'
* Gain: '<S590>/Gain'
* Product: '<S590>/Divide'
* Rounding: '<S590>/Rounding Function'
*/
tmp_0 = floor((real_T)AUAV_V3_TestSensors_B.CalculusTimeStep1 / (real_T)
AUAV_V3_TestSensors_B.CalculusTimeStep2 * 100.0);
if (rtIsNaN(tmp_0) || rtIsInf(tmp_0)) {
tmp_0 = 0.0;
} else {
tmp_0 = fmod(tmp_0, 256.0);
}
AUAV_V3_TestSensors_B.DataTypeConversion12 = (uint8_T)(tmp_0 < 0.0 ? (int16_T)
(uint8_T)-(int8_T)(uint8_T)-tmp_0 : (int16_T)(uint8_T)tmp_0);
/* End of DataTypeConversion: '<S590>/Data Type Conversion12' */
/* MATLAB Function: '<S630>/Embedded MATLAB Function' incorporates:
* Constant: '<S630>/Constant'
* Constant: '<S630>/Constant1'
* DataTypeConversion: '<S593>/Data Type Conversion6'
*/
A_EmbeddedMATLABFunction_f((real_T)rtb_Switch_d[11], 0.01, 0.02,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_np,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_np);
/* DataTypeConversion: '<S593>/Data Type Conversion8' incorporates:
* Constant: '<S625>/Bias'
* Product: '<S625>/Divide'
* Sum: '<S625>/Sum'
*/
rtb_Sum_o = (real32_T)floor((real32_T)(3.1760616302490234 *
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_np.y) + 911.698242F);
if (rtIsNaNF(rtb_Sum_o) || rtIsInfF(rtb_Sum_o)) {
rtb_Sum_o = 0.0F;
} else {
rtb_Sum_o = (real32_T)fmod(rtb_Sum_o, 65536.0F);
}
AUAV_V3_TestSensors_B.DataTypeConversion8 = rtb_Sum_o < 0.0F ? (uint16_T)
-(int16_T)(uint16_T)-rtb_Sum_o : (uint16_T)rtb_Sum_o;
/* End of DataTypeConversion: '<S593>/Data Type Conversion8' */
/* S-Function (MCHP_C_function_Call): '<S581>/Update the Load and Power Data [updateSensorMcuState.c]1' */
updateLoadData(
AUAV_V3_TestSensors_B.DataTypeConversion12
, AUAV_V3_TestSensors_B.DataTypeConversion8
);
/* S-Function (MCHP_C_function_Call): '<S588>/Produce the GPS Main Data and update the AP State (lat lon hei cog sog) [gpsUblox.c]1' */
getGpsUbloxData(
&AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdatet[0]
);
/* MATLAB Function: '<S596>/Embedded MATLAB Function' incorporates:
* Constant: '<S589>/Gyro Gains'
* Constant: '<S596>/Constant'
* Constant: '<S596>/Constant1'
* DataTypeConversion: '<S589>/Data Type Conversion2'
* Gain: '<S599>/[ -1 1 -1]'
* Product: '<S589>/Divide'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[0] *
0.000266462477F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_f,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_f);
/* MATLAB Function: '<S596>/Embedded MATLAB Function1' incorporates:
* Constant: '<S589>/Gyro Gains'
* Constant: '<S596>/Constant2'
* Constant: '<S596>/Constant3'
* DataTypeConversion: '<S589>/Data Type Conversion2'
* Product: '<S589>/Divide'
*/
A_EmbeddedMATLABFunction_f((real_T)((real32_T)rtb_Switch_d[1] *
0.000266462477F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_i,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction1_i);
/* MATLAB Function: '<S596>/Embedded MATLAB Function2' incorporates:
* Constant: '<S589>/Gyro Gains'
* Constant: '<S596>/Constant4'
* Constant: '<S596>/Constant5'
* DataTypeConversion: '<S589>/Data Type Conversion2'
* Gain: '<S599>/[ -1 1 -1]'
* Product: '<S589>/Divide'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[2] *
0.000266462477F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_h,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction2_h);
/* MATLAB Function: '<S596>/myMux Fun' */
AUAV_V3_TestSen_myMuxFun_i(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_f.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_i.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_h.y,
&AUAV_V3_TestSensors_B.sf_myMuxFun_i);
/* DataTypeConversion: '<S589>/Data Type Conversion7' */
AUAV_V3_TestSensors_B.DataTypeConversion7[0] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_i.y[0];
AUAV_V3_TestSensors_B.DataTypeConversion7[1] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_i.y[1];
AUAV_V3_TestSensors_B.DataTypeConversion7[2] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_i.y[2];
/* MATLAB Function: '<S597>/Embedded MATLAB Function' incorporates:
* Constant: '<S589>/Gyro Gains1'
* Constant: '<S597>/Constant'
* Constant: '<S597>/Constant1'
* DataTypeConversion: '<S589>/Data Type Conversion1'
* Gain: '<S600>/[ -1 1 -1]'
* Product: '<S589>/Divide1'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[3] *
0.000599060033F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_kq,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_kq);
/* MATLAB Function: '<S597>/Embedded MATLAB Function1' incorporates:
* Constant: '<S589>/Gyro Gains1'
* Constant: '<S597>/Constant2'
* Constant: '<S597>/Constant3'
* DataTypeConversion: '<S589>/Data Type Conversion1'
* Product: '<S589>/Divide1'
*/
A_EmbeddedMATLABFunction_f((real_T)((real32_T)rtb_Switch_d[4] *
0.000599060033F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_f,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction1_f);
/* MATLAB Function: '<S597>/Embedded MATLAB Function2' incorporates:
* Constant: '<S589>/Gyro Gains1'
* Constant: '<S597>/Constant4'
* Constant: '<S597>/Constant5'
* DataTypeConversion: '<S589>/Data Type Conversion1'
* Gain: '<S600>/[ -1 1 -1]'
* Product: '<S589>/Divide1'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[5] *
0.000599060033F), 0.01, 40.0,
&AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_f,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction2_f);
/* MATLAB Function: '<S597>/myMux Fun' */
AUAV_V3_TestSen_myMuxFun_i
(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_kq.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_f.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_f.y,
&AUAV_V3_TestSensors_B.sf_myMuxFun_m);
/* DataTypeConversion: '<S589>/Data Type Conversion3' */
AUAV_V3_TestSensors_B.DataTypeConversion3[0] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_m.y[0];
AUAV_V3_TestSensors_B.DataTypeConversion3[1] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_m.y[1];
AUAV_V3_TestSensors_B.DataTypeConversion3[2] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_m.y[2];
/* MATLAB Function: '<S598>/Embedded MATLAB Function' incorporates:
* Constant: '<S589>/Gyro Gains2'
* Constant: '<S598>/Constant'
* Constant: '<S598>/Constant1'
* DataTypeConversion: '<S589>/Data Type Conversion5'
* Product: '<S589>/Divide2'
*/
A_EmbeddedMATLABFunction_f((real_T)((real32_T)rtb_Switch_d[6] * 0.8F), 0.01,
40.0, &AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_ft,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction_ft);
/* MATLAB Function: '<S598>/Embedded MATLAB Function1' incorporates:
* Constant: '<S589>/Gyro Gains2'
* Constant: '<S598>/Constant2'
* Constant: '<S598>/Constant3'
* DataTypeConversion: '<S589>/Data Type Conversion5'
* Gain: '<S601>/[ 1 -1 -1]'
* Product: '<S589>/Divide2'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[8] * 0.8F), 0.01,
40.0, &AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_in,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction1_in);
/* MATLAB Function: '<S598>/Embedded MATLAB Function2' incorporates:
* Constant: '<S589>/Gyro Gains2'
* Constant: '<S598>/Constant4'
* Constant: '<S598>/Constant5'
* DataTypeConversion: '<S589>/Data Type Conversion5'
* Gain: '<S601>/[ 1 -1 -1]'
* Product: '<S589>/Divide2'
*/
A_EmbeddedMATLABFunction_f((real_T)-((real32_T)rtb_Switch_d[7] * 0.8F), 0.01,
40.0, &AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_fu,
&AUAV_V3_TestSensors_DWork.sf_EmbeddedMATLABFunction2_fu);
/* MATLAB Function: '<S598>/myMux Fun' */
AUAV_V3_TestSen_myMuxFun_i
(AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction_ft.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction1_in.y,
AUAV_V3_TestSensors_B.sf_EmbeddedMATLABFunction2_fu.y,
&AUAV_V3_TestSensors_B.sf_myMuxFun_a);
/* DataTypeConversion: '<S589>/Data Type Conversion6' */
AUAV_V3_TestSensors_B.DataTypeConversion6[0] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_a.y[0];
AUAV_V3_TestSensors_B.DataTypeConversion6[1] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_a.y[1];
AUAV_V3_TestSensors_B.DataTypeConversion6[2] = (real32_T)
AUAV_V3_TestSensors_B.sf_myMuxFun_a.y[2];
/* MATLAB Function: '<S622>/Enables//Disables the Computation of initial Baro Bias' */
EnablesDisablestheComputat
(&AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b,
&AUAV_V3_TestSensors_DWork.sf_EnablesDisablestheComputat_b);
/* Outputs for Enabled SubSystem: '<S622>/Initial Baro Bias' incorporates:
* EnablePort: '<S637>/Enable'
*/
if (AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b.tOut > 0.0) {
/* DataTypeConversion: '<S637>/Data Type Conversion' */
rtb_DataTypeConversion_n = rtb_Sum_d;
/* DiscreteZeroPole: '<S640>/Discrete Zero-Pole' */
{
rtb_DiscreteZeroPole_i = 0.014778325123152709*rtb_DataTypeConversion_n;
rtb_DiscreteZeroPole_i += 0.029119852459414206*
AUAV_V3_TestSensors_DWork.DiscreteZeroPole_DSTATE;
}
/* Saturate: '<S637>/[80k - 120k]' incorporates:
* DataTypeConversion: '<S637>/Data Type Conversion1'
*/
if ((real32_T)rtb_DiscreteZeroPole_i > 120000.0F) {
AUAV_V3_TestSensors_B.u0k120k = 120000.0F;
} else if ((real32_T)rtb_DiscreteZeroPole_i < 80000.0F) {
AUAV_V3_TestSensors_B.u0k120k = 80000.0F;
} else {
AUAV_V3_TestSensors_B.u0k120k = (real32_T)rtb_DiscreteZeroPole_i;
}
/* End of Saturate: '<S637>/[80k - 120k]' */
/* Update for DiscreteZeroPole: '<S640>/Discrete Zero-Pole' */
{
AUAV_V3_TestSensors_DWork.DiscreteZeroPole_DSTATE =
rtb_DataTypeConversion_n + 0.97044334975369462*
AUAV_V3_TestSensors_DWork.DiscreteZeroPole_DSTATE;
}
}
/* End of Outputs for SubSystem: '<S622>/Initial Baro Bias' */
/* Product: '<S633>/Divide' incorporates:
* Sum: '<S633>/Sum2'
*/
rtb_Sum_o = (rtb_Sum_d - AUAV_V3_TestSensors_B.u0k120k) /
AUAV_V3_TestSensors_B.u0k120k;
/* S-Function (MCHP_C_function_Call): '<S636>/Get the GS Location [updateSensorMCUState.c]1' */
getGSLocation(
&AUAV_V3_TestSensors_B.GettheGSLocationupdateSensorMCU[0]
);
/* Sum: '<S633>/Sum1' incorporates:
* Constant: '<S633>/Constant2'
* Constant: '<S633>/Constant3'
* Constant: '<S633>/Constant4'
* Constant: '<S633>/Constant5'
* Gain: '<S639>/Unit Conversion'
* Product: '<S633>/Divide1'
* Product: '<S633>/Divide2'
* Product: '<S633>/Divide3'
* Product: '<S633>/Divide4'
* Sum: '<S633>/Sum3'
*/
rtb_Sum_o = ((rtb_Sum_o * rtb_Sum_o * 0.093502529F + rtb_Sum_o * -0.188893303F)
+ 2.18031291E-5F) * 145473.5F * 0.3048F +
AUAV_V3_TestSensors_B.GettheGSLocationupdateSensorMCU[0];
/* Outputs for Enabled SubSystem: '<S622>/Zero Out Height' incorporates:
* EnablePort: '<S638>/Enable'
*/
if (AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b.tOut > 0.0) {
/* Sum: '<S638>/Sum' incorporates:
* Delay: '<S638>/Integer Delay'
*/
AUAV_V3_TestSensors_B.Sum =
AUAV_V3_TestSensors_B.GettheGSLocationupdateSensorMCU[0] -
AUAV_V3_TestSensors_DWork.IntegerDelay_DSTATE;
/* Update for Delay: '<S638>/Integer Delay' */
AUAV_V3_TestSensors_DWork.IntegerDelay_DSTATE = rtb_Sum_o;
}
/* End of Outputs for SubSystem: '<S622>/Zero Out Height' */
/* Outputs for Enabled SubSystem: '<S622>/Enabled Subsystem' */
/* Logic: '<S622>/Logical Operator' incorporates:
* Sum: '<S622>/Sum1'
*/
AUAV_V3_T_EnabledSubsystem
(!(AUAV_V3_TestSensors_B.sf_EnablesDisablestheComputat_b.tOut != 0.0),
AUAV_V3_TestSensors_B.Sum + rtb_Sum_o,
&AUAV_V3_TestSensors_B.EnabledSubsystem_m);
/* End of Outputs for SubSystem: '<S622>/Enabled Subsystem' */
/* Switch: '<S581>/Switch1' */
for (i = 0; i < 5; i++) {
if (rtb_LogicalOperator_df) {
AUAV_V3_TestSensors_B.Switch1[i] =
AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdat_a[i];
} else {
AUAV_V3_TestSensors_B.Switch1[i] =
AUAV_V3_TestSensors_B.ProducetheGPSMainDataandupdatet[i];
}
}
/* End of Switch: '<S581>/Switch1' */
/* S-Function (MCHP_C_function_Call): '<S16>/Checks if FixType is 3 [updateSensorMCUState.c]1' */
AUAV_V3_TestSensors_B.ChecksifFixTypeis3updateSensorM = isFixValid(
);
/* Outputs for Enabled SubSystem: '<S16>/Initialize GS Location' incorporates:
* EnablePort: '<S579>/Enable'
*/
/* Logic: '<S16>/Logical Operator' incorporates:
* DataStoreRead: '<S16>/Data Store Read'
*/
if ((AUAV_V3_TestSensors_B.ChecksifFixTypeis3updateSensorM != 0) &&
(AUAV_V3_TestSensors_DWork.GS_INIT_FLAG != 0.0)) {
/* DataStoreWrite: '<S579>/Data Store Write' incorporates:
* Constant: '<S579>/Constant'
*/
AUAV_V3_TestSensors_DWork.GS_INIT_FLAG = 0.0;
/* DataStoreWrite: '<S579>/Data Store Write1' incorporates:
* DataStoreRead: '<S579>/Data Store Read1'
* DataTypeConversion: '<S579>/Data Type Conversion'
* DataTypeConversion: '<S579>/Data Type Conversion1'
* DataTypeConversion: '<S579>/Data Type Conversion2'
*/
mlGSLocationFloat.lat = (real32_T)mlGpsData.lat;
mlGSLocationFloat.lon = (real32_T)mlGpsData.lon;
mlGSLocationFloat.alt = (real32_T)mlGpsData.alt;
}
/* End of Logic: '<S16>/Logical Operator' */
/* End of Outputs for SubSystem: '<S16>/Initialize GS Location' */
}
/* Function Definitions */
real_T fft_co2(const real_T X[64])
{
real_T snrdB;
real_T dv0[64];
real_T absFFT_sampled_data[64];
int32_T ix;
int32_T i0;
real_T FFT_sampled_data[64];
real_T mtmp;
boolean_T exitg1;
real_T y;
/* newFFT: - returns the discrete Fourier transform (DFT) of vector X, */
/* computed with a fast Fourier transform (FFT) algorithm. If X is a matrix, */
/* newFFT returns the Fourier transform of each column of the matrix */
/* Syntax: Y = newFFT(X,N) */
/* */
/* Inputs: */
/* X - Time domain input signal */
/* N - Number of FFT point */
/* */
/* Outputs: */
/* Y - Frequency domain N point DFT */
/* */
/* Other m-files required: */
/* Subfunctions: */
/* Upperfuncions: */
/* MAT-files required: */
/* */
/* ------------- BEGIN CODE -------------- */
/* If the length of X is greater than n, the sequence X is truncated. */
/* j = sqrt(-1); */
/* DFT Matrix */
for (ix = 0; ix < 64; ix++) {
dv0[ix] = 0.0;
for (i0 = 0; i0 < 64; i0++) {
dv0[ix] = rtNaN;
}
FFT_sampled_data[ix] = 0.015625 * dv0[ix];
absFFT_sampled_data[ix] = FFT_sampled_data[ix];
}
mtmp = absFFT_sampled_data[0];
if (rtIsNaN(absFFT_sampled_data[0])) {
ix = 2;
exitg1 = FALSE;
while ((exitg1 == FALSE) && (ix < 65)) {
if (!rtIsNaN(absFFT_sampled_data[ix - 1])) {
mtmp = absFFT_sampled_data[ix - 1];
exitg1 = TRUE;
} else {
ix++;
}
}
}
y = absFFT_sampled_data[0];
for (ix = 0; ix < 63; ix++) {
y += absFFT_sampled_data[ix + 1];
}
snrdB = 10.0 * log10(mtmp / ((y - mtmp) / 63.0));
/* ------------- END OF CODE -------------- */
return snrdB;
}
//
// Arguments : const double A[476]
// double U[476]
// double S[17]
// double V[289]
// Return Type : void
//
static void eml_xgesvd(const double A[476], double U[476], double S[17], double
V[289])
{
double b_A[476];
double s[17];
double e[17];
int kase;
double work[28];
int q;
int iter;
boolean_T apply_transform;
double ztest0;
int qp1jj;
int qs;
int m;
double rt;
double ztest;
double snorm;
int32_T exitg3;
boolean_T exitg2;
double f;
double varargin_1[5];
double mtmp;
boolean_T exitg1;
double sqds;
memcpy(&b_A[0], &A[0], 476U * sizeof(double));
for (kase = 0; kase < 17; kase++) {
s[kase] = 0.0;
e[kase] = 0.0;
}
memset(&work[0], 0, 28U * sizeof(double));
memset(&U[0], 0, 476U * sizeof(double));
memset(&V[0], 0, 289U * sizeof(double));
for (q = 0; q < 17; q++) {
iter = q + 28 * q;
apply_transform = false;
ztest0 = c_eml_xnrm2(28 - q, b_A, iter + 1);
if (ztest0 > 0.0) {
apply_transform = true;
if (b_A[iter] < 0.0) {
s[q] = -ztest0;
} else {
s[q] = ztest0;
}
if (fabs(s[q]) >= 1.0020841800044864E-292) {
ztest0 = 1.0 / s[q];
kase = (iter - q) + 28;
for (qp1jj = iter; qp1jj + 1 <= kase; qp1jj++) {
b_A[qp1jj] *= ztest0;
}
} else {
kase = (iter - q) + 28;
for (qp1jj = iter; qp1jj + 1 <= kase; qp1jj++) {
b_A[qp1jj] /= s[q];
}
}
b_A[iter]++;
s[q] = -s[q];
} else {
s[q] = 0.0;
}
for (qs = q + 1; qs + 1 < 18; qs++) {
kase = q + 28 * qs;
if (apply_transform) {
eml_xaxpy(28 - q, -(eml_xdotc(28 - q, b_A, iter + 1, b_A, kase + 1) /
b_A[q + 28 * q]), iter + 1, b_A, kase + 1);
}
e[qs] = b_A[kase];
}
for (qp1jj = q; qp1jj + 1 < 29; qp1jj++) {
U[qp1jj + 28 * q] = b_A[qp1jj + 28 * q];
}
if (q + 1 <= 15) {
ztest0 = d_eml_xnrm2(16 - q, e, q + 2);
if (ztest0 == 0.0) {
e[q] = 0.0;
} else {
if (e[q + 1] < 0.0) {
e[q] = -ztest0;
} else {
e[q] = ztest0;
}
ztest0 = e[q];
if (fabs(e[q]) >= 1.0020841800044864E-292) {
ztest0 = 1.0 / e[q];
for (qp1jj = q + 1; qp1jj + 1 < 18; qp1jj++) {
e[qp1jj] *= ztest0;
}
} else {
for (qp1jj = q + 1; qp1jj + 1 < 18; qp1jj++) {
e[qp1jj] /= ztest0;
}
}
e[q + 1]++;
e[q] = -e[q];
for (qp1jj = q + 1; qp1jj + 1 < 29; qp1jj++) {
work[qp1jj] = 0.0;
}
for (qs = q + 1; qs + 1 < 18; qs++) {
b_eml_xaxpy(27 - q, e[qs], b_A, (q + 28 * qs) + 2, work, q + 2);
}
for (qs = q + 1; qs + 1 < 18; qs++) {
c_eml_xaxpy(27 - q, -e[qs] / e[q + 1], work, q + 2, b_A, (q + 28 * qs)
+ 2);
}
}
for (qp1jj = q + 1; qp1jj + 1 < 18; qp1jj++) {
V[qp1jj + 17 * q] = e[qp1jj];
}
}
}
m = 15;
e[15] = b_A[463];
e[16] = 0.0;
for (q = 16; q > -1; q += -1) {
iter = q + 28 * q;
if (s[q] != 0.0) {
for (qs = q + 1; qs + 1 < 18; qs++) {
kase = (q + 28 * qs) + 1;
eml_xaxpy(28 - q, -(eml_xdotc(28 - q, U, iter + 1, U, kase) / U[iter]),
iter + 1, U, kase);
}
for (qp1jj = q; qp1jj + 1 < 29; qp1jj++) {
U[qp1jj + 28 * q] = -U[qp1jj + 28 * q];
}
U[iter]++;
for (qp1jj = 1; qp1jj <= q; qp1jj++) {
U[(qp1jj + 28 * q) - 1] = 0.0;
}
} else {
memset(&U[28 * q], 0, 28U * sizeof(double));
U[iter] = 1.0;
}
}
for (q = 16; q > -1; q += -1) {
if ((q + 1 <= 15) && (e[q] != 0.0)) {
kase = (q + 17 * q) + 2;
for (qs = q + 1; qs + 1 < 18; qs++) {
qp1jj = (q + 17 * qs) + 2;
d_eml_xaxpy(16 - q, -(b_eml_xdotc(16 - q, V, kase, V, qp1jj) / V[kase -
1]), kase, V, qp1jj);
}
}
memset(&V[17 * q], 0, 17U * sizeof(double));
V[q + 17 * q] = 1.0;
}
for (q = 0; q < 17; q++) {
ztest0 = e[q];
if (s[q] != 0.0) {
rt = fabs(s[q]);
ztest = s[q] / rt;
s[q] = rt;
if (q + 1 < 17) {
ztest0 = e[q] / ztest;
}
b_eml_xscal(ztest, U, 1 + 28 * q);
}
if ((q + 1 < 17) && (ztest0 != 0.0)) {
rt = fabs(ztest0);
ztest = rt / ztest0;
ztest0 = rt;
s[q + 1] *= ztest;
c_eml_xscal(ztest, V, 1 + 17 * (q + 1));
}
e[q] = ztest0;
}
iter = 0;
snorm = 0.0;
for (qp1jj = 0; qp1jj < 17; qp1jj++) {
ztest0 = fabs(s[qp1jj]);
ztest = fabs(e[qp1jj]);
if ((ztest0 >= ztest) || rtIsNaN(ztest)) {
} else {
ztest0 = ztest;
}
if ((snorm >= ztest0) || rtIsNaN(ztest0)) {
} else {
snorm = ztest0;
}
}
while ((m + 2 > 0) && (!(iter >= 75))) {
qp1jj = m;
do {
exitg3 = 0;
q = qp1jj + 1;
if (qp1jj + 1 == 0) {
exitg3 = 1;
} else {
ztest0 = fabs(e[qp1jj]);
if ((ztest0 <= 2.2204460492503131E-16 * (fabs(s[qp1jj]) + fabs(s[qp1jj +
1]))) || (ztest0 <= 1.0020841800044864E-292) || ((iter > 20) &&
(ztest0 <= 2.2204460492503131E-16 * snorm))) {
e[qp1jj] = 0.0;
exitg3 = 1;
} else {
qp1jj--;
}
}
} while (exitg3 == 0);
if (qp1jj + 1 == m + 1) {
kase = 4;
} else {
qs = m + 2;
kase = m + 2;
exitg2 = false;
while ((!exitg2) && (kase >= qp1jj + 1)) {
qs = kase;
if (kase == qp1jj + 1) {
exitg2 = true;
} else {
ztest0 = 0.0;
if (kase < m + 2) {
ztest0 = fabs(e[kase - 1]);
}
if (kase > qp1jj + 2) {
ztest0 += fabs(e[kase - 2]);
}
ztest = fabs(s[kase - 1]);
if ((ztest <= 2.2204460492503131E-16 * ztest0) || (ztest <=
1.0020841800044864E-292)) {
s[kase - 1] = 0.0;
exitg2 = true;
} else {
kase--;
}
}
}
if (qs == qp1jj + 1) {
kase = 3;
} else if (qs == m + 2) {
kase = 1;
} else {
kase = 2;
q = qs;
}
}
switch (kase) {
case 1:
f = e[m];
e[m] = 0.0;
for (qp1jj = m; qp1jj + 1 >= q + 1; qp1jj--) {
ztest0 = s[qp1jj];
eml_xrotg(&ztest0, &f, &ztest, &rt);
s[qp1jj] = ztest0;
if (qp1jj + 1 > q + 1) {
f = -rt * e[qp1jj - 1];
e[qp1jj - 1] *= ztest;
}
eml_xrot(V, 1 + 17 * qp1jj, 1 + 17 * (m + 1), ztest, rt);
}
break;
case 2:
f = e[q - 1];
e[q - 1] = 0.0;
for (qp1jj = q; qp1jj + 1 <= m + 2; qp1jj++) {
eml_xrotg(&s[qp1jj], &f, &ztest, &rt);
f = -rt * e[qp1jj];
e[qp1jj] *= ztest;
b_eml_xrot(U, 1 + 28 * qp1jj, 1 + 28 * (q - 1), ztest, rt);
}
break;
case 3:
varargin_1[0] = fabs(s[m + 1]);
varargin_1[1] = fabs(s[m]);
varargin_1[2] = fabs(e[m]);
varargin_1[3] = fabs(s[q]);
varargin_1[4] = fabs(e[q]);
kase = 1;
mtmp = varargin_1[0];
if (rtIsNaN(varargin_1[0])) {
qp1jj = 2;
exitg1 = false;
while ((!exitg1) && (qp1jj < 6)) {
kase = qp1jj;
if (!rtIsNaN(varargin_1[qp1jj - 1])) {
mtmp = varargin_1[qp1jj - 1];
exitg1 = true;
} else {
qp1jj++;
}
}
}
if (kase < 5) {
while (kase + 1 < 6) {
if (varargin_1[kase] > mtmp) {
mtmp = varargin_1[kase];
}
kase++;
}
}
f = s[m + 1] / mtmp;
ztest0 = s[m] / mtmp;
ztest = e[m] / mtmp;
sqds = s[q] / mtmp;
rt = ((ztest0 + f) * (ztest0 - f) + ztest * ztest) / 2.0;
ztest0 = f * ztest;
ztest0 *= ztest0;
if ((rt != 0.0) || (ztest0 != 0.0)) {
ztest = sqrt(rt * rt + ztest0);
if (rt < 0.0) {
ztest = -ztest;
}
ztest = ztest0 / (rt + ztest);
} else {
ztest = 0.0;
}
f = (sqds + f) * (sqds - f) + ztest;
ztest0 = sqds * (e[q] / mtmp);
for (qp1jj = q + 1; qp1jj <= m + 1; qp1jj++) {
eml_xrotg(&f, &ztest0, &ztest, &rt);
if (qp1jj > q + 1) {
e[qp1jj - 2] = f;
}
f = ztest * s[qp1jj - 1] + rt * e[qp1jj - 1];
e[qp1jj - 1] = ztest * e[qp1jj - 1] - rt * s[qp1jj - 1];
ztest0 = rt * s[qp1jj];
s[qp1jj] *= ztest;
eml_xrot(V, 1 + 17 * (qp1jj - 1), 1 + 17 * qp1jj, ztest, rt);
s[qp1jj - 1] = f;
eml_xrotg(&s[qp1jj - 1], &ztest0, &ztest, &rt);
f = ztest * e[qp1jj - 1] + rt * s[qp1jj];
s[qp1jj] = -rt * e[qp1jj - 1] + ztest * s[qp1jj];
ztest0 = rt * e[qp1jj];
e[qp1jj] *= ztest;
b_eml_xrot(U, 1 + 28 * (qp1jj - 1), 1 + 28 * qp1jj, ztest, rt);
}
e[m] = f;
iter++;
break;
default:
if (s[q] < 0.0) {
s[q] = -s[q];
c_eml_xscal(-1.0, V, 1 + 17 * q);
}
kase = q + 1;
while ((q + 1 < 17) && (s[q] < s[kase])) {
rt = s[q];
s[q] = s[kase];
s[kase] = rt;
b_eml_xswap(V, 1 + 17 * q, 1 + 17 * (q + 1));
c_eml_xswap(U, 1 + 28 * q, 1 + 28 * (q + 1));
q = kase;
kase++;
}
iter = 0;
m--;
break;
}
}
memcpy(&S[0], &s[0], 17U * sizeof(double));
}
//
// sample script to create volumetric mesh from
// multiple levelsets of a binary segmented head image.
// Arguments : void
// Return Type : void
//
static void c_meshing()
{
double fid;
static double face[362368];
static double elem[565750];
static double node[100564];
static int idx[113150];
int k;
boolean_T p;
static int idx0[113150];
int i;
int i2;
int j;
int pEnd;
int nb;
int b_k;
int qEnd;
int kEnd;
emxArray_real_T *b;
double x;
int32_T exitg1;
int exponent;
emxArray_real_T *b_b;
// create volumetric tetrahedral mesh from the two-layer 3D images
// head surface element size bound
b_fprintf();
TCHAR pwd[MAX_PATH];
GetCurrentDirectory(MAX_PATH,pwd);
std::string str= pwd;
std::string const command=char(34)+str+"\\bin\\cgalmesh.exe" +char(34)+" pre_cgalmesh.inr post_cgalmesh.mesh 30 4 0.5 3 100 1648335518";
system(command.c_str());
b_readmedit(node, elem, face);
// node=node*double(vol(1,1,1));
for (k = 0; k < 113150; k++) {
idx[k] = k + 1;
}
for (k = 0; k < 113150; k += 2) {
if ((elem[452600 + k] <= elem[k + 452601]) || rtIsNaN(elem[k + 452601])) {
p = true;
} else {
p = false;
}
if (p) {
} else {
idx[k] = k + 2;
idx[k + 1] = k + 1;
}
}
for (i = 0; i < 113150; i++) {
idx0[i] = 1;
}
i = 2;
while (i < 113150) {
i2 = i << 1;
j = 1;
for (pEnd = 1 + i; pEnd < 113151; pEnd = qEnd + i) {
nb = j;
b_k = pEnd - 1;
qEnd = j + i2;
if (qEnd > 113151) {
qEnd = 113151;
}
k = 0;
kEnd = qEnd - j;
while (k + 1 <= kEnd) {
if ((elem[idx[nb - 1] + 452599] <= elem[idx[b_k] + 452599]) || rtIsNaN
(elem[idx[b_k] + 452599])) {
p = true;
} else {
p = false;
}
if (p) {
idx0[k] = idx[nb - 1];
nb++;
if (nb == pEnd) {
while (b_k + 1 < qEnd) {
k++;
idx0[k] = idx[b_k];
b_k++;
}
}
} else {
idx0[k] = idx[b_k];
b_k++;
if (b_k + 1 == qEnd) {
while (nb < pEnd) {
k++;
idx0[k] = idx[nb - 1];
nb++;
}
}
}
k++;
}
for (k = 0; k + 1 <= kEnd; k++) {
idx[(j + k) - 1] = idx0[k];
}
j = qEnd;
}
i = i2;
}
emxInit_real_T(&b, 1);
i2 = b->size[0];
b->size[0] = 113150;
emxEnsureCapacity((emxArray__common *)b, i2, (int)sizeof(double));
for (k = 0; k < 113150; k++) {
b->data[k] = elem[idx[k] + 452599];
}
k = 0;
while ((k + 1 <= 113150) && rtIsInf(b->data[k]) && (b->data[k] < 0.0)) {
k++;
}
b_k = k;
k = 113150;
while ((k >= 1) && rtIsNaN(b->data[k - 1])) {
k--;
}
pEnd = 113150 - k;
while ((k >= 1) && rtIsInf(b->data[k - 1]) && (b->data[k - 1] > 0.0)) {
k--;
}
i2 = 113150 - (k + pEnd);
nb = -1;
if (b_k > 0) {
nb = 0;
}
i = (b_k + k) - b_k;
while (b_k + 1 <= i) {
x = b->data[b_k];
do {
exitg1 = 0;
b_k++;
if (b_k + 1 > i) {
exitg1 = 1;
} else {
fid = fabs(x / 2.0);
if ((!rtIsInf(fid)) && (!rtIsNaN(fid))) {
if (fid <= 2.2250738585072014E-308) {
fid = 4.94065645841247E-324;
} else {
frexp(fid, &exponent);
fid = ldexp(1.0, exponent - 53);
}
} else {
fid = rtNaN;
}
if ((fabs(x - b->data[b_k]) < fid) || (rtIsInf(b->data[b_k]) && rtIsInf
(x) && ((b->data[b_k] > 0.0) == (x > 0.0)))) {
p = true;
} else {
p = false;
}
if (!p) {
exitg1 = 1;
}
}
} while (exitg1 == 0);
nb++;
b->data[nb] = x;
}
if (i2 > 0) {
nb++;
b->data[nb] = b->data[i];
}
b_k = i + i2;
for (j = 1; j <= pEnd; j++) {
nb++;
b->data[nb] = b->data[(b_k + j) - 1];
}
if (1 > nb + 1) {
i = -1;
} else {
i = nb;
}
emxInit_real_T(&b_b, 1);
i2 = b_b->size[0];
b_b->size[0] = i + 1;
emxEnsureCapacity((emxArray__common *)b_b, i2, (int)sizeof(double));
for (i2 = 0; i2 <= i; i2++) {
b_b->data[i2] = b->data[i2];
}
i2 = b->size[0];
b->size[0] = b_b->size[0];
emxEnsureCapacity((emxArray__common *)b, i2, (int)sizeof(double));
i = b_b->size[0];
for (i2 = 0; i2 < i; i2++) {
b->data[i2] = b_b->data[i2];
}
emxFree_real_T(&b_b);
c_fprintf((double)b->size[0]);
d_fprintf();
emxFree_real_T(&b);
}
