Skip to content
This repository has been archived by the owner on Jul 26, 2023. It is now read-only.

Commit

Permalink
Merge pull request #22 from henryhallam/rebase-trackcorrelate
Browse files Browse the repository at this point in the history 
Rebase trackcorrelate; optional carrier aiding
  • Loading branch information
Henry Hallam committed Jun 30, 2015
2 parents a04dfd0 + 6e90bce commit 4256718
Show file tree
Hide file tree
Showing 3 changed files with 82 additions and 87 deletions.
101 changes: 26 additions & 75 deletions swiftnav/correlate.pyx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,85 +17,36 @@ cimport correlate_c
from common cimport *
from libc.math cimport M_PI

@cython.boundscheck(False)
def track_correlate_cy(np.ndarray[char, ndim=1, mode="c"] rawSignal,
double codeFreq,
double remCodePhase,
double carrFreq,
double remCarrPhase,
np.ndarray[long, ndim=1, mode="c"] caCode,
settings):
cdef double codePhaseStep = codeFreq/settings.samplingFreq
cdef double carrPhaseStep = carrFreq * 2.0 * M_PI / settings.samplingFreq
cdef unsigned int blksize = <unsigned int>libc.math.ceil((settings.codeLength - remCodePhase)/codePhaseStep)
cdef extern from "complexobject.h":

cdef double codePhase = remCodePhase
cdef double carrPhase = remCarrPhase
struct Py_complex:
double real
double imag

# A cunning trick
cdef double carrSin = libc.math.sin(remCarrPhase)
cdef double carrCos = libc.math.cos(remCarrPhase)
cdef double carrSin_, carrCos_
cdef double sinDelta = libc.math.sin(carrPhaseStep)
cdef double cosDelta = libc.math.cos(carrPhaseStep)
cdef double imag
ctypedef class __builtin__.complex [object PyComplexObject]:
cdef Py_complex cval

cdef double I_E, Q_E, I_P, Q_P, I_L, Q_L
I_E = Q_E = I_P = Q_P = I_L = Q_L = 0

cdef double earlyCode, promptCode, lateCode
cdef double qBasebandSignal, iBasebandSignal

cdef unsigned int i
for i in range(blksize):
earlyCode = caCode[<unsigned int>(libc.math.ceil(codePhase-0.5))]
promptCode = caCode[<unsigned int>(libc.math.ceil(codePhase))]
lateCode = caCode[<unsigned int>(libc.math.ceil(codePhase+0.5))]
#earlyCode = caCode[<unsigned int>(codePhase-0.5)]
#promptCode = caCode[<unsigned int>(codePhase)]
#lateCode = caCode[<unsigned int>(codePhase+0.5)]

#Mix signals to baseband
#qBasebandSignal = libc.math.cos(carrPhase)*rawSignal[i]
#iBasebandSignal = libc.math.sin(carrPhase)*rawSignal[i]
qBasebandSignal = carrCos*rawSignal[i]
iBasebandSignal = carrSin*rawSignal[i]
# Cunning trick - rotate unit vector by an angle carrPhaseStep (delta)
carrSin_ = carrSin*cosDelta + carrCos*sinDelta
carrCos_ = carrCos*cosDelta - carrSin*sinDelta
# This is unstable, need to normalise
#imag = 1.0 / libc.math.sqrt(carrSin_*carrSin_ + carrCos_*carrCos_)
imag = (3.0 - carrSin_*carrSin_ - carrCos_*carrCos_) / 2.0
carrSin = carrSin_ * imag
carrCos = carrCos_ * imag

#Get early, prompt, and late I/Q correlations
I_E += earlyCode * iBasebandSignal
Q_E += earlyCode * qBasebandSignal
I_P += promptCode * iBasebandSignal
Q_P += promptCode * qBasebandSignal
I_L += lateCode * iBasebandSignal
Q_L += lateCode * qBasebandSignal

codePhase += codePhaseStep
carrPhase += carrPhaseStep
remCodePhase = codePhase - 1023
remCarrPhase = carrPhase % (2.0*M_PI)
def track_correlate(np.ndarray[char, ndim=1, mode="c"] samples,
code_freq, code_phase, carr_freq, carr_phase,
np.ndarray[char, ndim=1, mode="c"] code,
sampling_freq):
cdef double init_code_phase = code_phase
cdef double init_carr_phase = carr_phase
cdef double I_E, Q_E, I_P, Q_P, I_L, Q_L
cdef unsigned int blksize

return (I_E, Q_E, I_P, Q_P, I_L, Q_L, blksize, remCodePhase, remCarrPhase)
correlate_c.track_correlate(
<s8*>&samples[0], <s8*>&code[0],
&init_code_phase, code_freq / sampling_freq,
&init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq,
&I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize
)

# TODO: Pass pointers to Python complex number real and imag parts directly
# to C function
E = I_E + Q_E*1.j
P = I_P + Q_P*1.j
L = I_L + Q_L*1.j

def track_correlate(np.ndarray[char, ndim=1, mode="c"] rawSignal,
codeFreq, remCodePhase, carrFreq, remCarrPhase,
np.ndarray[char, ndim=1, mode="c"] caCode,
settings):
cdef double init_code_phase = remCodePhase
cdef double init_carr_phase = remCarrPhase
cdef double I_E, Q_E, I_P, Q_P, I_L, Q_L
cdef unsigned int blksize
correlate_c.track_correlate(<s8*>&rawSignal[0], <s8*>&caCode[0],
&init_code_phase, codeFreq/settings.samplingFreq,
&init_carr_phase, carrFreq * 2.0 * M_PI / settings.samplingFreq,
&I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize)
return (I_E, Q_E, I_P, Q_P, I_L, Q_L, blksize, init_code_phase, init_carr_phase)
return (E, P, L, blksize, init_code_phase, init_carr_phase)

49 changes: 42 additions & 7 deletions swiftnav/track.pyx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,7 +150,7 @@ cdef class NavigationMeasurement:

def calc_navigation_measurement(double t, chan_meas, es):
n_channels = len(chan_meas)
nav_meas = [NavigationMeasurement(0, 0, 0, 0, 0, (0,0,0), (0,0,0), 0, 0, 0, 0, 0) for n in range(n_channels)]
nav_meas = [NavigationMeasurement(0, 0, 0, 0, 0, (0,0,0), (0,0,0), 0, 0, GpsTime(0,0), 0, 0) for n in range(n_channels)]

cdef channel_measurement_t** chan_meas_ptrs = <channel_measurement_t**>malloc(n_channels*sizeof(channel_measurement_t*))
cdef navigation_measurement_t** nav_meas_ptrs = <navigation_measurement_t**>malloc(n_channels*sizeof(navigation_measurement_t*))
Expand Down Expand Up @@ -448,22 +448,31 @@ cdef class AidedTrackingLoop:
----------
code_params : (float, float, float)
Code tracking loop parameter tuple, `(bw, zeta, k)`.
carr_params : (float, float, float)
Carrier tracking loop parameter tuple, `(bw, zeta, k)`.
loop_freq : float
The frequency with which loop updates are performed.
carr_freq_igain : float
FLL aiding gain
carr_to_code : float
Ratio of carrier to code frequency (1540 for GPS L1 C/A) or zero
to disable carrier aiding.
"""

cdef track_c.aided_tl_state_t s
cdef float loop_freq
cdef float code_bw, code_zeta, code_k
cdef float carr_to_code
cdef float carr_bw, carr_zeta, carr_k
cdef float carr_freq_igain

def __cinit__(self, code_params, carr_params, loop_freq, carr_freq_igain):
def __cinit__(self, code_params, carr_params, loop_freq, carr_freq_igain, carr_to_code):
self.loop_freq = loop_freq
self.code_bw, self.code_zeta, self.code_k = code_params
self.carr_bw, self.carr_zeta, self.carr_k = carr_params
self.carr_freq_igain = carr_freq_igain
self.carr_to_code = carr_to_code

def start(self, code_freq, carr_freq):
"""
Expand All @@ -478,12 +487,39 @@ cdef class AidedTrackingLoop:
"""
track_c.aided_tl_init(&self.s, self.loop_freq,
code_freq, self.code_bw,
self.code_zeta, self.code_k,
carr_freq, self.carr_bw,
self.carr_zeta, self.carr_k,
code_freq,
self.code_bw, self.code_zeta, self.code_k,
self.carr_to_code,
carr_freq,
self.carr_bw, self.carr_zeta, self.carr_k,
self.carr_freq_igain)

def retune(self, code_params, carr_params, loop_freq, carr_freq_igain, carr_to_code):
"""
Retune the tracking loop.
Parameters
----------
code_params : (float, float, float)
Code tracking loop parameter tuple, `(bw, zeta, k)`.
carr_params : (float, float, float)
Carrier tracking loop parameter tuple, `(bw, zeta, k)`.
loop_freq : float
The frequency with which loop updates are performed.
carr_freq_igain : float
FLL aiding gain
"""
self.loop_freq = loop_freq
self.code_bw, self.code_zeta, self.code_k = code_params
self.carr_bw, self.carr_zeta, self.carr_k = carr_params
self.carr_freq_igain = carr_freq_igain
track_c.aided_tl_retune(&self.s, self.loop_freq,
self.code_bw, self.code_zeta, self.code_k,
self.carr_to_code,
self.carr_bw, self.carr_zeta, self.carr_k,
self.carr_freq_igain)

def update(self, complex e, complex p, complex l):
"""
Wraps the function :libswiftnav:`aided_tl_update`.
Expand Down Expand Up @@ -525,7 +561,6 @@ cdef class AidedTrackingLoop:
def __get__(self):
return self.s.carr_freq


cdef class CompTrackingLoop:
"""
Wraps the `libswiftnav` code/carrier phase complimentary filter tracking loop
Expand Down
19 changes: 14 additions & 5 deletions swiftnav/track_c.pxd
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ cdef extern from "libswiftnav/track.h":
simple_lf_state_t code_filt
float prev_I
float prev_Q
float carr_to_code

ctypedef struct comp_tl_state_t:
float code_freq
Expand Down Expand Up @@ -93,11 +94,19 @@ cdef extern from "libswiftnav/track.h":
void simple_tl_update(simple_tl_state_t *s, correlation_t cs[3])

void aided_tl_init(aided_tl_state_t *s, float loop_freq,
float code_freq, float code_bw,
float code_zeta, float code_k,
float carr_freq, float carr_bw,
float carr_zeta, float carr_k,
float carr_freq_igain)
float code_freq,
float code_bw, float code_zeta, float code_k,
float carr_to_code,
float carr_freq,
float carr_bw, float carr_zeta, float carr_k,
float carr_freq_b1)

void aided_tl_retune(aided_tl_state_t *s, float loop_freq,
float code_bw, float code_zeta, float code_k,
float carr_to_code,
float carr_bw, float carr_zeta, float carr_k,
float carr_freq_b1)

void aided_tl_update(aided_tl_state_t *s, correlation_t cs[3])

void comp_tl_init(comp_tl_state_t *s, float loop_freq,
Expand Down

0 comments on commit 4256718

Please sign in to comment.