28、星历处理任务实现

\qquad 下面是HD-GR GNSS导航软件的星历处理任务实现代码：

// main_ephemeris.c -- Ephemeris processing task.

/* 
 * 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 : ephemeris.c
 * Modes    : LED handling for debugging commented/replaced
 * version  : V1.0
 * date     : 21st/Dec/2006
 */

/* 
 * HD-GR GNSS receiver project
 * Modes    : Inherited the code of ephemeris.c and ephemeris.h 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 "includes.h"
#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"
#include "sys/alt_irq.h"
#include "main_position.h"
#include "gps_ephemeris.h"
#include "gps_almanac.h"
#include "b1i_ephemeris.h"
#include "b1i_almanac.h"

/******************************************************************************
 * Defines
 ******************************************************************************/

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

OS_FLAG_GRP* m_EphemerisChannelFlag;
OS_FLAG_GRP* m_EphemerisSubframeFlags[TOT_MAX_CHANNELS];
OS_EVENT *m_EphTabMutex = 0;

/******************************************************************************
 * Statics
 ******************************************************************************/

// None

void initialize_ephetable( void)
{
	gps_initialize_ephetable();
	b1i_initialize_ephetable();
}


/****************************************************************************
FUNCTION satpos_almanac(double time, almanac_t *p)
RETURNS  SATELLITE POSITION.

PARAMETERS
	time	-- time of week
	pSatAlm	-- satellite almanac

PURPOSE
	THIS SUBROUTINE CALCULATES THE SATELLITE POSITION BASED ON ALMANAC DATA

	R    - RADIUS OF SATELLITE AT TIME T
	SLAT - SATELLITE LATITUDE
	SLONG- SATELLITE LONGITUDE
	T    - TIME FROM START OF WEEKLY EPOCH
	ETY  - ORBITAL ECCENTRICITY
	TOA  - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
	INC  - ORBITAL INCLINATION
	RRA  - RATE OF RIGHT ASCENSION
	SQA  - SQUARE ROOT OF SEMIMAJOR AXIS
	LAN  - LONGITUDE OF NODE AT WEEKLY EPOCH
	AOP  - ARGUMENT OF PERIGEE
	MA   - MEAN ANOMALY AT TOA

WRITTEN BY
	Clifford Kelley
****************************************************************************/

xyz_t satpos_almanac(double time, almanac_t *pSatAlm)
{
	xyz_t result;
	double ei, ea, diff, r, ta, la, aol, xp, yp, d_toa;

	// MA IS THE ANGLE FROM PERIGEE AT TOA
	d_toa = time - pSatAlm->toa;
	if (d_toa > 302400.0)
		d_toa = d_toa - 604800.0;
	else if (d_toa < -302400.0)
		d_toa = d_toa + 604800.0;
	ei = pSatAlm->ma + d_toa * pSatAlm->w;
	ea = ei;
	do {
		diff = (ei - (ea - pSatAlm->ety * sin (ea))) / (1. - pSatAlm->ety * cos (ea));
		ea = ea + diff;
	} while (fabs (diff) > 1.0e-6);
	// EA IS THE ECCENTRIC ANOMALY

	if (pSatAlm->ety != 0.0) {
		ta = atan2 (sqrt (1. - SQ (pSatAlm->ety)) * sin (ea), cos (ea) - pSatAlm->ety);
	}
	else {
		ta = ea;
	}
	// TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)

	r = SQ (pSatAlm->sqa) * (1. - pSatAlm->ety * cos (ea));
	// R IS THE RADIUS OF SATELLITE ORBIT AT TIME T

	aol = ta + pSatAlm->aop;
	// AOL IS THE ARGUMENT OF LATITUDE

	// LA IS THE LONGITUDE OF THE ASCENDING NODE
	la = pSatAlm->lan + (pSatAlm->rra - OMEGA_E) * d_toa - pSatAlm->toa * OMEGA_E;

	xp = r * cos (aol);
	yp = r * sin (aol);
	result.x = xp * cos (la) - yp * cos (pSatAlm->inc) * sin (la);
	result.y = xp * sin (la) + yp * cos (pSatAlm->inc) * cos (la);
	result.z = yp * sin (pSatAlm->inc);
	return result;
}

/****************************************************************************
FUNCTION satfind()
RETURNS  None.

PARAMETERS None.

PURPOSE

	THIS FUNCTION DETERMINES THE SATELLITES TO SEARCH FOR
	WHEN ALMANAC DATA IS AVAILABLE

WRITTEN BY
	Clifford Kelley

*****************************************************************************/

int satfind (
	unsigned short bgps,
	int prn,
	stdtime_t gmt,
	pvt_t rec_pos,
	neu_t rec_neu,
	azel_t *pazel,
	double *pvel)
{
	if (prn < 0 || prn > 31 || !pazel) {
		return 0;
	}

	almanac_t *pSatAlm = bgps ? (&m_gps_alm[prn]):(&m_b1i_alm[prn]);
	if (!pSatAlm->update || pSatAlm->health) {
		return 0;
	}

	int gps_week;
	double jd_day, alm_time, almanac_date;

	pazel->az = 0;
	pazel->el = 0;

	jd_day = to_julian_time(gmt);
	gps_week = (int)((jd_day - 2444244.5) / 7.);
	almanac_date = pSatAlm->week * 7.0 + 2444244.5;
	if (gps_week - pSatAlm->week > 512) {
		almanac_date += 1024 * 7.0;
	}
	alm_time = (jd_day - almanac_date) * 86400.;

	// CALCULATE THE POSITION OF THE SATELLITES
	if (prn > 0 && pSatAlm->inc > 0.0) {
		xyz_t gnsspos;
		xyzt_t satpost;

		gnsspos = satpos_almanac (alm_time, &m_gps_alm[prn]);
		satpost.tb = 0;
		satpost.x = gnsspos.x;
		satpost.y = gnsspos.y;
		satpost.z = gnsspos.z;
		*pazel = satellite_azel(satpost, rec_pos, rec_neu);
		if (pvel) {
			xyz_t gpsspd;
			double range, xls, yls, zls;

			gpsspd = satpos_almanac (alm_time + 1.0, &m_gps_alm[prn]);
			gpsspd.x -= gnsspos.x;
			gpsspd.y -= gnsspos.y;
			gpsspd.z -= gnsspos.z;

			xls = rec_pos.x - gnsspos.x;
			yls = rec_pos.y - gnsspos.y;
			zls = rec_pos.z - gnsspos.z;
			range = sqrt (xls * xls + yls * yls + zls * zls);
			xls = xls / range;
			yls = yls / range;
			zls = zls / range;
			*pvel = gpsspd.x * xls + gpsspd.y * yls + gpsspd.z * zls;
		}
		return 1;
	}
	return 0;
}

/******************************************************************************
 * Stuff incoming bits from the message task into words and subframes in
 * the messages structure.
 ******************************************************************************/
void ephemeris_task(void* pdata) // input 'pdata' not used
{
	INT8U err;
	unsigned short ch;
    OS_FLAGS channels_ready;
    
    // There's no way that we're going to get a subframe before this thread
    // is first executed, so it's ok to run the flag inits here.
	m_EphemerisChannelFlag = OSFlagCreate((OS_FLAGS)0, &err);
#ifdef GNSS_ENABLE_MUTEX
	m_EphTabMutex = OSMutexCreate(EPHEM_TASK_PRIORITY, &err);
#endif

    for (ch = 0; ch < TOT_MAX_CHANNELS; ch++) {
		m_EphemerisSubframeFlags[ch] = OSFlagCreate((OS_FLAGS)0, &err);
	}
    
    while (1) {
		// Block if there are no channels_ready with bits ready. Wake up if any
		// bits from the 20 channels_ready (0xFFFFF) are set. Clear the flag on
		// wake up, with the bits saved in channels_ready_with_bits.
		channels_ready = OSFlagPend(m_EphemerisChannelFlag,
			(1 << TOT_MAX_CHANNELS) - 1,
			OS_FLAG_WAIT_SET_ANY + OS_FLAG_CONSUME, 
			0, &err);

		// led_turnon(LED3);
		if (m_sys_posconst & POS_CONSTELL_BDS) {
			b1i_ephemeris_task(channels_ready >> GPS_MAX_CHANNELS);
		}
		if (m_sys_posconst & POS_CONSTELL_GPS) {
			gps_ephemeris_task(channels_ready);
		}
		// led_turnoff(LED3);
	}
}