FGFS-4.1/gr-air-modes
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'speed'

Browse Source 
Conflicts:
	src/python/uhd_modes.py
This commit is contained in:
Nick Foster 2011-05-16 16:08:41 -07:00
commit 462b7fbed3
13 changed files with 574 additions and 295 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
src/lib/air_modes_framer.cc
View File

@ -28,6 +28,9 @@
#include <gr_io_signature.h>
#include <air_modes_types.h>
#include <modes_energy.h>
#include <gr_tag_info.h>
#include <iostream>
#include <string.h>
air_modes_framer_sptr air_make_modes_framer(int channel_rate)
{
@ -36,16 +39,21 @@ air_modes_framer_sptr air_make_modes_framer(int channel_rate)
air_modes_framer::air_modes_framer(int channel_rate) :
gr_sync_block ("modes_framer",
gr_make_io_signature2 (2, 2, sizeof(float), sizeof(unsigned char)), //stream 0 is received data, stream 1 is binary preamble detector output
gr_make_io_signature (1, 1, sizeof(unsigned char))) //output is [0, 1, 2]: [no frame, short frame, long frame]
gr_make_io_signature (1, 1, sizeof(float)), //stream 0 is received data
gr_make_io_signature (1, 1, sizeof(float))) //raw samples passed back out
{
//initialize private data here
d_chip_rate = 2000000; //2Mchips per second
d_samples_per_chip = channel_rate / d_chip_rate; //must be integer number of samples per chip to work
d_samples_per_symbol = d_samples_per_chip * 2;
d_check_width = 120 * d_samples_per_symbol; //gotta be able to look at two long frame lengths at a time in the event that FRUIT occurs near the end of the first frame
set_output_multiple(1+d_check_width*2);
d_check_width = 120 * d_samples_per_symbol; //gotta be able to look at two long frame lengths at a time
//in the event that FRUIT occurs near the end of the first frame
//set_history(d_check_width*2);
std::stringstream str;
str << name() << unique_id();
d_me = pmt::pmt_string_to_symbol(str.str());
d_key = pmt::pmt_string_to_symbol("frame_info");
}
int air_modes_framer::work(int noutput_items,
@ -54,60 +62,62 @@ int air_modes_framer::work(int noutput_items,
{
//do things!
const float *inraw = (const float *) input_items[0];
const unsigned char *inattrib = (const unsigned char *) input_items[1];
//float *outraw = (float *) output_items[0];
unsigned char *outattrib = (unsigned char *) output_items[0];
int size = noutput_items - d_check_width; //need to be able to look ahead a full frame
float reference_level = 0;
float *outraw = (float *) output_items[0];
//unsigned char *outattrib = (unsigned char *) output_items[0];
int size = noutput_items - d_check_width*2;
float reference_level;
framer_packet_type packet_attrib;
for(int i = 0; i < size; i++) {
packet_attrib = No_Packet;
if(!inattrib[i]) {
outattrib[i] = packet_attrib;
continue; //if there's no preamble marker, forget it, move on
}
std::vector<pmt::pmt_t> tags;
uint64_t abs_sample_cnt = nitems_read(0);
get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + size, pmt::pmt_string_to_symbol("preamble_found"));
std::vector<pmt::pmt_t>::iterator tag_iter;
memcpy(outraw, inraw, size * sizeof(float));
for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) {
uint64_t i = gr_tags::get_nitems(*tag_iter) - abs_sample_cnt;
//first, assume we have a long packet
packet_attrib = Long_Packet;
//let's use the preamble marker to get a reference level for the packet
reference_level = (bit_energy(&inraw[i], d_samples_per_chip)
+ bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4;
+ bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4;
//armed with our reference level, let's look for marks within 3dB of the reference level in bits 57-62 (65-70, see above)
//if bits 57-62 have marks in either chip, we've got a long packet
//otherwise we have a short packet
//NOTE: you can change the default here to be short packet, and then check for a long packet. don't know which way is better.
for(int j = (65 * d_samples_per_symbol); j < (70 * d_samples_per_symbol); j += d_samples_per_symbol) {
float t_max = (bit_energy(&inraw[i+j], d_samples_per_chip) > bit_energy(&inraw[i+j+d_samples_per_chip], d_samples_per_chip)) ? bit_energy(&inraw[i+j], d_samples_per_chip) : bit_energy(&inraw[i+j+d_samples_per_chip], d_samples_per_chip);
if(t_max < (reference_level / 2)) packet_attrib = Short_Packet;
float t_max = std::max(bit_energy(&inraw[i+j], d_samples_per_chip),
bit_energy(&inraw[i+j+d_samples_per_chip], d_samples_per_chip)
);
if(t_max < (reference_level / 2.0)) packet_attrib = Short_Packet;
}
//BUT: we must also loop through the entire packet to make sure it is clear of additional preamble markers! if it has another preamble marker, it's been FRUITed, and we must only
//BUT: we must also loop through the entire packet to make sure it is clear of additional preamble markers!
//if it has another preamble marker, it's been FRUITed, and we must only
//mark the new packet (i.e., just continue).
int lookahead;
if(packet_attrib == Long_Packet) lookahead = 112;
else lookahead = 56;
for(int j = i+1; j < i+(lookahead * d_samples_per_symbol); j++) {
if(inattrib[j]) packet_attrib = Fruited_Packet; //FRUITed by mode S! in this case, we drop this first packet
//if(inraw[j] > (reference_level * 2)) packet_attrib = Fruited_Packet; //catches strong Mode A/C fruit inside the packet
//but good error correction should cope with that
}
outattrib[i] = packet_attrib;
if(packet_attrib == Long_Packet) lookahead = 112 * d_samples_per_symbol;
else lookahead = 56 * d_samples_per_symbol;
//ok we have to re-do lookahead for this tagged version.
//we can do this by looking for tags that fall within that window
std::vector<pmt::pmt_t> fruit_tags;
get_tags_in_range(fruit_tags, 0, abs_sample_cnt+i+1, abs_sample_cnt+i+lookahead, pmt::pmt_string_to_symbol("preamble_found"));
if(fruit_tags.size() > 0) packet_attrib = Fruited_Packet;
//insert tag here
add_item_tag(0, //stream ID
nitems_written(0)+i, //sample
d_key, //preamble_found
pmt::pmt_from_long((long)packet_attrib),
d_me //block src id
);
}
return size;

1
src/lib/air_modes_framer.h
View File

@ -44,6 +44,7 @@ private:
int d_chip_rate;
int d_samples_per_chip;
int d_samples_per_symbol;
pmt::pmt_t d_me, d_key;
public:
int work (int noutput_items,

129
src/lib/air_modes_preamble.cc
View File

@ -27,6 +27,8 @@
#include <air_modes_preamble.h>
#include <gr_io_signature.h>
#include <modes_energy.h>
#include <string.h>
#include <iostream>
air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db)
{
@ -36,9 +38,8 @@ air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshol
air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) :
gr_sync_block ("modes_preamble",
gr_make_io_signature2 (2, 2, sizeof(float), sizeof(float)), //stream 0 is received data, stream 1 is moving average for reference
gr_make_io_signature (1, 1, sizeof(unsigned char)))
gr_make_io_signature (1, 1, sizeof(float))) //the original data. we pass it out in order to use tags.
{
//initialize private data here
d_chip_rate = 2000000; //2Mchips per second
d_samples_per_chip = channel_rate / d_chip_rate; //must be integer number of samples per chip to work
d_samples_per_symbol = d_samples_per_chip * 2;
@ -46,6 +47,11 @@ air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) :
d_threshold_db = threshold_db;
d_threshold = powf(10., threshold_db/10.); //the level that the sample must be above the moving average in order to qualify as a pulse
set_output_multiple(1+d_check_width*2);
std::stringstream str;
str << name() << unique_id();
d_me = pmt::pmt_string_to_symbol(str.str());
d_key = pmt::pmt_string_to_symbol("preamble_found");
set_history(d_check_width);
}
int air_modes_preamble::work(int noutput_items,
@ -56,12 +62,15 @@ int air_modes_preamble::work(int noutput_items,
const float *inraw = (const float *) input_items[0];
const float *inavg = (const float *) input_items[1];
//float *outraw = (float *) output_items[0];
unsigned char *outattrib = (unsigned char *) output_items[0];
float *outraw = (float *) output_items[0];
int size = noutput_items - d_check_width;
int size = noutput_items;// - d_check_width;
int pulse_offsets[4];
float bit_energies[4];
memcpy(outraw, inraw, size * sizeof(float));
uint64_t abs_out_sample_cnt = nitems_written(0);
for(int i = d_samples_per_chip; i < size; i++) {
float pulse_threshold = bit_energy(&inavg[i], d_samples_per_chip) * d_threshold;
@ -69,31 +78,19 @@ int air_modes_preamble::work(int noutput_items,
float gate_sum_now = 0, gate_sum_early = 0, gate_sum_late = 0;
if(bit_energy(&inraw[i], d_samples_per_chip) > pulse_threshold) { //if the sample is greater than the reference level by the specified amount
//while(inraw[i+1] > inraw[i]) i++;
//if(inraw[i+1] > inraw[i]) continue; //we're still coming up on the pulse peak, so let's just fall out and look at it next time around
//if(inraw[i-1] > inraw[i]) continue; //we're past the peak, so it's no longer a valid pulse
//a note on the above. this simple early/late gate system works for decim = 16, but doesn't work so great for decim = 8. the extra samples, subject to noise,
//mean the peak is not necessarily the center of the bit, and you get fooled into sampling at strange bit edges. the solution is an area integral, a real early/late gate
//system, computing the area of a bit and maximizing so you sample at the center of the bit shape for any decimation. for decim = 16 it won't really matter since you only have
//one possible bit center.
int gate_sum = early_late(&inraw[i], d_samples_per_chip); //see modes_energy.cc
if(gate_sum != 0) continue; //if either the early gate or the late gate had greater energy, keep moving.
// if(gate_sum_late > gate_sum_now) continue;
//the packets are so short we choose not to do any sort of closed-loop synchronization after this simple gating.
//if we get a good center sample, the drift should be negligible.
pulse_offsets[0] = 0;
pulse_offsets[1] = int(1.0 * d_samples_per_symbol);
pulse_offsets[2] = int(3.5 * d_samples_per_symbol);
pulse_offsets[3] = int(4.5 * d_samples_per_symbol);
//the packets are so short we choose not to do any sort of closed-loop synchronization after this simple gating. if we get a good center sample, the drift should be negligible.
pulse_offsets[0] = i;
pulse_offsets[1] = i+int(1.0 * d_samples_per_symbol);
pulse_offsets[2] = i+int(3.5 * d_samples_per_symbol);
pulse_offsets[3] = i+int(4.5 * d_samples_per_symbol);
bit_energies[0] = bit_energy(&inraw[pulse_offsets[0]], d_samples_per_chip);
bit_energies[1] = bit_energy(&inraw[pulse_offsets[1]], d_samples_per_chip);
bit_energies[2] = bit_energy(&inraw[pulse_offsets[2]], d_samples_per_chip);
bit_energies[3] = bit_energy(&inraw[pulse_offsets[3]], d_samples_per_chip);
bit_energies[0] = bit_energy(&inraw[i+pulse_offsets[0]], d_samples_per_chip);
bit_energies[1] = bit_energy(&inraw[i+pulse_offsets[1]], d_samples_per_chip);
bit_energies[2] = bit_energy(&inraw[i+pulse_offsets[2]], d_samples_per_chip);
bit_energies[3] = bit_energy(&inraw[i+pulse_offsets[3]], d_samples_per_chip);
//search for the rest of the pulses at their expected positions
if( bit_energies[1] < pulse_threshold) continue;
@ -112,58 +109,52 @@ int air_modes_preamble::work(int noutput_items,
for(int j = 5 * d_samples_per_symbol; j <= 7.5 * d_samples_per_symbol; j+=d_samples_per_chip)
if(bit_energy(&inraw[i+j], d_samples_per_chip) > space_threshold) valid_preamble = false;
//make sure all four peaks are within 2dB of each other
float minpeak = avgpeak * 0.631; //-2db
float maxpeak = avgpeak * 1.585; //2db
//make sure all four peaks are within 3dB of each other
float minpeak = avgpeak * 0.5;//-3db, was 0.631; //-2db
float maxpeak = avgpeak * 2.0;//3db, was 1.585; //2db
if(bit_energies[0] < minpeak || bit_energies[0] > maxpeak) continue;
if(bit_energies[1] < minpeak || bit_energies[1] > maxpeak) continue;
if(bit_energies[2] < minpeak || bit_energies[2] > maxpeak) continue;
if(bit_energies[3] < minpeak || bit_energies[3] > maxpeak) continue;
}
//just for kicks, after validating a preamble, you might want to use all four peaks to form a more accurate "average" center sample time, so that if noise corrupts the first leading edge
//sample, you don't mis-sample the entire packet.
//this could also be done in a separate packet, although it probably saves CPU to do it here
//for the 2 samples per chip case, you can just add up the peaks at the expected peak times, then do the same for +1, and -1.
if(valid_preamble) {
//get a more accurate chip center by finding the energy peak across all four preamble peaks
//there's some weirdness in the early part, so i ripped it out.
bool early, late;
do {
early = late = false;
//gate_sum_early= bit_energy(&inraw[i+pulse_offsets[0]-1], d_samples_per_chip)
// + bit_energy(&inraw[i+pulse_offsets[1]-1], d_samples_per_chip)
// + bit_energy(&inraw[i+pulse_offsets[2]-1], d_samples_per_chip)
// + bit_energy(&inraw[i+pulse_offsets[3]-1], d_samples_per_chip);
gate_sum_now = bit_energy(&inraw[i+pulse_offsets[0]], d_samples_per_chip)
+ bit_energy(&inraw[i+pulse_offsets[1]], d_samples_per_chip)
+ bit_energy(&inraw[i+pulse_offsets[2]], d_samples_per_chip)
+ bit_energy(&inraw[i+pulse_offsets[3]], d_samples_per_chip);
gate_sum_late = bit_energy(&inraw[i+pulse_offsets[0]+1], d_samples_per_chip)
+ bit_energy(&inraw[i+pulse_offsets[1]+1], d_samples_per_chip)
+ bit_energy(&inraw[i+pulse_offsets[2]+1], d_samples_per_chip)
+ bit_energy(&inraw[i+pulse_offsets[3]+1], d_samples_per_chip);
gate_sum_now = bit_energies[0] + bit_energies[1] + bit_energies[2] + bit_energies[3];
// gate_sum_early = bit_energy(&inraw[pulse_offsets[0]-1], d_samples_per_chip)
// + bit_energy(&inraw[pulse_offsets[1]-1], d_samples_per_chip)
// + bit_energy(&inraw[pulse_offsets[2]-1], d_samples_per_chip)
// + bit_energy(&inraw[pulse_offsets[3]-1], d_samples_per_chip);
gate_sum_late = bit_energy(&inraw[pulse_offsets[0]+1], d_samples_per_chip)
+ bit_energy(&inraw[pulse_offsets[1]+1], d_samples_per_chip)
+ bit_energy(&inraw[pulse_offsets[2]+1], d_samples_per_chip)
+ bit_energy(&inraw[pulse_offsets[3]+1], d_samples_per_chip);
/*
if(d_samples_per_chip <= 2) {
gate_sum_now = inraw[pulse_offsets[0]+0] + inraw[pulse_offsets[1]+0] + inraw[pulse_offsets[2]+0] + inraw[pulse_offsets[3]+0];
gate_sum_early = inraw[pulse_offsets[0]-1] + inraw[pulse_offsets[1]-1] + inraw[pulse_offsets[2]-1] + inraw[pulse_offsets[3]-1];
gate_sum_late = inraw[pulse_offsets[0]+1] + inraw[pulse_offsets[1]+1] + inraw[pulse_offsets[2]+1] + inraw[pulse_offsets[3]+1];
} else {
for(int j = 1-d_samples_per_chip/2; j < d_samples_per_chip/2; j++) {
gate_sum_now += inraw[j+pulse_offsets[0]+0] + inraw[j+pulse_offsets[1]+0] + inraw[j+pulse_offsets[2]+0] + inraw[j+pulse_offsets[3]+0];
gate_sum_early += inraw[j+pulse_offsets[0]-1] + inraw[j+pulse_offsets[1]-1] + inraw[j+pulse_offsets[2]-1] + inraw[j+pulse_offsets[3]-1];
gate_sum_late += inraw[j+pulse_offsets[0]+1] + inraw[j+pulse_offsets[1]+1] + inraw[j+pulse_offsets[2]+1] + inraw[j+pulse_offsets[3]+1];
}
}
*/
// if(gate_sum_early > gate_sum_now) { //i think this is redundant
// outattrib[i-1] = 1;
// }
/*else*/ if(gate_sum_late > gate_sum_now) {
outattrib[i+1] = 1;
i+=1; //so we skip the next one and don't overwrite it
}
else outattrib[i] = 1;
early = (gate_sum_early > gate_sum_now);
late = (gate_sum_late > gate_sum_now);
if(late) i++;
//else if(early) i--;
//if(early && late) early = late = false;
} while(late);
} else outattrib[i] = 0;
//finally after all this, let's post the preamble!
add_item_tag(0, //stream ID
nitems_written(0)+i, //sample
d_key, //preamble_found
pmt::PMT_T, //meaningless for us
d_me //block src id
);
}
}
return size;
}

1
src/lib/air_modes_preamble.h
View File

@ -47,6 +47,7 @@ private:
int d_samples_per_symbol;
float d_threshold_db;
float d_threshold;
pmt::pmt_t d_me, d_key;
public:
int work (int noutput_items,

334
src/lib/air_modes_slicer.cc
View File

@ -31,6 +31,7 @@
#include <iomanip>
#include <modes_parity.h>
#include <modes_energy.h>
#include <gr_tag_info.h>
extern "C"
{
@ -45,7 +46,7 @@ air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr_msg_queue_sptr
air_modes_slicer::air_modes_slicer(int channel_rate, gr_msg_queue_sptr queue) :
gr_sync_block ("modes_slicer",
gr_make_io_signature2 (2, 2, sizeof(float), sizeof(unsigned char)), //stream 0 is received data, stream 1 is binary preamble detector output
gr_make_io_signature (1, 1, sizeof(float)), //stream 0 is received data, stream 1 is binary preamble detector output
gr_make_io_signature (0, 0, 0) )
{
//initialize private data here
@ -54,195 +55,228 @@ air_modes_slicer::air_modes_slicer(int channel_rate, gr_msg_queue_sptr queue) :
d_samples_per_symbol = d_samples_per_chip * 2;
d_check_width = 120 * d_samples_per_symbol; //how far you will have to look ahead
d_queue = queue;
d_secs_per_sample = 1.0 / channel_rate;
set_output_multiple(1+d_check_width * 2); //how do you specify buffer size for sinks?
}
static bool pmtcompare(pmt::pmt_t x, pmt::pmt_t y)
{
uint64_t t_x, t_y;
t_x = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(x, 0));
t_y = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(y, 0));
return t_x < t_y;
}
int air_modes_slicer::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
//do things!
const float *inraw = (const float *) input_items[0];
const unsigned char *inattrib = (const unsigned char *) input_items[1];
int size = noutput_items - d_check_width; //since it's a sync block, i assume that it runs with ninput_items = noutput_items
int i;
std::vector<pmt::pmt_t> tags;
uint64_t abs_sample_cnt = nitems_read(0);
get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + size, pmt::pmt_string_to_symbol("frame_info"));
std::vector<pmt::pmt_t>::iterator tag_iter;
for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) {
uint64_t i = gr_tags::get_nitems(*tag_iter) - abs_sample_cnt;
modes_packet rx_packet;
framer_packet_type packet_type = framer_packet_type(pmt::pmt_to_long(gr_tags::get_value(*tag_iter)));
for(i = 0; i < size; i++) {
if(inattrib[i] == framer_packet_type(Short_Packet) || inattrib[i] == framer_packet_type(Long_Packet)) { //if there's a packet starting here....
modes_packet rx_packet;
int packet_length;
packet_length = packet_type == framer_packet_type(Short_Packet) ? 56 : 112;
int packet_length = 112;
if(inattrib[i] == framer_packet_type(Short_Packet)) packet_length = 56;
//printf("Packet received from framer w/length %i\n", packet_length);
rx_packet.type = framer_packet_type(inattrib[i]);
memset(&rx_packet.data, 0x00, 14 * sizeof(unsigned char));
memset(&rx_packet.lowconfbits, 0x00, 24 * sizeof(unsigned char));
rx_packet.numlowconf = 0;
rx_packet.type = packet_type;
memset(&rx_packet.data, 0x00, 14 * sizeof(unsigned char));
memset(&rx_packet.lowconfbits, 0x00, 24 * sizeof(unsigned char));
rx_packet.numlowconf = 0;
//let's use the preamble marker to get a reference level for the packet
rx_packet.reference_level = (bit_energy(&inraw[i], d_samples_per_chip)
+ bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4;
rx_packet.reference_level = (bit_energy(&inraw[i], d_samples_per_chip)
+ bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip)
+ bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4;
i += 8 * d_samples_per_symbol; //move to the center of the first bit of the data
i += 8 * d_samples_per_symbol; //move to the center of the first bit of the data
//here we calculate the total energy contained in each chip of the symbol
for(int j = 0; j < packet_length; j++) {
int firstchip = i+j*d_samples_per_symbol;
int secondchip = firstchip + d_samples_per_chip;
bool slice, confidence;
float firstchip_energy=0, secondchip_energy=0;
//here we calculate the total energy contained in each chip of the symbol
for(int j = 0; j < packet_length; j++) {
int firstchip = i+j*d_samples_per_symbol;
int secondchip = firstchip + d_samples_per_chip;
bool slice, confidence;
float firstchip_energy=0, secondchip_energy=0;
firstchip_energy = bit_energy(&inraw[firstchip], d_samples_per_chip);
secondchip_energy = bit_energy(&inraw[secondchip], d_samples_per_chip);
firstchip_energy = bit_energy(&inraw[firstchip], d_samples_per_chip);
secondchip_energy = bit_energy(&inraw[secondchip], d_samples_per_chip);
//3dB limits for bit slicing and confidence measurement
float highlimit=rx_packet.reference_level*2;
float lowlimit=rx_packet.reference_level*0.5;
bool firstchip_inref = ((firstchip_energy > lowlimit) && (firstchip_energy < highlimit));
bool secondchip_inref = ((secondchip_energy > lowlimit) && (secondchip_energy < highlimit));
//3dB limits for bit slicing and confidence measurement
float highlimit=rx_packet.reference_level*2;
float lowlimit=rx_packet.reference_level*0.5;
bool firstchip_inref = ((firstchip_energy > lowlimit) && (firstchip_energy < highlimit));
bool secondchip_inref = ((secondchip_energy > lowlimit) && (secondchip_energy < highlimit));
//these two lines for a super simple naive slicer.
// slice = firstchip_energy > secondchip_energy;
// confidence = bool(int(firstchip_inref) + int(secondchip_inref)); //one and only one chip in the reference zone
//these two lines for a super simple naive slicer.
// slice = firstchip_energy > secondchip_energy;
// confidence = bool(int(firstchip_inref) + int(secondchip_inref)); //one and only one chip in the reference zone
//below is the Lincoln Labs slicer. it may produce greater bit errors. supposedly it is more resistant to mode A/C FRUIT.
//below is the Lincoln Labs slicer. it may produce greater bit errors. supposedly it is more resistant to mode A/C FRUIT.
//see http://adsb.tc.faa.gov/WG3_Meetings/Meeting8/Squitter-Lon.pdf
if(firstchip_inref && !secondchip_inref) {
slice = 1;
confidence = 1;
}
else if(secondchip_inref && !firstchip_inref) {
slice = 0;
confidence = 1;
}
else if(firstchip_inref && secondchip_inref) {
slice = firstchip_energy > secondchip_energy;
confidence = 0;
}
else if(!firstchip_inref && !secondchip_inref) { //in this case, we determine the bit by whichever is larger, and we determine high confidence if the low chip is 6dB below reference.
slice = firstchip_energy > secondchip_energy;
if(slice) {
if(secondchip_energy < lowlimit * 0.5) confidence = 1;
else confidence = 0;
} else {
if(firstchip_energy < lowlimit * 0.5) confidence = 1;
else confidence = 0;
}
}
//put the data into the packet
if(firstchip_inref && !secondchip_inref) {
slice = 1;
confidence = 1;
}
else if(secondchip_inref && !firstchip_inref) {
slice = 0;
confidence = 1;
}
else if(firstchip_inref && secondchip_inref) {
slice = firstchip_energy > secondchip_energy;
confidence = 0;
}
else if(!firstchip_inref && !secondchip_inref) { //in this case, we determine the bit by whichever is larger, and we determine high confidence if the low chip is 6dB below reference.
slice = firstchip_energy > secondchip_energy;
if(slice) {
rx_packet.data[j/8] += 1 << (7-(j%8));
}
//put the confidence decision into the packet
if(confidence) {
//rx_packet.confidence[j/8] += 1 << (7-(j%8));
if(secondchip_energy < lowlimit * 0.5) confidence = 1;
else confidence = 0;
} else {
if(rx_packet.numlowconf < 24) rx_packet.lowconfbits[rx_packet.numlowconf++] = j;
if(firstchip_energy < lowlimit * 0.5) confidence = 1;
else confidence = 0;
}
}
i += packet_length * d_samples_per_symbol;
//here you might want to traverse the whole packet and if you find all 0's, just toss it. don't know why these packets turn up, but they pass ECC.
bool zeroes = 1;
for(int m = 0; m < 14; m++) {
if(rx_packet.data[m]) zeroes = 0;
//put the data into the packet
if(slice) {
rx_packet.data[j/8] += 1 << (7-(j%8));
}
if(zeroes) continue; //toss it
rx_packet.message_type = (rx_packet.data[0] >> 3) & 0x1F; //get the message type for the parser to conveniently use, and to make decisions on ECC methods
//we note that short packets other than type 11 CANNOT be reliably decoded, since the a/c address is encoded with the parity bits.
//mode S in production ATC use relies on the fact that these short packets are reply squitters to transponder requests,
//and so the radar should already know the expected a/c reply address. so, error-correction makes no sense on short packets (other than type 11)
//this means two things: first, we will DROP short packets (other than type 11) with ANY low-confidence bits, since we can't be confident that we're seeing real data
//second, we will only perform error correction on LONG type S packets.
//the limitation on short packets means in practice a short packet has to be at least 6dB above the noise floor in order to be output. long packets can theoretically
//be decoded at the 3dB SNR point. below that and the preamble detector won't fire.
//in practice, this limitation causes you to see a HUGE number of type 11 packets which pass CRC through random luck.
//these packets necessarily have large numbers of low-confidence bits, so we toss them with an arbitrary limit of 10.
//that's a pretty dang low threshold so i don't think we'll drop many legit packets
if(rx_packet.type == Short_Packet && rx_packet.message_type != 11 && rx_packet.numlowconf != 0) continue;
if(rx_packet.type == Short_Packet && rx_packet.message_type == 11 && rx_packet.numlowconf >= 10) continue;
//if(rx_packet.numlowconf >= 24) continue; //don't even try, this is the maximum number of errors ECC could possibly correct
//the above line should be part of ECC, and only checked if the message has parity errors
rx_packet.parity = modes_check_parity(rx_packet.data, packet_length);
if(rx_packet.parity && rx_packet.type == Long_Packet) {
// long before = rx_packet.parity;
bruteResultTypeDef bruteResult = modes_ec_brute(rx_packet);
if(bruteResult == No_Solution) {
//printf("No solution!\n");
continue;
} else if(bruteResult == Multiple_Solutions) {
// printf("Multiple solutions!\n");
continue;
} else if(bruteResult == Too_Many_LCBs) {
//printf("Too many LCBs (%i)!\n", rx_packet.numlowconf);
continue;
} else if(bruteResult == No_Error) {
// printf("No error!\n");
} else if(bruteResult == Solution_Found) {
// printf("Solution found for %i LCBs!\n", rx_packet.numlowconf);
}
// rx_packet.parity = modes_check_parity(rx_packet.data, packet_length);
// if(rx_packet.parity) printf("Error: packet fails parity check after correction, was %x, now %x\n", before, rx_packet.parity);
//put the confidence decision into the packet
if(confidence) {
//rx_packet.confidence[j/8] += 1 << (7-(j%8));
} else {
if(rx_packet.numlowconf < 24) rx_packet.lowconfbits[rx_packet.numlowconf++] = j;
}
}
/******************** BEGIN TIMESTAMP BS ******************/
rx_packet.timestamp_secs = 0;
rx_packet.timestamp_frac = 0;
uint64_t abs_sample_cnt = nitems_read(0);
std::vector<pmt::pmt_t> tags;
uint64_t timestamp_secs, timestamp_sample, timestamp_delta;
double timestamp_frac;
pmt::pmt_t timestamp;
get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i, pmt::pmt_string_to_symbol("packet_time_stamp"));
//tags.back() is the most recent timestamp, then.
if(tags.size() > 0) {
//if nobody but the USRP is producing timestamps this isn't necessary
//std::sort(tags.begin(), tags.end(), pmtcompare);
timestamp = tags.back();
timestamp_secs = pmt_to_uint64(pmt_tuple_ref(gr_tags::get_value(timestamp), 0));
timestamp_frac = pmt_to_double(pmt_tuple_ref(gr_tags::get_value(timestamp), 1));
timestamp_sample = gr_tags::get_nitems(timestamp);
//now we have to offset the timestamp based on the current sample number
timestamp_delta = (abs_sample_cnt + i) - timestamp_sample;
timestamp_frac += timestamp_delta * d_secs_per_sample;
if(timestamp_frac > 1.0) {
timestamp_frac -= 1.0;
timestamp_secs++;
}
rx_packet.timestamp_secs = timestamp_secs;
rx_packet.timestamp_frac = timestamp_frac;
}
// if(rx_packet.parity && rx_packet.type == Long_Packet) printf("Error! Bad packet forwarded to the queue.\n");
//now we have a complete packet with confidence data, let's print it to the message queue
//here, rather than send the entire packet, since we've already done parity checking and ECC in C++, we'll
//send just the data (no confidence bits), separated into fields for easier parsing.
/******************* END TIMESTAMP BS *********************/
//increment for the next round
//we'll replicate some data by sending the message type as the first field, followed by the first 8+24=32 bits of the packet, followed by
//56 long packet data bits if applicable (zero-padded if not), followed by parity
//here you might want to traverse the whole packet and if you find all 0's, just toss it. don't know why these packets turn up, but they pass ECC.
bool zeroes = 1;
for(int m = 0; m < 14; m++) {
if(rx_packet.data[m]) zeroes = 0;
}
if(zeroes) continue; //toss it
d_payload.str("");
rx_packet.message_type = (rx_packet.data[0] >> 3) & 0x1F; //get the message type for the parser to conveniently use, and to make decisions on ECC methods
d_payload << std::dec << std::setw(2) << std::setfill('0') << rx_packet.message_type << std::hex << " ";
//we note that short packets other than type 11 CANNOT be reliably decoded, since the a/c address is encoded with the parity bits.
//mode S in production ATC use relies on the fact that these short packets are reply squitters to transponder requests,
//and so the radar should already know the expected a/c reply address. so, error-correction makes no sense on short packets (other than type 11)
//this means two things: first, we will DROP short packets (other than type 11) with ANY low-confidence bits, since we can't be confident that we're seeing real data
//second, we will only perform error correction on LONG type S packets.
for(int m = 0; m < 4; m++) {
//the limitation on short packets means in practice a short packet has to be at least 6dB above the noise floor in order to be output. long packets can theoretically
//be decoded at the 3dB SNR point. below that and the preamble detector won't fire.
//in practice, this limitation causes you to see a HUGE number of type 11 packets which pass CRC through random luck.
//these packets necessarily have large numbers of low-confidence bits, so we toss them with an arbitrary limit of 10.
//that's a pretty dang low threshold so i don't think we'll drop many legit packets
if(rx_packet.type == Short_Packet && rx_packet.message_type != 11 && rx_packet.numlowconf != 0) continue;
if(rx_packet.type == Short_Packet && rx_packet.message_type == 11 && rx_packet.numlowconf >= 10) continue;
//if(rx_packet.numlowconf >= 24) continue; //don't even try, this is the maximum number of errors ECC could possibly correct
//the above line should be part of ECC, and only checked if the message has parity errors
rx_packet.parity = modes_check_parity(rx_packet.data, packet_length);
if(rx_packet.parity && rx_packet.type == Long_Packet) {
bruteResultTypeDef bruteResult = modes_ec_brute(rx_packet);
if(bruteResult == No_Solution) {
continue;
} else if(bruteResult == Multiple_Solutions) {
continue;
} else if(bruteResult == Too_Many_LCBs) {
continue;
} else if(bruteResult == No_Error) {
} else if(bruteResult == Solution_Found) {
// printf("Solution found for %i LCBs!\n", rx_packet.numlowconf);
}
}
//we'll replicate some data by sending the message type as the first field, followed by the first 8+24=32 bits of the packet, followed by
//56 long packet data bits if applicable (zero-padded if not), followed by parity
d_payload.str("");
d_payload << std::dec << std::setw(2) << std::setfill('0') << rx_packet.message_type << std::hex << " ";
for(int m = 0; m < 4; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
}
d_payload << " ";
if(packet_length == 112) {
for(int m = 4; m < 11; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
}
d_payload << " ";
if(packet_length == 112) {
for(int m = 4; m < 11; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
}
d_payload << " ";
for(int m = 11; m < 14; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
}
} else {
for(int m = 4; m < 11; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(0);
}
d_payload << " ";
for(int m = 4; m < 7; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
}
for(int m = 11; m < 14; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
}
} else {
for(int m = 4; m < 11; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(0);
}
d_payload << " ";
for(int m = 4; m < 7; m++) {
d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
}
d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level;
gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str()));
d_queue->handle(msg);
}
d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level
<< " " << rx_packet.timestamp_secs
<< " " << std::setprecision(10) << std::setw(10) << rx_packet.timestamp_frac;
gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str()));
d_queue->handle(msg);
}
return size;

1
src/lib/air_modes_slicer.h
View File

@ -45,6 +45,7 @@ private:
int d_chip_rate;
int d_samples_per_chip;
int d_samples_per_symbol;
double d_secs_per_sample;
gr_msg_queue_sptr d_queue;
std::ostringstream d_payload;

2
src/lib/air_modes_types.h
View File

@ -36,6 +36,8 @@ struct modes_packet {
framer_packet_type type; //what length packet are we
unsigned int message_type;
float reference_level;
unsigned long timestamp_secs; //timestamp from tags
double timestamp_frac;
};
#endif

13
src/python/mlat-test.py Executable file
View File

@ -0,0 +1,13 @@
#!/usr/bin/python
import mlat
import numpy
replies = []
for i in range(0, len(mlat.teststations)):
replies.append((mlat.teststations[i], mlat.teststamps[i]))
ans = mlat.mlat(replies, mlat.testalt)
error = numpy.linalg.norm(numpy.array(mlat.llh2ecef(ans))-numpy.array(mlat.testplane))
range = numpy.linalg.norm(mlat.llh2geoid(ans)-numpy.array(mlat.llh2geoid(mlat.teststations[0])))
print "Error: %.2fm" % (error)
print "Range: %.2fkm (from first station in list)" % (range/1000)

235
src/python/mlat.py Executable file
View File

@ -0,0 +1,235 @@
#!/usr/bin/python
import math
import numpy
from scipy.ndimage import map_coordinates
#functions for multilateration.
#NB: because of the way this solver works, at least 3 stations and timestamps
#are required. this function will not return hyperbolae for underconstrained systems.
#TODO: get HDOP out of this so we can draw circles of likely position and indicate constraint
##########################NOTES#########################################
#you should test your solver with a reference dataset that you've calculated. let's put one together.
#let's say we have you here in SF, ER in MV, and some place in Fremont. the plane can be at SFO at 24000'.
#your pos: 37.76225, -122.44254, 300'
#ettus: 37.409044, -122.077748, 300'
#fremont: 37.585085, -121.986395, 300'
#airplane: 37.617175,-122.380843, 24000'
#calculated using a third-party tool and verified against the algorithms in this file:
#you: -2708399, -4260759, 3884677
#ettus: -2693916, -4298177, 3853611
#fremont: -2680759, -4292379, 3869113
#airplane: -2712433, -4277271, 3876757
#here's how I did it in Octave...
#prange_est = ((stations(:, 1) - xguess(1)).^2 + (stations(:, 2) - xguess(2)).^2 + (stations(:,3) - xguess(3)).^2).^0.5;
#dphat = prange_obs - prange_est;
#H = [[-(stations(:,1)-xguess(1)) ./ prange_est, -(stations(:,2)-xguess(2)) ./ prange_est, -(stations(:,3)-xguess(3)) ./ prange_est]];
#xerr = (H'*H\H'*dphat)';
#xguess = xguess + xerr
#remember the last line of stations is the 0 vector. remember the last line of prange_obs is the height of the a/c above geoid.
#use one of the station positions as a starting guess. calculate alt-above-geoid once as though it were located directly above the starting guess; 300 miles
#is like 4 degrees of latitude and i don't think it'll introduce more than a few meters of error.
#well, there are some places where 4 degrees of latitude would count for 20 meters of error. but since our accuracy is really +/- 250m anyway it's probably not
#a big deal. saves a lot of CPU to only do the lookup once.
#so, the above solver works for pseudorange, but what if you only have time-of-flight info?
#let's modify the prange eq to deal with time difference of arrival instead of pseudorange
#knowns: time of arrival at each station, positions of each station, altitude of a/c
#from this, you can say "it arrived x ns sooner at each of the other stations"
#thus you can say "it's this much closer/farther to station y than to me"
#the stations vector is RELATIVE TO YOU -- so it's [ettus-me; fremont-me; [0,0,0]-me]
#prange_obs is a vector of TDOAs; the first value (earliest) is truncated since the difference is zero (just like stations)
#this implies the TDOAs should arrive sorted, which is probably a good idea, or at the very least the closest station should be first
#prange_est = [norm(stations(1,:)-xguess);
# norm(stations(2,:)-xguess);
# norm(stations(3,:)-xguess)]; #only valid for the three-station case we're testing Octave with
#dphat = prange_obs - prange_est;
#H = [[-(stations(:,1)-xguess(1)) ./ prange_est, -(stations(:,2)-xguess(2)) ./ prange_est, -(stations(:,3)-xguess(3)) ./ prange_est]];
#xguess += (H'*H\H'*dphat)';
#err=norm(airplane-(xguess+me)) #just for calculating convergence
#it converges for 500km position error in the initial guess, so it seems pretty good.
#seems to converge quickly in the terminal phase. 250m timing offset gives 450m position error
#250m time offset in the local receiver (830ns) gives 325m position error
#the last question is how to use the altitude data in prange_obs; calculate height above geoid for YOUR position at a/c alt and use that to get height above [0,0,0]
#this will be close enough. you could iterate this along with the rest of it but it won't change more than the variation in geoid height between you and a/c.
#maybe after convergence you can iterate a few more times with the a/c geoid height if you're worried about it.
#there's probably a way to use YOU as the alt. station and just use height above YOU instead. but this works.
########################END NOTES#######################################
#this is a 10x10-degree WGS84 geoid datum, in meters relative to the WGS84 reference ellipsoid. given the maximum slope, you should probably interpolate.
#NIMA suggests a 2x2 interpolation using four neighbors. we'll go cubic spline JUST BECAUSE WE CAN
wgs84_geoid = numpy.array([[13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13], #90N
[3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1], #80N
[2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4], #70N
[2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6], #60N
[-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2], #50N
[-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11], #40N
[-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6], #30N
[5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10], #20N
[13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26], #10N
[22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32], #0
[36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52], #10S
[51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63], #20S
[46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45], #30S
[21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20], #40S
[-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10], #50S
[-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43], #60S
[-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59], #70S
[-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52], #80S
[-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30]], #90S
dtype=numpy.float)
#ok this calculates the geoid offset from the reference ellipsoid
#combined with LLH->ECEF this gets you XYZ for a ground-referenced point
def wgs84_height(lat, lon):
yi = numpy.array([9-lat/10.0])
xi = numpy.array([18+lon/10.0])
return float(map_coordinates(wgs84_geoid, [yi, xi]))
#WGS84 reference ellipsoid constants
wgs84_a = 6378137.0
wgs84_b = 6356752.314245
wgs84_e2 = 0.0066943799901975848
wgs84_a2 = wgs84_a**2 #to speed things up a bit
wgs84_b2 = wgs84_b**2
#convert ECEF to lat/lon/alt without geoid correction
#returns alt in meters
def ecef2llh((x,y,z)):
ep = math.sqrt((wgs84_a2 - wgs84_b2) / wgs84_b2)
p = math.sqrt(x**2+y**2)
th = math.atan2(wgs84_a*z, wgs84_b*p)
lon = math.atan2(y, x)
lat = math.atan2(z+ep**2*wgs84_b*math.sin(th)**3, p-wgs84_e2*wgs84_a*math.cos(th)**3)
N = wgs84_a / math.sqrt(1-wgs84_e2*math.sin(lat)**2)
alt = p / math.cos(lat) - N
lon *= (180. / math.pi)
lat *= (180. / math.pi)
return [lat, lon, alt]
#convert lat/lon/alt coords to ECEF without geoid correction, WGS84 model
#remember that alt is in meters
def llh2ecef((lat, lon, alt)):
lat *= (math.pi / 180.0)
lon *= (math.pi / 180.0)
n = lambda x: wgs84_a / math.sqrt(1 - wgs84_e2*(math.sin(x)**2))
x = (n(lat) + alt)*math.cos(lat)*math.cos(lon)
y = (n(lat) + alt)*math.cos(lat)*math.sin(lon)
z = (n(lat)*(1-wgs84_e2)+alt)*math.sin(lat)
return [x,y,z]
#do both of the above to get a geoid-corrected x,y,z position
def llh2geoid((lat, lon, alt)):
(x,y,z) = llh2ecef((lat, lon, alt + wgs84_height(lat, lon)))
return [x,y,z]
c = 299792458 / 1.0003 #modified for refractive index of air, why not
#here's some test data to validate the algorithm
teststations = [[37.76225, -122.44254, 100], [37.409044, -122.077748, 100], [37.585085, -121.986395, 100]]
testalt = 8000
testplane = numpy.array(llh2ecef([37.617175,-122.380843, testalt]))
testme = llh2geoid(teststations[0])
teststamps = [10,
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[1]))) / c,
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[2]))) / c,
]
#this function is the iterative solver core of the mlat function below
#we use limit as a goal to stop solving when we get "close enough" (error magnitude in meters for that iteration)
#basically 20 meters is way less than the anticipated error of the system so it doesn't make sense to continue
#it's possible this could fail in situations where the solution converges slowly
#because the change in ERROR is not necessarily the error itself
#still, it should converge quickly when close, so it SHOULDN'T give more than 2-3x the precision in total error
def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 50):
xerr = [1e9, 1e9, 1e9]
rounds = 0
while numpy.linalg.norm(xerr) > limit:
prange_est = []
for station in rel_stations:
prange_est.append([numpy.linalg.norm(station - xguess)])
dphat = prange_obs - prange_est
H = []
for row in range(0,len(rel_stations)):
H.append((numpy.array(-rel_stations[row,:])-xguess) / prange_est[row])
H = numpy.array(H)
#now we have H, the Jacobian, and can solve for residual error
xerr = numpy.dot(numpy.linalg.solve(numpy.dot(H.T,H), H.T), dphat).flatten()
xguess += xerr
rounds += 1
if rounds > maxrounds:
raise Exception("Failed to converge!")
break
return xguess
#func mlat:
#uses a modified GPS pseudorange solver to locate aircraft by multilateration.
#replies is a list of reports, in ([lat, lon, alt], timestamp) format
#altitude is the barometric altitude of the aircraft as returned by the aircraft
#returns the estimated position of the aircraft in (lat, lon, alt) geoid-corrected WGS84.
#let's make it take a list of tuples so we can sort by them
def mlat(replies, altitude):
sorted_replies = sorted(replies, key=lambda time: time[1])
stations = []
timestamps = []
#i really couldn't figure out how to unpack this, sorry
for i in range(0, len(replies)):
stations.append( sorted_replies[i][0] )
timestamps.append( sorted_replies[i][1] )
me_llh = stations[0]
me = llh2geoid(stations[0])
rel_stations = [] #list of stations in XYZ relative to me
for station in stations[1:]:
rel_stations.append(numpy.array(llh2geoid(station)) - numpy.array(me))
rel_stations.append([0,0,0]-numpy.array(me)) #arne saknussemm, reporting in
rel_stations = numpy.array(rel_stations) #convert list of arrays to 2d array
tdoa = []
for stamp in timestamps[1:]:
tdoa.append(stamp - timestamps[0])
prange_obs = []
for stamp in tdoa:
prange_obs.append([c * stamp])
#so here we calc the estimated pseudorange to the center of the earth, using station[0] as a reference point for the geoid
#this is a necessary approximation since we don't know the location of the aircraft yet
#if the dang earth were actually round this wouldn't be an issue
prange_obs.append( [numpy.linalg.norm(llh2ecef((me_llh[0], me_llh[1], altitude)))] ) #use ECEF not geoid since alt is MSL not GPS
#prange_obs.append( [numpy.linalg.norm(testplane)]) #test for error
prange_obs = numpy.array(prange_obs)
xyzpos = mlat_iter(rel_stations, prange_obs)
llhpos = ecef2llh(xyzpos+me)
#now, we could return llhpos right now and be done with it.
#but the assumption we made above, namely that the aircraft is directly above the
#nearest station, results in significant error due to the oblateness of the Earth's geometry.
#so now we solve AGAIN, but this time with the corrected pseudorange of the aircraft altitude
#this might not be really useful in practice but the sim shows >50m errors without it
prange_obs[-1] = [numpy.linalg.norm(llh2ecef((llhpos[0], llhpos[1], altitude)))]
xyzpos_corr = mlat_iter(rel_stations, prange_obs, xyzpos) #start off with a really close guess
llhpos = ecef2llh(xyzpos_corr+me)
return llhpos

11
src/python/modes_print.py
View File

@ -30,13 +30,14 @@ class modes_output_print(modes_parse.modes_parse):
modes_parse.modes_parse.__init__(self, mypos)
def parse(self, message):
[msgtype, shortdata, longdata, parity, ecc, reference] = message.split()
[msgtype, shortdata, longdata, parity, ecc, reference, time_secs, time_frac] = message.split()
shortdata = long(shortdata, 16)
longdata = long(longdata, 16)
parity = long(parity, 16)
ecc = long(ecc, 16)
reference = float(reference)
time_secs = long(time_secs)
time_frac = float(time_frac)
msgtype = int(msgtype)
@ -53,15 +54,15 @@ class modes_output_print(modes_parse.modes_parse):
elif msgtype == 17:
output = self.print17(shortdata, longdata, parity, ecc)
else:
output = "No handler for message type " + str(msgtype) + " from " + str(ecc)
output = "No handler for message type " + str(msgtype) + " from %x" % ecc
if reference == 0.0:
refdb = 0.0
refdb = -150.0
else:
refdb = 10.0*math.log10(reference)
if output is not None:
output = "(%.0f) " % (refdb) + output
output = "(%.0f %u %f) " % (refdb, time_secs, time_frac) + output
print output
def print0(self, shortdata, parity, ecc):

2
src/python/modes_sbs1.py
View File

@ -59,7 +59,7 @@ class modes_output_sbs1(modes_parse.modes_parse):
#assembles a SBS-1-style output string from the received message
#this version ignores anything that isn't Type 17 for now, because we just don't care
[msgtype, shortdata, longdata, parity, ecc, reference] = message.split()
[msgtype, shortdata, longdata, parity, ecc, reference, time_secs, time_frac] = message.split()
shortdata = long(shortdata, 16)
longdata = long(longdata, 16)

2
src/python/modes_sql.py
View File

@ -68,7 +68,7 @@ class modes_output_sql(modes_parse.modes_parse):
def make_insert_query(self, message):
#assembles a SQL query tailored to our database
#this version ignores anything that isn't Type 17 for now, because we just don't care
[msgtype, shortdata, longdata, parity, ecc, reference] = message.split()
[msgtype, shortdata, longdata, parity, ecc, reference, time_secs, time_frac] = message.split()
shortdata = long(shortdata, 16)
longdata = long(longdata, 16)

42
src/python/uhd_modes.py
View File

@ -19,7 +19,8 @@
# Boston, MA 02110-1301, USA.
#
my_position = [37.76225, -122.44254]
#my_position = [37.76225, -122.44254]
my_position = [37.409066,-122.077836]
from gnuradio import gr, gru, optfir, eng_notation, blks2, air
from gnuradio import uhd
@ -56,9 +57,9 @@ class adsb_rx_block (gr.top_block):
if options.filename is None:
self.u = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32, 1)
if(options.rx_subdev_spec is None):
options.rx_subdev_spec = ""
self.u.set_subdev_spec(options.rx_subdev_spec)
#if(options.rx_subdev_spec is None):
# options.rx_subdev_spec = ""
#self.u.set_subdev_spec(options.rx_subdev_spec)
rate = options.rate
self.u.set_samp_rate(rate)
@ -66,20 +67,21 @@ class adsb_rx_block (gr.top_block):
if options.gain is None: #set to halfway
g = self.u.get_gain_range()
options.gain = (g.min+g.max) / 2.0
options.gain = (g.start()+g.stop()) / 2.0
if not(self.tune(options.freq)):
print "Failed to set initial frequency"
print "Setting gain to %i" % (options.gain,)
self.u.set_gain(options.gain)
print "Gain is %i" % (self.u.get_gain(),)
else:
rate = options.rate
self.u = gr.file_source(gr.sizeof_gr_complex, options.filename)
print "Rate is %i" % (rate,)
pass_all = 0
if options.output_all :
pass_all = 1
@ -89,31 +91,19 @@ class adsb_rx_block (gr.top_block):
#the DBSRX especially tends to be spur-prone; the LPF keeps out the
#spur multiple that shows up at 2MHz
self.filtcoeffs = gr.firdes.low_pass(1, rate, 1.8e6, 200e3)
self.filter = gr.fir_filter_fff(1, self.filtcoeffs)
# self.filtcoeffs = gr.firdes.low_pass(1, rate, 1.8e6, 200e3)
# self.filter = gr.fir_filter_fff(1, self.filtcoeffs)
self.preamble = air.modes_preamble(rate, options.threshold)
self.framer = air.modes_framer(rate)
self.slicer = air.modes_slicer(rate, queue)
self.connect(self.u, self.demod, self.filter)
self.connect(self.filter, self.avg)
self.connect(self.filter, (self.preamble, 0))
self.connect(self.avg, (self.preamble, 1))
self.connect(self.filter, (self.framer, 0))
self.connect(self.preamble, (self.framer, 1))
self.connect(self.filter, (self.slicer, 0))
self.connect(self.framer, (self.slicer, 1))
#use this flowgraph instead to omit the filter
# self.connect(self.u, self.demod)
# self.connect(self.demod, self.avg)
# self.connect(self.demod, (self.preamble, 0))
# self.connect(self.avg, (self.preamble, 1))
# self.connect(self.demod, (self.framer, 0))
# self.connect(self.preamble, (self.framer, 1))
# self.connect(self.demod, (self.slicer, 0))
# self.connect(self.framer, (self.slicer, 1))
self.connect(self.u, self.demod)
self.connect(self.demod, self.avg)
self.connect(self.demod, (self.preamble, 0))
self.connect(self.avg, (self.preamble, 1))
self.connect((self.preamble, 0), (self.framer, 0))
self.connect(self.framer, self.slicer)
def tune(self, freq):
result = self.u.set_center_freq(freq, 0)