/* 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); } }