use bladeRF async API

This commit is contained in:
Sebastien Bourdeauducq 2023-09-13 14:31:19 +08:00
parent bd85526290
commit 17cd2f28da
1 changed files with 180 additions and 119 deletions

299
main.cpp
View File

@ -8,6 +8,7 @@
#include <complex>
#include <chrono>
#include <format>
#include <semaphore>
#include <experimental/net>
#include <GLFW/glfw3.h>
@ -19,6 +20,7 @@
#include <libbladeRF.h>
namespace net = std::experimental::net;
using namespace std::literals::chrono_literals;
void fft_mag(std::complex<float>* in, float* out, size_t len);
@ -26,96 +28,182 @@ void fft_mag(std::complex<float>* in, float* out, size_t len);
static const size_t block_len = 16384;
static const int wf_width = 1000;
static const int wf_height = 2000;
static const int wf_height = 1200;
static std::atomic<bool> shutdown_threads;
static std::atomic<bool> terminate_dsp;
static std::atomic<float> tec_current;
static struct bladerf* bladerf_dev;
static void tec_thread()
{
net::io_context io_context;
net::ip::tcp::socket tec_socket(io_context);
tec_socket.connect(net::ip::tcp::endpoint(net::ip::make_address("192.168.1.27"), 23));
static std::mutex wf_data_mutex;
static unsigned int wf_data[wf_width*wf_height];
while(!shutdown_threads) {
// FIXME: net::write seems unimplemented as of libstdc++ 13
tec_socket.write_some(net::buffer(std::format("pwm 0 i_set {:.6f}\n", (float)tec_current)));
std::string reply;
net::read(tec_socket, net::dynamic_buffer(reply),
[&reply](auto ec, auto n) -> std::size_t
{
if(ec || (reply.size() > 0 && reply.compare(reply.size()-1, 1, "\n") == 0))
return 0;
else
return 1;
});
std::this_thread::sleep_for(100ms);
}
}
static std::atomic<bool> poh_accept_input;
static std::binary_semaphore poh_input_ready{0};
static std::complex<float> poh_input_block[block_len];
static std::mutex wf_poh_mutex;
static unsigned int wf_poh[wf_width*wf_height];
static std::atomic<float> freq_setpoint;
static std::atomic<float> freq_peak;
static std::atomic<float> tec_bias;
static std::atomic<float> tec_p;
static std::atomic<float> tec_current;
static std::atomic<int> fps;
static void dsp_thread()
static void poh_thread()
{
using namespace std::literals::chrono_literals;
std::vector<float> block_mag(block_len);
net::io_context io_context;
net::ip::tcp::socket tec_socket(io_context);
tec_socket.connect(net::ip::tcp::endpoint(net::ip::make_address("192.168.1.27"), 23));
while(!shutdown_threads) {
poh_accept_input = true;
poh_input_ready.acquire();
std::vector<std::complex<int16_t>> frames(block_len);
std::vector<std::complex<float>> frames_f(block_len);
std::vector<float> frames_mag(block_len);
int iterations = 0;
auto last_second = std::chrono::steady_clock::now();
auto last_tec = std::chrono::steady_clock::now();
while(!terminate_dsp) {
int status;
if((status = bladerf_sync_rx(bladerf_dev, frames.data(), block_len, NULL, 0)) != 0) {
std::cerr << "failed to receive samples from bladeRF: " << bladerf_strerror(status) << std::endl;
break;
}
fft_mag(frames_f.data(), frames_mag.data(), block_len);
fft_mag(poh_input_block, block_mag.data(), block_len);
// stabilize laser
bool tick = false;
if((std::chrono::steady_clock::now() - last_tec) >= 100ms) {
float freq_peak_local;
freq_peak_local = 40.0f*float(distance(frames_mag.begin(), max_element(frames_mag.begin(), frames_mag.end())))/float(block_len);
freq_peak = freq_peak_local;
float freq_error = freq_peak_local - freq_setpoint;
float tec_current_local = std::max(tec_bias+tec_p*freq_error, 0.0f);
tec_current = tec_current_local;
// FIXME: net::write seems unimplemented as of libstdc++ 13
tec_socket.write_some(net::buffer(std::format("pwm 0 i_set {:.6f}\n", tec_current_local)));
std::string reply;
net::read(tec_socket, net::dynamic_buffer(reply),
[&reply](auto ec, auto n) -> std::size_t
{
if(ec || (reply.size() > 0 && reply.compare(reply.size()-1, 1, "\n") == 0))
return 0;
else
return 1;
});
last_tec += 100ms;
tick = true;
}
float freq_peak_local;
freq_peak_local = 40.0f*float(distance(block_mag.begin(), max_element(block_mag.begin(), block_mag.end())))/float(block_len);
freq_peak = freq_peak_local;
float freq_error = freq_peak_local - freq_setpoint;
tec_current = std::max(tec_bias+tec_p*freq_error, 0.0f);
// update waterfall
{
std::lock_guard<std::mutex> guard(wf_data_mutex);
std::memmove(&wf_data[wf_width], &wf_data[0], sizeof(int)*wf_width*(wf_height - 1));
std::lock_guard<std::mutex> guard(wf_poh_mutex);
std::memmove(&wf_poh[wf_width], &wf_poh[0], sizeof(int)*wf_width*(wf_height - 1));
for(int i=0;i<wf_width;i++) {
int j = i*block_len/wf_width;
wf_data[i] = 0xff000000 | 0x010101*std::min(int(frames_mag[j]/900.0f), 255);
wf_poh[i] = 0xff000000 | 0x010101*std::min(int(block_mag[j]/900.0f), 255);
}
wf_data[int(freq_setpoint*wf_width)/40] = 0xff0000ff;
if(tick)
for(int i=0;i<100;i++)
wf_data[i] = 0xffff0000;
}
// FPS counter
iterations++;
if((std::chrono::steady_clock::now() - last_second) >= 1s) {
fps = iterations;
iterations = 0;
last_second += 1s;
wf_poh[int(freq_setpoint*wf_width)/40] = 0xff0000ff;
}
}
}
static std::atomic<int> frames_processed;
static std::atomic<int> frames_dropped;
static auto brf_last_second = std::chrono::time_point<std::chrono::steady_clock>::min();
static int brf_frames_processed_cnt = 0;
static int brf_frames_dropped_cnt = 0;
static void* brf_dispatch(struct bladerf* dev, struct bladerf_stream* stream,
struct bladerf_metadata* meta, void* samples_v, size_t num_samples, void* user_data)
{
if(poh_accept_input) {
std::complex<int16_t>* samples = (std::complex<int16_t>*)samples_v;
for(size_t i=0;i<block_len;i++) {
poh_input_block[i] = samples[2*i];
}
poh_accept_input = false;
poh_input_ready.release();
brf_frames_processed_cnt++;
} else
brf_frames_dropped_cnt++;
if((std::chrono::steady_clock::now() - brf_last_second) >= 1s) {
frames_processed = brf_frames_processed_cnt;
frames_dropped = brf_frames_dropped_cnt;
brf_frames_processed_cnt = 0;
brf_frames_dropped_cnt = 0;
brf_last_second += 1s;
}
if(shutdown_threads)
return BLADERF_STREAM_SHUTDOWN;
else
return samples_v;
}
static void brf_thread()
{
struct bladerf_devinfo brf_dev_info;
struct bladerf* brf_dev;
struct bladerf_stream* brf_stream;
void** brf_buffers;
int status;
bladerf_init_devinfo(&brf_dev_info);
if((status = bladerf_open_with_devinfo(&brf_dev, &brf_dev_info)) != 0) {
std::cerr << "cannot open bladeRF device: " << bladerf_strerror(status) << std::endl;
return;
}
for(int i=0;i<2;i++) {
if((status = bladerf_set_frequency(brf_dev, BLADERF_CHANNEL_RX(i), 1'655'000'000)) != 0) {
std::cerr << "failed to set bladeRF frequency: " << bladerf_strerror(status) << std::endl;
bladerf_close(brf_dev);
return;
}
if((status = bladerf_set_sample_rate(brf_dev, BLADERF_CHANNEL_RX(i), 40'000'000, NULL)) != 0) {
std::cerr << "failed to set bladeRF sample rate: " << bladerf_strerror(status) << std::endl;
bladerf_close(brf_dev);
return;
}
if((status = bladerf_set_bandwidth(brf_dev, BLADERF_CHANNEL_RX(i), 35'000'000, NULL)) != 0) {
std::cerr << "failed to set bladeRF bandwidth: " << bladerf_strerror(status) << std::endl;
bladerf_close(brf_dev);
return;
}
if((status = bladerf_set_gain(brf_dev, BLADERF_CHANNEL_RX(i), 20)) != 0) {
std::cerr << "failed to set bladeRF gain: " << bladerf_strerror(status) << std::endl;
bladerf_close(brf_dev);
return;
}
}
if((status = bladerf_init_stream(&brf_stream, brf_dev, brf_dispatch, &brf_buffers,
16, BLADERF_FORMAT_SC16_Q11, 2*block_len, 8, NULL)) != 0) {
std::cerr << "failed to init bladeRF stream: " << bladerf_strerror(status) << std::endl;
return;
}
if((status = bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), true) != 0)) {
std::cerr << "failed to enable bladeRF RX: " << bladerf_strerror(status) << std::endl;
bladerf_deinit_stream(brf_stream);
bladerf_close(brf_dev);
return;
}
if((status = bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(1), true) != 0)) {
std::cerr << "failed to enable bladeRF RX: " << bladerf_strerror(status) << std::endl;
bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), false);
bladerf_deinit_stream(brf_stream);
bladerf_close(brf_dev);
return;
}
brf_last_second = std::chrono::steady_clock::now();
if((status = bladerf_stream(brf_stream, BLADERF_RX_X2)) != 0) {
std::cerr << "bladeRF stream failed: " << bladerf_strerror(status) << std::endl;
bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(1), false);
bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), false);
bladerf_deinit_stream(brf_stream);
bladerf_close(brf_dev);
return;
};
bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(1), false);
bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), false);
bladerf_deinit_stream(brf_stream);
bladerf_close(brf_dev);
}
int main(int argc, char* argv[])
{
if(!glfwInit()) {
@ -161,57 +249,29 @@ int main(int argc, char* argv[])
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
}
int status;
struct bladerf_devinfo dev_info;
bladerf_init_devinfo(&dev_info);
if((status = bladerf_open_with_devinfo(&bladerf_dev, &dev_info)) != 0) {
std::cerr << "cannot open bladeRF device: " << bladerf_strerror(status) << std::endl;
return 1;
}
static auto BladeRFClose = []() {
bladerf_close(bladerf_dev);
static std::thread brf_thread_h = std::thread(brf_thread);
static auto JoinBRF = []() {
brf_thread_h.join();
};
std::atexit(BladeRFClose);
if((status = bladerf_set_frequency(bladerf_dev, BLADERF_CHANNEL_RX(0), 1'655'000'000)) != 0) {
std::cerr << "failed to set bladeRF frequency: " << bladerf_strerror(status) << std::endl;
return 1;
}
if((status = bladerf_set_sample_rate(bladerf_dev, BLADERF_CHANNEL_RX(0), 40'000'000, NULL)) != 0) {
std::cerr << "failed to set bladeRF sample rate: " << bladerf_strerror(status) << std::endl;
return 1;
}
if((status = bladerf_set_bandwidth(bladerf_dev, BLADERF_CHANNEL_RX(0), 35'000'000, NULL)) != 0) {
std::cerr << "failed to set bladeRF bandwidth: " << bladerf_strerror(status) << std::endl;
return 1;
}
if((status = bladerf_set_gain(bladerf_dev, BLADERF_CHANNEL_RX(0), 20)) != 0) {
std::cerr << "failed to set bladeRF gain: " << bladerf_strerror(status) << std::endl;
return 1;
}
if((status = bladerf_sync_config(bladerf_dev, BLADERF_RX_X1, BLADERF_FORMAT_SC16_Q11,
16, /* num_buffers */
8192, /* buffer_size */
8, /* num_transfers */
3500 /* timeout_ms */)) != 0) {
std::cerr << "failed to set bladeRF sync settings: " << bladerf_strerror(status) << std::endl;
return 1;
}
if((status = bladerf_enable_module(bladerf_dev, BLADERF_RX, true) != 0)) {
std::cerr << "failed to enable bladeRF RX: " << bladerf_strerror(status) << std::endl;
return 1;
}
static auto BladeRFDisableRX = []() {
bladerf_enable_module(bladerf_dev, BLADERF_RX, false);
};
std::atexit(BladeRFDisableRX);
std::atexit(JoinBRF);
terminate_dsp = false;
static std::thread dsp_thread_h = std::thread(dsp_thread);
static auto TerminateDSP = []() {
terminate_dsp = true;
dsp_thread_h.join();
static std::thread poh_thread_h = std::thread(poh_thread);
static auto JoinPOH = []() {
poh_thread_h.join();
};
std::atexit(TerminateDSP);
std::atexit(JoinPOH);
static std::thread tec_thread_h = std::thread(tec_thread);
static auto JoinTEC = []() {
tec_thread_h.join();
};
std::atexit(JoinTEC);
shutdown_threads = false;
static auto SetShutdown = []() {
shutdown_threads = true;
};
std::atexit(SetShutdown);
bool exit = false;
bool update_wf = true;
@ -229,13 +289,13 @@ int main(int argc, char* argv[])
#endif
glBindTexture(GL_TEXTURE_2D, wftex[0]);
{
std::lock_guard<std::mutex> guard(wf_data_mutex);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_data);
std::lock_guard<std::mutex> guard(wf_poh_mutex);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_poh);
}
glBindTexture(GL_TEXTURE_2D, wftex[1]);
{
std::lock_guard<std::mutex> guard(wf_data_mutex);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_data);
std::lock_guard<std::mutex> guard(wf_poh_mutex);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_poh);
}
}
@ -251,7 +311,7 @@ int main(int argc, char* argv[])
ImGui::BeginTable("fastsa", 3, ImGuiTableFlags_SizingStretchSame | ImGuiTableFlags_Resizable);
ImGui::TableSetupColumn("SBS laser control", 0, 280.0f);
ImGui::TableSetupColumn("Pump/output heterodyne", 0, 1000.0f);
ImGui::TableSetupColumn("Linewidth", 0, 1000.0f);
ImGui::TableSetupColumn("Lineshape", 0, 1000.0f);
ImGui::TableHeadersRow();
ImGui::TableNextColumn();
ImGui::Checkbox("Update waterfall plots", &update_wf);
@ -265,7 +325,8 @@ int main(int argc, char* argv[])
tec_p = 0.05f*tec_p_local;
if(ImGui::Button("Exit"))
exit = true;
ImGui::Text("FPS: %d", (int)fps);
ImGui::Text("Frames processed: %d", (int)frames_processed);
ImGui::Text("Frames dropped: %d", (int)frames_dropped);
ImGui::TableNextColumn();
ImGui::Image((void*)(intptr_t)wftex[0], ImVec2(wf_width, wf_height));
ImGui::TableNextColumn();