SRSUE: compute speed from TA commands

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_rar(uint32_t tti, uint32_t ta_cmd) = 0;
-  virtual void set_timeadv(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;

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_rar(uint32_t tti, uint32_t ta_cmd) final;
-  void set_timeadv(uint32_t ta_cmd) final;
+  void set_timeadv(uint32_t tti, uint32_t ta_cmd) final;
 
   /* Activate / Disactivate SCell*/
   void deactivate_scells() final;

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;
 };

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_t, MAX_NOF_SPEED_VALUES> 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<uint32_t>(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<std::mutex> lock(mutex);
 
@@ -84,6 +110,10 @@ public:
     // Update base in seconds
     next_base_sec = static_cast<float>(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<std::mutex> 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<float>(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<std::mutex> 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<std::mutex> 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;
   }
 };
 

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;
 

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();

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);

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()

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])

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.sfo         = srslte_ue_sync_get_sfo(&ue_sync);
-  metrics.cfo   = srslte_ue_sync_get_cfo(&ue_sync);
+  metrics.cfo         = srslte_ue_sync_get_cfo(&ue_sync);
-  metrics.ta_us = worker_com->ta.get_usec();
+  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);
   }

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(),

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);

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_rar(uint32_t tti, uint32_t ta_cmd) override { rar_time_adv = ta_cmd; }
-  void set_timeadv(uint32_t ta_cmd) override{};
+  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
   {

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_rar(uint32_t tti, uint32_t ta_cmd) override;
-  void     set_timeadv(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;

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__);
 }