30、GPS L1星历处理实现

\qquad 下面是HD-GR GNSS导航软件的GPS L1星历处理实现代码，入口函数gps_ephemeris_task (…)：

// gps_ephemeris.c -- GPS L1 ephemeris processing.

/* 
 * Copyright (C) 2005 Andrew Greenberg
 * Distributed under the GNU GENERAL PUBLIC LICENSE (GPL) Version 2 (June 1991).
 * See the "COPYING" file distributed with this software for more information.
 */

/* Namuru GPS receiver project
 * Original : ephemeris.c
 * Modes    : LED handling for debugging commented/replaced
 * version  : V1.0
 * date     : 21st/Dec/2006
 */

/* 
 * HD-GR GNSS receiver project
 * Modes    : Inherited the code of ephemeris.c and ephemeris.h in the Namuru 
 *            GPS receiver project V1.0 and made necessary adjustments to adapt 
 *            to the new RTOS and functions.
 * version  : V1.0
 * date     : xx/xx/2015
 */

#include <io.h>
#include <stdio.h>
#include <math.h>
#include "includes.h"
#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"
#include "sys/alt_irq.h"
#include "gnsstime.h"
#include "gps_message.h"
#include "gps_ephemeris.h"
#include "gps_almanac.h"

/******************************************************************************
 * Defines
 ******************************************************************************/

/******************************************************************************
 * Globals
 ******************************************************************************/

gps_ephemeris_t g_gps_ephemeris[GPS_MAX_CHANNELS];
gps_ephemeris_t m_gps_ephetable[GPS_MAX_SATELLITES];

unsigned short m_gps_new_almanac = 0;	// 有新历书有待保存标志
int g_gps_alm_savtime = 0;				// 历书上次保存时间(秒)

/******************************************************************************
 * Statics
 ******************************************************************************/

// None

/******************************************************************************
 * Initialize ephemeris table.
 ******************************************************************************/
void gps_initialize_ephetable( void)
{
	unsigned short sv;
	for (sv=0; sv<GPS_MAX_SATELLITES; sv++) {
		m_gps_ephetable[sv].valid = 0;
	}
}

/******************************************************************************
 * If the channel is reallocated, then clear the ephemeris data for that
 * channel. Called from allocate.c
 ******************************************************************************/
void gps_clear_ephemeris(unsigned short ch)
{
	g_gps_ephemeris[ch].valid = 0;
	g_gps_ephemeris[ch].have_subframe = 0;
	g_gps_ephemeris[ch].prn = 0;
}

   
/******************************************************************************
 * Convert subframe bits to ephemeris values.
 *
 * Note that subframes aren't passed up from the message_thread unless they're
 * already considered valid (all parity checks have passed and been removed)
 ******************************************************************************/
static void gps_process_subframe1(unsigned short ch)
{
	signed long temp;
	unsigned long utemp;

	// map the messages structure to OSGPS's "sf"
	gps_subframe_t * sf = m_gps_messages[ch].subframes;
	unsigned short sv = m_gps_messages[ch].prn-1;
    
	// Calculate the `Issue Of Data Clock' (IODC)
	utemp = ((sf[1-1].word[3-1] & 0x3) << 8 ) | (sf[1-1].word[8-1] >> 16);
    
	// Skedaddle if we already have a valid ephemeris and we have this subframe,
	// and the IODC hasn't changed.
	if ((m_gps_ephetable[sv].valid) &&
		(m_gps_ephetable[sv].have_subframe & (1 << 0))&&
		(m_gps_ephetable[sv].iodc == (unsigned short)utemp)) {
		return;
	}
        
	g_gps_ephemeris[ch].iodc = (unsigned short)utemp;
    
	g_gps_ephemeris[ch].ura = (unsigned short)((sf[1-1].word[3-1] & 0xF00) >> 8);
	g_gps_ephemeris[ch].health = (unsigned short)((sf[1-1].word[3-1] & 0xFC) >> 2);

	// According to ICD-GPDS-200C sect. 20.3.3.3.1.4, if the MSB of the 6 bit
	// health is set, the satellite's nav message is toast. Bad satellite!
	if (g_gps_ephemeris[ch].health & (1 << 5)) {
#ifdef GNSS_ENABLE_MUTEX
		OSMutexPend(m_EphTabMutex, 0, &err);
#endif
		m_gps_ephetable[sv].valid = 0;
#ifdef GNSS_ENABLE_MUTEX
		OSMutexPost(m_EphTabMutex);
#endif
		return;
	}
    
	// Grab the PRN for good measure
	g_gps_ephemeris[ch].prn = m_gps_messages[ch].prn;

	// If we haven't already, then update the time with the week number in
	// this subframe. Note that we have NO stinking clue what the true
	// year is because the week is modulo 1024 which is about 20 years.
	// So we'll just guess it's past 2000 :) and before ~ 2020 which
	// means adding 1024 to the current week.
	utemp = (sf[1-1].word[3-1] >> 14) + 1024;
	set_time_with_weeks( (unsigned short)utemp, 0);
    
	// Get the rest of the ephemerides         
	utemp = sf[1-1].word[8-1] & 0xffff;
	g_gps_ephemeris[ch].toc = (double)(utemp << 4);
	//	g_gps_ephemeris[ch].toc = (double)utemp * c_2p4;

	// The following variables are signed integers so if the MSB is set,
	// 'deal 
	// with the sign. Standard sign extending |= 0xFFFFFF00 wasn't
	// working for whatever reason?! Yes, this sucks but the if makes it
	// faster than another multiply, even by -1.
	// TODO try (-((1<<n) - x)) for (x) of (n)bits.
	temp = sf[1-1].word[7-1] & 0xff;
	if (temp & (1 << 7))
		temp |= ~0xff;
	g_gps_ephemeris[ch].tgd = (double)temp * c_2m31;
            
	temp = sf[1-1].word[9-1] >> 16;
	if (temp & (1 << 7))
		temp |= ~0xff;
	g_gps_ephemeris[ch].af2 = (double)temp * c_2m55;

	temp = sf[1-1].word[9-1] & 0xffff;
	if (temp & (1 << 15))
		temp |= ~0xFFFF;
	g_gps_ephemeris[ch].af1 = (double)temp * c_2m43;
            
	temp = sf[1-1].word[10-1] >> 2;
	if (temp & (1 << 21))
		temp |= ~0x3FFFFF;
	g_gps_ephemeris[ch].af0 = (double)temp * c_2m31;
    
	// Got subframe 1
	g_gps_ephemeris[ch].have_subframe |= (1 << 0);
}
        

static void gps_process_subframe2( unsigned short ch)
{
	long temp;
	unsigned long ultemp;
	unsigned short short_temp;

	// map the messages structure to OSGPS's "sf"
	gps_subframe_t * sf = m_gps_messages[ch].subframes;
	unsigned short sv = m_gps_messages[ch].prn-1;

	short_temp = (unsigned short)(sf[2-1].word[3-1] >> 16);

	// If we already have a valid ephemeris, and we have this
	// subframe, and the `Issue Of Data Ephemeris' (IODE) hasn't changed.
	if ((m_gps_ephetable[sv].valid) &&
		(m_gps_ephetable[sv].have_subframe & (1 << 1)) &&
		(m_gps_ephetable[sv].iode1 == short_temp)) {
		return;
	}

	// Some of these data words are signed; check their sign bit and extend
	// as appropriate

	g_gps_ephemeris[ch].iode1 = (unsigned short)short_temp;

	// Grab the PRN for good measure    
	g_gps_ephemeris[ch].prn = m_gps_messages[ch].prn;

	temp = sf[2-1].word[3-1] & 0xFFFF;
	if (temp & (1 << 15))
		temp |= ~0xFFFF;
	g_gps_ephemeris[ch].crs = (double)temp * c_2m5;
    
	temp = sf[2-1].word[4-1] >> 8;
	if (temp & (1 << 15))
		temp |= ~0xFFFF;
	g_gps_ephemeris[ch].dn = (double)temp * (c_2m43 * PI);

	temp = ((sf[2-1].word[4-1] & 0xFF) << 24) | sf[2-1].word[5-1];
	g_gps_ephemeris[ch].ma = (double)temp * (c_2m31 * PI);

	temp = sf[2-1].word[6-1] >> 8;
	if (temp & (1 << 15))
		temp |= ~0xFFFF;
	g_gps_ephemeris[ch].cuc = (double)temp * c_2m29;

	temp = ((sf[2-1].word[6-1] & 0xFF) << 24) | sf[2-1].word[7-1];
	g_gps_ephemeris[ch].ety = (double)temp * c_2m33;

	temp = sf[2-1].word[8-1] >> 8;
	if (temp & (1 << 15))
		temp |= ~0xFFFF;
	g_gps_ephemeris[ch].cus = (double)temp * c_2m29;

	ultemp = (((sf[2-1].word[8-1] & 0xFF) << 24) | sf[2-1].word[9-1]);
	g_gps_ephemeris[ch].sqra = (double)ultemp * c_2m19;
    
	ultemp = (sf[2-1].word[10-1] >> 8);
//	g_gps_ephemeris[ch].toe = (double)ultemp * c_2p4;
	g_gps_ephemeris[ch].toe = (double)(ultemp << 4);

	// Got subframe 2
	g_gps_ephemeris[ch].have_subframe |= (1 << 1);
}
        
            
static void gps_process_subframe3( unsigned short ch)
{
	long temp;
	unsigned short short_temp;

	// map the messages structure to OSGPS's "sf"
	gps_subframe_t * sf = m_gps_messages[ch].subframes;
	unsigned short sv = m_gps_messages[ch].prn-1;

	short_temp  =  (unsigned short)(sf[3-1].word[10-1] >> 16);

	// If we already have a valid ephemeris, we have this subframe,
	// and the IODE hasn't changed.

	if ((m_gps_ephetable[sv].valid) &&
		(m_gps_ephetable[sv].have_subframe & (1 << 3)) &&
		(m_gps_ephetable[sv].iode2 == short_temp)) {
		return;
	}

	// Some of these data words are signed; check their sign bit and extend
	// as appropriate

	g_gps_ephemeris[ch].iode2 = (unsigned short)short_temp;

	// Grab the PRN for good measure    
	g_gps_ephemeris[ch].prn = m_gps_messages[ch].prn;

	// Some of these data words are signed; check their sign bit and extend
	// as appropriate

	temp = sf[3-1].word[3-1] >> 8;
	if (temp & (1 << 15))
		temp |=  ~0xFFFF;
	g_gps_ephemeris[ch].cic = (double)temp * c_2m29;

	temp = ((sf[3-1].word[3-1] & 0xFF) << 24) | sf[3-1].word[4-1];
	g_gps_ephemeris[ch].w0 = (double)temp * (c_2m31 * PI);

	temp = sf[3-1].word[5-1] >> 8;
	if (temp & (1 << 15))
		temp |=  ~0xFFFF;
	g_gps_ephemeris[ch].cis = (double)temp * c_2m29;

	temp = ((sf[3-1].word[5-1] & 0xFF) << 24) | sf[3-1].word[6-1];
	g_gps_ephemeris[ch].inc0 = (double)temp * (c_2m31 * PI);

	temp = sf[3-1].word[7-1] >> 8;
	if (temp & (1 << 15))
		temp |=  ~0xFFFF;
	g_gps_ephemeris[ch].crc = (double)temp * c_2m5;
    
	temp = ((sf[3-1].word[7-1] & 0xFF) << 24) | sf[3-1].word[8-1];
	g_gps_ephemeris[ch].w = (double)temp * (c_2m31 * PI);

	temp = sf[3-1].word[9-1];
	if (temp & (1 << 23))
		temp |=  ~0xFFFFFF;
	g_gps_ephemeris[ch].omegadot = (double)temp * (c_2m43 * PI);
    
	temp = (sf[3-1].word[10-1] >> 2) & 0x3FFF;
	if (temp & (1 << 13))
		temp |=  ~0x3FFF;
	g_gps_ephemeris[ch].idot = (double)temp * (c_2m43 * PI);

	// Got subframe 3
	g_gps_ephemeris[ch].have_subframe |=  (1 << 2);
}

static void gps_process_subframe4( unsigned short ch)
{
	gps_almanac_process_subframe4(ch);

	// Sure, you got subframe 4, why not?
	g_gps_ephemeris[ch].have_subframe |=  (1 << 3);
}
    
static void gps_process_subframe5( unsigned short ch)
{
	gps_almanac_process_subframe5(ch);

	// Sure, you got subframe 5, why not?
	g_gps_ephemeris[ch].have_subframe |=  (1 << 4);
}

void gps_ephemeris_task(OS_FLAGS channels_ready)
{
	INT8U err;
	unsigned short ch, sv;
    OS_FLAGS subframes;
    
	for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
		if (channels_ready & (1 << ch)) {
			subframes = OSFlagAccept(m_EphemerisSubframeFlags[ch], 0x01f,
				OS_FLAG_WAIT_SET_ANY + OS_FLAG_CONSUME, &err);

			// Look for subframes to process
			if (subframes & (1 << 0)) {
				gps_process_subframe1(ch);
			}

			if (subframes & (1 << 1)) {
				gps_process_subframe2(ch);
			}

			if (subframes & (1 << 2)) {
				gps_process_subframe3(ch);
			}

			// Almanac decoding
			if (subframes & (1 << 3)) {
				gps_process_subframe4(ch);
			}

			if (subframes & (1 << 4)) {
				gps_process_subframe5(ch);
			}

			// We've processed all available subframes in this channel.
			if (((g_gps_ephemeris[ch].have_subframe & 7) == 7) &&
				(g_gps_ephemeris[ch].ura < 8) &&
				(g_gps_ephemeris[ch].iode2 == g_gps_ephemeris[ch].iode1) &&
				((g_gps_ephemeris[ch].iodc & 0xff) == g_gps_ephemeris[ch].iode1)) {
				// Save g_gps_ephemeris[ch] to m_gps_ephetable[sv]
				sv = g_gps_ephemeris[ch].prn-1;
#ifdef GNSS_ENABLE_MUTEX
				OSMutexPend(m_EphTabMutex, 0, &err);
#endif
				m_gps_ephetable[sv] = g_gps_ephemeris[ch];
				m_gps_ephetable[sv].valid = 1;
#ifdef GNSS_ENABLE_MUTEX
				OSMutexPost(m_EphTabMutex);
#endif
				// Clear g_gps_ephemeris[ch], so as not to save the same results 
				// when the ephemeris is not updated.
				gps_clear_ephemeris(ch);
//					g_gps_ephemeris[ch].valid = 1;
			}
			else {
//					g_gps_ephemeris[ch].valid = 0;
			}
        }
    }
}