mirror of
https://github.com/30hours/blah2.git
synced 2024-11-18 12:33:58 +00:00
Fix all Werror problems
This commit is contained in:
parent
03fcf0cc18
commit
95a8b2da7c
16 changed files with 93 additions and 80 deletions
|
@ -201,7 +201,7 @@ int main(int argc, char **argv)
|
||||||
std::string savePath, saveMapPath;
|
std::string savePath, saveMapPath;
|
||||||
if (saveIq || saveMap)
|
if (saveIq || saveMap)
|
||||||
{
|
{
|
||||||
char startTimeStr[15];
|
char startTimeStr[16];
|
||||||
struct timeval currentTime = {0, 0};
|
struct timeval currentTime = {0, 0};
|
||||||
gettimeofday(¤tTime, NULL);
|
gettimeofday(¤tTime, NULL);
|
||||||
strftime(startTimeStr, 16, "%Y%m%d-%H%M%S", localtime(¤tTime.tv_sec));
|
strftime(startTimeStr, 16, "%Y%m%d-%H%M%S", localtime(¤tTime.tv_sec));
|
||||||
|
@ -234,7 +234,7 @@ int main(int argc, char **argv)
|
||||||
// extract data from buffer
|
// extract data from buffer
|
||||||
buffer1->lock();
|
buffer1->lock();
|
||||||
buffer2->lock();
|
buffer2->lock();
|
||||||
for (int i = 0; i < nSamples; i++)
|
for (uint32_t i = 0; i < nSamples; i++)
|
||||||
{
|
{
|
||||||
x->push_back(buffer1->pop_front());
|
x->push_back(buffer1->pop_front());
|
||||||
y->push_back(buffer2->pop_front());
|
y->push_back(buffer2->pop_front());
|
||||||
|
|
|
@ -120,7 +120,7 @@ int HackRf::rx_callback(hackrf_transfer* transfer)
|
||||||
|
|
||||||
buffer_blah2->lock();
|
buffer_blah2->lock();
|
||||||
|
|
||||||
for (size_t i = 0; i < transfer->buffer_length; i=i+2)
|
for (int i = 0; i < transfer->buffer_length; i=i+2)
|
||||||
{
|
{
|
||||||
double iqi = static_cast<double>(buffer_hackrf[i]);
|
double iqi = static_cast<double>(buffer_hackrf[i]);
|
||||||
double iqq = static_cast<double>(buffer_hackrf[i+1]);
|
double iqq = static_cast<double>(buffer_hackrf[i+1]);
|
||||||
|
|
|
@ -11,7 +11,7 @@ Kraken::Kraken(std::string _type, uint32_t _fc, uint32_t _fs,
|
||||||
: Source(_type, _fc, _fs, _path, _saveIq)
|
: Source(_type, _fc, _fs, _path, _saveIq)
|
||||||
{
|
{
|
||||||
// convert gain to tenths of dB
|
// convert gain to tenths of dB
|
||||||
for (int i = 0; i < _gain.size(); i++)
|
for (size_t i = 0; i < _gain.size(); i++)
|
||||||
{
|
{
|
||||||
gain.push_back(static_cast<int>(_gain[i]*10));
|
gain.push_back(static_cast<int>(_gain[i]*10));
|
||||||
channelIndex.push_back(i);
|
channelIndex.push_back(i);
|
||||||
|
@ -33,7 +33,7 @@ Kraken::Kraken(std::string _type, uint32_t _fc, uint32_t _fs,
|
||||||
check_status(status, "Failed to close device for available gains.");
|
check_status(status, "Failed to close device for available gains.");
|
||||||
|
|
||||||
// update gains to next value if invalid
|
// update gains to next value if invalid
|
||||||
for (int i = 0; i < _gain.size(); i++)
|
for (size_t i = 0; i < _gain.size(); i++)
|
||||||
{
|
{
|
||||||
int adjustedGain = static_cast<int>(_gain[i] * 10);
|
int adjustedGain = static_cast<int>(_gain[i] * 10);
|
||||||
auto it = std::lower_bound(validGains.begin(),
|
auto it = std::lower_bound(validGains.begin(),
|
||||||
|
|
|
@ -115,9 +115,13 @@ void RspDuo::replay(IqData *_buffer1, IqData *_buffer2, std::string _file, bool
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
rv = fread(&i1, 1, sizeof(short), file_replay);
|
rv = fread(&i1, 1, sizeof(short), file_replay);
|
||||||
|
if (rv != sizeof(short)) break;
|
||||||
rv = fread(&q1, 1, sizeof(short), file_replay);
|
rv = fread(&q1, 1, sizeof(short), file_replay);
|
||||||
|
if (rv != sizeof(short)) break;
|
||||||
rv = fread(&i2, 1, sizeof(short), file_replay);
|
rv = fread(&i2, 1, sizeof(short), file_replay);
|
||||||
|
if (rv != sizeof(short)) break;
|
||||||
rv = fread(&q2, 1, sizeof(short), file_replay);
|
rv = fread(&q2, 1, sizeof(short), file_replay);
|
||||||
|
if (rv != sizeof(short)) break;
|
||||||
buffer1->lock();
|
buffer1->lock();
|
||||||
buffer2->lock();
|
buffer2->lock();
|
||||||
if (buffer1->get_length() < buffer1->get_n())
|
if (buffer1->get_length() < buffer1->get_n())
|
||||||
|
@ -214,7 +218,7 @@ void RspDuo::open_api()
|
||||||
|
|
||||||
void RspDuo::get_device()
|
void RspDuo::get_device()
|
||||||
{
|
{
|
||||||
int i;
|
unsigned int i;
|
||||||
unsigned int ndev;
|
unsigned int ndev;
|
||||||
unsigned int chosenIdx = 0;
|
unsigned int chosenIdx = 0;
|
||||||
|
|
||||||
|
@ -413,8 +417,8 @@ void RspDuo::stream_a_callback(short *xi, short *xq,
|
||||||
sdrplay_api_StreamCbParamsT *params, unsigned int numSamples,
|
sdrplay_api_StreamCbParamsT *params, unsigned int numSamples,
|
||||||
unsigned int reset, void *cbContext)
|
unsigned int reset, void *cbContext)
|
||||||
{
|
{
|
||||||
int i = 0;
|
unsigned int i = 0;
|
||||||
int j = 0;
|
unsigned int j = 0;
|
||||||
|
|
||||||
// process stream callback data
|
// process stream callback data
|
||||||
buffer_16_ar = (short int *)malloc(numSamples * 4 * sizeof(short));
|
buffer_16_ar = (short int *)malloc(numSamples * 4 * sizeof(short));
|
||||||
|
@ -456,8 +460,8 @@ void RspDuo::stream_b_callback(short *xi, short *xq,
|
||||||
sdrplay_api_StreamCbParamsT *params, unsigned int numSamples,
|
sdrplay_api_StreamCbParamsT *params, unsigned int numSamples,
|
||||||
unsigned int reset, void *cbContext)
|
unsigned int reset, void *cbContext)
|
||||||
{
|
{
|
||||||
int i = 0;
|
unsigned int i = 0;
|
||||||
int j = 0;
|
unsigned int j = 0;
|
||||||
|
|
||||||
// xxxxIIQQ
|
// xxxxIIQQ
|
||||||
for (i = 0; i < numSamples; i++)
|
for (i = 0; i < numSamples; i++)
|
||||||
|
@ -473,7 +477,7 @@ unsigned int reset, void *cbContext)
|
||||||
// write data to IqData
|
// write data to IqData
|
||||||
buffer1->lock();
|
buffer1->lock();
|
||||||
buffer2->lock();
|
buffer2->lock();
|
||||||
for (int i = 0; i < numSamples*4; i+=4)
|
for (i = 0; i < numSamples*4; i+=4)
|
||||||
{
|
{
|
||||||
buffer1->push_back({(double)buffer_16_ar[i], (double)buffer_16_ar[i+1]});
|
buffer1->push_back({(double)buffer_16_ar[i], (double)buffer_16_ar[i+1]});
|
||||||
buffer2->push_back({(double)buffer_16_ar[i+2], (double)buffer_16_ar[i+3]});
|
buffer2->push_back({(double)buffer_16_ar[i+2], (double)buffer_16_ar[i+3]});
|
||||||
|
|
|
@ -52,21 +52,21 @@ std::string Detection::to_json(uint64_t timestamp)
|
||||||
|
|
||||||
// store delay array
|
// store delay array
|
||||||
rapidjson::Value arrayDelay(rapidjson::kArrayType);
|
rapidjson::Value arrayDelay(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < get_nDetections(); i++)
|
for (size_t i = 0; i < get_nDetections(); i++)
|
||||||
{
|
{
|
||||||
arrayDelay.PushBack(delay[i], allocator);
|
arrayDelay.PushBack(delay[i], allocator);
|
||||||
}
|
}
|
||||||
|
|
||||||
// store Doppler array
|
// store Doppler array
|
||||||
rapidjson::Value arrayDoppler(rapidjson::kArrayType);
|
rapidjson::Value arrayDoppler(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < get_nDetections(); i++)
|
for (size_t i = 0; i < get_nDetections(); i++)
|
||||||
{
|
{
|
||||||
arrayDoppler.PushBack(doppler[i], allocator);
|
arrayDoppler.PushBack(doppler[i], allocator);
|
||||||
}
|
}
|
||||||
|
|
||||||
// store snr array
|
// store snr array
|
||||||
rapidjson::Value arraySnr(rapidjson::kArrayType);
|
rapidjson::Value arraySnr(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < get_nDetections(); i++)
|
for (size_t i = 0; i < get_nDetections(); i++)
|
||||||
{
|
{
|
||||||
arraySnr.PushBack(snr[i], allocator);
|
arraySnr.PushBack(snr[i], allocator);
|
||||||
}
|
}
|
||||||
|
@ -92,7 +92,7 @@ std::string Detection::delay_bin_to_km(std::string json, uint32_t fs)
|
||||||
document.Parse(json.c_str());
|
document.Parse(json.c_str());
|
||||||
|
|
||||||
document["delay"].Clear();
|
document["delay"].Clear();
|
||||||
for (int i = 0; i < delay.size(); i++)
|
for (size_t i = 0; i < delay.size(); i++)
|
||||||
{
|
{
|
||||||
document["delay"].PushBack(1.0*delay[i]*(Constants::c/(double)fs)/1000, allocator);
|
document["delay"].PushBack(1.0*delay[i]*(Constants::c/(double)fs)/1000, allocator);
|
||||||
}
|
}
|
||||||
|
|
|
@ -96,14 +96,14 @@ std::string IqData::to_json(uint64_t timestamp)
|
||||||
|
|
||||||
// store frequency array
|
// store frequency array
|
||||||
rapidjson::Value arrayFrequency(rapidjson::kArrayType);
|
rapidjson::Value arrayFrequency(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < frequency.size(); i++)
|
for (size_t i = 0; i < frequency.size(); i++)
|
||||||
{
|
{
|
||||||
arrayFrequency.PushBack(frequency[i], allocator);
|
arrayFrequency.PushBack(frequency[i], allocator);
|
||||||
}
|
}
|
||||||
|
|
||||||
// store spectrum array
|
// store spectrum array
|
||||||
rapidjson::Value arraySpectrum(rapidjson::kArrayType);
|
rapidjson::Value arraySpectrum(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < spectrum.size(); i++)
|
for (size_t i = 0; i < spectrum.size(); i++)
|
||||||
{
|
{
|
||||||
arraySpectrum.PushBack(10 * std::log10(std::abs(spectrum[i])), allocator);
|
arraySpectrum.PushBack(10 * std::log10(std::abs(spectrum[i])), allocator);
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,7 +23,7 @@ template <class T>
|
||||||
void Map<T>::set_row(uint32_t i, std::vector<T> row)
|
void Map<T>::set_row(uint32_t i, std::vector<T> row)
|
||||||
{
|
{
|
||||||
//data[i].swap(row);
|
//data[i].swap(row);
|
||||||
for (int j = 0; j < nCols; j++)
|
for (uint32_t j = 0; j < nCols; j++)
|
||||||
{
|
{
|
||||||
data[i][j] = row[j];
|
data[i][j] = row[j];
|
||||||
}
|
}
|
||||||
|
@ -32,7 +32,7 @@ void Map<T>::set_row(uint32_t i, std::vector<T> row)
|
||||||
template <class T>
|
template <class T>
|
||||||
void Map<T>::set_col(uint32_t i, std::vector<T> col)
|
void Map<T>::set_col(uint32_t i, std::vector<T> col)
|
||||||
{
|
{
|
||||||
for (int j = 0; j < nRows; j++)
|
for (uint32_t j = 0; j < nRows; j++)
|
||||||
{
|
{
|
||||||
data[j][i] = col[j];
|
data[j][i] = col[j];
|
||||||
}
|
}
|
||||||
|
@ -61,7 +61,7 @@ std::vector<T> Map<T>::get_col(uint32_t col)
|
||||||
{
|
{
|
||||||
std::vector<T> colData;
|
std::vector<T> colData;
|
||||||
|
|
||||||
for (int i = 0; i < nRows; i++)
|
for (uint32_t i = 0; i < nRows; i++)
|
||||||
{
|
{
|
||||||
colData.push_back(data[i][col]);
|
colData.push_back(data[i][col]);
|
||||||
}
|
}
|
||||||
|
@ -74,9 +74,9 @@ Map<double> *Map<T>::get_map_db()
|
||||||
{
|
{
|
||||||
Map<double> *map = new Map<double>(nRows, nCols);
|
Map<double> *map = new Map<double>(nRows, nCols);
|
||||||
|
|
||||||
for (int i = 0; i < nRows; i++)
|
for (uint32_t i = 0; i < nRows; i++)
|
||||||
{
|
{
|
||||||
for (int j = 0; j < nCols; j++)
|
for (uint32_t j = 0; j < nCols; j++)
|
||||||
{
|
{
|
||||||
map->data[i][j] = (double)10 * std::log10(std::abs(data[i][j]));
|
map->data[i][j] = (double)10 * std::log10(std::abs(data[i][j]));
|
||||||
}
|
}
|
||||||
|
@ -88,9 +88,9 @@ Map<double> *Map<T>::get_map_db()
|
||||||
template <class T>
|
template <class T>
|
||||||
void Map<T>::print()
|
void Map<T>::print()
|
||||||
{
|
{
|
||||||
for (int i = 0; i < nRows; i++)
|
for (uint32_t i = 0; i < nRows; i++)
|
||||||
{
|
{
|
||||||
for (int j = 0; j < nCols; j++)
|
for (uint32_t j = 0; j < nCols; j++)
|
||||||
{
|
{
|
||||||
std::cout << data[i][j];
|
std::cout << data[i][j];
|
||||||
std::cout << " ";
|
std::cout << " ";
|
||||||
|
@ -121,10 +121,10 @@ std::string Map<T>::to_json(uint64_t timestamp)
|
||||||
|
|
||||||
// store data array
|
// store data array
|
||||||
rapidjson::Value array(rapidjson::kArrayType);
|
rapidjson::Value array(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < data.size(); i++)
|
for (size_t i = 0; i < data.size(); i++)
|
||||||
{
|
{
|
||||||
rapidjson::Value subarray(rapidjson::kArrayType);
|
rapidjson::Value subarray(rapidjson::kArrayType);
|
||||||
for (int j = 0; j < data[i].size(); j++)
|
for (size_t j = 0; j < data[i].size(); j++)
|
||||||
{
|
{
|
||||||
subarray.PushBack(10 * std::log10(std::abs(data[i][j])) - noisePower, document.GetAllocator());
|
subarray.PushBack(10 * std::log10(std::abs(data[i][j])) - noisePower, document.GetAllocator());
|
||||||
}
|
}
|
||||||
|
@ -133,14 +133,14 @@ std::string Map<T>::to_json(uint64_t timestamp)
|
||||||
|
|
||||||
// store delay array
|
// store delay array
|
||||||
rapidjson::Value arrayDelay(rapidjson::kArrayType);
|
rapidjson::Value arrayDelay(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < delay.size(); i++)
|
for (size_t i = 0; i < delay.size(); i++)
|
||||||
{
|
{
|
||||||
arrayDelay.PushBack(delay[i], allocator);
|
arrayDelay.PushBack(delay[i], allocator);
|
||||||
}
|
}
|
||||||
|
|
||||||
// store Doppler array
|
// store Doppler array
|
||||||
rapidjson::Value arrayDoppler(rapidjson::kArrayType);
|
rapidjson::Value arrayDoppler(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < get_nRows(); i++)
|
for (uint32_t i = 0; i < get_nRows(); i++)
|
||||||
{
|
{
|
||||||
arrayDoppler.PushBack(doppler[i], allocator);
|
arrayDoppler.PushBack(doppler[i], allocator);
|
||||||
}
|
}
|
||||||
|
@ -171,7 +171,7 @@ std::string Map<T>::delay_bin_to_km(std::string json, uint32_t fs)
|
||||||
document.Parse(json.c_str());
|
document.Parse(json.c_str());
|
||||||
|
|
||||||
document["delay"].Clear();
|
document["delay"].Clear();
|
||||||
for (int i = 0; i < delay.size(); i++)
|
for (size_t i = 0; i < delay.size(); i++)
|
||||||
{
|
{
|
||||||
document["delay"].PushBack(1.0*delay[i]*(Constants::c/(double)fs)/1000, allocator);
|
document["delay"].PushBack(1.0*delay[i]*(Constants::c/(double)fs)/1000, allocator);
|
||||||
}
|
}
|
||||||
|
@ -191,9 +191,9 @@ void Map<T>::set_metrics()
|
||||||
double value;
|
double value;
|
||||||
double noisePower = 0;
|
double noisePower = 0;
|
||||||
double maxPower = 0;
|
double maxPower = 0;
|
||||||
for (int i = 0; i < nRows; i++)
|
for (uint32_t i = 0; i < nRows; i++)
|
||||||
{
|
{
|
||||||
for (int j = 0; j < nCols; j++)
|
for (uint32_t j = 0; j < nCols; j++)
|
||||||
{
|
{
|
||||||
value = 10 * std::log10(std::abs(data[i][j]));
|
value = 10 * std::log10(std::abs(data[i][j]));
|
||||||
noisePower = noisePower + value;
|
noisePower = noisePower + value;
|
||||||
|
|
|
@ -177,7 +177,7 @@ std::string Track::to_json(uint64_t timestamp)
|
||||||
|
|
||||||
// store track data
|
// store track data
|
||||||
rapidjson::Value dataArray(rapidjson::kArrayType);
|
rapidjson::Value dataArray(rapidjson::kArrayType);
|
||||||
for (int i = 0; i < get_n(); i++)
|
for (uint64_t i = 0; i < get_n(); i++)
|
||||||
{
|
{
|
||||||
if (get_state(i) != STATE_TENTATIVE)
|
if (get_state(i) != STATE_TENTATIVE)
|
||||||
{
|
{
|
||||||
|
|
|
@ -33,7 +33,7 @@ std::string Timing::to_json()
|
||||||
document.AddMember("nCpi", n, allocator);
|
document.AddMember("nCpi", n, allocator);
|
||||||
document.AddMember("uptime", uptime, allocator);
|
document.AddMember("uptime", uptime, allocator);
|
||||||
rapidjson::Value name_value;
|
rapidjson::Value name_value;
|
||||||
for (int i = 0; i < time.size(); i++)
|
for (size_t i = 0; i < time.size(); i++)
|
||||||
{
|
{
|
||||||
name_value = rapidjson::StringRef(name[i].c_str());
|
name_value = rapidjson::StringRef(name[i].c_str());
|
||||||
document.AddMember(name_value, time[i], allocator);
|
document.AddMember(name_value, time[i], allocator);
|
||||||
|
|
|
@ -8,16 +8,20 @@
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
Ambiguity::Ambiguity(int32_t _delayMin, int32_t _delayMax, int32_t _dopplerMin, int32_t _dopplerMax, uint32_t _fs, uint32_t _n, bool _roundHamming)
|
Ambiguity::Ambiguity(int32_t _delayMin, int32_t _delayMax,
|
||||||
: delayMin{_delayMin}
|
int32_t _dopplerMin, int32_t _dopplerMax, uint32_t _fs,
|
||||||
, delayMax{_delayMax}
|
uint32_t _n, bool _roundHamming)
|
||||||
, dopplerMin{_dopplerMin}
|
|
||||||
, dopplerMax{_dopplerMax}
|
|
||||||
, fs{_fs}
|
|
||||||
, nSamples{_n}
|
|
||||||
, nDelayBins{static_cast<uint16_t>(_delayMax - _delayMin + 1)} // If delayMin > delayMax = trouble, what's the exception policy?
|
|
||||||
, dopplerMiddle{(_dopplerMin + _dopplerMax) / 2.0}
|
|
||||||
{
|
{
|
||||||
|
// init
|
||||||
|
delayMin = _delayMin;
|
||||||
|
delayMax = _delayMax;
|
||||||
|
dopplerMin = _dopplerMin;
|
||||||
|
dopplerMax = _dopplerMax;
|
||||||
|
fs = _fs;
|
||||||
|
nSamples = _n;
|
||||||
|
nDelayBins = static_cast<uint16_t>(_delayMax - _delayMin + 1);
|
||||||
|
dopplerMiddle = (_dopplerMin + _dopplerMax) / 2.0;
|
||||||
|
|
||||||
// doppler calculations
|
// doppler calculations
|
||||||
std::deque<double> doppler;
|
std::deque<double> doppler;
|
||||||
double resolutionDoppler = 1.0 / (static_cast<double>(_n) / static_cast<double>(_fs));
|
double resolutionDoppler = 1.0 / (static_cast<double>(_n) / static_cast<double>(_fs));
|
||||||
|
@ -91,22 +95,22 @@ Map<std::complex<double>> *Ambiguity::process(IqData *x, IqData *y)
|
||||||
if (dopplerMiddle != 0)
|
if (dopplerMiddle != 0)
|
||||||
{
|
{
|
||||||
std::complex<double> j = {0, 1};
|
std::complex<double> j = {0, 1};
|
||||||
for (int i = 0; i < x->get_length(); i++)
|
for (uint32_t i = 0; i < x->get_length(); i++)
|
||||||
{
|
{
|
||||||
x->push_back(x->pop_front() * std::exp(1.0 * j * 2.0 * M_PI * dopplerMiddle * ((double)i / fs)));
|
x->push_back(x->pop_front() * std::exp(1.0 * j * 2.0 * M_PI * dopplerMiddle * ((double)i / fs)));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// range processing
|
// range processing
|
||||||
for (int i = 0; i < nDopplerBins; i++)
|
for (uint16_t i = 0; i < nDopplerBins; i++)
|
||||||
{
|
{
|
||||||
for (int j = 0; j < nCorr; j++)
|
for (uint16_t j = 0; j < nCorr; j++)
|
||||||
{
|
{
|
||||||
dataXi[j] = x->pop_front();
|
dataXi[j] = x->pop_front();
|
||||||
dataYi[j] = y->pop_front();
|
dataYi[j] = y->pop_front();
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int j = nCorr; j < nfft; j++)
|
for (uint16_t j = nCorr; j < nfft; j++)
|
||||||
{
|
{
|
||||||
dataXi[j] = {0, 0};
|
dataXi[j] = {0, 0};
|
||||||
dataYi[j] = {0, 0};
|
dataYi[j] = {0, 0};
|
||||||
|
@ -116,7 +120,7 @@ Map<std::complex<double>> *Ambiguity::process(IqData *x, IqData *y)
|
||||||
fftw_execute(fftYi);
|
fftw_execute(fftYi);
|
||||||
|
|
||||||
// compute correlation
|
// compute correlation
|
||||||
for (int j = 0; j < nfft; j++)
|
for (uint32_t j = 0; j < nfft; j++)
|
||||||
{
|
{
|
||||||
dataZi[j] = (dataYi[j] * std::conj(dataXi[j])) / (double)nfft;
|
dataZi[j] = (dataYi[j] * std::conj(dataXi[j])) / (double)nfft;
|
||||||
}
|
}
|
||||||
|
@ -124,18 +128,18 @@ Map<std::complex<double>> *Ambiguity::process(IqData *x, IqData *y)
|
||||||
fftw_execute(fftZi);
|
fftw_execute(fftZi);
|
||||||
|
|
||||||
// extract center of corr
|
// extract center of corr
|
||||||
for (int j = 0; j < nDelayBins; j++)
|
for (uint16_t j = 0; j < nDelayBins; j++)
|
||||||
{
|
{
|
||||||
dataCorr[j] = dataZi[nfft - nDelayBins + j];
|
dataCorr[j] = dataZi[nfft - nDelayBins + j];
|
||||||
}
|
}
|
||||||
for (int j = 0; j < nDelayBins + 1; j++)
|
for (uint16_t j = 0; j < nDelayBins + 1; j++)
|
||||||
{
|
{
|
||||||
dataCorr[j + nDelayBins] = dataZi[j];
|
dataCorr[j + nDelayBins] = dataZi[j];
|
||||||
}
|
}
|
||||||
|
|
||||||
// cast from std::complex to std::vector
|
// cast from std::complex to std::vector
|
||||||
corr.clear();
|
corr.clear();
|
||||||
for (int j = 0; j < nDelayBins; j++)
|
for (uint16_t j = 0; j < nDelayBins; j++)
|
||||||
{
|
{
|
||||||
corr.push_back(dataCorr[nDelayBins + delayMin + j - 1 + 1]);
|
corr.push_back(dataCorr[nDelayBins + delayMin + j - 1 + 1]);
|
||||||
}
|
}
|
||||||
|
@ -144,10 +148,10 @@ Map<std::complex<double>> *Ambiguity::process(IqData *x, IqData *y)
|
||||||
}
|
}
|
||||||
|
|
||||||
// doppler processing
|
// doppler processing
|
||||||
for (int i = 0; i < nDelayBins; i++)
|
for (uint16_t i = 0; i < nDelayBins; i++)
|
||||||
{
|
{
|
||||||
delayProfile = map->get_col(i);
|
delayProfile = map->get_col(i);
|
||||||
for (int j = 0; j < nDopplerBins; j++)
|
for (uint16_t j = 0; j < nDopplerBins; j++)
|
||||||
{
|
{
|
||||||
dataDoppler[j] = {delayProfile[j].real(), delayProfile[j].imag()};
|
dataDoppler[j] = {delayProfile[j].real(), delayProfile[j].imag()};
|
||||||
}
|
}
|
||||||
|
@ -155,7 +159,7 @@ Map<std::complex<double>> *Ambiguity::process(IqData *x, IqData *y)
|
||||||
fftw_execute(fftDoppler);
|
fftw_execute(fftDoppler);
|
||||||
|
|
||||||
corr.clear();
|
corr.clear();
|
||||||
for (int j = 0; j < nDopplerBins; j++)
|
for (uint16_t j = 0; j < nDopplerBins; j++)
|
||||||
{
|
{
|
||||||
corr.push_back(dataDoppler[(j + int(nDopplerBins / 2) + 1) % nDopplerBins]);
|
corr.push_back(dataDoppler[(j + int(nDopplerBins / 2) + 1) % nDopplerBins]);
|
||||||
}
|
}
|
||||||
|
|
|
@ -6,6 +6,7 @@
|
||||||
/// @author 30hours
|
/// @author 30hours
|
||||||
/// @todo Ambiguity maps are still offset by 1 bin.
|
/// @todo Ambiguity maps are still offset by 1 bin.
|
||||||
/// @todo Write a performance test for hamming assisted ambiguity processing.
|
/// @todo Write a performance test for hamming assisted ambiguity processing.
|
||||||
|
/// @todo If delayMin > delayMax = trouble, what's the exception policy?
|
||||||
|
|
||||||
#include "data/IqData.h"
|
#include "data/IqData.h"
|
||||||
#include "data/Map.h"
|
#include "data/Map.h"
|
||||||
|
@ -73,12 +74,12 @@ private:
|
||||||
/// @brief Number of samples.
|
/// @brief Number of samples.
|
||||||
uint32_t nSamples;
|
uint32_t nSamples;
|
||||||
|
|
||||||
/// @brief Center of Doppler bins (Hz).
|
|
||||||
double dopplerMiddle;
|
|
||||||
|
|
||||||
/// @brief Number of delay bins.
|
/// @brief Number of delay bins.
|
||||||
uint16_t nDelayBins;
|
uint16_t nDelayBins;
|
||||||
|
|
||||||
|
/// @brief Center of Doppler bins (Hz).
|
||||||
|
double dopplerMiddle;
|
||||||
|
|
||||||
/// @brief Number of Doppler bins.
|
/// @brief Number of Doppler bins.
|
||||||
uint16_t nDopplerBins;
|
uint16_t nDopplerBins;
|
||||||
|
|
||||||
|
|
|
@ -57,11 +57,12 @@ WienerHopf::~WienerHopf()
|
||||||
|
|
||||||
bool WienerHopf::process(IqData *x, IqData *y)
|
bool WienerHopf::process(IqData *x, IqData *y)
|
||||||
{
|
{
|
||||||
|
uint32_t i, j;
|
||||||
xData = x->get_data();
|
xData = x->get_data();
|
||||||
yData = y->get_data();
|
yData = y->get_data();
|
||||||
|
|
||||||
// change deque to std::complex
|
// change deque to std::complex
|
||||||
for (int i = 0; i < nSamples; i++)
|
for (i = 0; i < nSamples; i++)
|
||||||
{
|
{
|
||||||
dataX[i] = xData[(((i - delayMin) % nSamples) + nSamples) % nSamples];
|
dataX[i] = xData[(((i - delayMin) % nSamples) + nSamples) % nSamples];
|
||||||
dataY[i] = yData[i];
|
dataY[i] = yData[i];
|
||||||
|
@ -72,21 +73,21 @@ bool WienerHopf::process(IqData *x, IqData *y)
|
||||||
fftw_execute(fftY);
|
fftw_execute(fftY);
|
||||||
|
|
||||||
// auto-correlation matrix A
|
// auto-correlation matrix A
|
||||||
for (int i = 0; i < nSamples; i++)
|
for (i = 0; i < nSamples; i++)
|
||||||
{
|
{
|
||||||
dataA[i] = (dataOutX[i] * std::conj(dataOutX[i]));
|
dataA[i] = (dataOutX[i] * std::conj(dataOutX[i]));
|
||||||
}
|
}
|
||||||
fftw_execute(fftA);
|
fftw_execute(fftA);
|
||||||
for (int i = 0; i < nBins; i++)
|
for (i = 0; i < nBins; i++)
|
||||||
{
|
{
|
||||||
a[i] = std::conj(dataA[i]) / (double)nSamples;
|
a[i] = std::conj(dataA[i]) / (double)nSamples;
|
||||||
}
|
}
|
||||||
A = arma::toeplitz(a);
|
A = arma::toeplitz(a);
|
||||||
|
|
||||||
// conjugate upper diagonal as arma does not
|
// conjugate upper diagonal as arma does not
|
||||||
for (int i = 0; i < nBins; i++)
|
for (i = 0; i < nBins; i++)
|
||||||
{
|
{
|
||||||
for (int j = 0; j < nBins; j++)
|
for (j = 0; j < nBins; j++)
|
||||||
{
|
{
|
||||||
if (i > j)
|
if (i > j)
|
||||||
{
|
{
|
||||||
|
@ -96,12 +97,12 @@ bool WienerHopf::process(IqData *x, IqData *y)
|
||||||
}
|
}
|
||||||
|
|
||||||
// cross-correlation vector b
|
// cross-correlation vector b
|
||||||
for (int i = 0; i < nSamples; i++)
|
for (i = 0; i < nSamples; i++)
|
||||||
{
|
{
|
||||||
dataB[i] = (dataOutY[i] * std::conj(dataOutX[i]));
|
dataB[i] = (dataOutY[i] * std::conj(dataOutX[i]));
|
||||||
}
|
}
|
||||||
fftw_execute(fftB);
|
fftw_execute(fftB);
|
||||||
for (int i = 0; i < nBins; i++)
|
for (i = 0; i < nBins; i++)
|
||||||
{
|
{
|
||||||
b[i] = dataB[i] / (double)nSamples;
|
b[i] = dataB[i] / (double)nSamples;
|
||||||
}
|
}
|
||||||
|
@ -121,21 +122,21 @@ bool WienerHopf::process(IqData *x, IqData *y)
|
||||||
}
|
}
|
||||||
|
|
||||||
// assign and pad x
|
// assign and pad x
|
||||||
for (int i = 0; i < nSamples; i++)
|
for (i = 0; i < nSamples; i++)
|
||||||
{
|
{
|
||||||
filtX[i] = dataX[i];
|
filtX[i] = dataX[i];
|
||||||
}
|
}
|
||||||
for (int i = nSamples; i < nBins + nSamples + 1; i++)
|
for (i = nSamples; i < nBins + nSamples + 1; i++)
|
||||||
{
|
{
|
||||||
filtX[i] = {0, 0};
|
filtX[i] = {0, 0};
|
||||||
}
|
}
|
||||||
|
|
||||||
// assign and pad w
|
// assign and pad w
|
||||||
for (int i = 0; i < nBins; i++)
|
for (i = 0; i < nBins; i++)
|
||||||
{
|
{
|
||||||
filtW[i] = w[i];
|
filtW[i] = w[i];
|
||||||
}
|
}
|
||||||
for (int i = nBins; i < nBins + nSamples + 1; i++)
|
for (i = nBins; i < nBins + nSamples + 1; i++)
|
||||||
{
|
{
|
||||||
filtW[i] = {0, 0};
|
filtW[i] = {0, 0};
|
||||||
}
|
}
|
||||||
|
@ -145,7 +146,7 @@ bool WienerHopf::process(IqData *x, IqData *y)
|
||||||
fftw_execute(fftFiltW);
|
fftw_execute(fftFiltW);
|
||||||
|
|
||||||
// compute convolution/filter
|
// compute convolution/filter
|
||||||
for (int i = 0; i < nBins + nSamples + 1; i++)
|
for (i = 0; i < nBins + nSamples + 1; i++)
|
||||||
{
|
{
|
||||||
filt[i] = (filtW[i] * filtX[i]);
|
filt[i] = (filtW[i] * filtX[i]);
|
||||||
}
|
}
|
||||||
|
@ -153,7 +154,7 @@ bool WienerHopf::process(IqData *x, IqData *y)
|
||||||
|
|
||||||
// update surveillance signal
|
// update surveillance signal
|
||||||
y->clear();
|
y->clear();
|
||||||
for (int i = 0; i < nSamples; i++)
|
for (i = 0; i < nSamples; i++)
|
||||||
{
|
{
|
||||||
y->push_back(dataY[i] - (filt[i] / (double)(nBins + nSamples + 1)));
|
y->push_back(dataY[i] - (filt[i] / (double)(nBins + nSamples + 1)));
|
||||||
}
|
}
|
||||||
|
|
|
@ -25,11 +25,11 @@ HammingNumber::HammingNumber(const std::vector<unsigned int> &pfs)
|
||||||
|
|
||||||
const HammingNumber &HammingNumber::operator++()
|
const HammingNumber &HammingNumber::operator++()
|
||||||
{
|
{
|
||||||
for (int i = 0; i < H.size(); i++)
|
for (std::vector<unsigned int>::size_type i = 0; i < H.size(); i++)
|
||||||
for (; hv[i] <= x.back(); hv[i] = x[++hp[i]] * H[i])
|
for (; hv[i] <= x.back(); hv[i] = x[++hp[i]] * H[i])
|
||||||
;
|
;
|
||||||
x.push_back(hv[0]);
|
x.push_back(hv[0]);
|
||||||
for (int i = 1; i < H.size(); i++)
|
for (std::vector<unsigned int>::size_type i = 1; i < H.size(); i++)
|
||||||
if (hv[i] < x.back())
|
if (hv[i] < x.back())
|
||||||
x.back() = hv[i];
|
x.back() = hv[i];
|
||||||
return *this;
|
return *this;
|
||||||
|
|
|
@ -31,8 +31,9 @@ SpectrumAnalyser::~SpectrumAnalyser()
|
||||||
void SpectrumAnalyser::process(IqData *x)
|
void SpectrumAnalyser::process(IqData *x)
|
||||||
{
|
{
|
||||||
// load data and FFT
|
// load data and FFT
|
||||||
|
uint32_t i;
|
||||||
std::deque<std::complex<double>> data = x->get_data();
|
std::deque<std::complex<double>> data = x->get_data();
|
||||||
for (int i = 0; i < nfft; i++)
|
for (i = 0; i < nfft; i++)
|
||||||
{
|
{
|
||||||
dataX[i] = data[i];
|
dataX[i] = data[i];
|
||||||
}
|
}
|
||||||
|
@ -40,14 +41,14 @@ void SpectrumAnalyser::process(IqData *x)
|
||||||
|
|
||||||
// fftshift
|
// fftshift
|
||||||
std::vector<std::complex<double>> fftshift;
|
std::vector<std::complex<double>> fftshift;
|
||||||
for (int i = 0; i < nfft; i++)
|
for (i = 0; i < nfft; i++)
|
||||||
{
|
{
|
||||||
fftshift.push_back(dataX[(i + int(nfft / 2) + 1) % nfft]);
|
fftshift.push_back(dataX[(i + int(nfft / 2) + 1) % nfft]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// decimate
|
// decimate
|
||||||
std::vector<std::complex<double>> spectrum;
|
std::vector<std::complex<double>> spectrum;
|
||||||
for (int i = 0; i < nfft; i+=decimation)
|
for (i = 0; i < nfft; i+=decimation)
|
||||||
{
|
{
|
||||||
spectrum.push_back(fftshift[i]);
|
spectrum.push_back(fftshift[i]);
|
||||||
}
|
}
|
||||||
|
@ -60,7 +61,7 @@ void SpectrumAnalyser::process(IqData *x)
|
||||||
{
|
{
|
||||||
offset = bandwidth/2;
|
offset = bandwidth/2;
|
||||||
}
|
}
|
||||||
for (int i = -nSpectrum/2; i < nSpectrum/2; i++)
|
for (i = -nSpectrum/2; i < nSpectrum/2; i++)
|
||||||
{
|
{
|
||||||
frequency.push_back(((i*bandwidth)+offset+204640000)/1000);
|
frequency.push_back(((i*bandwidth)+offset+204640000)/1000);
|
||||||
}
|
}
|
||||||
|
|
|
@ -56,7 +56,9 @@ void Tracker::update(Detection *detection, uint64_t current)
|
||||||
std::vector<double> snr = detection->get_snr();
|
std::vector<double> snr = detection->get_snr();
|
||||||
|
|
||||||
// init
|
// init
|
||||||
double delayPredict, dopplerPredict, acc;
|
double delayPredict = 0.0;
|
||||||
|
double dopplerPredict = 0.0;
|
||||||
|
double acc = 0.0;
|
||||||
uint32_t nRemove = 0;
|
uint32_t nRemove = 0;
|
||||||
std::string state;
|
std::string state;
|
||||||
|
|
||||||
|
@ -65,7 +67,7 @@ void Tracker::update(Detection *detection, uint64_t current)
|
||||||
timestamp = current;
|
timestamp = current;
|
||||||
|
|
||||||
// loop over each track
|
// loop over each track
|
||||||
for (int i = 0; i < track.get_n(); i++)
|
for (uint64_t i = 0; i < track.get_n(); i++)
|
||||||
{
|
{
|
||||||
// predict next position
|
// predict next position
|
||||||
Detection detectionCurrent = track.get_current(i);
|
Detection detectionCurrent = track.get_current(i);
|
||||||
|
|
|
@ -20,12 +20,12 @@ private:
|
||||||
/// @brief Common MTU size for all socket objects.
|
/// @brief Common MTU size for all socket objects.
|
||||||
static const uint32_t MTU;
|
static const uint32_t MTU;
|
||||||
|
|
||||||
/// @brief The ASIO socket.
|
|
||||||
asio::ip::tcp::socket socket;
|
|
||||||
|
|
||||||
/// @brief The ASIO endpoint.
|
/// @brief The ASIO endpoint.
|
||||||
asio::ip::tcp::endpoint endpoint;
|
asio::ip::tcp::endpoint endpoint;
|
||||||
|
|
||||||
|
/// @brief The ASIO socket.
|
||||||
|
asio::ip::tcp::socket socket;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// @brief Constructor for Socket.
|
/// @brief Constructor for Socket.
|
||||||
/// @param ip IP address of data destination.
|
/// @param ip IP address of data destination.
|
||||||
|
|
Loading…
Reference in a new issue