onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(gps): make ephemeris flash persistence hardware-safe

Browse Source 
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.
This commit is contained in:
onur
2026-08-13 00:50:57 +03:00
parent 97e3a71ab9
commit a25a1a61d0
43 changed files with 722126 additions and 713769 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/Makefile
+8 -2
View File
@@ -12,6 +12,9 @@ APP ?= hello_uart
GPS_DEBUG ?= 1
GPS_DEBUG_LEVEL ?= 1
GPS_ESKF_ENABLE ?= 1
GPS_EPHEMERIS_FLASH_ENABLE ?= 1
GPS_EPHEMERIS_FLASH_SEED_INITIAL ?= 1
GPS_NAV_CONTEXT_ENABLE ?= 1
IMU_SENSOR ?= auto
BSP_UART_CLOCK_HZ ?= 100000000
BSP_UART_BAUDRATE ?= 460800
@@ -46,6 +49,9 @@ help:
@echo " BSP_UART_BAUDRATE=$(BSP_UART_BAUDRATE)"
@echo " GPS_SYSTEM_UART_BAUDRATE=$(GPS_SYSTEM_UART_BAUDRATE)"
@echo " GPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE)"
@echo " GPS_EPHEMERIS_FLASH_ENABLE=$(GPS_EPHEMERIS_FLASH_ENABLE)"
@echo " GPS_EPHEMERIS_FLASH_SEED_INITIAL=$(GPS_EPHEMERIS_FLASH_SEED_INITIAL)"
@echo " GPS_NAV_CONTEXT_ENABLE=$(GPS_NAV_CONTEXT_ENABLE)"
@echo " IMU_SENSOR=$(IMU_SENSOR)"
@echo " BSP_CLINT_TIMEBASE_HZ=$(BSP_CLINT_TIMEBASE_HZ)"
@echo " GPS_SYSTEM_CLINT_TIMEBASE_HZ=$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)"
@@ -71,11 +77,11 @@ steval-mki217v1-test:
@make -C steval_mki217v1_test TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/steval_mki217v1_test" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
gps-system:
@make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(GPS_SYSTEM_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)" IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES="$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
@make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" GPS_EPHEMERIS_FLASH_ENABLE="$(GPS_EPHEMERIS_FLASH_ENABLE)" GPS_EPHEMERIS_FLASH_SEED_INITIAL="$(GPS_EPHEMERIS_FLASH_SEED_INITIAL)" GPS_NAV_CONTEXT_ENABLE="$(GPS_NAV_CONTEXT_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(GPS_SYSTEM_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)" IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES="$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
app:
@test -d "$(APP)"
@make -C "$(APP)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/$(APP)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
@make -C "$(APP)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/$(APP)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" GPS_EPHEMERIS_FLASH_ENABLE="$(GPS_EPHEMERIS_FLASH_ENABLE)" GPS_EPHEMERIS_FLASH_SEED_INITIAL="$(GPS_EPHEMERIS_FLASH_SEED_INITIAL)" GPS_NAV_CONTEXT_ENABLE="$(GPS_NAV_CONTEXT_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
flash-image: app
@mkdir -p "$(FLASH_DIR)"
soc/software/bsp/drivers/spi_flash.c
+199 -15
View File
@@ -3,6 +3,8 @@
#include "bsp/mmio.h"
#include "bsp/soc.h"
#include <stddef.h>
#define SPI_CTRL (BSP_SPI_BASE + 0x00u)
#define SPI_TX (BSP_SPI_BASE + 0x04u)
#define SPI_RX (BSP_SPI_BASE + 0x08u)
@@ -11,12 +13,25 @@
#define SPI_CTRL_BUSY 0x1u
#define SPI_CTRL_CSN 0x2u
static void delay_cycles(uint32_t cycles)
{
for (volatile uint32_t i = 0; i < cycles; ++i) {
asm volatile ("" ::: "memory");
}
}
#define FLASH_CMD_READ_STATUS 0x05u
#define FLASH_CMD_WRITE_DISABLE 0x04u
#define FLASH_CMD_WRITE_ENABLE 0x06u
#define FLASH_CMD_READ_CONFIG 0x35u
#define FLASH_CMD_CLEAR_STATUS 0x30u
#define FLASH_CMD_RESET 0xf0u
#define FLASH_CMD_READ_4B 0x13u
#define FLASH_CMD_PROGRAM_4B 0x12u
#define FLASH_CMD_ERASE_4B 0xdcu
#define FLASH_STATUS_WIP (1u << 0)
#define FLASH_STATUS_WEL (1u << 1)
#define FLASH_STATUS_E_ERR (1u << 5)
#define FLASH_STATUS_P_ERR (1u << 6)
#define FLASH_STATUS_ERROR (FLASH_STATUS_E_ERR | FLASH_STATUS_P_ERR)
#define FLASH_RESET_DELAY_US 300u
#define FLASH_PROGRAM_TIMEOUT_US 100000u
#define FLASH_ERASE_TIMEOUT_US 3000000u
static void spi_wait_ready(void)
{
@@ -47,6 +62,92 @@ static void flash_cmd(uint8_t cmd)
spi_select(0);
}
static void flash_delay_us(uint32_t microseconds)
{
uint64_t ticks =
((uint64_t)BSP_CLINT_TIMEBASE_HZ * microseconds + 999999u) / 1000000u;
uint64_t start = bsp_read64(BSP_CLINT_MTIME);
while ((bsp_read64(BSP_CLINT_MTIME) - start) < ticks) {
}
}
static void flash_send_address(uint32_t flash_addr)
{
(void)spi_xfer8((uint8_t)(flash_addr >> 24));
(void)spi_xfer8((uint8_t)(flash_addr >> 16));
(void)spi_xfer8((uint8_t)(flash_addr >> 8));
(void)spi_xfer8((uint8_t)flash_addr);
}
static uint8_t flash_read_register(uint8_t command)
{
uint8_t value;
spi_select(1);
(void)spi_xfer8(command);
value = spi_xfer8(0u);
spi_select(0);
return value;
}
static bool flash_range_valid(uint32_t flash_addr, uint32_t len)
{
return (flash_addr < BSP_SPI_FLASH_SIZE) &&
(len <= (BSP_SPI_FLASH_SIZE - flash_addr));
}
static bool flash_wait_complete(uint32_t timeout_us)
{
uint64_t timeout_ticks =
((uint64_t)BSP_CLINT_TIMEBASE_HZ * timeout_us + 999999u) / 1000000u;
uint64_t start = bsp_read64(BSP_CLINT_MTIME);
uint8_t status;
do {
status = bsp_spi_flash_read_status();
if ((status & FLASH_STATUS_ERROR) != 0u) {
/* S25FL-S deliberately holds WIP high after a program/erase error
* until CLSR is issued. Clear the latched failure so a later request
* can recover, while still reporting this operation as failed. */
flash_cmd(FLASH_CMD_CLEAR_STATUS);
return false;
}
if ((status & FLASH_STATUS_WIP) == 0u) {
return true;
}
} while ((bsp_read64(BSP_CLINT_MTIME) - start) < timeout_ticks);
return false;
}
static bool flash_write_enable(void)
{
flash_cmd(FLASH_CMD_CLEAR_STATUS);
flash_cmd(FLASH_CMD_WRITE_ENABLE);
return (bsp_spi_flash_read_status() &
(FLASH_STATUS_WIP | FLASH_STATUS_WEL | FLASH_STATUS_ERROR)) ==
FLASH_STATUS_WEL;
}
static bool flash_verify(uint32_t flash_addr, const uint8_t *expected,
uint32_t len)
{
uint8_t readback[BSP_SPI_FLASH_PAGE_SIZE];
while (len != 0u) {
uint32_t chunk = len > sizeof(readback) ? sizeof(readback) : len;
bsp_spi_flash_read(flash_addr, readback, chunk);
for (uint32_t i = 0u; i < chunk; ++i) {
if (readback[i] != expected[i]) {
return false;
}
}
flash_addr += chunk;
expected += chunk;
len -= chunk;
}
return true;
}
void bsp_spi_flash_init(void)
{
bsp_write32(SPI_CTRL, SPI_CTRL_CSN);
@@ -55,10 +156,8 @@ void bsp_spi_flash_init(void)
void bsp_spi_flash_reset(void)
{
spi_select(0);
flash_cmd(0x66u);
delay_cycles(1000);
flash_cmd(0x99u);
delay_cycles(200000);
flash_cmd(FLASH_CMD_RESET);
flash_delay_us(FLASH_RESET_DELAY_US);
}
uint32_t bsp_spi_flash_read_id(void)
@@ -73,18 +172,103 @@ uint32_t bsp_spi_flash_read_id(void)
return id;
}
uint8_t bsp_spi_flash_read_status(void)
{
return flash_read_register(FLASH_CMD_READ_STATUS);
}
uint8_t bsp_spi_flash_read_config(void)
{
return flash_read_register(FLASH_CMD_READ_CONFIG);
}
void bsp_spi_flash_read(uint32_t flash_addr, void *dst, uint32_t len)
{
uint8_t *out = (uint8_t *)dst;
if ((len == 0u) || (dst == NULL) || !flash_range_valid(flash_addr, len)) {
return;
}
spi_select(1);
(void)spi_xfer8(0x13u);
(void)spi_xfer8((uint8_t)(flash_addr >> 24));
(void)spi_xfer8((uint8_t)(flash_addr >> 16));
(void)spi_xfer8((uint8_t)(flash_addr >> 8));
(void)spi_xfer8((uint8_t)flash_addr);
(void)spi_xfer8(FLASH_CMD_READ_4B);
flash_send_address(flash_addr);
for (uint32_t i = 0; i < len; ++i) {
out[i] = spi_xfer8(0u);
}
spi_select(0);
}
bool bsp_spi_flash_erase_sector(uint32_t flash_addr)
{
if ((flash_addr & (BSP_SPI_FLASH_SECTOR_SIZE - 1u)) != 0u ||
!flash_range_valid(flash_addr, BSP_SPI_FLASH_SECTOR_SIZE) ||
((flash_addr >= (BSP_SPI_FLASH_SIZE - (2u * BSP_SPI_FLASH_SECTOR_SIZE))) &&
((bsp_spi_flash_read_config() & BSP_SPI_FLASH_CONFIG_TBPARM) != 0u))) {
return false;
}
if (!flash_wait_complete(FLASH_PROGRAM_TIMEOUT_US) || !flash_write_enable()) {
return false;
}
spi_select(1);
(void)spi_xfer8(FLASH_CMD_ERASE_4B);
flash_send_address(flash_addr);
spi_select(0);
if (!flash_wait_complete(FLASH_ERASE_TIMEOUT_US)) {
flash_cmd(FLASH_CMD_WRITE_DISABLE);
return false;
}
uint8_t erased[BSP_SPI_FLASH_PAGE_SIZE];
for (uint32_t i = 0u; i < sizeof(erased); ++i) {
erased[i] = 0xffu;
}
return flash_verify(flash_addr, erased, sizeof(erased)) &&
flash_verify(flash_addr + BSP_SPI_FLASH_SECTOR_SIZE - sizeof(erased),
erased, sizeof(erased));
}
bool bsp_spi_flash_program(uint32_t flash_addr, const void *src, uint32_t len)
{
const uint8_t *bytes = (const uint8_t *)src;
if (len == 0u) {
return true;
}
if ((src == NULL) || !flash_range_valid(flash_addr, len)) {
return false;
}
while (len != 0u) {
uint32_t page_remaining =
BSP_SPI_FLASH_PAGE_SIZE - (flash_addr & (BSP_SPI_FLASH_PAGE_SIZE - 1u));
uint32_t chunk = len < page_remaining ? len : page_remaining;
if (!flash_wait_complete(FLASH_PROGRAM_TIMEOUT_US) ||
!flash_write_enable()) {
return false;
}
spi_select(1);
(void)spi_xfer8(FLASH_CMD_PROGRAM_4B);
flash_send_address(flash_addr);
for (uint32_t i = 0u; i < chunk; ++i) {
(void)spi_xfer8(bytes[i]);
}
spi_select(0);
if (!flash_wait_complete(FLASH_PROGRAM_TIMEOUT_US) ||
!flash_verify(flash_addr, bytes, chunk)) {
flash_cmd(FLASH_CMD_WRITE_DISABLE);
return false;
}
flash_addr += chunk;
bytes += chunk;
len -= chunk;
}
return true;
}
soc/software/bsp/include/bsp/spi_flash.h
+11
View File
@@ -1,11 +1,22 @@
#ifndef BSP_SPI_FLASH_H
#define BSP_SPI_FLASH_H
#include <stdbool.h>
#include <stdint.h>
#define BSP_SPI_FLASH_SIZE 0x02000000UL
#define BSP_SPI_FLASH_PAGE_SIZE 256UL
#define BSP_SPI_FLASH_SECTOR_SIZE 0x00010000UL
#define BSP_SPI_FLASH_JEDEC_ID 0x00010219UL
#define BSP_SPI_FLASH_CONFIG_TBPARM (1u << 2)
void bsp_spi_flash_init(void);
void bsp_spi_flash_reset(void);
uint32_t bsp_spi_flash_read_id(void);
uint8_t bsp_spi_flash_read_status(void);
uint8_t bsp_spi_flash_read_config(void);
void bsp_spi_flash_read(uint32_t flash_addr, void *dst, uint32_t len);
bool bsp_spi_flash_erase_sector(uint32_t flash_addr);
bool bsp_spi_flash_program(uint32_t flash_addr, const void *src, uint32_t len);
#endif
soc/software/gps_system/Makefile
+4
View File
@@ -101,6 +101,8 @@ GPS_EPHEMERIS_FLASH_ENABLE ?= 1
# both sectors at boot. Both must be 0 in a production image.
GPS_EPHEMERIS_STORE_MAX_SLOTS ?= 0
GPS_EPHEMERIS_FLASH_FORCE_FORMAT ?= 0
GPS_EPHEMERIS_FLASH_SEED_INITIAL ?= 1
GPS_NAV_CONTEXT_ENABLE ?= 1
GPS_PPS_STATUS_LOG_INTERVAL ?= 10
GPS_PPS_DOPPLER_PREDICTOR_ENABLE ?= 1
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES ?= 0
@@ -120,6 +122,8 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
-DGPS_EPHEMERIS_FLASH_ENABLE=$(GPS_EPHEMERIS_FLASH_ENABLE) \
-DGPS_EPHEMERIS_STORE_MAX_SLOTS=$(GPS_EPHEMERIS_STORE_MAX_SLOTS)u \
-DGPS_EPHEMERIS_FLASH_FORCE_FORMAT=$(GPS_EPHEMERIS_FLASH_FORCE_FORMAT) \
-DGPS_EPHEMERIS_FLASH_SEED_INITIAL=$(GPS_EPHEMERIS_FLASH_SEED_INITIAL) \
-DGPS_NAV_CONTEXT_ENABLE=$(GPS_NAV_CONTEXT_ENABLE) \
-DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL) \
-DGPS_PPS_DOPPLER_PREDICTOR_ENABLE=$(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) \
-DGPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=$(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES)
soc/software/gps_system/header/gps/gps_ephemeris_store.h
+11 -1
View File
@@ -29,7 +29,8 @@
#define GPS_EPH_STORE_SLOT_SIZE 256u /* one flash page */
#define GPS_EPH_STORE_MAGIC 0x45504853u /* "EPHS" */
#define GPS_EPH_STORE_HEADER_MAGIC 0x45504844u /* "EPHD" */
#define GPS_EPH_STORE_VERSION 1u
#define GPS_EPH_STORE_VERSION 2u
#define GPS_EPH_STORE_INITIAL_SEED_COUNT 9u
typedef struct {
/* Reads never fail on NOR flash; erase/program report success. */
@@ -124,4 +125,13 @@ bool gps_ephemeris_store_save(
/* Erase both sectors and start an empty generation. */
bool gps_ephemeris_store_format(gps_ephemeris_store_t* store);
/*
* Populate a newly formatted store with structurally valid but deliberately
* stale LNAV records. These records exercise the complete flash/CRC/parser
* restore path on first boot; the normal ephemeris age gate prevents them from
* entering a position solution. Returns the number of records written.
*/
unsigned int gps_ephemeris_store_seed_initial(
gps_ephemeris_store_t* store);
#endif
soc/software/gps_system/header/gps/gps_ip_defines.h
+3
View File
@@ -161,6 +161,9 @@
#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
soc/software/gps_system/src/gps/gps_ephemeris_store.c
+103 -3
View File
@@ -17,7 +17,7 @@ typedef struct {
uint16_t record_len;
uint32_t sequence;
uint32_t toe_sec;
uint16_t week;
uint16_t toe_week;
uint16_t iodc;
uint8_t prn;
uint8_t iode;
@@ -385,6 +385,8 @@ bool gps_ephemeris_store_save(
if((store == NULL) || !store->mounted || store->capacity_exceeded ||
(ephemeris == NULL) ||
(subframe_words == NULL) || (prn >= GPS_PRN_COUNT) ||
!ephemeris->toe_week_valid || (ephemeris->toe.week < 0) ||
(ephemeris->toe.week > 0xffff) ||
(gps_ephemeris_check_static(ephemeris, prn) != GPS_EPHEMERIS_OK))
{
return false;
@@ -426,7 +428,7 @@ bool gps_ephemeris_store_save(
record.record_len = (uint16_t)sizeof(record);
record.sequence = store->sequence + 1u;
record.toe_sec = (uint32_t)ephemeris->toe.sec;
record.week = (uint16_t)ephemeris->WN;
record.toe_week = (uint16_t)ephemeris->toe.week;
record.iodc = (uint16_t)ephemeris->iodc;
record.prn = (uint8_t)prn;
record.iode = ephemeris->iode;
@@ -508,11 +510,23 @@ unsigned int gps_ephemeris_store_load_all(
break;
}
}
if(!parsed || !frame.gps_ephems.ephemeris_valid)
if(!parsed || !frame.gps_ephems.ephemeris_valid ||
(record.toe_sec != (uint32_t)frame.gps_ephems.toe.sec) ||
(record.iodc != (uint16_t)frame.gps_ephems.iodc) ||
(record.iode != frame.gps_ephems.iode) ||
(record.health != frame.gps_ephems.svhlth) ||
(record.flags != (frame.gps_ephems.fit_flag ? 1u : 0u)) ||
((record.toe_week & 0x03ffu) !=
((uint16_t)frame.gps_ephems.toe.week & 0x03ffu)))
{
store->records_rejected++;
break;
}
/* Raw LNAV only carries WN modulo 1024. Version 2 records retain
* the continuous week resolved from HOW when the live set was
* committed, so flash restore preserves the age domain. */
frame.gps_ephems.toe.week = (int)record.toe_week;
frame.gps_ephems.toe_week_valid = true;
frame.gps_ephems.SVID = (unsigned char)prn;
if(gps_ephemeris_check_static(&frame.gps_ephems, prn) !=
GPS_EPHEMERIS_OK)
@@ -527,3 +541,89 @@ unsigned int gps_ephemeris_store_load_all(
}
return delivered;
}
static void build_initial_seed_subframes(
unsigned int words[3][10], unsigned int iodc)
{
/*
* Week 380 and Toe Tuesday 00:00 are intentionally old. Keep the orbital
* fields structurally plausible so restore exercises the same decoder and
* validity rules as a live LNAV data set, while the age check guarantees
* that this diagnostic seed can never be used as current ephemeris.
*/
const unsigned int week = 380u;
const unsigned int toe_sec = 86400u;
const unsigned int how_count = toe_sec / 6u;
const unsigned int iode = iodc & 0xffu;
const uint32_t ecc_raw = (uint32_t)(0.008 * 8589934592.0);
const uint32_t sqrta_raw = (uint32_t)(5153.6 * 524288.0);
memset(words, 0, sizeof(unsigned int) * 3u * 10u);
for(unsigned int sf = 0u; sf < 3u; sf++)
{
words[sf][1] = ((how_count + sf) << 13) | ((sf + 1u) << 8);
}
words[0][2] = (week << 20) | (2u << 14) |
(((iodc >> 8) & 0x03u) << 6);
words[0][7] = ((iodc & 0xffu) << 22) | ((toe_sec >> 4) << 6);
words[1][2] = iode << 22;
words[1][5] = ((ecc_raw >> 24) & 0xffu) << 6;
words[1][6] = (ecc_raw & 0xffffffu) << 6;
words[1][7] = ((sqrta_raw >> 24) & 0xffu) << 6;
words[1][8] = (sqrta_raw & 0xffffffu) << 6;
words[1][9] = (toe_sec >> 4) << 14;
words[2][9] = iode << 22;
}
unsigned int gps_ephemeris_store_seed_initial(
gps_ephemeris_store_t* store)
{
if((store == NULL) || !store->mounted || (store->generation == 0u))
{
return 0u;
}
unsigned int seeded = 0u;
for(unsigned int prn = 0u;
prn < GPS_EPH_STORE_INITIAL_SEED_COUNT;
prn++)
{
unsigned int words[3][10];
build_initial_seed_subframes(words, 0x120u + prn);
gps_frame_t frame;
memset(&frame, 0, sizeof(frame));
frame.gps_ephems.SVID = (unsigned char)prn;
bool parsed = true;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
unsigned int how_tow = 0u;
if(parse_subframe(words[sf], &frame, &how_tow) !=
(unsigned char)(sf + 1u))
{
parsed = false;
break;
}
}
frame.gps_ephems.SVID = (unsigned char)prn;
if(!parsed || !frame.gps_ephems.ephemeris_valid ||
(gps_ephemeris_check_static(&frame.gps_ephems, prn) !=
GPS_EPHEMERIS_OK))
{
break;
}
bool wrote = false;
if(!gps_ephemeris_store_save(
store, prn, &frame.gps_ephems,
(const unsigned int (*)[10])words, &wrote) || !wrote)
{
break;
}
seeded++;
}
return seeded;
}
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+162 -34
View File
@@ -363,6 +363,40 @@ static void gps_nav_epoch_account(uint32_t expected_mask,
}
}
static bool gps_nav_channel_ephemeris_ready(unsigned int tracking_index)
{
if(tracking_index >= GPS_TRCK_IP_COUNT)
{
return false;
}
volatile gps_ephem_ll_t* node =
get_element_by_ip_number(&tracking_node,
(unsigned char)tracking_index);
if((node == NULL) || (node->data_p == NULL) ||
!node->gps_how_anchor_valid)
{
return false;
}
int time_week;
double time_sec;
if(node->gps_nav_solve_sequence != 0u)
{
time_week = node->gps_last_nav_solve_tow.week;
time_sec = node->gps_last_nav_solve_tow.sec;
}
else
{
time_week = node->gps_how_tow.week;
time_sec = node->gps_how_tow.sec;
}
unsigned int prn = trck_ips[tracking_index]->PRN_NUMBER;
return gps_ephemeris_check(
(const gps_ephemeris_t*)node->data_p, prn, true,
time_week, time_sec) == GPS_EPHEMERIS_OK;
}
static uint32_t gps_nav_ready_channel_mask(void)
{
uint32_t eligible_mask = 0u;
@@ -380,7 +414,7 @@ static uint32_t gps_nav_ready_channel_mask(void)
* an unproven seed cannot hold up a coalescing epoch; it joins as soon
* as its first epoch validates. */
if(!tracking_allocated[i] || !busy || !locked || !nav_irq_enabled ||
trk_seed_unconfirmed[i])
trk_seed_unconfirmed[i] || !gps_nav_channel_ephemeris_ready(i))
{
continue;
}
@@ -408,7 +442,7 @@ static uint32_t gps_nav_ready_channel_mask(void)
return eligible_mask;
}
#if (GPS_EPHEMERIS_FLASH_ENABLE == 1)
#if ((GPS_EPHEMERIS_FLASH_ENABLE == 1) || (GPS_NAV_CONTEXT_ENABLE == 1))
static void gps_flash_read(void* ctx, uint32_t address, void* dst,
uint32_t len)
{
@@ -429,6 +463,28 @@ static bool gps_flash_program(void* ctx, uint32_t address, const void* src,
return bsp_spi_flash_program(address, src, len);
}
static bool gps_flash_device_init(void)
{
bsp_spi_flash_init();
bsp_spi_flash_reset();
uint32_t id = bsp_spi_flash_read_id();
uint8_t status = bsp_spi_flash_read_status();
uint8_t config = bsp_spi_flash_read_config();
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
"SPI flash: id=0x%06lx status=0x%02x config=0x%02x\r\n",
(unsigned long)id, status, config);
if(id != BSP_SPI_FLASH_JEDEC_ID)
{
GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_NAV,
"unsupported SPI flash JEDEC id=0x%06lx expected=0x%06lx\r\n",
(unsigned long)id,
(unsigned long)BSP_SPI_FLASH_JEDEC_ID);
return false;
}
return true;
}
#if (GPS_EPHEMERIS_FLASH_ENABLE == 1)
static void gps_ephemeris_flash_loaded(void* ctx, unsigned int prn,
const gps_ephemeris_t* ephemeris)
{
@@ -450,12 +506,14 @@ static void gps_ephemeris_flash_loaded(void* ctx, unsigned int prn,
prn, ephemeris->iode, ephemeris->toe.sec);
}
}
#endif
/*
* Persistent navigation context: almanac, ionosphere model, last position.
* Kept separate from the ephemeris store because it has a different lifetime
* (weeks, not hours) and a different shape (one small blob, not a per-PRN log).
*/
#if (GPS_NAV_CONTEXT_ENABLE == 1)
static bool gps_receiver_time_now(int* week_out, double* tow_sec_out);
static gps_nav_context_store_t gps_nav_context_store;
@@ -696,6 +754,9 @@ static void gps_nav_context_flash_service(void)
}
}
#endif /* GPS_NAV_CONTEXT_ENABLE */
#if (GPS_EPHEMERIS_FLASH_ENABLE == 1)
static void gps_ephemeris_flash_init(void)
{
gps_ephemeris_store_flash_t flash;
@@ -716,14 +777,18 @@ static void gps_ephemeris_flash_init(void)
"persistent ephemeris store unavailable\r\n");
return;
}
if((bsp_spi_flash_read_config() & BSP_SPI_FLASH_CONFIG_TBPARM) != 0u)
{
gps_ephemeris_store_ready = false;
gps_runtime_diagnostics.flash_ephemeris_error_total++;
GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_NAV,
"ephemeris flash disabled: CR1.TBPARM places 4 KiB sectors at the top\r\n");
return;
}
#if (GPS_EPHEMERIS_STORE_MAX_SLOTS != 0u)
gps_ephemeris_store_limit_slots(&gps_ephemeris_store,
GPS_EPHEMERIS_STORE_MAX_SLOTS);
#endif
unsigned int loaded = gps_ephemeris_store_load_all(
&gps_ephemeris_store, gps_ephemeris_flash_loaded, NULL);
gps_runtime_diagnostics.flash_ephemeris_loaded_total = loaded;
/*
* A blank or unreadable store needs two sector erases before it can hold
* anything. Spend them here, during init, rather than letting the first
@@ -744,8 +809,35 @@ static void gps_ephemeris_flash_init(void)
gps_runtime_diagnostics.flash_ephemeris_error_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"persistent ephemeris store format failed; persistence disabled\r\n");
return;
}
}
unsigned int loaded = gps_ephemeris_store_load_all(
&gps_ephemeris_store, gps_ephemeris_flash_loaded, NULL);
#if (GPS_EPHEMERIS_FLASH_SEED_INITIAL == 1)
if(loaded == 0u)
{
unsigned int seeded =
gps_ephemeris_store_seed_initial(&gps_ephemeris_store);
if(seeded != GPS_EPH_STORE_INITIAL_SEED_COUNT)
{
gps_runtime_diagnostics.flash_ephemeris_error_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"initial stale ephemeris seed incomplete: wrote=%u expected=%u\r\n",
seeded, GPS_EPH_STORE_INITIAL_SEED_COUNT);
}
else
{
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
"initial stale ephemeris seed written: records=%u\r\n",
seeded);
}
loaded = gps_ephemeris_store_load_all(
&gps_ephemeris_store, gps_ephemeris_flash_loaded, NULL);
}
#endif
gps_runtime_diagnostics.flash_ephemeris_loaded_total = loaded;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"persistent ephemeris store mounted: generation=%u loaded=%u rejected=%u\r\n",
gps_ephemeris_store.generation, loaded,
@@ -831,6 +923,7 @@ static void gps_ephemeris_flash_queue(unsigned int prn,
gps_ephemeris_flash_pending_mask |= (1u << prn);
}
#endif /* GPS_EPHEMERIS_FLASH_ENABLE */
#endif /* GPS_EPHEMERIS_FLASH_ENABLE || GPS_NAV_CONTEXT_ENABLE */
static void gps_prn_context_store(unsigned int prn,
int week,
@@ -1065,6 +1158,7 @@ static void gps_refresh_acq_visibility(void)
* retried slowly, and it self-corrects the moment a real fix replaces
* the seed.
*/
#if (GPS_NAV_CONTEXT_ENABLE == 1)
if(!gps_nav_context_position_seed_valid)
{
gps_acq_manager_set_search_mask(&gps_acq_manager, 0xffffffffu);
@@ -1086,6 +1180,10 @@ static void gps_refresh_acq_visibility(void)
* back to its normal value.
*/
mask_elevation_deg = GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG;
#else
gps_acq_manager_set_search_mask(&gps_acq_manager, 0xffffffffu);
return;
#endif
}
uint32_t mask = 0xffffffffu;
@@ -1293,10 +1391,19 @@ static void gps_runtime_state_init(void)
gps_ephemeris_cache_init(
&gps_ephemeris_cache,
GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS);
#if ((GPS_EPHEMERIS_FLASH_ENABLE == 1) || (GPS_NAV_CONTEXT_ENABLE == 1))
bool gps_flash_ready = gps_flash_device_init();
#if (GPS_EPHEMERIS_FLASH_ENABLE == 1)
gps_ephemeris_flash_init();
if(gps_flash_ready)
{
gps_ephemeris_flash_init();
}
#endif
#if (GPS_NAV_CONTEXT_ENABLE == 1)
gps_nav_context_flash_init();
if(gps_flash_ready)
{
gps_nav_context_flash_init();
}
#endif
#endif
@@ -2147,32 +2254,10 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
unsigned char subframe_id = parse_subframe((unsigned int*)gps_subframe,
(gps_frame_t*)gps_frame_parsed,
&how_tow_sec);
if((subframe_id >= 1u) && (subframe_id <= 3u) &&
(gps_frame_parsed->pending_ephemeris_mask == 0u))
{
bool ephemeris_changed = false;
if(gps_ephemeris_cache_store_if_new(
&gps_ephemeris_cache, event->prn,
(const gps_ephemeris_t*)&gps_frame_parsed->gps_ephems,
event->tick, &ephemeris_changed) &&
ephemeris_changed)
{
gps_runtime_diagnostics.ephemeris_cache_store_total++;
#if (GPS_EPHEMERIS_FLASH_ENABLE == 1)
/* Only a genuinely new data set reaches flash; the same
* broadcast decoded again is filtered out above. */
gps_ephemeris_flash_queue(
event->prn,
(const unsigned int (*)[10])
gps_frame_parsed->pending_ephemeris_words);
#endif
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"stored ephemeris cache: prn=%u trk=%u iode=%u toe=%.0f\r\n",
event->prn, trck_ip_no,
gps_frame_parsed->gps_ephems.iode,
gps_frame_parsed->gps_ephems.toe.sec);
}
}
bool ephemeris_committed =
(subframe_id >= 1u) && (subframe_id <= 3u) &&
(gps_frame_parsed->pending_ephemeris_mask == 0u);
bool how_anchor_accepted = false;
if((subframe_id >= 1u) && (subframe_id <= 5u) &&
(how_tow_sec < (unsigned int)GPS_TIME_WEEK_SECONDS))
{
@@ -2226,6 +2311,19 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
(double)how_tow_sec, event,
node->gps_how_week_valid ||
(subframe_id == 1u));
/* The decoder can only reconstruct Toe in the broadcast's
* modulo-1024 week domain. Once HOW has supplied the receiver's
* continuous week, lift Toe into that same domain before the
* curve-fit age check compares them. */
if(gps_frame_parsed->gps_ephems.toe_week_valid)
{
gps_frame_parsed->gps_ephems.toe.week =
gps_time_extend_lnav_week(
true, anchor_week,
(unsigned int)gps_frame_parsed->gps_ephems.toe.week);
}
gps_frame_parsed->gps_ephems.tow.week = anchor_week;
how_anchor_accepted = true;
GPS_LOG(recovered_anchor ? DEBUG_WARNING : DEBUG_TRACE,
GPS_LOG_CAT_NAV,
"%s HOW anchor: trk=%u prn=%u week=%d tow=%.3f abs=%u corrected=%.6f\r\n",
@@ -2258,6 +2356,36 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
trck_ip_no, subframe_id, how_tow_sec);
}
/* Cache only after Toe has been lifted from WN modulo 1024 into the HOW
* anchor's continuous week. Flash persists that full week; storing before
* this point makes a fresh record appear 1024 or 2048 weeks old at boot. */
if(ephemeris_committed && how_anchor_accepted)
{
bool ephemeris_changed = false;
if(gps_ephemeris_cache_store_if_new(
&gps_ephemeris_cache, event->prn,
(const gps_ephemeris_t*)&gps_frame_parsed->gps_ephems,
event->tick, &ephemeris_changed) &&
ephemeris_changed)
{
gps_runtime_diagnostics.ephemeris_cache_store_total++;
#if (GPS_EPHEMERIS_FLASH_ENABLE == 1)
/* Only a genuinely new data set reaches flash; the same
* broadcast decoded again is filtered out above. */
gps_ephemeris_flash_queue(
event->prn,
(const unsigned int (*)[10])
gps_frame_parsed->pending_ephemeris_words);
#endif
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"stored ephemeris cache: prn=%u trk=%u iode=%u toe=%d/%.0f\r\n",
event->prn, trck_ip_no,
gps_frame_parsed->gps_ephems.iode,
gps_frame_parsed->gps_ephems.toe.week,
gps_frame_parsed->gps_ephems.toe.sec);
}
}
gps_mark_node_fresh(node, event->status, event->tick);
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_TRK, "nav bits: trk=%u prn=%u mask=0x%x status=0x%x\r\n",
soc/software/gps_system/src/gps/gps_ip.c
+19 -10
View File
@@ -171,14 +171,6 @@ static bool gps_node_has_committed_ephemeris(volatile gps_ephem_ll_t* node)
node->data_p->ephemeris_valid;
}
static bool gps_ephemeris_ready_for_position(
const gps_ephemeris_t* eph,
unsigned int expected_svid)
{
return gps_ephemeris_check_static(eph, expected_svid) ==
GPS_EPHEMERIS_OK;
}
/*
* Same structural rules plus the IS-GPS-200 curve fit window evaluated at the
* epoch's own transmit time. This is what keeps a cached or flash-restored
@@ -229,9 +221,26 @@ static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
return false;
}
return gps_ephemeris_ready_for_position(
if(!node->gps_how_anchor_valid)
{
return false;
}
int time_week;
double time_sec;
if(node->gps_nav_solve_sequence != 0u)
{
time_week = node->gps_last_nav_solve_tow.week;
time_sec = node->gps_last_nav_solve_tow.sec;
}
else
{
time_week = node->gps_how_tow.week;
time_sec = node->gps_how_tow.sec;
}
return gps_ephemeris_check(
(const gps_ephemeris_t*)node->data_p,
trck_ips[node->tracking_ip_no]->PRN_NUMBER);
trck_ips[node->tracking_ip_no]->PRN_NUMBER,
true, time_week, time_sec) == GPS_EPHEMERIS_OK;
}
static bool gps_channel_nav_solve_enabled(unsigned char trck_ip_no)
soc/software/gps_system/tests/test_gps_ephemeris_store.c
+82
View File
@@ -167,6 +167,7 @@ typedef struct {
unsigned int prn[GPS_PRN_COUNT];
unsigned char iode[GPS_PRN_COUNT];
double toe[GPS_PRN_COUNT];
gps_ephemeris_t ephemeris[GPS_PRN_COUNT];
} load_result_t;
static void load_handler(void* ctx, unsigned int prn,
@@ -176,6 +177,7 @@ static void load_handler(void* ctx, unsigned int prn,
result->prn[result->count] = prn;
result->iode[result->count] = ephemeris->iode;
result->toe[result->count] = ephemeris->toe.sec;
result->ephemeris[result->count] = *ephemeris;
result->count++;
}
@@ -208,6 +210,84 @@ static bool save_one(gps_ephemeris_store_t* store, unsigned int prn,
/* ------------------------------------------------------------------ */
static void initial_seed_populates_a_blank_store_with_stale_records(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "seed mount failed");
CHECK(gps_ephemeris_store_format(&store), "seed format failed");
CHECK(gps_ephemeris_store_seed_initial(&store) ==
GPS_EPH_STORE_INITIAL_SEED_COUNT,
"initial seed did not write every record");
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
"seed remount failed");
load_result_t result;
memset(&result, 0, sizeof(result));
CHECK(gps_ephemeris_store_load_all(&remounted, load_handler, &result) ==
GPS_EPH_STORE_INITIAL_SEED_COUNT,
"initial seed did not survive remount");
for(unsigned int i = 0u; i < result.count; i++)
{
CHECK(result.prn[i] == i, "initial seed PRN order is wrong");
CHECK(result.toe[i] == 86400.0, "initial seed Toe is wrong");
CHECK(gps_ephemeris_check(&result.ephemeris[i], i, true,
2431, 86400.0) ==
GPS_EPHEMERIS_REJECT_AGE,
"initial diagnostic seed was not stale");
}
}
static void continuous_toe_week_survives_flash_round_trip(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface),
"full-week mount failed");
CHECK(gps_ephemeris_store_format(&store),
"full-week format failed");
unsigned int words[3][10];
build_subframes(words, 0x121u, 86400u, 0u);
gps_frame_t frame;
memset(&frame, 0, sizeof(frame));
frame.gps_ephems.SVID = 4u;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
unsigned int how = 0u;
(void)parse_subframe(words[sf], &frame, &how);
}
frame.gps_ephems.SVID = 4u;
frame.gps_ephems.toe.week = 2428;
frame.gps_ephems.toe_week_valid = true;
bool wrote = false;
CHECK(gps_ephemeris_store_save(
&store, 4u, &frame.gps_ephems,
(const unsigned int (*)[10])words, &wrote) && wrote,
"full-week record was not written");
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
"full-week remount failed");
load_result_t result;
memset(&result, 0, sizeof(result));
CHECK(gps_ephemeris_store_load_all(&remounted, load_handler, &result) == 1u,
"full-week record was not restored");
CHECK(result.ephemeris[0].toe.week == 2428,
"continuous Toe week was truncated during flash restore");
CHECK(gps_ephemeris_check(&result.ephemeris[0], 4u, true,
2428, 86400.0) == GPS_EPHEMERIS_OK,
"restored full-week record is not valid in its own week");
CHECK(gps_ephemeris_check(&result.ephemeris[0], 4u, true,
2431, 86400.0) == GPS_EPHEMERIS_REJECT_AGE,
"restored full-week record lost its multi-week age");
}
static void empty_flash_mounts_and_first_save_formats(void)
{
sim_flash_t flash;
@@ -644,6 +724,8 @@ static void compaction_that_cannot_fit_latches_instead_of_eroding_flash(void)
int main(void)
{
crc_detects_single_bit_corruption();
initial_seed_populates_a_blank_store_with_stale_records();
continuous_toe_week_survives_flash_round_trip();
compaction_that_cannot_fit_latches_instead_of_eroding_flash();
slot_cap_forces_compaction_on_a_real_sector();
empty_flash_mounts_and_first_save_formats();
soc/software/gps_system/tests/test_gps_time_anchor.c
+4
View File
@@ -50,6 +50,10 @@ static void week_rollover_is_consistent(void)
"previous modulo week was not extended backward");
CHECK(gps_time_extend_lnav_week(false, 0, 380u) == 380,
"initial LNAV week was altered");
CHECK(gps_time_extend_lnav_week(true, 2431, 383u) == 2431,
"modulo-1024 ephemeris week was not lifted to the HOW week");
CHECK(gps_time_extend_lnav_week(true, 2431, 384u) == 2432,
"Toe in the following full week was not preserved");
}
static void temporary_week_can_upgrade_on_first_subframe_one(void)