26、BDS B1I电文处理实现

本文主要是介绍26、BDS B1I电文处理实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
$\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-order0x0000, 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 correctionunsigned 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-encoderunsigned 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 checkif ((D1 & 0xf) == (sw1 & 0xf) && (D2 & 0xf) == (sw2 & 0xf)) {// Store the word in the subframes arraym_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 validm_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 checkif ((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 TLMTLM = m_b1i_messages[ch].wordbuf1;SOW = m_b1i_messages[ch].wordbuf0;// Test for preambleif ((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 IDcurrent_subframe = (int)((TLM >> 12) & 7);// subframe range testif ((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 subframem_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 wordsm_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 structurem_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;elsem_b1i_messages[ch].wordbuf1 = (m_b1i_messages[ch].wordbuf1 << 1);b1i_look_for_preamble(ch);// If we just found sync, then reset the countersif( 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 preambleif (!m_b1i_messages[ch].frame_sync) {
#ifndef SYNC_PROCESS_FRAME_SYNCb1i_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 bitb1i_store_bit( ch, (channel_bits & pbit) ? 1:0);// If we have 30 bits, that's a word so store itm_b1i_messages[ch].bitcount++;if (m_b1i_messages[ch].bitcount >= 30) {// Store the word in the subframes arrayb1i_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 subframesif (m_b1i_messages[ch].subframes[m_b1i_messages[ch].subframe].valid == 0x3ff) {// 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_b1i_messages[ch].subframe);OSFlagPost(m_EphemerisSubframeFlags[GPS_MAX_CHANNELS+ch], which_subframe, OS_FLAG_SET, &err);}}}}}}
}