ogrt_multisat.c

The OpenGPSRec receiver software runs on the real time operating system RTAI-Linux. It compiles with

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogrt_navigate.c                                         *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   09.12.02                                                *
   *                                                                      *
   *    Author:   C. Kelley, G. Beyerle                                   *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * Copyright (C) 2001-2003  C. Kelley, G. Beyerle                       *
   *                                                                      *
   * This program is free software; you can redistribute it and/or modify *
   * it under the terms of the GNU General Public License as published by *
   * the Free Software Foundation; either version 2 of the License, or    *
   * (at your option) any later version.                                  *
   *                                                                      *
   * This program is distributed in the hope that it will be useful,      *
   * but WITHOUT ANY WARRANTY; without even the implied warranty of       *
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        *
   * GNU General Public License for more details.                         *
   *                                                                      *
   * You should have received a copy of the GNU General Public License    *
   * along with this program; if not, write to the Free Software          *
   * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.            *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * OpenGPS is based on Clifford Kelley's OpenSourceGPS distribution.    *
   * The OpenSourceGPS distribution can be obtained from                  *
   *             http://www.home.earthlink.net/~cwkelley                  *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *                         Real-time process                            *
   *                                                                      *
   ************************************************************************ */

/* ******************************* changes ********************************

   26.01.03 - replace 'trackch' by 'RT_Ctrl->leadch'
              use fields RT_Ctrl->accum_missed & ->accum_new
   18.07.03 - Chan[].ms_count wraps at 6000 (instead of 30000)
   18.07.03 - move Ms_Count to struct Chan[]
   31.07.03 - fixed bug in find/read_preamble()
   03.08.03 - copy carrier-aided code tracking from OpenSourceGPS

   ************************************************************************ */

/***********************************************************************
  GPS RECEIVER (GPSRCVR) Ver. 1.02
  12 Channel All-in-View GPS Receiver Program based on 
  Mitel GP2021 chipset
  Clifford Kelley [e-mail]
  Copyright (c) 1996-2002 Clifford Kelley.  All Rights Reserved.
  This LICENSE must be included with the OGR code.
***********************************************************************/

/*
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

      CONDITIONS

1. Redistributions of GPSRCVR source code must retain the above copyright
notice, this list of conditions, and the following disclaimer.

2. Redistributions in binary form must contain the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.

3. All modifications to the source code must be clearly marked as
such.  Binary redistributions based on modified source code must be
clearly marked as modified versions in the documentation and/or other
materials provided with the distribution.

4. Notice must be given of the location of the availability of the
unmodified current source code, e.g.,
  http://www.Kelley.com/
or
  ftp://ftp.Kelley.com
in the documentation and/or other materials provided with the
distribution.

5. All advertising and published materials mentioning features or use
of this software must display the following acknowledgment:  "This
product includes software developed by Clifford Kelley and other
contributors."

6. The name of Clifford Kelley may not be used to endorse or promote
products derived from this software without specific prior written
permission.

      DISCLAIMER

This software is provided by Clifford Kelley and contributors "as is" and
any expressed or implied warranties, including, but not limited to, the
implied warranties of merchantability and fitness for a particular
purpose are disclaimed.  In no event shall Clifford Kelley or
contributors be liable for any direct, indirect, incidental, special,
exemplary, or consequential damages (including, but not limited to,
procurement of substitute goods or services; loss of use, data, or
profits; or business interruption) however caused and on any theory of
liability, whether in contract, strict liability, or tort (including
negligence or otherwise) arising in any way out of the use of this
software, even if advised of the possibility of such damage.
*/

/* ------------------------------- includes ------------------------------- */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>

#include <rtai.h>
#include <rtai_sched.h>
#include <rtai_fifos.h>
#include <rtai_shm.h>

#include  "port.h"
#include  "ogr_defines.h"
#include  "ogr_structs.h"

#include  "ogrt_defines.h"
#include  "ogrt_globals.h"
#include  "ogrt_prototypes.h"

MODULE_LICENSE( "GPL");

/* ------------------------------ globals --------------------------------- */

static UINT16 Align_Hist_Count[NOFCHN];

static UINT16 Repeat_Align[NOFCHN];

static INT16 Cod_Slew[NOFCHN],
             Confirm_Count[NOFCHN],
             Confirm_Above_Thresh[NOFCHN],
             Pull_In_Count[NOFCHN],
             Align_Count[NOFCHN];

static long  Cod_Phs[NOFCHN],
             Cod_Phs_Prv[NOFCHN],
             Car_Phs[NOFCHN],
             Car_Phs_Prv[NOFCHN],
             Sum[NOFCHN],
             Avg_20ms[NOFCHN],
             Theta_Prv[NOFCHN],
             Th_Rms[NOFCHN],
             Prompt_Mag[NOFCHN],
             Dith_Mag[NOFCHN];

/* ------------------------------ prototypes ------------------------------ */

void ch_setup( INT16 ch);

/* ------------------------------ procedures ------------------------------ */

/* ------------------------------------------------------------------------- *
FUNCTION init_navigate()
RETURNS  None.

PARAMETERS
  ch   channel number (0-11)

PURPOSE
  init 'navigate' mode
* ------------------------------------------------------------------------- */

void init_navigate( void)
{
  INT16 ch;

  RT_Ctrl->leadch = -1;

  for ( ch=0; ch<12; ch++)
    ch_setup( ch);

  return;
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_setup()
RETURNS  None.

PARAMETERS
  ch : channel number

PURPOSE
  initialize GP2021 channel registers with values set in struct
  Chan[] in user space.
  initialize Chan[] structure
* ------------------------------------------------------------------------- */

void ch_setup( INT16 ch)
{
  INT16 dithermode;

  ch_carrier( ch, Chan[ch].car_frq);

// 1.023 MHz chipping rate
  ch_code( ch, Chan[ch].cod_frq);

// 0xa000 for late select satellite

// 0x8000: GPS mode, early select, UPDATE mode
// 0xa000: GPS mode, late select, UPDATE mode

  if ( RT_Ctrl->earlylate > 0)
    dithermode = 0xa000;
  else
    dithermode = 0x8000;

  ch_cntl( ch, PRN_Code[Chan[ch].prn] | dithermode);  

  return;
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_initialize()
RETURNS  None.

PARAMETERS
  ch : channel number

PURPOSE
  initialize GP2021 channel registers with values set in struct
  Chan[] in user space.
  initialize Chan[] structure
* ------------------------------------------------------------------------- */

void ch_initialize( INT16 ch)
{
  INT16 dithermode;

  ch_carrier( ch, Chan[ch].car_frq);

// 1.023 MHz chipping rate
  ch_code( ch, Chan[ch].cod_frq);

// 0xa000 for late select satellite

// 0x8000: GPS mode, early select, UPDATE mode
// 0xa000: GPS mode, late select, UPDATE mode

  if ( RT_Ctrl->earlylate > 0)
    dithermode = 0xa000;
  else
    dithermode = 0x8000;

  ch_cntl( ch, PRN_Code[Chan[ch].prn] | dithermode);  

//  (re)set some Chan[] fields
  Chan[ch].bit_count    = 0;
  Chan[ch].ms_count     = 0;
  Chan[ch].frame_count  = 0;
  Chan[ch].n_freq       = RT_Ctrl->search_min_f;
  Chan[ch].delta_frq    = 1;

//  Chan[ch].codslew = 0;
  Cod_Slew[ch] = 0;

  Chan[ch].tow_sync  = 0;

//  now we can proceed
  Chan[ch].state = acquisition;

//  for (i=0; i<CHIPSPERBIT; i++)
//    Hist_Nav[ch][i] = 0;
//
//  Hist_Idx[ch] = 0;

  return;
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_acquisition()
RETURNS  None.

PARAMETERS None.

PURPOSE
  Step thru carrier space and try to find signal

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

void ch_acquisition( INT16 ch)
{
  long prompt_mag, dith_mag;

//  we need to find preamble (again)
  Chan[ch].tow_sync  = 0;

  if ( abs( Chan[ch].n_freq) <= search_max_f)     //  search frequencies
  {
    prompt_mag = rss( Chan[ch].i_prompt, Chan[ch].q_prompt);
    dith_mag   = rss( Chan[ch].i_dith, Chan[ch].q_dith);

    if (( dith_mag > acq_thresh) && (prompt_mag > acq_thresh))
    {

//  slew back 1 chip so we can confirm the signal
      ch_code_slew( ch, 2044);        
      Chan[ch].state                 = confirm;
      Confirm_Count[ch]         = 0;
      Confirm_Above_Thresh[ch]  = 0;
    }
    else
    {
//  slew forward 1 chip = 2 halfchips
      ch_code_slew( ch, 2);
      Cod_Slew[ch] += 2;
    }

    if ( Cod_Slew[ch] == 2044)
    {
      Chan[ch].n_freq   += Chan[ch].delta_frq;

/*
 * go forward and backwards with increasing 
 * step size, i.e.: 0,1,-1,2,-2,...
 */
      Chan[ch].delta_frq = -(Chan[ch].delta_frq + sign( Chan[ch].delta_frq));

      Chan[ch].car_frq   = CARRIER_REF + Chan[ch].car_corr +
        frq_step * Chan[ch].n_freq;

      ch_carrier( ch, Chan[ch].car_frq);      // update carrier frq

      Cod_Slew[ch] = 0;
    }
  }
  else
  {
    Chan[ch].n_freq    = search_min_f;         // keep looping
    Chan[ch].delta_frq = 1;
    Chan[ch].car_frq    = CARRIER_REF + Chan[ch].car_corr +
      frq_step * Chan[ch].n_freq;                   // set carrier

    ch_carrier( ch, Chan[ch].car_frq);             // select carrier

    Cod_Slew[ch] = 0;
  }

  return;
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_confirm()
RETURNS  None.

PARAMETERS
  ch : channel number

PURPOSE
  confirm enhanced prompt_mag / dith_mag for 'confirm_count_max' ms.

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

void ch_confirm( INT16 ch)
{
  long prompt_mag, dith_mag;

  prompt_mag = rss( Chan[ch].i_prompt, Chan[ch].q_prompt);
  dith_mag   = rss( Chan[ch].i_dith, Chan[ch].q_dith);

  if (( prompt_mag > acq_thresh) || ( dith_mag > acq_thresh))
    Confirm_Above_Thresh[ch] += 1;

  if ( Confirm_Count[ch] >= confirm_count_max)
  {
    if ( Confirm_Above_Thresh[ch] >= confirm_above_thresh_max)
    {
// in n of m cases we've been above threshold -> transition to state 'pull_in'
      Chan[ch].state         = pull_in;
      Pull_In_Count[ch] = 0;
      Sum[ch]           = 0;
      Th_Rms[ch]        = 0;

//  clear globals
      car_phs_res_prv[ch]    = 0;
      cod_phs_res_prv[ch]    = 0;
    }
    else 
// ... we're below threshold -> back to state 'acquisition'
      Chan[ch].state         = acquisition;
  }

  Confirm_Count[ch] += 1;

  return;
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_pull_in()
RETURNS  None.

PARAMETERS
  ch : channel number

PURPOSE
  pull in the frequency by trying to track the signal with a
  combination FLL and PLL
  it will attempt to track for xxx ms, the last xxx ms of data will be
  gathered to determine if we have both code and carrier lock.
  if so we will transition to track

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

void ch_pull_in( INT16 ch)
{
  long dcod_phs, dcar_phs, 
       theta_e, wdot_gain,
       i_sum, q_sum,
       theta, theta_dot,
       prompt_mag, dith_mag, 
       pull_code_k =  50, pull_code_d =  7, 
       pull_carr_k = -12, pull_carr_d = 28,
       phase_test  = 500, rms = 312;

//  rt_printk("in ch_pull_in() ch = %d\n", ch);

//  GP2021 has two tracking arms only: prompt & dither
  i_sum = Chan[ch].i_prompt + Chan[ch].i_dith;
  q_sum = Chan[ch].q_prompt + Chan[ch].q_dith;
//  i_sum = Chan[ch].i_prompt;
//  q_sum = Chan[ch].q_prompt;

  prompt_mag   = rss( Chan[ch].i_prompt, Chan[ch].q_prompt);
  dith_mag     = rss( Chan[ch].i_dith, Chan[ch].q_dith);
  Sum[ch] += prompt_mag + dith_mag;

// code tracking loop