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 
#include 
#include 
#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_MUTEXOSMutexPend(m_EphTabMutex, 0, &err);
#endifm_gps_ephetable[sv].valid = 0;
#ifdef GNSS_ENABLE_MUTEXOSMutexPost(m_EphTabMutex);
#endifreturn;}// Grab the PRN for good measureg_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<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 1g_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 appropriateg_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 2g_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 appropriateg_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 appropriatetemp = 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 3g_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 processif (subframes & (1 << 0)) {gps_process_subframe1(ch);}if (subframes & (1 << 1)) {gps_process_subframe2(ch);}if (subframes & (1 << 2)) {gps_process_subframe3(ch);}// Almanac decodingif (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_MUTEXOSMutexPend(m_EphTabMutex, 0, &err);
#endifm_gps_ephetable[sv] = g_gps_ephemeris[ch];m_gps_ephetable[sv].valid = 1;
#ifdef GNSS_ENABLE_MUTEXOSMutexPost(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;}}}
}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！