mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity
gnss-sdr/drivers/gr-dbfcttc/lib/raw_array_impl.cc

459 lines
15 KiB
C++
Raw Blame History

/*!
* \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;
//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;
//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++)
{
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++;
}else{
if (d_fifo_full==false)
{
printf("FIFO full\n");
fflush(stdout);
d_fifo_full=true;
}
}
}
//frame counter check for overflows!
numframebyte=(unsigned char)packet[16+snapshots_per_frame*2*number_of_channels+1];
//std::cout<<"numframebyte="<<numframebyte<<std::endl;
//test RX
// **** CTTC DBF PACKET DECODER ***
if (d_flag_start_frame == true)
{
d_last_frame_counter=numframebyte;
d_flag_start_frame=false;
}else{
if ((d_last_frame_counter-numframebyte)>1)
{
if (abs(d_last_frame_counter-numframebyte)!=255 )
{
d_num_rx_errors=d_num_rx_errors + 1;
printf("RAW Array driver overflow RX %d\n",numframebyte);
}
}
}
d_last_frame_counter=numframebyte;
};//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 */
Reference in New Issue View Git Blame Copy Permalink