mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-11-28 04:45:16 +00:00
Experimental 8 channels antenna array source (GNURadio and GRC compliant) using a Gigabit Ethernet bus
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@479 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas
441
drivers/gr-dbfcttc/lib/raw_array_impl.cc
Normal file
441
drivers/gr-dbfcttc/lib/raw_array_impl.cc
Normal file
@@ -0,0 +1,441 @@
|
||||
/* -*- c++ -*- */
|
||||
/*
|
||||
* Copyright 2014 <+YOU OR YOUR COMPANY+>.
|
||||
*
|
||||
* This 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, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* This software 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 this software; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, Inc., 51 Franklin Street,
|
||||
* Boston, MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#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("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!
|
||||
}
|
||||
fprintf(stdout,"Destructing DBFCTTC\n");
|
||||
fflush(stdout);
|
||||
if(descr != NULL)
|
||||
{
|
||||
pcap_breakloop(descr);
|
||||
d_pcap_thread->join();
|
||||
pcap_close(descr);
|
||||
}
|
||||
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];
|
||||
fifo_buff_ch[ch][fifo_write_ptr] = std::complex<float>(real, imag);
|
||||
//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("Error ethernet 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
Block a user