\qquad 下面是HD-GR GNSS导航软件的BDS B1I电文处理实现代码，入口函数b1i _process_message (…)：
// b1i_message.c -- BDS B1I 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 <string.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 "b1i_message.h"
#include "b1i_accum_task.h"
#include "main_ephemeris.h"
/******************************************************************************
* Defines
******************************************************************************/
#define B1I_PREAMBLE (0x712 << (30-11)) // b1i_look_for_preamble is 0x712, but it's
// located in the MSBits of a 30 bit word.
// ROM table list for BDS error correction
unsigned short m_BCH1511_ErrCorrTable[] __attribute__ ((section(".isrdata.rwdata"))) = {
0x0000, 0x0001, 0x0002, 0x0010, 0x0004, 0x0100, 0x0020, 0x0400,
0x0008, 0x4000, 0x0200, 0x0080, 0x0040, 0x2000, 0x0800, 0x1000};
/*
unsigned short m_BCH1511_ErrCorrTable[] = { // reverse bit-order
0x0000, 0x4000, 0x2000, 0x0400, 0x1000, 0x0040, 0x0200, 0x0010,
0x0800, 0x0001, 0x0020, 0x0080, 0x0100, 0x0002, 0x0008, 0x0004};
*/
/******************************************************************************
* 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.
b1i_message_t m_b1i_messages[B1I_MAX_CHANNELS] __attribute__ ((section(".isrdata.rwdata")));
/******************************************************************************
* Statics
******************************************************************************/
static void b1i_look_for_preamble( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static int DecodeBCH1511(unsigned short *pus) __attribute__ ((section(".isrcode.txt")));
static int DecodeBCH3022(unsigned long *pul) __attribute__ ((section(".isrcode.txt")));
static void b1i_store_bit( unsigned short ch, unsigned short bit) __attribute__ ((section(".isrcode.txt")));
static void b1i_store_word( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
/******************************************************************************
* 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 b1i_clear_messages(unsigned short ch)
{
unsigned short i;
m_b1i_messages[ch].frame_sync = 0;
m_b1i_messages[ch].subframe = 0;
m_b1i_messages[ch].set_epoch_flag = 0;
m_b1i_messages[ch].wordbuf0 = 0;
m_b1i_messages[ch].wordbuf1 = 0;
for (i = 0; i < 5; ++i)
m_b1i_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 b1i_store_bit( unsigned short ch, unsigned short bit)
{
// If the data is inverted, flip the incoming bit.
if (m_b1i_messages[ch].data_inverted) {
bit ^= 1;
}
unsigned short G1;
unsigned short idx = m_b1i_messages[ch].bitcount / 2;
bchdc_t *pbch_dc = (m_b1i_messages[ch].bitcount & 1) ?
(&m_b1i_messages[ch].bch_dc2):(&m_b1i_messages[ch].bch_dc1);
// decoder:
// G1 = D[3];
// D[3] = D[2];
// D[2] = D[1];
// D[1] = D[0] ^ G1;
// D[0] = bit ^ G1;
G1 = pbch_dc->D & 8;
pbch_dc->D <<= 1;
pbch_dc->D |= bit;
if (G1) {
pbch_dc->D ^= 3;
}
pbch_dc->swd |= bit;
if (idx<14) {
pbch_dc->swd <<= 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 parity
* check.
*
*****************************************************************************/
static void b1i_store_word( unsigned short ch)
{
unsigned short sf = m_b1i_messages[ch].subframe;
unsigned short wc = m_b1i_messages[ch].wordcount;
// error correction
unsigned short sw1 = m_b1i_messages[ch].bch_dc1.swd ^ m_BCH1511_ErrCorrTable[m_b1i_messages[ch].bch_dc1.D & 0xf];
unsigned short sw2 = m_b1i_messages[ch].bch_dc2.swd ^ m_BCH1511_ErrCorrTable[m_b1i_messages[ch].bch_dc2.D & 0xf];
// re-encoder
unsigned short i, bit;
unsigned short G1, D1=0, D2=0;
unsigned short us1 = sw1;
unsigned short us2 = sw2;
for (i=0; i<11; i++, us1<<=1, us2<<=1) {
bit = (us1 & 0x4000) ? 1:0;
G1 = ((D1 & 8) ? 1:0) ^ bit;
D1 <<= 1;
if (G1) {
D1 ^= 3;
}
bit = (us2 & 0x4000) ? 1:0;
G1 = ((D2 & 8) ? 1:0) ^ bit;
D2 <<= 1;
if (G1) {
D2 ^= 3;
}
}
// parity check
if ((D1 & 0xf) == (sw1 & 0xf) && (D2 & 0xf) == (sw2 & 0xf)) {
// Store the word in the subframes array
m_b1i_messages[ch].subframes[sf].word[wc] =
((unsigned long)(sw1 & 0xfff0) << 15)|
((unsigned long)(sw2 & 0xfff0) << 4) |
((unsigned long)(sw1 & 0x000f) << 4) |
((unsigned long)(sw2 & 0x000f));
// Mark it as valid
m_b1i_messages[ch].subframes[sf].valid |= (1 << wc);
}
}
static int DecodeBCH1511(unsigned short *pus)
{
unsigned short i, us, bit;
unsigned short G1, D=0, DEC=0;
// decoder:
// G1 = D[3];
// D[3] = D[2];
// D[2] = D[1];
// D[1] = D[0] ^ G1;
// D[0] = bit ^ G1;
us = *pus;
for (i=0; i<15; i++, us<<=1) {
bit = (us & 0x4000) ? 1:0;
G1 = (D & 8);
D <<= 1;
D |= bit;
if (G1) {
D ^= 3;
}
}
// error correction
*pus ^= m_BCH1511_ErrCorrTable[D & 0xf];
// re-encoder
// G1 = D[3]^bit;
// D[3] = D[2];
// D[2] = D[1];
// D[1] = D[0] ^ G1;
// D[0] = G1;
us = *pus;
for (i=0; i<11; i++, us<<=1) {
bit = (us & 0x4000) ? 1:0;
G1 = ((DEC & 8) ? 1:0) ^ bit;
DEC <<= 1;
if (G1) {
DEC ^= 3;
}
}
// parity check
if ((DEC & 0xf) == (*pus & 0xf)) {
return 1;
}
return 0;
}
static int DecodeBCH3022(unsigned long *pul)
{
unsigned long ul;
unsigned short i, us1, us2;
ul = *pul;
i = us1 = us2 = 0;
while (i < 15) {
if (ul & 0x20000000)
us1 |= 1;
ul <<= 1;
if (ul & 0x20000000)
us2 |= 1;
ul <<= 1;
i ++;
if (i < 15) {
us1 <<= 1;
us2 <<= 1;
}
}
if (DecodeBCH1511(&us1) && DecodeBCH1511(&us2)) {
*pul = ((unsigned long)(us1 & 0xfff0) << 15)|
((unsigned long)(us2 & 0xfff0) << 4) |
((unsigned long)(us1 & 0x000f) << 4) |
((unsigned long)(us2 & 0x000f));
return 1;
}
return 0;
}
/*******************************************************************************
* This function finds the preamble, TLM and HOW in the navigation message and
* synchronizes to the nav message.
*******************************************************************************/
static void b1i_look_for_preamble( unsigned short ch)
{
unsigned long TLM, SOW;
unsigned short current_subframe;
unsigned short previous_subframe;
unsigned short data_inverted, us;
// Note: Bits are stored in wordbuf0/1 in store_bits()
// Save local copies of the wordbuf's for local checks of TLM
TLM = m_b1i_messages[ch].wordbuf1;
SOW = m_b1i_messages[ch].wordbuf0;
// Test for preamble
if ((TLM & 0x3ff80000) == B1I_PREAMBLE) {
data_inverted = 0;
}
else {
TLM = ~TLM;
if ((TLM & 0x3ff80000) == B1I_PREAMBLE) {
SOW = ~SOW;
data_inverted = 1;
}
else {
return;
}
}
us = TLM & 0x7fff;
if (!DecodeBCH1511(&us)) {
return;
}
TLM = (TLM & 0xffff8000) | us;
if (!DecodeBCH3022(&SOW)) {
return;
}
// Subframe ID
current_subframe = (int)((TLM >> 12) & 7);
// subframe range test
if ((current_subframe < 1) || (current_subframe > 5)) {
return;
}
// Hooray! We found a valid preamble and a SOW 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 SOW has incremented by exactly 6.
m_b1i_messages[ch].frame_sync = 1;
// Record the current subframe number (from zero)
m_b1i_messages[ch].subframe = --current_subframe;
// Flag whether the bits are inverted.
m_b1i_messages[ch].data_inverted = data_inverted;
// We've just stored two words into the current subframe
m_b1i_messages[ch].subframes[current_subframe].word[0] = TLM;
m_b1i_messages[ch].subframes[current_subframe].word[1] = SOW;
m_b1i_messages[ch].wordcount = 2;
// Flag Words 0 and 1 as valid words
m_b1i_messages[ch].subframes[current_subframe].valid = 3;
// Extract and store the SOW from the TPW and SOW so we can easily compare
// it to future SOWs to verify our frame sync.
m_b1i_messages[ch].subframes[current_subframe].SOW =
((TLM & 0xff0) << 8) | ((SOW >> 18) & 0xfff);
previous_subframe = (current_subframe > 0) ? (current_subframe - 1):4;
// Even if the previous subframe had valid TLM and SOW words, kill both the
// current and previous subframes if their SOW's are not incrementally
// different.
if (m_b1i_messages[ch].subframes[previous_subframe].valid & 3) {
unsigned long time_in_ms;
unsigned short IsInvalid = 0;
unsigned short D1Msg = IS_D1_NAVMESSAGE(m_b1i_messages[ch].prn);
if (D1Msg) {
if ((m_b1i_messages[ch].subframes[current_subframe].SOW) !=
(m_b1i_messages[ch].subframes[previous_subframe].SOW + 6)) {
IsInvalid = 1;
}
}
else {
if (current_subframe == 0) {
if ((m_b1i_messages[ch].subframes[current_subframe].SOW) !=
(m_b1i_messages[ch].subframes[previous_subframe].SOW + 3)) {
IsInvalid = 1;
}
}
else {
if ((m_b1i_messages[ch].subframes[current_subframe].SOW) !=
(m_b1i_messages[ch].subframes[previous_subframe].SOW)) {
IsInvalid = 1;
}
}
}
if (IsInvalid) {
// We're not actually synced. Kill everything and start
// the sync search over again.
b1i_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.
m_B1I_CH[ch].backto_pull_in = 1;
}
else {
// Now that we have a valid TLM/SOW, we know the actual time of week.
time_in_ms = m_b1i_messages[ch].subframes[current_subframe].SOW * 1000;
if (D1Msg) {
// Note that the SOW in the SOW word is actually the time at the start
// of the subframe.
// Update the gps "time_in_ms". Given that we know the current bit
// is the last bit of the SOW word (the 60th bit of the subframe), we
// can calculate the gps time in ms. Note, time_in_bits is incremented
// in the tracking.c lock() function.
time_in_ms += 1200;
}
else {
time_in_ms += current_subframe * 600 + 120;
}
// Update the GNSS time in seconds (the receiver's main clock).
set_time_with_sow(time_in_ms/1000.0);
time_in_ms /= 20;
if (time_in_ms >= BITS_IN_WEEK_50HZ)
time_in_ms -= BITS_IN_WEEK_50HZ;
GNSS_ENTER_CRITICAL();
m_B1I_CH[ch].time_in_bits = time_in_ms;
m_B1I_CH[ch].ms_count_20 = 19;
if (!m_b1i_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_B1I_CH[ch].sync_20ms_epoch_count = 50 + (time_in_ms % 50);
m_b1i_messages[ch].set_epoch_flag = 1;
}
}
}
// Hand off the current satellite to the message structure
m_b1i_messages[ch].prn = m_B1I_CH[ch].prn;
GNSS_EXIT_CRITICAL();
}
void b1i_sync_frame(unsigned short ch, unsigned short bit)
{
// NAV messages come MSBit 1st, so the most recent bit is the LSBit.
m_b1i_messages[ch].wordbuf0 = (m_b1i_messages[ch].wordbuf0 << 1) | bit;
// NAV words are 30 bits long. Note that we use wordbuf1 to look for the
// preamble (TLM and SOW at the same time)
if( m_b1i_messages[ch].wordbuf0 & (1 << 30))
m_b1i_messages[ch].wordbuf1 = (m_b1i_messages[ch].wordbuf1 << 1) | 1;
else
m_b1i_messages[ch].wordbuf1 = (m_b1i_messages[ch].wordbuf1 << 1);
b1i_look_for_preamble(ch);
// If we just found sync, then reset the counters
if( m_b1i_messages[ch].frame_sync) {
m_b1i_messages[ch].bitcount = 0;
memset(&m_b1i_messages[ch].bch_dc1, 0, sizeof(bchdc_t));
memset(&m_b1i_messages[ch].bch_dc2, 0, sizeof(bchdc_t));
}
}
void b1i_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 < B1I_MAX_CHANNELS; ch++, pbit <<= 1) {
if (channels_with_bits & pbit) {
// If the channel isn't sync'd to the message frame,
// store the bit and look for the preamble
if (!m_b1i_messages[ch].frame_sync) {
#ifndef SYNC_PROCESS_FRAME_SYNC
b1i_sync_frame( ch, (channel_bits & pbit) ? 1:0);
#endif // SYNC_PROCESS_FRAME_SYNC
}
// Frame is sync'd, so get bits and words.
else {
// Store the bit
b1i_store_bit( ch, (channel_bits & pbit) ? 1:0);
// If we have 30 bits, that's a word so store it
m_b1i_messages[ch].bitcount++;
if (m_b1i_messages[ch].bitcount >= 30) {
// Store the word in the subframes array
b1i_store_word( ch);
m_b1i_messages[ch].bitcount = 0;
memset(&m_b1i_messages[ch].bch_dc1, 0, sizeof(bchdc_t));
memset(&m_b1i_messages[ch].bch_dc2, 0, sizeof(bchdc_t));
m_b1i_messages[ch].wordcount++;
if (m_b1i_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_b1i_messages[ch].frame_sync = 0;
m_b1i_messages[ch].wordbuf0 = 0;
m_b1i_messages[ch].wordbuf1 = 0;
// Only send along complete, error free subframes
if (m_b1i_messages[ch].subframes[m_b1i_messages[ch].subframe].valid == 0x3ff) {
// 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_b1i_messages[ch].subframe);
OSFlagPost(m_EphemerisSubframeFlags[GPS_MAX_CHANNELS+ch], which_subframe, OS_FLAG_SET, &err);
}
}
}
}
}
}
}