mirror of https://github.com/gnss-sdr/gnss-sdr
196 lines
7.6 KiB
C++
196 lines
7.6 KiB
C++
/*!
|
|
* \file gnss_sdr_sample_counter.cc
|
|
* \brief Simple block to report the current receiver time based on the output of the tracking or telemetry blocks
|
|
* \author Javier Arribas 2018. jarribas(at)cttc.es
|
|
*
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*
|
|
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
|
* This file is part of GNSS-SDR.
|
|
*
|
|
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
|
* SPDX-License-Identifier: GPL-3.0-or-later
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*/
|
|
|
|
#include "gnss_sdr_sample_counter.h"
|
|
#include "gnss_synchro.h"
|
|
#include "gnss_time.h"
|
|
#include <gnuradio/io_signature.h>
|
|
#include <pmt/pmt.h> // for from_double
|
|
#include <pmt/pmt_sugar.h> // for mp
|
|
#include <cmath> // for round
|
|
#include <iostream> // for operator<<
|
|
#include <memory>
|
|
#include <string> // for string
|
|
#include <vector>
|
|
|
|
#if PMT_USES_BOOST_ANY
|
|
#include <boost/any.hpp>
|
|
namespace wht = boost;
|
|
#else
|
|
#include <any>
|
|
namespace wht = std;
|
|
#endif
|
|
|
|
gnss_sdr_sample_counter::gnss_sdr_sample_counter(
|
|
double _fs,
|
|
int32_t _interval_ms,
|
|
size_t _size)
|
|
: gr::sync_decimator("sample_counter",
|
|
gr::io_signature::make(1, 1, _size),
|
|
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
|
static_cast<uint32_t>(std::round(_fs * static_cast<double>(_interval_ms) / 1e3))),
|
|
fs(_fs),
|
|
current_T_rx_ms(0),
|
|
sample_counter(0),
|
|
interval_ms(_interval_ms),
|
|
report_interval_ms(1000), // default reporting 1 second
|
|
samples_per_output(std::round(fs * static_cast<double>(interval_ms) / 1e3)),
|
|
current_s(0),
|
|
current_m(0),
|
|
current_h(0),
|
|
current_days(0),
|
|
flag_m(false),
|
|
flag_h(false),
|
|
flag_days(false),
|
|
flag_enable_send_msg(false) // enable it for reporting time with asynchronous message
|
|
{
|
|
message_port_register_out(pmt::mp("sample_counter"));
|
|
set_max_noutput_items(1);
|
|
set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work()
|
|
}
|
|
|
|
|
|
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int32_t _interval_ms, size_t _size)
|
|
{
|
|
gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _interval_ms, _size));
|
|
return sample_counter_;
|
|
}
|
|
|
|
|
|
int64_t gnss_sdr_sample_counter::uint64diff(uint64_t first, uint64_t second)
|
|
{
|
|
uint64_t abs_diff = (first > second) ? (first - second) : (second - first);
|
|
assert(abs_diff <= INT64_MAX);
|
|
return (first > second) ? (int64_t)abs_diff : -(int64_t)abs_diff;
|
|
}
|
|
|
|
|
|
int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)),
|
|
gr_vector_const_void_star &input_items __attribute__((unused)),
|
|
gr_vector_void_star &output_items)
|
|
{
|
|
auto *out = reinterpret_cast<Gnss_Synchro *>(output_items[0]);
|
|
out[0] = Gnss_Synchro();
|
|
out[0].Flag_valid_symbol_output = false;
|
|
out[0].Flag_valid_word = false;
|
|
out[0].Channel_ID = -1;
|
|
out[0].fs = fs;
|
|
if ((current_T_rx_ms % report_interval_ms) == 0)
|
|
{
|
|
current_s++;
|
|
if ((current_s % 60) == 0)
|
|
{
|
|
current_s = 0;
|
|
current_m++;
|
|
flag_m = true;
|
|
if ((current_m % 60) == 0)
|
|
{
|
|
current_m = 0;
|
|
current_h++;
|
|
flag_h = true;
|
|
if ((current_h % 24) == 0)
|
|
{
|
|
current_h = 0;
|
|
current_days++;
|
|
flag_days = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (flag_days)
|
|
{
|
|
std::string day;
|
|
if (current_days == 1)
|
|
{
|
|
day = " day ";
|
|
}
|
|
else
|
|
{
|
|
day = " days ";
|
|
}
|
|
std::cout << "Current receiver time: " << current_days << day << current_h << " h " << current_m << " min " << current_s << " s\n";
|
|
}
|
|
else
|
|
{
|
|
if (flag_h)
|
|
{
|
|
std::cout << "Current receiver time: " << current_h << " h " << current_m << " min " << current_s << " s\n";
|
|
}
|
|
else
|
|
{
|
|
if (flag_m)
|
|
{
|
|
std::cout << "Current receiver time: " << current_m << " min " << current_s << " s\n";
|
|
}
|
|
else
|
|
{
|
|
std::cout << "Current receiver time: " << current_s << " s\n";
|
|
}
|
|
}
|
|
}
|
|
if (flag_enable_send_msg)
|
|
{
|
|
message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast<double>(current_T_rx_ms) / 1000.0));
|
|
}
|
|
}
|
|
sample_counter += samples_per_output;
|
|
out[0].Tracking_sample_counter = sample_counter;
|
|
current_T_rx_ms += interval_ms;
|
|
|
|
// *************** time tags ****************
|
|
std::vector<gr::tag_t> tags_vec;
|
|
// notice that nitems_read is updated in decimation blocks after leaving work() with return 1, equivalent to call consume_each
|
|
this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + samples_per_output);
|
|
for (const auto &it : tags_vec)
|
|
{
|
|
try
|
|
{
|
|
if (pmt::any_ref(it.value).type().hash_code() == typeid(const std::shared_ptr<GnssTime>).hash_code())
|
|
{
|
|
// recompute timestamp to match the last sample in the consumed samples in this batch
|
|
int64_t diff_samplecount = uint64diff(out[0].Tracking_sample_counter, it.offset);
|
|
const auto last_timetag = wht::any_cast<const std::shared_ptr<GnssTime>>(pmt::any_ref(it.value));
|
|
double intpart;
|
|
last_timetag->tow_ms_fraction += modf(1000.0 * static_cast<double>(diff_samplecount) / fs, &intpart);
|
|
|
|
last_timetag->tow_ms = last_timetag->tow_ms + static_cast<int>(intpart);
|
|
last_timetag->rx_time = static_cast<double>(out[0].Tracking_sample_counter) / fs;
|
|
add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(last_timetag));
|
|
// std::cout << "COUNTER TAG: this->nitems_read(0):" << this->nitems_read(0) << " sample_counter:" << sample_counter
|
|
// << " it->offset:" << it->offset << " diff:" << diff_samplecount << "\n";
|
|
// getchar();
|
|
}
|
|
else
|
|
{
|
|
std::cout << "hash code not match\n";
|
|
}
|
|
}
|
|
catch (const wht::bad_any_cast &e)
|
|
{
|
|
std::cout << "msg Bad any_cast: " << e.what();
|
|
}
|
|
catch (const std::exception &ee)
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
// ************ end time tags **************
|
|
|
|
return 1;
|
|
}
|