From 2782d9617024ef0e6c8b98b5f7cad06724ef360d Mon Sep 17 00:00:00 2001 From: Xavier Arteaga Date: Tue, 9 Mar 2021 13:08:36 +0100 Subject: [PATCH] SRSUE: compute speed from TA commands --- .../srslte/interfaces/ue_phy_interfaces.h | 4 +- srsue/hdr/phy/phy.h | 4 +- srsue/hdr/phy/phy_metrics.h | 2 + srsue/hdr/phy/ta_control.h | 93 ++++++++++++++++++- srsue/hdr/stack/mac/demux.h | 2 +- srsue/hdr/stack/mac/proc_ra.h | 2 +- srsue/src/metrics_csv.cc | 8 +- srsue/src/phy/phy.cc | 8 +- srsue/src/phy/phy_common.cc | 2 + srsue/src/phy/sync.cc | 8 +- srsue/src/stack/mac/demux.cc | 8 +- srsue/src/stack/mac/proc_ra.cc | 8 +- srsue/test/mac_test.cc | 4 +- srsue/test/ttcn3/hdr/lte_ttcn3_phy.h | 4 +- srsue/test/ttcn3/src/lte_ttcn3_phy.cc | 4 +- 15 files changed, 128 insertions(+), 33 deletions(-) diff --git a/lib/include/srslte/interfaces/ue_phy_interfaces.h b/lib/include/srslte/interfaces/ue_phy_interfaces.h index 5faa63be0..2290d895e 100644 --- a/lib/include/srslte/interfaces/ue_phy_interfaces.h +++ b/lib/include/srslte/interfaces/ue_phy_interfaces.h @@ -104,8 +104,8 @@ class phy_interface_mac_common { public: /* Time advance commands */ - virtual void set_timeadv_rar(uint32_t ta_cmd) = 0; - virtual void set_timeadv(uint32_t ta_cmd) = 0; + virtual void set_timeadv_rar(uint32_t tti, uint32_t ta_cmd) = 0; + virtual void set_timeadv(uint32_t tti, uint32_t ta_cmd) = 0; /* Activate / Disactivate SCell*/ virtual void set_activation_deactivation_scell(uint32_t cmd, uint32_t tti) = 0; diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h index 3d90ee33f..034f996f7 100644 --- a/srsue/hdr/phy/phy.h +++ b/srsue/hdr/phy/phy.h @@ -140,8 +140,8 @@ public: int sr_last_tx_tti() final; // Time advance commands - void set_timeadv_rar(uint32_t ta_cmd) final; - void set_timeadv(uint32_t ta_cmd) final; + void set_timeadv_rar(uint32_t tti, uint32_t ta_cmd) final; + void set_timeadv(uint32_t tti, uint32_t ta_cmd) final; /* Activate / Disactivate SCell*/ void deactivate_scells() final; diff --git a/srsue/hdr/phy/phy_metrics.h b/srsue/hdr/phy/phy_metrics.h index 5384f4822..51dd6ff3c 100644 --- a/srsue/hdr/phy/phy_metrics.h +++ b/srsue/hdr/phy/phy_metrics.h @@ -24,6 +24,8 @@ struct info_metrics_t { struct sync_metrics_t { float ta_us; + float distance_km; + float speed_kmph; float cfo; float sfo; }; diff --git a/srsue/hdr/phy/ta_control.h b/srsue/hdr/phy/ta_control.h index 8cb6b9ff8..a6013c853 100644 --- a/srsue/hdr/phy/ta_control.h +++ b/srsue/hdr/phy/ta_control.h @@ -22,11 +22,34 @@ namespace srsue { class ta_control { private: + static const size_t MAX_NOF_SPEED_VALUES = 50; ///< Maximum number of data to store for speed calculation + static const size_t MIN_NOF_SPEED_VALUES = 1; ///< Minimum number of data for calculating the speed + static const size_t MAX_AGE_SPEED_VALUES = 10000; ///< Maximum age of speed data in milliseconds. Discards older data. + srslog::basic_logger& logger; mutable std::mutex mutex; uint32_t next_base_nta = 0; float next_base_sec = 0.0f; + // Vector containing data for calculating speed. The first value is the time increment from TTI and the second value + // is the distance increment from the TA command + struct speed_data_t { + uint32_t tti; + float delta_t; + float delta_d; + }; + std::array speed_data = {}; + int32_t last_tti = -1; // Last TTI writen, -1 if none + uint32_t write_idx = 0; + uint32_t read_idx = 0; + + void reset_speed_data() + { + write_idx = 0; + read_idx = 0; + last_tti = -1; + } + public: ta_control(srslog::basic_logger& logger) : logger(logger) {} @@ -45,6 +68,9 @@ public: // Update base in nta next_base_nta = static_cast(roundf(next_base_sec / SRSLTE_LTE_TS)); + // Reset speed data + reset_speed_data(); + logger.info("PHY: Set TA base: n_ta: %d, ta_usec: %.1f", next_base_nta, next_base_sec * 1e6f); } @@ -74,7 +100,7 @@ public: * * @param ta_cmd Time Alignment command */ - void add_ta_cmd_rar(uint32_t ta_cmd) + void add_ta_cmd_rar(uint32_t tti, uint32_t ta_cmd) { std::lock_guard lock(mutex); @@ -84,6 +110,10 @@ public: // Update base in seconds next_base_sec = static_cast(next_base_nta) * SRSLTE_LTE_TS; + // Reset speed data + reset_speed_data(); + last_tti = tti; + logger.info("PHY: Set TA RAR: ta_cmd: %d, n_ta: %d, ta_usec: %.1f", ta_cmd, next_base_nta, next_base_sec * 1e6f); } @@ -92,9 +122,10 @@ public: * * @param ta_cmd Time Alignment command */ - void add_ta_cmd_new(uint32_t ta_cmd) + void add_ta_cmd_new(uint32_t tti, uint32_t ta_cmd) { std::lock_guard lock(mutex); + float prev_base_sec = next_base_sec; // Update base nta next_base_nta = srslte_N_ta_new(next_base_nta, ta_cmd); @@ -103,6 +134,26 @@ public: next_base_sec = static_cast(next_base_nta) * SRSLTE_LTE_TS; logger.info("PHY: Set TA: ta_cmd: %d, n_ta: %d, ta_usec: %.1f", ta_cmd, next_base_nta, next_base_sec * 1e6f); + + // Calculate speed data + if (last_tti > 0) { + float delta_t = TTI_SUB(tti, last_tti) * 1e-3f; // Calculate the elapsed time since last time command + float delta_d = (next_base_sec - prev_base_sec) * 3e8f / 2.0f; // Calculate distance difference in metres + + // Write new data + speed_data[write_idx].tti = tti; + speed_data[write_idx].delta_t = delta_t; + speed_data[write_idx].delta_d = delta_d; + + // Advance write index + write_idx = (write_idx + 1) % MAX_NOF_SPEED_VALUES; + + // Advance read index if overlaps with write + if (write_idx == read_idx) { + read_idx = (read_idx + 1) % MAX_NOF_SPEED_VALUES; + } + } + last_tti = tti; // Update last TTI } /** @@ -141,7 +192,43 @@ public: std::lock_guard lock(mutex); // Returns the current base, one direction distance - return next_base_sec * (3.6f * 3e8f / 2.0f); + return next_base_sec * (3e8f / 2.0f); + } + + /** + * Calculates approximated speed in km/h from the TA commands + * + * @return Distance based on the current time base if enough data has been gathered + */ + float get_speed_kmph(uint32_t tti) + { + std::lock_guard lock(mutex); + + // Advance read pointer for old TTI + while (read_idx != write_idx and TTI_SUB(tti, speed_data[read_idx].tti) > MAX_AGE_SPEED_VALUES) { + read_idx = (read_idx + 1) % MAX_NOF_SPEED_VALUES; + } + + // Early return if there is not enough data to calculate speed + uint32_t nof_values = ((write_idx + MAX_NOF_SPEED_VALUES) - read_idx) % MAX_NOF_SPEED_VALUES; + if (nof_values < MIN_NOF_SPEED_VALUES) { + return 0.0f; + } + + // Compute speed from gathered data + float sum = 0.0f; + float square_sum = 0.0f; + for (uint32_t i = read_idx; i != write_idx; i = (i + 1) % MAX_NOF_SPEED_VALUES) { + square_sum += speed_data[i].delta_t * speed_data[i].delta_t; + sum += speed_data[i].delta_t * speed_data[i].delta_d; + } + if (!std::isnormal(square_sum)) { + return 0.0f; // Avoid zero division + } + float speed_mps = sum / square_sum; // Speed in m/s + + // Returns the speed in km/h + return speed_mps * 3.6f; } }; diff --git a/srsue/hdr/stack/mac/demux.h b/srsue/hdr/stack/mac/demux.h index 94dc1479c..4e4b9f9cc 100644 --- a/srsue/hdr/stack/mac/demux.h +++ b/srsue/hdr/stack/mac/demux.h @@ -78,7 +78,7 @@ private: void process_sch_pdu(srslte::sch_pdu* pdu); void process_mch_pdu(srslte::mch_pdu* pdu); bool process_ce(srslte::sch_subh* subheader, uint32_t tti); - void parse_ta_cmd(srslte::sch_subh* subh); + void parse_ta_cmd(srslte::sch_subh* subh, uint32_t tti); bool is_uecrid_successful = false; diff --git a/srsue/hdr/stack/mac/proc_ra.h b/srsue/hdr/stack/mac/proc_ra.h index 39e03cf32..63d268ae6 100644 --- a/srsue/hdr/stack/mac/proc_ra.h +++ b/srsue/hdr/stack/mac/proc_ra.h @@ -70,7 +70,7 @@ private: void state_backoff_wait(uint32_t tti); void state_contention_resolution(); - void process_timeadv_cmd(uint32_t ta_cmd); + void process_timeadv_cmd(uint32_t tti, uint32_t ta_cmd); void initialization(); void resource_selection(); void preamble_transmission(); diff --git a/srsue/src/metrics_csv.cc b/srsue/src/metrics_csv.cc index 456e04a32..081c11a54 100644 --- a/srsue/src/metrics_csv.cc +++ b/srsue/src/metrics_csv.cc @@ -74,7 +74,7 @@ void metrics_csv::set_metrics(const ue_metrics_t& metrics, const uint32_t period if (file.is_open() && ue != NULL) { if (n_reports == 0 && !file_exists) { file << "time;cc;earfcn;pci;rsrp;pl;cfo;pci_neigh;rsrp_neigh;cfo_neigh;dl_mcs;dl_snr;dl_turbo;dl_brate;dl_bler;" - "ul_ta;ul_mcs;ul_buff;ul_brate;ul_" + "ul_ta;distance_km;speed_kmph;ul_mcs;ul_buff;ul_brate;ul_" "bler;" "rf_o;rf_" "u;rf_l;is_attached;" @@ -130,6 +130,8 @@ void metrics_csv::set_metrics(const ue_metrics_t& metrics, const uint32_t period } file << float_to_string(metrics.phy.sync[r].ta_us, 2); + file << float_to_string(metrics.phy.sync[r].distance_km, 2); + file << float_to_string(metrics.phy.sync[r].speed_kmph, 2); file << float_to_string(metrics.phy.ul[r].mcs, 2); file << float_to_string((float)metrics.stack.mac[r].ul_buffer, 2); @@ -154,11 +156,11 @@ void metrics_csv::set_metrics(const ue_metrics_t& metrics, const uint32_t period file << (metrics.stack.rrc.state == RRC_STATE_CONNECTED ? "1.0" : "0.0") << ";"; // Write system metrics. - const srslte::sys_metrics_t &m = metrics.sys; + const srslte::sys_metrics_t& m = metrics.sys; file << float_to_string(m.process_realmem, 2); file << std::to_string(m.process_realmem_kB) << ";"; file << float_to_string(m.process_virtualmem, 2); - file << std::to_string(m.process_virtualmem_kB) << ";" ; + file << std::to_string(m.process_virtualmem_kB) << ";"; file << float_to_string(m.system_mem, 2); file << float_to_string(m.process_cpu_usage, 2); file << std::to_string(m.thread_count); diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc index 39a36df40..4e0a326f4 100644 --- a/srsue/src/phy/phy.cc +++ b/srsue/src/phy/phy.cc @@ -204,14 +204,14 @@ void phy::get_metrics(phy_metrics_t* m) m->nof_active_cc = args.nof_lte_carriers; } -void phy::set_timeadv_rar(uint32_t ta_cmd) +void phy::set_timeadv_rar(uint32_t tti, uint32_t ta_cmd) { - common.ta.add_ta_cmd_rar(ta_cmd); + common.ta.add_ta_cmd_rar(tti, ta_cmd); } -void phy::set_timeadv(uint32_t ta_cmd) +void phy::set_timeadv(uint32_t tti, uint32_t ta_cmd) { - common.ta.add_ta_cmd_new(ta_cmd); + common.ta.add_ta_cmd_new(tti, ta_cmd); } void phy::deactivate_scells() diff --git a/srsue/src/phy/phy_common.cc b/srsue/src/phy/phy_common.cc index ddcd26df4..300f5419c 100644 --- a/srsue/src/phy/phy_common.cc +++ b/srsue/src/phy/phy_common.cc @@ -865,6 +865,8 @@ void phy_common::set_sync_metrics(const uint32_t& cc_idx, const sync_metrics_t& sync_metrics[cc_idx].cfo = sync_metrics[cc_idx].cfo + (m.cfo - sync_metrics[cc_idx].cfo) / sync_metrics_count[cc_idx]; sync_metrics[cc_idx].sfo = sync_metrics[cc_idx].sfo + (m.sfo - sync_metrics[cc_idx].sfo) / sync_metrics_count[cc_idx]; sync_metrics[cc_idx].ta_us = m.ta_us; + sync_metrics[cc_idx].distance_km = m.distance_km; + sync_metrics[cc_idx].speed_kmph = m.speed_kmph; } void phy_common::get_sync_metrics(sync_metrics_t m[SRSLTE_MAX_CARRIERS]) diff --git a/srsue/src/phy/sync.cc b/srsue/src/phy/sync.cc index 02bdc67bd..37afdc076 100644 --- a/srsue/src/phy/sync.cc +++ b/srsue/src/phy/sync.cc @@ -467,9 +467,11 @@ void sync::run_camping_in_sync_state(lte::sf_worker* lte_worker, Debug("SYNC: Worker %d synchronized", lte_worker->get_id()); - metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync); - metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync); - metrics.ta_us = worker_com->ta.get_usec(); + metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync); + metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync); + metrics.ta_us = worker_com->ta.get_usec(); + metrics.distance_km = worker_com->ta.get_km(); + metrics.speed_kmph = worker_com->ta.get_speed_kmph(tti); for (uint32_t i = 0; i < worker_com->args->nof_lte_carriers; i++) { worker_com->set_sync_metrics(i, metrics); } diff --git a/srsue/src/stack/mac/demux.cc b/srsue/src/stack/mac/demux.cc index 0b6c8c86a..c3a4dd5d6 100644 --- a/srsue/src/stack/mac/demux.cc +++ b/srsue/src/stack/mac/demux.cc @@ -96,7 +96,7 @@ void demux::push_pdu_temp_crnti(uint8_t* buff, uint32_t nof_bytes) } break; case srslte::dl_sch_lcid::TA_CMD: - parse_ta_cmd(pending_mac_msg.get()); + parse_ta_cmd(pending_mac_msg.get(), 0); break; default: break; @@ -275,7 +275,7 @@ bool demux::process_ce(srslte::sch_subh* subh, uint32_t tti) // Do nothing break; case srslte::dl_sch_lcid::TA_CMD: - parse_ta_cmd(subh); + parse_ta_cmd(subh, tti); break; case srslte::dl_sch_lcid::SCELL_ACTIVATION: { uint32_t cmd = (uint32_t)subh->get_activation_deactivation_cmd(); @@ -293,9 +293,9 @@ bool demux::process_ce(srslte::sch_subh* subh, uint32_t tti) return true; } -void demux::parse_ta_cmd(srslte::sch_subh* subh) +void demux::parse_ta_cmd(srslte::sch_subh* subh, uint32_t tti) { - phy_h->set_timeadv(subh->get_ta_cmd()); + phy_h->set_timeadv(tti, subh->get_ta_cmd()); Info("Received TA=%d (%d/%d) ", subh->get_ta_cmd(), time_alignment_timer->time_elapsed(), diff --git a/srsue/src/stack/mac/proc_ra.cc b/srsue/src/stack/mac/proc_ra.cc index a5cf40b8c..e5ca78907 100644 --- a/srsue/src/stack/mac/proc_ra.cc +++ b/srsue/src/stack/mac/proc_ra.cc @@ -302,11 +302,11 @@ void ra_proc::preamble_transmission() } // Process Timing Advance Command as defined in Section 5.2 -void ra_proc::process_timeadv_cmd(uint32_t ta) +void ra_proc::process_timeadv_cmd(uint32_t tti, uint32_t ta) { if (preambleIndex == 0) { // Preamble not selected by UE MAC - phy_h->set_timeadv_rar(ta); + phy_h->set_timeadv_rar(tti, ta); // Only if timer is running reset the timer if (time_alignment_timer->is_running()) { time_alignment_timer->run(); @@ -315,7 +315,7 @@ void ra_proc::process_timeadv_cmd(uint32_t ta) } else { // Preamble selected by UE MAC if (!time_alignment_timer->is_running()) { - phy_h->set_timeadv_rar(ta); + phy_h->set_timeadv_rar(tti, ta); time_alignment_timer->run(); logger.debug("Applying RAR TA CMD %d", ta); } else { @@ -377,7 +377,7 @@ void ra_proc::tb_decoded_ok(const uint8_t cc_idx, const uint32_t tti) while (rar_pdu_msg.next()) { if (rar_pdu_msg.get()->has_rapid() && rar_pdu_msg.get()->get_rapid() == sel_preamble) { rar_received = true; - process_timeadv_cmd(rar_pdu_msg.get()->get_ta_cmd()); + process_timeadv_cmd(tti, rar_pdu_msg.get()->get_ta_cmd()); // TODO: Indicate received target power // phy_h->set_target_power_rar(iniReceivedTargetPower, (preambleTransmissionCounter-1)*powerRampingStep); diff --git a/srsue/test/mac_test.cc b/srsue/test/mac_test.cc index 25f7eba9a..85dca15db 100644 --- a/srsue/test/mac_test.cc +++ b/srsue/test/mac_test.cc @@ -144,8 +144,8 @@ public: void set_mch_period_stop(uint32_t stop) override{}; // phy_interface_mac_common - void set_timeadv_rar(uint32_t ta_cmd) override { rar_time_adv = ta_cmd; } - void set_timeadv(uint32_t ta_cmd) override{}; + void set_timeadv_rar(uint32_t tti, uint32_t ta_cmd) override { rar_time_adv = ta_cmd; } + void set_timeadv(uint32_t tti, uint32_t ta_cmd) override{}; void set_activation_deactivation_scell(uint32_t cmd, uint32_t tti) override { scell_cmd = cmd; }; void set_rar_grant(uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN], uint16_t rnti) override { diff --git a/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h b/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h index cace63e8d..0d0170c8b 100644 --- a/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h +++ b/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h @@ -83,8 +83,8 @@ public: int sr_last_tx_tti() override; // phy_interface_mac_common - void set_timeadv_rar(uint32_t ta_cmd) override; - void set_timeadv(uint32_t ta_cmd) override; + void set_timeadv_rar(uint32_t tti, uint32_t ta_cmd) override; + void set_timeadv(uint32_t tti, uint32_t ta_cmd) override; void set_rar_grant(uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN], uint16_t rnti) override; uint32_t get_current_tti() override; float get_phr() override; diff --git a/srsue/test/ttcn3/src/lte_ttcn3_phy.cc b/srsue/test/ttcn3/src/lte_ttcn3_phy.cc index 7a2f0f4d7..4012b3bcb 100644 --- a/srsue/test/ttcn3/src/lte_ttcn3_phy.cc +++ b/srsue/test/ttcn3/src/lte_ttcn3_phy.cc @@ -216,12 +216,12 @@ int lte_ttcn3_phy::sr_last_tx_tti() // The RAT-agnostic interface for MAC /* Time advance commands */ -void lte_ttcn3_phy::set_timeadv_rar(uint32_t ta_cmd) +void lte_ttcn3_phy::set_timeadv_rar(uint32_t tti, uint32_t ta_cmd) { logger.debug("%s not implemented.", __FUNCTION__); } -void lte_ttcn3_phy::set_timeadv(uint32_t ta_cmd) +void lte_ttcn3_phy::set_timeadv(uint32_t tti, uint32_t ta_cmd) { logger.debug("%s not implemented.", __FUNCTION__); }