/* Function Definitions */
static void b_equalizeOFDM(testMACRouterStackData *SD, const emlrtStack *sp,
const creal_T recv_data[1280], const real_T tx_longPreamble[53], const real_T
tx_pilots[48], const real_T c_tx_pilotLocationsWithoutGuard[4], const real_T
tx_dataSubcarrierIndexies_data[48], const int32_T
tx_dataSubcarrierIndexies_size[2], c_struct_T *estimate, OFDMDemodulator_1
*hPreambleDemod, OFDMDemodulator_1 *hDataDemod, creal_T R[576],
emxArray_creal_T *Rraw)
{
OFDMDemodulator_1 *obj;
const mxArray *y;
static const int32_T iv198[2] = { 1, 45 };
const mxArray *m49;
char_T cv241[45];
int32_T i;
static const char_T cv242[45] = { 'M', 'A', 'T', 'L', 'A', 'B', ':', 's', 'y',
's', 't', 'e', 'm', ':', 'm', 'e', 't', 'h', 'o', 'd', 'C', 'a', 'l', 'l',
'e', 'd', 'W', 'h', 'e', 'n', 'R', 'e', 'l', 'e', 'a', 's', 'e', 'd', 'C',
'o', 'd', 'e', 'g', 'e', 'n' };
const mxArray *b_y;
static const int32_T iv199[2] = { 1, 4 };
char_T cv243[4];
static const char_T cv244[4] = { 's', 't', 'e', 'p' };
const mxArray *c_y;
static const int32_T iv200[2] = { 1, 51 };
char_T cv245[51];
static const char_T cv246[51] = { 'M', 'A', 'T', 'L', 'A', 'B', ':', 's', 'y',
's', 't', 'e', 'm', ':', 'm', 'e', 't', 'h', 'o', 'd', 'C', 'a', 'l', 'l',
'e', 'd', 'W', 'h', 'e', 'n', 'L', 'o', 'c', 'k', 'e', 'd', 'R', 'e', 'l',
'e', 'a', 's', 'e', 'd', 'C', 'o', 'd', 'e', 'g', 'e', 'n' };
const mxArray *d_y;
static const int32_T iv201[2] = { 1, 5 };
char_T cv247[5];
static const char_T cv248[5] = { 's', 'e', 't', 'u', 'p' };
int8_T fullGrid[64];
int32_T k;
static const int8_T iv202[11] = { 0, 1, 2, 3, 4, 5, 59, 60, 61, 62, 63 };
int8_T ii_data[64];
int32_T ii;
boolean_T exitg2;
boolean_T guard2 = FALSE;
int8_T b_ii_data[64];
creal_T recv[64];
emxArray_creal_T *RLongFirst;
const mxArray *e_y;
static const int32_T iv203[2] = { 1, 45 };
const mxArray *f_y;
static const int32_T iv204[2] = { 1, 4 };
const mxArray *g_y;
static const int32_T iv205[2] = { 1, 51 };
const mxArray *h_y;
static const int32_T iv206[2] = { 1, 5 };
boolean_T exitg1;
boolean_T guard1 = FALSE;
creal_T b_recv[64];
emxArray_creal_T *RLongSecond;
emxArray_creal_T *b_R;
creal_T c_R[106];
real_T b_tx_longPreamble[106];
creal_T RNormal[106];
real_T dv13[106];
real_T dv14[106];
real_T REnergy[53];
creal_T RConj[53];
creal_T b_RConj[53];
const mxArray *i_y;
static const int32_T iv207[2] = { 1, 45 };
const mxArray *j_y;
static const int32_T iv208[2] = { 1, 4 };
const mxArray *k_y;
static const int32_T iv209[2] = { 1, 51 };
const mxArray *l_y;
static const int32_T iv210[2] = { 1, 5 };
int8_T b_fullGrid[768];
static const int16_T iv211[48] = { 11, 25, 39, 53, 75, 89, 103, 117, 139, 153,
167, 181, 203, 217, 231, 245, 267, 281, 295, 309, 331, 345, 359, 373, 395,
409, 423, 437, 459, 473, 487, 501, 523, 537, 551, 565, 587, 601, 615, 629,
651, 665, 679, 693, 715, 729, 743, 757 };
boolean_T c_fullGrid[768];
int32_T ii_size[1];
int32_T c_ii_data[768];
real_T d_ii_data[768];
int32_T b_ii_size[1];
creal_T RXPilots[48];
creal_T preambleGainsFull[636];
int32_T ia;
int32_T iv212[3];
static const int8_T iv213[3] = { 48, 12, 1 };
int32_T b_Rraw[3];
creal_T PilotNormal[48];
real_T pilotEqGains_re;
real_T pilotEqGains_im;
real_T a[48];
real_T PilotEnergy[48];
creal_T b_PilotNormal[48];
creal_T pilotEqGains[576];
creal_T preambleGainsFull_data[576];
creal_T b_preambleGainsFull_data[576];
creal_T c_preambleGainsFull_data[576];
real_T preambleGainsFull_data_re;
real_T preambleGainsFull_data_im;
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &c_st;
d_st.tls = c_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
/* equalizeOFDM: Equalize the entire OFDM frame through the use of both */
/* the long preamble from the 802.11a standard and four pilot tones in */
/* the data frames themselves. The gains from the pilots are */
/* interpolated across frequency to fill the data frame and apply gains */
/* to all data subcarriers. */
/* %% Use Long Preamble frame to estimate channel in frequency domain */
/* Separate out received preambles */
emlrtVectorVectorIndexCheckR2012b(1280, 1, 1, 160, &y_emlrtECI, sp);
/* Demod */
st.site = &xr_emlrtRSI;
obj = hPreambleDemod;
if (!obj->isReleased) {
} else {
y = NULL;
m49 = mxCreateCharArray(2, iv198);
for (i = 0; i < 45; i++) {
cv241[i] = cv242[i];
}
emlrtInitCharArrayR2013a(&st, 45, m49, cv241);
emlrtAssign(&y, m49);
b_y = NULL;
m49 = mxCreateCharArray(2, iv199);
for (i = 0; i < 4; i++) {
cv243[i] = cv244[i];
}
emlrtInitCharArrayR2013a(&st, 4, m49, cv243);
emlrtAssign(&b_y, m49);
b_st.site = &cb_emlrtRSI;
c_error(&b_st, message(&b_st, y, b_y, &emlrtMCI), &emlrtMCI);
}
if (!obj->isInitialized) {
b_st.site = &cb_emlrtRSI;
if (!obj->isInitialized) {
} else {
c_y = NULL;
m49 = mxCreateCharArray(2, iv200);
for (i = 0; i < 51; i++) {
cv245[i] = cv246[i];
}
emlrtInitCharArrayR2013a(&b_st, 51, m49, cv245);
emlrtAssign(&c_y, m49);
d_y = NULL;
m49 = mxCreateCharArray(2, iv201);
for (i = 0; i < 5; i++) {
cv247[i] = cv248[i];
}
emlrtInitCharArrayR2013a(&b_st, 5, m49, cv247);
emlrtAssign(&d_y, m49);
c_st.site = &cb_emlrtRSI;
c_error(&c_st, message(&c_st, c_y, d_y, &emlrtMCI), &emlrtMCI);
}
c_st.site = &cb_emlrtRSI;
obj->isInitialized = TRUE;
d_st.site = &db_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
memset(&fullGrid[0], 1, sizeof(int8_T) << 6);
for (k = 0; k < 11; k++) {
fullGrid[iv202[k]] = 0;
}
d_st.site = &fs_emlrtRSI;
i = 0;
ii = 1;
exitg2 = FALSE;
while ((exitg2 == FALSE) && (ii < 65)) {
guard2 = FALSE;
if (fullGrid[ii - 1] == 1) {
i++;
ii_data[i - 1] = (int8_T)ii;
if (i >= 64) {
exitg2 = TRUE;
} else {
guard2 = TRUE;
}
} else {
guard2 = TRUE;
}
if (guard2 == TRUE) {
ii++;
}
}
if (1 > i) {
i = 0;
}
for (k = 0; k < i; k++) {
b_ii_data[k] = ii_data[k];
}
for (k = 0; k < i; k++) {
ii_data[k] = b_ii_data[k];
}
d_st.site = &fs_emlrtRSI;
k = obj->pDataLinearIndices->size[0];
obj->pDataLinearIndices->size[0] = i;
emxEnsureCapacity(&d_st, (emxArray__common *)obj->pDataLinearIndices, k,
(int32_T)sizeof(real_T), &pb_emlrtRTEI);
for (k = 0; k < i; k++) {
obj->pDataLinearIndices->data[k] = ii_data[k];
}
c_st.site = &cb_emlrtRSI;
}
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
memcpy(&recv[0], &recv_data[192], sizeof(creal_T) << 6);
emxInit_creal_T(&st, &RLongFirst, 3, &yb_emlrtRTEI, TRUE);
b_st.site = &cb_emlrtRSI;
OFDMDemodulator_stepImpl(&b_st, obj, recv, RLongFirst);
/* First half of long preamble */
st.site = &yr_emlrtRSI;
obj = hPreambleDemod;
if (!obj->isReleased) {
} else {
e_y = NULL;
m49 = mxCreateCharArray(2, iv203);
for (i = 0; i < 45; i++) {
cv241[i] = cv242[i];
}
emlrtInitCharArrayR2013a(&st, 45, m49, cv241);
emlrtAssign(&e_y, m49);
f_y = NULL;
m49 = mxCreateCharArray(2, iv204);
for (i = 0; i < 4; i++) {
cv243[i] = cv244[i];
}
emlrtInitCharArrayR2013a(&st, 4, m49, cv243);
emlrtAssign(&f_y, m49);
b_st.site = &cb_emlrtRSI;
c_error(&b_st, message(&b_st, e_y, f_y, &emlrtMCI), &emlrtMCI);
}
if (!obj->isInitialized) {
b_st.site = &cb_emlrtRSI;
if (!obj->isInitialized) {
} else {
g_y = NULL;
m49 = mxCreateCharArray(2, iv205);
for (i = 0; i < 51; i++) {
cv245[i] = cv246[i];
}
emlrtInitCharArrayR2013a(&b_st, 51, m49, cv245);
emlrtAssign(&g_y, m49);
h_y = NULL;
m49 = mxCreateCharArray(2, iv206);
for (i = 0; i < 5; i++) {
cv247[i] = cv248[i];
}
emlrtInitCharArrayR2013a(&b_st, 5, m49, cv247);
emlrtAssign(&h_y, m49);
c_st.site = &cb_emlrtRSI;
c_error(&c_st, message(&c_st, g_y, h_y, &emlrtMCI), &emlrtMCI);
}
c_st.site = &cb_emlrtRSI;
obj->isInitialized = TRUE;
d_st.site = &db_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
memset(&fullGrid[0], 1, sizeof(int8_T) << 6);
for (k = 0; k < 11; k++) {
fullGrid[iv202[k]] = 0;
}
d_st.site = &fs_emlrtRSI;
i = 0;
ii = 1;
exitg1 = FALSE;
while ((exitg1 == FALSE) && (ii < 65)) {
guard1 = FALSE;
if (fullGrid[ii - 1] == 1) {
i++;
ii_data[i - 1] = (int8_T)ii;
if (i >= 64) {
exitg1 = TRUE;
} else {
guard1 = TRUE;
}
} else {
guard1 = TRUE;
}
if (guard1 == TRUE) {
ii++;
}
}
if (1 > i) {
i = 0;
}
for (k = 0; k < i; k++) {
b_ii_data[k] = ii_data[k];
}
for (k = 0; k < i; k++) {
ii_data[k] = b_ii_data[k];
}
d_st.site = &fs_emlrtRSI;
k = obj->pDataLinearIndices->size[0];
obj->pDataLinearIndices->size[0] = i;
emxEnsureCapacity(&d_st, (emxArray__common *)obj->pDataLinearIndices, k,
(int32_T)sizeof(real_T), &pb_emlrtRTEI);
for (k = 0; k < i; k++) {
obj->pDataLinearIndices->data[k] = ii_data[k];
}
c_st.site = &cb_emlrtRSI;
}
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
memcpy(&b_recv[0], &recv_data[256], sizeof(creal_T) << 6);
emxInit_creal_T(&st, &RLongSecond, 3, &ac_emlrtRTEI, TRUE);
emxInit_creal_T(&st, &b_R, 3, &pb_emlrtRTEI, TRUE);
b_st.site = &cb_emlrtRSI;
OFDMDemodulator_stepImpl(&b_st, obj, b_recv, RLongSecond);
/* Second half of long preamble */
/* %% Preamble Equalization */
/* Get Equalizer tap gains */
k = RLongFirst->size[0];
ii = RLongSecond->size[0];
emlrtDimSizeEqCheckFastR2012b(k, ii, &x_emlrtECI, sp);
st.site = &as_emlrtRSI;
k = b_R->size[0] * b_R->size[1] * b_R->size[2];
b_R->size[0] = RLongFirst->size[0];
b_R->size[1] = 2;
b_R->size[2] = 1;
emxEnsureCapacity(&st, (emxArray__common *)b_R, k, (int32_T)sizeof(creal_T),
&pb_emlrtRTEI);
i = RLongFirst->size[0];
for (k = 0; k < i; k++) {
b_R->data[k] = RLongFirst->data[k];
}
emxFree_creal_T(&RLongFirst);
i = RLongSecond->size[0];
for (k = 0; k < i; k++) {
b_R->data[k + b_R->size[0]] = RLongSecond->data[k];
}
emxFree_creal_T(&RLongSecond);
/* Calculate Equalizer Taps with preamble symbols */
/* Calculate non-normalized channel gains */
for (k = 0; k < 53; k++) {
ii = b_R->size[0];
i = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i, 1, ii, &lb_emlrtBCI, &st);
}
i = b_R->size[0];
for (k = 0; k < 2; k++) {
for (ii = 0; ii < 53; ii++) {
c_R[ii + 53 * k] = b_R->data[ii + i * k];
}
}
emxFree_creal_T(&b_R);
for (k = 0; k < 53; k++) {
b_tx_longPreamble[k] = tx_longPreamble[k];
b_tx_longPreamble[53 + k] = tx_longPreamble[k];
}
b_st.site = &bt_emlrtRSI;
b_rdivide(c_R, b_tx_longPreamble, RNormal);
/* Known is the original Long Preamble symbols */
/* Scale channel gains */
b_st.site = &ct_emlrtRSI;
d_abs(RNormal, dv13);
memcpy(&dv14[0], &dv13[0], 106U * sizeof(real_T));
b_st.site = &ct_emlrtRSI;
b_power(dv14, dv13);
b_st.site = &ct_emlrtRSI;
c_mean(dv13, REnergy);
b_st.site = &dt_emlrtRSI;
d_mean(RNormal, RConj);
for (k = 0; k < 53; k++) {
RConj[k].im = -RConj[k].im;
b_RConj[k] = RConj[k];
}
b_st.site = &et_emlrtRSI;
c_rdivide(b_RConj, REnergy, RConj);
/* Separate data from preambles */
/* recvData = recv(length(tx.preambles)+1:length(tx.preambles)+(hDataDemod.NumSymbols)*(tx.FFTLength+hDataDemod.CyclicPrefixLength)); */
emlrtVectorVectorIndexCheckR2012b(1280, 1, 1, 960, &w_emlrtECI, sp);
/* CG */
/* OFDM Demod */
st.site = &bs_emlrtRSI;
obj = hDataDemod;
if (!obj->isReleased) {
} else {
i_y = NULL;
m49 = mxCreateCharArray(2, iv207);
for (i = 0; i < 45; i++) {
cv241[i] = cv242[i];
}
emlrtInitCharArrayR2013a(&st, 45, m49, cv241);
emlrtAssign(&i_y, m49);
j_y = NULL;
m49 = mxCreateCharArray(2, iv208);
for (i = 0; i < 4; i++) {
cv243[i] = cv244[i];
}
emlrtInitCharArrayR2013a(&st, 4, m49, cv243);
emlrtAssign(&j_y, m49);
b_st.site = &cb_emlrtRSI;
c_error(&b_st, message(&b_st, i_y, j_y, &emlrtMCI), &emlrtMCI);
}
if (!obj->isInitialized) {
b_st.site = &cb_emlrtRSI;
if (!obj->isInitialized) {
} else {
k_y = NULL;
m49 = mxCreateCharArray(2, iv209);
for (i = 0; i < 51; i++) {
cv245[i] = cv246[i];
}
emlrtInitCharArrayR2013a(&b_st, 51, m49, cv245);
emlrtAssign(&k_y, m49);
l_y = NULL;
m49 = mxCreateCharArray(2, iv210);
for (i = 0; i < 5; i++) {
cv247[i] = cv248[i];
}
emlrtInitCharArrayR2013a(&b_st, 5, m49, cv247);
emlrtAssign(&l_y, m49);
c_st.site = &cb_emlrtRSI;
c_error(&c_st, message(&c_st, k_y, l_y, &emlrtMCI), &emlrtMCI);
}
c_st.site = &cb_emlrtRSI;
g_SystemProp_matlabCodegenSetAn(obj);
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_SystemCore_validateProperties();
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
memset(&b_fullGrid[0], 1, 768U * sizeof(int8_T));
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 11; ii++) {
b_fullGrid[iv202[ii] + (k << 6)] = 0;
}
b_fullGrid[32 + (k << 6)] = 0;
}
d_st.site = &st_emlrtRSI;
d_st.site = &st_emlrtRSI;
for (k = 0; k < 48; k++) {
b_fullGrid[iv211[k]] = 2;
}
d_st.site = &fs_emlrtRSI;
for (k = 0; k < 768; k++) {
c_fullGrid[k] = (b_fullGrid[k] == 1);
}
eml_find(c_fullGrid, c_ii_data, ii_size);
b_ii_size[0] = ii_size[0];
i = ii_size[0];
for (k = 0; k < i; k++) {
d_ii_data[k] = c_ii_data[k];
}
d_st.site = &fs_emlrtRSI;
h_SystemProp_matlabCodegenSetAn(&d_st, obj, d_ii_data, b_ii_size);
c_st.site = &cb_emlrtRSI;
}
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_OFDMDemodulator_stepImpl(SD, &b_st, obj, *(creal_T (*)[960])&recv_data[320],
Rraw, RXPilots);
/* Expand equalizer gains to full frame size */
st.site = &cs_emlrtRSI;
i = 0;
for (ii = 0; ii < 12; ii++) {
ia = 0;
for (k = 0; k < 53; k++) {
preambleGainsFull[i] = RConj[ia];
b_st.site = &ng_emlrtRSI;
ia++;
b_st.site = &og_emlrtRSI;
i++;
}
}
/* Isolate pilot gains from preamble equalizer */
/* Needed to correctly adjust pilot gains */
/* Apply preamble equalizer gains to data and pilots */
/* Correct pilots */
for (i = 0; i < 3; i++) {
iv212[i] = iv213[i];
}
for (k = 0; k < 3; k++) {
b_Rraw[k] = Rraw->size[k];
}
emlrtSizeEqCheckNDR2012b(iv212, b_Rraw, &v_emlrtECI, sp);
/* Correct data */
/* %% Pilot Equalization */
/* Get pilot-based equalizer gains */
st.site = &ds_emlrtRSI;
/* Calculate Equalizer Taps with pilot symbols */
/* Calculate non-normalized channel gains */
b_st.site = &vt_emlrtRSI;
c_st.site = &k_emlrtRSI;
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 4; ii++) {
pilotEqGains_re = preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].re * RXPilots[ii + (k
<< 2)].re - preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].im * RXPilots[ii + (k
<< 2)].im;
pilotEqGains_im = preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].re * RXPilots[ii + (k
<< 2)].im + preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].im * RXPilots[ii + (k
<< 2)].re;
if (pilotEqGains_im == 0.0) {
PilotNormal[ii + (k << 2)].re = pilotEqGains_re / tx_pilots[ii + (k << 2)];
PilotNormal[ii + (k << 2)].im = 0.0;
} else if (pilotEqGains_re == 0.0) {
PilotNormal[ii + (k << 2)].re = 0.0;
PilotNormal[ii + (k << 2)].im = pilotEqGains_im / tx_pilots[ii + (k << 2)];
} else {
PilotNormal[ii + (k << 2)].re = pilotEqGains_re / tx_pilots[ii + (k << 2)];
PilotNormal[ii + (k << 2)].im = pilotEqGains_im / tx_pilots[ii + (k << 2)];
}
}
}
/* Scale channel gains */
b_st.site = &wt_emlrtRSI;
for (k = 0; k < 48; k++) {
c_st.site = &qc_emlrtRSI;
d_st.site = &co_emlrtRSI;
a[k] = muDoubleScalarHypot(PilotNormal[k].re, PilotNormal[k].im);
}
b_st.site = &wt_emlrtRSI;
c_st.site = &n_emlrtRSI;
d_st.site = &hp_emlrtRSI;
for (k = 0; k < 48; k++) {
d_st.site = &o_emlrtRSI;
PilotEnergy[k] = a[k] * a[k];
}
b_st.site = &xt_emlrtRSI;
c_st.site = &k_emlrtRSI;
/* Interpolate to data carrier size */
for (k = 0; k < 48; k++) {
if (-PilotNormal[k].im == 0.0) {
b_PilotNormal[k].re = PilotNormal[k].re / PilotEnergy[k];
b_PilotNormal[k].im = 0.0;
} else if (PilotNormal[k].re == 0.0) {
b_PilotNormal[k].re = 0.0;
b_PilotNormal[k].im = -PilotNormal[k].im / PilotEnergy[k];
} else {
b_PilotNormal[k].re = PilotNormal[k].re / PilotEnergy[k];
b_PilotNormal[k].im = -PilotNormal[k].im / PilotEnergy[k];
}
}
b_st.site = &yt_emlrtRSI;
resample(SD, &b_st, b_PilotNormal, pilotEqGains);
/* Apply Equalizer from Pilots */
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 48; ii++) {
preambleGainsFull_data[ii + 48 * k] = preambleGainsFull[((int32_T)
tx_dataSubcarrierIndexies_data[tx_dataSubcarrierIndexies_size[0] * ii] +
53 * k) - 1];
}
}
for (k = 0; k < 12; k++) {
memcpy(&b_preambleGainsFull_data[48 * k], &preambleGainsFull_data[48 * k],
48U * sizeof(creal_T));
}
i = Rraw->size[0];
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 48; ii++) {
c_preambleGainsFull_data[ii + 48 * k].re = b_preambleGainsFull_data[ii +
48 * k].re * Rraw->data[ii + i * k].re - b_preambleGainsFull_data[ii +
48 * k].im * Rraw->data[ii + i * k].im;
c_preambleGainsFull_data[ii + 48 * k].im = b_preambleGainsFull_data[ii +
48 * k].re * Rraw->data[ii + i * k].im + b_preambleGainsFull_data[ii +
48 * k].im * Rraw->data[ii + i * k].re;
}
}
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 48; ii++) {
pilotEqGains_re = pilotEqGains[ii + 48 * k].re;
pilotEqGains_im = pilotEqGains[ii + 48 * k].im;
preambleGainsFull_data_re = c_preambleGainsFull_data[ii + 48 * k].re;
preambleGainsFull_data_im = c_preambleGainsFull_data[ii + 48 * k].im;
R[ii + 48 * k].re = pilotEqGains_re * preambleGainsFull_data_re -
pilotEqGains_im * preambleGainsFull_data_im;
R[ii + 48 * k].im = pilotEqGains_re * preambleGainsFull_data_im +
pilotEqGains_im * preambleGainsFull_data_re;
}
}
/* Save Gains for visualization */
estimate->pilotEqGains.size[0] = 48;
estimate->pilotEqGains.size[1] = 12;
for (k = 0; k < 576; k++) {
estimate->pilotEqGains.data[k] = pilotEqGains[k];
}
estimate->preambleEqGains.size[0] = 53;
for (k = 0; k < 53; k++) {
estimate->preambleEqGains.data[k] = RConj[k];
}
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
void locateShortpreamble(const emlrtStack *sp, const real_T M_data[1224], real_T
*preambleEstimatedLocation, real_T *numPeaks)
{
int32_T ixstart;
real_T thresholdNorm;
int32_T ix;
boolean_T exitg1;
boolean_T b_M_data[1224];
int32_T ii_size[1];
int32_T ii_data[1224];
int32_T MLocations_size_idx_0;
int32_T loop_ub;
real_T MLocations_data[1224];
int16_T unnamed_idx_0;
int32_T peaks_data[1224];
int32_T i20;
real_T b_MLocations_data[1224];
int32_T MLocations_size[1];
real_T MLocations[8];
int32_T ib_size[1];
int32_T ib_data[8];
int32_T ia_size[1];
int32_T ia_data[8];
int32_T c_size[1];
real_T c_data[8];
int32_T peaks_size[1];
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
/* Locate of the start of the actual preamble from timing metric */
/* % Find peaks of correlation */
/* Adjust threshold */
st.site = &pp_emlrtRSI;
b_st.site = &we_emlrtRSI;
ixstart = 1;
thresholdNorm = M_data[0];
if (muDoubleScalarIsNaN(M_data[0])) {
ix = 2;
exitg1 = FALSE;
while ((exitg1 == FALSE) && (ix <= 1224)) {
ixstart = ix;
if (!muDoubleScalarIsNaN(M_data[ix - 1])) {
thresholdNorm = M_data[ix - 1];
exitg1 = TRUE;
} else {
ix++;
}
}
}
if (ixstart < 1224) {
while (ixstart + 1 <= 1224) {
if (M_data[ixstart] > thresholdNorm) {
thresholdNorm = M_data[ixstart];
}
ixstart++;
}
}
st.site = &pp_emlrtRSI;
thresholdNorm *= 0.6;
st.site = &qp_emlrtRSI;
for (ixstart = 0; ixstart < 1224; ixstart++) {
b_M_data[ixstart] = (M_data[ixstart] > thresholdNorm);
}
b_st.site = &vc_emlrtRSI;
b_eml_find(b_M_data, ii_data, ii_size);
/* Correct estimate to start of preamble not its center */
MLocations_size_idx_0 = ii_size[0];
loop_ub = ii_size[0];
for (ixstart = 0; ixstart < loop_ub; ixstart++) {
MLocations_data[ixstart] = (real_T)ii_data[ixstart] - 9.0;
}
/* Frame Detection */
unnamed_idx_0 = (int16_T)ii_size[0];
loop_ub = (int16_T)ii_size[0];
for (ixstart = 0; ixstart < loop_ub; ixstart++) {
peaks_data[ixstart] = 0;
}
/* Determine correct peak */
st.site = &rp_emlrtRSI;
ix = 0;
while (ix <= ii_size[0] - 1) {
ixstart = ix + 1;
emlrtDynamicBoundsCheckFastR2012b(ixstart, 1, ii_size[0], &cb_emlrtBCI, sp);
if (ix + 1 > ii_size[0]) {
ixstart = 1;
i20 = 1;
} else {
ixstart = emlrtDynamicBoundsCheckFastR2012b(ix + 1, 1, ii_size[0],
&db_emlrtBCI, sp);
i20 = emlrtDynamicBoundsCheckFastR2012b(ii_size[0], 1, ii_size[0],
&db_emlrtBCI, sp) + 1;
}
emlrtVectorVectorIndexCheckR2012b(ii_size[0], 1, 1, i20 - ixstart,
&s_emlrtECI, sp);
st.site = &sp_emlrtRSI;
b_st.site = &eq_emlrtRSI;
MLocations_size[0] = i20 - ixstart;
loop_ub = i20 - ixstart;
for (i20 = 0; i20 < loop_ub; i20++) {
b_MLocations_data[i20] = MLocations_data[(ixstart + i20) - 1];
}
for (ixstart = 0; ixstart < 8; ixstart++) {
MLocations[ixstart] = MLocations_data[ix] + (16.0 + 16.0 * (real_T)ixstart);
}
c_st.site = &fq_emlrtRSI;
do_vectors(&c_st, b_MLocations_data, MLocations_size, MLocations, c_data,
c_size, ia_data, ia_size, ib_data, ib_size);
st.site = &sp_emlrtRSI;
ixstart = (int16_T)ii_size[0];
peaks_data[emlrtDynamicBoundsCheckFastR2012b(ix + 1, 1, ixstart,
&gb_emlrtBCI, sp) - 1] = c_size[0];
ix++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, sp);
}
/* Have at least 5 peaks for positive match */
/* (TUNABLE) */
peaks_size[0] = (int16_T)ii_size[0];
loop_ub = (int16_T)ii_size[0];
for (ixstart = 0; ixstart < loop_ub; ixstart++) {
b_M_data[ixstart] = (peaks_data[ixstart] < 7);
}
st.site = &tp_emlrtRSI;
eml_li_find(&st, b_M_data, peaks_size, ii_data, ii_size);
loop_ub = ii_size[0];
for (ixstart = 0; ixstart < loop_ub; ixstart++) {
i20 = (int16_T)MLocations_size_idx_0;
peaks_data[emlrtDynamicBoundsCheckFastR2012b(ii_data[ixstart], 1, i20,
&eb_emlrtBCI, sp) - 1] = 0;
}
/* Pick earliest peak in time */
if (!(unnamed_idx_0 == 0)) {
st.site = &up_emlrtRSI;
b_st.site = &ir_emlrtRSI;
ixstart = peaks_data[0];
loop_ub = 1;
if (unnamed_idx_0 > 1) {
for (ix = 2; ix <= unnamed_idx_0; ix++) {
if (peaks_data[ix - 1] > ixstart) {
ixstart = peaks_data[ix - 1];
loop_ub = ix;
}
}
}
*numPeaks = ixstart;
if (ixstart > 0) {
*preambleEstimatedLocation =
MLocations_data[emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1,
MLocations_size_idx_0, &fb_emlrtBCI, sp) - 1];
} else {
*preambleEstimatedLocation = -1.0;
/* no desirable location found */
}
} else {
*preambleEstimatedLocation = -1.0;
*numPeaks = 0.0;
}
/* Normalize max peaks found */
st.site = &vp_emlrtRSI;
b_st.site = &j_emlrtRSI;
*numPeaks /= 8.0;
}
/* Function Definitions */
void OFDMbits2letters(const emlrtStack *sp, const boolean_T bits_data[560],
const int32_T bits_size[2], real_T Letters_data[80],
int32_T Letters_size[1])
{
real_T a;
int32_T i32;
int32_T i33;
boolean_T p;
const mxArray *y;
static const int32_T iv237[2] = { 1, 28 };
const mxArray *m43;
char_T cv284[28];
int32_T i;
static const char_T cv285[28] = { 'C', 'o', 'd', 'e', 'r', ':', 'M', 'A', 'T',
'L', 'A', 'B', ':', 'N', 'o', 'n', 'I', 'n', 't', 'e', 'g', 'e', 'r', 'I',
'n', 'p', 'u', 't' };
const mxArray *b_y;
const mxArray *c_y;
int32_T maxdimlen;
boolean_T y_data[560];
const mxArray *d_y;
static const int32_T iv238[2] = { 1, 40 };
char_T cv286[40];
static const char_T cv287[40] = { 'C', 'o', 'd', 'e', 'r', ':', 'M', 'A', 'T',
'L', 'A', 'B', ':', 'g', 'e', 't', 'R', 'e', 's', 'h', 'a', 'p', 'e', 'D',
'i', 'm', 's', '_', 'n', 'o', 't', 'S', 'a', 'm', 'e', 'N', 'u', 'm', 'e',
'l' };
boolean_T b_bits_data[560];
char_T s[7];
int32_T exitg1;
const mxArray *e_y;
static const int32_T iv239[2] = { 1, 34 };
char_T cv288[34];
static const char_T cv289[34] = { 'M', 'A', 'T', 'L', 'A', 'B', ':', 'b', 'i',
'n', '2', 'd', 'e', 'c', ':', 'I', 'l', 'l', 'e', 'g', 'a', 'l', 'B', 'i',
'n', 'a', 'r', 'y', 'S', 't', 'r', 'i', 'n', 'g' };
real_T varargin_1;
real_T p2;
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
emlrtStack e_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &b_st;
d_st.tls = b_st.tls;
e_st.prev = &st;
e_st.tls = st.tls;
/* OFDMbits2letters: Convert input bits from a double array to ascii */
/* integers, which can be converted to letters by the char() function */
/* Make input into column */
/* Trim extra bits */
st.site = &qab_emlrtRSI;
st.site = &qab_emlrtRSI;
b_st.site = &m_emlrtRSI;
c_st.site = &n_emlrtRSI;
a = (real_T)(bits_size[0] * bits_size[1]) / 7.0;
st.site = &qab_emlrtRSI;
b_floor(&a);
st.site = &qab_emlrtRSI;
a *= 7.0;
if (1.0 > a) {
i32 = 0;
} else {
i32 = bits_size[0] * bits_size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i32, &rb_emlrtBCI, sp);
i32 = bits_size[0] * bits_size[1];
i33 = (int32_T)a;
i32 = emlrtDynamicBoundsCheckFastR2012b(i33, 1, i32, &rb_emlrtBCI, sp);
}
emlrtVectorVectorIndexCheckR2012b(bits_size[0] * bits_size[1], 1, 1, i32,
&x_emlrtECI, sp);
/* Shape into letters */
st.site = &rab_emlrtRSI;
st.site = &rab_emlrtRSI;
b_st.site = &m_emlrtRSI;
c_st.site = &n_emlrtRSI;
a = (real_T)i32 / 7.0;
st.site = &rab_emlrtRSI;
b_st.site = &tab_emlrtRSI;
c_st.site = &uab_emlrtRSI;
if (a != muDoubleScalarFloor(a)) {
p = FALSE;
} else {
p = TRUE;
}
if (p) {
c_st.site = &uab_emlrtRSI;
p = TRUE;
} else {
p = FALSE;
}
if (p) {
} else {
y = NULL;
m43 = mxCreateCharArray(2, iv237);
for (i = 0; i < 28; i++) {
cv284[i] = cv285[i];
}
emlrtInitCharArrayR2013a(&b_st, 28, m43, cv284);
emlrtAssign(&y, m43);
b_y = NULL;
m43 = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
*(int32_T *)mxGetData(m43) = MIN_int32_T;
emlrtAssign(&b_y, m43);
c_y = NULL;
m43 = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
*(int32_T *)mxGetData(m43) = MAX_int32_T;
emlrtAssign(&c_y, m43);
c_st.site = &uab_emlrtRSI;
d_st.site = &bcb_emlrtRSI;
c_error(&c_st, c_message(&d_st, y, b_y, c_y, &ac_emlrtMCI), &bc_emlrtMCI);
}
c_st.site = &ny_emlrtRSI;
c_st.site = &ny_emlrtRSI;
b_st.site = &ky_emlrtRSI;
c_st.site = &af_emlrtRSI;
maxdimlen = i32;
if (1 > i32) {
maxdimlen = 1;
}
b_st.site = &ky_emlrtRSI;
c_st.site = &af_emlrtRSI;
if (i32 < maxdimlen) {
} else {
maxdimlen = i32;
}
if (7 > maxdimlen) {
b_st.site = &jy_emlrtRSI;
c_eml_error(&b_st);
}
if ((int8_T)(int32_T)a > maxdimlen) {
b_st.site = &jy_emlrtRSI;
c_eml_error(&b_st);
}
b_st.site = &iy_emlrtRSI;
c_st.site = &v_emlrtRSI;
if (i32 == 7 * (int32_T)a) {
} else {
d_y = NULL;
m43 = mxCreateCharArray(2, iv238);
for (i = 0; i < 40; i++) {
cv286[i] = cv287[i];
}
emlrtInitCharArrayR2013a(&st, 40, m43, cv286);
emlrtAssign(&d_y, m43);
b_st.site = &iy_emlrtRSI;
e_st.site = &rbb_emlrtRSI;
c_error(&b_st, b_message(&e_st, d_y, &tb_emlrtMCI), &ub_emlrtMCI);
}
b_st.site = &hy_emlrtRSI;
c_st.site = &dg_emlrtRSI;
for (maxdimlen = 0; maxdimlen + 1 <= i32; maxdimlen++) {
y_data[maxdimlen] = bits_data[maxdimlen];
}
for (i32 = 0; i32 < 7; i32++) {
maxdimlen = (int32_T)a;
for (i33 = 0; i33 < maxdimlen; i33++) {
b_bits_data[i33 + (int32_T)a * i32] = y_data[i32 + 7 * i33];
}
}
/* Convert bits to letters */
Letters_size[0] = (int32_T)a;
maxdimlen = (int32_T)a;
for (i32 = 0; i32 < maxdimlen; i32++) {
Letters_data[i32] = 0.0;
}
i = 0;
while (i <= (int32_T)a - 1) {
st.site = &sab_emlrtRSI;
i32 = (int32_T)a;
i33 = 1 + i;
emlrtDynamicBoundsCheckFastR2012b(i33, 1, i32, &qb_emlrtBCI, &st);
b_st.site = &of_emlrtRSI;
b_st.site = &vab_emlrtRSI;
for (maxdimlen = 0; maxdimlen < 7; maxdimlen++) {
s[maxdimlen] = '0';
if (b_bits_data[i + (int32_T)a * maxdimlen]) {
s[maxdimlen] = '1';
}
}
st.site = &sab_emlrtRSI;
b_st.site = &wab_emlrtRSI;
maxdimlen = 0;
do {
exitg1 = 0;
if (maxdimlen < 7) {
if ((s[maxdimlen] != '0') && (s[maxdimlen] != '1')) {
p = FALSE;
exitg1 = 1;
} else {
maxdimlen++;
}
} else {
p = TRUE;
exitg1 = 1;
}
} while (exitg1 == 0);
if (p) {
} else {
e_y = NULL;
m43 = mxCreateCharArray(2, iv239);
for (maxdimlen = 0; maxdimlen < 34; maxdimlen++) {
cv288[maxdimlen] = cv289[maxdimlen];
}
emlrtInitCharArrayR2013a(&st, 34, m43, cv288);
emlrtAssign(&e_y, m43);
b_st.site = &wab_emlrtRSI;
e_st.site = &qbb_emlrtRSI;
c_error(&b_st, b_message(&e_st, e_y, &cc_emlrtMCI), &dc_emlrtMCI);
}
b_st.site = &xab_emlrtRSI;
varargin_1 = 0.0;
p2 = 1.0;
for (maxdimlen = 0; maxdimlen < 7; maxdimlen++) {
if (s[6 - maxdimlen] == '1') {
varargin_1 += p2;
}
p2 += p2;
}
st.site = &sab_emlrtRSI;
i32 = (int32_T)varargin_1;
emlrtDynamicBoundsCheckFastR2012b(i32, 0, 255, &emlrtBCI, &st);
i32 = (int32_T)a;
i33 = 1 + i;
Letters_data[emlrtDynamicBoundsCheckFastR2012b(i33, 1, i32, &sb_emlrtBCI, sp)
- 1] = (int8_T)varargin_1;
i++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, sp);
}
}
/* Function Definitions */
void update_eq_17_state(const real_T xk_data[6250], const int32_T xk_size[2],
const emxArray_real_T *vk, const real_T uk_data[250], const int32_T uk_size[1],
real_T xkPlus_data[6250], int32_T xkPlus_size[2])
{
int32_T i0;
int32_T i1;
uint8_T uv0[2];
int32_T loop_ub;
int32_T k;
real_T xdot[17];
real_T vk_data[100];
real_T wi[3];
real_T ai[3];
real_T b_xk_data[25];
real_T xk[9];
real_T b_xk[3];
real_T c_xk[3];
real_T dv0[9];
real_T B;
real_T dv1[9];
real_T d_xk[12];
real_T dv2[12];
static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
real_T xkPlus[4];
real_T dv3[9];
real_T e_xk[9];
real_T c_xk_data[25];
real_T dv4[9];
uint8_T tmp_data[25];
int32_T iv0[1];
static const int32_T iv1[1] = { 17 };
real_T f_xk[17];
/* xk: 17 x 2N+1, inertial position, inertial velocity, inertial quaternion, relative position, and relative quaternion */
/* vk: 12 x 2N+1, noise on wi,(wj_est),ai,(vj_inertial) */
/* uk[wi;ai;mag_i] : 9 x 2N+1 */
/* IMU measurements of me */
emlrtVectorVectorIndexCheckR2012b(uk_size[0], 1, 1, 3, &l_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 1 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, uk_size[0], &j_emlrtBCI,
emlrtRootTLSGlobal);
}
emlrtVectorVectorIndexCheckR2012b(uk_size[0], 1, 1, 3, &k_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 4 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, uk_size[0], &k_emlrtBCI,
emlrtRootTLSGlobal);
}
for (i0 = 0; i0 < 2; i0++) {
uv0[i0] = (uint8_T)xk_size[i0];
}
xkPlus_size[0] = uv0[0];
xkPlus_size[1] = uv0[1];
loop_ub = uv0[0] * uv0[1];
for (i0 = 0; i0 < loop_ub; i0++) {
xkPlus_data[i0] = 0.0;
}
k = 0;
while (k <= xk_size[1] - 1) {
i0 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i0, 1, xk_size[1], &i_emlrtBCI,
emlrtRootTLSGlobal);
/* hist position relative to my. My body frame */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 3, &j_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 11 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &l_emlrtBCI,
emlrtRootTLSGlobal);
}
/* my velocity */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 3, &i_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 4 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &m_emlrtBCI,
emlrtRootTLSGlobal);
}
/* my attitude */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 4, &h_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = 7 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &n_emlrtBCI,
emlrtRootTLSGlobal);
}
/* his attitude relative to mine. His body frame */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 4, &g_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = 14 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &o_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = vk->size[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &h_emlrtBCI, emlrtRootTLSGlobal);
memset(&xdot[0], 0, 17U * sizeof(real_T));
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &f_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 10 + i1;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i0, &p_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &e_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 4 + i1;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i0, &q_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &d_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
loop_ub = vk->size[0];
for (i1 = 0; i1 < loop_ub; i1++) {
vk_data[i1] = vk->data[i1 + vk->size[0] * k];
}
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 1 + i1;
wi[i1] = uk_data[i1] + vk_data[emlrtDynamicBoundsCheckFastR2012b(loop_ub,
1, i0, &r_emlrtBCI, emlrtRootTLSGlobal) - 1];
}
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &c_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
loop_ub = vk->size[0];
for (i1 = 0; i1 < loop_ub; i1++) {
vk_data[i1] = vk->data[i1 + vk->size[0] * k];
}
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 7 + i1;
ai[i1] = uk_data[i1 + 3] + vk_data[emlrtDynamicBoundsCheckFastR2012b
(loop_ub, 1, i0, &s_emlrtBCI, emlrtRootTLSGlobal) - 1];
}
/* cosine matrix */
/* Tim Woodbury */
/* modified for AERO 622 */
/* */
/* [Y] = attpar(X,I,options) */
/* Function to convert between attitude parametrizations */
/* */
/* INPUTS: */
/* X - matrix input of appropriate dimension (detailed later) */
/* I - 2 x 1 indexing vector indicating the input (I(1)) and output (I(2)) */
/* attitude parametrizations, corresponding to the numbers in the section, */
/* "I/O SPECIFICATION PARAMETERS". */
/* options - a data structure. Currently only allows output Euler angle */
/* sequence to be defined. Supported members are "seq" which should be a */
/* [3 x 1] vector describing the first, second, and third rotations in */
/* the desired output sequence. */
/* */
/* OUTPUTS: */
/* Y - output matrix of appropriate dimensions. */
/* All angles are in radians. */
/* */
/* I/O SPECIFICATION PARAMETERS: */
/* 1 - Direction cosine matrix, dimensions [3 x 3] */
/* 2 - Euler principal axis/angle, [3 x 2]. [:,1] is the principal axis; */
/* [1,2] is the principal angle (rad). */
/* 3 - 2-angle parametrization, [3 x 4] */
/* 4 - Euler angle sequence, [3 x 2]. [:,1] are the rotation angles in radians, */
/* and [2,1]-[2,2]-[2,3] is the rotation sequence. */
/* [3-1-3] sequence is default for output. Other sequences may be specified */
/* by passing an optional data structure with a [3 x 1] member "seq" whose */
/* entries [a;b;c] correspond to the sequence a-b-c. Any input sequence */
/* may be used by specifying the second column of input appropriately. */
/* 5 - Classical Rodrigues parameters [3 x 1] */
/* 6 - quaternion [4 x 1]. The scalar part of the quaternion comes FIRST. */
/* 7 - modified Rodrigues parameters [3 x 1] */
/* 8 - exponential matrix, [3 x 3] */
/* 9 - Cayley-Klein parameters, [2 x 2] */
/* check if any options are passed */
/* %check if output and inp are the same - if so do nothing */
/* for each inp, convert to DCM */
/* quaternion */
/* disp('Input value specified as quaternion.'); */
/* convert DCM to output form */
/* DCM */
/* disp('Output value specified as direction cosine matrix.'); */
/* time rate of my position */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
xk[0] = ((xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7] -
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8]) -
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9]) +
xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6];
xk[1] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 8] +
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 6]);
xk[2] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 9] -
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 6]);
xk[3] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 8] -
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 6]);
xk[4] = ((xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8] -
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7]) -
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9]) +
xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6];
xk[5] = 2.0 * (xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 9] +
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 6]);
xk[6] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 9] +
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 6]);
xk[7] = 2.0 * (xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 9] -
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 6]);
xk[8] = ((xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9] -
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7]) -
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8]) +
xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6];
for (i0 = 0; i0 < 3; i0++) {
b_xk[i0] = b_xk_data[i0 + 3];
}
for (i0 = 0; i0 < 3; i0++) {
c_xk[i0] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
c_xk[i0] += xk[i0 + 3 * i1] * b_xk[i1];
}
xdot[i0] = c_xk[i0];
}
/* time rate of my velocity */
/* returns the cross product matrix of the vector x */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv0[0] = 0.0;
dv0[3] = -wi[2];
dv0[6] = wi[1];
dv0[1] = wi[2];
dv0[4] = 0.0;
dv0[7] = -wi[0];
dv0[2] = -wi[1];
dv0[5] = wi[0];
dv0[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
b_xk[i0] = b_xk_data[i0 + 3];
}
for (i0 = 0; i0 < 3; i0++) {
B = 0.0;
for (i1 = 0; i1 < 3; i1++) {
B += dv0[i0 + 3 * i1] * b_xk[i1];
}
c_xk[i0] = ai[i0] - B;
}
for (i0 = 0; i0 < 3; i0++) {
xdot[3 + i0] = c_xk[i0];
}
/* time rate of my attitude */
/* A matrix for qin */
/* returns the cross product matrix of the vector x */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv1[0] = 0.0;
dv1[3] = -xk_data[xk_size[0] * k + 9];
dv1[6] = xk_data[xk_size[0] * k + 8];
dv1[1] = xk_data[xk_size[0] * k + 9];
dv1[4] = 0.0;
dv1[7] = -xk_data[xk_size[0] * k + 7];
dv1[2] = -xk_data[xk_size[0] * k + 8];
dv1[5] = xk_data[xk_size[0] * k + 7];
dv1[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
d_xk[i0 << 2] = -b_xk_data[i0 + 7];
for (i1 = 0; i1 < 3; i1++) {
d_xk[(i1 + (i0 << 2)) + 1] = xk_data[xk_size[0] * k + 6] * (real_T)b[i1
+ 3 * i0] + dv1[i1 + 3 * i0];
}
for (i1 = 0; i1 < 4; i1++) {
dv2[i1 + (i0 << 2)] = 0.5 * d_xk[i1 + (i0 << 2)];
}
}
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
xkPlus[i0] += dv2[i0 + (i1 << 2)] * wi[i1];
}
xdot[6 + i0] = xkPlus[i0];
}
/* relative attitude cosine matrix */
/* Tim Woodbury */
/* modified for AERO 622 */
/* */
/* [Y] = attpar(X,I,options) */
/* Function to convert between attitude parametrizations */
/* */
/* INPUTS: */
/* X - matrix input of appropriate dimension (detailed later) */
/* I - 2 x 1 indexing vector indicating the input (I(1)) and output (I(2)) */
/* attitude parametrizations, corresponding to the numbers in the section, */
/* "I/O SPECIFICATION PARAMETERS". */
/* options - a data structure. Currently only allows output Euler angle */
/* sequence to be defined. Supported members are "seq" which should be a */
/* [3 x 1] vector describing the first, second, and third rotations in */
/* the desired output sequence. */
/* */
/* OUTPUTS: */
/* Y - output matrix of appropriate dimensions. */
/* All angles are in radians. */
/* */
/* I/O SPECIFICATION PARAMETERS: */
/* 1 - Direction cosine matrix, dimensions [3 x 3] */
/* 2 - Euler principal axis/angle, [3 x 2]. [:,1] is the principal axis; */
/* [1,2] is the principal angle (rad). */
/* 3 - 2-angle parametrization, [3 x 4] */
/* 4 - Euler angle sequence, [3 x 2]. [:,1] are the rotation angles in radians, */
/* and [2,1]-[2,2]-[2,3] is the rotation sequence. */
/* [3-1-3] sequence is default for output. Other sequences may be specified */
/* by passing an optional data structure with a [3 x 1] member "seq" whose */
/* entries [a;b;c] correspond to the sequence a-b-c. Any input sequence */
/* may be used by specifying the second column of input appropriately. */
/* 5 - Classical Rodrigues parameters [3 x 1] */
/* 6 - quaternion [4 x 1]. The scalar part of the quaternion comes FIRST. */
/* 7 - modified Rodrigues parameters [3 x 1] */
/* 8 - exponential matrix, [3 x 3] */
/* 9 - Cayley-Klein parameters, [2 x 2] */
/* check if any options are passed */
/* %check if output and inp are the same - if so do nothing */
/* for each inp, convert to DCM */
/* quaternion */
/* disp('Input value specified as quaternion.'); */
/* convert DCM to output form */
/* DCM */
/* disp('Output value specified as direction cosine matrix.'); */
/* relative angular velocity in j frame */
/* A matrix for q_ji */
/* returns the cross product matrix of the vector x */
/* time rate of relative attitude */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv3[0] = 0.0;
dv3[3] = -xk_data[xk_size[0] * k + 16];
dv3[6] = xk_data[xk_size[0] * k + 15];
dv3[1] = xk_data[xk_size[0] * k + 16];
dv3[4] = 0.0;
dv3[7] = -xk_data[xk_size[0] * k + 14];
dv3[2] = -xk_data[xk_size[0] * k + 15];
dv3[5] = xk_data[xk_size[0] * k + 14];
dv3[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
d_xk[i0 << 2] = -b_xk_data[i0 + 14];
for (i1 = 0; i1 < 3; i1++) {
d_xk[(i1 + (i0 << 2)) + 1] = xk_data[xk_size[0] * k + 13] * (real_T)b[i1
+ 3 * i0] + dv3[i1 + 3 * i0];
}
}
loop_ub = vk->size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
vk_data[i0] = vk->data[i0 + vk->size[0] * k];
}
e_xk[0] = ((xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14] -
xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15]) -
xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16]) +
xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13];
e_xk[3] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 15]
+ xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k +
13]);
e_xk[6] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 16]
- xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k +
13]);
e_xk[1] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 15]
- xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k +
13]);
e_xk[4] = ((xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15] -
xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14]) -
xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16]) +
xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13];
e_xk[7] = 2.0 * (xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 16]
+ xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k +
13]);
e_xk[2] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 16]
+ xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k +
13]);
e_xk[5] = 2.0 * (xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 16]
- xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k +
13]);
e_xk[8] = ((xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16] -
xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14]) -
xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15]) +
xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13];
for (i0 = 0; i0 < 3; i0++) {
for (i1 = 0; i1 < 4; i1++) {
dv2[i1 + (i0 << 2)] = 0.5 * d_xk[i1 + (i0 << 2)];
}
}
for (i0 = 0; i0 < 3; i0++) {
B = 0.0;
for (i1 = 0; i1 < 3; i1++) {
B += e_xk[i0 + 3 * i1] * wi[i1];
}
c_xk[i0] = vk_data[i0 + 3] - B;
}
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
xkPlus[i0] += dv2[i0 + (i1 << 2)] * c_xk[i1];
}
xdot[13 + i0] = xkPlus[i0];
}
/* time rate of relative position */
/* returns the cross product matrix of the vector x */
loop_ub = vk->size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
vk_data[i0] = vk->data[i0 + vk->size[0] * k];
}
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
c_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv4[0] = 0.0;
dv4[3] = -wi[2];
dv4[6] = wi[1];
dv4[1] = wi[2];
dv4[4] = 0.0;
dv4[7] = -wi[0];
dv4[2] = -wi[1];
dv4[5] = wi[0];
dv4[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
b_xk[i0] = c_xk_data[i0 + 10];
}
for (i0 = 0; i0 < 3; i0++) {
B = 0.0;
for (i1 = 0; i1 < 3; i1++) {
B += dv4[i0 + 3 * i1] * b_xk[i1];
}
c_xk[i0] = (vk_data[i0 + 9] - b_xk_data[i0 + 3]) - B;
}
for (i0 = 0; i0 < 3; i0++) {
xdot[10 + i0] = c_xk[i0];
}
emlrtSizeEqCheck1DFastR2012b(xk_size[0], 17, &b_emlrtECI, emlrtRootTLSGlobal);
loop_ub = uv0[0];
for (i0 = 0; i0 < loop_ub; i0++) {
tmp_data[i0] = (uint8_T)i0;
}
i0 = uv0[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &g_emlrtBCI, emlrtRootTLSGlobal);
iv0[0] = uv0[0];
emlrtSubAssignSizeCheckR2012b(iv0, 1, iv1, 1, &emlrtECI, emlrtRootTLSGlobal);
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
c_xk_data[i0] = b_xk_data[i0];
}
for (i0 = 0; i0 < 17; i0++) {
f_xk[i0] = c_xk_data[i0] + Ts * xdot[i0];
}
loop_ub = uv0[0];
for (i0 = 0; i0 < loop_ub; i0++) {
xkPlus_data[tmp_data[i0] + uv0[0] * k] = f_xk[i0];
}
/* re-normalize */
i0 = uv0[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &f_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 7 + i0;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &t_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &e_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 7 + i0;
xkPlus[i0] = xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1,
&u_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1];
}
B = norm(xkPlus);
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &d_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = xkPlus_data[(i0 + uv0[0] * k) + 6] / B;
}
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 7 + i0;
xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &v_emlrtBCI,
emlrtRootTLSGlobal) + uv0[0] * k) - 1] = xkPlus[i0];
}
i0 = uv0[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &c_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 14 + i0;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &w_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &b_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 14 + i0;
xkPlus[i0] = xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1,
&x_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1];
}
B = norm(xkPlus);
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = xkPlus_data[(i0 + uv0[0] * k) + 13] / B;
}
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 14 + i0;
xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &y_emlrtBCI,
emlrtRootTLSGlobal) + uv0[0] * k) - 1] = xkPlus[i0];
}
k++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, emlrtRootTLSGlobal);
}
}
