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);}}}}}}
}

我的新浪博客账号
我的存档免费软件
我的存档学习资料

26、BDS B1I电文处理实现相关推荐

4.1-4.2 HD-GR导航软件概述和软件任务结构介绍
\qquad本文首先概述HD-GR导航软件的基本功能.开源传承.运行开发环境.并发任务目标.等等,然后比较全面地介绍HD-GR导航软件的任务结构和处理逻辑. 4.1 概述 \qquadHD-GR导航软 ...
halocn标定找旋转中心_王嘉琛：BDS接收机天线相位中心标定
BDS接收机天线相位中心标定王嘉琛1,2, 刘根友1, 郭爱智1, 肖恭伟1,2, 王彬彬1,2, 高铭1,2, 王生亮1,2 1. 中国科学院测量与地球物理研究所大地测量与地球动力学国家重点实验室 ...
4.5 星历（历书）解码
\qquad在HD-GR导航软件中,负责卫星导航电文处理和星历(历书)解码的分别是电文处理任务(message_task)和星历处理任务(ephemeris_task),它们各自的核心目标是:对于每个 ...
关于 GNSS 系统的信号频点频率 + 可用卫星
一.频点+卫星 1.各大系统的常用信号频率 GPS 系统频段名 band 频率/MHZ 波长/cm L1 1 1575.42 19.04 L2 2 1227.60 24.44 L5 5 1176.45 ...
截止8月10日！2022年芜湖市核心技术攻关项目“揭榜挂帅”榜单申报奖补流程
2022年芜湖市核心技术攻关项目揭榜挂帅榜单申报奖补流程指南,如果芜湖市的朋友们想要申请市核心技术攻关项目揭榜挂帅榜单的话,可以随时找作者昵称了解. 一.芜湖市核心技术攻关项目揭榜挂榜单发布面向全市 ...
c语言RTK算法,C-RTK 9P定位系统
C-RTK 9P厘米级定位模块概述高精度定位: 很多无人机应用需要高于传统GNSS提供的精度,例如,无人机绘图.农业测量.高精度起降等应用. C-RTK 9P是由CUAV推出的一款专业级RTK定位 ...
OBU与千寻魔方的定位性能分析
OBU的定位定位与同步方式 BeiDou/GPS/GLONASS/Galileo 高精度定位 + IMU PP1S+TOD(外同步接口) 千寻魔方的定位官方网站:https://www.q ...
高精度定位的几种解决方案
高精度定位解决途径常见的高精度定位解决途径有三种:RTK.SBAS."中国精度"星基增强服务. 1. RTK 载波相位差分技术又称RTK(Real Time Kinematic) ...
室外定位：高精度北斗RTK定位技术
北斗RTK定位技术,也称北斗差分定位技术,利用我国自主研发的北斗卫星定位系统实现精确定位功能.定位精度可根据需要,通过选择不同精度的人员定位终端来实现. 在科技强国的战略驱动下,北斗RTK定位技术迎来 ...

26、BDS B1I电文处理实现

26、BDS B1I电文处理实现相关推荐

最新文章

热门文章