20、通道分配任务实现

\qquad 下面是HD-GR GNSS导航软件的通道分配任务实现代码：

// main_allocate.c -- Allocate channels.

/* 
 * 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: allocate.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 allocate.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 "includes.h"
#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"
#include "sys/alt_irq.h"
#include "constants.h"
#include "gnsstime.h"
#include "main_allocate.h"
#include "main_position.h"
#include "gps_accum_task.h"
#include "gps_message.h"
#include "gps_ephemeris.h"
#include "b1i_accum_task.h"
#include "b1i_message.h"
#include "b1i_ephemeris.h"


/*******************************************************************************
 * Global variables
 ******************************************************************************/

OS_FLAG_GRP* m_AllocateFlag;

static unsigned short GPS_PrnCode[GPS_MAX_SATELLITES+1] =
{	0x000,
	0x3ec,0x3d8,0x3b0,0x04b,0x096,0x2cb,0x196,0x32c,
	0x3ba,0x374,0x1d0,0x3a0,0x340,0x280,0x100,0x113,
	0x226,0x04c,0x098,0x130,0x260,0x267,0x338,0x270,
	0x0e0,0x1c0,0x380,0x22b,0x056,0x0ac,0x158,0x058
	};

static unsigned short B1I_PrnCode[B1I_MAX_SATELLITES+1] =
{	0x000,
	0x70c,0x4e3,0x33c,0x483,0x503,0x0fc,0x303,0x4fc,
	0x107,0x3ef,0x430,0x38f,0x20f,0x7f0,0x40f,0x3f0,
	0x7df,0x060,0x1e0,0x41f,0x7e0,0x01f,0x7bf,0x63f,
	0x3c0,0x03f,0x7c0,0x180,0x47f,0x780,0x07f,0x5ff
	};


/******************************************************************************
 * Initialize a channel into acquition mode

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

static void initialize_gps_channel( unsigned short ch, unsigned short prn, signed short nfreq)
{
	// Allocate the free satellite and get ready to check/allocate the next
	m_GPS_CH[ch].prn = prn;

	// Set SATCNTL register for C/A code
	write_to_correlator( (GPS_CH00_BASE + ch * CH_BASE_STEP + PRN_KEY), GPS_PrnCode[prn]);

	// Set carrier DCO
	// This is wrong. FIXME
	// carrier_corr is used to correct the "set_carrier_dco_rate()" (carrier DCO) settings.
	// If the system clock is in error by (x)[Hz] the correction to the carrier DCO
	// setting is -(IF_FREQ/(CARR_FREQ_RES * SYSTEM_CLOCK))
	// This is assuming that (x) is the error in the SYSTEM_CLOCK, which is not
	// clear (and should be checked), anyway, L1/CARR_FREQ_RES is wrong.
	// ADG: should it be: chan[ch].carrier_freq = carrier_ref + chan[ch].
	// carrier_corr + d_freq * chan[ch].n_freq;   // set carrier
	// ADG: removed static double ClockOffset = 0.0; and
	//   m_GPS_CH[ch].carrier_corr = (long)(-ClockOffset * 1575.42 / 42.57475e-3);
	m_GPS_CH[ch].carrier_corr = 0;
	m_GPS_CH[ch].carrier_freq = GPS_CARRIER_REF + m_GPS_CH[ch].carrier_corr;
	gps_set_carrier_dco_rate(ch, m_GPS_CH[ch].carrier_freq);

	// Initialize the code and frequency search variables
	m_GPS_CH[ch].codes = 0;
	m_GPS_CH[ch].n_freq = nfreq;
	m_GPS_CH[ch].ms_count = 0;

	// Set code DCO
	m_GPS_CH[ch].code_freq = GPS_CODE_REF;
	gps_set_code_dco_rate(ch, m_GPS_CH[ch].code_freq);

	// Bit sync and frame sync flags
	m_GPS_CH[ch].bit_sync = 0;

	// Clear prompt and dither vector magnitudes
	m_GPS_CH[ch].e_mag = 0;
	m_GPS_CH[ch].p_mag = 0;
	m_GPS_CH[ch].l_mag = 0;

	// Epoch counter set flags
	m_GPS_CH[ch].load_1ms_epoch_count = 0;
	m_GPS_CH[ch].sync_20ms_epoch_count = 0;
	m_GPS_CH[ch].backto_pull_in = 0;

	// Clear the number of bits since the week began
	m_GPS_CH[ch].time_in_bits = 0;

	// Clear the sat navigation message for this channel,
	// including the ephemeris since we're switching sats
	gps_clear_messages(ch);
	gps_clear_ephemeris(ch);

	m_GPS_CH[ch].state = CHANNEL_ACQUISITION;
}

static void initialize_b1i_channel( unsigned short ch, unsigned short prn, signed short nfreq)
{
	// Allocate the free satellite and get ready to check/allocate the next
	m_B1I_CH[ch].prn = prn;

	// Set SATCNTL register for C/A code
	write_to_correlator( (B1I_CH00_BASE + ch * CH_BASE_STEP + PRN_KEY), B1I_PrnCode[prn]);

	// Set carrier DCO
	// This is wrong. FIXME
	// carrier_corr is used to correct the "set_carrier_dco_rate()" (carrier DCO) settings.
	// If the system clock is in error by (x)[Hz] the correction to the carrier DCO
	// setting is -(IF_FREQ/(CARR_FREQ_RES * SYSTEM_CLOCK))
	// This is assuming that (x) is the error in the SYSTEM_CLOCK, which is not
	// clear (and should be checked), anyway, L1/CARR_FREQ_RES is wrong.
	// ADG: should it be: chan[ch].carrier_freq = carrier_ref + chan[ch].
	// carrier_corr + d_freq * chan[ch].n_freq;   // set carrier
	// ADG: removed static double ClockOffset = 0.0; and
	//   m_B1I_CH[ch].carrier_corr = (long)(-ClockOffset * 1575.42 / 42.57475e-3);
	m_B1I_CH[ch].carrier_corr = 0;
	m_B1I_CH[ch].carrier_freq = B1I_CARRIER_REF + m_B1I_CH[ch].carrier_corr;
	b1i_set_carrier_dco_rate(ch, m_B1I_CH[ch].carrier_freq);

	// Initialize the code and frequency search variables
	m_B1I_CH[ch].codes = 0;
	m_B1I_CH[ch].n_freq = nfreq;

	m_B1I_CH[ch].ms_maxval = IS_D1_NAVMESSAGE(prn) ? 19:1;
	m_B1I_CH[ch].ms_count = 0;

	// Set code DCO
	m_B1I_CH[ch].code_freq = B1I_CODE_REF;
	b1i_set_code_dco_rate(ch, m_B1I_CH[ch].code_freq);

	// Bit sync and frame sync flags
	m_B1I_CH[ch].bit_sync = 0;

	// Clear prompt and dither vector magnitudes
	m_B1I_CH[ch].e_mag = 0;
	m_B1I_CH[ch].p_mag = 0;
	m_B1I_CH[ch].l_mag = 0;

	// Epoch counter set flags
	m_B1I_CH[ch].load_1ms_epoch_count = 0;
	m_B1I_CH[ch].sync_20ms_epoch_count = 0;
	m_B1I_CH[ch].backto_pull_in = 0;

	// Clear the number of bits since the week began
	m_B1I_CH[ch].time_in_bits = 0;

	// Clear the sat navigation message for this channel,
	// including the ephemeris since we're switching sats
	b1i_clear_messages(ch);
	b1i_clear_ephemeris(ch);

	m_B1I_CH[ch].state = CHANNEL_ACQUISITION;
}

/******************************************************************************
 * We don't know anything about any satellite (cold power on with no memory)
 * so blindly go through satellites, one at a now through channels.
 ******************************************************************************/
void cold_allocate_gps_channel( unsigned short ch)
{
	// Search satellites 1st to last
	static unsigned short next_satellite = 0;

	unsigned short i;
	unsigned short already_allocated;

	do {
		next_satellite++;
		// check satellites 1 to 32
		if (next_satellite > GPS_MAX_SATELLITES) {
			next_satellite = 1;
		}

		already_allocated = 0;
		for (i = 0; i < GPS_MAX_CHANNELS; i++) {
			// Modified to fix an allocation bug (AA)
			if (m_GPS_CH[i].prn == next_satellite) // && m_GPS_CH[i].state != CHANNEL_OFF) (AA)
			{
				already_allocated = 1;
				break;
			}
		}
	} while (already_allocated);

    initialize_gps_channel( ch, next_satellite, 1);
}

void cold_allocate_b1i_channel( unsigned short ch)
{
	// Search satellites 1st to last
	static unsigned short next_satellite = 0;

	unsigned short i;
	unsigned short already_allocated;

	do {
		next_satellite++;
		// check satellites 1 to 32
		if (next_satellite > B1I_MAX_SATELLITES) {
			next_satellite = 1;
		}

		already_allocated = 0;
		for (i = 0; i < B1I_MAX_SATELLITES; i++) {
			// Modified to fix an allocation bug (AA)
			if (m_B1I_CH[i].prn == next_satellite) // && m_B1I_CH[i].state != CHANNEL_OFF) (AA)
			{
				already_allocated = 1;
				break;
			}
		}
	} while (already_allocated);

    initialize_b1i_channel( ch, next_satellite, 1);
}


void initialize_allocation( void)
{
	unsigned short ch;

	if (m_sys_posconst & POS_CONSTELL_GPS) {
		for (ch = 0; ch < GPS_MAX_CHANNELS; ch ++) {
			cold_allocate_gps_channel(ch);
		}
	}
	if (m_sys_posconst & POS_CONSTELL_BDS) {
		for (ch = 0; ch < B1I_MAX_CHANNELS; ch ++) {
			cold_allocate_b1i_channel(ch);
		}
	}
}

void allocate_task(void* pdata) // input 'pdata' not used
{
	INT8U err;
	unsigned short ch;

	// There SHOULDN'T be a race condition here because initialize_allocate()
	// should keep this from being called for a while on startup.
	m_AllocateFlag = OSFlagCreate((OS_FLAGS)0, &err);

	while(1) {
		// Block. If tracking() in tracking.c turns off any channels, then it 
		// will set the flag bits and wake this thread up.
		OSFlagPend(m_AllocateFlag,
			(1 << TOT_MAX_CHANNELS) - 1,
			OS_FLAG_WAIT_SET_ANY + OS_FLAG_CONSUME, 
			0, &err);

		if (m_sys_posconst & POS_CONSTELL_GPS) {
			for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
				if (m_GPS_CH[ch].state == CHANNEL_OFF)
					cold_allocate_gps_channel(ch);
			}
		}

		if (m_sys_posconst & POS_CONSTELL_BDS) {
			for (ch = 0; ch < B1I_MAX_CHANNELS; ch++) {
				if (m_B1I_CH[ch].state == CHANNEL_OFF)
					cold_allocate_b1i_channel(ch);
			}
		}
    }
}