track.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 Swift Navigation Inc.
 * Contact: Fergus Noble <fergus@swift-nav.com>
 *
 * This source is subject to the license found in the file 'LICENSE' which must
 * be be distributed together with this source. All other rights reserved.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 */

#ifndef LIBSWIFTNAV_TRACK_H
#define LIBSWIFTNAV_TRACK_H

#include "common.h"
#include "ephemeris.h"

typedef struct {
  float pgain;         
  float igain;         
  float aiding_igain;  
  float prev_error;    
  float y;             
} aided_lf_state_t;

typedef struct {
  float pgain;      
  float igain;      
  float prev_error; 
  float y;          
} simple_lf_state_t;

typedef struct { //TODO, add carrier aiding to the code loop.
  float carr_freq;             
  aided_lf_state_t carr_filt ; 
  float code_freq;             
  simple_lf_state_t code_filt; 
  float prev_I;                
  float prev_Q;                
} aided_tl_state_t;

typedef struct {
  float code_freq;             
  float carr_freq;             
  simple_lf_state_t code_filt; 
  simple_lf_state_t carr_filt; 
} simple_tl_state_t;

typedef struct {
  float code_freq;             
  float carr_freq;             
  simple_lf_state_t code_filt; 
  simple_lf_state_t carr_filt; 
  u32 sched;                   
  u32 n;                       
  float A;                     
  float carr_to_code;          
} comp_tl_state_t;


typedef struct {
  float I; 
  float Q; 
} correlation_t;

typedef struct {
  float log_bw;     
  float A;          
  float I_prev_abs; 
  float nsr;        
} cn0_est_state_t;

typedef struct {
  u8 prn;                  
  double code_phase_chips; 
  double code_phase_rate;  
  double carrier_phase;    
  double carrier_freq;     
  u32 time_of_week_ms;     
  double receiver_time;    
  double snr;              
  u16 lock_counter;        
} channel_measurement_t;

typedef struct {
  double raw_pseudorange;
  double pseudorange;
  double carrier_phase;
  double raw_doppler;
  double doppler;
  double sat_pos[3];
  double sat_vel[3];
  double snr;
  double lock_time;
  gps_time_t tot;
  u8 prn;
  u16 lock_counter;
} navigation_measurement_t;

void calc_loop_gains(float bw, float zeta, float k, float loop_freq,
                     float *pgain, float *igain);
float costas_discriminator(float I, float Q);
float frequency_discriminator(float I, float Q, float prev_I, float prev_Q);
float dll_discriminator(correlation_t cs[3]);

void aided_lf_init(aided_lf_state_t *s, float y0,
                   float pgain, float igain,
                   float aiding_igain);
float aided_lf_update(aided_lf_state_t *s, float p_i_error, float aiding_error);

void simple_lf_init(simple_lf_state_t *s, float y0,
                    float pgain, float igain);
float simple_lf_update(simple_lf_state_t *s, float error);


void simple_tl_init(simple_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);
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);
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,
                    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 tau, float cpc, u32 sched);
void comp_tl_update(comp_tl_state_t *s, correlation_t cs[3]);

void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0,
                  float cutoff_freq, float loop_freq);
float cn0_est(cn0_est_state_t *s, float I);

void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[],
                                 navigation_measurement_t nav_meas[],
                                 double nav_time, ephemeris_t ephemerides[]);
void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[],
                                  navigation_measurement_t* nav_meas[],
                                  double nav_time, ephemeris_t* ephemerides[]);

int nav_meas_cmp(const void *a, const void *b);
u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new,
                u8 n_old, navigation_measurement_t *m_old,
                navigation_measurement_t *m_corrected);

#endif /* LIBSWIFTNAV_TRACK_H */