Add processing times and detection draft

CMakeLists.txt

@@ -21,8 +21,10 @@ add_executable(blah2
   ${PROJECT_SOURCE_DIR}/capture/rspduo/RspDuo.cpp
   ${PROJECT_SOURCE_DIR}/process/ambiguity/Ambiguity.cpp
   ${PROJECT_SOURCE_DIR}/process/clutter/WienerHopf.cpp
+  ${PROJECT_SOURCE_DIR}/process/detection/CfarDetector1D.cpp
   ${PROJECT_SOURCE_DIR}/data/IqData.cpp
   ${PROJECT_SOURCE_DIR}/data/Map.cpp
+  ${PROJECT_SOURCE_DIR}/data/Detection.cpp
 )
 
 add_library(ryml ${PROJECT_LIB_DIR}/rapidyaml-0.5.0/ryml-0.5.0.hpp)
@@ -61,4 +63,5 @@ include_directories("${PROJECT_SOURCE_DIR}/capture/")
 include_directories("${PROJECT_SOURCE_DIR}/capture/rspduo/")
 include_directories("${PROJECT_SOURCE_DIR}/process/ambiguity/")
 include_directories("${PROJECT_SOURCE_DIR}/process/clutter/")
+include_directories("${PROJECT_SOURCE_DIR}/process/detection/")
 include_directories("${PROJECT_SOURCE_DIR}/data/")

api/server.js

@@ -1,10 +1,12 @@
 const express = require('express');
 const dgram = require('dgram');
+const net = require("net");
 
 // constants
 const PORT = 3000;
 const HOST = '0.0.0.0';
 var map = '';
+var detection = '';
 var data = '';
 var capture = false;
 
@@ -24,6 +26,9 @@ app.get('/', (req, res) => {
 app.get('/map', (req, res) => {
   res.send(map);
 });
+app.get('/detection', (req, res) => {
+  res.send(detection);
+});
 // read state of capture
 app.get('/capture', (req, res) => {
   res.send(capture);
@@ -37,9 +42,8 @@ app.listen(PORT, HOST, () => {
   console.log(`Running on http://${HOST}:${PORT}`);
 });
 
-// tcp listener
-const net = require("net");
-const server = net.createServer((socket)=>{
+// tcp listener map
+const server_map = net.createServer((socket)=>{
     socket.write("Hello From Server!")
     socket.on("data",(msg)=>{
         data = data + msg.toString();
@@ -54,4 +58,22 @@ const server = net.createServer((socket)=>{
         console.log("Connection closed.");
     })
 });
-server.listen(3001);
+server_map.listen(3001);
+
+// tcp listener detection
+const server_detection = net.createServer((socket)=>{
+  socket.write("Hello From Server!")
+  socket.on("data",(msg)=>{
+      data = data + msg.toString();
+      if (data.slice(-1) === "}")
+      {
+        console.log('EOF');
+        detection = data;
+        data = '';
+      }
+  });
+  socket.on("close",()=>{
+      console.log("Connection closed.");
+  })
+});
+server_detection.listen(3002);

api/server.js.old

@@ -1,17 +0,0 @@
-'use strict';
-
-const express = require('express');
-
-// Constants
-const PORT = 8080;
-const HOST = '0.0.0.0';
-
-// App
-const app = express();
-app.get('/', (req, res) => {
-  res.send('Hello World');
-});
-
-app.listen(PORT, HOST, () => {
-  console.log(`Running on http://${HOST}:${PORT}`);
-});

config/radar4.yml

@@ -10,17 +10,17 @@ capture:
 process:
   data:
     cpi: 1
-    buffer: 3
+    buffer: 1.5
     overlap: 0
   ambiguity:
     delayMin: -10
     delayMax: 300
-    dopplerMin: -250
-    dopplerMax: 250
+    dopplerMin: -300
+    dopplerMax: 300
   clutter:
     delayMin: -10
     delayMax: 300
-  detect:
+  detection:
     pfa: 0.000001
     nGuard: 10
     nTrain: 20
@@ -30,10 +30,10 @@ network:
   ports:
     api: 3000
     map: 3001
-    detect: 3002
+    detection: 3002
 
 save:
   iq: true
   map: true
-  detect: false
+  detection: false
   path: "/opt/blah2/"

src/blah2.cpp

@@ -9,8 +9,10 @@
 #include <Capture.h>
 #include <Ambiguity.h>
 #include <WienerHopf.h>
+#include <CfarDetector1D.h>
 #include <IqData.h>
 #include <Map.h>
+#include <Detection.h>
 #include <sys/types.h>
 #include <getopt.h>
 #include <string>
@@ -79,7 +81,8 @@ int main(int argc, char **argv)
   IqData *y = new IqData(nSamples);
   Map<std::complex<double>> *map;
   Map<double> *mapdb;
-  std::string mapJson;
+  std::string mapJson, detectionJson;
+  Detection *detection;
 
   // setup fftw multithread
   if (fftw_init_threads() == 0)
@@ -90,16 +93,22 @@ int main(int argc, char **argv)
   fftw_plan_with_nthreads(4);
 
   // setup socket
-  uint16_t port;
+  uint16_t port_map, port_detection;
   std::string ip;
-  tree["network"]["ports"]["map"] >> port;
+  tree["network"]["ports"]["map"] >> port_map;
+  tree["network"]["ports"]["detection"] >> port_detection;
   tree["network"]["ip"] >> ip;
   asio::io_service io_service;
-  asio::ip::tcp::socket socket(io_service);
-  asio::ip::tcp::endpoint endpoint;
-  endpoint = asio::ip::tcp::endpoint(
-    asio::ip::address::from_string(ip), port);
-  socket.connect(endpoint);
+  asio::ip::tcp::socket socket_map(io_service);
+  asio::ip::tcp::socket socket_detection(io_service);
+  asio::ip::tcp::endpoint endpoint_map;
+  asio::ip::tcp::endpoint endpoint_detection;
+  endpoint_map = asio::ip::tcp::endpoint(
+    asio::ip::address::from_string(ip), port_map);
+  endpoint_detection = asio::ip::tcp::endpoint(
+    asio::ip::address::from_string(ip), port_detection);
+  socket_map.connect(endpoint_map);
+  socket_detection.connect(endpoint_detection);
   asio::error_code err;
   std::string subdata;
   uint32_t MTU = 1024;
@@ -120,6 +129,14 @@ int main(int argc, char **argv)
   tree["process"]["clutter"]["delayMax"] >> delayMaxClutter;
   WienerHopf *filter = new WienerHopf(delayMinClutter, delayMaxClutter, nSamples);
 
+  // setup process detection
+  double pfa;
+  int8_t nGuard, nTrain;
+  tree["process"]["detection"]["pfa"] >> pfa;
+  tree["process"]["detection"]["nGuard"] >> nGuard;
+  tree["process"]["detection"]["nTrain"] >> nTrain;
+  CfarDetector1D *cfarDetector1D = new CfarDetector1D(pfa, nGuard, nTrain);
+
   // setup output data
   bool saveMap;
   tree["save"]["map"] >> saveMap;
@@ -143,6 +160,8 @@ int main(int argc, char **argv)
       {
         if ((buffer1->get_length() > nSamples) && (buffer2->get_length() > nSamples))
         {
+          auto t0 = std::chrono::high_resolution_clock::now();
+          
           // extract data from buffer
           buffer1->set_doNotPush(true);
           buffer2->set_doNotPush(true);
@@ -153,15 +172,32 @@ int main(int argc, char **argv)
           }
           buffer1->set_doNotPush(false);
           buffer2->set_doNotPush(false);
+          auto t1 = std::chrono::high_resolution_clock::now();
+          double delta_t1 = std::chrono::duration<double, std::milli>(t1-t0).count();
+          std::cout << "Extract data from buffer (ms): " << delta_t1 << std::endl;
 
-          // radar processing
+          // clutter filter
           if (!filter->process(x, y))
           {
             continue;
           }
-          map = ambiguity->process(x, y);
+          auto t2 = std::chrono::high_resolution_clock::now();
+          double delta_t2 = std::chrono::duration<double, std::milli>(t2-t1).count();
+          std::cout << "Clutter filter (ms): " << delta_t2 << std::endl;
 
-          // output data
+          // ambiguity process
+          map = ambiguity->process(x, y);
+          auto t3 = std::chrono::high_resolution_clock::now();
+          double delta_t3 = std::chrono::duration<double, std::milli>(t3-t2).count();
+          std::cout << "Ambiguity processing (ms): " << delta_t3 << std::endl;
+
+          // detection process
+          // detection = cfarDetector1D->process(map);
+          auto t4 = std::chrono::high_resolution_clock::now();
+          double delta_t4 = std::chrono::duration<double, std::milli>(t4-t3).count();
+          std::cout << "Detection processing (ms): " << delta_t4 << std::endl;
+
+          // output map data
           map->set_metrics();
           mapJson = map->to_json();
           if (saveMap)
@@ -171,10 +207,27 @@ int main(int argc, char **argv)
           for (int i = 0; i < (mapJson.size() + MTU - 1) / MTU; i++)
           {
             subdata = mapJson.substr(i * MTU, MTU);
-            socket.write_some(asio::buffer(subdata, subdata.size()), err);
+            socket_map.write_some(asio::buffer(subdata, subdata.size()), err);
           }
+          auto t5 = std::chrono::high_resolution_clock::now();
+          double delta_t5 = std::chrono::duration<double, std::milli>(t5-t4).count();
+          std::cout << "Output map data (ms): " << delta_t5 << std::endl;
 
-          std::cout << "CPI PROCESSED" << std::endl;
+          // output detection data
+          // detectionJson = detection->to_json();
+          // for (int i = 0; i < (detectionJson.size() + MTU - 1) / MTU; i++)
+          // {
+          //   subdata = detectionJson.substr(i * MTU, MTU);
+          //   socket_detection.write_some(asio::buffer(subdata, subdata.size()), err);
+          // }
+          // delete detection;
+          auto t6 = std::chrono::high_resolution_clock::now();
+          double delta_t6 = std::chrono::duration<double, std::milli>(t6-t5).count();
+          std::cout << "Output detection data (ms): " << delta_t6 << std::endl;
+
+          auto t7 = std::chrono::high_resolution_clock::now();
+          double delta_t7 = std::chrono::duration<double, std::milli>(t7-t0).count();
+          std::cout << "CPI time (ms): " << delta_t7 << std::endl;
         }
       }
     });

src/data/Detection.cpp

@@ -0,0 +1,120 @@
+#include "Detection.h"
+#include <iostream>
+#include <cstdlib>
+#include <chrono>
+
+#include "rapidjson/document.h"
+#include "rapidjson/writer.h"
+#include "rapidjson/stringbuffer.h"
+#include "rapidjson/filewritestream.h"
+
+// constructor
+Detection::Detection(std::vector<double> _delay, std::vector<double> _doppler, std::vector<double> _snr)
+{
+  delay = _delay;
+  doppler = _doppler;
+  snr = _snr;
+}
+
+uint8_t Detection::get_nDetections()
+{
+  return delay.size();
+}
+
+std::string Detection::to_json()
+{
+  rapidjson::Document document;
+  document.SetObject();
+  rapidjson::Document::AllocatorType &allocator = document.GetAllocator();
+
+  // store delay array
+  rapidjson::Value arrayDelay(rapidjson::kArrayType);
+  for (int i = 0; i < get_nDetections(); i++)
+  {
+    arrayDelay.PushBack(delay[i], allocator);
+  }
+
+  // store Doppler array
+  rapidjson::Value arrayDoppler(rapidjson::kArrayType);
+  for (int i = 0; i < get_nDetections(); i++)
+  {
+    arrayDoppler.PushBack(doppler[i], allocator);
+  }
+
+  // store snr array
+  rapidjson::Value arraySnr(rapidjson::kArrayType);
+  for (int i = 0; i < get_nDetections(); i++)
+  {
+    arraySnr.PushBack(snr[i], allocator);
+  }
+
+  // get posix time
+  uint64_t timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+  document.AddMember("timestamp", timestamp/1000, allocator);
+  document.AddMember("delay", arrayDelay, allocator);
+  document.AddMember("doppler", arrayDoppler, allocator);
+  document.AddMember("snr", arraySnr, allocator);
+  
+  rapidjson::StringBuffer strbuf;
+  rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
+  writer.SetMaxDecimalPlaces(2);
+  document.Accept(writer);
+
+  return strbuf.GetString();
+}
+
+bool Detection::save(std::string _json, std::string filename)
+{
+  using namespace rapidjson;
+
+  rapidjson::Document document;
+
+  // create file if it doesn't exist
+  if (FILE *fp = fopen(filename.c_str(), "r"); !fp)
+  {
+    if (fp = fopen(filename.c_str(), "w"); !fp)
+      return false;
+    fputs("[]", fp);
+    fclose(fp);
+  }
+
+  // add the document to the file
+  if (FILE *fp = fopen(filename.c_str(), "rb+"); fp)
+  {
+    // check if first is [
+    std::fseek(fp, 0, SEEK_SET);
+    if (getc(fp) != '[')
+    {
+      std::fclose(fp);
+      return false;
+    }
+
+    // is array empty?
+    bool isEmpty = false;
+    if (getc(fp) == ']')
+      isEmpty = true;
+
+    // check if last is ]
+    std::fseek(fp, -1, SEEK_END);
+    if (getc(fp) != ']')
+    {
+      std::fclose(fp);
+      return false;
+    }
+
+    // replace ] by ,
+    fseek(fp, -1, SEEK_END);
+    if (!isEmpty)
+      fputc(',', fp);
+
+    // add json element
+    fwrite(_json.c_str(), sizeof(char), _json.length(), fp);
+
+    // close the array
+    std::fputc(']', fp);
+    fclose(fp);
+    return true;
+  }
+  return false;
+}

src/data/Detection.h

@@ -0,0 +1,47 @@
+/// @file Detection.h
+/// @class Detection
+/// @brief A class to store detection data.
+/// @author 30hours
+
+#ifndef DETECTION_H
+#define DETECTION_H
+
+#include <stdint.h>
+#include <vector>
+#include <complex>
+
+class Detection
+{
+private:
+  /// @brief Detections in delay (bins)
+  std::vector<double> delay;
+
+  /// @brief Detections in Doppler (Hz)
+  std::vector<double> doppler;
+
+  /// @brief Detections in SNR
+  std::vector<double> snr;
+
+public:
+  /// @brief Constructor.
+  /// @param delay Detections in delay (bins).
+  /// @param doppler Detections in Doppler (Hz).
+  /// @return The object.
+  Detection(std::vector<double> delay, std::vector<double> doppler, std::vector<double> snr);
+
+  /// @brief Get number of detections.
+  /// @return Number of detections
+  uint8_t get_nDetections();
+
+  /// @brief Generate JSON of the detections and metadata.
+  /// @return JSON string.
+  std::string to_json();
+
+  /// @brief Append the detections to a save file.
+  /// @param json JSON string of detections and metadata.
+  /// @param path Path of file to save.
+  /// @return True is save is successful.
+  bool save(std::string json, std::string path);
+};
+
+#endif

src/process/detection/CfarDetector1D.cpp

@@ -0,0 +1,88 @@
+#include "CfarDetector1D.h"
+#include <iostream>
+#include <vector>
+#include <cmath>
+
+// constructor
+CfarDetector1D::CfarDetector1D(double _pfa, int8_t _nGuard, int8_t _nTrain)
+{
+  // input
+  pfa = _pfa;
+  nGuard = _nGuard;
+  nTrain = _nTrain;
+}
+
+CfarDetector1D::~CfarDetector1D()
+{
+}
+
+Detection *CfarDetector1D::process(Map<std::complex<double>> *x)
+{
+  std::vector<std::vector<double>> dataSnr;
+  std::vector<std::vector<double>> dataSquare;
+
+  // compute square of Map
+  for (int i = 0; i < x->data.size(); i++)
+  {
+    std::vector<double> dataSnrRow;
+    std::vector<double> dataSquareRow;
+    for (int j = 0; j < x->data[i].size(); j++)
+    {
+      dataSnrRow.push_back(10 * log10(std::abs(x->data[i][j])) - x->noisePower);
+      dataSquareRow.push_back(std::abs(x->data[i][j]) * std::abs(x->data[i][j]));
+    }
+    dataSnr.push_back(dataSnrRow);
+    dataSquare.push_back(dataSquareRow);
+  }
+
+  int32_t nDelayBins = x->get_nRows();
+  int32_t nDopplerBins = x->get_nCols();
+
+  // store detections temporarily
+  std::vector<double> delay;
+  std::vector<double> doppler;
+  std::vector<double> snr;
+
+  // loop over every cell
+  for (int iDelay = 0; iDelay < nDelayBins; iDelay++)
+  {
+    for (int iDoppler = 0; iDoppler < nDopplerBins; iDoppler++)
+    {
+
+      // get train cell indices
+      std::vector<int> iTrain;
+      for (int k = iDelay - nGuard - nTrain; k <= iDelay + nGuard + nTrain; ++k)
+      {
+        if (k >= 1 && k <= nDelayBins)
+        {
+          iTrain.push_back(k);
+        }
+      }
+
+      // compute threshold
+      int nCells = iTrain.size();
+      double alpha = nCells * (pow(pfa, -1.0 / nCells) - 1);
+      double trainNoise = 0.0;
+      for (int k = 0; k < nCells; ++k)
+      {
+        trainNoise += dataSquare[iDoppler][iTrain[k] - 1];
+      }
+      trainNoise /= nCells;
+      double threshold = alpha * trainNoise;
+
+      // detection if over threshold
+      if (dataSquare[iDoppler][iDelay] > threshold)
+      {
+        delay.push_back(iDelay + x->delay[0] - 1);
+        doppler.push_back(x->doppler[iDoppler]);
+        //snr.push_back(dataSnr[iDoppler][iDelay]);
+        snr.push_back(-1);
+      }
+    }
+  }
+
+  // create detection
+  Detection *detection = new Detection(delay, doppler, snr);
+
+  return detection;
+}

src/process/detection/CfarDetector1D.h

@@ -0,0 +1,49 @@
+/// @file CfarDetector1D.h
+/// @class CfarDetector1D
+/// @brief A class to implement a 1D CFAR detector.
+/// @details Converts an AmbiguityMap to DetectionData. 1D CFAR operates across delay, to minimise detections from the zero-Doppler line.
+/// @author 30hours
+/// @todo SNR value is broken in process(), temp value -1.
+
+#ifndef CFARDETECTOR1D_H
+#define CFARDETECTOR1D_H
+
+#include <Map.h>
+#include <Detection.h>
+#include <stdint.h>
+#include <complex>
+
+class CfarDetector1D
+{
+private:
+  /// @brief Probability of false alarm, numeric in [0,1]
+  double pfa;
+
+  /// @brief Number of single-sided guard cells.
+  int8_t nGuard;
+
+  /// @brief Number of single-sided training cells.
+  int8_t nTrain;
+
+  /// @brief Pointer to detection data to store result.
+  Detection *detection;
+
+public:
+  /// @brief Constructor.
+  /// @param pfa Probability of false alarm, numeric in [0,1].
+  /// @param nGuard Number of single-sided guard cells.
+  /// @param nTrain Number of single-sided training cells.
+  /// @return The object.
+  CfarDetector1D(double pfa, int8_t nGuard, int8_t nTrain);
+
+  /// @brief Destructor.
+  /// @return Void.
+  ~CfarDetector1D();
+
+  /// @brief Implement the 1D CFAR detector.
+  /// @param x Ambiguity map data of IQ samples.
+  /// @return Detections from the 1D CFAR detector.
+  Detection *process(Map<std::complex<double>> *x);
+};
+
+#endif