kopia lustrzana https://github.com/proto17/dji_droneid
Created new library for misc function
This is now usable from pythongr-droneid-update
David Protzman
rodzic
4cb3ed52d3
commit
6ecd15c4b9
|
|
@ -21,5 +21,6 @@
|
|||
install(FILES
|
||||
droneid_extractor.block.yml
|
||||
droneid_time_sync.block.yml
|
||||
droneid_demodulation.block.yml DESTINATION share/gnuradio/grc/blocks
|
||||
droneid_demodulation.block.yml
|
||||
droneid_misc_utils.block.yml DESTINATION share/gnuradio/grc/blocks
|
||||
)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,29 @@
|
|||
id: droneid_misc_utils
|
||||
label: misc_utils
|
||||
category: '[droneid]'
|
||||
|
||||
templates:
|
||||
imports: import droneid
|
||||
make: droneid.misc_utils()
|
||||
|
||||
# Make one 'parameters' list entry for every parameter you want settable from the GUI.
|
||||
# Keys include:
|
||||
# * id (makes the value accessible as \$keyname, e.g. in the make entry)
|
||||
# * label (label shown in the GUI)
|
||||
# * dtype (e.g. int, float, complex, byte, short, xxx_vector, ...)
|
||||
parameters:
|
||||
|
||||
# Make one 'inputs' list entry per input and one 'outputs' list entry per output.
|
||||
# Keys include:
|
||||
# * label (an identifier for the GUI)
|
||||
# * domain (optional - stream or message. Default is stream)
|
||||
# * dtype (e.g. int, float, complex, byte, short, xxx_vector, ...)
|
||||
# * vlen (optional - data stream vector length. Default is 1)
|
||||
# * optional (optional - set to 1 for optional inputs. Default is 0)
|
||||
inputs:
|
||||
|
||||
outputs:
|
||||
|
||||
# 'file_format' specifies the version of the GRC yml format used in the file
|
||||
# and should usually not be changed.
|
||||
file_format: 1
|
||||
|
|
@ -26,5 +26,6 @@ install(FILES
|
|||
extractor.h
|
||||
time_sync.h
|
||||
../../lib/utils.h
|
||||
demodulation.h DESTINATION include/droneid
|
||||
demodulation.h
|
||||
misc_utils.h DESTINATION include/droneid
|
||||
)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,63 @@
|
|||
/* -*- c++ -*- */
|
||||
/*
|
||||
* Copyright 2022 gr-droneid author.
|
||||
*
|
||||
* This is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* This software is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, Inc., 51 Franklin Street,
|
||||
* Boston, MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDED_DRONEID_MISC_UTILS_H
|
||||
#define INCLUDED_DRONEID_MISC_UTILS_H
|
||||
|
||||
#include <droneid/api.h>
|
||||
#include <vector>
|
||||
#include <complex>
|
||||
|
||||
namespace gr {
|
||||
namespace droneid {
|
||||
|
||||
/*!
|
||||
* \brief <+description+>
|
||||
*
|
||||
*/
|
||||
class DRONEID_API misc_utils {
|
||||
public:
|
||||
static constexpr double CARRIER_SPACING = 15e3;
|
||||
static constexpr uint32_t OCCUPIED_CARRIERS_EXC_DC = 600;
|
||||
static constexpr uint32_t OCCUPIED_CARRIERS_INC_DC = 601;
|
||||
|
||||
static uint32_t get_long_cp_len(double sample_rate);
|
||||
static uint32_t get_short_cp_len(double sample_rate);
|
||||
static uint32_t get_fft_size(double sample_rate);
|
||||
|
||||
static std::vector<std::complex<float>> create_zc_sequence(double sample_rate, uint32_t root);
|
||||
static std::vector<std::complex<float>> conj(const std::vector<std::complex<float>> & input);
|
||||
|
||||
static void write_samples(const std::string &path, const std::complex<float> * samples, uint32_t element_count);
|
||||
static void write_samples(const std::string & path, const std::vector<std::complex<float>> & samples);
|
||||
|
||||
|
||||
misc_utils();
|
||||
|
||||
~misc_utils();
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
} // namespace droneid
|
||||
} // namespace gr
|
||||
|
||||
#endif /* INCLUDED_DRONEID_MISC_UTILS_H */
|
||||
|
||||
|
|
@ -28,6 +28,7 @@ list(APPEND droneid_sources
|
|||
time_sync_impl.cc
|
||||
utils.cpp
|
||||
demodulation_impl.cc
|
||||
misc_utils.cc
|
||||
)
|
||||
|
||||
set(droneid_sources "${droneid_sources}" PARENT_SCOPE)
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include "demodulation_impl.h"
|
||||
#include "utils.h"
|
||||
#include "droneid/misc_utils.h"
|
||||
#include <volk/volk.h>
|
||||
#include <gnuradio/fft/fft.h>
|
||||
#include <gnuradio/fft/fft_shift.h>
|
||||
|
|
@ -46,8 +46,8 @@ namespace gr {
|
|||
: gr::sync_block("demodulation",
|
||||
gr::io_signature::make(0, 0, 0),
|
||||
gr::io_signature::make(0, 0, 0)),
|
||||
sample_rate_(sample_rate), fft_size_(get_fft_size(sample_rate)), long_cp_len_(
|
||||
get_long_cp_len(sample_rate)), short_cp_len_(get_short_cp_len(sample_rate)) {
|
||||
sample_rate_(sample_rate), fft_size_(misc_utils::get_fft_size(sample_rate)), long_cp_len_(
|
||||
misc_utils::get_long_cp_len(sample_rate)), short_cp_len_(misc_utils::get_short_cp_len(sample_rate)) {
|
||||
message_port_register_in(pmt::mp("pdus"));
|
||||
message_port_register_out(pmt::mp("pdus"));
|
||||
set_msg_handler(pmt::mp("pdus"), [this](pmt::pmt_t pdu){handle_msg(pdu);});
|
||||
|
|
@ -74,7 +74,7 @@ namespace gr {
|
|||
|
||||
std::cout << "FFT SIZE: " << fft_size_ << ", sample rate: " << sample_rate_ << "\n";
|
||||
|
||||
zc_ = create_zc_sequence(sample_rate_, 600);
|
||||
zc_ = misc_utils::create_zc_sequence(sample_rate_, 600);
|
||||
fft_ = std::unique_ptr<gr::fft::fft_complex>(new gr::fft::fft_complex(static_cast<int>(fft_size_), true, 1));
|
||||
fft_shift_ = std::unique_ptr<gr::fft::fft_shift<std::complex<float>>>(new gr::fft::fft_shift<std::complex<float>>(fft_size_));
|
||||
|
||||
|
|
@ -85,7 +85,7 @@ namespace gr {
|
|||
fft_shift_->shift(zc_);
|
||||
channel_.resize(fft_size_);
|
||||
|
||||
write_samples("/tmp/bursts/zc", zc_);
|
||||
misc_utils::write_samples("/tmp/bursts/zc", zc_);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -94,19 +94,6 @@ namespace gr {
|
|||
demodulation_impl::~demodulation_impl() {
|
||||
}
|
||||
|
||||
void demodulation_impl::write_samples(const std::string &path, const std::vector<std::complex<float>> &samples) {
|
||||
write_samples(path, &samples[0], samples.size());
|
||||
}
|
||||
|
||||
void demodulation_impl::write_samples(const std::string &path, const std::complex<float> * const samples, const uint32_t element_count) {
|
||||
FILE * handle = fopen(path.c_str(), "wb");
|
||||
if (!handle) {
|
||||
throw std::runtime_error("Failed to open output file");
|
||||
}
|
||||
fwrite(samples, sizeof(samples[0]), element_count, handle);
|
||||
fclose(handle);
|
||||
}
|
||||
|
||||
void demodulation_impl::handle_msg(pmt::pmt_t pdu) {
|
||||
const auto meta = pmt::car(pdu);
|
||||
const auto vec = pmt::cdr(pdu);
|
||||
|
|
@ -120,10 +107,10 @@ namespace gr {
|
|||
const auto start_idx_pmt = pmt::dict_ref(meta, pmt::mp("start_idx"), pmt::from_uint64(0));
|
||||
const auto start_idx = pmt::to_uint64(start_idx_pmt);
|
||||
|
||||
if (start_idx == 0) {
|
||||
std::cerr << "Invalid start index!\n";
|
||||
return;
|
||||
}
|
||||
// if (start_idx == 0) {
|
||||
// std::cerr << "Invalid start index!\n";
|
||||
// return;
|
||||
// }
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// CFO detection and adjustment using cyclic prefix
|
||||
|
|
@ -159,7 +146,7 @@ namespace gr {
|
|||
/// Channel estimation and equalization
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
volk_32fc_x2_divide_32fc(&channel_[0], &zc_[0], &symbols_[3][0], fft_size_);
|
||||
write_samples("/tmp/bursts/channel", channel_);
|
||||
misc_utils::write_samples("/tmp/bursts/channel", channel_);
|
||||
|
||||
for (uint32_t symbol_idx = 0; symbol_idx < cp_lengths_.size(); symbol_idx++) {
|
||||
for (uint32_t idx = 0; idx < fft_size_; idx++) {
|
||||
|
|
|
|||
|
|
@ -34,8 +34,6 @@ namespace gr {
|
|||
const uint32_t fft_size_;
|
||||
const uint32_t long_cp_len_;
|
||||
const uint32_t short_cp_len_;
|
||||
void write_samples(const std::string &path, const std::complex<float> * samples, uint32_t element_count);
|
||||
void write_samples(const std::string & path, const std::vector<std::complex<float>> & samples);
|
||||
|
||||
std::unique_ptr<gr::fft::fft_complex> fft_;
|
||||
std::unique_ptr<gr::fft::fft_shift<std::complex<float>>> fft_shift_;
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include "extractor_impl.h"
|
||||
#include "droneid/misc_utils.h"
|
||||
|
||||
namespace gr {
|
||||
namespace droneid {
|
||||
|
|
@ -41,14 +42,15 @@ namespace gr {
|
|||
extractor_impl::extractor_impl(double sample_rate)
|
||||
: gr::sync_block("extractor",
|
||||
gr::io_signature::make2(2, 2, sizeof(gr_complex), sizeof(float)),
|
||||
gr::io_signature::make(0, 0, 0)), fft_size_(round(sample_rate / CARRIER_SPACING)),
|
||||
long_cp_len_(round(sample_rate / 192000)), short_cp_len_(round(0.0000046875 * sample_rate)),
|
||||
extract_samples_count_((fft_size_ * 9) + (long_cp_len_ * 2) + (short_cp_len_ * 7) + (fft_size_ * 2)){
|
||||
gr::io_signature::make(0, 0, 0)), fft_size_(misc_utils::get_fft_size(sample_rate)),
|
||||
long_cp_len_(misc_utils::get_long_cp_len(sample_rate)), short_cp_len_(misc_utils::get_short_cp_len(sample_rate)),
|
||||
extract_samples_count_((fft_size_ * 9) + (long_cp_len_ * 2) + (short_cp_len_ * 7)){
|
||||
this->message_port_register_out(pmt::mp("pdus"));
|
||||
|
||||
buffer_.resize(extract_samples_count_);
|
||||
current_state_ = state_t::WAITING_FOR_THRESHOLD;
|
||||
collected_samples_ = 0;
|
||||
total_samples_read_ = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -35,6 +35,8 @@ namespace gr {
|
|||
|
||||
|
||||
static constexpr float CARRIER_SPACING = 15e3;
|
||||
uint64_t start_sample_index_;
|
||||
uint64_t total_samples_read_;
|
||||
state_t current_state_;
|
||||
uint32_t collected_samples_;
|
||||
const uint32_t fft_size_;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,109 @@
|
|||
/* -*- c++ -*- */
|
||||
/*
|
||||
* Copyright 2022 gr-droneid author.
|
||||
*
|
||||
* This is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* This software is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, Inc., 51 Franklin Street,
|
||||
* Boston, MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
#include "config.h"
|
||||
#endif
|
||||
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <droneid/misc_utils.h>
|
||||
#include <gnuradio/fft/fft.h>
|
||||
|
||||
namespace gr {
|
||||
namespace droneid {
|
||||
|
||||
misc_utils::misc_utils() {
|
||||
}
|
||||
|
||||
misc_utils::~misc_utils() {
|
||||
}
|
||||
|
||||
uint32_t misc_utils::get_long_cp_len(double sample_rate) {
|
||||
return static_cast<uint32_t>(std::round(sample_rate / 192000));
|
||||
}
|
||||
|
||||
uint32_t misc_utils::get_short_cp_len(double sample_rate) {
|
||||
return static_cast<uint32_t>(round(0.0000046875 * sample_rate));
|
||||
}
|
||||
|
||||
uint32_t misc_utils::get_fft_size(double sample_rate) {
|
||||
return static_cast<uint32_t>(round(sample_rate / CARRIER_SPACING));
|
||||
}
|
||||
|
||||
std::vector<std::complex<float>> misc_utils::create_zc_sequence(const double sample_rate, const uint32_t root) {
|
||||
const auto fft_size = get_fft_size(sample_rate);
|
||||
std::vector<std::complex<float>> sequence(fft_size, {0, 0});
|
||||
|
||||
const uint32_t guard_carriers = fft_size - OCCUPIED_CARRIERS_EXC_DC;
|
||||
const auto left_guards = guard_carriers / 2;
|
||||
|
||||
std::vector<std::complex<float>> g (OCCUPIED_CARRIERS_INC_DC);
|
||||
const auto I = std::complex<double>(0, 1);
|
||||
for (int idx = 0; idx < OCCUPIED_CARRIERS_INC_DC; idx++) {
|
||||
// Doing the arith below in double precision and then casting down to a float. Using floats the whole
|
||||
// way will result in an output that's very far off from the MATLAB implementation. The errors will accumulate
|
||||
// down the vector
|
||||
sequence[left_guards + idx] = std::exp(-I * (M_PIf64 * (double)root * (double)idx * (double)(idx + 1) / 601.0));
|
||||
}
|
||||
|
||||
// Null out the DC carrier
|
||||
sequence[(fft_size / 2) - 1] = 0;
|
||||
|
||||
// Create an FFT object that is configured to run an inverse FFT
|
||||
gr::fft::fft_complex ifft(static_cast<int>(fft_size), false, 1);
|
||||
|
||||
// FFT-shift the inputs (swap the left and right halves) and store in the IFFT input buffer
|
||||
std::copy(sequence.begin() + (fft_size/2), sequence.begin() + fft_size, ifft.get_inbuf());
|
||||
std::copy(sequence.begin(), sequence.begin() + (fft_size/2), ifft.get_inbuf() + (fft_size/2));
|
||||
|
||||
// Run the IFFT
|
||||
ifft.execute();
|
||||
|
||||
// Copy the IFFT'd samples out
|
||||
std::copy(ifft.get_outbuf(), ifft.get_outbuf() + fft_size, sequence.begin());
|
||||
|
||||
// The samples need to be scaled by the FFT size to get the power back down
|
||||
std::for_each(sequence.begin(), sequence.end(), [fft_size](std::complex<float> & sample){sample /= static_cast<float>(fft_size);});
|
||||
|
||||
return sequence;
|
||||
}
|
||||
|
||||
std::vector<std::complex<float>> misc_utils::conj(const std::vector<std::complex<float>> &input) {
|
||||
auto vec = input;
|
||||
std::for_each(vec.begin(), vec.end(), [](std::complex<float> & sample){sample = std::conj(sample);});
|
||||
return vec;
|
||||
}
|
||||
|
||||
void misc_utils::write_samples(const std::string &path, const std::vector<std::complex<float>> &samples) {
|
||||
write_samples(path, &samples[0], samples.size());
|
||||
}
|
||||
|
||||
void misc_utils::write_samples(const std::string &path, const std::complex<float> * const samples, const uint32_t element_count) {
|
||||
FILE * handle = fopen(path.c_str(), "wb");
|
||||
if (!handle) {
|
||||
throw std::runtime_error("Failed to open output file");
|
||||
}
|
||||
fwrite(samples, sizeof(samples[0]), element_count, handle);
|
||||
fclose(handle);
|
||||
}
|
||||
|
||||
} /* namespace droneid */
|
||||
} /* namespace gr */
|
||||
|
||||
|
|
@ -26,7 +26,7 @@
|
|||
#include "time_sync_impl.h"
|
||||
#include <volk/volk.h>
|
||||
|
||||
#include "utils.h"
|
||||
#include "droneid/misc_utils.h"
|
||||
#include <gnuradio/filter/fir_filter.h>
|
||||
|
||||
namespace gr {
|
||||
|
|
@ -47,9 +47,9 @@ namespace gr {
|
|||
gr::io_signature::make(0, 0, 0),
|
||||
gr::io_signature::make(0, 0, 0)),
|
||||
sample_rate_(sample_rate),
|
||||
fft_size_(round(sample_rate / CARRIER_SPACING)), long_cp_len_(round(sample_rate / 192000)),
|
||||
short_cp_len_(round(0.0000046875 * sample_rate)) {
|
||||
auto zc_sequence = create_zc_sequence(sample_rate_, 600);
|
||||
fft_size_(misc_utils::get_fft_size(sample_rate)), long_cp_len_(misc_utils::get_long_cp_len(sample_rate)),
|
||||
short_cp_len_(misc_utils::get_short_cp_len(sample_rate)) {
|
||||
auto zc_sequence = misc_utils::create_zc_sequence(sample_rate_, 600);
|
||||
std::for_each(zc_sequence.begin(), zc_sequence.end(), [](std::complex<float> & sample){ sample = std::conj(sample); });
|
||||
std::reverse(zc_sequence.begin(), zc_sequence.end());
|
||||
correlator_ptr_ = std::unique_ptr<filter_t>(new filter_t(1, zc_sequence));
|
||||
|
|
@ -65,24 +65,11 @@ namespace gr {
|
|||
time_sync_impl::~time_sync_impl() {
|
||||
}
|
||||
|
||||
void time_sync_impl::write_samples(const std::string &path, const std::vector<std::complex<float>> &samples) {
|
||||
write_samples(path, &samples[0], samples.size());
|
||||
}
|
||||
|
||||
void time_sync_impl::write_samples(const std::string &path, const std::complex<float> * const samples, const uint32_t element_count) {
|
||||
FILE * handle = fopen(path.c_str(), "wb");
|
||||
if (!handle) {
|
||||
throw std::runtime_error("Failed to open output file");
|
||||
}
|
||||
fwrite(samples, sizeof(samples[0]), element_count, handle);
|
||||
fclose(handle);
|
||||
}
|
||||
|
||||
void time_sync_impl::msg_handler(const pmt::pmt_t & pdu) {
|
||||
auto meta = pmt::car(pdu);
|
||||
auto vec = pmt::cdr(pdu);
|
||||
auto burst_ptr = pmt::c32vector_elements(vec, pdu_element_count_);
|
||||
// write_samples("/tmp/bursts/file_"+std::to_string(file_counter_), burst_ptr, element_count);
|
||||
misc_utils::write_samples("/tmp/bursts/file_"+std::to_string(file_counter_), burst_ptr, pdu_element_count_);
|
||||
|
||||
if (buffer_.size() < pdu_element_count_) {
|
||||
buffer_.resize(pdu_element_count_);
|
||||
|
|
@ -92,7 +79,7 @@ namespace gr {
|
|||
// Run the correlator ignoring the last `ntaps` elements so as not to walk off the end of the array
|
||||
correlator_ptr_->filterN(&buffer_[0], burst_ptr, pdu_element_count_ - correlator_ptr_->ntaps());
|
||||
|
||||
// write_samples("/tmp/bursts/corr_file_"+std::to_string(file_counter_), buffer);
|
||||
misc_utils::write_samples("/tmp/bursts/corr_file_"+std::to_string(file_counter_), buffer_);
|
||||
|
||||
// Find the max correlation peak
|
||||
// TODO: Only search through the area where the ZC is expected to be
|
||||
|
|
|
|||
|
|
@ -31,7 +31,6 @@ namespace gr {
|
|||
class time_sync_impl : public time_sync {
|
||||
private:
|
||||
using filter_t = gr::filter::kernel::fir_filter_ccc;
|
||||
static constexpr float CARRIER_SPACING = 15e3;
|
||||
const double sample_rate_;
|
||||
const uint32_t fft_size_;
|
||||
const uint32_t long_cp_len_;
|
||||
|
|
@ -43,8 +42,6 @@ namespace gr {
|
|||
std::unique_ptr<filter_t> correlator_ptr_;
|
||||
uint32_t file_counter_ = 0;
|
||||
void msg_handler(const pmt::pmt_t & pdu);
|
||||
void write_samples(const std::string &path, const std::complex<float> * samples, uint32_t element_count);
|
||||
void write_samples(const std::string & path, const std::vector<std::complex<float>> & samples);
|
||||
|
||||
public:
|
||||
time_sync_impl(double sample_rate);
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@
|
|||
#include "droneid/extractor.h"
|
||||
#include "droneid/time_sync.h"
|
||||
#include "droneid/demodulation.h"
|
||||
#include "droneid/misc_utils.h"
|
||||
//#include "droneid/utils.h"
|
||||
%}
|
||||
|
||||
|
|
@ -28,3 +29,4 @@ GR_SWIG_BLOCK_MAGIC2(droneid, time_sync);
|
|||
//%}
|
||||
%include "droneid/demodulation.h"
|
||||
GR_SWIG_BLOCK_MAGIC2(droneid, demodulation);
|
||||
%include "droneid/misc_utils.h"
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue