From b0b4e303648e85baa23210bcafcd14c619eceb30 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 16 Nov 2023 22:54:45 +0900 Subject: [PATCH] Use freq. input to calculate rate with Doppler if freq. is available --- tool/navigation/GPS_Solver_Base.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/tool/navigation/GPS_Solver_Base.h b/tool/navigation/GPS_Solver_Base.h index 272462d28..9b6aa51bc 100644 --- a/tool/navigation/GPS_Solver_Base.h +++ b/tool/navigation/GPS_Solver_Base.h @@ -209,8 +209,13 @@ struct GPS_Solver_Base { if((res = find_value(values, measurement_items_t::L1_RANGE_RATE, buf))){ }else if((res = find_value(values, measurement_items_t::L1_DOPPLER, buf))){ - // Fall back to doppler - buf *= -space_node_t::L1_WaveLength(); + // Fall back to Doppler + float_t freq; + if(find_value(values, measurement_items_t::L1_FREQUENCY, freq)){ + buf *= -(space_node_t::light_speed / freq); + }else{ + buf *= -space_node_t::L1_WaveLength(); + } } return res; } @@ -221,8 +226,13 @@ struct GPS_Solver_Base { if((res = find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf))){ }else if((res = find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, buf))){ - // Fall back to doppler - buf *= space_node_t::L1_WaveLength(); + // Fall back to Doppler + float_t freq; + if(find_value(values, measurement_items_t::L1_FREQUENCY, freq)){ + buf *= (space_node_t::light_speed / freq); + }else{ + buf *= space_node_t::L1_WaveLength(); + } } return res; }