40、伪距计算函数实现

最新推荐文章于 2023-01-16 20:56:07 发布

Turing_321

最新推荐文章于 2023-01-16 20:56:07 发布

阅读量467

点赞数 2

分类专栏： # 4、HD-GR导航软件文章标签： c语言 GNSS观测量

本文链接：https://blog.csdn.net/turing321_huaide/article/details/127286131

版权

4、HD-GR导航软件专栏收录该内容

39 篇文章 29 订阅

订阅专栏

$\qquad$ 下面是HD-GR GNSS导航软件的伪距计算函数实现代码，入口函数calculate_pseudorange(…)

// main_pseudorange.c -- Process measurements into pseudoranges.

/* 
 * 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 : pseudorange.c
 * Modes    : removed includes to gp4020 HW
 * version  : V1.0
 * date     : 21st/Dec/2006
 */

/* 
 * HD-GR GNSS receiver project
 * Modes    : Inherited the code of pseudorange.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  <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_pseudorange.h"
#include "main_measure.h"
#include "main_position.h"

/******************************************************************************
 * Globals
 ******************************************************************************/

pseudorange_t g_pr[2];
unsigned short g_pr_idx = 0;
pseudorange_t *g_ppr_cur = &g_pr[0];
pseudorange_t *m_ppr_pos = 0;

/******************************************************************************
 * Calculate them pseudoranges.
 ******************************************************************************/
void gps_calculate_pseudorange( unsigned short ch)
{
    // sat_time
	g_ppr_cur->chan_pr[ch].sat_time =
    		m_raw_meas.chan_meas[ch].meas_bit_time*0.02 + GPS_CODE_TIME *
    		( m_raw_meas.chan_meas[ch].epoch_codes +
    		 ( 1/(double)GPS_MAX_CODE_PHASE) *
    		 ( m_raw_meas.chan_meas[ch].code_phase +
    		   m_raw_meas.chan_meas[ch].code_dco_phase / (double)GPS_CODE_DCO_LENGTH)
    		 );

	// range
	g_ppr_cur->chan_pr[ch].range = (g_ppr_cur->pr_time.seconds - g_ppr_cur->chan_pr[ch].sat_time) * SPEED_OF_LIGHT;

	// Record the following information for debugging
	g_ppr_cur->chan_pr[ch].prn = m_raw_meas.chan_meas[ch].prn;
	g_ppr_cur->chan_pr[ch].power = m_raw_meas.chan_meas[ch].power;
}

/******************************************************************************
 * Calculate them pseudoranges.
 ******************************************************************************/
void b1i_calculate_pseudorange( unsigned short ch)
{
    // sat_time
	g_ppr_cur->chan_pr[ch].sat_time =
    		m_raw_meas.chan_meas[ch].meas_bit_time*0.02 + B1I_CODE_TIME *
    		(m_raw_meas.chan_meas[ch].epoch_codes +
    		 (1/(double)B1I_MAX_CODE_PHASE) *
    		 (m_raw_meas.chan_meas[ch].code_phase +
    		  m_raw_meas.chan_meas[ch].code_dco_phase / (double)B1I_CODE_DCO_LENGTH)
    		);

	// range
	g_ppr_cur->chan_pr[ch].range = (g_ppr_cur->pr_time.seconds - g_ppr_cur->chan_pr[ch].sat_time) * SPEED_OF_LIGHT;

	// Record the following information for debugging
	g_ppr_cur->chan_pr[ch].prn = m_raw_meas.chan_meas[ch].prn;
	g_ppr_cur->chan_pr[ch].power = m_raw_meas.chan_meas[ch].power;
}

/******************************************************************************
 * Wake up on valid measurements and produce pseudoranges. Flag the navigation
 * thread if we have four or more valid pseudoranges
 ******************************************************************************/
void calculate_pseudorange(OS_FLAGS measurements_ready)
{
	unsigned short ch;
	unsigned short pr_count;

	// led_turnon(LED3);

	// Copy over the measurement time so we know when the rho's were computed.
	g_ppr_cur->pr_time = m_raw_meas.meas_time;
	g_ppr_cur->pr_tic = m_raw_meas.meas_tic;

	// OK we're awake: for each measurement that we get, (Which we assume
	// is valid, since it wouldn't get up to this thread if it weren't!),
	// produce a pseudorange. Clear it to zero if it's not valid.
	pr_count = 0;
	if (m_sys_posconst & POS_CONSTELL_GPS) {
		for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
			if (measurements_ready & (1 << ch)) {
				gps_calculate_pseudorange( ch);
				g_ppr_cur->chan_pr[ch].valid = 1;
				pr_count++;
			}
			else {
				g_ppr_cur->chan_pr[ch].valid = 0;
			}
		}
	}

	if (m_sys_posconst & POS_CONSTELL_BDS) {
		for (ch = GPS_MAX_CHANNELS; ch < TOT_MAX_CHANNELS; ch++) {
			if (measurements_ready & (1 << ch)) {
				b1i_calculate_pseudorange( ch);
				g_ppr_cur->chan_pr[ch].valid = 1;
				pr_count++;
			}
			else {
				g_ppr_cur->chan_pr[ch].valid = 0;
			}
		}
	}

	// If we have any satellites, send them to the position thread. But
	// note that the position thread is going to take a BZILLION years to
	// process it all. Because we want to keep producing psuedoranges while
	// the position thread runs, we copy over the g_ppr_cur's so the position
	// thread can use a private copy.
	if  (pr_count > 0) {
#ifdef GNSS_ENABLE_MUTEX
		OSMutexPend(m_PrCbsMutex, 0, &err);
#endif
		if (m_ppr_pos == 0) {
			m_ppr_pos = g_ppr_cur;
			g_pr_idx = (g_pr_idx+1) & 1;
			g_ppr_cur = &g_pr[g_pr_idx];
#ifdef GNSS_ENABLE_MUTEX
			OSMutexPost(m_PrCbsMutex);
#endif
			// Run the position thread which will clear m_ppr_pos when it's done.
			OSSemPost(m_SemPosi);
        }
#ifdef GNSS_ENABLE_MUTEX
		OSMutexPost(m_PrCbsMutex);
#endif
    }
	// led_turnoff(LED3);
}