some mlat changes and update to work with the "real" tags interface

This commit is contained in:
Nick Foster 2011-08-24 18:19:57 -07:00
parent 6beb78fcf3
commit 90fbf70c5e
7 changed files with 39 additions and 103 deletions

View File

@ -28,6 +28,7 @@
#include <gr_io_signature.h>
#include <string.h>
#include <iostream>
#include <gr_tag_info.h>
air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db)
{
@ -45,7 +46,7 @@ air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) :
d_check_width = 120 * d_samples_per_symbol; //only search to this far from the end of the stream buffer
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
d_secs_per_sample = 1.0 / channel_rate;
set_output_multiple(1+d_check_width*2);
std::stringstream str;
@ -77,6 +78,18 @@ static double correlate_preamble(const float *in, int samples_per_chip) {
return corr;
}
static double pmt_to_timestamp(pmt::pmt_t tstamp, uint64_t abs_sample_cnt, double secs_per_sample) {
uint64_t ts_sample, ticks;
if(pmt::pmt_symbol_to_string(gr_tags::get_key(tstamp)) != "timestamp") return 0;
ticks = pmt_to_uint64(gr_tags::get_value(tstamp));
ts_sample = gr_tags::get_nitems(tstamp);
//std::cout << "HEY WE GOT A STAMP AT " << ticks << " TICKS AT SAMPLE " << ts_sample << " ABS SAMPLE CNT IS " << abs_sample_cnt << std::endl;
return double(ticks+abs_sample_cnt) * secs_per_sample;
}
int air_modes_preamble::general_work(int noutput_items,
gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items,
@ -94,7 +107,13 @@ int air_modes_preamble::general_work(int noutput_items,
int(9 * d_samples_per_chip)
};
uint64_t abs_out_sample_cnt = nitems_written(0);
uint64_t abs_sample_cnt = nitems_read(0);
std::vector<pmt::pmt_t> tstamp_tags;
get_tags_in_range(tstamp_tags, 0, abs_sample_cnt, abs_sample_cnt + ninputs, pmt::pmt_string_to_symbol("timestamp"));
//tags.back() is the most recent timestamp, then.
if(tstamp_tags.size() > 0) {
d_timestamp = tstamp_tags.back();
}
for(int i=0; i < ninputs; i++) {
float pulse_threshold = inavg[i] * d_threshold;
@ -148,13 +167,21 @@ int air_modes_preamble::general_work(int noutput_items,
integrate_and_dump(out, &in[i], 240, d_samples_per_chip);
}
//get the timestamp of the preamble
double tstamp = 0;
if(d_timestamp) {
tstamp = pmt_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
}
//now tag the preamble
add_item_tag(0, //stream ID
nitems_written(0), //sample
d_key, //frame_info
pmt::pmt_from_double((double) space_threshold),
pmt::pmt_from_double(tstamp),
d_me //block src id
);
//std::cout << "PREAMBLE" << std::endl;
//produce only one output per work call

View File

@ -48,6 +48,8 @@ private:
float d_threshold_db;
float d_threshold;
pmt::pmt_t d_me, d_key;
pmt::pmt_t d_timestamp;
double d_secs_per_sample;
public:
int general_work (int noutput_items,

View File

@ -55,9 +55,8 @@ 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 / d_chip_rate;
set_output_multiple(1+d_check_width * 2); //how do you specify buffer size for sinks?
set_output_multiple(1+d_check_width); //how do you specify buffer size for sinks?
}
//FIXME i'm sure this exists in gr
@ -106,35 +105,18 @@ static slice_result_t slicer(const float bit0, const float bit1, const float ref
return result;
}
/*
static double pmt_to_timestamp(pmt::pmt_t tstamp, uint64_t sample_cnt, double secs_per_sample) {
double frac;
uint64_t secs, sample, sample_age;
if(gr_tags::get_name(tstamp) != "time") return 0;
secs = pmt_to_uint64(pmt_tuple_ref(gr_tags::get_value(tstamp), 0));
frac = pmt_to_double(pmt_tuple_ref(gr_tags::get_value(tstamp), 1));
sample = gr_tags::get_nitems(d_timestamp);
//now we have to offset the timestamp based on the current sample number
sample_age = (sample_cnt + i) - sample;
return sample_age * secs_per_sample + frac + secs;
}
*/
int air_modes_slicer::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const float *in = (const float *) input_items[0];
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("preamble_found"));
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;
@ -181,23 +163,7 @@ int air_modes_slicer::work(int noutput_items,
}
/******************** BEGIN TIMESTAMP BS ******************/
rx_packet.timestamp = 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;
get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i, pmt::pmt_string_to_symbol("time"));
//tags.back() is the most recent timestamp, then.
if(tags.size() > 0) {
d_timestamp = tags.back();
}
if(d_timestamp) {
rx_packet.timestamp = pmt_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
}
*/
rx_packet.timestamp = pmt_to_double(gr_tags::get_value(*tag_iter));
/******************* END TIMESTAMP BS *********************/
//increment for the next round

View File

@ -45,10 +45,8 @@ 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;
pmt::pmt_t d_timestamp;
public:
int work (int noutput_items,

View File

@ -3,7 +3,7 @@ import mlat
import numpy
#here's some test data to validate the algorithm
teststations = [[37.76225, -122.44254, 100], [37.409044, -122.077748, 100], [37.63816,-122.378082, 100], [37.701207,-122.309418, 100]]
teststations = [[37.76225, -122.44254, 100], [37.680016,-121.772461, 100], [37.385844,-122.083082, 100], [37.701207,-122.309418, 100]]
testalt = 8000
testplane = numpy.array(mlat.llh2ecef([37.617175,-122.400843, testalt]))
testme = mlat.llh2geoid(teststations[0])

View File

@ -13,62 +13,6 @@ from scipy.ndimage import map_coordinates
#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#######################################
@ -216,8 +160,8 @@ def mlat(replies, altitude):
prange_obs = numpy.array(prange_obs)
#xguess = llh2ecef([37.617175,-122.400843, 8000])-numpy.array(me)
xguess = [0,0,0]
#xguess = numpy.array(llh2ecef([stations[2][0], stations[2][1], altitude])) - numpy.array(me)
#xguess = [0,0,0]
xguess = numpy.array(llh2ecef([me_llh[0], me_llh[1], altitude])) - numpy.array(me)
xyzpos = mlat_iter(rel_stations, prange_obs, xguess)
llhpos = ecef2llh(xyzpos+me)
@ -228,7 +172,6 @@ def mlat(replies, altitude):
#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)

View File

@ -61,7 +61,7 @@ class modes_output_print(modes_parse.modes_parse):
refdb = 10.0*math.log10(reference)
if output is not None:
output = "(%.0f %f) " % (refdb, timestamp) + output
output = "(%.0f %.10f) " % (refdb, timestamp) + output
print output
def print0(self, shortdata, parity, ecc):