mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 20:20:35 +00:00
851f98f879
Fix compilation issues with mac osx git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@483 64b25241-fba3-4117-9849-534c7e92360d
515 lines
16 KiB
C++
515 lines
16 KiB
C++
/*!
|
|
* \file raw_array_impl.cc
|
|
* \brief GNU Radio source block to acces to experimental GNSS Array platform.
|
|
* \author Javier Arribas, 2014. jarribas(at)cttc.es
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*
|
|
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
|
*
|
|
* GNSS-SDR is a software defined Global Navigation
|
|
* Satellite Systems receiver
|
|
*
|
|
* This file is part of GNSS-SDR.
|
|
*
|
|
* GNSS-SDR 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 of the License, or
|
|
* at your option) any later version.
|
|
*
|
|
* GNSS-SDR 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 GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include <gnuradio/io_signature.h>
|
|
#include "raw_array_impl.h"
|
|
#include <arpa/inet.h>
|
|
#include <net/if.h>
|
|
#include <net/ethernet.h>
|
|
#include <netinet/if_ether.h>
|
|
#include <sys/ioctl.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
|
|
#define FIFO_SIZE 1000000
|
|
#define DBFCTTC_NUM_CHANNELS 8
|
|
|
|
namespace gr {
|
|
namespace dbfcttc {
|
|
|
|
raw_array::sptr
|
|
raw_array::make(const char *src_device,short number_of_channels, int snapshots_per_frame, int inter_frame_delay, int sampling_freq)
|
|
{
|
|
return gnuradio::get_initial_sptr
|
|
(new raw_array_impl(src_device, number_of_channels, snapshots_per_frame, inter_frame_delay, sampling_freq));
|
|
}
|
|
|
|
/*
|
|
* The private constructor
|
|
*/
|
|
raw_array_impl::raw_array_impl(const char *src_device,short number_of_channels, int snapshots_per_frame, int inter_frame_delay, int sampling_freq)
|
|
: gr::sync_block("raw_array",
|
|
gr::io_signature::make(0, 0, 0),
|
|
gr::io_signature::make(8, 8, sizeof(gr_complex)))
|
|
{
|
|
|
|
// constructor code here
|
|
fprintf(stdout,"DBFCTTC Start\n");
|
|
|
|
d_src_device=src_device;
|
|
d_number_of_channels=number_of_channels;
|
|
d_snapshots_per_frame=snapshots_per_frame;
|
|
d_inter_frame_delay=inter_frame_delay;
|
|
d_sampling_freq=sampling_freq;
|
|
|
|
d_flag_start_frame=true;
|
|
d_fifo_full=false;
|
|
d_last_frame_counter=0;
|
|
d_num_rx_errors=0;
|
|
flag_16_bits_sample=true;
|
|
|
|
//allocate signal samples buffer
|
|
//TODO: Check memory pointers
|
|
fifo_buff_ch=new gr_complex*[DBFCTTC_NUM_CHANNELS];
|
|
for (int i=0;i<DBFCTTC_NUM_CHANNELS;i++)
|
|
{
|
|
fifo_buff_ch[i]=new gr_complex[FIFO_SIZE];
|
|
}
|
|
|
|
fifo_read_ptr=0;
|
|
fifo_write_ptr=0;
|
|
fifo_items=0;
|
|
|
|
//open the ethernet device
|
|
if (open()==true)
|
|
{
|
|
// start pcap capture thread
|
|
d_pcap_thread=new boost::thread(boost::bind(&raw_array_impl::my_pcap_loop_thread,this,descr));
|
|
|
|
|
|
// send array configuration frame
|
|
if (configure_array()==true)
|
|
{
|
|
if (start_array()==true)
|
|
{
|
|
printf("Array ready!\n");
|
|
}else{
|
|
exit(1); //ethernet error!
|
|
}
|
|
}else{
|
|
exit(1); //ethernet error!
|
|
}
|
|
}else{
|
|
exit(1); //ethernet error!
|
|
}
|
|
}
|
|
|
|
bool raw_array_impl::open()
|
|
{
|
|
char errbuf[PCAP_ERRBUF_SIZE];
|
|
boost::mutex::scoped_lock lock(d_mutex); // hold mutex for duration of this function
|
|
char *dev;
|
|
/* open device for reading */
|
|
descr = pcap_open_live(d_src_device,1500,1,1000,errbuf);
|
|
if(descr == NULL)
|
|
{
|
|
printf("Error openning ethernet device: %s\n",d_src_device);
|
|
printf("Fatal Error in pcap_open_live(): %s\n",errbuf);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool raw_array_impl::configure_array()
|
|
{
|
|
//prepare the config data for the ethernet frame
|
|
// note: command=1 -> beamforming config
|
|
// command=2 -> start
|
|
// command=3 -> stop
|
|
// command=4 -> raw array config
|
|
|
|
char data[20];
|
|
|
|
for(int i=0;i<20;i++)
|
|
{
|
|
data[i]=0x00;
|
|
}
|
|
|
|
data[0]=4; //command to activate RAW array
|
|
data[1]=d_number_of_channels;
|
|
data[2]=d_snapshots_per_frame>>8;
|
|
data[3]=d_snapshots_per_frame & 255;
|
|
|
|
printf("\n Total bytes in snapshots payload = %i\n",d_snapshots_per_frame*d_number_of_channels*2);
|
|
printf("\n Estimated eth RAW frame size [bytes] %i\n",12+2+3+d_snapshots_per_frame*d_number_of_channels*2+1);
|
|
|
|
data[4]=d_inter_frame_delay>>8;
|
|
data[5]=d_inter_frame_delay & 255;
|
|
|
|
data[6]=0xB;
|
|
data[7]=0xF;
|
|
|
|
//send the frame
|
|
struct ether_header myheader;
|
|
myheader.ether_type=0xbfcd; //this is the ethenet layer II protocol ID for the CTTC array hardware
|
|
memset(myheader.ether_dhost,0xff,sizeof(myheader.ether_dhost));
|
|
memset(myheader.ether_shost,0x11,sizeof(myheader.ether_shost));
|
|
|
|
unsigned char frame[sizeof(struct ether_header)+sizeof(data)];
|
|
memcpy(frame,&myheader,sizeof(struct ether_header));
|
|
memcpy(frame+sizeof(struct ether_header),&data,sizeof(data));
|
|
|
|
if (pcap_inject(descr,frame,sizeof(frame))==-1) {
|
|
printf("Error sending configuration packet\n");
|
|
pcap_perror(descr,0);
|
|
return false;
|
|
}else{
|
|
printf("Sent configuration packet OK\n");
|
|
return true;
|
|
}
|
|
}
|
|
|
|
bool raw_array_impl::start_array()
|
|
{
|
|
char data[20];
|
|
|
|
for(int i=0;i<20;i++)
|
|
{
|
|
data[i]=0x00;
|
|
}
|
|
|
|
data[0]=2; //command to start the array operation (configured previously)
|
|
|
|
//send the frame
|
|
struct ether_header myheader;
|
|
myheader.ether_type=0xbfcd; //this is the ethenet layer II protocol ID for the CTTC array hardware
|
|
memset(myheader.ether_dhost,0xff,sizeof(myheader.ether_dhost));
|
|
memset(myheader.ether_shost,0x11,sizeof(myheader.ether_shost));
|
|
|
|
unsigned char frame[sizeof(struct ether_header)+sizeof(data)];
|
|
memcpy(frame,&myheader,sizeof(struct ether_header));
|
|
memcpy(frame+sizeof(struct ether_header),&data,sizeof(data));
|
|
|
|
if (pcap_inject(descr,frame,sizeof(frame))==-1) {
|
|
printf("Error sending start packet\n");
|
|
pcap_perror(descr,0);
|
|
return false;
|
|
}else{
|
|
printf("Sent start packet OK\n");
|
|
return true;
|
|
}
|
|
}
|
|
|
|
bool raw_array_impl::stop_array()
|
|
{
|
|
char data[20];
|
|
|
|
for(int i=0;i<20;i++)
|
|
{
|
|
data[i]=0x00;
|
|
}
|
|
|
|
data[0]=3; //command to stop the array operation (configured previously)
|
|
|
|
//send the frame
|
|
struct ether_header myheader;
|
|
myheader.ether_type=0xbfcd; //this is the ethenet layer II protocol ID for the CTTC array hardware
|
|
memset(myheader.ether_dhost,0xff,sizeof(myheader.ether_dhost));
|
|
memset(myheader.ether_shost,0x11,sizeof(myheader.ether_shost));
|
|
|
|
unsigned char frame[sizeof(struct ether_header)+sizeof(data)];
|
|
memcpy(frame,&myheader,sizeof(struct ether_header));
|
|
memcpy(frame+sizeof(struct ether_header),&data,sizeof(data));
|
|
|
|
if (pcap_inject(descr,frame,sizeof(frame))==-1) {
|
|
printf("Error sending stop packet\n");
|
|
pcap_perror(descr,0);
|
|
return false;
|
|
}else{
|
|
printf("Sent stop packet OK\n");
|
|
return true;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Our virtual destructor.
|
|
*/
|
|
raw_array_impl::~raw_array_impl()
|
|
{
|
|
// destructor code here
|
|
if (stop_array()==true)
|
|
{
|
|
printf("Array stopped!\n");
|
|
}else{
|
|
exit(1); //ethernet error!
|
|
}
|
|
if(descr != NULL)
|
|
{
|
|
pcap_breakloop(descr);
|
|
d_pcap_thread->join();
|
|
pcap_close(descr);
|
|
}
|
|
|
|
for (int i=0;i<DBFCTTC_NUM_CHANNELS;i++)
|
|
{
|
|
delete[] fifo_buff_ch[i];
|
|
}
|
|
delete fifo_buff_ch;
|
|
fprintf(stdout,"All stopped OK\n");
|
|
|
|
}
|
|
|
|
void raw_array_impl::static_pcap_callback(u_char *args, const struct pcap_pkthdr* pkthdr,
|
|
const u_char* packet)
|
|
{
|
|
//
|
|
raw_array_impl *bridge=(raw_array_impl*) args;
|
|
bridge->pcap_callback(args, pkthdr, packet);
|
|
}
|
|
|
|
void raw_array_impl::pcap_callback(u_char *args, const struct pcap_pkthdr* pkthdr,
|
|
const u_char* packet)
|
|
{
|
|
boost::mutex::scoped_lock lock(d_mutex); // hold mutex for duration of this function
|
|
int numframebyte;
|
|
short int real,imag;
|
|
// eth frame parameters
|
|
int number_of_channels;
|
|
unsigned short int snapshots_per_frame;
|
|
|
|
// **** CTTC DBF PACKET DECODER ****
|
|
if ((packet[12]==0xCD) & (packet[13]==0xBF))
|
|
{
|
|
//printf(".");
|
|
// control parameters
|
|
number_of_channels=(int)packet[14];
|
|
//std::cout<<"number_of_channels="<<number_of_channels<<std::endl;
|
|
snapshots_per_frame=packet[15] << 8 | packet[16];
|
|
//std::cout<<"snapshots_per_frame="<<snapshots_per_frame<<std::endl;
|
|
//frame counter check for overflows!
|
|
numframebyte=(unsigned char)packet[16+snapshots_per_frame*2*number_of_channels+1];
|
|
//std::cout<<"numframebyte="<<numframebyte<<std::endl;
|
|
//Overflow detector and mitigator
|
|
if (d_flag_start_frame == true)
|
|
{
|
|
d_last_frame_counter=numframebyte;
|
|
d_flag_start_frame=false;
|
|
}else{
|
|
|
|
if ((d_last_frame_counter-numframebyte)>1)
|
|
{
|
|
int missing_frames=abs(d_last_frame_counter-numframebyte);
|
|
if (missing_frames!=255 )
|
|
{
|
|
//fake samples generation to help tracking loops
|
|
std::complex<float> last_sample[DBFCTTC_NUM_CHANNELS];
|
|
if (fifo_write_ptr == 0)
|
|
{
|
|
for (int ch=0;ch<number_of_channels;ch++)
|
|
{
|
|
last_sample[ch]=fifo_buff_ch[ch][FIFO_SIZE];
|
|
}
|
|
}else{
|
|
for (int ch=0;ch<number_of_channels;ch++)
|
|
{
|
|
last_sample[ch]=fifo_buff_ch[ch][fifo_write_ptr-1];
|
|
}
|
|
}
|
|
for(int i=0;i<(snapshots_per_frame*missing_frames);i++)
|
|
{
|
|
if (fifo_items <= FIFO_SIZE) {
|
|
for (int ch=0;ch<number_of_channels;ch++)
|
|
{
|
|
fifo_buff_ch[ch][fifo_write_ptr] = last_sample[ch];
|
|
}
|
|
fifo_write_ptr++;
|
|
if (fifo_write_ptr == FIFO_SIZE) fifo_write_ptr = 0;
|
|
fifo_items++;
|
|
if (d_fifo_full==true)
|
|
{
|
|
d_fifo_full=false;
|
|
}
|
|
}else{
|
|
if (d_fifo_full==false)
|
|
{
|
|
printf("FIFO full\n");
|
|
fflush(stdout);
|
|
d_fifo_full=true;
|
|
}
|
|
}
|
|
}
|
|
d_num_rx_errors=d_num_rx_errors + 1;
|
|
printf("RAW Array driver overflow RX %d\n",numframebyte);
|
|
}
|
|
}
|
|
}
|
|
d_last_frame_counter=numframebyte;
|
|
|
|
};
|
|
|
|
|
|
//snapshots reading..
|
|
for(int i=0;i<snapshots_per_frame;i++)
|
|
{
|
|
if (fifo_items <= FIFO_SIZE) {
|
|
for (int ch=0;ch<number_of_channels;ch++)
|
|
{
|
|
if (flag_16_bits_sample==true)
|
|
{
|
|
//(2i+2q)*8channels =32 bytes
|
|
real=(signed short int)(packet[17 + ch*4 + i * 32] << 8 | packet[17 + ch*4 + 1 + i * 32]);
|
|
imag=(signed short int)(packet[17 + ch*4 + 2 + i * 32] << 8 | packet[17 + ch*4 + 3 + i * 32]);
|
|
}else{
|
|
//(1i+1q)*8channels =16 bytes
|
|
real = (signed char)packet[17 + ch*2 + i * 16];
|
|
imag = (signed char)packet[17 + ch*2 + 1 + i * 16];
|
|
}
|
|
//todo: invert IQ in FPGA
|
|
//fifo_buff_ch[ch][fifo_write_ptr] = std::complex<float>(real, imag);
|
|
fifo_buff_ch[ch][fifo_write_ptr] = std::complex<float>(imag, real); //inverted due to inversion in front-end
|
|
//std::cout<<"["<<ch<<"]["<<fifo_write_ptr<<"]"<<fifo_buff_ch[ch][fifo_write_ptr]<<std::endl;
|
|
}
|
|
fifo_write_ptr++;
|
|
if (fifo_write_ptr == FIFO_SIZE) fifo_write_ptr = 0;
|
|
fifo_items++;
|
|
if (d_fifo_full==true)
|
|
{
|
|
d_fifo_full=false;
|
|
}
|
|
}else{
|
|
if (d_fifo_full==false)
|
|
{
|
|
printf("FIFO full\n");
|
|
fflush(stdout);
|
|
d_fifo_full=true;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
//test RX
|
|
|
|
// **** CTTC DBF PACKET DECODER ***
|
|
//else{
|
|
//std::cout<<"RX PKT ID="<<(int)packet[12]<<","<<(int)packet[13]<<std::endl;
|
|
//}
|
|
|
|
// // *** END CTTC DBF PACKET DECODER ***
|
|
}
|
|
|
|
|
|
void raw_array_impl::my_pcap_loop_thread(pcap_t *pcap_handle)
|
|
|
|
{
|
|
|
|
pcap_loop(pcap_handle, -1, raw_array_impl::static_pcap_callback, (u_char *)this);
|
|
|
|
}
|
|
|
|
|
|
|
|
int
|
|
raw_array_impl::work(int noutput_items,
|
|
gr_vector_const_void_star &input_items,
|
|
gr_vector_void_star &output_items)
|
|
{
|
|
|
|
//gr_complex *out = (gr_complex *) output_items[0];
|
|
// channel output buffers
|
|
// gr_complex *ch1 = (gr_complex *) output_items[0];
|
|
// gr_complex *ch2 = (gr_complex *) output_items[1];
|
|
// gr_complex *ch3 = (gr_complex *) output_items[2];
|
|
// gr_complex *ch4 = (gr_complex *) output_items[3];
|
|
// gr_complex *ch5 = (gr_complex *) output_items[4];
|
|
// gr_complex *ch6 = (gr_complex *) output_items[5];
|
|
// gr_complex *ch7 = (gr_complex *) output_items[6];
|
|
// gr_complex *ch8 = (gr_complex *) output_items[7];
|
|
|
|
// send samples to next GNU Radio block
|
|
|
|
boost::mutex::scoped_lock lock(d_mutex); // hold mutex for duration of this function
|
|
int num_samples_readed;
|
|
|
|
if (noutput_items<fifo_items)
|
|
{
|
|
num_samples_readed=noutput_items;//read all
|
|
}else{
|
|
num_samples_readed=fifo_items;//read what we have
|
|
}
|
|
|
|
|
|
int aligned_read_items=FIFO_SIZE-fifo_read_ptr;
|
|
|
|
if (aligned_read_items>=num_samples_readed)
|
|
{
|
|
//read all in a single memcpy
|
|
for (int ch=0;ch<DBFCTTC_NUM_CHANNELS;ch++)
|
|
{
|
|
//((gr_complex*)output_items[ch])[i]=fifo_buff_ch[ch][fifo_read_ptr];
|
|
memcpy(&((gr_complex*)output_items[ch])[0],&fifo_buff_ch[ch][fifo_read_ptr],sizeof(std::complex<float> )*num_samples_readed);
|
|
}
|
|
fifo_read_ptr=fifo_read_ptr+num_samples_readed; //increase the fifo pointer
|
|
if (fifo_read_ptr==FIFO_SIZE) fifo_read_ptr=0;
|
|
}else{
|
|
//two step wrap read
|
|
for (int ch=0;ch<DBFCTTC_NUM_CHANNELS;ch++)
|
|
{
|
|
//((gr_complex*)output_items[ch])[i]=fifo_buff_ch[ch][fifo_read_ptr];
|
|
memcpy(&((gr_complex*)output_items[ch])[0],&fifo_buff_ch[ch][fifo_read_ptr],sizeof(std::complex<float> )*aligned_read_items);
|
|
}
|
|
fifo_read_ptr=fifo_read_ptr+aligned_read_items; //increase the fifo pointer
|
|
|
|
if (fifo_read_ptr==FIFO_SIZE) fifo_read_ptr=0;
|
|
|
|
for (int ch=0;ch<DBFCTTC_NUM_CHANNELS;ch++)
|
|
{
|
|
//((gr_complex*)output_items[ch])[i]=fifo_buff_ch[ch][fifo_read_ptr];
|
|
memcpy(&((gr_complex*)output_items[ch])[aligned_read_items],&fifo_buff_ch[ch][fifo_read_ptr],sizeof(std::complex<float>)*(num_samples_readed-aligned_read_items));
|
|
}
|
|
fifo_read_ptr=fifo_read_ptr+(num_samples_readed-aligned_read_items); //increase the fifo pointer
|
|
}
|
|
|
|
fifo_items=fifo_items-num_samples_readed;
|
|
|
|
|
|
// int num_samples_readed=0;
|
|
// for(int i=0;i<noutput_items;i++)
|
|
// {
|
|
//
|
|
// if (fifo_items > 0) {
|
|
// //TODO: optimize-me with memcpy!!
|
|
// for (int ch=0;ch<DBFCTTC_NUM_CHANNELS;ch++)
|
|
// {
|
|
// ((gr_complex*)output_items[ch])[i]=fifo_buff_ch[ch][fifo_read_ptr];
|
|
// }
|
|
// fifo_read_ptr++;
|
|
// if (fifo_read_ptr==FIFO_SIZE) fifo_read_ptr=0;
|
|
// fifo_items--;
|
|
// num_samples_readed++;
|
|
// } else {
|
|
// break;
|
|
// }
|
|
//
|
|
// }
|
|
|
|
// Tell runtime system how many output items we produced.
|
|
return num_samples_readed;
|
|
}
|
|
|
|
} /* namespace dbfcttc */
|
|
} /* namespace gr */
|
|
|