diff --git a/CMakeLists.txt b/CMakeLists.txt index dc612a7..b2b54f4 100644 --- a/CMakeLists.txt +++ b/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/") diff --git a/api/server.js b/api/server.js index bcea8e0..6fa9423 100644 --- a/api/server.js +++ b/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); diff --git a/api/server.js.old b/api/server.js.old deleted file mode 100644 index cca509e..0000000 --- a/api/server.js.old +++ /dev/null @@ -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}`); -}); diff --git a/config/radar4.yml b/config/radar4.yml index 330133d..22074b7 100644 --- a/config/radar4.yml +++ b/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/" diff --git a/src/blah2.cpp b/src/blah2.cpp index a406c89..47318ad 100644 --- a/src/blah2.cpp +++ b/src/blah2.cpp @@ -9,8 +9,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -79,7 +81,8 @@ int main(int argc, char **argv) IqData *y = new IqData(nSamples); Map> *map; Map *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(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(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(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(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(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(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(t7-t0).count(); + std::cout << "CPI time (ms): " << delta_t7 << std::endl; } } }); diff --git a/src/data/.IqData.h.swp b/src/data/.IqData.h.swp deleted file mode 100644 index 8c7f2d3..0000000 Binary files a/src/data/.IqData.h.swp and /dev/null differ diff --git a/src/data/Detection.cpp b/src/data/Detection.cpp new file mode 100644 index 0000000..ba5f774 --- /dev/null +++ b/src/data/Detection.cpp @@ -0,0 +1,120 @@ +#include "Detection.h" +#include +#include +#include + +#include "rapidjson/document.h" +#include "rapidjson/writer.h" +#include "rapidjson/stringbuffer.h" +#include "rapidjson/filewritestream.h" + +// constructor +Detection::Detection(std::vector _delay, std::vector _doppler, std::vector _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::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 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; +} diff --git a/src/data/Detection.h b/src/data/Detection.h new file mode 100644 index 0000000..ac048b3 --- /dev/null +++ b/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 +#include +#include + +class Detection +{ +private: + /// @brief Detections in delay (bins) + std::vector delay; + + /// @brief Detections in Doppler (Hz) + std::vector doppler; + + /// @brief Detections in SNR + std::vector snr; + +public: + /// @brief Constructor. + /// @param delay Detections in delay (bins). + /// @param doppler Detections in Doppler (Hz). + /// @return The object. + Detection(std::vector delay, std::vector doppler, std::vector 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 diff --git a/src/process/detection/CfarDetector1D.cpp b/src/process/detection/CfarDetector1D.cpp new file mode 100644 index 0000000..eb046b0 --- /dev/null +++ b/src/process/detection/CfarDetector1D.cpp @@ -0,0 +1,88 @@ +#include "CfarDetector1D.h" +#include +#include +#include + +// constructor +CfarDetector1D::CfarDetector1D(double _pfa, int8_t _nGuard, int8_t _nTrain) +{ + // input + pfa = _pfa; + nGuard = _nGuard; + nTrain = _nTrain; +} + +CfarDetector1D::~CfarDetector1D() +{ +} + +Detection *CfarDetector1D::process(Map> *x) +{ + std::vector> dataSnr; + std::vector> dataSquare; + + // compute square of Map + for (int i = 0; i < x->data.size(); i++) + { + std::vector dataSnrRow; + std::vector 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 delay; + std::vector doppler; + std::vector 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 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; +} diff --git a/src/process/detection/CfarDetector1D.h b/src/process/detection/CfarDetector1D.h new file mode 100644 index 0000000..f22b88b --- /dev/null +++ b/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 +#include +#include +#include + +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> *x); +}; + +#endif