onur
a25a1a61d0
Add freestanding S25FL256S read/program/erase/verify support and seed a newly formatted ephemeris store with stale records. Age-gate navigation readiness, persist the continuous Toe week, and regenerate the Genesys2 MAX2771 integration and flash image.
739 lines
32 KiB
C
739 lines
32 KiB
C
#ifndef GPS_IP_DEFINES
|
|
#define GPS_IP_DEFINES
|
|
|
|
#include <stdbool.h>
|
|
#include <stddef.h>
|
|
#include <float.h>
|
|
#include <stdint.h>
|
|
#include <bsp/soc.h>
|
|
#include "gps/matrixLib.h"
|
|
#include "gps/gps_macros.h"
|
|
#include "gps/gps_almanac.h"
|
|
|
|
#ifdef __riscv
|
|
#define GPS_IRQ_HANDLER_ATTR __attribute__((interrupt("machine"), aligned(4)))
|
|
#else
|
|
#define GPS_IRQ_HANDLER_ATTR
|
|
#endif
|
|
|
|
|
|
|
|
// |_ _| _ \ | _ \| ____| ___|_ _| \ | | ____/ ___|
|
|
// | || |_) | | | | | _| | |_ | || \| | _| \___ \
|
|
// | || __/ | |_| | |___| _| | || |\ | |___ ___) |
|
|
// |___|_| |____/|_____|_| |___|_| \_|_____|____/
|
|
|
|
#define GPS_TRCK_UNLOCK_TIMEOUT_TICKS 500000000u
|
|
#define GPS_PRN_COUNT 32u
|
|
#define GPS_PRN_INVALID 0xffu
|
|
|
|
#ifndef BSP_GPS_ACQ_COUNT
|
|
#define BSP_GPS_ACQ_COUNT 1UL
|
|
#endif
|
|
|
|
#ifndef BSP_GPS_ACQ_STRIDE
|
|
#define BSP_GPS_ACQ_STRIDE BSP_GPS_TRCK_STRIDE
|
|
#endif
|
|
|
|
#ifndef BSP_GPS_ACQ_IRQ_STRIDE
|
|
#define BSP_GPS_ACQ_IRQ_STRIDE 1UL
|
|
#endif
|
|
|
|
#ifndef ACQ_ENGINE_COUNT
|
|
#define ACQ_ENGINE_COUNT BSP_GPS_ACQ_COUNT
|
|
#endif
|
|
|
|
#define GPS_ACQ_IP_COUNT ACQ_ENGINE_COUNT
|
|
#define GPS_ACQ_IP_BASE_ADDR(index) (BSP_GPS_ACQ_BASE + ((uintptr_t)(index) * BSP_GPS_ACQ_STRIDE))
|
|
#define GPS_ACQ_INTR_LINE(index) (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE))
|
|
/* Sized for the worst foreground stall, which is now a flash
|
|
* compaction sector erase (hundreds of ms). At 9 channels the
|
|
* 200 ms nav-solve events alone contribute ~23 entries during such
|
|
* a stall; 64 left no headroom for a concurrent burst and the
|
|
* startup burst was already overflowing. */
|
|
#define GPS_EVENT_QUEUE_SIZE 128u
|
|
/* Genesys2 CLINT is 500 kHz. Keep foreground maintenance at
|
|
* 10 Hz; the tracking IP's 20 ms NAV timer is independent. */
|
|
#define GPS_SYSTICK_HZ 10u
|
|
#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
|
|
#ifndef GPS_NAV_SOLVE_TIMER_TICKS
|
|
#define GPS_NAV_SOLVE_TIMER_TICKS 50u
|
|
#endif
|
|
#define GPS_NAV_BITS_PER_WEEK 30240000u
|
|
#define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
|
|
#define GPS_NAV_BITS_PER_SUBFRAME 300u
|
|
/* Keep integrity statistics active, but do not let leave-one-out FDE
|
|
* replace the full-WLS position until the real measurement-noise model
|
|
* is calibrated. The live stationary test produced a 22.7% exclusion
|
|
* rate and substantially worse position dispersion. */
|
|
#define GPS_NAV_ENABLE_FDE 0u
|
|
/*
|
|
* Measurement weighting. The elevation term is 3 m at zenith
|
|
* mapped by 1/sin(elevation); the floor bounds it where 1/sin
|
|
* diverges, because a zero weight is a failed epoch rather than a
|
|
* discounted satellite. Three degrees leaves the term at 57 m -
|
|
* some 350 times lighter than zenith - and, unlike the ten degree
|
|
* floor it replaces, lets the mapping actually work across the
|
|
* range where a satellite carries most of the error.
|
|
*/
|
|
#define GPS_NAV_WEIGHT_MIN_ELEVATION_DEG 3.0
|
|
/*
|
|
* Charged on top when a satellite is at or below the horizon, so
|
|
* the extrapolated atmospheric correction and the reflection it
|
|
* probably arrived by are paid for in variance rather than
|
|
* assumed away.
|
|
*/
|
|
#define GPS_NAV_BELOW_HORIZON_SIGMA_M 50.0
|
|
/* Upper bound on the carrier-to-noise term, before calibration. */
|
|
#define GPS_NAV_CN0_SIGMA_MAX_M 30.0
|
|
#define GPS_PROFILE_CPU_HZ 50000000ULL
|
|
#define GPS_ACQ_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ / 2u)
|
|
#define GPS_ACQ_HOT_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 30u)
|
|
#define GPS_ACQ_REACQ_PRIORITY_TICKS \
|
|
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
|
|
#define GPS_ACQ_MAX_HOT_STREAK 4u
|
|
/*
|
|
* Fast retries granted to a freshly lost satellite before the long
|
|
* hot backoff applies. Six attempts at the 0.5 s cold cadence is
|
|
* ~3 s, comfortably longer than the tracking loop needs to settle
|
|
* after a signal interruption, and far below the 30 s backoff that
|
|
* previously applied from the first retry.
|
|
*/
|
|
#define GPS_ACQ_HOT_FAST_ATTEMPTS 6u
|
|
/*
|
|
* Almanac-directed acquisition. A PRN predicted below the mask
|
|
* is retried on this slow cadence instead of the 0.5 s sweep, so
|
|
* the single acquisition engine spends its time on satellites
|
|
* that are actually up. It is a demotion, not an exclusion: the
|
|
* PRN is still revisited so a stale almanac or a stale position
|
|
* self-corrects, and a satellite that has just risen is found
|
|
* without waiting for the next almanac refresh.
|
|
*/
|
|
#define GPS_ACQ_UNLIKELY_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 20u)
|
|
#define GPS_ACQ_VISIBILITY_REFRESH_TICKS (GPS_SYSTICK_HZ * 10u)
|
|
/*
|
|
* Search a little below the horizon: almanac elevation carries a
|
|
* few degrees of error when the data is days old, and a satellite
|
|
* about to rise is worth finding early.
|
|
*/
|
|
#define GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG (-5.0)
|
|
/*
|
|
* The same mask while the receiver position is still the seed
|
|
* read back from flash rather than a fix taken this run. The
|
|
* seed says where the receiver was; nothing yet says it has not
|
|
* been moved since. A displacement of D km moves a look angle by
|
|
* roughly D/111 degrees, so -25 tolerates a couple of thousand
|
|
* kilometres of unknown migration while still ruling out the far
|
|
* side of the earth.
|
|
*/
|
|
#define GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG (-25.0)
|
|
#define GPS_ACQ_STUCK_WARN_TICKS (GPS_SYSTICK_HZ * 8u)
|
|
#define GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS \
|
|
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
|
|
#define GPS_ACQ_STATUS_LOG_PERIOD_TICKS (GPS_SYSTICK_HZ * 5u)
|
|
#define GPS_NAV_STATUS_LOG_PERIOD_TICKS GPS_SYSTICK_HZ
|
|
#define GPS_TRK_MEASUREMENT_STALE_TICKS (GPS_SYSTICK_HZ * 3u)
|
|
#define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS 25u
|
|
#define GPS_NAV_EPOCH_HOLDOFF_MS 30u
|
|
#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
|
|
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
|
|
#define GPS_HOW_CONTINUITY_MAX_GAP_SEC 120.0
|
|
#define GPS_HOW_CONTINUITY_TOLERANCE_SEC 0.050
|
|
/* Warm/hot reacquisition phase seeding. The PRN context is only
|
|
* reused while the receiver clock prediction stays far inside one
|
|
* 20 ms navigation bit; at a few ppm the 15 minute bound leaves
|
|
* roughly two orders of magnitude of margin, and broadcast
|
|
* ephemeris validity caps reuse well before that anyway. */
|
|
/* Persistent ephemeris cache. The Genesys2 image places the
|
|
* bitstream at 0 and the application at 0x01800000 on a 32 MiB
|
|
* S25FL256S, so the last two 64 KiB sectors are free. Keeping the
|
|
* store at the very top leaves the application region free to grow. */
|
|
#ifndef GPS_EPHEMERIS_FLASH_ENABLE
|
|
#define GPS_EPHEMERIS_FLASH_ENABLE 1
|
|
#endif
|
|
/* Test hooks. MAX_SLOTS caps the usable slots per sector so
|
|
* compaction can be exercised on real hardware without writing 256
|
|
* records; 0 means use the whole sector. FORCE_FORMAT erases both
|
|
* sectors at boot so the format path can be observed. */
|
|
#ifndef GPS_EPHEMERIS_STORE_MAX_SLOTS
|
|
#define GPS_EPHEMERIS_STORE_MAX_SLOTS 0u
|
|
#endif
|
|
#ifndef GPS_EPHEMERIS_FLASH_FORCE_FORMAT
|
|
#define GPS_EPHEMERIS_FLASH_FORCE_FORMAT 0
|
|
#endif
|
|
#ifndef GPS_EPHEMERIS_FLASH_SEED_INITIAL
|
|
#define GPS_EPHEMERIS_FLASH_SEED_INITIAL 1
|
|
#endif
|
|
#ifndef GPS_EPHEMERIS_FLASH_BASE
|
|
#define GPS_EPHEMERIS_FLASH_BASE 0x01FE0000UL
|
|
#endif
|
|
/*
|
|
* Persistent navigation context (almanac, ionosphere model, last
|
|
* position) in the two 64 KiB sectors immediately below the
|
|
* ephemeris store. The application image lives at 0x01800000 and
|
|
* is far smaller than the gap.
|
|
*/
|
|
#ifndef GPS_NAV_CONTEXT_FLASH_BASE
|
|
#define GPS_NAV_CONTEXT_FLASH_BASE 0x01FC0000UL
|
|
#endif
|
|
#ifndef GPS_NAV_CONTEXT_ENABLE
|
|
#define GPS_NAV_CONTEXT_ENABLE 1
|
|
#endif
|
|
/*
|
|
* How often the context is rebuilt and offered to the store. The
|
|
* store compares a CRC first, so a save that would not change the
|
|
* bytes never reaches the flash; this period only bounds how
|
|
* quickly a genuine change is captured.
|
|
*/
|
|
#define GPS_NAV_CONTEXT_SAVE_PERIOD_TICKS (GPS_SYSTICK_HZ * 60u)
|
|
/*
|
|
* What counts as the receiver having moved, for the purpose of
|
|
* the persisted position seed, and how often that seed may be
|
|
* refreshed. The seed only aims a visibility prediction and a
|
|
* look angle changes by ~0.003 degrees per kilometre of receiver
|
|
* error, so 5 km is far inside what the mask can notice. The two
|
|
* limits together are what keep the erase count tied to real news
|
|
* rather than to the save timer: a stationary receiver offers
|
|
* byte-identical payloads for ever and never touches the flash.
|
|
*/
|
|
#define GPS_NAV_CONTEXT_POSITION_EPSILON_M 5000.0
|
|
#define GPS_NAV_CONTEXT_POSITION_REFRESH_TICKS \
|
|
(GPS_SYSTICK_HZ * 60u * 60u)
|
|
#ifndef GPS_WARM_SEED_ENABLE
|
|
#define GPS_WARM_SEED_ENABLE 1
|
|
#endif
|
|
#define GPS_NAV_WARM_SEED_MAX_AGE_SEC 900.0
|
|
/* Never arm within this distance of a navigation-bit boundary, so
|
|
* the seed cannot be applied one bit late. */
|
|
#define GPS_NAV_WARM_SEED_GUARD_SEC 0.004
|
|
/* A seeded epoch must land on the known nav-solve grid within this
|
|
* tolerance before its time anchor is trusted. */
|
|
#define GPS_NAV_WARM_SEED_SNAP_SEC 0.050
|
|
/*
|
|
* A channel is only expected to contribute to an epoch while it is
|
|
* actually producing nav-solve events. busy+locked+IRQ-enabled is
|
|
* not the same thing: the RTL timer returns to IDLE whenever
|
|
* trck_busy drops and will not restart without a subframe or a
|
|
* seed, so a channel that dips and regains lock looks ready while
|
|
* emitting nothing. Left in the expected set it holds every epoch
|
|
* to its deadline until its next subframe, up to six seconds.
|
|
*/
|
|
#define GPS_NAV_SILENT_CHANNEL_EPOCHS 2u
|
|
|
|
#if (GPS_NAV_SOLVE_TIMER_TICKS == 0u)
|
|
#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
|
|
#elif ((GPS_NAV_BITS_PER_WEEK % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
|
|
#error "GPS_NAV_SOLVE_TIMER_TICKS must divide 30240000 GPS navigation bits per week"
|
|
#elif ((GPS_NAV_BITS_PER_SUBFRAME % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
|
|
#error "GPS_NAV_SOLVE_TIMER_TICKS must divide the 300-bit LNAV subframe"
|
|
#endif
|
|
_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
|
|
"GPS_NAV_ENABLE_FDE must be zero or one");
|
|
|
|
#if (ACQ_ENGINE_COUNT < 1UL)
|
|
#error "ACQ_ENGINE_COUNT must be at least 1"
|
|
#endif
|
|
|
|
#if (ACQ_ENGINE_COUNT > 1UL) && ((BSP_GPS_ACQ_BASE + ((ACQ_ENGINE_COUNT) * BSP_GPS_ACQ_STRIDE)) > BSP_GPS_TRCK0_BASE)
|
|
#error "multi-engine ACQ configuration overlaps BSP_GPS_TRCK0_BASE; update BSP_GPS_ACQ_STRIDE/BSP_GPS_TRCK0_BASE"
|
|
#endif
|
|
|
|
#define ACQ0_IP_BASE_ADDR BSP_GPS_ACQ_BASE
|
|
#define TRCK0_IP_BASE_ADDR BSP_GPS_TRCK0_BASE
|
|
#define TRCK1_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 1UL * BSP_GPS_TRCK_STRIDE)
|
|
#define TRCK2_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 2UL * BSP_GPS_TRCK_STRIDE)
|
|
#define TRCK3_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 3UL * BSP_GPS_TRCK_STRIDE)
|
|
#define TRCK4_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 4UL * BSP_GPS_TRCK_STRIDE)
|
|
#define TRCK5_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 5UL * BSP_GPS_TRCK_STRIDE)
|
|
#define TRCK6_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 6UL * BSP_GPS_TRCK_STRIDE)
|
|
#define TRCK7_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 7UL * BSP_GPS_TRCK_STRIDE)
|
|
#define TRCK8_IP_BASE_ADDR (BSP_GPS_TRCK0_BASE + 8UL * BSP_GPS_TRCK_STRIDE)
|
|
|
|
#define ACQ0_INTR_LINE BSP_GPS_ACQ_IRQ
|
|
#define TRCK0_INTR_LINE BSP_GPS_TRCK0_IRQ
|
|
#define TRCK1_INTR_LINE (BSP_GPS_TRCK0_IRQ + 1UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
#define TRCK2_INTR_LINE (BSP_GPS_TRCK0_IRQ + 2UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
#define TRCK3_INTR_LINE (BSP_GPS_TRCK0_IRQ + 3UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
#define TRCK4_INTR_LINE (BSP_GPS_TRCK0_IRQ + 4UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
#define TRCK5_INTR_LINE (BSP_GPS_TRCK0_IRQ + 5UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
#define TRCK6_INTR_LINE (BSP_GPS_TRCK0_IRQ + 6UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
#define TRCK7_INTR_LINE (BSP_GPS_TRCK0_IRQ + 7UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
#define TRCK8_INTR_LINE (BSP_GPS_TRCK0_IRQ + 8UL * BSP_GPS_TRCK_IRQ_STRIDE)
|
|
|
|
// acq ip control reg bits.
|
|
#define GPS_ACQ_START 0
|
|
#define GPS_ACQ_INTPRT_CLR 1
|
|
#define GPS_ACQ_INTPRT_ENABLE 2
|
|
#define GPS_ACQ_INTRPT_PENDING 3
|
|
#define GPS_ACQ_AFE_START 4
|
|
|
|
// trck ip control reg bits.
|
|
#define GPS_TRCK_START_POS 0
|
|
#define GPS_TRCK_STOP_POS 1
|
|
#define GPS_TRCK_INTR_EN_POS 2
|
|
#define GPS_TRCK_NAV_BITS_INTR_EN_POS 3
|
|
#define GPS_TRCK_NAV_BITS_FLAG_EN_POS 4 // (Default 1)
|
|
#define GPS_TRCK_INTR_CLR_POS 5 // generic clear signal. Clears all internal flags.
|
|
#define GPS_TRCK_RESET_POS 6
|
|
#define GPS_TRCK_STOP_UNLOCK_EN_POS 7
|
|
#define GPS_TRCK_UNLOCK_INTR_EN_POS 8
|
|
#define GPS_TRCK_UNLOCK_FLAG_EN_POS 9 // (Default 1)
|
|
#define GPS_TRCK_NAV_SOLVE_EN_POS 10
|
|
#define GPS_TRCK_NAV_SOLVE_INTR_EN_POS 11
|
|
#define GPS_TRCK_NAV_SOLVE_FLAG_EN_POS 12 // (Default 1)
|
|
#define GPS_TRCK_MEASUREMENT_INTR_EN_POS 13
|
|
#define GPS_TRCK_MEASUREMENT_FLAG_EN_POS 14
|
|
/* Arms NAV_SOLVE_PHASE_SEED. Hardware answers and clears this bit, so it
|
|
* reads back zero once the timer has evaluated the request. */
|
|
#define GPS_TRCK_NAV_SOLVE_SEED_ARM_POS 15
|
|
|
|
|
|
// acq ip status reg bits.
|
|
#define GPS_ACQ_STATUS_POS 0
|
|
#define GPS_ACQ_BUSY_POS 1
|
|
|
|
// tracking ip status reg bits.
|
|
#define GPS_TRCK_BUSY_POS 0
|
|
#define GPS_TRCK_LOCK_POS 1
|
|
#define GPS_TRCK_OVFL_POS 2
|
|
#define GPS_TRCK_INTR_PEND_POS 3
|
|
#define GPS_TRCK_NAV_BITS_RCV_POS 4
|
|
#define GPS_TRCK_STOP_UNLOCK_POS 5
|
|
#define GPS_TRCK_NAV_SOLVE_FLAG_POS 6
|
|
#define GPS_TRCK_MEASUREMENT_FLAG_POS 7
|
|
/* NAV solve epochs are running on a firmware seed no subframe has confirmed. */
|
|
#define GPS_TRCK_NAV_SOLVE_PROVISIONAL_POS 8
|
|
|
|
#define GPS_TRCK_MEAS_META_VALID_POS 0
|
|
#define GPS_TRCK_MEAS_META_NAV_BIT_POS 1
|
|
#define GPS_TRCK_MEAS_META_LOCK_POS 2
|
|
#define GPS_TRCK_MEAS_META_COUNT_SHIFT 8
|
|
#define GPS_TRCK_MEAS_META_COUNT_MASK 0x1fu
|
|
#define GPS_TRCK_MEAS_META_VERSION_SHIFT 24
|
|
#define GPS_TRCK_MEAS_META_VERSION 1u
|
|
#define GPS_TRCK_CARRIER_Q7_SCALE 128.0
|
|
#define GPS_TRCK_IF_CENTER_HZ 4092000.0
|
|
#define GPS_L1_CARRIER_HZ 1575420000.0
|
|
|
|
|
|
|
|
#define GPS_TRCK_IP_COUNT BSP_GPS_TRCK_COUNT
|
|
|
|
|
|
// gps defines from plutogpssim.
|
|
|
|
/* Current tracking RTL validates the subframe on the ninth following
|
|
* navigation bit: 9 * 20 ms = 180 ms. satpos() and the PPS receive-time
|
|
* anchor must use this same delivery-epoch correction exactly once. */
|
|
#define POSTPONE_TOW (double)(0.180)
|
|
#define MAX_SAT (12) // 32 (TODO. OBSOLATED)
|
|
|
|
#define SECONDS_IN_WEEK 604800.0
|
|
#define SECONDS_IN_HALF_WEEK 302400.0
|
|
#define SECONDS_IN_DAY 86400.0
|
|
#define SECONDS_IN_HOUR 3600.0
|
|
#define SECONDS_IN_MINUTE 60.0
|
|
|
|
//#define SAMPLE_RATE 16368000
|
|
#define STARTOFFSET 68.802
|
|
|
|
/*! \brief Maximum number of iterations for least square search */
|
|
#define NUMBER_OF_ITERATIONS (100)
|
|
|
|
// Conventional values employed in GPS ephemeris model (ICD-GPS-200)
|
|
#define GM_EARTH 398600500000000
|
|
#define OMEGA_EARTH 7.2921151467e-5
|
|
#define PI 3.1415926535898
|
|
|
|
#define WGS84_RADIUS 6378137.0
|
|
#define WGS84_ECCENTRICITY 0.0818191908426
|
|
|
|
#define R2D 57.2957795131
|
|
|
|
#define SPEED_OF_LIGHT 299792458
|
|
#define LAMBDA_L1 0.19
|
|
|
|
|
|
// ____ _ _____ _
|
|
// | _ \ / \|_ _|/ \
|
|
// | | | |/ _ \ | | / _ \
|
|
// | |_| / ___ \| |/ ___ \
|
|
// |____/_/ \_\_/_/ \_\
|
|
//
|
|
// ____ _____ ____ _ _ ____ _____ _ _ ____ _____ ____
|
|
// / ___|_ _| _ \| | | |/ ___|_ _| | | | _ \| ____/ ___|
|
|
// \___ \ | | | |_) | | | | | | | | | | | |_) | _| \___ \
|
|
// ___) || | | _ <| |_| | |___ | | | |_| | _ <| |___ ___) |
|
|
// |____/ |_| |_| \_\\___/ \____| |_| \___/|_| \_\_____|____/
|
|
|
|
typedef struct{
|
|
uint32_t CONTROL_REG; // 'h00
|
|
uint32_t PRN_NUMBER; // 'h04
|
|
uint32_t PEAK_METRIC; // 'h08
|
|
uint32_t CODE_PHASE; // 'h0c
|
|
uint32_t ABS_SAMPLE; // 'h10
|
|
uint32_t STATUS_REG; // 'h14
|
|
uint32_t DOPPLER_REG; // 'h18
|
|
}gps_acq_t;
|
|
|
|
typedef struct{
|
|
uint32_t CONTROL_REG; // 'h00
|
|
uint32_t PRN_NUMBER; // 'h04
|
|
uint32_t CODE_PHASE; // 'h08
|
|
uint32_t ABS_SAMPLE; // 'h0c
|
|
uint32_t STATUS_REG; // 'h10
|
|
uint32_t DOPPLER_REG; // 'h14
|
|
uint32_t NAV_BIT_READ_REG0; // 'h18
|
|
uint32_t NAV_BIT_READ_REG1; // 'h1c
|
|
uint32_t NAV_BIT_READ_REG2; // 'h20
|
|
uint32_t NAV_BIT_READ_REG3; // 'h24
|
|
uint32_t NAV_BIT_READ_REG4; // 'h28
|
|
uint32_t NAV_BIT_READ_REG5; // 'h2c
|
|
uint32_t NAV_BIT_READ_REG6; // 'h30
|
|
uint32_t NAV_BIT_READ_REG7; // 'h34
|
|
uint32_t NAV_BIT_READ_REG8; // 'h38
|
|
uint32_t NAV_BIT_READ_REG9; // 'h3c
|
|
uint32_t TIMEOUT_REG; // 'h40
|
|
uint32_t NAV_SOLVE_TIMER; // 'h44
|
|
uint32_t RESERVED_48; // 'h48
|
|
uint32_t REM_CODE_PHASE_REG; // 'h4c
|
|
uint32_t MEASUREMENT_META; // 'h50
|
|
uint32_t MEASUREMENT_SEQUENCE; // 'h54
|
|
int32_t MEASUREMENT_CARRIER_Q7; // 'h58
|
|
uint32_t MEASUREMENT_ABS_SAMPLE; // 'h5c
|
|
uint32_t MEASUREMENT_REM_CODE_PHASE; // 'h60
|
|
int32_t MEASUREMENT_SUM_I; // 'h64
|
|
int32_t MEASUREMENT_SUM_Q; // 'h68
|
|
uint32_t MEASUREMENT_WBP_LO; // 'h6c
|
|
uint32_t MEASUREMENT_WBP_HI; // 'h70
|
|
uint32_t NAV_SOLVE_PHASE_SEED; // 'h74
|
|
}gps_trck_t;
|
|
|
|
typedef enum {
|
|
GPS_ACQ_ENGINE_IDLE = 0,
|
|
GPS_ACQ_ENGINE_BUSY
|
|
} gps_acq_engine_state_t;
|
|
|
|
typedef struct {
|
|
unsigned char engine_id;
|
|
unsigned char prn;
|
|
bool acquired;
|
|
uint32_t status;
|
|
uint32_t code_phase;
|
|
uint32_t doppler;
|
|
uint32_t peak_metric;
|
|
uint32_t abs_sample;
|
|
uint64_t tick;
|
|
} gps_acq_result_t;
|
|
|
|
#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L)
|
|
_Static_assert(ACQ_ENGINE_COUNT >= 1UL, "ACQ_ENGINE_COUNT must be at least 1");
|
|
_Static_assert(GPS_EVENT_QUEUE_SIZE >= (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 1UL), "GPS event queue too small");
|
|
_Static_assert(offsetof(gps_acq_t, PRN_NUMBER) == 0x04, "gps_acq_t PRN offset mismatch");
|
|
_Static_assert(offsetof(gps_acq_t, STATUS_REG) == 0x14, "gps_acq_t STATUS offset mismatch");
|
|
_Static_assert(offsetof(gps_acq_t, DOPPLER_REG) == 0x18, "gps_acq_t DOPPLER offset mismatch");
|
|
_Static_assert(offsetof(gps_trck_t, PRN_NUMBER) == 0x04, "gps_trck_t PRN offset mismatch");
|
|
_Static_assert(offsetof(gps_trck_t, STATUS_REG) == 0x10, "gps_trck_t STATUS offset mismatch");
|
|
_Static_assert(offsetof(gps_trck_t, DOPPLER_REG) == 0x14, "gps_trck_t DOPPLER offset mismatch");
|
|
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_META) == 0x50, "gps_trck_t measurement meta offset mismatch");
|
|
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_CARRIER_Q7) == 0x58, "gps_trck_t measurement carrier offset mismatch");
|
|
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_WBP_HI) == 0x70, "gps_trck_t measurement WBP offset mismatch");
|
|
_Static_assert(offsetof(gps_trck_t, NAV_SOLVE_PHASE_SEED) == 0x74, "gps_trck_t nav solve phase seed offset mismatch");
|
|
#endif
|
|
|
|
|
|
// include gps_ip_defines.h instead of plutogpssim.h.
|
|
typedef struct {
|
|
int week; /*!< GPS week number (since January 1980) */
|
|
//unsigned int sec; /*!< second inside the GPS \a week */ //eskiden double idi C_IMP
|
|
double sec; /*!< second inside the GPS \a week */ //eskiden double idi C_IMP
|
|
} gpstime_t;
|
|
|
|
typedef struct {
|
|
int y; /*!< Calendar year */
|
|
int m; /*!< Calendar month */
|
|
int d; /*!< Calendar day */
|
|
int hh; /*!< Calendar hour */
|
|
int mm; /*!< Calendar minutes */
|
|
double sec; /*!< Calendar seconds */
|
|
} datetime_t;
|
|
|
|
|
|
/* LNAV subframe 4, page 18 ionosphere and UTC parameters. */
|
|
typedef struct {
|
|
bool enable;
|
|
bool vflg;
|
|
double alpha0, alpha1, alpha2, alpha3;
|
|
double beta0, beta1, beta2, beta3;
|
|
double A0, A1;
|
|
int dtls, tot, wnt;
|
|
int dtlsf, dn, wnlsf;
|
|
} ionoutc_t;
|
|
|
|
typedef struct{
|
|
unsigned int WN; // from subframe 1
|
|
unsigned char code; // from subframe 1
|
|
unsigned char sva; // from subframe 1
|
|
unsigned char svhlth; // from subframe 1
|
|
unsigned int iodc; // from subframe 1
|
|
unsigned char l2_data_flag; // from subframe 1
|
|
double tgd; // from subframe 1
|
|
gpstime_t tow; // tow value of last reached subframe, calculated from TOW in Handover Word.
|
|
gpstime_t toc; // from subframe 1
|
|
gpstime_t toe; // from subframe 2; .week resolved at commit
|
|
/*
|
|
* LNAV broadcasts Toe as a second-of-week only, so toe.week has to be
|
|
* reconstructed from the subframe 1 week number and the handover word
|
|
* of the subframe that carried it. Without it the curve-fit age check
|
|
* can only work modulo one week, and a record exactly N weeks old
|
|
* reads as brand new. False until that reconstruction has happened.
|
|
*/
|
|
bool toe_week_valid;
|
|
double af0; // from subframe 1
|
|
double af1; // from subframe 1
|
|
double af2; // from subframe 1
|
|
unsigned char iode; // from subframe 2
|
|
bool fit_flag; // from subframe 2 word 10: curve fit interval > 4 h.
|
|
double crs; // from subframe 2
|
|
double deltan; // from subframe 2
|
|
double m0; // from subframe 2
|
|
double cuc; // from subframe 2
|
|
double ecc; // from subframe 2
|
|
double cus; // from subframe 2
|
|
double sqrta; // from subframe 2
|
|
double A; // from subframe 2
|
|
double cic; // from subframe 3
|
|
double omgdot; // from subframe 3
|
|
double cis; // from subframe 3
|
|
double inc0; // from subframe 3
|
|
double crc; // from subframe 3
|
|
double omg0; // from subframe 3
|
|
double omgkdot; // from subframe 3
|
|
double idot; // from subframe 3
|
|
unsigned char update; // from subframe 3
|
|
bool ephemeris_valid; // true only after a matching SF1/SF2/SF3 set is committed.
|
|
unsigned char sf4_data_id; // from subframe 4 word 3.
|
|
unsigned char sf4_sv_id; // page identifier encoded in subframe 4 word 3.
|
|
unsigned char sf4_page; // decoded page number, or 0 if not mapped.
|
|
unsigned char sf5_data_id; // from subframe 5 word 3.
|
|
unsigned char sf5_sv_id; // page identifier encoded in subframe 5 word 3.
|
|
unsigned char sf5_page; // decoded page number, or 0 if not mapped.
|
|
bool almanac_ref_valid;
|
|
gpstime_t almanac_toa; // subframe 5 page 25, seconds in GPS week.
|
|
unsigned int almanac_wn; // subframe 5 page 25, modulo 256.
|
|
ionoutc_t ionoutc; // subframe 4 page 18 ionosphere/UTC data.
|
|
double abs_sample; // corrected receiver sample for pseudorange: ABS_SAMPLE - REM_CODE_PHASE_REG/CODE_FREQ_BASIS.
|
|
unsigned char SVID; // space vehicle id that is known to user that is responsible to initialize trck & acq.
|
|
}gps_ephemeris_t;
|
|
|
|
// Frame data for subframes 1..5: ephemeris comes from 1..3, ionosphere/UTC
|
|
// from subframe 4 page 18, and almanac reference time from subframe 5 page 25.
|
|
typedef struct{
|
|
gps_ephemeris_t gps_ephems; // parsed ephemeris from all subframes.
|
|
unsigned int pending_ephemeris_words[3][10]; // staged subframes 1..3.
|
|
unsigned char pending_ephemeris_mask;
|
|
}gps_frame_t;
|
|
|
|
|
|
typedef struct {
|
|
gpstime_t g;
|
|
double range; // pseudorange
|
|
double rate;
|
|
double d; // geometric distance
|
|
double azel[2];
|
|
double iono_delay;
|
|
} range_t;
|
|
|
|
// ____ _ ___ ____ _ _ ____ _ _____ _
|
|
// / ___| | / _ \| __ ) / \ | | | _ \ / \|_ _|/ \
|
|
// | | _| | | | | | _ \ / _ \ | | | | | |/ _ \ | | / _ \
|
|
// | |_| | |__| |_| | |_) / ___ \| |___ | |_| / ___ \| |/ ___ \
|
|
// \____|_____\___/|____/_/ \_\_____| |____/_/ \_\_/_/ \_\
|
|
|
|
|
|
|
|
// gps subframe storages
|
|
extern int unsigned gps_subframes[GPS_TRCK_IP_COUNT][10];
|
|
#define gps0_subframe gps_subframes[0]
|
|
#define gps1_subframe gps_subframes[1]
|
|
#define gps2_subframe gps_subframes[2]
|
|
#define gps3_subframe gps_subframes[3]
|
|
#define gps4_subframe gps_subframes[4]
|
|
#define gps5_subframe gps_subframes[5]
|
|
#define gps6_subframe gps_subframes[6]
|
|
#define gps7_subframe gps_subframes[7]
|
|
#define gps8_subframe gps_subframes[8]
|
|
|
|
extern volatile gps_acq_t* const acq0_ip; // acq ip #0 compatibility alias
|
|
extern volatile gps_acq_t* acq_ips[GPS_ACQ_IP_COUNT];
|
|
extern unsigned int acq_ips_intr_lines[GPS_ACQ_IP_COUNT];
|
|
|
|
extern volatile gps_trck_t* const trck0_ip; // trck ip #0
|
|
extern volatile gps_trck_t* const trck1_ip; // trck ip #1
|
|
extern volatile gps_trck_t* const trck2_ip; // trck ip #2
|
|
extern volatile gps_trck_t* const trck3_ip; // trck ip #3
|
|
extern volatile gps_trck_t* const trck4_ip; // trck ip #4
|
|
extern volatile gps_trck_t* const trck5_ip; // trck ip #5
|
|
extern volatile gps_trck_t* const trck6_ip; // trck ip #6
|
|
extern volatile gps_trck_t* const trck7_ip; // trck ip #7
|
|
extern volatile gps_trck_t* const trck8_ip; // trck ip #8
|
|
|
|
extern volatile gps_trck_t* const trck_ips[GPS_TRCK_IP_COUNT]; // array to store all tracking ip count.
|
|
extern unsigned int trck_ips_intr_lines[GPS_TRCK_IP_COUNT];
|
|
extern volatile uint64_t gps_systick_ticks;
|
|
|
|
// gps frame data storage parsed from all frame (sf 1 thru 5)
|
|
extern volatile gps_frame_t gps_frames_parsed[GPS_TRCK_IP_COUNT];
|
|
#define gps0_frame_parsed gps_frames_parsed[0]
|
|
#define gps1_frame_parsed gps_frames_parsed[1]
|
|
#define gps2_frame_parsed gps_frames_parsed[2]
|
|
#define gps3_frame_parsed gps_frames_parsed[3]
|
|
#define gps4_frame_parsed gps_frames_parsed[4]
|
|
#define gps5_frame_parsed gps_frames_parsed[5]
|
|
#define gps6_frame_parsed gps_frames_parsed[6]
|
|
#define gps7_frame_parsed gps_frames_parsed[7]
|
|
#define gps8_frame_parsed gps_frames_parsed[8]
|
|
|
|
// _____ _ _ _ _ ____ _____ ___ ___ _ _
|
|
// | ___| | | | \ | |/ ___|_ _|_ _/ _ \| \ | |
|
|
// | |_ | | | | \| | | | | | | | | | \| |
|
|
// | _| | |_| | |\ | |___ | | | | |_| | |\ |
|
|
// |_| \___/|_| \_|\____| |_| |___\___/|_| \_|
|
|
//
|
|
// ____ ____ ___ _____ ___ _______ ______ _____ ____
|
|
// | _ \| _ \ / _ \_ _/ _ \_ _\ \ / / _ \| ____/ ___|
|
|
// | |_) | |_) | | | || || | | || | \ V /| |_) | _| \___ \
|
|
// | __/| _ <| |_| || || |_| || | | | | __/| |___ ___) |
|
|
// |_| |_| \_\\___/ |_| \___/ |_| |_| |_| |_____|____/
|
|
|
|
void acq_enable_intr(volatile gps_acq_t* gps_handle, unsigned char intr_no);
|
|
void acq_afe_start(volatile gps_acq_t* gps_handler);
|
|
void acq_start(volatile gps_acq_t* gps_handler, unsigned char prn_no);
|
|
|
|
// trck utility control functions
|
|
void trck_enable_nav_solve(volatile gps_trck_t* trck_ip); // enables tracking ip nav solve functionality.
|
|
void trck_disable_nav_solve(volatile gps_trck_t* trck_ip); // disables tracking ip nav solve functionality.
|
|
void trck_disable_nav_solve_flag(volatile gps_trck_t* trck_ip); // disables tracking ip nav solve flag generation.
|
|
void trck_enable_nav_solve_flag(volatile gps_trck_t* trck_ip); // disables tracking ip nav solve flag generation.
|
|
void trck_enable_nav_solve_intr(volatile gps_trck_t* trck_ip); // enables tracking ip nav solve interrupt generation.
|
|
void trck_arm_nav_solve_phase_seed(volatile gps_trck_t* trck_ip,
|
|
uint32_t phase_seed); // warm-start navigation-bit phase for nav solve epochs.
|
|
bool trck_nav_solve_provisional(volatile gps_trck_t* trck_ip); // epochs run on an unconfirmed firmware seed.
|
|
bool trck_nav_solve_seed_pending(volatile gps_trck_t* trck_ip); // arm bit still set; hardware has not answered yet.
|
|
void trck_disable_nav_solve_intr(volatile gps_trck_t* trck_ip); // enables tracking ip nav solve interrupt generation.
|
|
|
|
void trck_disable_stop_on_unlock(volatile gps_trck_t* trck_ip); // enable functionality.
|
|
void trck_enable_stop_on_unlock(volatile gps_trck_t* trck_ip); // disable functionality.
|
|
void trck_enable_stop_on_unlock_flag(volatile gps_trck_t* trck_ip); // enable internal ip flag generation.
|
|
void trck_disable_stop_on_unlock_flag(volatile gps_trck_t* trck_ip); // disable internal ip flag generation.
|
|
void trck_disable_stop_on_unlock_intr(volatile gps_trck_t* trck_ip); // disable int generation due to unlock..
|
|
void trck_enable_stop_on_unlock_intr(volatile gps_trck_t* trck_ip); // enable int generation due to unnlock.
|
|
|
|
|
|
void trck_disable_nav_bits_flag(volatile gps_trck_t* trck_ip); // disable nav bits internal flag genertaion.
|
|
void trck_enable_nav_bits_flag(volatile gps_trck_t* trck_ip); // enable nav bits internal flag generation.
|
|
void trck_enable_nav_bits_intr(volatile gps_trck_t* trck_ip); // enable nav bits intr generation.
|
|
void trck_disable_nav_bits_intr(volatile gps_trck_t* trck_ip); // disable nav bits intr generation.
|
|
void trck_enable_measurement_intr(volatile gps_trck_t* trck_ip);
|
|
void trck_disable_measurement_intr(volatile gps_trck_t* trck_ip);
|
|
void trck_enable_measurement_flag(volatile gps_trck_t* trck_ip);
|
|
void trck_disable_measurement_flag(volatile gps_trck_t* trck_ip);
|
|
|
|
void trck_set_nav_solve_timer(unsigned int tick, volatile gps_trck_t* trck_ip); // in case of gps, 1 tick is 20 ms.
|
|
|
|
void trck_clear_intr_flag(volatile gps_trck_t* trck_ip); // clear tracking generic interrupt flag.
|
|
|
|
|
|
|
|
void trck_stop(volatile gps_trck_t* trck_handler);
|
|
void trck_enable_intr(volatile gps_trck_t* gps_handle, unsigned char intr_no);
|
|
void trck_disable_intr(volatile gps_trck_t* gps_handle, unsigned char intr_no);
|
|
void set_timeout(volatile gps_trck_t* gps_handle, unsigned int timeout_val);
|
|
|
|
volatile gps_acq_t* gps_get_acq_ip(unsigned int engine_id);
|
|
unsigned int gps_get_acq_intr_line(unsigned int engine_id);
|
|
bool gps_prn_valid(unsigned int prn_no);
|
|
uint64_t gps_get_systick(void);
|
|
static inline uint64_t gps_read_cycle_counter(void)
|
|
{
|
|
#ifdef __riscv
|
|
uint64_t cycles;
|
|
asm volatile("rdcycle %0" : "=r"(cycles));
|
|
return cycles;
|
|
#else
|
|
return 0u;
|
|
#endif
|
|
}
|
|
bool gps_get_ionoutc(ionoutc_t* ionoutc);
|
|
void gps_lnav_reset_ionoutc(void);
|
|
/*
|
|
* Receiver-global almanac collected from subframe 4/5 pages on every
|
|
* tracked channel. Used to predict which PRNs are above the horizon
|
|
* so acquisition does not sweep the whole constellation blindly.
|
|
*/
|
|
const gps_almanac_t* gps_lnav_get_almanac(void);
|
|
void gps_lnav_reset_almanac(void);
|
|
/*
|
|
* Raw LNAV words for the pages worth persisting, and the matching
|
|
* restore entry points. Restoring re-parses through the live decoder,
|
|
* so a corrupt or stale flash record is rejected exactly as a bad
|
|
* broadcast would be.
|
|
*/
|
|
bool gps_lnav_get_iono_page(uint32_t words_out[10]);
|
|
uint32_t gps_lnav_get_almanac_pages(
|
|
uint32_t words_out[GPS_ALMANAC_PRN_COUNT][10]);
|
|
bool gps_lnav_restore_iono_page(const unsigned int* subframe);
|
|
bool gps_lnav_restore_almanac_page(const unsigned int* subframe,
|
|
unsigned char subframe_id);
|
|
void gps_lnav_restore_almanac_reference(unsigned int week,
|
|
double toa_sec);
|
|
/*
|
|
* Last converged WLS receiver position, for consumers that need a
|
|
* coarse position rather than a fix - the almanac visibility model in
|
|
* particular. False until the first solution.
|
|
*/
|
|
bool gps_nav_last_receiver_ecef(double ecef_out[3]);
|
|
bool gps_lnav_broadcast_iode(const unsigned int* subframe,
|
|
unsigned char subframe_id,
|
|
unsigned char* iode_out);
|
|
void gps_background_task(void);
|
|
void gps_process_isr_events(void);
|
|
void gps_acq_scheduler_run(void);
|
|
// parse functions. Generic parse function.
|
|
unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
|
|
unsigned int* how_tow_sec_out);
|
|
struct gps_nav_solution;
|
|
#ifdef EKF_ENABLE
|
|
bool calculate_position(Ekf* g_ekf,
|
|
struct gps_nav_solution* solution_out,
|
|
uint64_t* measurement_timestamp_ticks_out);
|
|
#else
|
|
bool calculate_position(struct gps_nav_solution* solution_out,
|
|
uint64_t* measurement_timestamp_ticks_out);
|
|
#endif
|
|
void gps_startup_initialization(unsigned int nsp_tick);
|
|
void gps_interrupt_init(void);
|
|
void gps_irq_handler(void) GPS_IRQ_HANDLER_ATTR;
|
|
|
|
|
|
#ifdef EKF_ENABLE
|
|
void acq_start_loop(Ekf* g_ekf);
|
|
#else
|
|
void acq_start_loop();
|
|
#endif
|
|
|
|
#ifdef EKF_ENABLE
|
|
#include "ekf_wrapper.h"
|
|
#include "spi_mpu.h"
|
|
// EKF Global Variables
|
|
extern Ekf* g_ekf;
|
|
extern uint64_t g_time_now_us;
|
|
extern int g_gps_position_valid;
|
|
extern mpu6500_data_t g_imu_data;
|
|
|
|
// GPS position output (LLH)
|
|
extern double g_user_llh[3]; // lat, lon, alt
|
|
#endif
|
|
|
|
#endif
|