\qquad 下面是HD-GR GNSS导航软件的观测量读取任务实现代码，入口函数meas_task(…)
// main_measure.c -- Grab measurements each TIC interrupt (~100ms).
/*
* 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 : measure.c
* Modes : Some code has been modified for adaption to the Namuru HW by Peter Mumford
* version : V1.0
* date : 21st/Dec/2006
*/
/*
* HD-GR GNSS receiver project
* Modes : Inherited the code of measure.c in the Namuru GPS receiver project V1.0
* and made necessary adjustments to adapt to the new HW, RTOS and functions.
* version : V1.0
* date : xx/xx/2015
*/
#include <io.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "includes.h"
#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"
#include "sys/alt_irq.h"
#include "main_measure.h"
#include "gps_accum_task.h"
#include "b1i_accum_task.h"
#include "gps_message.h"
#include "b1i_message.h"
#include "main_pseudorange.h"
#include "main_position.h"
/******************************************************************************
* Globals
******************************************************************************/
OS_EVENT *m_SemMeas;
measurement_t m_raw_meas;
OS_FLAGS channels_meas_ready;
void initialize_measurements( void)
{
unsigned short ch;
for (ch = 0; ch < TOT_MAX_CHANNELS; ch++) {
m_raw_meas.chan_meas[ch].valid = 0;
}
}
void grab_bit_times( void)
{
unsigned short ch = 0, ch1 = 0;
GNSS_ENTER_CRITICAL();
for (; ch < GPS_MAX_CHANNELS; ch++) {
m_raw_meas.chan_meas[ch].sync_bit_time = m_GPS_CH[ch].time_in_bits;
}
for (; ch < TOT_MAX_CHANNELS; ch++, ch1++) {
m_raw_meas.chan_meas[ch].sync_bit_time = m_B1I_CH[ch1].time_in_bits;
}
GNSS_EXIT_CRITICAL();
}
void fill_ch_pars( void)
{
unsigned short ch, ch1;
unsigned short ch_index;
if (m_sys_posconst & POS_CONSTELL_GPS) {
// If the channel is 1) locked and 2) the signal is good and 3) the
// clock is set vaguely correctly and 4) we've set the epoch counter
// for this channel, THEN grab the measurement data.
ch_index = GPS_CH00_BASE;
GNSS_ENTER_CRITICAL();
for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
if ((m_GPS_CH[ch].state == CHANNEL_LOCK) &&
(m_GPS_CH[ch].p_mag > LOCK_THRESHOLD) &&
(m_gps_messages[ch].set_epoch_flag) &&
(m_GPS_CH[ch].sync_20ms_epoch_count == 0)) {
// Note: meas_bit_time set in grab_bit_times() above.
m_raw_meas.chan_meas[ch].meas_bit_time = m_raw_meas.chan_meas[ch].sync_bit_time;
m_raw_meas.chan_meas[ch].doppler = m_GPS_CH[ch].carrier_freq;
m_raw_meas.chan_meas[ch].prn = m_GPS_CH[ch].prn;
m_raw_meas.chan_meas[ch].power = m_GPS_CH[ch].p_mag;
m_raw_meas.chan_meas[ch].valid = 1;
// Can't use this measurement
else {
m_raw_meas.chan_meas[ch].valid = 0;
}
// increment channel index to top of next channel map
ch_index += CH_BASE_STEP;
}
GNSS_EXIT_CRITICAL();
}
if (m_sys_posconst & POS_CONSTELL_BDS) {
// If the channel is 1) locked and 2) the signal is good and 3) the
// clock is set vaguely correctly and 4) we've set the epoch counter
// for this channel, THEN grab the measurement data.
ch_index = B1I_CH00_BASE;
GNSS_ENTER_CRITICAL();
for (ch1 = GPS_MAX_CHANNELS, ch = 0; ch1 < TOT_MAX_CHANNELS; ch1++, ch++) {
if ((m_B1I_CH[ch].state == CHANNEL_LOCK) &&
(m_B1I_CH[ch].p_mag > LOCK_THRESHOLD) &&
(m_b1i_messages[ch].set_epoch_flag) &&
(m_B1I_CH[ch].sync_20ms_epoch_count == 0)) {
// Note: meas_bit_time set in grab_bit_times() above.
m_raw_meas.chan_meas[ch1].meas_bit_time = m_raw_meas.chan_meas[ch1].sync_bit_time;
m_raw_meas.chan_meas[ch1].doppler = m_B1I_CH[ch].carrier_freq;
m_raw_meas.chan_meas[ch1].prn = m_B1I_CH[ch].prn;
m_raw_meas.chan_meas[ch1].power = m_B1I_CH[ch].p_mag;
m_raw_meas.chan_meas[ch1].valid = 1;
}
else {
m_raw_meas.chan_meas[ch1].valid = 0; // Can't use this measurement
}
// increment channel index to top of next channel map
ch_index += CH_BASE_STEP;
}
GNSS_EXIT_CRITICAL();
}
}
void gps_meas_task()
{
unsigned short ch;
unsigned short ch_index;
unsigned short raw_epoch;
unsigned long long_temp;
unsigned short bit_count_remainder;
unsigned long bit_count_modded, bit_time, meas_bit_time;
// Grab the measurement data.
ch_index = GPS_CH00_BASE;
for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
if (m_raw_meas.chan_meas[ch].valid) {
// Grab the latched data from the correlators (in their
// numerical order -- go optimized compiler, go!)
long_temp = read_from_correlator( ch_index + CODE_MEASUREMENT );
m_raw_meas.chan_meas[ch].code_phase = (unsigned short)((long_temp >> 10) & 0x7FF); // bits [20:10]
m_raw_meas.chan_meas[ch].code_dco_phase = (unsigned short)(long_temp & 0x3FF); // bits [9:0]
#ifdef ENABLE_CARRIER_PHASE_MEASUREMENT
long_temp = read_from_correlator( ch_index + CARRIER_MEASUREMENT );
m_raw_meas.chan_meas[ch].carrier_cycles = long_temp >> 10; // bits [31:10]
m_raw_meas.chan_meas[ch].carrier_dco_phase = (unsigned short)(long_temp & 0x3FF); // bits [9:0]
#endif // ENABLE_CARRIER_PHASE_MEASUREMENT
raw_epoch = (unsigned short)(read_from_correlator( ch_index + EPOCH ));
m_raw_meas.chan_meas[ch].epoch_bits = (raw_epoch >> 5) & 0x3F; // bits [10:5]
m_raw_meas.chan_meas[ch].epoch_codes = raw_epoch & 0x1F; // bits [4:0]
// Fix up in case the epoch counter is on a different side of
// a 50 bit "word" from us.
bit_time = m_raw_meas.chan_meas[ch].meas_bit_time;
bit_count_remainder = bit_time % 50;
bit_count_modded = bit_time - bit_count_remainder;
if ((bit_count_remainder < 10) && (m_raw_meas.chan_meas[ch].epoch_bits > 40))
bit_count_modded -= 50;
if ((bit_count_remainder > 40) && (m_raw_meas.chan_meas[ch].epoch_bits < 10))
bit_count_modded += 50;
meas_bit_time = bit_count_modded + m_raw_meas.chan_meas[ch].epoch_bits;
m_raw_meas.chan_meas[ch].meas_bit_time = meas_bit_time;
// check meas_bit_time
if (labs(bit_time-meas_bit_time) >= 10) {
m_gps_messages[ch].set_epoch_flag = 0;
}
// Tell the pseudorange thread which measurements it can use.
// Note that we don't want to just call the pr thread from here
// because we're in a DSR and should get out ASAP
channels_meas_ready |= (1 << ch);
}
// increment channel index to top of next channel map
ch_index += CH_BASE_STEP;
}
}
void b1i_meas_task()
{
unsigned short ch, ch1;
unsigned short ch_index;
unsigned short raw_epoch;
unsigned long long_temp;
unsigned short bit_count_remainder;
unsigned long bit_count_modded, bit_time, meas_bit_time;
// Grab the measurement data.
ch_index = B1I_CH00_BASE;
for (ch1 = GPS_MAX_CHANNELS, ch = 0; ch1 < TOT_MAX_CHANNELS; ch1++, ch++) {
if (m_raw_meas.chan_meas[ch1].valid) {
// Grab the latched data from the correlators (in their
// numerical order -- go optimized compiler, go!)
long_temp = read_from_correlator( ch_index + CODE_MEASUREMENT );
m_raw_meas.chan_meas[ch1].code_phase = (short)(long_temp >> 11); // bits [22:11]
m_raw_meas.chan_meas[ch1].code_dco_phase = (short)(long_temp & 0x7FF); // bits [10:0]
#ifdef ENABLE_CARRIER_PHASE_MEASUREMENT
long_temp = read_from_correlator( ch_index + CARRIER_MEASUREMENT );
m_raw_meas.chan_meas[ch1].carrier_cycles = long_temp >> 10; // bits [31:10]
m_raw_meas.chan_meas[ch1].carrier_dco_phase = (short)(long_temp & 0x3FF); // bits [9:0]
#endif // ENABLE_CARRIER_PHASE_MEASUREMENT
raw_epoch = (short)(read_from_correlator( ch_index + EPOCH ));
m_raw_meas.chan_meas[ch1].epoch_bits = raw_epoch >> 5;
m_raw_meas.chan_meas[ch1].epoch_codes = raw_epoch & 0x1f;
// Fix up in case the epoch counter is on a different side of
// a 50 bit "word" from us.
bit_time = m_raw_meas.chan_meas[ch1].meas_bit_time;
bit_count_remainder = bit_time % 50;
bit_count_modded = bit_time - bit_count_remainder;
if ((bit_count_remainder < 10) && (m_raw_meas.chan_meas[ch1].epoch_bits > 40))
bit_count_modded -= 50;
if ((bit_count_remainder > 40) && (m_raw_meas.chan_meas[ch1].epoch_bits < 10))
bit_count_modded += 50;
meas_bit_time = bit_count_modded + m_raw_meas.chan_meas[ch1].epoch_bits;
m_raw_meas.chan_meas[ch1].meas_bit_time = meas_bit_time;
// check meas_bit_time
if (labs(bit_time-meas_bit_time) >= 10) {
m_b1i_messages[ch].set_epoch_flag = 0;
}
// Tell the pseudorange thread which measurements it can use.
// Note that we don't want to just call the pr thread from here
// because we're in a DSR and should get out ASAP
channels_meas_ready |= (1 << ch);
}
// increment channel index to top of next channel map
ch_index += CH_BASE_STEP;
}
}
void set_pps_org( double seconds, pvt_share_t* ppvt_share)
{
double ms;
long tic, delay;
clock_state_t clk_state = get_clock_state();
pps_state_t pps_state = get_pps_state();
static long pvt_id = 0;
static long pps_org_ofs = 0; // unit: ns
static int pps_tic_slew = 0; // unit: clock
static long pps_tic_add1 = VALUE_TIC_ADD1; // unit: clock
static double tm_ofs_start = 0; // unit: GPS second
switch (pps_state) {
// 用本地时钟校准TIC
case PPS_INIT_ED:
if (clk_state >= SF1_CLOCK) {
seconds -= (long)seconds;
tic = m_raw_meas.meas_tic - (long)(seconds*10.0 + 0.5);
if (tic < 0) tic += 10;
load_tic_count(tic);
set_pps_state(PPS_TIC_ED);
}
break;
// 用定位解算时间将TIC校准到1个ACCUM_INT准确性, 缺省约0.78ms
case PPS_TIC_ED:
if ((ppvt_share->valid) && // 定位解算有效
(ppvt_share->id > 1) && // 非第1次有效解算
(fabs(ppvt_share->bs) < 1.0E-5)) {// 时钟偏差小于10微秒
ms = seconds - (long)seconds; // 取小数秒
ms = seconds*10.; // 以0.1秒为单位
tic = (long)ms; // 得TIC计数值
ms -= tic; // 得1个TIC中的小数
ms *= TIC_2_ACCUM_DIV; // 以ACCUM_INT间隔为单位
delay = (long)ms; // 总ACCUM_INT计数
if (delay == 0) {
load_tic_count(tic % 10); // load tic count
}
else {
ms -= delay; // 得ACCUM_INT余数
delay *= (VALUE_ACCUM_INT+1); // 当前TIC时钟位置
delay = (VALUE_TIC_DIV+1) - delay; // 要延迟的时钟数
load_tic_count((++tic) % 10); // 将TIC计数加1
set_tic_delay(delay, 0, 0); // 延迟到TIC起点
}
// 保存剩余延迟值，单位: 纳秒
pps_org_ofs = (long)(ms*((1.0E+8)/TIC_2_ACCUM_DIV));
// 保存当前时间
tm_ofs_start = seconds;
set_pps_state(PPS_ORG_ING);
}
break;
// 精细校准TIC原点
case PPS_ORG_ING:
// 异常情况处理: 假设时钟漂移小于1微秒/s(1PPM)且允许在50s内完成
// 精细校准, 因此精细校准期间累积的时钟漂移值不应大于50微秒.
if (!ppvt_share->valid) {
seconds -= tm_ofs_start;
if (seconds < 0) seconds += SECONDS_IN_WEEK;
if (seconds > 50) set_pps_state(PPS_TIC_ED);
break;
}
// 延迟100微秒
if (pps_org_ofs >= 100000) {
// 100毫秒 = 1000*100微秒
delay = VALUE_TIC_ADD1/1000;
set_tic_delay(-delay, 0, 1);
pps_org_ofs -= 100000;
}
// 延迟剩余微秒
else if (pps_org_ofs >= 1000) {
// 取微秒数
tic = pps_org_ofs/1000;
delay = (tic*VALUE_TIC_ADD1)/100000;
set_tic_delay(-delay, 0, 1);
pps_org_ofs -= tic*1000;
}
// 延迟微秒小数
else if (pps_org_ofs > 0) {
// 以0.1秒为单位计算延迟值
ms = (double)pps_org_ofs*(1.0E-8);
delay = (long)(ms*(double)VALUE_TIC_ADD1);
set_tic_delay(-delay, 0, 1);
pps_org_ofs = 0;
}
// TIC原点校准结束
if (pps_org_ofs < 10) {
pvt_id = ppvt_share->id;
pps_org_ofs = 0;
pps_tic_slew = 0;
pps_tic_add1 = VALUE_TIC_ADD1;
set_pps_state(PPS_OFS_ING);
}
break;
// 校准TIC漂移
case PPS_OFS_ING:
// 异常情况处理: 假设时钟漂移小于1微秒/s(1PPM)且允许在最长50s内
// 完成漂移校准, 因此漂移校准期间积累的漂移值不应大于50微秒, 加上
// "精细校准TIC原点"可能传递过来的50微秒最大漂移，从而总漂移值不
// 应大于100微秒.
if (!ppvt_share->valid) {
seconds -= tm_ofs_start;
if (seconds < 0) seconds += SECONDS_IN_WEEK;
if (seconds > 50) set_pps_state(PPS_TIC_ED);
break;
}
// 自TIC原点校准结束后，至少完成了1次有效解算, 且时钟偏差小于10微秒
if ((ppvt_share->id < (pvt_id+1)) ||
(fabs(ppvt_share->bs) > 1.0E-5)) {
break;
}
// 计算时间差
ms = seconds - tm_ofs_start;
if (ms < 0) ms += SECONDS_IN_WEEK;
// 以约2秒为间隔调整延迟
if (ms >= 2) {
long slew, mod;
tic = (long)(ms*10.0 + 0.5); // 实际TIC次数
ms = (seconds-(long)seconds); // 取整秒尾数
ms *= (1.0E+4); // 以100微秒为单位
ms -= (long)(ms+0.5); // 取100微秒尾数
ms *= (double)pps_tic_add1/1000.; // 转换为时钟数
slew = (long)(ms*10.0/tic+0.5); // 计算1秒钟上的延迟量
mod = labs(slew) % 10; // 计算平均延迟后的余量
slew /= 10; // 计算平均延迟量
if (labs(slew) >= 128) { // 每个TIC上的延迟量太大
set_pps_state(PPS_TIC_ED); // 回到PPS_TIC_ED状态
} // 正常时这不应出现
delay = (long)(ms+0.5); // 截止上个TIC时间点的总延迟量
delay += 2*slew; // 附加在2个TIC时间区间中的延迟量
pps_tic_slew -= slew;
pps_tic_add1 = VALUE_TIC_ADD1 + pps_tic_slew;
set_tic_slew(-delay, pps_tic_slew, mod);
pvt_id = ppvt_share->id;
tm_ofs_start = seconds;
}
break;
case PPS_OFS_ED:
default:
break;
}
}
/******************************************************************************
* Grab the latched measurement data from the accumulators after a TIC.
******************************************************************************/
void meas_task(void* pdata)
{
INT8U err;
m_SemMeas = OSSemCreate(0);
while (1) {
OSSemPend(m_SemMeas, 0, &err);
// wake up when the ACCUM DSR detects a new TIC
if (get_clock_state() >= SF1_CLOCK) {
// Get the current GPS time to mark the time of measurement
m_raw_meas.meas_time = get_time();
m_raw_meas.meas_tic = (unsigned short)read_from_correlator(TIC_COUNT);
channels_meas_ready = 0;
if ((m_raw_meas.meas_tic % 2) == 0) {
b1i_meas_task();
channels_meas_ready <<= GPS_MAX_CHANNELS;
gps_meas_task();
}
// And finally, flag all the valid measurements to the pseudorange
// thread and if there are none, but there were last TIC, then
// explicitely clear the pseudoranges.
if (channels_meas_ready) {
calculate_pseudorange(channels_meas_ready);
}
// Finally, make sure we didn't miss any other posts to the measure
// semaphore- if we did, complain about it.
err = OSSemAccept(m_SemMeas);
if (err > 0) {
#ifdef ENABLE_TRACKING_LOG
printf("MEASURE THREAD: MISSED SEMAPHORE.\r\n");
#endif // ENABLE_TRACKING_LOG
do {
err = OSSemAccept(m_SemMeas);
} while(err > 0);
}
else {
pvt_share_t pvt_share;
// Transfer solution status and local clock bias to measure thread.
#ifdef GNSS_ENABLE_MUTEX
OSMutexPend(m_PrCbsMutex, 0, &err);
#endif
pvt_share = m_pvt_share;
#ifdef GNSS_ENABLE_MUTEX
OSMutexPost(m_PrCbsMutex);
#endif
set_pps_org(m_raw_meas.meas_time.seconds, &pvt_share);
}
}
}
}
1432
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
