correlate.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013 Swift Navigation Inc.
00003  * Contact: Fergus Noble <fergus@swift-nav.com>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 #include <math.h>
00014 
00015 #ifdef __SSSE3__
00016 #include <tmmintrin.h>
00017 #endif
00018 
00019 #include "correlate.h"
00020 
00025 #ifndef __SSSE3__
00026 
00027 void track_correlate(s8* samples, s8* code,
00028                      double* init_code_phase, double code_step, double* init_carr_phase, double carr_step,
00029                      double* I_E, double* Q_E, double* I_P, double* Q_P, double* I_L, double* Q_L, u32* num_samples)
00030 {
00031   double code_phase = *init_code_phase;
00032   double carr_phase = *init_carr_phase;
00033 
00034   double carr_sin = sin(carr_phase);
00035   double carr_cos = cos(carr_phase);
00036   double sin_delta = sin(carr_step);
00037   double cos_delta = cos(carr_step);
00038 
00039   *I_E = *Q_E = *I_P = *Q_P = *I_L = *Q_L = 0;
00040 
00041   double code_E, code_P, code_L;
00042   double baseband_Q, baseband_I;
00043 
00044   *num_samples = (int)ceil((1023.0 - code_phase) / code_step);
00045 
00046   for (u32 i=0; i<*num_samples; i++) {
00047     /*code_E = get_chip(code, (int)ceil(code_phase-0.5));*/
00048     /*code_P = get_chip(code, (int)ceil(code_phase));*/
00049     /*code_L = get_chip(code, (int)ceil(code_phase+0.5));*/
00050     /*code_E = code[(int)ceil(code_phase-0.5)];*/
00051     /*code_P = code[(int)ceil(code_phase)];*/
00052     /*code_L = code[(int)ceil(code_phase+0.5)];*/
00053     code_E = code[(int)(code_phase+0.5)];
00054     code_P = code[(int)(code_phase+1.0)];
00055     code_L = code[(int)(code_phase+1.5)];
00056 
00057     baseband_Q = carr_cos * samples[i];
00058     baseband_I = carr_sin * samples[i];
00059 
00060     double carr_sin_ = carr_sin*cos_delta + carr_cos*sin_delta;
00061     double carr_cos_ = carr_cos*cos_delta - carr_sin*sin_delta;
00062     double i_mag = (3.0 - carr_sin_*carr_sin_ - carr_cos_*carr_cos_) / 2.0;
00063     carr_sin = carr_sin_ * i_mag;
00064     carr_cos = carr_cos_ * i_mag;
00065 
00066     *I_E += code_E * baseband_I;
00067     *Q_E += code_E * baseband_Q;
00068     *I_P += code_P * baseband_I;
00069     *Q_P += code_P * baseband_Q;
00070     *I_L += code_L * baseband_I;
00071     *Q_L += code_L * baseband_Q;
00072 
00073     code_phase += code_step;
00074     carr_phase += carr_step;
00075   }
00076   *init_code_phase = code_phase - 1023;
00077   *init_carr_phase = fmod(carr_phase, 2*M_PI);
00078 }
00079 
00080 #else
00081 
00082 void track_correlate(s8* samples, s8* code,
00083                      double* init_code_phase, double code_step, double* init_carr_phase, double carr_step,
00084                      double* I_E, double* Q_E, double* I_P, double* Q_P, double* I_L, double* Q_L, u32* num_samples)
00085 {
00086   double code_phase = *init_code_phase;
00087 
00088   float carr_sin = sin(*init_carr_phase);
00089   float carr_cos = cos(*init_carr_phase);
00090   float sin_delta = sin(carr_step);
00091   float cos_delta = cos(carr_step);
00092 
00093   *num_samples = (int)ceil((1023.0 - code_phase) / code_step);
00094 
00095   __m128 IE_QE_IP_QP;
00096   __m128 CE_CE_CP_CP;
00097   __m128 IL_QL_X_X;
00098   __m128 CL_CL_X_X;
00099   __m128 S_C_S_C;
00100   __m128 BI_BQ_BI_BQ;
00101   __m128 dC_dS_dS_dC;
00102   __m128 a1, a2, a3;
00103 
00104   IE_QE_IP_QP = _mm_set_ps(0, 0, 0, 0);
00105   IL_QL_X_X = _mm_set_ps(0, 0, 0, 0);
00106   S_C_S_C = _mm_set_ps(carr_sin, carr_cos, carr_sin, carr_cos);
00107   dC_dS_dS_dC = _mm_set_ps(cos_delta, sin_delta, sin_delta, cos_delta);
00108 
00109   for (u32 i=0; i<*num_samples; i++) {
00110     CE_CE_CP_CP = _mm_set_ps(code[(int)(code_phase+0.5)],
00111                              code[(int)(code_phase+0.5)],
00112                              code[(int)(code_phase+1.0)],
00113                              code[(int)(code_phase+1.0)]);
00114     CL_CL_X_X = _mm_set_ps(code[(int)(code_phase+1.5)],
00115                            code[(int)(code_phase+1.5)],
00116                            0, 0);
00117 
00118     /* Load sample and multiply by sin/cos carrier to mix down to baseband. */
00119     a1 = _mm_set1_ps((float)samples[i]); // S, S, S, S
00120     BI_BQ_BI_BQ = _mm_mul_ps(a1, S_C_S_C);
00121 
00122     /* Update carrier sin/cos values by multiplying by the constant rotation
00123      * matrix corresponding to carr_step. */
00124     a1 = _mm_mul_ps(S_C_S_C, dC_dS_dS_dC); // SdC, CdS, SdS, CdC
00125     a2 = _mm_shuffle_ps(a1, a1, _MM_SHUFFLE(3, 0, 3, 0)); // SdC_CdC_SdC_CdC
00126     a3 = _mm_shuffle_ps(a1, a1, _MM_SHUFFLE(2, 1, 2, 1)); // CdS_SdS_CdS_SdS
00127     // S = SdC + CdS, C = CdC - SdS
00128     S_C_S_C = _mm_addsub_ps(a2, a3); // C_S_C_S
00129 
00130     /* Multiply code and baseband signal. */
00131     a1 = _mm_mul_ps(CE_CE_CP_CP, BI_BQ_BI_BQ);
00132     a2 = _mm_mul_ps(CL_CL_X_X, BI_BQ_BI_BQ);
00133 
00134     /* Increment accumulators. */
00135     IE_QE_IP_QP = _mm_add_ps(IE_QE_IP_QP, a1);
00136     IL_QL_X_X   = _mm_add_ps(IL_QL_X_X, a2);
00137 
00138     code_phase += code_step;
00139   }
00140   *init_code_phase = code_phase - 1023;
00141   *init_carr_phase = fmod(*init_carr_phase + *num_samples*carr_step, 2*M_PI);
00142 
00143   float res[8];
00144   _mm_storeu_ps(res, IE_QE_IP_QP);
00145   _mm_storeu_ps(res+4, IL_QL_X_X);
00146 
00147   *I_E = res[3];
00148   *Q_E = res[2];
00149   *I_P = res[1];
00150   *Q_P = res[0];
00151   *I_L = res[7];
00152   *Q_L = res[6];
00153 }
00154 
00155 #endif /* !__SSSE3__ */
00156 


enu
Author(s): Mike Purvis
autogenerated on Fri Jan 3 2014 11:21:07