uhd time sync

#include <uhd/utils/thread_priority.hpp>
#include <uhd/utils/safe_main.hpp>
#include <uhd/usrp/multi_usrp.hpp>
#include <boost/program_options.hpp>
#include <boost/thread.hpp>
#include <fstream>
#include <thread>
#include <csignal>

static bool stop_signal_called = false;

void sig_int_handler(int) {
    std::cout << "SIG INT HANDLED" << std::endl;
    stop_signal_called = true;
}

/*!
 * Wrapper on top of uhd device
 */
class USRPController {
public:
    uhd::usrp::multi_usrp::sptr device;

    USRPController(std::string serial, double rate, double freq, double gain, std::string file_name,
                   std::string time_source) :
            serial(serial), rate(rate), freq(freq), gain(gain) {
        log_prepend = "[DEV " + serial + "] ";
        std::cout << log_prepend << "Creating.." << std::endl;

        // I using b205mini, that means it don't have ethernet interface, just usb (access via serial number only)
        // Now UHD doesn't support multi-serials in dev-args
        this->device = uhd::usrp::multi_usrp::make("serial=" + serial);
        this->device->set_time_source(time_source); //applies to all mboards
        this->device->set_rx_subdev_spec(std::string("A:A"));

        uhd::tune_request_t tune_request(this->freq);
        this->device->set_rx_freq(tune_request);
        this->device->set_rx_gain(this->gain);
        this->device->set_rx_rate(this->rate);
        this->device->set_rx_bandwidth(this->rate);
        this->device->set_rx_antenna("RX2");

        std::string cpu_format = "fc32";
        std::string wire_format = "sc16";
        uhd::stream_args_t stream_args(cpu_format, wire_format);
        this->rx_stream = this->device->get_rx_stream(stream_args);

        full_file_name = "/path/to/files/" + file_name;

        size_t samps_per_buff = 10000;// samples per buffer
        this->buff = std::vector<std::complex<float>>(samps_per_buff);
        outfile.open(full_file_name.c_str(), std::ofstream::binary);
    }

    void start(uhd::time_spec_t cmd_time) {
        //setup streaming
        uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
        stream_cmd.num_samps = size_t(0);
        stream_cmd.stream_now = false;
        stream_cmd.time_spec = uhd::time_spec_t(cmd_time);
        this->rx_stream->issue_stream_cmd(stream_cmd);
    }

    /*!
     * Read from device & writing to file will be executed in separate thread
     * @return
     */
    bool work() {
        size_t num_rx_samps = this->rx_stream->recv(&buff.front(), buff.size(), md, 3.0, true);

        if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_TIMEOUT) {
            std::cout << log_prepend << boost::format("Timeout while streaming") << std::endl;
        } else if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_OVERFLOW) {
            if (overflow_message) {
                overflow_message = false;
                std::cerr << log_prepend << boost::format(
                        "Got an overflow indication. Please consider the following:\n"
                                "  Your write medium must sustain a rate of %fMB/s.\n"
                                "  Dropped samples will not be written to the file.\n"
                                "  Please modify this example for your purposes.\n"
                                "  This message will not appear again.\n"
                ) % (this->device->get_rx_rate() * sizeof(std::complex<float>) / 1e6);
            }
            return true;
        } else if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_NONE) {
//            its o'k'?))
        } else {
            std::string error = str(boost::format("Uncaught error at %s %s") % (log_prepend, md.strerror()));
            throw std::runtime_error(error);
        }

        if (outfile.is_open()) {
            outfile.write((const char *) &buff.front(), num_rx_samps * sizeof(std::complex<float>));
        }

        return true;
    }

private:
    std::string serial;
    double rate;
    double freq;
    double gain;

    uhd::rx_streamer::sptr rx_stream;
    std::vector<std::complex<float>> buff;
    uhd::rx_metadata_t md;
    bool overflow_message = true;
    std::string full_file_name;
    std::string log_prepend;
    std::ofstream outfile;
};

void deviceThreadFunc(USRPController *controller) {
    while (not stop_signal_called) {
        if (not controller->work()) break;
    }
}

/*!
 * Just check if time is same on devices
 * @param dev1
 * @param dev2
 */
void ppsControl(USRPController *dev1, USRPController *dev2) {
    while (not stop_signal_called) {
        const uhd::time_spec_t dtime1 = dev1->device->get_time_now();
        const uhd::time_spec_t dtime2 = dev2->device->get_time_now();
        if (dtime1.get_real_secs() != dtime2.get_real_secs()) {
            std::cout << "not same: " << dtime1.get_real_secs() << " " << dtime2.get_real_secs() << std::endl;
        } else {
            std::cout << "is same! : " << dtime1.get_real_secs() << " " << dtime2.get_real_secs() << std::endl;
        }
        boost::this_thread::sleep(boost::posix_time::milliseconds(100));
    }
}

int UHD_SAFE_MAIN(int argc, char *argv[]) {
    uhd::set_thread_priority_safe();

    std::signal(SIGINT, &sig_int_handler);
    std::signal(SIGHUP, &sig_int_handler);

    double rate(5e6);
    double gain(32);

    USRPController *device_one = new USRPController("serial1", rate, 400e6, gain, "channels1", "internal");
    USRPController *device_two = new USRPController("serial2", rate, 400e6, gain, "channels2", "internal");

    boost::this_thread::sleep(boost::posix_time::seconds(3));

    // Got time Once and share to devices
    long ttt = clock();
    device_one->device->set_time_now(ttt);
    device_two->device->set_time_now(ttt);

    std::thread ppmControlThread(ppsControl, device_one, device_two);

    boost::this_thread::sleep(boost::posix_time::seconds(3));

    uhd::time_spec_t start_timeout = uhd::time_spec_t(7.0);
    uhd::time_spec_t cmd_time_one = device_one->device->get_time_now() + start_timeout;
    uhd::time_spec_t cmd_time_two = device_two->device->get_time_now() + start_timeout;

    device_one->start(cmd_time_one);
    device_two->start(cmd_time_two);

    std::thread first(deviceThreadFunc, device_one);
    std::thread second(deviceThreadFunc, device_two);

    first.join();
    second.join();

    return EXIT_SUCCESS;
}