ogr_main.c

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

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogr_main.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.            *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * The files 'ogr_user.c', 'ogr_navdecode.c', 'ogr_navsolve.c',         *
   * 'ogr_rtproc.c' are modified versions of the files 'gpsfuncs.cpp',    *
   * 'gpsrcvr.cpp' from Clifford Kelley's OpenSourceGPS distribution.     *
   * The unmodified files can be obtained from                            *
   *             http://www.home.earthlink.net/~cwkelley                  *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *                         User space process                           *
   *                                                                      *
   ************************************************************************ */

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

   02.04.03 - reduce CN0 threshold to 20
              call rest_monosat()
   27.04.03 - write pseudo range debug data to file
   03.05.03 - cosmetical changes
   10.12.03 - Msg[].subframe_full = 0 if loss of lock

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

/***********************************************************************
  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 GPSRCVR 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 <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>
#include <string.h>
#include <time.h>
#include <assert.h>
#include <unistd.h>
#include <ctype.h>

#include <fcntl.h>
#include <signal.h>

#include <sys/time.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/stat.h>


#define KEEP_STATIC_INLINE // nam2num macro requires this definition
#include <rtai_shm.h>

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

#define OGRMAIN
# include  "ogr_globals.h"
#undef OGRMAIN

/* ------------------------------- defines -------------------------------- */

#define DBG_PRANGE 0

#define CN0_THRESH 10.0

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

static void reset_monosat( void);
static INT16 bitsum32( unsigned long x);
static void cleanup( void);

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

//static INT16 n_track = 0;   // number of tracked sats

#if DBG_PRANGE
 static FILE *fp_dbg_prange = NULL;
#endif

//UINT16 Parity_Failed[NOFCHN],    // number of subframes with wrong parity
//       Parity_Passed[NOFCHN];    // number of subframes with correct parity

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

/*
 *  OpenGPSRec error handling routine
 */
void ogr_error( char *fmt, ...)
{
  va_list ap;

  va_start( ap, fmt);

  fprintf( stderr, "ogr: ");
  
  for ( ; *fmt; fmt++)
    if ( *fmt == '%')
      switch(*++fmt)
      {
      case 'd':
        fprintf( stderr, "%d", va_arg( ap, int));
        break;
      case 'e':
        fprintf( stderr, "%e", va_arg( ap, double));
        break;
      case 's':
        fprintf( stderr, "%s", va_arg( ap, char *));
        break;
      default:
        fputc( *fmt, stderr);
        break;
      }
    else
      fputc( *fmt, stderr);
      
  va_end( ap);
  fprintf( stderr, "\n");

  cleanup();
  exit( -1);
}

#if 0 
/*
 *  OpenGPSRec message handling routine
 */
void ogr_message( char *fmt, ...)
{
#ifdef NO_SCROLLING
  gotoxy( 1, 24);
#endif

  va_list ap;

  va_start( ap, fmt);

  fprintf( stdout, "ogr: ");
  
  for ( ; *fmt; fmt++)
    if ( *fmt == '%')
      switch(*++fmt)
      {
      case 'd':
        fprintf( stdout, "%d", va_arg( ap, int));
        break;
      case 'e':
        fprintf( stdout, "%e", va_arg( ap, double));
        break;
      case 's':
        fprintf( stdout, "%s", va_arg( ap, char *));
        break;
      default:
        fputc( *fmt, stderr);
        break;
      }
    else
      fputc( *fmt, stdout);
      
  va_end( ap);
  fprintf( stdout, "\n");
  return;
}
#endif

/*
 *
 */
void check_module_loaded( void)
{
  if ( RT_Ctrl->mod_loaded != 0x1234)
  {
    printf("ogr: module ogrt_module.o is not loaded!");
    exit(-1);
  }
}

/*******************************************************************************
FUNCTION display_navigate()
RETURNS  None.

PARAMETERS None.

PURPOSE
  This function displays the current status of the receiver on the
  computer screen.  It is called when there is nothing else to do

WRITTEN BY
  Clifford Kelley

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

static void display_navigate( void)
{
  INT16 n, ch;
  char *Tmp;
  LLH llh;

  static INT16 prn = 1;

  gotoxy( 1, 1);
//  clrscr();

//  ptr = ctime( &Ctrl->thetime);
//  ptr[strlen( ptr)-1] = '\0';     // remove CR/LF
//  printf( "%s        TOW: %6ld\n", ctime( &Ctrl->thetime), Ctrl->clock_tow);

  printf( "OpenGPSRec v%s   TOW: %6ld  %s", 
    PROGRAMVERSION, RT_Ctrl->tow_tic_count/10, ctime( &The_Time));

#if 0
  cur_lat.deg  = (INT16)( rec_pos_llh.lat*RAD2DEG);
  cur_lat.min  = (INT16)( (rec_pos_llh.lat*RAD2DEG - cur_lat.deg)*60);
  cur_lat.sec  = (rec_pos_llh.lat*RAD2DEG - cur_lat.deg -
    cur_lat.min / 60.0) * 3600.0;
  cur_long.deg = (INT16)( rec_pos_llh.lon*RAD2DEG);
  cur_long.min = (INT16)( (rec_pos_llh.lon*RAD2DEG - cur_long.deg)*60);
  cur_long.sec = (rec_pos_llh.lon*RAD2DEG - cur_long.deg -
    cur_long.min / 60.0) * 3600.0;
#endif

#if 0
//  printf("   latitude    longitude          HAE      clock error (ppm)\n");
  printf( "lat: %4d:%02d:%05.2f lon: %4d:%02d:%05.2f HAE: %6.2f m clk err: %f ppm\n",
    cur_lat.deg, abs( cur_lat.min), fabs( cur_lat.sec),
    cur_long.deg, abs( cur_long.min), fabs( cur_long.sec),
    rec_pos_llh.hae, clock_offset);
#endif

  llh = Rcvr_Info.llh;

  printf( "lat: %9.5f lon: %10.5f HAE: %6.2f m clk err: %f ppm\n",
    llh.lat*RAD2DEG, llh.lon*RAD2DEG, llh.hae, Ctrl.clock_offset);

  printf( "speed: %.2f m/s   heading: %.2f deg        dump file: %s\n",
    Rcvr_Info.speed, Rcvr_Info.heading * RAD2DEG, 
    (FpDmp ? DmpFileName : (FpDmpASCII ? DmpASCIIFileName : "-/-                  ")));

//  dilution of precision
  printf( "GDOP: %4.1f  HDOP: %4.1f  VDOP: %4.1f\n",
    Dops.gdop, Dops.hdop, Dops.vdop);

//  GP2021 register access/miss flags
  printf( "accum_new: %03x  accum_missed: %04x  leading chn: %d\n", 
    RT_Ctrl->accum_new, RT_Ctrl->accum_missed, RT_Ctrl->leadch);

  switch (Ctrl.status)
  {
  case mode_tracking:
    Tmp = "track";
  case mode_navigate:
    Tmp = "nav. ";
  default:
    Tmp = "acqu.";
    break;
  }

  printf( "mode: %s  tracking: %2d    almanach valid: %2d    gps week: %4d\n",
    Tmp, Ctrl.nof_trk, bitsum32( Ctrl.almanac_read), Ctrl.gps_week % 1024);

//   summary over all 12 channels
  printf( " ch prn st frq  az  el doppler   cnt frm sfd val pg sync miss CN0 parity\n");

//  in navigation mode, different PRN for each correlator
  for ( ch=0; ch<NOFCHN; ch++)
  {
    n = Chan[ch].prn;
    printf( " %2d %2d %2d %3d %4.0f %3.0f %8ld %4d %3d  %1d   %1d %2d/%2d %1d %5d %4.1f %d/%d\n",
        ch, Chan[ch].prn, Chan[ch].state,
        Chan[ch].n_freq,
        Xmit_Info[n].azimuth * RAD2DEG,
        Xmit_Info[n].elevation * RAD2DEG, 
        Chan[ch].car_frq ? Chan[ch].car_frq - CARRIER_REF : 0,
        Msg[ch].idx,
        Chan[ch].frame_count,                         
        Chan[ch].sfid,
//        GPS_Eph[Chan[ch].prn].ura,
        GPS_Eph[Chan[ch].prn].valid,
        Chan[ch].page4,
        Chan[ch].page5,
        Chan[ch].tow_sync,
        Chan[ch].missed,
        Chan[ch].CN0,
        Parity_Passed[ch],
        Parity_Failed[ch]);
  }

#if 0
  printf( "\n");
  if ( GPS_Alm[prn].text_message[0])
    printf( "PRN = %2d   >%s< \n", prn, GPS_Alm[prn].text_message);
#endif

  prn += 1;
  if ( prn > 32) 
    prn = 1;

  return;
}

/*******************************************************************************
FUNCTION display_monosat()
RETURNS  None.

PARAMETERS None.

PURPOSE
  This function displays the current status of the receiver on the
  computer screen.  It is called when there is nothing else to do

WRITTEN BY
  Clifford Kelley

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

static void display_monosat( void)
{
  INT16 n, ch, leadch;
  LLH llh;

  gotoxy( 1, 1);
//  clrscr();

//  ptr = ctime( &Ctrl->thetime);
//  ptr[strlen( ptr)-1] = '\0';     // remove CR/LF
//  printf( "%s        TOW: %6ld\n", ctime( &Ctrl->thetime), Ctrl->clock_tow);

  printf( "OpenGPSRec v%s   TOW: %6ld  %s", 
    PROGRAMVERSION, RT_Ctrl->tow_tic_count/10, ctime( &The_Time));

#if 0
  cur_lat.deg  = (INT16)( rec_pos_llh.lat*RAD2DEG);
  cur_lat.min  = (INT16)( (rec_pos_llh.lat*RAD2DEG - cur_lat.deg)*60);
  cur_lat.sec  = (rec_pos_llh.lat*RAD2DEG - cur_lat.deg -
    cur_lat.min / 60.0) * 3600.0;
  cur_long.deg = (INT16)( rec_pos_llh.lon*RAD2DEG);
  cur_long.min = (INT16)( (rec_pos_llh.lon*RAD2DEG - cur_long.deg)*60);
  cur_long.sec = (rec_pos_llh.lon*RAD2DEG - cur_long.deg -
    cur_long.min / 60.0) * 3600.0;
#endif

#if 0
//  printf("   latitude    longitude          HAE      clock error (ppm)\n");
  printf( "lat: %4d:%02d:%05.2f lon: %4d:%02d:%05.2f HAE: %6.2f m clk err: %f ppm\n",
    cur_lat.deg, abs( cur_lat.min), fabs( cur_lat.sec),
    cur_long.deg, abs( cur_long.min), fabs( cur_long.sec),
    rec_pos_llh.hae, clock_offset);
#endif

  llh = Rcvr_Info.llh;

  printf( "lat: %9.5f lon: %10.5f HAE: %6.2f m clk err: %f ppm\n",
    llh.lat*RAD2DEG, llh.lon*RAD2DEG,
    llh.hae, Ctrl.clock_offset);

  printf( "speed: %.2f m/s   heading: %.2f deg        dump file: %s\n",
    Rcvr_Info.speed, Rcvr_Info.heading * RAD2DEG, (FpDmp ? DmpFileName : "-/-                  "));

//  dilution of precision
  printf( "GDOP: %4.1f  HDOP: %4.1f  VDOP: %4.1f\n",
    Dops.gdop, Dops.hdop, Dops.vdop);

//  GP2021 register access/miss flags
  printf( "accum_new: %03x  accum_missed: %04x  leading chn: %d\n", 
    RT_Ctrl->accum_new, RT_Ctrl->accum_missed, RT_Ctrl->leadch);

  printf( "tracking: %2d    almanach valid: %2d    gps week: %4d\n",
    Ctrl.nof_trk, bitsum32( Ctrl.almanac_read), Ctrl.gps_week % 1024);

//   summary over all 12 channels
  printf( " ch prn st frq  az  el doppler   cnt frm sfd ura pg sync miss CN0 parity\n");

//  'monosat' mode
  leadch = RT_Ctrl->leadch;
  n = Chan[leadch].prn; 
  printf( " %2d %2d %2d %3d %4.0f %3.0f %8ld %4d %3d %3d %3d %3d %5d %4.1f %d/%d\n",
        leadch, Chan[leadch].prn, Chan[leadch].state,
        Chan[leadch].n_freq,
        Xmit_Info[n].azimuth * RAD2DEG,
        Xmit_Info[n].elevation * RAD2DEG, 
        Chan[leadch].car_frq ? Chan[leadch].car_frq - CARRIER_REF : 0,
        Chan[leadch].bit_count,
        Chan[leadch].frame_count,                         
        Chan[leadch].sfid,
        GPS_Eph[n].ura,
        Chan[leadch].page5,
        Chan[leadch].missed,
        Chan[leadch].CN0,
        Parity_Passed[leadch],
        Parity_Failed[leadch]);

  printf( "\n");

  printf( " ch prn st frq carph codph chpph ms    I     Q\n");
  for ( ch=0; ch<NOFCHN; ch++)
  {
    printf(" %2d %2d %2d %3d %5d %5d %5d %4d %6d %6d\n",
        ch, Chan[ch].prn,
        Chan[ch].state,
        Chan[ch].n_freq,
//        Chan[ch].carr_dco_phase - Chan[leadch].carr_dco_phase,
//        Chan[ch].code_phase - Chan[leadch].code_phase,
//        Chan[ch].code_dco_phase - Chan[leadch].code_dco_phase,
// slave trailing : delay +ve; slave advancing : delay -ve
        Chan[leadch].carr_dco_phase - Chan[ch].carr_dco_phase,
        Chan[leadch].code_phase - Chan[ch].code_phase,
        Chan[leadch].code_dco_phase - Chan[ch].code_dco_phase,
        Chan[ch].missed,
        Chan[ch].i_prompt,
        Chan[ch].q_prompt);
  }

  return;
}

/*******************************************************************************
FUNCTION display_riseset()
RETURNS  None.

PARAMETERS None.

PURPOSE
  This function displays the current status of the receiver on the
  computer screen.  It is called when there is nothing else to do

WRITTEN BY
  Clifford Kelley

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

static void display_riseset( void)
{
  INT16 n, ch, leadch;
  LLH llh;

  gotoxy( 1, 1);
//  clrscr();

//  ptr = ctime( &Ctrl->thetime);
//  ptr[strlen( ptr)-1] = '\0';     // remove CR/LF
//  printf( "%s        TOW: %6ld\n", ctime( &Ctrl->thetime), Ctrl->clock_tow);

  printf( "OpenGPSRec v%s   TOW: %6ld  %s", 
    PROGRAMVERSION, RT_Ctrl->tow_tic_count/10, ctime( &The_Time));

#if 0
  cur_lat.deg  = (INT16)( rec_pos_llh.lat*RAD2DEG);
  cur_lat.min  = (INT16)( (rec_pos_llh.lat*RAD2DEG - cur_lat.deg)*60);
  cur_lat.sec  = (rec_pos_llh.lat*RAD2DEG - cur_lat.deg -
    cur_lat.min / 60.0) * 3600.0;
  cur_long.deg = (INT16)( rec_pos_llh.lon*RAD2DEG);
  cur_long.min = (INT16)( (rec_pos_llh.lon*RAD2DEG - cur_long.deg)*60);
  cur_long.sec = (rec_pos_llh.lon*RAD2DEG - cur_long.deg -
    cur_long.min / 60.0) * 3600.0;
#endif