/* Atomically increases the semaphore's count (not blocking) */
int SDL_SemPost(SDL_sem *sem)
{
if ( ! sem ) {
SDL_SetError("Passed a NULL semaphore");
return -1;
}
if ( unlock_semaphore(sem->id) != 0 ) {
SDL_SetError("release_sem() failed");
return -1;
}
return 0;
}
int main( int argc, char** argv )
{
if ( 2 == argc )
{
int nSema = atoi( argv[1] );
int nError = unlock_semaphore( nSema );
if ( 0 != nError ) {
printf( "ReleaseSemaphore failed : %s\n", strerror( errno ) );
}
}
else
{
printf( "Invalid argument count\n" );
}
}
int UHD_SAFE_MAIN(int argc, char *argv[]){
uhd::set_thread_priority_safe();
size_t rxshm_size, txshm_size;
bool mimic_active;
float mimic_delay;
unsigned int iSide, iSwing; // often used loop variables
int32_t rx_worker_status = 0;
int32_t clrfreq_rx_worker_status = 0;
int32_t mute_output = 0; // used if rx_worker error happends
int32_t rx_stream_reset_count = 0;
int32_t rx_stream_error_count = 0;
std::vector<sem_t> sem_rx_vec(nSwings), sem_tx_vec(nSwings);
std::vector<uint32_t> state_vec(nSwings, ST_INIT);
uint32_t swing; // = SWING0;
size_t nSamples_rx, nSamples_tx_pulse, nSamples_pause_after_rx, nSamples_auto_clear_freq, nSamples_rx_total;
size_t auto_clear_freq_available = 0;
uint32_t npulses, nerrors;
ssize_t cmd_status;
uint32_t usrp_driver_base_port, ip_part;
int32_t connect_retrys = MAX_SOCKET_RETRYS;
int32_t sockopt;
struct sockaddr_in sockaddr;
struct sockaddr_storage client_addr;
socklen_t addr_size;
uint32_t exit_driver = 0;
uint32_t tx_worker_active;
uhd::time_spec_t start_time, rx_start_time;
// vector of all pulse start times over an integration period
std::vector<uhd::time_spec_t> pulse_time_offsets;
// vector of the sample index of pulse start times over an integration period
std::vector<uint64_t> pulse_sample_idx_offsets;
boost::thread_group uhd_threads;
boost::thread_group clrfreq_threads;
// process config file for port and SHM sizes
DEBUG_PRINT("USRP_DRIVER starting to read driver_config.ini\n");
boost::property_tree::ptree pt;
boost::property_tree::ini_parser::read_ini("../driver_config.ini", pt);
// DEBUG_PRINT("USRP_DRIVER reading rxshm_size\n");
// std::cout << pt.get<std::string>("shm_settings.rxshm_size") << '\n';
rxshm_size = std::stoi(pt.get<std::string>("shm_settings.rxshm_size"));
// DEBUG_PRINT("USRP_DRIVER reading txshm_size\n");
txshm_size = std::stoi(pt.get<std::string>("shm_settings.txshm_size"));
usrp_driver_base_port = std::stoi(pt.get<std::string>("network_settings.USRPDriverPort"));
boost::property_tree::ptree pt_array;
DEBUG_PRINT("USRP_DRIVER starting to read array_config.ini\n");
boost::property_tree::ini_parser::read_ini("../array_config.ini", pt_array);
mimic_active = std::stof(pt_array.get<std::string>("mimic.mimic_active")) != 0;
mimic_delay = std::stof(pt_array.get<std::string>("mimic.mimic_delay"));
fprintf(stderr, "read from ini: mimic_active=%d, mimic_delay=%f\n", mimic_active, mimic_delay);
init_all_dirs();
// TODO also read usrp_config.ini and get antenna and side information from it. remove antenna input argument.
// process command line arguments
struct arg_lit *al_help = arg_lit0(NULL, "help", "Prints help information and then exits");
// struct arg_int *ai_ant = arg_intn("a", "antenna", NULL, 1, 2, "Antenna position index for the USRP");
struct arg_int *ai_ant_a = arg_int0("a", "antennaA", NULL, "Antenna position index for the USRP on side A");
struct arg_int *ai_ant_b = arg_int0("b", "antennaB", NULL, "Antenna position index for the USRP on side B");
struct arg_str *as_host = arg_str0("h", "host", NULL, "Hostname or IP address of USRP to control (e.g usrp1)");
struct arg_lit *al_intclk = arg_lit0("i", "intclk", "Select internal clock (default is external)");
struct arg_lit *al_interferometer = arg_lit0("x", "interferometer", "Disable tx_worker for interferometer antennas");
struct arg_end *ae_argend = arg_end(ARG_MAXERRORS);
void* argtable[] = {al_help, ai_ant_a, ai_ant_b, as_host, al_intclk, al_interferometer, ae_argend};
double txrate, rxrate, txfreq, rxfreq;
double txrate_new, rxrate_new, txfreq_new, rxfreq_new;
DEBUG_PRINT("usrp_driver debug mode enabled\n");
if (SUPRESS_UHD_PRINTS) {
uhd::msg::register_handler(&uhd_term_message_handler);
}
nerrors = arg_parse(argc,argv,argtable);
if (nerrors > 0) {
arg_print_errors(stdout,ae_argend,"usrp_driver");
exit(1);
}
if (argc == 1) {
printf("No arguments found, try running again with --help for more information.\n");
exit(1);
}
if(al_help->count > 0) {
printf("Usage: ");
arg_print_syntax(stdout,argtable,"\n");
arg_print_glossary(stdout,argtable," %-25s %s\n");
arg_freetable(argtable, sizeof(argtable)/sizeof(argtable[0]));
return 0;
}
unsigned int nSides = ai_ant_a->count + ai_ant_b->count;
if( nSides == 0 ) {
printf("No antenna index, exiting...");
return 0;
}
if(as_host->sval == NULL) {
printf("Missing usrp host command line argument, exiting...");
return 0;
}
std::vector<int> antennaVector(nSides);
std::vector<uint64_t> channel_numbers;
// both sides
if( nSides == 2 ) {
DEBUG_PRINT("Setting side A: ant_idx %d\n",ai_ant_a->ival[0]);
antennaVector[0] = ai_ant_a->ival[0];
channel_numbers.push_back(0);
DEBUG_PRINT("Setting side B: ant_idx %d\n",ai_ant_b->ival[0]);
antennaVector[1] = ai_ant_b->ival[0];
channel_numbers.push_back(1);
} else {
// side A
if (ai_ant_a->count == 1) {
DEBUG_PRINT("Setting side A: ant_idx %d\n",ai_ant_a->ival[0]);
antennaVector[0] = ai_ant_a->ival[0];
channel_numbers.push_back(0);
// side B
} else {
DEBUG_PRINT("Setting side B: ant_idx %d\n",ai_ant_b->ival[0]);
antennaVector[0] = ai_ant_b->ival[0];
channel_numbers.push_back(1);
DEBUG_PRINT("Warning: For one side use DIO output is always on Side A!!!!!!!!!!!!!"); // TODO correct this
}
}
// pointers to shared memory
std::vector<std::vector<void *>> shm_rx_vec(nSides, std::vector<void *>( nSwings));
std::vector<std::vector<void *>> shm_tx_vec(nSides, std::vector<void *>( nSwings));
// local buffers for tx and rx
std::vector<std::vector<std::complex<int16_t>>> tx_samples(nSides, std::vector<std::complex<int16_t>>(MAX_PULSE_LENGTH,0));
std::vector<std::vector<std::complex<int16_t>>> rx_data_buffer(nSides, std::vector<std::complex<int16_t>>(0));
std::vector<std::vector<std::complex<int16_t>>> rx_auto_clear_freq(nSides, std::vector<std::complex<int16_t>>(0));
std::string usrpargs(as_host->sval[0]);
usrpargs = "addr0=" + usrpargs + ",master_clock_rate=200.0e6";
// usrpargs = "addr0=" + usrpargs + ",master_clock_rate=200.0e6,recv_frame_size=50000000";
uhd::usrp::multi_usrp::sptr usrp = uhd::usrp::multi_usrp::make(usrpargs);
// usrp->set_rx_subdev_spec(uhd::usrp::subdev_spec_t("A:A B:A"));
// usrp->set_tx_subdev_spec(uhd::usrp::subdev_spec_t("A:A B:A"));
boost::this_thread::sleep(boost::posix_time::seconds(SETUP_WAIT));
uhd::stream_args_t stream_args("sc16", "sc16");
if (usrp->get_rx_num_channels() < nSides || usrp->get_tx_num_channels() < nSides) {
DEBUG_PRINT("ERROR: Number of defined channels (%i) is smaller than avaialable channels:\n usrp->get_rx_num_channels(): %lu \n usrp->get_tx_num_channels(): %lu \n\n", nSides, usrp->get_rx_num_channels(), usrp->get_tx_num_channels());
return -1;
}
stream_args.channels = channel_numbers;
uhd::rx_streamer::sptr rx_stream = usrp->get_rx_stream(stream_args);
uhd::tx_streamer::sptr tx_stream = usrp->get_tx_stream(stream_args);
// TODO: retry uhd connection if fails..
// Determine port from 3rd part of ip (192.168.x.2 => port = base_port + x )
int start_idx = usrpargs.find(".");
start_idx = usrpargs.find(".", start_idx+1);
int end_idx = usrpargs.find(".", start_idx+1);
ip_part = atoi(usrpargs.substr(start_idx+1, end_idx-start_idx-1).c_str());
// initialize rxfe gpio
kodiak_init_rxfe(usrp, nSides);
// initialize other gpio on usrp
init_timing_signals(usrp, mimic_active, nSides);
//if(CAPTURE_ERRORS) {
// signal(SIGINT, siginthandler);
//}
// open shared memory buffers and semaphores created by cuda_driver.py
// for dual polarization we use antenna numbers 20 to 35 (side is always 0)
for(iSwing = 0; iSwing < nSwings; iSwing++) {
for(iSide = 0; iSide < nSides; iSide++) {
int shm_side = 0;
shm_rx_vec[iSide][iSwing] = open_sample_shm(antennaVector[iSide], RXDIR, shm_side, iSwing, rxshm_size);
shm_tx_vec[iSide][iSwing] = open_sample_shm(antennaVector[iSide], TXDIR, shm_side, iSwing, txshm_size);
DEBUG_PRINT("usrp_driver rx shm addr: %p iSide: %d iSwing: %d\n", shm_rx_vec[iSide][iSwing], iSide, iSwing);
if (antennaVector[iSide] < 19 ) { // semaphores only for antennas of first polarization TODO check if this is enough
sem_rx_vec[iSwing] = open_sample_semaphore(antennaVector[iSide], iSwing, RXDIR);
sem_tx_vec[iSwing] = open_sample_semaphore(antennaVector[iSide], iSwing, TXDIR);
}
}
}
if(al_interferometer->count > 0) {
DEBUG_PRINT("Disable tx_worker ...\n");
tx_worker_active = 0;
} else {
tx_worker_active = 1;
}
if(al_intclk->count > 0) {
usrp->set_clock_source("internal");
usrp->set_time_source("internal");
}
else {
// sync clock with external 10 MHz and PPS
DEBUG_PRINT("Set clock: external\n");
usrp->set_clock_source("external", 0);
DEBUG_PRINT("Set time: external\n");
usrp->set_time_source("external", 0);
DEBUG_PRINT("Done setting time and clock\n");
}
while(true) {
if(driversock) {
close(driverconn);
close(driversock);
}
boost::this_thread::sleep(boost::posix_time::seconds(SETUP_WAIT));
// bind to socket for communication with usrp_server.py:
driversock = socket(AF_INET, SOCK_STREAM, 0);
if(driversock < 0){
perror("opening stream socket\n");
exit(1);
}
sockopt = 1;
setsockopt(driversock, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof(int32_t));
sockaddr.sin_family = AF_INET;
// TODO: maybe limit addr to interface connected to usrp_server
sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
fprintf(stderr, "listening on port: %d\n", usrp_driver_base_port + ip_part);
sockaddr.sin_port = htons(usrp_driver_base_port + ip_part);
if( bind(driversock, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) < 0){
perror("binding tx stream socket");
exit(1);
}
// wait for connection...
listen(driversock, 5);
// and accept it
fprintf(stderr, "waiting for socket connection\n");
addr_size = sizeof(client_addr);
driverconn = accept(driversock, (struct sockaddr *) &client_addr, &addr_size);
fprintf(stderr, "accepted socket connection\n");
while(true) {
// wait for transport endpoint to connect?
DEBUG_PRINT("USRP_DRIVER waiting for command\n");
uint8_t command = sock_get_cmd(driverconn, &cmd_status);
DEBUG_PRINT("USRP_DRIVER received command, status: %zu\n", cmd_status);
// see if socket is closed..
if(cmd_status == 11 || cmd_status == 0 || cmd_status < 0) {
DEBUG_PRINT("USRP_DRIVER lost connection to usrp_server, waiting for fresh connection, %d tries remaining\n", connect_retrys);
close(driversock);
if(connect_retrys-- < 0) {
exit(1);
}
sleep(1);
break;
}
connect_retrys = MAX_SOCKET_RETRYS;
switch(command) {
case USRP_SETUP: {
// receive infomation about a pulse sequence/integration period
// transmit/receive center frequenies and sampling rates
// number of tx/rx samples
// number of pulse sequences per integration period, and pulse start times
swing = sock_get_int16( driverconn);
DEBUG_PRINT("entering USRP_SETUP command (swing %d)\n", swing);
txfreq_new = sock_get_float64(driverconn);
rxfreq_new = sock_get_float64(driverconn);
txrate_new = sock_get_float64(driverconn);
rxrate_new = sock_get_float64(driverconn);
npulses = sock_get_uint32(driverconn);
nSamples_rx = sock_get_uint64(driverconn);
nSamples_pause_after_rx = sock_get_uint64(driverconn);
nSamples_auto_clear_freq = sock_get_uint64(driverconn);
nSamples_tx_pulse = sock_get_uint64(driverconn);
nSamples_rx_total = nSamples_rx + nSamples_pause_after_rx + nSamples_auto_clear_freq;
DEBUG_PRINT("USRP_SETUP number of requested rx samples: %d + %d pause + %d auto clear freq\n", (uint32_t) nSamples_rx, nSamples_pause_after_rx, nSamples_auto_clear_freq);
DEBUG_PRINT("USRP_SETUP number of requested tx samples per pulse: %d\n", (uint32_t) nSamples_tx_pulse);
DEBUG_PRINT("USRP_SETUP existing tx rate : %f (swing %d)\n", txrate, swing);
DEBUG_PRINT("USRP_SETUP requested tx rate: %f\n", txrate_new);
// resize
pulse_sample_idx_offsets.resize(npulses);
pulse_time_offsets.resize(npulses);
for(uint32_t i = 0; i < npulses; i++) {
// DEBUG_PRINT("USRP_SETUP waiting for pulse offset %d of %d\n", i+2, npulses);
pulse_sample_idx_offsets[i] = sock_get_uint64(driverconn);
// DEBUG_PRINT("USRP_SETUP received %zu pulse offset\n", pulse_sample_idx_offsets[i]);
}
DEBUG_PRINT("USRP_SETUP resize autoclear freq\n");
// RESIZE LOCAL BUFFERS
if(rx_data_buffer[0].size() < nSamples_rx_total) {
for(iSide = 0; iSide < nSides; iSide++) {
rx_data_buffer[iSide].resize(nSamples_rx_total);
}
}
DEBUG_PRINT("USRP_SETUP resize autoclear freq\n");
if(nSamples_auto_clear_freq != 0 and rx_auto_clear_freq[0].size() < nSamples_auto_clear_freq) {
for(iSide = 0; iSide < nSides; iSide++) {
rx_auto_clear_freq[iSide].resize(nSamples_auto_clear_freq);
}
}
// TODO use return argument of set_xx to save new rate/freq
// if necessary, retune USRP frequency and sampling rate
if(rxrate != rxrate_new) {
usrp->set_rx_rate(rxrate_new);
rxrate = usrp->get_rx_rate();
}
if(txrate != txrate_new) {
usrp->set_tx_rate(txrate_new);
txrate = usrp->get_tx_rate();
}
if(rxfreq != rxfreq_new) {
for(iSide = 0; iSide < nSides; iSide++) {
usrp->set_rx_freq(rxfreq_new, iSide);
}
rxfreq = usrp->get_rx_freq();
}
if(txfreq != txfreq_new) {
for(iSide = 0; iSide < nSides; iSide++) {
usrp->set_tx_freq(txfreq_new, iSide);
}
txfreq = usrp->get_tx_freq();
}
if(verbose) {
std::cout << boost::format("Actual RX Freq: %f MHz...") % (usrp->get_rx_freq()/1e6) << std::endl;
std::cout << boost::format("Actual RX Rate: %f Msps...") % (usrp->get_rx_rate()/1e6) << std::endl;
std::cout << boost::format("Actual TX Freq: %f MHz...") % (usrp->get_tx_freq()/1e6) << std::endl;
std::cout << boost::format("Actual TX Rate: %f Msps...") % (usrp->get_tx_rate()/1e6) << std::endl;
}
// TODO: set the number of samples in a pulse. this is calculated from the pulse duration and the sampling rate
// when do we know this? after USRP_SETUP
// create local copy of transmit pulse data from shared memory
std::complex<int16_t> *shm_pulseaddr;
size_t spb = tx_stream->get_max_num_samps();
size_t pulse_bytes = sizeof(std::complex<int16_t>) * nSamples_tx_pulse;
size_t number_of_pulses = pulse_time_offsets.size();
size_t num_samples_per_pulse_with_padding = nSamples_tx_pulse + 2*spb;
DEBUG_PRINT("spb %d, pulse length %d samples, pulse with padding %d\n", spb, nSamples_tx_pulse, num_samples_per_pulse_with_padding);
// TODO unpack and pad tx sample
for (iSide = 0; iSide<nSides; iSide++) {
tx_samples[iSide].resize(number_of_pulses * (num_samples_per_pulse_with_padding));
for(uint32_t p_i = 0; p_i < number_of_pulses; p_i++) {
shm_pulseaddr = &((std::complex<int16_t> *) shm_tx_vec[iSide][swing])[p_i*nSamples_tx_pulse];
memcpy(&tx_samples[iSide][spb + p_i*(num_samples_per_pulse_with_padding)], shm_pulseaddr, pulse_bytes);
}
}
if(SAVE_RAW_SAMPLES_DEBUG) {
FILE *raw_dump_fp;
char raw_dump_name[80];
// DEBUG_PRINT("Exporting %i raw tx_samples (%i + 2* %i)\n", num_samples_per_pulse_with_padding, nSamples_tx_pulse, spb);
for (iSide =0; iSide < nSides; iSide++){
sprintf(raw_dump_name,"%s/raw_samples_tx_ant_%d.cint16", diag_dir, antennaVector[iSide]);
raw_dump_fp = fopen(raw_dump_name, "wb");
fwrite(&tx_samples[iSide][0], sizeof(std::complex<int16_t>),num_samples_per_pulse_with_padding*number_of_pulses, raw_dump_fp);
fclose(raw_dump_fp);
}
}
state_vec[swing] = ST_READY;
DEBUG_PRINT("changing state_vec[swing] to ST_READY\n");
sock_send_uint8(driverconn, USRP_SETUP);
break;
}
case RXFE_SET: {
DEBUG_PRINT("entering RXFE_SET command\n");
RXFESettings rf_settings;
rf_settings.amp1 = sock_get_uint8(driverconn);
rf_settings.amp2 = sock_get_uint8(driverconn);
uint8_t attTimes2 = sock_get_uint8(driverconn);
rf_settings.att_05_dB = ( attTimes2 & 0x01 ) != 0;
rf_settings.att_1_dB = ( attTimes2 & 0x02 ) != 0;
rf_settings.att_2_dB = ( attTimes2 & 0x04 ) != 0;
rf_settings.att_4_dB = ( attTimes2 & 0x08 ) != 0;
rf_settings.att_8_dB = ( attTimes2 & 0x10 ) != 0;
rf_settings.att_16_dB = ( attTimes2 & 0x20 ) != 0;
kodiak_set_rxfe(usrp, rf_settings, nSides);
sock_send_uint8(driverconn, RXFE_SET);
break;
}
case TRIGGER_PULSE: {
swing = sock_get_int16( driverconn);
DEBUG_PRINT("entering TRIGGER_PULSE command (swing %d)\n", swing );
if (state_vec[swing] != ST_READY) {
sock_send_uint8(driverconn, TRIGGER_BUSY);
DEBUG_PRINT("TRIGGER_PULSE busy in state_vec[swing] %d, returning\n", state_vec[swing]);
}
else {
DEBUG_PRINT("TRIGGER_PULSE ready\n");
state_vec[swing] = ST_PULSE;
DEBUG_PRINT("TRIGGER_PULSE locking semaphore\n");
lock_semaphore(sem_rx_vec[swing]);
lock_semaphore(sem_tx_vec[swing]);
DEBUG_PRINT("TRIGGER_PULSE semaphore locked\n");
// create local copy of transmit pulse data from shared memory
size_t spb = tx_stream->get_max_num_samps();
size_t pulse_bytes = sizeof(std::complex<int16_t>) * nSamples_tx_pulse;
size_t number_of_pulses = pulse_time_offsets.size();
size_t num_samples_per_pulse_with_padding = nSamples_tx_pulse + 2*spb;
DEBUG_PRINT("spb %d, pulse length %d samples, pulse with padding %d\n", spb, nSamples_tx_pulse, num_samples_per_pulse_with_padding);
// read in time for start of pulse sequence over socket
uint32_t pulse_time_full = sock_get_uint32(driverconn);
double pulse_time_frac = sock_get_float64(driverconn);
start_time = uhd::time_spec_t(pulse_time_full, pulse_time_frac);
double tr_to_pulse_delay = sock_get_float64(driverconn);
// calculate usrp clock time of the start of each pulse over the integration period
// so we can schedule the io (perhaps we will have to move io off of the usrp if it can't keep up)
for(uint32_t p_i = 0; p_i < number_of_pulses; p_i++) {
double offset_time = pulse_sample_idx_offsets[p_i] / txrate;
pulse_time_offsets[p_i] = offset_time_spec(start_time, offset_time);
// DEBUG_PRINT("TRIGGER_PULSE pulse time %d is %2.5f\n", p_i, pulse_time_offsets[p_i].get_real_secs());
}
DEBUG_PRINT("first TRIGGER_PULSE time is %2.5f and last is %2.5f\n", pulse_time_offsets[0].get_real_secs(), pulse_time_offsets.back().get_real_secs());
rx_start_time = offset_time_spec(start_time, tr_to_pulse_delay/1e6);
rx_start_time = offset_time_spec(rx_start_time, pulse_sample_idx_offsets[0]/txrate);
// send_timing_for_sequence(usrp, start_time, pulse_times);
double pulseLength = nSamples_tx_pulse / txrate;
// float debugt = usrp->get_time_now().get_real_secs();
// DEBUG_PRINT("USRP_DRIVER: spawning worker threads at usrp_time %2.4f\n", debugt);
DEBUG_PRINT("TRIGGER_PULSE creating rx and tx worker threads on swing %d (nSamples_rx= %d + %d pause + %d auto clear freq )\n", swing,(int) nSamples_rx, nSamples_pause_after_rx, nSamples_auto_clear_freq);
// works fine with tx_worker and dio_worker, fails if rx_worker is enabled
uhd_threads.create_thread(boost::bind(usrp_rx_worker, usrp, rx_stream, &rx_data_buffer, nSamples_rx_total, rx_start_time, &rx_worker_status));
useconds_t usecs=1000;
if (tx_worker_active) {
usleep(usecs);
uhd_threads.create_thread(boost::bind(usrp_tx_worker, tx_stream, &tx_samples, num_samples_per_pulse_with_padding, start_time, pulse_sample_idx_offsets));
}
usleep(usecs);
uhd_threads.create_thread(boost::bind(send_timing_for_sequence, usrp, start_time, pulse_time_offsets, pulseLength, mimic_active, mimic_delay, nSides));
sock_send_uint8(driverconn, TRIGGER_PULSE);
uhd_threads.join_all(); // wait for transmit threads to finish, drawn from shared memory..
DEBUG_PRINT("TRIGGER_PULSE rx_worker, tx_worker and dio threads on swing %d\n joined.", swing);
}
break;
}
case READY_DATA: {
swing = sock_get_int16( driverconn);
DEBUG_PRINT("READY_DATA command (swing %d), waiting for uhd threads to join back\n", swing);
DEBUG_PRINT("READY_DATA unlocking swing a semaphore\n");
unlock_semaphore(sem_rx_vec[swing]);
unlock_semaphore(sem_tx_vec[swing]);
DEBUG_PRINT("READY_DATA usrp worker threads joined, semaphore unlocked, sending metadata\n");
// TODO: handle multiple channels of data.., use channel index to pick correct swath of memory to copy into shm
// rx_worker_status =1; //DEBUG
if(rx_worker_status){
fprintf(stderr, "Error in rx_worker. Setting state to %d.\n", rx_worker_status);
state_vec[swing] = rx_worker_status;
rx_worker_status = 0;
mute_output = 1;
rx_stream_error_count++;
if (rx_stream_reset_count >= MAX_STREAM_RESETS) {
fprintf(stderr, "READY_DATA: shutting down usrp_driver to avoid streamer reset overflow (after %dth reset)\n", rx_stream_reset_count);
// send all data to server, clean up and exit after that
exit_driver = 1;
}
if((rx_worker_status != RX_WORKER_STREAM_TIME_ERROR) && (rx_stream_error_count > 4)) {
// recreate rx_stream unless the error was from sending the stream command too late
rx_stream_reset_count++;
fprintf(stderr, "READY_DATA: recreating rx_stream %dth time! (buffer overflow will occur for 126th time)\n", rx_stream_reset_count);
rx_stream.reset();
rx_stream = usrp->get_rx_stream(stream_args);
}
auto_clear_freq_available = 0;
}
else {
rx_stream_error_count = 0;
auto_clear_freq_available = 1;
}
DEBUG_PRINT("READY_DATA state: %d, ant: %d, num_samples: %zu\n", state_vec[swing], antennaVector[0], nSamples_rx);
sock_send_int32(driverconn, state_vec[swing]); // send status
sock_send_int32(driverconn, antennaVector[0]); // send antenna TODO do this for both antennas?
sock_send_int32(driverconn, nSamples_rx); // nsamples; send send number of samples
// read FAULT status
bool fault;
for (iSide =0; iSide<nSides; iSide++){
fault = read_FAULT_status_from_control_board(usrp, iSide);
}
// TODO move this in loop as soon as usrp_server receives both sides
sock_send_bool(driverconn, fault); // FAULT status from conrol board
if (mute_output) {
DEBUG_PRINT("READY_DATA: Filling SHM with zeros (because of rx_worker error) \n");
for (iSide = 0; iSide<nSides; iSide++) {
memset(shm_rx_vec[iSide][swing], 0, rxshm_size);
std::fill(rx_auto_clear_freq[iSide].begin(), rx_auto_clear_freq[iSide].end(), 0);
}
mute_output = 0;
}
else {
DEBUG_PRINT("READY_DATA starting copying rx data buffer to shared memory\n");
// regural rx data
for (iSide = 0; iSide<nSides; iSide++) {
// DEBUG_PRINT("usrp_drivercopy to rx shm addr: %p iSide: %d iSwing: %d\n", shm_rx_vec[iSide][swing], iSide, iSwing);
memcpy(shm_rx_vec[iSide][swing], &rx_data_buffer[iSide][0], sizeof(std::complex<int16_t>) * nSamples_rx);
}
// auto clear freq samples
for (iSide = 0; iSide<nSides; iSide++) {
for (int iSample = 0; iSample < nSamples_auto_clear_freq; iSample++) {
rx_auto_clear_freq[iSide][iSample] = rx_data_buffer[iSide][nSamples_rx+nSamples_pause_after_rx+ iSample];
}
}
}
if(SAVE_RAW_SAMPLES_DEBUG) {
FILE *raw_dump_fp;
char raw_dump_name[80];
for (iSide=0; iSide<nSides; iSide++) {
sprintf(raw_dump_name,"%s/raw_samples_rx_ant_%d.cint16", diag_dir, antennaVector[iSide]);
raw_dump_fp = fopen(raw_dump_name, "wb");
fwrite(&rx_data_buffer[iSide], sizeof(std::complex<int16_t>), nSamples_rx_total, raw_dump_fp);
fclose(raw_dump_fp);
}
}
DEBUG_PRINT("READY_DATA finished copying rx data buffer to shared memory\n");
state_vec[swing] = ST_READY;
DEBUG_PRINT("changing state_vec[swing] to ST_READY\n");
DEBUG_PRINT("READY_DATA returning command success \n");
sock_send_uint8(driverconn, READY_DATA);
break;
}
case UHD_GETTIME: {
DEBUG_PRINT("entering UHD_GETTIME command\n");
start_time = usrp->get_time_now();
uint32_t real_time = start_time.get_real_secs();
double frac_time = start_time.get_frac_secs();
sock_send_uint32(driverconn, real_time);
sock_send_float64(driverconn, frac_time);
DEBUG_PRINT("UHD_GETTIME current UHD time: %d %.2f command\n", real_time, frac_time);
sock_send_uint8(driverconn, UHD_GETTIME);
break;
}
// command to reset time, sync time with external PPS pulse
case UHD_SYNC: {
DEBUG_PRINT("entering UHD_SYNC command\n");
// if --intclk flag passed to usrp_driver, set clock source as internal and do not sync time
if(al_intclk->count > 0) {
usrp->set_time_now(uhd::time_spec_t(0.0));
}
else {
/* const uhd::time_spec_t last_pps_time = usrp->get_time_last_pps();
while (last_pps_time == usrp->get_time_last_pps()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
}
usrp->set_time_next_pps(uhd::time_spec_t(0.0));
boost::this_thread::sleep(boost::posix_time::milliseconds(1100));
*/
DEBUG_PRINT("Start setting unknown pps\n");
usrp->set_time_unknown_pps(uhd::time_spec_t(11.0));
DEBUG_PRINT("end setting unknown pps\n");
}
sock_send_uint8(driverconn, UHD_SYNC);
break;
}
case AUTOCLRFREQ: {
// has to be called after GET_DATA and before USRP_SETUP
DEBUG_PRINT("entering getting auto clear freq command\n");
// uint32_t num_clrfreq_samples = sock_get_uint32(driverconn);
iSide = 0;// TODO both sides!
if (auto_clear_freq_available) {
DEBUG_PRINT("AUTOCLRFREQ samples sending %d samples for antenna %d...\n", rx_auto_clear_freq[iSide].size(),antennaVector[iSide]);
sock_send_int32(driverconn, (int32_t) antennaVector[iSide]);
sock_send_uint32(driverconn, (uint32_t) rx_auto_clear_freq[iSide].size());
// send samples
send(driverconn, &rx_auto_clear_freq[iSide][0], sizeof(std::complex<short int>) * rx_auto_clear_freq[iSide].size() , 0);
}
else {
sock_send_int32(driverconn, (int32_t) -1);
}
sock_send_uint8(driverconn, AUTOCLRFREQ);
break;
}
case CLRFREQ: {
DEBUG_PRINT("entering CLRFREQ command\n");
uint32_t num_clrfreq_samples = sock_get_uint32(driverconn);
uint32_t clrfreq_time_full = sock_get_uint32(driverconn);
double clrfreq_time_frac = sock_get_float64(driverconn);
double clrfreq_cfreq = sock_get_float64(driverconn);
double clrfreq_rate = sock_get_float64(driverconn);
std::vector<std::vector<std::complex<int16_t>>> clrfreq_data_buffer(nSides, std::vector<std::complex<int16_t>>(num_clrfreq_samples));
uint32_t real_time;
double frac_time;
DEBUG_PRINT("CLRFREQ time: %d . %.2f \n", clrfreq_time_full, clrfreq_time_frac);
DEBUG_PRINT("CLRFREQ rate: %.2f, CLRFREQ_nsamples %d, freq: %.2f\n", clrfreq_rate, num_clrfreq_samples, clrfreq_cfreq);
uhd::time_spec_t clrfreq_start_time = uhd::time_spec_t(clrfreq_time_full, clrfreq_time_frac);
real_time = clrfreq_start_time.get_real_secs();
frac_time = clrfreq_start_time.get_frac_secs();
DEBUG_PRINT("CLRFREQ UHD clrfreq target time: %d %.2f \n", real_time, frac_time);
// TODO: only set rate if it is different!
if(rxrate != clrfreq_rate) {
usrp->set_rx_rate(clrfreq_rate);
rxrate = usrp->get_rx_rate();
clrfreq_rate = rxrate;
}
DEBUG_PRINT("CLRFREQ actual rate: %.2f\n", clrfreq_rate);
//clrfreq_cfreq = usrp->get_rx_freq();
//DEBUG_PRINT("CLRFREQ actual freq: %.2f\n", clrfreq_cfreq);
clrfreq_threads.create_thread(boost::bind(usrp_rx_worker, usrp, rx_stream, &clrfreq_data_buffer, num_clrfreq_samples, clrfreq_start_time, &clrfreq_rx_worker_status));
clrfreq_threads.join_all();
if(clrfreq_rx_worker_status){
fprintf(stderr, "Error in clrfreq_rx_worker, resetting rx_stream: %d.\n", clrfreq_rx_worker_status);
rx_stream_reset_count++;
fprintf(stderr, "CLRFREQ: recreating rx_stream %dth time! (buffer overflow will occur for 126th time)\n", rx_stream_reset_count);
rx_stream.reset();
rx_stream = usrp->get_rx_stream(stream_args);
}
if (rx_stream_reset_count >= MAX_STREAM_RESETS) {
fprintf(stderr, "CLRFREQ: shutting down usrp_driver to avoid streamer reset overflow (after %dth reset)\n", rx_stream_reset_count);
// finish clrfreq command, then clean up and exit to avoid buffer overflow
exit_driver = 1;
}
DEBUG_PRINT("CLRFREQ received samples, relaying %d samples back...\n", num_clrfreq_samples);
sock_send_int32(driverconn, (int32_t) antennaVector[0]); // TODO both sides?
sock_send_float64(driverconn, clrfreq_rate);
// send back samples
send(driverconn, &clrfreq_data_buffer[0][0], sizeof(std::complex<short int>) * num_clrfreq_samples, 0);
//for(uint32_t i = 0; i < num_clrfreq_samples; i++) {
//DEBUG_PRINT("sending %d - %d\n", i, clrfreq_data_buffer[0][i]);
// sock_send_cshort(driverconn, clrfreq_data_buffer[0][i]);
// }
DEBUG_PRINT("CLRFREQ samples sent for antenna %d...\n", antennaVector[0]);
// restore usrp rates
usrp->set_rx_rate(rxrate);
usrp->set_rx_freq(rxfreq);
sock_send_uint8(driverconn, CLRFREQ);
start_time = usrp->get_time_now();
real_time = start_time.get_real_secs();
frac_time = start_time.get_frac_secs();
DEBUG_PRINT("CLRFREQ finished at UHD time: %d %.2f \n", real_time, frac_time);
break;
}
case EXIT: {
DEBUG_PRINT("entering EXIT command\n");
exit_driver = 1;
break;
}
default: {
printf("USRP_DRIVER unrecognized command: %d, %c, exiting..\n", command, command);
sleep(10);
exit(1);
break;
}
}
if (not check_clock_lock(usrp)) {
fprintf(stderr, "Error: Lost clock for USRP: %s\n ", as_host->sval[0]);
exit_driver = 1;
}
// clean exit
if (exit_driver) {
DEBUG_PRINT("Shutting down driver\n");
close(driverconn);
for(iSide = 0; iSide < nSides; iSide++) {
for(iSwing = 0; iSwing < nSwings; iSwing++) {
// fill SHM with zeros
memset(shm_rx_vec[iSide][iSwing], 0, rxshm_size);
memset(shm_tx_vec[iSide][iSwing], 0, txshm_size);
munmap(shm_rx_vec[iSide][iSwing], rxshm_size);
munmap(shm_tx_vec[iSide][iSwing], txshm_size);
sem_close(&sem_rx_vec[iSwing]);
sem_close(&sem_tx_vec[iSwing]);
}
}
// TODO: close usrp streams?
// sock_send_uint8(driverconn, EXIT);
exit(1);
}
}
}
return 0;
}
/*!
* \brief Routine for updating command to the powercubes.
*
* Depending on the operation mode (position/velocity) either position or velocity goals are sent to the hardware.
*/
void updatePCubeCommands()
{
if (isInitialized_ == true)
{
std::string operationMode;
n_.getParam("OperationMode", operationMode);
if (operationMode == "position")
{
ROS_DEBUG("moving powercubes in position mode");
if (PCube_->statusMoving() == false)
{
//feedback_.isMoving = false;
ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
if (traj_point_nr_ < traj_.points.size())
{
// if powercubes are not moving and not reached last point of trajectory, then send new target point
ROS_DEBUG("...moving to trajectory point[%d]",traj_point_nr_);
traj_point_ = traj_.points[traj_point_nr_];
lock_semaphore(can_sem);
printf("cob_powercube_chain: Moving to position: ");
for (int i = 0; i < traj_point_.positions.size(); i++)
{
printf("%f ",traj_point_.positions[i]);
}
printf("\n");
PCube_->MoveJointSpaceSync(traj_point_.positions);
unlock_semaphore(can_sem);
traj_point_nr_++;
//feedback_.isMoving = true;
//feedback_.pointNr = traj_point_nr;
//as_.publishFeedback(feedback_);
}
else
{
ROS_DEBUG("...reached end of trajectory");
finished_ = true;
}
}
else
{
ROS_DEBUG("...powercubes still moving to point[%d]",traj_point_nr_);
}
}
else if (operationMode == "velocity")
{
ROS_DEBUG("moving powercubes in velocity mode");
if(newvel_)
{
ROS_INFO("MoveVel Call");
lock_semaphore(can_sem);
PCube_->MoveVel(cmd_vel_);
newvel_ = false;
unlock_semaphore(can_sem);
}
}
else
{
ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
}
}
else
{
ROS_DEBUG("powercubes not initialized");
}
}
/*!
* \brief Routine for publishing joint_states.
*
* Gets current positions and velocities from the hardware and publishes them as joint_states.
*/
void publishJointState()
{
updater_.update();
if (isInitialized_ == true)
{
// publish joint state message
int DOF = ModIds_param_.size();
std::vector<double> ActualPos;
std::vector<double> ActualVel;
ActualPos.resize(DOF);
ActualVel.resize(DOF);
lock_semaphore(can_sem);
int success = PCube_->getConfig(ActualPos);
if (!success) return;
PCube_->getJointVelocities(ActualVel);
unlock_semaphore(can_sem);
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.name = JointNames_;
for (int i = 0; i<DOF; i++ )
{
msg.position[i] = ActualPos[i];
msg.velocity[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
topicPub_JointState_.publish(msg);
// publish controller state message
pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
controllermsg.header.stamp = ros::Time::now();
controllermsg.joint_names.resize(DOF);
controllermsg.desired.positions.resize(DOF);
controllermsg.desired.velocities.resize(DOF);
controllermsg.actual.positions.resize(DOF);
controllermsg.actual.velocities.resize(DOF);
controllermsg.error.positions.resize(DOF);
controllermsg.error.velocities.resize(DOF);
controllermsg.joint_names = JointNames_;
for (int i = 0; i<DOF; i++ )
{
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
if (traj_point_.positions.size() != 0)
controllermsg.desired.positions[i] = traj_point_.positions[i];
else
controllermsg.desired.positions[i] = 0.0;
controllermsg.desired.velocities[i] = 0.0;
controllermsg.actual.positions[i] = ActualPos[i];
controllermsg.actual.velocities[i] = ActualVel[i];
controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
}
topicPub_ControllerState_.publish(controllermsg);
}
}
size_t sys_get_msg_size( port_id hPort )
{
MsgPort_s *psPort = NULL;
int nError = 0;
lock_mutex( g_hPortListSema, true );
psPort = get_port_from_handle( hPort );
if ( NULL == psPort )
{
printk( "sys_get_msg_size() attempt to receive message from invalid port %d\n", hPort );
nError = -EINVAL;
goto error;
}
again:
if ( psPort->mp_psFirstMsg == NULL )
{
sem_id hSyncSema;
// We take a backup here since the port might get deleted
// when we release the g_hPortListSema semaphore.
// If the port is deleted so is the semaphore and lock_semaphore()
// will return an error. It's then very important not to
// touch the port again until we has revalidated the port-handle.
hSyncSema = psPort->mp_hSyncSema;
unlock_mutex( g_hPortListSema );
nError = lock_semaphore( hSyncSema, 0, INFINITE_TIMEOUT );
lock_mutex( g_hPortListSema, true );
if ( nError < 0 )
goto error;
// We must revalidate the handle after unlocking g_hPortListSema
psPort = get_port_from_handle( hPort );
if ( NULL == psPort )
{
nError = -EINVAL;
printk( "Message port %d deleted while we were wating for a message\n", hPort );
goto error;
}
}
else
if( lock_semaphore( psPort->mp_hSyncSema, SEM_NOSIG, 0LL ) < 0 )
printk( "sys_get_msg_size() failed to lock mp_hSyncSema even though there were pending messages\n" );
if ( psPort->mp_psFirstMsg == NULL )
{
printk( "sys_get_msg_size() locked mp_hSyncSema even though the message list was empty on port %d\n", hPort );
goto again;
}
nError = psPort->mp_psFirstMsg->mn_nSize;
unlock_semaphore( psPort->mp_hSyncSema );
error:
unlock_mutex( g_hPortListSema );
return nError;
}
