From 69d47ef9a0f65d726f8a2d450c25ba4097e448ec Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 11:57:01 +0200 Subject: [PATCH 01/14] Updated searchers and added tracker --- src/algorithms/gradient_ascend.cpp | 186 ++++++++++++++++++----------- src/algorithms/gradient_ascend.h | 7 +- src/algorithms/pso_seeker.cpp | 3 - 3 files changed, 120 insertions(+), 76 deletions(-) diff --git a/src/algorithms/gradient_ascend.cpp b/src/algorithms/gradient_ascend.cpp index 40c48de..a899b3f 100644 --- a/src/algorithms/gradient_ascend.cpp +++ b/src/algorithms/gradient_ascend.cpp @@ -3,6 +3,7 @@ GradientParticle::GradientParticle(Antenna &antenna, Streams *streams) : antenna(antenna), streams(streams) { this->epsilon = 1e-9f; this->delta = TO_RADIANS(7); + this->tracking = false; random(); } @@ -19,31 +20,22 @@ void GradientParticle::jump() { directionCurrent.theta = clip(directionCurrent.theta, 0.0, M_PI / 2.0); } -void GradientParticle::nearby() { -#if 1 - std::vector near = directionCurrent.nearby(delta); -#else - // North - directionNearby[NORTH].theta = directionCurrent.theta + delta; - directionNearby[NORTH].phi = directionCurrent.phi; +void GradientParticle::step(double rate) { + update(); - // East - directionNearby[EAST].theta = directionCurrent.theta; - directionNearby[EAST].phi = directionCurrent.phi + delta;// / sin(epsilon + directionCurrent.theta); + Spherical newDirection; - // South - directionNearby[SOUTH].theta = directionCurrent.theta - delta; - directionNearby[SOUTH].phi = directionCurrent.phi; + newDirection.theta = directionCurrent.theta + rate * directionGradient.theta; + newDirection.phi = directionCurrent.phi + (rate * directionGradient.phi) / sin(1e-9 + directionCurrent.theta); - // West - directionNearby[WEST].theta = directionCurrent.theta; - directionNearby[WEST].phi = directionCurrent.phi - delta;// / sin(epsilon + directionCurrent.theta); -#endif - //for (Spherical& spherical : near) { - // spherical.phi = wrapAngle(spherical.phi); - // spherical.theta = clip(spherical.theta, 0.0, M_PI / 2.0); - // directionNearby[i] = - //} + newDirection.phi = wrapAngle(newDirection.phi); + newDirection.theta = clip(newDirection.theta, 0.0, M_PI / 2.0); + + directionCurrent = newDirection; +} + +void GradientParticle::nearby() { + std::vector near = directionCurrent.nearby(delta); for (int i = 0; i < 4; i++) { directionNearby[i] = near[i]; directionNearby[i].phi = wrapAngle(directionNearby[i].phi); @@ -79,12 +71,8 @@ void GradientParticle::update() { int i = antenna.index[s]; float fraction = fractional_delays[n][i]; - //std::cout << "Using index: " << i << std::endl; - int offset = offset_delays[n][i]; - //std::cout << offset << std::endl; - float *signal = streams->get_signal(i, offset); delay(&out[0], signal, fraction); //delay_corrected(&out[0], signal, fraction, antenna.power_correction_mask[s]); @@ -97,19 +85,17 @@ void GradientParticle::update() { power[n] /= (float) N_SAMPLES; } + float thetaPower = fmax(fabs(power[NORTH]), fabs(power[SOUTH])); + float phiPower = fmax(fabs(power[EAST]), fabs(power[WEST])); - float thetaPower = ((power[NORTH] + power[SOUTH]) / 2.0); - float phiPower = ((power[NORTH] + power[SOUTH]) / 2.0); - directionGradient.theta = (double) - ((power[NORTH] - power[SOUTH]) / thetaPower); /// (2.0 * delta)); + directionGradient.theta = (double) ((power[SOUTH] - power[NORTH]) / thetaPower);/// (2.0 * delta)); directionGradient.phi = (double) ((power[EAST] - power[WEST]) / phiPower);/// (2.0 * delta)); magnitude = (power[NORTH] + power[EAST] + power[SOUTH] + power[WEST]) / 4.0; - - //std::cout << "theta=" << directionGradient.theta << " phi=" << directionGradient.phi<< std::endl; + gradient = fabs(directionGradient.theta) + fabs(directionGradient.phi); } -SphericalGradient::SphericalGradient(Pipeline *pipeline, Antenna &antenna, bool *running, std::size_t swarm_size, std::size_t iterations) : Worker(pipeline, running), swarm_size(swarm_size), iterations(iterations) { - this->antenna = antenna; +SphericalGradient::SphericalGradient(Pipeline *pipeline, Antenna &antenna, bool *running, std::size_t swarm_size, std::size_t iterations) : Worker(pipeline, running), antenna(antenna), swarm_size(swarm_size), iterations(iterations) { this->streams = pipeline->getStreams(); thread_loop = std::thread(&SphericalGradient::loop, this); } @@ -119,7 +105,8 @@ void SphericalGradient::initialize_particles() { particles.clear(); for (int i = 0; i < swarm_size; i++) { particles.emplace_back(this->antenna, this->streams); - //GradientParticle &particle = particles.back(); + GradientParticle &particle = particles.back(); + particle.delta = TO_RADIANS(7.0);//1.0 + drandom() * 1.0); } } @@ -135,6 +122,33 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) { lock.lock(); +#if 1 + for (GradientParticle &particle: currentTrackers) { + if (!particle.tracking) { + continue; + } + // If we convert to sphere with radius 0.5 we don't have to normalize it other than add + // 0.5 to get the sphere in the first sector in the cartesian coordinate system + Cartesian position = Cartesian::convert(particle.directionCurrent, 1.0); + + // Use the position values to plot over the heatmap + int x = (int) (x_res * (position.x / 2.0 + 0.5)); + int y = (int) (y_res * (position.y / 2.0 + 0.5)); + + float gradient = 1.0 - clip(particle.gradient, 0.0, 2.0) / 2.0; + + heatmap->at(y, x) = (255); + + int m = 255;//(int) clip(particle.magnitude / 1e-6, 0.0, 255.0); + + cv::Mat &frame = *heatmap; + + cv::circle(frame, cv::Point(x, y), 1 + (int) (gradient * 10.0), cv::Scalar(m, m, m), cv::FILLED, 8, 0); + + + heatmap->at(y, x) = 255; + } +#else for (GradientParticle &particle: particles) { // If we convert to sphere with radius 0.5 we don't have to normalize it other than add // 0.5 to get the sphere in the first sector in the cartesian coordinate system @@ -144,11 +158,18 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) { int x = (int) (x_res * (position.x / 2.0 + 0.5)); int y = (int) (y_res * (position.y / 2.0 + 0.5)); - cv::circle(*heatmap, cv::Point(y, x), 3, cv::Scalar(255, 255, 255), cv::FILLED, 8, 0); + float gradient = 1.0 - clip(particle.gradient, 0.0, 2.0) / 2.0; + + heatmap->at(x,y) = (255); + + int m = (int)clip(particle.magnitude / 1e-6, 0.0, 255.0); + + cv::circle(*heatmap, cv::Point(y, x), 1 + (int)(gradient * 10.0), cv::Scalar(m,m,m), cv::FILLED, 8, 0); heatmap->at(x, y) = 255; } +#endif lock.unlock(); } @@ -156,35 +177,30 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) { void SphericalGradient::loop() { std::cout << "Starting loop" << std::endl; double learning_rate; - constexpr double start_rate = 5e-3; + constexpr double start_rate = 1e-1; // Place particles on dome int it = 0; initialize_particles(); + + int n_trackers = 9; + + for (int i = 0; i < n_trackers; i++) { + currentTrackers.emplace_back(this->antenna, this->streams); + GradientParticle& particle = currentTrackers.back(); + particle.delta = TO_RADIANS(3); + } + while (looping && pipeline->isRunning()) { // Wait for incoming data pipeline->barrier(); lock.lock(); - -// if (it % 40 == 0) { -// // Place particles on dome -// std::cout << "Replacing" << std::endl; -// for (auto &particle: particles) { -// particle.jump(); -// } -// //initialize_particles(); -// } -// -// if (it % 500 == 0) { -// // Place particles on dome -// //std::cout << "Replacing" << std::endl; -// //for (auto &particle: particles) { -// // particle.jump(); -// //} -// initialize_particles(); -// } -// it++; + + if (it % 500 == 0) { + initialize_particles(); + } + it++; @@ -197,26 +213,54 @@ void SphericalGradient::loop() { //std::cout << "Too slow: " << i + 1 << "/" << iterations << std::endl; break;// Too slow! } - for (auto &particle: particles) { - - particle.update(); - - - Spherical newDirection; - learning_rate = ((double)(iterations - i) / (double)iterations); - particle.delta = TO_RADIANS(learning_rate * 10.0); - - learning_rate *= start_rate; - newDirection.theta = particle.directionCurrent.theta + learning_rate * particle.directionGradient.theta; - newDirection.phi = particle.directionCurrent.phi + (learning_rate * particle.directionGradient.phi)/ sin(1e-9+particle.directionCurrent.theta); - - newDirection.phi = wrapAngle(newDirection.phi); - newDirection.theta = clip(newDirection.theta, 0.0, M_PI / 2.0); + int n_tracking = 0; + for (auto &particle : currentTrackers) { + if (particle.tracking) { + for (int i = 0; i < 5; i++) + particle.step(start_rate / 100.0); + if (particle.gradient > 1.0) { + particle.tracking = false; + } else { + n_tracking++; + } + } + } - particle.directionCurrent = newDirection; + for (int m = 0; m < n_trackers; m++) { + if (!currentTrackers[m].tracking) { + continue; + } + + for (int n = m+1; n < n_trackers; n++) { + if (!currentTrackers[n].tracking) { + continue; + } + if (currentTrackers[m].directionCurrent.angle(currentTrackers[n].directionCurrent) < M_PI / 20.0) { + currentTrackers[m].tracking = false; + } + } + } + for (auto &particle: particles) { + particle.step(start_rate); + if (n_tracking < n_trackers) { + if (particle.gradient < 1e-5) { + for (auto &tracker : currentTrackers) { + if (!tracker.tracking && tracker.directionCurrent.angle(particle.directionCurrent) > M_PI / 20.0) { + tracker.tracking = true; + tracker.directionCurrent = particle.directionCurrent; + } + } + } + } } } + tracking.clear(); + for (auto &tracker : currentTrackers) { + if (tracker.tracking) { + tracking.emplace_back(tracker.directionCurrent, tracker.magnitude, 1 / tracker.gradient); + } + } lock.unlock(); } diff --git a/src/algorithms/gradient_ascend.h b/src/algorithms/gradient_ascend.h index 6b79621..99cef2d 100644 --- a/src/algorithms/gradient_ascend.h +++ b/src/algorithms/gradient_ascend.h @@ -12,11 +12,13 @@ class GradientParticle { public: + bool tracking; Spherical directionCurrent; Spherical directionNearby[4]; Spherical directionGradient; Spherical directionBest; float magnitude; + float gradient; Antenna &antenna; Streams *streams; @@ -30,6 +32,7 @@ class GradientParticle { void nearby(); void jump(); + void step(double rate); void random(); void update(); @@ -72,9 +75,9 @@ class SphericalGradient : public Worker { std::size_t iterations; std::size_t swarm_size; Streams *streams; - Antenna antenna; + Antenna &antenna; std::vector particles; - + std::vector currentTrackers; }; diff --git a/src/algorithms/pso_seeker.cpp b/src/algorithms/pso_seeker.cpp index 9c004bd..6c27d92 100644 --- a/src/algorithms/pso_seeker.cpp +++ b/src/algorithms/pso_seeker.cpp @@ -9,9 +9,6 @@ #define USE_LUT 0 -#define SWARM_SIZE 100 -#define SWARM_ITERATIONS - //#define IN_FIRST_SECTOR(i) ((i < 28) && ) From 6d840a6874392291d9da1297056a74f1188d171e Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 11:57:28 +0200 Subject: [PATCH 02/14] Added synthetic data generation --- src/awpu.cpp | 38 +++++++++++++++++++++++++++++++++----- src/awpu.h | 4 +++- src/awpu_main.cpp | 33 ++++++++++++++++++++++++++++++--- 3 files changed, 66 insertions(+), 9 deletions(-) diff --git a/src/awpu.cpp b/src/awpu.cpp index 964dee3..248d086 100644 --- a/src/awpu.cpp +++ b/src/awpu.cpp @@ -4,13 +4,22 @@ AWProcessingUnit::AWProcessingUnit(const char *address, const int port, int verb // Allocate memory for pipeline this->pipeline = new Pipeline(address, port); + + // Connect to FPGA +#if 1 this->pipeline->connect(); + int n_sources = this->pipeline->get_n_sensors() / ELEMENTS; +#else + this->spherical.theta = TO_RADIANS(0); + this->spherical.phi = TO_RADIANS(0); // = Spherical(TO_RADIANS(30), TO_RADIANS(0)); + this->pipeline->start_synthetic(this->spherical); + int n_sources = 1; +#endif this->running = false; - for (int a = 0; a < this->pipeline->get_n_sensors() / ELEMENTS; a++) { + for (int a = 0; a < n_sources; a++) { Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); - //antennas.emplace_back(); antennas.push_back(antenna); } } @@ -50,7 +59,7 @@ bool AWProcessingUnit::start(const worker_t worker) { job = nullptr; break; case GRADIENT: - job = (Worker *) new SphericalGradient(pipeline, antennas[0], &running, 10, 30); + job = (Worker *) new SphericalGradient(pipeline, antennas[0], &running, 50, 10); break; default: return false; @@ -175,6 +184,25 @@ void AWProcessingUnit::calibrate(const float reference_power_level) { } } +void AWProcessingUnit::synthetic_calibration() { + for (int a = 0; a < antennas.size(); a++) { + Antenna &antenna = antennas[a]; + antenna.usable = ELEMENTS; + antenna.index = new int[antenna.usable]; + for (int i = 0; i < ELEMENTS; i++) { + antenna.index[i] = i; + } + + std::cout << "Calibrated antenna " << a << " Usable: " << antenna.usable << " Mean: " << antenna.mean << " Median: " << antenna.median << std::endl; + + for (int s = 0; s < antenna.usable; s++) { + std::cout << "Mic: " << antenna.index[s] << " Correction: " << antenna.power_correction_mask[s] << std::endl; + } + + std::cout << std::endl; + } +} + bool AWProcessingUnit::stop(const worker_t worker) { for (auto it = workers.begin(); it != workers.end();) { if ((*it)->get_type() == worker) { @@ -205,6 +233,6 @@ void AWProcessingUnit::draw_heatmap(cv::Mat *heatmap) { workers[0]->draw_heatmap(heatmap); } -Spherical AWProcessingUnit::target() { - return workers[0]->getDirection(); +std::vector AWProcessingUnit::targets() { + return workers[0]->getTargets(); } \ No newline at end of file diff --git a/src/awpu.h b/src/awpu.h index 9df6062..19a058a 100644 --- a/src/awpu.h +++ b/src/awpu.h @@ -23,7 +23,8 @@ class AWProcessingUnit { void resume(); void draw_heatmap(cv::Mat *heatmap); void calibrate(const float reference_power_level = 1e-5); - Spherical target(); + void synthetic_calibration(); + std::vector targets(); protected: @@ -31,6 +32,7 @@ class AWProcessingUnit { std::vector workers; Pipeline *pipeline; bool running; + Spherical spherical; std::vector antennas; }; diff --git a/src/awpu_main.cpp b/src/awpu_main.cpp index 7ec654f..5c12af6 100644 --- a/src/awpu_main.cpp +++ b/src/awpu_main.cpp @@ -35,7 +35,7 @@ void point3d(const Spherical& spherical1, const Spherical& spherical2, const dou std::cout << "Point: (" < targets = awpu8.targets(); + + //std::cout << "Tracking: " << targets.size() << " objects" << std::endl; + + //for () //awpu.draw_heatmap(&frame); //std::cout << "Best direction: " << awpu5.target() << std::endl; // Apply color map @@ -114,4 +125,20 @@ int main() { std::cout << "Stopping program" << std::endl; return 0; -} \ No newline at end of file +} + +#else + +int main(int argc, char const *argv[]) +{ + Spherical a(TO_RADIANS(90), TO_RADIANS(179)); + Spherical b(TO_RADIANS(90), TO_RADIANS(0)); + + double theta = a.angle(b); + + std::cout << theta << std::endl; + + return 0; +} + +#endif \ No newline at end of file From 0cf469927623e5fc02118aeeb597f57935653ca4 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 11:57:46 +0200 Subject: [PATCH 03/14] Created rotation functions --- src/geometry.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/geometry.h | 4 ++++ 2 files changed, 40 insertions(+) diff --git a/src/geometry.cpp b/src/geometry.cpp index 759833a..9bd7785 100644 --- a/src/geometry.cpp +++ b/src/geometry.cpp @@ -87,6 +87,26 @@ Eigen::Vector3d Spherical::toCartesian() { return cartesian; } +Eigen::Vector3d Spherical::toCartesian() const { + Eigen::Vector3d cartesian; + cartesian(0) = radius * sin(theta) * cos(phi); + cartesian(1) = radius * sin(theta) * sin(phi); + cartesian(2) = radius * cos(theta); + + return cartesian; +} + +double Spherical::angle(const Spherical &spherical) { + // Function to compute the geodesic distance between two points on a sphere + double sin_theta1 = sin(M_PI / 2.0 - theta); + double sin_theta2 = sin(M_PI / 2.0 - spherical.theta); + double cos_theta1 = cos(M_PI / 2.0 - theta); + double cos_theta2 = cos(M_PI / 2.0 - spherical.theta); + double delta_phi = phi - spherical.phi; + + return acos(sin_theta1 * sin_theta2 + cos_theta1 * cos_theta2 * cos(delta_phi)); +} + Eigen::MatrixXd rotateTo(const Eigen::MatrixXd points, const double theta, const double phi) { Eigen::Matrix3d Rz, Rx, Ry; @@ -147,4 +167,20 @@ std::vector Spherical::nearby(const double spread) { } return near; +} + +Eigen::Matrix3f rotateZ(const float angle) { + Eigen::Matrix3f Rz; + Rz << (float) cos((double)angle), -(float) sin((double)angle), 0.0f, + (float) sin((double)angle), (float) cos((double)angle), 0.0f, + 0.0f, 0.0f, 1.0f; + return Rz; +} + +Eigen::Matrix3f rotateY(const float angle) { + Eigen::Matrix3f Ry; + Ry << (float) cos((double)angle), 0.0, (float) sin((double)angle),// + 0.0, 1.0, 0.0, // + -(float) sin((double)angle), 0.0, (float) cos((double)angle); + return Ry; } \ No newline at end of file diff --git a/src/geometry.h b/src/geometry.h index 61872b7..8f02c70 100644 --- a/src/geometry.h +++ b/src/geometry.h @@ -27,6 +27,9 @@ double smallestAngle(const double target, const double current); typedef Eigen::Vector3f Position; +Eigen::Matrix3f rotateZ(const float angle); +Eigen::Matrix3f rotateY(const float angle); + /** * Convert spherical coordinates to cartesian coordinates */ @@ -62,6 +65,7 @@ struct Spherical { double distanceTo(const Spherical &spherical); std::vector nearby(const double spread); + double angle(const Spherical &spherical); static Eigen::Vector3d toCartesian(const Spherical &spherical); Eigen::Vector3d toCartesian() const; From b32bdeea5fc13016992e54a03970a4b04fd6d982 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 11:58:09 +0200 Subject: [PATCH 04/14] Added synthetic sine generation --- src/pipeline.cpp | 76 ++++++++++++++++++++++++++++++++++++++++++++++++ src/pipeline.h | 6 ++++ 2 files changed, 82 insertions(+) diff --git a/src/pipeline.cpp b/src/pipeline.cpp index d2663c8..d0a8138 100644 --- a/src/pipeline.cpp +++ b/src/pipeline.cpp @@ -58,6 +58,82 @@ int Pipeline::connect() { return 0; } +void Pipeline::start_synthetic(const Spherical &angle) { + n_sensors = ELEMENTS; + + // Allocate memory for UDP packet data + exposure_buffer = new float*[n_sensors]; + + for (int sensor_index = 0; sensor_index < n_sensors; sensor_index++) { + exposure_buffer[sensor_index] = new float[N_SAMPLES]; + this->streams->create_stream(sensor_index); + +#if DEBUG_PIPELINE + std::cout << "[Pipeline] Adding stream: " << sensor_index << std::endl; +#endif + } + + connected = 1; + + receiver_thread = std::thread(&Pipeline::synthetic_producer, this, angle); +} + +#define PHASE(delay, frequency) 2 * M_PI * frequency * delay + +void Pipeline::synthetic_producer(const Spherical &angle) { + Antenna antenna = create_antenna(Position(0,0,0), COLUMNS, ROWS, DISTANCE); + //Spherical angle(TO_RADIANS(30), TO_RADIANS(0)); + Eigen::VectorXf tmp_delays1 = steering_vector_spherical(antenna, angle.theta, angle.phi); + Eigen::VectorXf tmp_delays2 = steering_vector_spherical(antenna, angle.theta + TO_RADIANS(10), angle.phi); + std::cout << tmp_delays1 << std::endl; + + const std::chrono::milliseconds targetDuration(static_cast(1000.0 * N_SAMPLES / SAMPLE_RATE)); + int p = 0; + int p2 = 0; + constexpr float carrier = 4e3; + constexpr float carrier2 = 4e3; + while (isRunning()) { + + auto start = std::chrono::steady_clock::now(); + + float signals[ELEMENTS][N_SAMPLES]; + + for (int s = 0; s < ELEMENTS; s++) { + float delay1 = tmp_delays1(s); + float delay2 = tmp_delays2(s); + for (int i = 0; i < N_SAMPLES; i++) { + signals[s][i] = 0.0; + float t = static_cast(p + i) / SAMPLE_RATE;// Time in seconds + signals[s][i] += sin(2 * M_PI * carrier * t + PHASE(delay1, carrier)); + signals[s][i] += sin(2 * M_PI * carrier2 * t + PHASE(delay2, carrier2)); + + signals[s][i] /= 10.0; + } + streams->write_stream(s, &signals[s][0]); + } + + p += N_SAMPLES; + p2 += N_SAMPLES; + + + { + std::unique_lock lock(barrier_mutex); + streams->forward(); + } + // Measure time taken + auto end = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + // Sleep if the elapsed time is less than 5 ms + if (elapsed < targetDuration) { + std::this_thread::sleep_for(targetDuration - elapsed); + //std::cout << "Sleeping: " << 0 << " " << streams->get_signal(0, 0)[5] << std::endl; + } + + release_barrier(); + } +} + /** * Disconnect beamformer from antenna */ diff --git a/src/pipeline.h b/src/pipeline.h index f86f36b..e2a49d4 100644 --- a/src/pipeline.h +++ b/src/pipeline.h @@ -4,6 +4,8 @@ #include "options.h" #include "receiver.h" #include "streams.hpp" +#include "geometry.h" +#include "antenna.h" #include #include @@ -60,6 +62,10 @@ class Pipeline { int save_pipeline(std::string path); + void start_synthetic(const Spherical &angle); + void synthetic_producer(const Spherical &angle); + + private: // Buffer for storing incoming UDP mic data float **exposure_buffer; From ffb8fedfd71e1935309976702b6b7096132b89c2 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 11:58:28 +0200 Subject: [PATCH 05/14] Updated antenna coordinates --- src/antenna.cpp | 190 +++--------------------------------------------- 1 file changed, 9 insertions(+), 181 deletions(-) diff --git a/src/antenna.cpp b/src/antenna.cpp index 7fffcb3..d78aef9 100644 --- a/src/antenna.cpp +++ b/src/antenna.cpp @@ -10,99 +10,6 @@ #include -typedef double Radian; - -///** -// * Compute the distance between two spherical directions -// */ -//double Spherical::distanceTo(const Spherical &spherical) { -// return sqrt( -// 2.0 - 2.0 * ( -// sin(this->theta) * sin(spherical.theta) * cos(this->phi - spherical.phi) + cos(this->theta) * cos(spherical.theta) -// ) -// ); -//} -// -//Spherical Horizontal::toSpherical(const Horizontal &horizontal) { -// double x = sin(horizontal.azimuth); -// double y = sin(horizontal.elevation); -// double phi = atan2(y, x); -// -// // We assume elevation 0 is horizontal -// double flipped_theta = PI_HALF - horizontal.elevation; -// -// // z -// double z_height = sin(flipped_theta) * cos(horizontal.azimuth); -// double theta = PI_HALF - asin(z_height); -// -// return Spherical(theta, phi); -//} -// -//Horizontal Spherical::toHorizontal(const Spherical &spherical) { -// Position position = spherical_to_cartesian(spherical.theta, spherical.phi, 1.0); -// double elevation = asin(position(Y_INDEX)); -// double azimuth = asin(position(X_INDEX)); -// -// return Horizontal(azimuth, elevation); -//} -// -// -/////** -//// * Convert spherical coordinates to cartesian coordinates -//// */ -////Position Spherical::toCartesian(const Spherical &spherical, const double radius = 1.0) { -//// Position point; -//// -//// point(X_INDEX) = (float) (radius * (sin(spherical.theta) * cos(spherical.phi))); -//// point(Y_INDEX) = (float) (radius * (sin(spherical.theta) * sin(spherical.phi))); -//// point(Z_INDEX) = (float) (radius * (cos(spherical.theta))); -//// -//// return point; -////} -// -// -//Cartesian Cartesian::convert(const Spherical &spherical, const double radius = 1.0) { -// Cartesian point; -// -// point.x = (radius * (sin(spherical.theta) * cos(spherical.phi))); -// point.y = (radius * (sin(spherical.theta) * sin(spherical.phi))); -// point.z = (radius * (cos(spherical.theta))); -// -// return point; -//} - - -/** - * return np.sqrt( - 2 - - 2 - * ( - np.sin(direction1[0]) - * np.sin(direction2[0]) - * np.cos(direction1[1] - direction2[1]) - + np.cos(direction1[0]) * np.cos(direction2[0]) - ) - ) - */ -//double Direction::distanceTo(const double theta, const double phi) { -// return sqrt( -// 2.0 - 2.0 * ( -// sin(this->theta) * sin(theta) * cos(this->phi - phi) + cos(this->theta) * cos(theta) -// ) -// ); -//} -// -//double Direction::distanceTo(const Direction &direction) { -// return this->distanceTo(direction.theta, direction.phi); -//} - -//double Direction::distanceTo(const Direction &direction) { -// -//} -//Direction Direction::directionTo(const Direction &direction, const float step) { -// -//} - Direction test(double azimuth, double elevation) { double x = sin(azimuth); double y = sin(elevation); @@ -187,15 +94,15 @@ void place_antenna(Antenna &antenna, const Position position) { Antenna create_antenna(const Position &position, const int columns, const int rows, const float distance) { float half = distance / 2; - Eigen::MatrixXf points(rows * columns, 3);// (id, X|Y|Z) + Eigen::MatrixXf points(3, rows * columns);// (id, X|Y|Z) // Compute the positions of the antenna in 3D space int i = 0; - for (int y = 0; y < columns; y++) { - for (int x = 0; x < rows; x++) { - points(i, X_INDEX) = x * distance - rows * half + half; - points(i, Y_INDEX) = y * distance - columns * half + half; - points(i, Z_INDEX) = 0.f; + for (int r = 0; r < rows; r++) { + for (int c = 0; c < columns; c++) { + points(X_INDEX, i) = (float)c * distance - rows * half + half; + points(Y_INDEX, i) = (float)r * distance - columns * half + half; + points(Z_INDEX, i) = 0.f; i++; } @@ -208,7 +115,7 @@ Antenna create_antenna(const Position &position, const int columns, antenna.points = points; // Now we place the antenna for the user - place_antenna(antenna, position); + //place_antenna(antenna, position); return antenna; } @@ -221,7 +128,7 @@ Antenna create_antenna(const Position &position, const int columns, */ Eigen::VectorXf compute_delays(const Antenna &antenna) { Eigen::VectorXf delays = - antenna.points.col(Z_INDEX).array() * (SAMPLE_RATE / PROPAGATION_SPEED); + antenna.points.row(Z_INDEX).array() * (SAMPLE_RATE / PROPAGATION_SPEED); // There is no need to delay the element that should be closest to source delays.array() -= delays.minCoeff(); @@ -239,41 +146,15 @@ Eigen::VectorXf compute_delays(const Antenna &antenna) { * elements have crossed paths. */ inline Antenna steer(const Antenna &antenna, const double theta, const double phi) { - Eigen::Matrix3f Rz1, Rx, Rz2; - - Rz1 << (float) cos(phi), -(float) sin(phi), 0.0f, - (float) sin(phi), (float) cos(phi), 0.0f, - 0.0f, 0.0f, 1.0f; - Rx << 1.0f, 0.0f, 0.0f, - 0.0f, (float) cos(theta), -(float) sin(theta), - 0.0f, (float) sin(theta), (float) cos(theta); - Rz2 << (float) cos(-phi), -(float) sin(-phi), 0.0f, - (float) sin(-phi), (float) cos(-phi), 0.0f, - 0.0f, 0.0f, 1.0f; // Perform the rotation. Order of operations are important - Eigen::Matrix3f rotation = Rz2 * (Rx * Rz1); - Antenna rotated; - rotated.points = antenna.points * rotation; + rotated.points = (rotateY(-(float)theta) * (rotateZ((float)phi) * antenna.points)); rotated.id = antenna.id; return rotated; } -///** -// * Convert spherical coordinates to cartesian coordinates -// */ -//Position spherical_to_cartesian(const double theta, const double phi, const double radius = 1.0) { -// Position point; -// -// point(X_INDEX) = (float) (radius * (sin(theta) * cos(phi))); -// point(Y_INDEX) = (float) (radius * (sin(theta) * sin(phi))); -// point(Z_INDEX) = (float) (radius * (cos(theta))); -// -// return point; -//} - /** * Steer the antenna using horizontal angles. bore-sight is the x-axis and azimuth is the left-to right angles and elevation * is up and down. @@ -393,56 +274,3 @@ void test_lookup_table(const Eigen::MatrixXf &dome, const Eigen::MatrixXi &looku } std::cout << "Completed " << TEST_CASES - failed_tests << "/" << TEST_CASES << " tests" << std::endl; } - -#if 0 - - - -//void printSpherical(const Spherical &direction) { -// std::cout << "Spherical: θ=" << degrees(direction.theta) << " φ=" << degrees(direction.phi) << " " << std::endl; -//} -// -//void printHorizontal(const Horizontal &direction) { -// std::cout << "Spherical: x=" << degrees(direction.azimuth) << " y=" << degrees(direction.elevation) << " " << std::endl; -//} - -int main() { - - double theta1 = TO_RADIANS(45); - double phi1 = TO_RADIANS(10); - - double theta2 = TO_RADIANS(45); - double phi2 = TO_RADIANS(11); - - Spherical direction(theta1, phi1); - - std::cout << "second " << direction << std::endl; - - Spherical other(theta2, phi2); - - std::cout << "first " << other << std::endl; - - double azimuth = TO_RADIANS(30); - double elevation = TO_RADIANS(-10); - Horizontal horizontal(azimuth, elevation); - - Spherical spherical = Horizontal::toSpherical(horizontal); - - Horizontal h2 = Spherical::toHorizontal(spherical); - - std::cout << horizontal << " -> " << spherical << std::endl; - - std::cout << spherical << " -> " << h2 << std::endl; - - //double distance = direction.distanceTo(other); - - //std::cout << "Distance to: " << distance << std::endl; - - //Horizontal newd = test(TO_RADIANS(0), TO_RADIANS(1)); - //std::cout << "New " << newd << std::endl; - - - return 0; -} - -#endif From 4b8d798b62c86720481eae34bc11d22ebf460b03 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 11:58:41 +0200 Subject: [PATCH 06/14] Added target --- src/worker.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/worker.h b/src/worker.h index 63dcaf5..472cdbf 100644 --- a/src/worker.h +++ b/src/worker.h @@ -3,11 +3,18 @@ #include #include // cv::Mat +#include "geometry.h" inline double drandom() { return static_cast(rand()) / RAND_MAX; } +struct Target { + Spherical direction; + float power; + float probability; +}; + enum worker_t { PSO, @@ -46,8 +53,11 @@ class Worker { void loop() {}; + std::vector getTargets() { return tracking;}; + protected: Spherical direction; + std::vector tracking; std::mutex lock; }; From 0465fd284838158f2678d9bd4e3e27169433e194 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 13:46:12 +0200 Subject: [PATCH 07/14] Documented code --- src/awpu.cpp | 2 +- src/geometry.h | 32 ++++++++++++++++++++------------ src/pipeline.cpp | 15 +++++++-------- src/pipeline.h | 14 +++++++------- src/receiver.h | 1 - 5 files changed, 35 insertions(+), 29 deletions(-) diff --git a/src/awpu.cpp b/src/awpu.cpp index 248d086..0ff275a 100644 --- a/src/awpu.cpp +++ b/src/awpu.cpp @@ -18,7 +18,7 @@ AWProcessingUnit::AWProcessingUnit(const char *address, const int port, int verb #endif this->running = false; - for (int a = 0; a < n_sources; a++) { + for (int n_antenna = 0; n_antenna < n_sources; n_antenna++) { Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); antennas.push_back(antenna); } diff --git a/src/geometry.h b/src/geometry.h index 8f02c70..f372245 100644 --- a/src/geometry.h +++ b/src/geometry.h @@ -18,16 +18,32 @@ #define SOUTH 2 #define WEST 3 +/** + * Clip value between two values + */ double clip(const double n, const double lower, const double upper); +/** + * Modulo for radians + */ double wrapAngle(const double angle); +/** + * Retrieve the relative smallest angle between two angles + */ double smallestAngle(const double target, const double current); typedef Eigen::Vector3f Position; +/** + * Rotation around Z-axis + */ Eigen::Matrix3f rotateZ(const float angle); + +/** + * Rotation around Y-axis + */ Eigen::Matrix3f rotateY(const float angle); /** @@ -39,17 +55,9 @@ Position spherical_to_cartesian(const double theta, const double phi, const doub struct Spherical; struct Horizontal;// Horizontal with a 90degree rotation on y-axis -inline double degrees(const double angle) { - return angle * 180.0 / M_PI; -} - -struct Direction { - double theta, phi; - - Direction(){}; - Direction(double theta, double phi) : theta(theta), phi(phi){}; -}; - +/** + * Spherical representation of a position + */ struct Spherical { double theta;// Inclination angle double phi; // Azimuth angle @@ -95,7 +103,7 @@ struct Horizontal { //} friend std::ostream &operator<<(std::ostream &out, const Horizontal &direction) { - out << "Horizontal: azimuth=" << degrees(direction.azimuth) << " elevation=" << degrees(direction.elevation); + out << "Horizontal: azimuth=" << TO_DEGREES(direction.azimuth) << " elevation=" << TO_DEGREES(direction.elevation); return out; } }; diff --git a/src/pipeline.cpp b/src/pipeline.cpp index d0a8138..c9322f1 100644 --- a/src/pipeline.cpp +++ b/src/pipeline.cpp @@ -29,8 +29,7 @@ int Pipeline::connect() { if (socket_desc == -1) { std::cerr << "Unable to establish a connection to antenna" << std::endl; return -1; - } - else { + } else { connected = 1; } @@ -42,7 +41,7 @@ int Pipeline::connect() { n_sensors = msg.n_arrays * ELEMENTS; // Allocate memory for UDP packet data - exposure_buffer = new float*[n_sensors]; + exposure_buffer = new float *[n_sensors]; for (int sensor_index = 0; sensor_index < n_sensors; sensor_index++) { exposure_buffer[sensor_index] = new float[N_SAMPLES]; @@ -62,7 +61,7 @@ void Pipeline::start_synthetic(const Spherical &angle) { n_sensors = ELEMENTS; // Allocate memory for UDP packet data - exposure_buffer = new float*[n_sensors]; + exposure_buffer = new float *[n_sensors]; for (int sensor_index = 0; sensor_index < n_sensors; sensor_index++) { exposure_buffer[sensor_index] = new float[N_SAMPLES]; @@ -78,15 +77,15 @@ void Pipeline::start_synthetic(const Spherical &angle) { receiver_thread = std::thread(&Pipeline::synthetic_producer, this, angle); } -#define PHASE(delay, frequency) 2 * M_PI * frequency * delay +#define PHASE(delay, frequency) 2 * M_PI *frequency *delay void Pipeline::synthetic_producer(const Spherical &angle) { - Antenna antenna = create_antenna(Position(0,0,0), COLUMNS, ROWS, DISTANCE); + Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); //Spherical angle(TO_RADIANS(30), TO_RADIANS(0)); Eigen::VectorXf tmp_delays1 = steering_vector_spherical(antenna, angle.theta, angle.phi); Eigen::VectorXf tmp_delays2 = steering_vector_spherical(antenna, angle.theta + TO_RADIANS(10), angle.phi); std::cout << tmp_delays1 << std::endl; - + const std::chrono::milliseconds targetDuration(static_cast(1000.0 * N_SAMPLES / SAMPLE_RATE)); int p = 0; int p2 = 0; @@ -234,7 +233,7 @@ void Pipeline::receive_exposure() { int inverted = 0; // Actual microphone index - unsigned index; + unsigned index; for (unsigned sensor_index = 0; sensor_index < n_sensors; sensor_index++) { // Arrays are daisy-chained so flip on columns diff --git a/src/pipeline.h b/src/pipeline.h index e2a49d4..80130d3 100644 --- a/src/pipeline.h +++ b/src/pipeline.h @@ -1,12 +1,6 @@ #ifndef PIPELINE_H #define PIPELINE_H -#include "options.h" -#include "receiver.h" -#include "streams.hpp" -#include "geometry.h" -#include "antenna.h" - #include #include #include @@ -18,6 +12,12 @@ #include #include +#include "antenna.h" +#include "geometry.h" +#include "options.h" +#include "receiver.h" +#include "streams.hpp" + #define DEBUG_PIPELINE 1 /** @@ -85,7 +85,7 @@ class Pipeline { // Number of sensors in current constallation int n_sensors; - // Ring buffer for storing data + // Ring buffer for storing data Streams *streams; // Receiver thread diff --git a/src/receiver.h b/src/receiver.h index 849d420..2f055da 100644 --- a/src/receiver.h +++ b/src/receiver.h @@ -9,7 +9,6 @@ #include #include - #include "config.h" From e982ed4ac35e6e327a47dba410b2840f9762a366 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 13:46:40 +0200 Subject: [PATCH 08/14] Documented classes --- src/algorithms/gradient_ascend.cpp | 2 +- src/antenna.cpp | 23 ++++++----------------- 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/src/algorithms/gradient_ascend.cpp b/src/algorithms/gradient_ascend.cpp index a899b3f..2db607f 100644 --- a/src/algorithms/gradient_ascend.cpp +++ b/src/algorithms/gradient_ascend.cpp @@ -122,7 +122,7 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) { lock.lock(); -#if 1 +#if 0 for (GradientParticle &particle: currentTrackers) { if (!particle.tracking) { continue; diff --git a/src/antenna.cpp b/src/antenna.cpp index d78aef9..ca63853 100644 --- a/src/antenna.cpp +++ b/src/antenna.cpp @@ -10,21 +10,9 @@ #include -Direction test(double azimuth, double elevation) { - double x = sin(azimuth); - double y = sin(elevation); - double phi = atan2(y, x); - - // We assume elevation 0 is horizontal - double flipped_theta = PI_HALF - elevation; - - // z - double z_height = sin(flipped_theta) * cos(azimuth); - double theta = PI_HALF - asin(z_height); - - return Direction(theta, phi); -} - +/** + * Check if integer is in sector + */ bool in_sector(const int *sector, const int i) { return (((sector[0] <= i) && (i <= sector[3])) || ((sector[4] <= i) && (i <= sector[7])) || @@ -32,6 +20,9 @@ bool in_sector(const int *sector, const int i) { ((sector[12] <= i) && (i <= sector[15]))); } +/** + * Check if integer is in sector + */ bool in_sector(const int sector_index, const int i) { const int *sector; @@ -78,8 +69,6 @@ inline Position find_middle(const Antenna &antenna) { return antenna.points.colwise().mean(); } -// ********** Antenna in space ********** - /** * Place the antenna by positioning the center @ new position */ From 0e27b7e2da387a7bee3587fc55f2dc3baa8f27e2 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 13:47:06 +0200 Subject: [PATCH 09/14] Documentation and formatting --- src/worker.h | 59 ++++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 50 insertions(+), 9 deletions(-) diff --git a/src/worker.h b/src/worker.h index 472cdbf..6b432e9 100644 --- a/src/worker.h +++ b/src/worker.h @@ -2,21 +2,35 @@ #define WORKER_H #include -#include // cv::Mat +#include // cv::Mat + #include "geometry.h" +/** + * Random double between 0.0 and 1.0 + */ inline double drandom() { return static_cast(rand()) / RAND_MAX; } +/** + * Target from the AWPU + */ struct Target { + // Direction of target in spherical coordinate system Spherical direction; + + // Power level in the direction float power; + + // Pseudo-scientific value of how valid the target actually is float probability; }; +/** + * Worker types for beamforming algorithms + */ enum worker_t { - PSO, MIMO, SOUND, @@ -25,13 +39,20 @@ enum worker_t { class Worker { public: + // If the worker should be running bool *running; + + // If the worker is running bool looping; + + // Worker thread since parent thread is returned std::thread thread_loop; + + // Inherited pipeline Pipeline *pipeline; - Worker(Pipeline *pipeline, bool *running) : looping(true), pipeline(pipeline), running(running) {}; + Worker(Pipeline *pipeline, bool *running) : looping(true), pipeline(pipeline), running(running){}; - ~Worker(){ + ~Worker() { looping = false; std::cout << "Waiting for thread to return" << std::endl; thread_loop.join(); @@ -40,24 +61,44 @@ class Worker { std::cout << "Destroyed worker" << std::endl; }; + /** + * Worker type + */ worker_t get_type(); - Spherical getDirection() { + /** + * Getter for worker direction + */ + Spherical getDirection() const { return direction; }; - virtual void draw_heatmap(cv::Mat *heatmap) - { + /** + * Getter for current targets + */ + std::vector getTargets() const { return tracking; }; + + /** + * Draw values onto a heatmap + */ + virtual void draw_heatmap(cv::Mat *heatmap) { std::cout << "Wrong drawer" << std::endl; }; - void loop() {}; + /** + * Worker main loop + */ + virtual void loop() {}; - std::vector getTargets() { return tracking;}; protected: + // Current direction Spherical direction; + + // Current tracking objects std::vector tracking; + + // Lock to prevent multiple access from outside and inside std::mutex lock; }; From ebf5943d28957efc9bf62a7263026fd20de7a679 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 15:11:40 +0200 Subject: [PATCH 10/14] Added autocalibration on boot --- src/awpu.cpp | 30 +++++++++++++++++++----------- src/awpu.h | 3 ++- src/awpu_main.cpp | 25 ++++++------------------- 3 files changed, 27 insertions(+), 31 deletions(-) diff --git a/src/awpu.cpp b/src/awpu.cpp index 0ff275a..52b9d76 100644 --- a/src/awpu.cpp +++ b/src/awpu.cpp @@ -1,27 +1,30 @@ #include "awpu.h" -AWProcessingUnit::AWProcessingUnit(const char *address, const int port, int verbose) : verbose(verbose) { +AWProcessingUnit::AWProcessingUnit(const char *address, const int port, int verbose, bool debug) : verbose(verbose), debug(debug){ // Allocate memory for pipeline this->pipeline = new Pipeline(address, port); - + int n_sources; // Connect to FPGA -#if 1 - this->pipeline->connect(); - int n_sources = this->pipeline->get_n_sensors() / ELEMENTS; -#else - this->spherical.theta = TO_RADIANS(0); - this->spherical.phi = TO_RADIANS(0); // = Spherical(TO_RADIANS(30), TO_RADIANS(0)); - this->pipeline->start_synthetic(this->spherical); - int n_sources = 1; -#endif + if (debug) { + this->spherical.theta = TO_RADIANS(0); + this->spherical.phi = TO_RADIANS(0);// = Spherical(TO_RADIANS(30), TO_RADIANS(0)); + this->pipeline->start_synthetic(this->spherical); + n_sources = 1; + } else { + this->pipeline->connect(); + n_sources = this->pipeline->get_n_sensors() / ELEMENTS; + } + this->running = false; for (int n_antenna = 0; n_antenna < n_sources; n_antenna++) { Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); antennas.push_back(antenna); } + + calibrate(); } AWProcessingUnit::~AWProcessingUnit() { @@ -79,6 +82,11 @@ bool AWProcessingUnit::start(const worker_t worker) { * this is not an absolute measure and should be used accordingly */ void AWProcessingUnit::calibrate(const float reference_power_level) { + + // Wait for full buffers + for (int i = 0; i < N_ITEMS_BUFFER / N_SAMPLES; i++) { + pipeline->barrier(); + } Streams *streams = pipeline->getStreams(); diff --git a/src/awpu.h b/src/awpu.h index 19a058a..3e4704f 100644 --- a/src/awpu.h +++ b/src/awpu.h @@ -14,7 +14,7 @@ class AWProcessingUnit { public: - AWProcessingUnit(const char *address, const int port, int verbose = 1); + AWProcessingUnit(const char *address, const int port, int verbose = 1, bool debug = false); ~AWProcessingUnit(); bool start(const worker_t worker); @@ -29,6 +29,7 @@ class AWProcessingUnit { protected: int verbose; + bool debug; std::vector workers; Pipeline *pipeline; bool running; diff --git a/src/awpu_main.cpp b/src/awpu_main.cpp index 5c12af6..9ed4998 100644 --- a/src/awpu_main.cpp +++ b/src/awpu_main.cpp @@ -42,31 +42,18 @@ int main() { #if 0 AWProcessingUnit awpu = AWProcessingUnit("127.0.0.1", 21844); #else - AWProcessingUnit awpu5 = AWProcessingUnit("10.0.0.1", 21875); + AWProcessingUnit awpu5 = AWProcessingUnit("10.0.0.1", 21875, 0, true); AWProcessingUnit awpu8 = AWProcessingUnit("10.0.0.1", 21878); #endif std::cout << "Connected to FPGA" << std::endl; - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - //awpu.calibrate(); -#if 1 - awpu5.calibrate(); - awpu8.calibrate(); -#else - awpu5.synthetic_calibration(); - awpu8.synthetic_calibration(); -#endif std::cout << "Starting Gradient" << std::endl; - //awpu.start(GRADIENT); - //awpu5.start(PSO); - //awpu8.start(PSO); + awpu5.start(GRADIENT); - //exit(0); awpu8.start(GRADIENT); std::cout << "Starting listening" << std::endl; - //awpu.resume(); // Create a window to display the beamforming data cv::namedWindow(APPLICATION_NAME, cv::WINDOW_NORMAL); @@ -95,12 +82,12 @@ int main() { //awpu.draw_heatmap(&frame); //std::cout << "Best direction: " << awpu5.target() << std::endl; // Apply color map - // Blur the image with a Gaussian kernel + hconcat(frame2,frame1,frame); - - cv::GaussianBlur(frame, colorFrame, - cv::Size(BLUR_KERNEL_SIZE, BLUR_KERNEL_SIZE), 0); + // Blur the image with a Gaussian kernel + //cv::GaussianBlur(frame, colorFrame, + // cv::Size(BLUR_KERNEL_SIZE, BLUR_KERNEL_SIZE), 0); cv::applyColorMap(colorFrame, colorFrame, cv::COLORMAP_JET); cv::imshow(APPLICATION_NAME, colorFrame); if (cv::waitKey(1) == 'q') { From 67d9c5bd895a13a8c7727fadf804a948bb3f2b22 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 15:12:56 +0200 Subject: [PATCH 11/14] Improved doc --- src/pipeline.cpp | 6 +++--- src/streams.hpp | 45 ++++++++++++++++++++++++++++++++++++++++----- 2 files changed, 43 insertions(+), 8 deletions(-) diff --git a/src/pipeline.cpp b/src/pipeline.cpp index c9322f1..d396662 100644 --- a/src/pipeline.cpp +++ b/src/pipeline.cpp @@ -89,7 +89,7 @@ void Pipeline::synthetic_producer(const Spherical &angle) { const std::chrono::milliseconds targetDuration(static_cast(1000.0 * N_SAMPLES / SAMPLE_RATE)); int p = 0; int p2 = 0; - constexpr float carrier = 4e3; + constexpr float carrier = 9e3; constexpr float carrier2 = 4e3; while (isRunning()) { @@ -104,9 +104,9 @@ void Pipeline::synthetic_producer(const Spherical &angle) { signals[s][i] = 0.0; float t = static_cast(p + i) / SAMPLE_RATE;// Time in seconds signals[s][i] += sin(2 * M_PI * carrier * t + PHASE(delay1, carrier)); - signals[s][i] += sin(2 * M_PI * carrier2 * t + PHASE(delay2, carrier2)); + //signals[s][i] += sin(2 * M_PI * carrier2 * t + PHASE(delay2, carrier2)); - signals[s][i] /= 10.0; + signals[s][i] *= 1e-2; } streams->write_stream(s, &signals[s][0]); } diff --git a/src/streams.hpp b/src/streams.hpp index 9081bdd..393fba5 100644 --- a/src/streams.hpp +++ b/src/streams.hpp @@ -28,16 +28,34 @@ #define BUFFER_BYTES N_SAMPLES * sizeof(float) +/** + * Multiple ring-buffers can be thought of as: + * + * id ring-buffer + * + * 0: [0 1 2 3 ... 0 1 2 3] + * 1: [0 1 2 3 ... 0 1 2 3] + * 2: [0 1 2 3 ... 0 1 2 3] + * · + * · + * · + * x: [0 1 2 3 ... 0 1 2 3] + * + */ class Streams { public: + // Multiple ring-buffers std::unordered_map buffers; - unsigned position; + // Reader start position in ring-buffer + unsigned position; - Streams() : position(0){ - }; + Streams() : position(0){}; + /** + * Create a ring-buffer for a specific index + */ bool create_stream(unsigned index) { if (this->buffers.find(index) != this->buffers.end()) { std::cout << "Stream: " << index << " already exists" << std::endl; @@ -55,14 +73,23 @@ class Streams { return true; } + /** + * Getter for a buffer with offset + */ float *get_signal(unsigned index, int offset) { return (float *) ((char *) this->buffers[index] + this->position + offset * sizeof(float)); } + /** + * Fast write to buffer using wrap-around memcpy + */ inline void write_stream(unsigned index, float *data) { memcpy((char *) this->buffers[index] + this->position, data, BUFFER_BYTES); } + /** + * Fast read from buffer using wrap-around memcpy + */ inline void read_stream(unsigned index, float *data, unsigned offset = 0) { memcpy(data, (char *) this->buffers[index] + this->position + offset * sizeof(float), BUFFER_BYTES); } @@ -81,8 +108,7 @@ class Streams { } /** - Reserved only for producer - + * Reserved only for producer */ void forward() { this->position = (this->position + BUFFER_BYTES) % PAGE_SIZE; @@ -90,6 +116,15 @@ class Streams { private: + /** + * Allocate virtual memory ring buffers. + * + * We use two pages of contigious memory mapped to the same region + * meaning that when data overflows from region1 over to region2 + * it is mapped to the beginning of the "actual" buffer. As long + * as reading and writing is less than the buffer size, no other checks + * must be done making it a very optimized buffer. + */ float *allocate_buffer() { int fd = memfd_create("float", 0); if (fd == -1) { From ffb8818742e745ff0fe39992438bd7ec0ac7d30d Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 15:13:12 +0200 Subject: [PATCH 12/14] Changed hyperparams --- src/algorithms/gradient_ascend.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/algorithms/gradient_ascend.cpp b/src/algorithms/gradient_ascend.cpp index 2db607f..60525d6 100644 --- a/src/algorithms/gradient_ascend.cpp +++ b/src/algorithms/gradient_ascend.cpp @@ -106,7 +106,7 @@ void SphericalGradient::initialize_particles() { for (int i = 0; i < swarm_size; i++) { particles.emplace_back(this->antenna, this->streams); GradientParticle &particle = particles.back(); - particle.delta = TO_RADIANS(7.0);//1.0 + drandom() * 1.0); + particle.delta = TO_RADIANS(10.0);//1.0 + drandom() * 1.0); } } @@ -122,7 +122,7 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) { lock.lock(); -#if 0 +#if 1 for (GradientParticle &particle: currentTrackers) { if (!particle.tracking) { continue; @@ -149,6 +149,14 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) { heatmap->at(y, x) = 255; } #else + float maxValue = 0.0; + for (GradientParticle &particle: particles) { + if (particle.magnitude > maxValue) { + maxValue = particle.magnitude; + } + } + + //maxValue = 20 * std::log10(maxValue * 1e5); for (GradientParticle &particle: particles) { // If we convert to sphere with radius 0.5 we don't have to normalize it other than add // 0.5 to get the sphere in the first sector in the cartesian coordinate system @@ -162,7 +170,9 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) { heatmap->at(x,y) = (255); - int m = (int)clip(particle.magnitude / 1e-6, 0.0, 255.0); + float mag = particle.magnitude; //20 * std::log10(particle.magnitude * 1e5); + + int m = (int) (mag / maxValue * 255.0); cv::circle(*heatmap, cv::Point(y, x), 1 + (int)(gradient * 10.0), cv::Scalar(m,m,m), cv::FILLED, 8, 0); @@ -197,7 +207,7 @@ void SphericalGradient::loop() { lock.lock(); - if (it % 500 == 0) { + if (it % 100 == 0) { initialize_particles(); } it++; @@ -243,7 +253,7 @@ void SphericalGradient::loop() { for (auto &particle: particles) { particle.step(start_rate); if (n_tracking < n_trackers) { - if (particle.gradient < 1e-5) { + if (particle.gradient < 1e-4) { for (auto &tracker : currentTrackers) { if (!tracker.tracking && tracker.directionCurrent.angle(particle.directionCurrent) > M_PI / 20.0) { tracker.tracking = true; From 95be9ad9d3d2311bbe73cd379dda70e23d19efd4 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 15:18:17 +0200 Subject: [PATCH 13/14] Added warning on n_samples and removed manual calibration --- config.yaml | 2 +- src/aw_control_unit.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/config.yaml b/config.yaml index c0bce6b..eded1e6 100644 --- a/config.yaml +++ b/config.yaml @@ -64,7 +64,7 @@ N_SENSORS_TMP: 256 # Beamforming N_FRAMES: 4 # How many "frames" that are stored in the ringbuffer -N_SAMPLES: 256 #16384 # 512 # Each frame, must be a power of 2 +N_SAMPLES: 512 # Warning cannot be larger than 512 since pagesize is 4096 bytes and size of float is 4 bytes N_OFFSET: N_SAMPLES / 2 BUFFER_LENGTH: N_FRAMES * N_SAMPLES diff --git a/src/aw_control_unit.cpp b/src/aw_control_unit.cpp index 1c56d9e..6b0e857 100644 --- a/src/aw_control_unit.cpp +++ b/src/aw_control_unit.cpp @@ -19,7 +19,6 @@ void AWControlUnit::Start() { } AWProcessingUnit awpu = AWProcessingUnit("10.0.0.1", 21878); - awpu.calibrate(); awpu.start(PSO); cv::namedWindow(APPLICATION_NAME, cv::WINDOW_NORMAL); From 5410a63006f24e2d4116eae665e93e13a9924f20 Mon Sep 17 00:00:00 2001 From: Irreq Date: Wed, 17 Jul 2024 16:10:18 +0200 Subject: [PATCH 14/14] Improved synthetic start and allow creation of AWPU with a pipeline --- src/awpu.cpp | 68 ++++++++++++++++++++++------------------------- src/awpu.h | 3 ++- src/awpu_main.cpp | 10 ++++--- src/pipeline.cpp | 49 +++++++++++++++++++++++++--------- src/pipeline.h | 15 ++++++++--- 5 files changed, 88 insertions(+), 57 deletions(-) diff --git a/src/awpu.cpp b/src/awpu.cpp index 52b9d76..1810e1a 100644 --- a/src/awpu.cpp +++ b/src/awpu.cpp @@ -3,23 +3,34 @@ AWProcessingUnit::AWProcessingUnit(const char *address, const int port, int verbose, bool debug) : verbose(verbose), debug(debug){ // Allocate memory for pipeline this->pipeline = new Pipeline(address, port); + this->pipeline->connect(); - - int n_sources; - // Connect to FPGA - if (debug) { - this->spherical.theta = TO_RADIANS(0); - this->spherical.phi = TO_RADIANS(0);// = Spherical(TO_RADIANS(30), TO_RADIANS(0)); - this->pipeline->start_synthetic(this->spherical); - n_sources = 1; - } else { - this->pipeline->connect(); - n_sources = this->pipeline->get_n_sensors() / ELEMENTS; + for (int n_antenna = 0; n_antenna < this->pipeline->get_n_sensors() / ELEMENTS; n_antenna++) { + Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); + antennas.push_back(antenna); } - - this->running = false; + // int n_sources; + // // Connect to FPGA + // if (debug) { + // this->spherical.theta = TO_RADIANS(0); + // this->spherical.phi = TO_RADIANS(0);// = Spherical(TO_RADIANS(30), TO_RADIANS(0)); + // this->pipeline->start_synthetic(this->spherical); + // n_sources = 1; + // } else { + // this->pipeline->connect(); + // n_sources = this->pipeline->get_n_sensors() / ELEMENTS; + // } + // for (int n_antenna = 0; n_antenna < n_sources; n_antenna++) { + // Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); + // antennas.push_back(antenna); + // } - for (int n_antenna = 0; n_antenna < n_sources; n_antenna++) { + calibrate(); +} + +AWProcessingUnit::AWProcessingUnit(Pipeline *pipeline, int verbose, bool debug) : pipeline(pipeline), verbose(verbose), debug(debug) { + this->pipeline->connect(); + for (int n_antenna = 0; n_antenna < this->pipeline->get_n_sensors() / ELEMENTS; n_antenna++) { Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); antennas.push_back(antenna); } @@ -182,32 +193,17 @@ void AWProcessingUnit::calibrate(const float reference_power_level) { antenna.power_correction_mask[s] = reference_power_level / current_valid_power; } - std::cout << "Calibrated antenna " << a << " Usable: " << antenna.usable << " Mean: " << antenna.mean << " Median: " << antenna.median << std::endl; - - for (int s = 0; s < antenna.usable; s++) { - std::cout << "Mic: " << antenna.index[s] << " Correction: " << antenna.power_correction_mask[s] << std::endl; - } - - std::cout << std::endl; - } -} - -void AWProcessingUnit::synthetic_calibration() { - for (int a = 0; a < antennas.size(); a++) { - Antenna &antenna = antennas[a]; - antenna.usable = ELEMENTS; - antenna.index = new int[antenna.usable]; - for (int i = 0; i < ELEMENTS; i++) { - antenna.index[i] = i; - } + if (verbose) { + std::cout << "Calibrated antenna " << a << " Usable: " << antenna.usable << " Mean: " << antenna.mean << " Median: " << antenna.median << std::endl; - std::cout << "Calibrated antenna " << a << " Usable: " << antenna.usable << " Mean: " << antenna.mean << " Median: " << antenna.median << std::endl; + for (int s = 0; s < antenna.usable; s++) { + std::cout << "Mic: " << antenna.index[s] << " Correction: " << antenna.power_correction_mask[s] << std::endl; + } - for (int s = 0; s < antenna.usable; s++) { - std::cout << "Mic: " << antenna.index[s] << " Correction: " << antenna.power_correction_mask[s] << std::endl; + std::cout << std::endl; } - std::cout << std::endl; + } } diff --git a/src/awpu.h b/src/awpu.h index 3e4704f..bdd3f2a 100644 --- a/src/awpu.h +++ b/src/awpu.h @@ -15,6 +15,7 @@ class AWProcessingUnit { public: AWProcessingUnit(const char *address, const int port, int verbose = 1, bool debug = false); + AWProcessingUnit(Pipeline *pipeline, int verbose = 1, bool debug = false); ~AWProcessingUnit(); bool start(const worker_t worker); @@ -32,7 +33,7 @@ class AWProcessingUnit { bool debug; std::vector workers; Pipeline *pipeline; - bool running; + bool running = false; Spherical spherical; std::vector antennas; diff --git a/src/awpu_main.cpp b/src/awpu_main.cpp index 9ed4998..dfc2fcc 100644 --- a/src/awpu_main.cpp +++ b/src/awpu_main.cpp @@ -41,8 +41,12 @@ int main() { std::cout << "Connecting to FPGA" << std::endl; #if 0 AWProcessingUnit awpu = AWProcessingUnit("127.0.0.1", 21844); +#elif 0 + Pipeline pipeline(5000, Spherical(0, 0)); + AWProcessingUnit awpu5(&pipeline); + AWProcessingUnit awpu8 = AWProcessingUnit("10.0.0.1", 21878); #else - AWProcessingUnit awpu5 = AWProcessingUnit("10.0.0.1", 21875, 0, true); + AWProcessingUnit awpu5 = AWProcessingUnit("10.0.0.1", 21875); AWProcessingUnit awpu8 = AWProcessingUnit("10.0.0.1", 21878); #endif @@ -86,8 +90,8 @@ int main() { hconcat(frame2,frame1,frame); // Blur the image with a Gaussian kernel - //cv::GaussianBlur(frame, colorFrame, - // cv::Size(BLUR_KERNEL_SIZE, BLUR_KERNEL_SIZE), 0); + cv::GaussianBlur(frame, colorFrame, + cv::Size(BLUR_KERNEL_SIZE, BLUR_KERNEL_SIZE), 0); cv::applyColorMap(colorFrame, colorFrame, cv::COLORMAP_JET); cv::imshow(APPLICATION_NAME, colorFrame); if (cv::waitKey(1) == 'q') { diff --git a/src/pipeline.cpp b/src/pipeline.cpp index d396662..3701183 100644 --- a/src/pipeline.cpp +++ b/src/pipeline.cpp @@ -1,6 +1,10 @@ #include "pipeline.h" -Pipeline::Pipeline(const char *address, const int port) : address(address), port(port) { +Pipeline::Pipeline(const char *address, const int port, bool verbose) : address(address), port(port), verbose(verbose), synthetic(false) { + streams = new Streams(); +} + +Pipeline::Pipeline(float frequency, Spherical direction, bool verbose) : verbose(verbose), synthetic(true), port(-1) { streams = new Streams(); } @@ -21,9 +25,19 @@ int Pipeline::connect() { if (connected) { std::cerr << "Beamformer is already connected" << std::endl; + return -1; } + if (synthetic) { + return connect_synthetic(); + } else { + return connect_real(); + } +} + +int Pipeline::connect_real() { + socket_desc = init_receiver(address, port); if (socket_desc == -1) { @@ -34,7 +48,10 @@ int Pipeline::connect() { } if (receive_message(socket_desc, &msg) < 0) { + std::cerr << "Unable to receive message" << std::endl; + + return -1; } // Populating configs @@ -47,9 +64,10 @@ int Pipeline::connect() { exposure_buffer[sensor_index] = new float[N_SAMPLES]; this->streams->create_stream(sensor_index); -#if DEBUG_PIPELINE - std::cout << "[Pipeline] Adding stream: " << sensor_index << std::endl; -#endif + if (verbose) { + std::cout << "[Pipeline] Adding stream: " << sensor_index << std::endl; + } + } receiver_thread = std::thread(&Pipeline::producer, this); @@ -57,7 +75,7 @@ int Pipeline::connect() { return 0; } -void Pipeline::start_synthetic(const Spherical &angle) { +int Pipeline::connect_synthetic() { n_sensors = ELEMENTS; // Allocate memory for UDP packet data @@ -67,21 +85,23 @@ void Pipeline::start_synthetic(const Spherical &angle) { exposure_buffer[sensor_index] = new float[N_SAMPLES]; this->streams->create_stream(sensor_index); -#if DEBUG_PIPELINE - std::cout << "[Pipeline] Adding stream: " << sensor_index << std::endl; -#endif - } + if (verbose) { + std::cout << "[Pipeline] Adding Synthetic stream: " << sensor_index << std::endl; + } + } connected = 1; - receiver_thread = std::thread(&Pipeline::synthetic_producer, this, angle); + receiver_thread = std::thread(&Pipeline::synthetic_producer, this); + + return 0; } #define PHASE(delay, frequency) 2 * M_PI *frequency *delay -void Pipeline::synthetic_producer(const Spherical &angle) { +void Pipeline::synthetic_producer() { Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE); - //Spherical angle(TO_RADIANS(30), TO_RADIANS(0)); + Spherical angle(TO_RADIANS(0), TO_RADIANS(0)); Eigen::VectorXf tmp_delays1 = steering_vector_spherical(antenna, angle.theta, angle.phi); Eigen::VectorXf tmp_delays2 = steering_vector_spherical(antenna, angle.theta + TO_RADIANS(10), angle.phi); std::cout << tmp_delays1 << std::endl; @@ -155,7 +175,10 @@ int Pipeline::disconnect() { std::cout << "Barrier released for pipeline" << std::endl; - close(socket_desc); + if (port != -1) { + close(socket_desc); + } + std::cout << "Destroyed pipeline" << std::endl; diff --git a/src/pipeline.h b/src/pipeline.h index 80130d3..e2d8999 100644 --- a/src/pipeline.h +++ b/src/pipeline.h @@ -27,7 +27,8 @@ */ class Pipeline { public: - Pipeline(const char *address, const int port); + Pipeline(const char *address, const int port, bool verbose = false); + Pipeline(float frequency, Spherical direction, bool verbose = false); ~Pipeline(); /** @@ -62,9 +63,6 @@ class Pipeline { int save_pipeline(std::string path); - void start_synthetic(const Spherical &angle); - void synthetic_producer(const Spherical &angle); - private: // Buffer for storing incoming UDP mic data @@ -109,6 +107,10 @@ class Pipeline { // Check if new data was received int modified = 0; + bool verbose; + + bool synthetic; + /** * Allow the worker threads to continue */ @@ -123,6 +125,11 @@ class Pipeline { * The main distributer of data to the threads (this is also a thread) */ void producer(); + + int connect_real(); + int connect_synthetic(); + + void synthetic_producer(); }; #endif \ No newline at end of file