\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_CHECK
static 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:[email protected]
***********************************************/
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 operation
b_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 result
gps_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;
else
m_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 array
m_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 valid
m_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, HandOffWord
unsigned 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 HOW
TLM = 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 data
if (HOW & 1) {
TLM = ~TLM;
HOW = ~HOW;
data_inverted = 1;
}
else {
data_inverted = 0;
}
// Flip bits 29 - 6 if the previous word's LSB is 1
if (TLM & (1 << 30))
TLM ^= 0x3fffffc0;
if (HOW & (1 << 30))
HOW ^= 0x3fffffc0;
// Test for preamble
if ((TLM & 0x3fc00000) != GPS_PREAMBLE)
return;
// Subframe ID
current_subframe = (int)((HOW >> 8) & 7);
// subframe range test
if ((current_subframe < 1) || (current_subframe > 5))
return;
// Confirm zeros
if (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 subframe
m_gps_messages[ch].wordcount = 2;
// Flag Words 0 and 1 as valid words
m_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 structure
m_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 wordbuf1
gps_store_bit( ch, (channel_bits & pbit) ? 1:0);
// If the channel isn't sync'd to the message frame,
// look for the preamble
if (!m_gps_messages[ch].frame_sync) {
gps_look_for_preamble(ch);
// If we just found sync, then reset the counters
if( 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 it
m_gps_messages[ch].bitcount++;
if (m_gps_messages[ch].bitcount >= 30) {
m_gps_messages[ch].bitcount = 0;
// Store the word in the subframes array
gps_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 subframes
if (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 thread
channels_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);
// }
}
}
}
}
}
}
}