feat(gps): expose Doppler and signal quality measurements
This commit is contained in:
@@ -6,6 +6,8 @@ TARGETS := test_imu_calibration test_gps_nav_epoch \
|
||||
test_gps_nav_epoch_coalescing test_gps_time_anchor \
|
||||
test_plic_warm_reset test_gps_acq_manager \
|
||||
test_gps_ephemeris_cache test_gps_receiver_hardware_sim
|
||||
TARGETS += test_gps_tracking_measurement
|
||||
TARGETS += test_gps_satellite_velocity
|
||||
|
||||
.PHONY: all run sanitize clean
|
||||
|
||||
@@ -37,6 +39,17 @@ test_gps_receiver_hardware_sim: test_gps_receiver_hardware_sim.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
|
||||
|
||||
test_gps_tracking_measurement: test_gps_tracking_measurement.c \
|
||||
$(ROOT)/src/gps/gps_tracking_measurement.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_satellite_velocity: test_gps_satellite_velocity.c \
|
||||
$(ROOT)/src/gps/NavEquations.c \
|
||||
$(ROOT)/src/gps/matrixLib.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Wno-unused-parameter \
|
||||
-Wno-unused-variable -Wno-sign-compare \
|
||||
-Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
run: $(TARGETS)
|
||||
./test_imu_calibration
|
||||
./test_gps_nav_epoch
|
||||
@@ -46,6 +59,8 @@ run: $(TARGETS)
|
||||
./test_gps_acq_manager
|
||||
./test_gps_ephemeris_cache
|
||||
./test_gps_receiver_hardware_sim
|
||||
./test_gps_tracking_measurement
|
||||
./test_gps_satellite_velocity
|
||||
|
||||
sanitize:
|
||||
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
|
||||
@@ -87,9 +102,31 @@ sanitize:
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 \
|
||||
./test_gps_receiver_hardware_sim_san
|
||||
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
|
||||
test_gps_tracking_measurement.c \
|
||||
$(ROOT)/src/gps/gps_tracking_measurement.c -lm \
|
||||
-o test_gps_tracking_measurement_san
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 \
|
||||
./test_gps_tracking_measurement_san
|
||||
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-Wno-comment -Wno-unused-parameter -Wno-unused-variable \
|
||||
-Wno-sign-compare -Ifake_bsp -I$(ROOT)/header \
|
||||
test_gps_satellite_velocity.c \
|
||||
$(ROOT)/src/gps/NavEquations.c \
|
||||
$(ROOT)/src/gps/matrixLib.c -lm \
|
||||
-o test_gps_satellite_velocity_san
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 \
|
||||
./test_gps_satellite_velocity_san
|
||||
|
||||
clean:
|
||||
rm -f $(TARGETS) test_gps_nav_epoch_coalescing_repro \
|
||||
test_gps_nav_epoch_san test_gps_nav_epoch_coalescing_san \
|
||||
test_gps_acq_manager_san test_gps_ephemeris_cache_san \
|
||||
test_gps_receiver_hardware_sim_san
|
||||
test_gps_receiver_hardware_sim_san \
|
||||
test_gps_tracking_measurement_san \
|
||||
test_gps_satellite_velocity_san
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
#include "gps/NavEquations.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static void test_broadcast_ephemeris_velocity(void)
|
||||
{
|
||||
gps_ephemeris_t ephemeris;
|
||||
memset(&ephemeris, 0, sizeof(ephemeris));
|
||||
ephemeris.tow.week = 2400;
|
||||
ephemeris.tow.sec = 345700.0;
|
||||
ephemeris.toe.week = 2400;
|
||||
ephemeris.toe.sec = 345600.0;
|
||||
ephemeris.toc.week = 2400;
|
||||
ephemeris.toc.sec = 345600.0;
|
||||
ephemeris.sqrta = 5153.7954775;
|
||||
ephemeris.A = ephemeris.sqrta * ephemeris.sqrta;
|
||||
ephemeris.ecc = 0.0093;
|
||||
ephemeris.m0 = 0.75;
|
||||
ephemeris.deltan = 4.5e-9;
|
||||
ephemeris.cuc = 1.0e-6;
|
||||
ephemeris.cus = -3.0e-6;
|
||||
ephemeris.crc = 180.0;
|
||||
ephemeris.crs = -75.0;
|
||||
ephemeris.cic = 1.2e-7;
|
||||
ephemeris.cis = -8.0e-8;
|
||||
ephemeris.inc0 = 0.96;
|
||||
ephemeris.idot = -2.0e-10;
|
||||
/* Legacy names: omgdot=Omega0, omg0=omega, omgkdot=Omega-dot. */
|
||||
ephemeris.omgdot = 1.1;
|
||||
ephemeris.omg0 = -0.4;
|
||||
ephemeris.omgkdot = -8.0e-9;
|
||||
ephemeris.af0 = 2.3e-5;
|
||||
ephemeris.af1 = -4.0e-12;
|
||||
ephemeris.af2 = 1.0e-19;
|
||||
ephemeris.tgd = -2.0e-9;
|
||||
|
||||
double position[3];
|
||||
double velocity[3];
|
||||
double clock[2];
|
||||
double correction;
|
||||
satpos(&ephemeris, position, velocity, clock, &correction);
|
||||
|
||||
const double dt = 0.05;
|
||||
gps_ephemeris_t before = ephemeris;
|
||||
gps_ephemeris_t after = ephemeris;
|
||||
before.tow.sec -= dt;
|
||||
after.tow.sec += dt;
|
||||
double position_before[3], position_after[3];
|
||||
double velocity_unused[3], clock_before[2], clock_after[2];
|
||||
double correction_before, correction_after;
|
||||
satpos(&before, position_before, velocity_unused,
|
||||
clock_before, &correction_before);
|
||||
satpos(&after, position_after, velocity_unused,
|
||||
clock_after, &correction_after);
|
||||
|
||||
double radius_squared = 0.0;
|
||||
double speed_squared = 0.0;
|
||||
for(unsigned int component = 0u; component < 3u; component++)
|
||||
{
|
||||
double finite_difference =
|
||||
(position_after[component] - position_before[component]) /
|
||||
(2.0 * dt);
|
||||
assert(fabs(velocity[component] - finite_difference) < 0.05);
|
||||
radius_squared += position[component] * position[component];
|
||||
speed_squared += velocity[component] * velocity[component];
|
||||
}
|
||||
double finite_clock_drift =
|
||||
(correction_after - correction_before) / (2.0 * dt);
|
||||
assert(fabs(clock[1] - finite_clock_drift) < 1.0e-13);
|
||||
assert(fabs(clock[0] - correction) < 1.0e-18);
|
||||
assert(sqrt(radius_squared) > 25000000.0);
|
||||
assert(sqrt(radius_squared) < 28000000.0);
|
||||
assert(sqrt(speed_squared) > 1000.0);
|
||||
assert(sqrt(speed_squared) < 5000.0);
|
||||
}
|
||||
|
||||
static void test_cn0_changes_position_weight(void)
|
||||
{
|
||||
const unsigned int satellite_count = 6u;
|
||||
const double receiver[3] = {6378137.0, 0.0, 0.0};
|
||||
const double directions[6][3] = {
|
||||
{ 1.0, 0.0, 0.0},
|
||||
{ 0.2, 0.9, 0.3872983346207417},
|
||||
{ 0.3, -0.4, 0.8660254037844386},
|
||||
{-0.2, 0.5, 0.8426149773176358},
|
||||
{ 0.4, -0.8, -0.4472135954999579},
|
||||
{-0.5, -0.5, 0.7071067811865475}
|
||||
};
|
||||
mat_type* satellite_positions =
|
||||
create_matrix((int)satellite_count, 3, "testSatPos");
|
||||
mat_type* observations =
|
||||
create_matrix(1, (int)satellite_count, "testObs");
|
||||
mat_type* az = create_matrix(1, (int)satellite_count, "testAz");
|
||||
mat_type* el = create_matrix(1, (int)satellite_count, "testEl");
|
||||
assert(satellite_positions != NULL);
|
||||
assert(observations != NULL);
|
||||
assert(az != NULL);
|
||||
assert(el != NULL);
|
||||
initialize_all(0.0, satellite_positions);
|
||||
initialize_all(0.0, observations);
|
||||
initialize_all(0.0, az);
|
||||
initialize_all(0.0, el);
|
||||
|
||||
unsigned char ura[6] = {0u, 0u, 0u, 0u, 0u, 0u};
|
||||
double cn0_dbhz[6] = {52.0, 22.0, 45.0, 45.0, 45.0, 45.0};
|
||||
for(unsigned int satellite = 0u;
|
||||
satellite < satellite_count;
|
||||
satellite++)
|
||||
{
|
||||
double range_squared = 0.0;
|
||||
for(unsigned int component = 0u; component < 3u; component++)
|
||||
{
|
||||
satellite_positions->matrix[satellite][component] =
|
||||
receiver[component] +
|
||||
directions[satellite][component] * 21000000.0;
|
||||
double difference =
|
||||
satellite_positions->matrix[satellite][component] -
|
||||
receiver[component];
|
||||
range_squared += difference * difference;
|
||||
}
|
||||
observations->matrix[0][satellite] =
|
||||
sqrt(range_squared) + 125.0;
|
||||
}
|
||||
|
||||
gps_nav_solution_t solution;
|
||||
assert(leastSquaresSTD(
|
||||
satellite_positions, observations, (int)satellite_count,
|
||||
az, el, ura, cn0_dbhz, NULL, 345700.0, &solution));
|
||||
assert(isfinite(solution.measurement_sigma_m[0]));
|
||||
assert(isfinite(solution.measurement_sigma_m[1]));
|
||||
assert(solution.measurement_sigma_m[1] >
|
||||
solution.measurement_sigma_m[0] + 20.0);
|
||||
|
||||
free_matrix(satellite_positions);
|
||||
free_matrix(observations);
|
||||
free_matrix(az);
|
||||
free_matrix(el);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_broadcast_ephemeris_velocity();
|
||||
test_cn0_changes_position_weight();
|
||||
puts("test_gps_satellite_velocity PASS");
|
||||
return 0;
|
||||
}
|
||||
@@ -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(®isters, 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(®isters, &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(®isters, &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;
|
||||
}
|
||||
Reference in New Issue
Block a user