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

Integrate asynchronous GPS IMU fusion

Browse Source
This commit is contained in:
onur
2026-07-16 23:49:43 +03:00
parent 38a96dd88f
commit 7cc3eb2a54
80 changed files with 3901 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv
Vendored
+5
View File
@@ -350,8 +350,12 @@ module tracking_wrapper #(
end : preamble_search_comb
logic [301 : 0] shift_reg_resultant;
logic [16 : 0] how_z_count;
assign shift_reg_resultant[301-:2] = shift_reg_p[309-:2]; // always assign last 2 bit from previous subframe
/* Normalized HOW word is nav word 2 at [269:240]; bits [29:13] are
* the 17-bit Z-count used to align NAV solve timer phase globally. */
assign how_z_count = shift_reg_resultant[269-:17];
generate
genvar j;
@@ -457,6 +461,7 @@ module tracking_wrapper #(
.counter_max_val(nav_solve_timer_reg_p), // timer generates interrupt when internal timer reaches this value.
.bit_valid(bit_valid_syned), // timer counts bit valid signals.
.subframe_ready(load_sigs), // signal that asserts 1 clock cycle after a subframe bit reached ( in essence, 300 bit + 8 preamble bit !!!)
.subframe_count(how_z_count), // HOW Z-count of this globally identified subframe boundary.
.trck_busy(busy_syned), // stop counting when tracking is unlocked or do not start to count anyways.
// output from module.
.nav_solve_timer_reached(nav_solve_timer_reached), // timer internal count is equal to predefined count value. (status flag) asserted 1 clock cycle. INTERRUPT SIGNAL TO CPU FROM TIMER TODO: will be integrated to main interrupt generation mechanishm.
hardware/external/gps/rtl/gps_timer/nav_solve_timer.sv
Vendored
+52 -3
View File
@@ -6,6 +6,7 @@ module nav_solve_timer(
input logic [31 : 0] counter_max_val,
input logic bit_valid,
input logic subframe_ready,
input logic [16 : 0] subframe_count,
input logic trck_busy,
output logic nav_solve_timer_reached,
output logic load_abs_sample
@@ -28,12 +29,20 @@ module nav_solve_timer(
assign nav_solve_timer_reached = intr_delay_ff;
logic [31 : 0] internal_counter_p, internal_counter_n;
logic [25 : 0] phase_dividend_p, phase_dividend_n;
logic [31 : 0] phase_remainder_p, phase_remainder_n;
logic [4 : 0] phase_bit_p, phase_bit_n;
logic [32 : 0] phase_candidate;
logic [32 : 0] phase_reduced;
nav_solve_timer_pkg::nav_solve_timer_e nav_solve_state_p, nav_solve_state_n;
always_ff @(posedge clk or negedge resetn) begin : counter_seq_logic
if(!resetn) begin
internal_counter_p <= '0;
phase_dividend_p <= '0;
phase_remainder_p <= '0;
phase_bit_p <= '0;
bit_valid_cc_p <= '0;
abs_sample_load_cc_p <= '0;
nav_solve_state_p <= nav_solve_timer_pkg::IDLE;
@@ -41,6 +50,9 @@ module nav_solve_timer(
end
else begin
internal_counter_p <= internal_counter_n;
phase_dividend_p <= phase_dividend_n;
phase_remainder_p <= phase_remainder_n;
phase_bit_p <= phase_bit_n;
bit_valid_cc_p <= bit_valid_cc_n;
abs_sample_load_cc_p <= abs_sample_load_cc_n;
nav_solve_state_p <= nav_solve_state_n;
@@ -50,10 +62,47 @@ module nav_solve_timer(
always_comb begin : counter_comb_logic
internal_counter_n = internal_counter_p;
phase_dividend_n = phase_dividend_p;
phase_remainder_n = phase_remainder_p;
phase_bit_n = phase_bit_p;
phase_candidate = '0;
phase_reduced = '0;
load_abs_sample_internal = '0;
nav_solve_state_n = nav_solve_state_p;
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) && (subframe_ready) && (trck_busy) && (enable)) begin // subframe ready is 1 cc pulse signal.
nav_solve_state_n = nav_solve_timer_pkg::COUNT;
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) && subframe_ready && trck_busy && enable && (counter_max_val != 0)) begin
/* HOW Z-count identifies this boundary globally in six-second
* subframes. Seed the per-channel counter with the corresponding
* navigation-bit phase so channels which lock on different
* subframes still produce the same NAV solve epochs for any period.
* Multiplication by 300 is constant shift/add; modulo is evaluated in
* ALIGN over 26 PCLK cycles, far below one 20 ms navigation bit. */
phase_dividend_n = ({9'b0, subframe_count} << 8) +
({9'b0, subframe_count} << 5) +
({9'b0, subframe_count} << 3) +
({9'b0, subframe_count} << 2);
phase_remainder_n = '0;
phase_bit_n = 5'd25;
nav_solve_state_n = nav_solve_timer_pkg::ALIGN;
end
if(nav_solve_state_p == nav_solve_timer_pkg::ALIGN) begin
if(!enable || !trck_busy || (counter_max_val == 0)) begin
nav_solve_state_n = nav_solve_timer_pkg::IDLE;
end
else begin
phase_candidate = ({1'b0, phase_remainder_p} << 1) |
phase_dividend_p[phase_bit_p];
phase_reduced = (phase_candidate >= {1'b0, counter_max_val})
? (phase_candidate - {1'b0, counter_max_val})
: phase_candidate;
phase_remainder_n = phase_reduced[31:0];
if(phase_bit_p == 0) begin
internal_counter_n = phase_reduced[31:0];
nav_solve_state_n = nav_solve_timer_pkg::COUNT;
end
else begin
phase_bit_n = phase_bit_p - 1'b1;
end
end
end
if(nav_solve_state_p == nav_solve_timer_pkg::COUNT) begin
if(!enable) nav_solve_state_n = nav_solve_timer_pkg::IDLE;
@@ -66,7 +115,7 @@ module nav_solve_timer(
end
end
if(nav_solve_state_p == nav_solve_timer_pkg::IDLE) begin
internal_counter_n = '0;
internal_counter_n = '0;
end
if((nav_solve_state_p == nav_solve_timer_pkg::COUNT) && (internal_counter_p == (counter_max_val - 1) && (bit_valid_pulse))) begin
hardware/external/gps/rtl/gps_timer/nav_solve_timer_pkg.sv
Vendored
+5 -4
View File
@@ -1,9 +1,10 @@
package nav_solve_timer_pkg;
typedef enum{
IDLE = 0,
COUNT = 1
}nav_solve_timer_e;
typedef enum logic [1 : 0] {
IDLE = 2'd0,
ALIGN = 2'd1,
COUNT = 2'd2
} nav_solve_timer_e;
endpackage
hardware/external/gps/rtl/gps_timer/tb/Makefile
Vendored
+21
View File
@@ -0,0 +1,21 @@
QUESTA_VLOG ?= vlog
QUESTA_VSIM ?= vsim
QUESTA_RUN_DIR ?= /tmp/gps_nav_solve_timer_questa
TIMER_DIR := $(abspath ..)
.PHONY: test clean
test:
rm -rf $(QUESTA_RUN_DIR)
mkdir -p $(QUESTA_RUN_DIR)
cd $(QUESTA_RUN_DIR) && vlib work
cd $(QUESTA_RUN_DIR) && $(QUESTA_VLOG) \
$(TIMER_DIR)/nav_solve_timer_pkg.sv \
$(TIMER_DIR)/nav_solve_timer.sv \
$(CURDIR)/nav_solve_timer_tb.sv
cd $(QUESTA_RUN_DIR) && $(QUESTA_VSIM) -c nav_solve_timer_tb \
-do 'run -all; quit -f'
clean:
rm -rf $(QUESTA_RUN_DIR)
hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_tb.sv
Vendored
+107
View File
@@ -0,0 +1,107 @@
`timescale 1ns/1ps
module nav_solve_timer_tb;
logic clk = 1'b0;
logic resetn = 1'b0;
logic enable = 1'b1;
logic [31:0] counter_max_val;
logic bit_valid = 1'b0;
logic subframe_ready_a = 1'b0;
logic subframe_ready_b = 1'b0;
logic [16:0] subframe_count_a = '0;
logic [16:0] subframe_count_b = '0;
logic trck_busy = 1'b1;
logic reached_a;
logic reached_b;
logic load_a;
logic load_b;
int unsigned global_bit;
bit b_active;
always #5 clk = ~clk;
nav_solve_timer timer_a (
.clk, .resetn, .enable, .counter_max_val, .bit_valid,
.subframe_ready(subframe_ready_a),
.subframe_count(subframe_count_a),
.trck_busy, .nav_solve_timer_reached(reached_a),
.load_abs_sample(load_a)
);
nav_solve_timer timer_b (
.clk, .resetn, .enable, .counter_max_val, .bit_valid,
.subframe_ready(subframe_ready_b),
.subframe_count(subframe_count_b),
.trck_busy, .nav_solve_timer_reached(reached_b),
.load_abs_sample(load_b)
);
task automatic clock_cycles(input int unsigned count);
repeat(count) @(posedge clk);
endtask
task automatic pulse_subframe(input bit start_a, input bit start_b,
input logic [16:0] z_count);
subframe_count_a = z_count;
subframe_count_b = z_count;
subframe_ready_a = start_a;
subframe_ready_b = start_b;
@(posedge clk);
subframe_ready_a = 1'b0;
subframe_ready_b = 1'b0;
/* ALIGN consumes 26 PCLK cycles before the next 20 ms bit event. */
clock_cycles(30);
endtask
task automatic pulse_nav_bit;
bit saw_a;
bit saw_b;
bit expected;
saw_a = 1'b0;
saw_b = 1'b0;
bit_valid = 1'b1;
@(posedge clk);
bit_valid = 1'b0;
repeat(8) begin
@(posedge clk);
saw_a |= reached_a;
saw_b |= reached_b;
end
global_bit++;
expected = ((global_bit % counter_max_val) == 0);
if(saw_a !== expected)
$fatal(1, "timer A phase mismatch: N=%0d bit=%0d got=%0d expected=%0d",
counter_max_val, global_bit, saw_a, expected);
if(b_active && (saw_b !== expected))
$fatal(1, "timer B phase mismatch: N=%0d bit=%0d got=%0d expected=%0d",
counter_max_val, global_bit, saw_b, expected);
endtask
task automatic run_late_join_scenario(input int unsigned period);
counter_max_val = period;
global_bit = 100 * 300;
b_active = 1'b0;
resetn = 1'b0;
clock_cycles(3);
resetn = 1'b1;
clock_cycles(2);
pulse_subframe(1'b1, 1'b0, 17'd100);
repeat(300) pulse_nav_bit();
/* Timer B locks one complete six-second subframe later. */
b_active = 1'b1;
pulse_subframe(1'b0, 1'b1, 17'd101);
repeat(period * 3) pulse_nav_bit();
endtask
initial begin
run_late_join_scenario(40);
run_late_join_scenario(37);
run_late_join_scenario(512);
run_late_join_scenario(1);
$display("nav_solve_timer_tb PASS");
$finish;
end
endmodule