24、GPS L1电文处理实现

本文主要是介绍24、GPS L1电文处理实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
$\qquad$ 下面是HD-GR GNSS导航软件的GPS L1电文处理实现代码，入口函数gps_process_message (…)：
// gps_message.c -- GPS L1 Navigation message 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 : message.c* Mods     : driving LED part has commented/replaced for Namuru HW* version  : V1.1* date     : 8/7/2008*//* * HD-GR GNSS receiver project* Modes    : Inherited the code of message.c 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_accum_task.h"
#include "main_ephemeris.h"/******************************************************************************* Defines******************************************************************************/#define LINLIANG_HAMMING_PARITY_CHECK// gps_look_for_preamble is 0x8b, but it's located in the MSBits of a 30 bit word.
#define GPS_PREAMBLE        (0x8b << (30-8))/******************************************************************************* Globals******************************************************************************/// Right now we're declaring a message structure per channel, since the
// messages come out of locked channels.. but you could argue they should be
// in a per satellite array.
gps_message_t  m_gps_messages[GPS_MAX_CHANNELS] __attribute__ ((section(".isrdata.rwdata")));/******************************************************************************* Statics******************************************************************************/static void gps_look_for_preamble( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static int gps_parity_check( unsigned long word) __attribute__ ((section(".isrcode.txt")));
#ifndef LINLIANG_HAMMING_PARITY_CHECK
static int gps_parity( unsigned long word) __attribute__ ((section(".isrcode.txt")));
#endif // LINLIANG_HAMMING_PARITY_CHECKstatic void gps_store_bit( unsigned short ch, unsigned short bit) __attribute__ ((section(".isrcode.txt")));
static void gps_store_word( unsigned short ch) __attribute__ ((section(".isrcode.txt")));#ifdef LINLIANG_HAMMING_PARITY_CHECK/*************************************************Function: bool gps_parity_check(unsigned word)*Revision: 1.00*Last Update: 24/12/2009*Description:Hamming Parity Check*Input: 32bit unsigned:D29*,D30*,b1-b24,parities*Output: true,gps_parity check ok.*Author:linliang*E-Mail:hxmlinliang@yahoo.com.cn***********************************************/
static int gps_parity_check( unsigned long word)
{unsigned long b_1,b_2,b_3,b_4,b_5,b_6,b_7;unsigned long XorResult,gps_parity;// Parallel XOR can realize the verification process of GPS navigation data, // which can avoid redundant and OR operationb_1 = word & 0xFBFFBF00;b_2 = Rotation_left(word,1) & 0x07FFBF01;b_3 = Rotation_left(word,2) & 0xFC0F8100;b_4 = Rotation_left(word,3) & 0xF81FFE02;b_5 = Rotation_left(word,4) & 0xFC00000E;b_6 = Rotation_left(word,5) & 0x07F00001;b_7 = Rotation_left(word,6) & 0x00003000;XorResult = b_1 ^ b_2 ^ b_3 ^ b_4 ^ b_5 ^ b_6 ^ b_7;// Shift XorResult into 5 blocks, 6 bits each time, then perform XOR to get the // final resultgps_parity = XorResult ^ Rotation_left(XorResult,6) ^ Rotation_left(XorResult,12)^ Rotation_left(XorResult,18) ^ Rotation_left(XorResult,24);gps_parity = gps_parity & 0x3F;return ((word&0x3F) == gps_parity);
}#else// GPS gps_parity bit-vectors
// The last 6 bits of a 30bit GPS word are gps_parity check bits.
// Each gps_parity bit is computed from the XOR of a selection of bits from the
// 1st 24 bits of the current GPS word, and the last 2 bits of the _previous_
// GPS word.
// These gps_parity bit-vectors are used to select which message bits will be used
// for computing each of the 6 gps_parity check bits.
// We assume the two bits from the previous message (b29, b30) and the 24 bits
// from the current message, are packed into a 32bit word in this order:
//   < b29, b30, b1, b2, b3, ... b23, b24, X, X, X, X, X, X > (X = don't care)
// Example: if PBn = 0x40000080,
// The gps_parity check bit "n" would be computed from the expression (b30 XOR b23).
#define PB1     0xbb1f3480
#define PB2     0x5d8f9a40
#define PB3     0xaec7cd00
#define PB4     0x5763e680
#define PB5     0x6bb1f340
#define PB6     0x8b7a89c0/******************************************************************************* Count the number of bits set in the input and return (1) if this count is* odd (0) otherwise.* This is used in the navigation message gps_parity check algorithm.** Note, on the ARM there is probably a more efficient way to do this.******************************************************************************/
static int gps_parity( unsigned long word)
{int count = 0;while (word) {if ((long)word < 0) count++;word <<= 1; // Want to go this way because we typically ignore some LSBits}return (count & 1);
}/******************************************************************************* Return 1 if and only if input 30bit word passes GPS gps_parity check, otherwise* return 0.** The input word is expected to be 32bits, with the 30bit word right justified.* The two most significant bits (b30 and b31) should contain the last two bits* of the _previous_ GPS word.******************************************************************************/
static int gps_parity_check( unsigned long word)
{return (!((word & 0x3f) ^ // Last 6 bits are the message gps_parity bits((gps_parity( word & PB1) << 5) | (gps_parity( word & PB2) << 4) |(gps_parity( word & PB3) << 3) | (gps_parity( word & PB4) << 2) |(gps_parity( word & PB5) << 1) |  gps_parity( word & PB6))));
}#endif // LINLIANG_HAMMING_PARITY_CHECK/******************************************************************************* New satellite in a channel; clear the message. For now that means just* clearing the valid flags (and the current subframe for display purposes)*****************************************************************************/
void gps_clear_messages(unsigned short ch)
{unsigned short i;m_gps_messages[ch].frame_sync = 0;m_gps_messages[ch].subframe = 0;m_gps_messages[ch].set_epoch_flag = 0;for (i = 0; i < 5; ++i)m_gps_messages[ch].subframes[i].valid = 0;
}/******************************************************************************* Load bits into wordbuf0 and wordbuf1. Flip the incoming bit if we're sync'd* onto a subframe and the bits are inverted.** Note, see coments about weird 2+ 30 bit pattern below in the words below.*****************************************************************************/
static void gps_store_bit( unsigned short ch, unsigned short bit)
{// If we're synced on the current message and the data is inverted,// flip the incoming bit.if (m_gps_messages[ch].frame_sync && m_gps_messages[ch].data_inverted)bit ^= 1;// GPS NAV messages come MSBit 1st, so the most recent bit is the LSBit.m_gps_messages[ch].wordbuf0 = (m_gps_messages[ch].wordbuf0 << 1) | bit;// NAV words are 30 bits long and the gps_parity check requires the upper// two bits to be the least two bits of the previous word. Note that we// use wordbuf1 to look for the preamble (TLM and HOW at the same time)if( m_gps_messages[ch].wordbuf0 & (1 << 30))m_gps_messages[ch].wordbuf1 = (m_gps_messages[ch].wordbuf1 << 1) | 1;elsem_gps_messages[ch].wordbuf1 = (m_gps_messages[ch].wordbuf1 << 1);
}/******************************************************************************* Take the message's buffer of 2 + 30 bits (2 from the previous word) and* store it in the subframe's word as 24 bits of data after completing a gps_parity* check.******************************************************************************/
static void gps_store_word( unsigned short ch)
{// If bit 30 is set then flip bits 29 - 6 as per GPS spec.if (m_gps_messages[ch].wordbuf0  & (1 << 30))m_gps_messages[ch].wordbuf0 ^= 0x3fffffc0;if( gps_parity_check(m_gps_messages[ch].wordbuf0)) {// Store the word without the 6 partiy bits and the 2 upper bits// in the subframes arraym_gps_messages[ch].subframes[m_gps_messages[ch].subframe].word[m_gps_messages[ch].wordcount]= (m_gps_messages[ch].wordbuf0 >> 6) & 0x00ffffff;// Mark it as validm_gps_messages[ch].subframes[m_gps_messages[ch].subframe].valid|= (1 << m_gps_messages[ch].wordcount);}
}/******************************************************************************** This function finds the preamble, TLM and HOW in the navigation message and* synchronizes to the nav message.
*******************************************************************************/
static void gps_look_for_preamble( unsigned short ch)
{// TLeMetry, HandOffWordunsigned long   TLM, HOW;unsigned short  current_subframe;unsigned short  previous_subframe;unsigned short  data_inverted;// Note: Bits are stored in wordbuf0/1 in store_bits()// Save local copies of the wordbuf's for local checks of TLM and HOWTLM = m_gps_messages[ch].wordbuf1;HOW = m_gps_messages[ch].wordbuf0;// Test for inverted data. Bit 0 and 1 of HOW must be zero.// Test for inverted dataif (HOW & 1) {TLM = ~TLM;HOW = ~HOW;data_inverted = 1;}else {data_inverted = 0;}// Flip bits 29 - 6 if the previous word's LSB is 1if (TLM  & (1 << 30))TLM ^= 0x3fffffc0;if (HOW  & (1 << 30))HOW ^= 0x3fffffc0;// Test for preambleif ((TLM & 0x3fc00000) != GPS_PREAMBLE)return;// Subframe IDcurrent_subframe = (int)((HOW >> 8) & 7);// subframe range testif ((current_subframe < 1) || (current_subframe > 5))return;// Confirm zerosif (HOW & 3)return;// The advantage of previous checks is saving time on false hits.// They do increase time on true hits though.if (!gps_parity_check(TLM))return;if (!gps_parity_check(HOW))return;// Hooray! We found a valid preamble and a sane HOW word, so for now// we'll assume we're synced. We won't really know until we get the next// subframe and check that the TOW has incremented by exactly 1.m_gps_messages[ch].frame_sync = 1;// Record the current subframe number (from zero)m_gps_messages[ch].subframe = --current_subframe;// Flag whether the bits are inverted, and if so invert wordbuf0 so we// don't lose the next incoming word.if (data_inverted) {m_gps_messages[ch].data_inverted = 1;m_gps_messages[ch].wordbuf0 = ~m_gps_messages[ch].wordbuf0;}else {m_gps_messages[ch].data_inverted = 0;}m_gps_messages[ch].subframes[current_subframe].word[0] = TLM;m_gps_messages[ch].subframes[current_subframe].word[1] = HOW;// We've just stored two words into the current subframem_gps_messages[ch].wordcount = 2;// Flag Words 0 and 1 as valid wordsm_gps_messages[ch].subframes[current_subframe].valid = 3;// Extract and store the TOW from the HOW so we can easily compare it to// future TOWs to verify our frame sync. (TOW == bits 1-17 of 30 bit word)// Maybe this should be a macro. Could be done faster if we assumed 32bits.m_gps_messages[ch].subframes[current_subframe].TOW =(HOW >> (30 - 17)) & ((1 << 17) - 1);previous_subframe = (current_subframe > 0) ? (current_subframe - 1):4;// Even if the previous subframe had valid TLM and HOW words, kill both the// current and previous subframes if their TOW's are not incrementally// different.if (m_gps_messages[ch].subframes[previous_subframe].valid & 3) {if (m_gps_messages[ch].subframes[current_subframe].TOW !=(m_gps_messages[ch].subframes[previous_subframe].TOW + 1)) {// We're not actually synced. Kill everything and start// the sync search over again.gps_clear_messages( ch);GNSS_ENTER_CRITICAL();// We must also go back to pull-in, as the sync process// is done there not in lock. If this is not done// the sync can get corrupted, leading to bad pseudoranges.// (pjm)m_GPS_CH[ch].backto_pull_in = 1;}else {// Now that we have a valid TLM/HOW, we know the actual time of week.// Note that the TOW in the HOW is actually the time at the start// of the next subframe.// Update the GNSS time in seconds (the receiver's main clock)set_time_with_tow( m_gps_messages[ch].subframes[current_subframe].TOW);// Given that we know the current bit// is the last bit of the HOW word (the 60th bit of the subframe), we// can calculate the gps time in bit-counts (1/50 seconds).// Note, time_in_bits is incremented in the tracking.c lock() function.if (m_gps_messages[ch].subframes[current_subframe].TOW) {HOW = m_gps_messages[ch].subframes[current_subframe].TOW * 300 - 240;}// The TOW can be zero so handle this case.else {HOW = BITS_IN_WEEK - 240;}GNSS_ENTER_CRITICAL();// Update the gps "time_in_bits"m_GPS_CH[ch].time_in_bits = HOW;if (!m_gps_messages[ch].set_epoch_flag) {// Finally, flag the tracking loop that the next bit (20ms epoch)// is the beginning of a new word. This will reset the epoch counter// to 0 every 30 bits to track words... but ONLY once.m_GPS_CH[ch].sync_20ms_epoch_count = 1;m_gps_messages[ch].set_epoch_flag = 1;}}}// Hand off the current satellite to the message structurem_gps_messages[ch].prn = m_GPS_CH[ch].prn;GNSS_EXIT_CRITICAL();
}void gps_process_message(OS_FLAGS channels_with_bits, OS_FLAGS channel_bits)
{INT8U err;INT32U pbit;unsigned short ch;OS_FLAGS which_subframe;for (ch = 0, pbit = 1; ch < GPS_MAX_CHANNELS; ch++, pbit <<= 1) {if (channels_with_bits & pbit) {// This channel has a bit to process: store the bit into// wordbuf0 and wordbuf1gps_store_bit( ch, (channel_bits & pbit) ? 1:0);// If the channel isn't sync'd to the message frame,// look for the preambleif (!m_gps_messages[ch].frame_sync) {gps_look_for_preamble(ch);// If we just found sync, then reset the countersif( m_gps_messages[ch].frame_sync) {m_gps_messages[ch].bitcount = 0;}}// Frame is sync'd, so get bits and words.else {// If we have 30 bits, that's a word so store itm_gps_messages[ch].bitcount++;if (m_gps_messages[ch].bitcount >= 30) {m_gps_messages[ch].bitcount = 0;// Store the word in the subframes arraygps_store_word( ch);m_gps_messages[ch].wordcount++;if (m_gps_messages[ch].wordcount >= 10) {// We've got 10 words so we have a subframe. Use// frame_sync to restart the subframe parsing.// Note that preamble will reset wordcount.m_gps_messages[ch].frame_sync = 0;// we assume in the preamble that the bit stream// hasn't been inverted when we check the TLM/HOW.// so if the channel IS inverted, make it non-// inverted as we check the TLM/HOW.if( m_gps_messages[ch].data_inverted)m_gps_messages[ch].wordbuf0 = ~m_gps_messages[ch].wordbuf0;// Only send along complete, error free subframesif (m_gps_messages[ch].subframes[m_gps_messages[ch].subframe].valid == 0x3ff) {
//                          unsigned short sv = m_gps_messages[ch].prn-1;
//                      	if (!m_ephetable[sv].valid) {// Set the flag to wake up the ephemeris threadchannels_with_subframes |= (1 << ch);// Set the subframe flag so we know which// subframe to process. We don't need a// shadow register here because we're only// going to set one subframe per channel// at a time.which_subframe = (1 << m_gps_messages[ch].subframe);OSFlagPost(m_EphemerisSubframeFlags[ch], which_subframe, OS_FLAG_SET, &err);//                      	}}}}}}}
}