feat(gps): expose Doppler and signal quality measurements

2026-07-25 11:30:38 +03:00
parent 64860d1ced
commit b2ca7db633
26 changed files with 2042 additions and 59 deletions
@@ -0,0 +1,179 @@
+#include "gps/gps_tracking_measurement.h"
+
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#define TEST_SATELLITES 6u
+
+static bool close_to(double actual, double expected, double tolerance)
+{
+	return fabs(actual - expected) <= tolerance;
+}
+
+static uint64_t cn0_wbp_for_target(
+	int32_t sum_i,
+	int32_t sum_q,
+	unsigned int sample_count,
+	double cn0_dbhz)
+{
+	double cn0_linear = pow(10.0, cn0_dbhz / 10.0);
+	double scaled = cn0_linear / 1000.0;
+	double ratio =
+		(scaled * (double)sample_count + 1.0) / (scaled + 1.0);
+	double narrowband =
+		(double)sum_i * (double)sum_i +
+		(double)sum_q * (double)sum_q;
+	return (uint64_t)llround(narrowband / ratio);
+}
+
+static void test_cn0_and_mailbox_decode(void)
+{
+	gps_trck_t registers;
+	memset(&registers, 0, sizeof(registers));
+
+	const int32_t sum_i = 20000;
+	const int32_t sum_q = 1000;
+	const uint64_t wbp =
+		cn0_wbp_for_target(sum_i, sum_q, 20u, 45.0);
+	const int32_t carrier_q7 =
+		(int32_t)llround((GPS_TRCK_IF_CENTER_HZ + 2500.0) *
+		                GPS_TRCK_CARRIER_Q7_SCALE);
+
+	registers.PRN_NUMBER = 7u;
+	registers.MEASUREMENT_META =
+		(GPS_TRCK_MEAS_META_VERSION <<
+		 GPS_TRCK_MEAS_META_VERSION_SHIFT) |
+		(20u << GPS_TRCK_MEAS_META_COUNT_SHIFT) |
+		(1u << GPS_TRCK_MEAS_META_VALID_POS) |
+		(1u << GPS_TRCK_MEAS_META_NAV_BIT_POS) |
+		(1u << GPS_TRCK_MEAS_META_LOCK_POS);
+	registers.MEASUREMENT_SEQUENCE = 0x1234u;
+	registers.MEASUREMENT_CARRIER_Q7 = carrier_q7;
+	registers.MEASUREMENT_ABS_SAMPLE = 0x89abcdefu;
+	registers.MEASUREMENT_REM_CODE_PHASE = 555u;
+	registers.MEASUREMENT_SUM_I = sum_i;
+	registers.MEASUREMENT_SUM_Q = sum_q;
+	registers.MEASUREMENT_WBP_LO = (uint32_t)wbp;
+	registers.MEASUREMENT_WBP_HI = (uint32_t)(wbp >> 32);
+
+	gps_tracking_measurement_t measurement;
+	assert(gps_tracking_measurement_read(&registers, &measurement));
+	assert(measurement.valid);
+	assert(measurement.locked);
+	assert(measurement.nav_bit);
+	assert(measurement.prn == 7u);
+	assert(measurement.sequence == 0x1234u);
+	assert(measurement.absolute_sample == 0x89abcdefu);
+	assert(measurement.remaining_code_phase == 555u);
+	assert(measurement.cn0_sample_count == 20u);
+	assert(measurement.cn0_wbp == wbp);
+	assert(close_to(measurement.cn0_dbhz, 45.0, 0.01));
+	assert(close_to(measurement.doppler_hz, 2500.0, 1.0 / 128.0));
+
+	const double expected_range_rate =
+		-2500.0 * ((double)SPEED_OF_LIGHT / GPS_L1_CARRIER_HZ);
+	assert(close_to(
+		measurement.pseudorange_rate_mps,
+		expected_range_rate,
+		1.0e-9));
+
+	registers.MEASUREMENT_META &=
+		~(1u << GPS_TRCK_MEAS_META_LOCK_POS);
+	assert(!gps_tracking_measurement_read(&registers, &measurement));
+	assert(!measurement.valid);
+}
+
+static void test_velocity_wls(void)
+{
+	const double receiver_position[3] = {6378137.0, 0.0, 0.0};
+	const double directions[TEST_SATELLITES][3] = {
+		{ 1.0,  0.0,  0.0},
+		{ 0.0,  1.0,  0.0},
+		{ 0.0,  0.0,  1.0},
+		{-0.5773502691896258,  0.5773502691896258,  0.5773502691896258},
+		{ 0.7071067811865475, -0.7071067811865475,  0.0},
+		{ 0.4082482904638631,  0.4082482904638631, -0.8164965809277261}
+	};
+	const double satellite_velocities[TEST_SATELLITES][3] = {
+		{  0.0,  2700.0,  100.0},
+		{-900.0,     0.0, 2500.0},
+		{1200.0, -2100.0,    0.0},
+		{1800.0,   400.0, -800.0},
+		{-500.0,  1900.0,  600.0},
+		{ 700.0, -1200.0, 2200.0}
+	};
+	const double satellite_clock_drift[TEST_SATELLITES] = {
+		1.0e-11, -2.0e-11, 0.5e-11, 1.5e-11, -0.7e-11, 2.2e-11
+	};
+	const double expected_velocity[3] = {12.0, -4.0, 1.5};
+	const double expected_clock_drift_mps = 18.0;
+
+	double satellite_positions[TEST_SATELLITES][3];
+	gps_tracking_measurement_t measurements[TEST_SATELLITES];
+	memset(measurements, 0, sizeof(measurements));
+	for(unsigned int satellite = 0u;
+	    satellite < TEST_SATELLITES;
+	    satellite++)
+	{
+		double los_satellite_velocity = 0.0;
+		double los_receiver_velocity = 0.0;
+		for(unsigned int component = 0u; component < 3u; component++)
+		{
+			satellite_positions[satellite][component] =
+				receiver_position[component] +
+				directions[satellite][component] * 21000000.0;
+			los_satellite_velocity +=
+				directions[satellite][component] *
+				satellite_velocities[satellite][component];
+			los_receiver_velocity +=
+				directions[satellite][component] *
+				expected_velocity[component];
+		}
+		measurements[satellite].valid = true;
+		measurements[satellite].locked = true;
+		measurements[satellite].cn0_dbhz = 42.0 + (double)satellite;
+		measurements[satellite].pseudorange_rate_mps =
+			los_satellite_velocity -
+			los_receiver_velocity +
+			expected_clock_drift_mps -
+			(double)SPEED_OF_LIGHT *
+			satellite_clock_drift[satellite];
+	}
+
+	double solved_velocity[3];
+	double solved_clock_drift;
+	double residual_rms;
+	assert(gps_tracking_velocity_wls(
+		receiver_position,
+		satellite_positions,
+		satellite_velocities,
+		satellite_clock_drift,
+		measurements,
+		TEST_SATELLITES,
+		solved_velocity,
+		&solved_clock_drift,
+		&residual_rms));
+
+	for(unsigned int component = 0u; component < 3u; component++)
+	{
+		assert(close_to(
+			solved_velocity[component],
+			expected_velocity[component],
+			1.0e-9));
+	}
+	assert(close_to(
+		solved_clock_drift,
+		expected_clock_drift_mps,
+		1.0e-9));
+	assert(residual_rms < 1.0e-9);
+}
+
+int main(void)
+{
+	test_cn0_and_mailbox_decode();
+	test_velocity_wls();
+	puts("test_gps_tracking_measurement PASS");
+	return 0;
+}