From 0e70d8642e483255cdf2c62b8f1a1f9aeba8e342 Mon Sep 17 00:00:00 2001
From: 30hours <nathan@30hours.dev>
Date: Tue, 28 Nov 2023 12:38:49 +0000
Subject: [PATCH] Fix some CFAR bugs (SNR was wrong)

---
 src/blah2.cpp                            | 10 +++++-----
 src/data/Detection.cpp                   | 15 +++++++++++++++
 src/data/Detection.h                     | 18 +++++++++++++++---
 src/process/detection/CfarDetector1D.cpp |  5 +++--
 4 files changed, 38 insertions(+), 10 deletions(-)

diff --git a/src/blah2.cpp b/src/blah2.cpp
index 09f4351..66824a8 100644
--- a/src/blah2.cpp
+++ b/src/blah2.cpp
@@ -213,6 +213,7 @@ int main(int argc, char **argv)
 
           // ambiguity process
           map = ambiguity->process(x, y);
+          map->set_metrics();
           uint64_t t3 = current_time_us();
           double delta_t3 = (double)(t3-t2) / 1000;
           timing_name.push_back("ambiguity_processing");
@@ -226,7 +227,6 @@ int main(int argc, char **argv)
           timing_time.push_back(delta_t4);
 
           // output map data
-          map->set_metrics();
           mapJson = map->to_json();
           mapJson = map->delay_bin_to_km(mapJson, fs);
           if (saveMap)
@@ -262,16 +262,16 @@ int main(int argc, char **argv)
           timing_time.push_back(delta_t6);
           std::cout << "CPI time (ms): " << delta_t6 << std::endl;
 
-          // output CPI timestamp for updating data
-          std::string t0_string = std::to_string(t0);
-          socket_timestamp.write_some(asio::buffer(t0_string, 100), err);
-
           // output timing data
           timing->update(t0/1000, timing_time, timing_name);
           jsonTiming = timing->to_json();
           socket_timing.write_some(asio::buffer(jsonTiming, 1500), err);
           timing_time.clear();
           timing_name.clear();
+
+          // output CPI timestamp for updating data
+          std::string t0_string = std::to_string(t0);
+          socket_timestamp.write_some(asio::buffer(t0_string, 100), err);
         }
       }
     });
diff --git a/src/data/Detection.cpp b/src/data/Detection.cpp
index a84d59b..2342464 100644
--- a/src/data/Detection.cpp
+++ b/src/data/Detection.cpp
@@ -16,6 +16,21 @@ Detection::Detection(std::vector<double> _delay, std::vector<double> _doppler, s
   snr = _snr;
 }
 
+std::vector<double> Detection::get_delay()
+{
+  return delay;
+}
+
+std::vector<double> Detection::get_doppler()
+{
+  return doppler;
+}
+
+std::vector<double> Detection::get_snr()
+{
+  return snr;
+}
+
 uint8_t Detection::get_nDetections()
 {
   return delay.size();
diff --git a/src/data/Detection.h b/src/data/Detection.h
index 6c4a612..a9faaf5 100644
--- a/src/data/Detection.h
+++ b/src/data/Detection.h
@@ -13,13 +13,13 @@
 class Detection
 {
 private:
-  /// @brief Detections in delay (bins)
+  /// @brief Detections in delay (bins).
   std::vector<double> delay;
 
-  /// @brief Detections in Doppler (Hz)
+  /// @brief Detections in Doppler (Hz).
   std::vector<double> doppler;
 
-  /// @brief Detections in SNR
+  /// @brief Detections in SNR.
   std::vector<double> snr;
 
 public:
@@ -29,6 +29,18 @@ public:
   /// @return The object.
   Detection(std::vector<double> delay, std::vector<double> doppler, std::vector<double> snr);
 
+  /// @brief Get detections in delay.
+  /// @return Detections in delay (bins).
+  std::vector<double> get_delay();
+
+  /// @brief Get detections in Doppler.
+  /// @return Detections in Doppler (Hz).
+  std::vector<double> get_doppler();
+
+  /// @brief Detections in SNR.
+  /// @return Detections in SNR.
+  std::vector<double> get_snr();
+
   /// @brief Get number of detections.
   /// @return Number of detections
   uint8_t get_nDetections();
diff --git a/src/process/detection/CfarDetector1D.cpp b/src/process/detection/CfarDetector1D.cpp
index 1a40461..7a00bb5 100644
--- a/src/process/detection/CfarDetector1D.cpp
+++ b/src/process/detection/CfarDetector1D.cpp
@@ -44,7 +44,7 @@ Detection *CfarDetector1D::process(Map<std::complex<double>> *x)
     for (int j = 0; j < nDelayBins; j++)
     {
       mapRowSquare.push_back((double) std::abs(mapRow[j]*mapRow[j]));
-      mapRowSnr.push_back((double)20 * std::log10(std::abs(mapRow[j])));
+      mapRowSnr.push_back((double)10 * std::log10(std::abs(mapRow[j])) - x->noisePower);
     }
     for (int j = 0; j < nDelayBins; j++)
     {
@@ -84,13 +84,14 @@ Detection *CfarDetector1D::process(Map<std::complex<double>> *x)
       // detection if over threshold
       if (mapRowSquare[j] > threshold)
       {
-        delay.push_back(j + x->delay[0] + 1);
+        delay.push_back(j + x->delay[0]);
         doppler.push_back(x->doppler[i]);
         snr.push_back(mapRowSnr[j]);
       }
       iTrain.clear();
     }
     mapRowSquare.clear();
+    mapRowSnr.clear();
   }
 
   // create detection