From 17f7cb9a5352ad90a3e0949cec98e5c484d6234c Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Wed, 13 Jun 2012 07:49:22 -0700 Subject: [PATCH] Backport Flightgear server from private tx branch, collapse commits. * Added Flightgear multiplayer output interface to uhd_modes.py. This allows flight with live traffic in fgfs. * Quaternion library borrowed from PyPi and modified to generate angle/axis representation and construct rotation quat from lat/lon to ECEF. * Miscellaneous enhancements and cleanup to fix timestamps, add aircraft type field (seems to be unused anyway), turnrate info --- apps/uhd_modes.py | 29 +- include/air_modes_types.h | 2 +- include/{modes_parity.h => modes_crc.h} | 8 +- lib/CMakeLists.txt | 2 +- lib/air_modes_preamble.cc | 16 +- lib/air_modes_slicer.cc | 14 +- lib/{modes_parity.cc => modes_crc.cc} | 29 +- python/CMakeLists.txt | 2 + python/Quaternion.py | 441 ++++++++++++++++++++++++ python/__init__.py | 2 + python/cpr.py | 37 +- python/modes_parse.py | 63 ++-- python/modes_print.py | 55 +-- 13 files changed, 567 insertions(+), 133 deletions(-) rename include/{modes_parity.h => modes_crc.h} (83%) rename lib/{modes_parity.cc => modes_crc.cc} (85%) create mode 100644 python/Quaternion.py diff --git a/apps/uhd_modes.py b/apps/uhd_modes.py index d18817b..4e328bd 100755 --- a/apps/uhd_modes.py +++ b/apps/uhd_modes.py @@ -19,8 +19,9 @@ # Boston, MA 02110-1301, USA. # -my_position = [37.76225, -122.44254] +#my_position = [37.76225, -122.44254] #my_position = [37.409066,-122.077836] +my_position = [30.2, -97.6] #my_position = None from gnuradio import gr, gru, optfir, eng_notation, blks2 @@ -53,7 +54,7 @@ class adsb_rx_block (gr.top_block): self.args = args rate = int(options.rate) - if options.filename is None: + if options.filename is None and options.udp is None: self.u = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32, 1) time_spec = uhd.time_spec(0.0) self.u.set_time_now(time_spec) @@ -79,7 +80,10 @@ class adsb_rx_block (gr.top_block): print "Gain is %i" % (self.u.get_gain(),) else: - self.u = gr.file_source(gr.sizeof_gr_complex, options.filename) + if options.filename is not None: + self.u = gr.file_source(gr.sizeof_gr_complex, options.filename) + else: + self.u = gr.udp_source(gr.sizeof_gr_complex, "localhost", options.udp) print "Rate is %i" % (rate,) @@ -92,14 +96,14 @@ 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.lpfiltcoeffs = gr.firdes.low_pass(1, rate, 1.8e6, 100e3) - self.lpfilter = gr.fir_filter_ccf(1, self.lpfiltcoeffs) + #self.lpfiltcoeffs = gr.firdes.low_pass(1, rate, 1.8e6, 100e3) + #self.lpfilter = gr.fir_filter_ccf(1, self.lpfiltcoeffs) self.preamble = air_modes.modes_preamble(rate, options.threshold) #self.framer = air_modes.modes_framer(rate) self.slicer = air_modes.modes_slicer(rate, queue) - self.connect(self.u, self.lpfilter, self.demod) + 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)) @@ -141,6 +145,10 @@ if __name__ == '__main__': help="disable printing decoded packets to stdout") parser.add_option("-l","--location", type="string", default=None, help="GPS coordinates of receiving station in format xx.xxxxx,xx.xxxxx") + parser.add_option("-u","--udp", type="int", default=None, + help="Use UDP source on specified port") + parser.add_option("-m","--multiplayer", type="string", default=None, + help="FlightGear server to send aircraft data, in format host:port") (options, args) = parser.parse_args() if options.location is not None: @@ -171,6 +179,11 @@ if __name__ == '__main__': outputs.append(printraw) updates.append(rawport.add_pending_conns) + if options.multiplayer is not None: + [fghost, fgport] = options.multiplayer.split(':') + fgout = air_modes.modes_flightgear(my_position, fghost, int(fgport)) + outputs.append(fgout.output) + fg = adsb_rx_block(options, args, queue) runner = top_block_runner(fg) @@ -183,8 +196,8 @@ if __name__ == '__main__': update() #main message handler - if queue.empty_p() == 0 : - while queue.empty_p() == 0 : + if not queue.empty_p() : + while not queue.empty_p() : msg = queue.delete_head() #blocking read for out in outputs: diff --git a/include/air_modes_types.h b/include/air_modes_types.h index 8464b06..9a80ffd 100644 --- a/include/air_modes_types.h +++ b/include/air_modes_types.h @@ -31,7 +31,7 @@ struct modes_packet { // unsigned char confidence[14]; //112 bits of boolean high/low confidence data for each bit unsigned char lowconfbits[24]; //positions of low confidence bits within the packet - unsigned long parity; + unsigned long crc; unsigned int numlowconf; framer_packet_type type; //what length packet are we unsigned int message_type; diff --git a/include/modes_parity.h b/include/modes_crc.h similarity index 83% rename from include/modes_parity.h rename to include/modes_crc.h index 5c1e20f..1d6a230 100644 --- a/include/modes_parity.h +++ b/include/modes_crc.h @@ -20,10 +20,10 @@ # */ -#ifndef INCLUDED_MODES_PARITY_H -#define INCLUDED_MODES_PARITY_H -extern const unsigned int modes_parity_table[112]; -int modes_check_parity(unsigned char data[], int length); +#ifndef INCLUDED_MODES_CRC_H +#define INCLUDED_MODES_CRC_H +extern const unsigned int modes_crc_table[112]; +int modes_check_crc(unsigned char data[], int length); bruteResultTypeDef modes_ec_brute(modes_packet &err_packet); unsigned next_set_of_n_elements(unsigned x); diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index b22d3f5..5a9504f 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -25,7 +25,7 @@ include(GrPlatform) #define LIB_SUFFIX add_library(air_modes SHARED air_modes_preamble.cc air_modes_slicer.cc - modes_parity.cc + modes_crc.cc ) target_link_libraries(air_modes ${Boost_LIBRARIES} ${GRUEL_LIBRARIES} ${GNURADIO_CORE_LIBRARIES}) set_target_properties(air_modes PROPERTIES DEFINE_SYMBOL "AIR_MODES_EXPORTS") diff --git a/lib/air_modes_preamble.cc b/lib/air_modes_preamble.cc index ec95af8..f688a63 100644 --- a/lib/air_modes_preamble.cc +++ b/lib/air_modes_preamble.cc @@ -84,13 +84,15 @@ static double tag_to_timestamp(gr_tag_t tstamp, uint64_t abs_sample_cnt, double uint64_t ts_sample, last_whole_stamp; double last_frac_stamp; - if(tstamp.key == NULL || pmt::pmt_symbol_to_string(tstamp.key) != "timestamp") return 0; + if(tstamp.key == NULL || pmt::pmt_symbol_to_string(tstamp.key) != "rx_time") return 0; last_whole_stamp = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(tstamp.value, 0)); last_frac_stamp = pmt::pmt_to_double(pmt::pmt_tuple_ref(tstamp.value, 1)); ts_sample = tstamp.offset; - //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(abs_sample_cnt * secs_per_sample) + last_whole_stamp + last_frac_stamp; + + double tstime = double(abs_sample_cnt * secs_per_sample) + last_whole_stamp + last_frac_stamp; + //std::cout << "HEY WE GOT A STAMP AT " << tstime << " TICKS AT SAMPLE " << ts_sample << " ABS SAMPLE CNT IS " << abs_sample_cnt << std::endl; + return tstime; } int air_modes_preamble::general_work(int noutput_items, @@ -112,7 +114,7 @@ int air_modes_preamble::general_work(int noutput_items, uint64_t abs_sample_cnt = nitems_read(0); std::vector tstamp_tags; - get_tags_in_range(tstamp_tags, 0, abs_sample_cnt, abs_sample_cnt + ninputs, pmt::pmt_string_to_symbol("timestamp")); + get_tags_in_range(tstamp_tags, 0, abs_sample_cnt, abs_sample_cnt + ninputs, pmt::pmt_string_to_symbol("rx_time")); //tags.back() is the most recent timestamp, then. if(tstamp_tags.size() > 0) { d_timestamp = tstamp_tags.back(); @@ -174,11 +176,7 @@ int air_modes_preamble::general_work(int noutput_items, } //get the timestamp of the preamble - double tstamp = 0; - - if(d_timestamp.offset != 0) { - tstamp = tag_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample); - } + double tstamp = tag_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample); //now tag the preamble add_item_tag(0, //stream ID diff --git a/lib/air_modes_slicer.cc b/lib/air_modes_slicer.cc index ffe2a66..00e8f35 100644 --- a/lib/air_modes_slicer.cc +++ b/lib/air_modes_slicer.cc @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include #include @@ -172,18 +172,18 @@ int air_modes_slicer::work(int noutput_items, if(rx_packet.type == Short_Packet && rx_packet.message_type != 11 && rx_packet.numlowconf > 0) {continue;} if(rx_packet.message_type == 11 && rx_packet.numlowconf >= 10) {continue;} - rx_packet.parity = modes_check_parity(rx_packet.data, packet_length); + rx_packet.crc = modes_check_crc(rx_packet.data, packet_length); - //parity for packets that aren't type 11 or type 17 is encoded with the transponder ID, which we don't know + //crc for packets that aren't type 11 or type 17 is encoded with the transponder ID, which we don't know //therefore we toss 'em if there's syndrome - //parity for the other short packets is usually nonzero, so they can't really be trusted that far - if(rx_packet.parity && (rx_packet.message_type == 11 || rx_packet.message_type == 17)) {continue;} + //crc for the other short packets is usually nonzero, so they can't really be trusted that far + if(rx_packet.crc && (rx_packet.message_type == 11 || rx_packet.message_type == 17)) {continue;} //we no longer attempt to brute force error correct via syndrome. it really only gets you 1% additional returns, //at the expense of a lot of CPU time and complexity //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 + //56 long packet data bits if applicable (zero-padded if not), followed by crc d_payload.str(""); d_payload << std::dec << std::setw(2) << std::setfill('0') << rx_packet.message_type << std::hex << " "; @@ -209,7 +209,7 @@ int air_modes_slicer::work(int noutput_items, } } - d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level + d_payload << " " << std::setw(6) << rx_packet.crc << " " << std::dec << rx_packet.reference_level << " " << std::setprecision(10) << std::setw(10) << rx_packet.timestamp; gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str())); d_queue->handle(msg); diff --git a/lib/modes_parity.cc b/lib/modes_crc.cc similarity index 85% rename from lib/modes_parity.cc rename to lib/modes_crc.cc index 3a8f578..05d3cba 100644 --- a/lib/modes_parity.cc +++ b/lib/modes_crc.cc @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -35,7 +35,7 @@ * Index is bit position with bit 0 being the first bit after preamble * On short frames an offset of 56 is used. */ -const unsigned int modes_parity_table[112] = +const unsigned int modes_crc_table[112] = { 0x3935ea, // Start of Long Frame CRC 0x1c9af5, @@ -150,29 +150,16 @@ const unsigned int modes_parity_table[112] = 0x000002, 0x000001, }; -int modes_check_parity(unsigned char data[], int length) + +int modes_check_crc(unsigned char data[], int length) { - int short_crc, long_crc, i; - // Check both long and short - short_crc = 0; - long_crc = 0; - for(i = 0; i < 56; i++) + int crc, i; + for(i = 0; i < length; i++) { if(data[i/8] & (1 << (7-(i%8)))) { - short_crc ^= modes_parity_table[i+56]; - long_crc ^= modes_parity_table[i]; - + crc ^= modes_crc_table[i+(112-length)]; } } - for( ; i < length; i++) - { - if(data[i/8] & (1 << (7-(i%8)))) - { - long_crc ^= modes_parity_table[i]; - } - } - if(length == 112) return long_crc; - else return short_crc; + return crc; } - diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 3381e07..0056be0 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -34,12 +34,14 @@ GR_PYTHON_INSTALL( altitude.py cpr.py mlat.py + modes_flightgear.py modes_kml.py modes_parse.py modes_print.py modes_raw_server.py modes_sbs1.py modes_sql.py + Quaternion.py DESTINATION ${GR_PYTHON_DIR}/air_modes ) diff --git a/python/Quaternion.py b/python/Quaternion.py new file mode 100644 index 0000000..4a41ce7 --- /dev/null +++ b/python/Quaternion.py @@ -0,0 +1,441 @@ +""" +Quaternion provides a class for manipulating quaternion objects. This class provides: + + - a convenient constructor to convert to/from Euler Angles (RA,Dec,Roll) + to/from quaternions + - class methods to multiply and divide quaternions +""" + +"""Copyright 2009 Smithsonian Astrophysical Observatory + Released under New BSD / 3-Clause BSD License + All rights reserved +""" +""" + Modified 2012 by Nick Foster + Modified from version 0.3.1 + http://pypi.python.org/pypi/Quaternion/0.03 + Added _get_angle_axis to get the angle-axis representation + Added _latlontoquat to get a rotation quat to ECEF from lat/lon +""" + + + +import numpy as np +from math import cos, sin, radians, degrees, atan2, sqrt, acos, pi + +class Quat(object): + """ + Quaternion class + + Example usage:: + + >>> from Quaternion import Quat + >>> quat = Quat((12,45,45)) + >>> quat.ra, quat.dec, quat.roll + (12, 45, 45) + >>> quat.q + array([ 0.38857298, -0.3146602 , 0.23486498, 0.8335697 ]) + >>> q2 = Quat([ 0.38857298, -0.3146602 , 0.23486498, 0.8335697]) + >>> q2.ra + 11.999999315925008 + + + Multiplication and division operators are overloaded for the class to + perform appropriate quaternion multiplication and division. + + Example usage:: + + >>> q1 = Quat((20,30,40)) + >>> q2 = Quat((30,40,50)) + >>> q = q1 / q2 + + Performs the operation as q1 * inverse q2 + + Example usage:: + + >>> q1 = Quat((20,30,40)) + >>> q2 = Quat((30,40,50)) + >>> q = q1 * q2 + + + :param attitude: initialization attitude for quat + + ``attitude`` may be: + * another Quat + * a 4 element array (expects x,y,z,w quat form) + * a 3 element array (expects ra,dec,roll in degrees) + * a 3x3 transform/rotation matrix + + """ + def __init__(self, attitude): + self._q = None + self._equatorial = None + self._T = None + # checks to see if we've been passed a Quat + if isinstance(attitude, Quat): + self._set_q(attitude.q) + else: + # make it an array and check to see if it is a supported shape + attitude = np.array(attitude) + if len(attitude) == 4: + self._set_q(attitude) + elif attitude.shape == (3,3): + self._set_transform(attitude) + elif attitude.shape == (3,): + self._set_equatorial(attitude) + elif attitude.shape == (2,): + self._set_latlon(attitude) + else: + raise TypeError("attitude is not one of possible types (2, 3 or 4 elements, Quat, or 3x3 matrix)") + + + def _set_q(self, q): + """ + Set the value of the 4 element quaternion vector + + :param q: list or array of normalized quaternion elements + """ + q = np.array(q) + if abs(np.sum(q**2) - 1.0) > 1e-6: + raise ValueError('Quaternion must be normalized so sum(q**2) == 1; use Quaternion.normalize') + self._q = (q if q[3] > 0 else -q) + # Erase internal values of other representations + self._equatorial = None + self._T = None + + def _get_q(self): + """ + Retrieve 4-vector of quaternion elements in [x, y, z, w] form + + :rtype: numpy array + + """ + if self._q is None: + # Figure out q from available values, doing nothing others are not defined + if self._equatorial is not None: + self._q = self._equatorial2quat() + elif self._T is not None: + self._q = self._transform2quat() + return self._q + + # use property to make this get/set automatic + q = property(_get_q, _set_q) + + def _set_equatorial(self, equatorial): + """Set the value of the 3 element equatorial coordinate list [RA,Dec,Roll] + expects values in degrees + bounds are not checked + + :param equatorial: list or array [ RA, Dec, Roll] in degrees + + """ + att = np.array(equatorial) + ra, dec, roll = att + self._ra0 = ra + if ( ra > 180 ): + self._ra0 = ra - 360 + self._roll0 = roll + if ( roll > 180): + self._roll0 = roll - 360 + self._equatorial = att + + def _set_latlon(self, latlon): + self._q = self._latlontoquat(latlon) + + def _get_equatorial(self): + """Retrieve [RA, Dec, Roll] + + :rtype: numpy array + """ + if self._equatorial is None: + if self._q is not None: + self._equatorial = self._quat2equatorial() + elif self._T is not None: + self._q = self._transform2quat() + self._equatorial = self._quat2equatorial() + return self._equatorial + + equatorial = property(_get_equatorial,_set_equatorial) + + def _get_ra(self): + """Retrieve RA term from equatorial system in degrees""" + return self.equatorial[0] + + def _get_dec(self): + """Retrieve Dec term from equatorial system in degrees""" + return self.equatorial[1] + + def _get_roll(self): + """Retrieve Roll term from equatorial system in degrees""" + return self.equatorial[2] + + ra = property(_get_ra) + dec = property(_get_dec) + roll = property(_get_roll) + + def _set_transform(self, T): + """ + Set the value of the 3x3 rotation/transform matrix + + :param T: 3x3 array/numpy array + """ + transform = np.array(T) + self._T = transform + + def _get_transform(self): + """ + Retrieve the value of the 3x3 rotation/transform matrix + + :returns: 3x3 rotation/transform matrix + :rtype: numpy array + + """ + if self._T is None: + if self._q is not None: + self._T = self._quat2transform() + elif self._equatorial is not None: + self._T = self._equatorial2transform() + return self._T + + transform = property(_get_transform, _set_transform) + + def _quat2equatorial(self): + """ + Determine Right Ascension, Declination, and Roll for the object quaternion + + :returns: RA, Dec, Roll + :rtype: numpy array [ra,dec,roll] + """ + + q = self.q + q2 = self.q**2 + + ## calculate direction cosine matrix elements from $quaternions + xa = q2[0] - q2[1] - q2[2] + q2[3] + xb = 2 * (q[0] * q[1] + q[2] * q[3]) + xn = 2 * (q[0] * q[2] - q[1] * q[3]) + yn = 2 * (q[1] * q[2] + q[0] * q[3]) + zn = q2[3] + q2[2] - q2[0] - q2[1] + + ##; calculate RA, Dec, Roll from cosine matrix elements + ra = degrees(atan2(xb , xa)) ; + dec = degrees(atan2(xn , sqrt(1 - xn**2))); + roll = degrees(atan2(yn , zn)) ; + if ( ra < 0 ): + ra += 360 + if ( roll < 0 ): + roll += 360 + + return np.array([ra, dec, roll]) + + + def _quat2transform(self): + """ + Transform a unit quaternion into its corresponding rotation matrix (to + be applied on the right side). + + :returns: transform matrix + :rtype: numpy array + + """ + x, y, z, w = self.q + xx2 = 2 * x * x + yy2 = 2 * y * y + zz2 = 2 * z * z + xy2 = 2 * x * y + wz2 = 2 * w * z + zx2 = 2 * z * x + wy2 = 2 * w * y + yz2 = 2 * y * z + wx2 = 2 * w * x + + rmat = np.empty((3, 3), float) + rmat[0,0] = 1. - yy2 - zz2 + rmat[0,1] = xy2 - wz2 + rmat[0,2] = zx2 + wy2 + rmat[1,0] = xy2 + wz2 + rmat[1,1] = 1. - xx2 - zz2 + rmat[1,2] = yz2 - wx2 + rmat[2,0] = zx2 - wy2 + rmat[2,1] = yz2 + wx2 + rmat[2,2] = 1. - xx2 - yy2 + + return rmat + + def _equatorial2quat( self ): + """Dummy method to return return quat. + + :returns: quaternion + :rtype: Quat + + """ + return self._transform2quat() + + def _equatorial2transform( self ): + """Construct the transform/rotation matrix from RA,Dec,Roll + + :returns: transform matrix + :rtype: 3x3 numpy array + + """ + ra = radians(self._get_ra()) + dec = radians(self._get_dec()) + roll = radians(self._get_roll()) + ca = cos(ra) + sa = sin(ra) + cd = cos(dec) + sd = sin(dec) + cr = cos(roll) + sr = sin(roll) + + # This is the transpose of the transformation matrix (related to translation + # of original perl code + rmat = np.array([[ca * cd, sa * cd, sd ], + [-ca * sd * sr - sa * cr, -sa * sd * sr + ca * cr, cd * sr], + [-ca * sd * cr + sa * sr, -sa * sd * cr - ca * sr, cd * cr]]) + + return rmat.transpose() + + def _transform2quat( self ): + """Construct quaternion from the transform/rotation matrix + + :returns: quaternion formed from transform matrix + :rtype: numpy array + """ + + # Code was copied from perl PDL code that uses backwards index ordering + T = self.transform.transpose() + den = np.array([ 1.0 + T[0,0] - T[1,1] - T[2,2], + 1.0 - T[0,0] + T[1,1] - T[2,2], + 1.0 - T[0,0] - T[1,1] + T[2,2], + 1.0 + T[0,0] + T[1,1] + T[2,2]]) + + max_idx = np.flatnonzero(den == max(den))[0] + + q = np.zeros(4) + q[max_idx] = 0.5 * sqrt(max(den)) + denom = 4.0 * q[max_idx] + if (max_idx == 0): + q[1] = (T[1,0] + T[0,1]) / denom + q[2] = (T[2,0] + T[0,2]) / denom + q[3] = -(T[2,1] - T[1,2]) / denom + if (max_idx == 1): + q[0] = (T[1,0] + T[0,1]) / denom + q[2] = (T[2,1] + T[1,2]) / denom + q[3] = -(T[0,2] - T[2,0]) / denom + if (max_idx == 2): + q[0] = (T[2,0] + T[0,2]) / denom + q[1] = (T[2,1] + T[1,2]) / denom + q[3] = -(T[1,0] - T[0,1]) / denom + if (max_idx == 3): + q[0] = -(T[2,1] - T[1,2]) / denom + q[1] = -(T[0,2] - T[2,0]) / denom + q[2] = -(T[1,0] - T[0,1]) / denom + + return q + + def _get_angle_axis(self): + lim = 1e-12 + norm = np.linalg.norm(self.q) + if norm < lim: + angle = 0 + axis = [0,0,0] + else: + rnorm = 1.0 / norm + angle = acos(max(-1, min(1, rnorm*self.q[3]))); + sangle = sin(angle) + if sangle < lim: + axis = [0,0,0] + else: + axis = (rnorm / sangle) * np.array(self.q[0:3]) + + angle *= 2 + + return (angle, axis) + + def _latlontoquat ( self, latlon ): + q = np.zeros(4) + + lon = latlon[1]*(pi/180.) + lat = latlon[0]*(pi/180.) + zd2 = 0.5*lon + yd2 = -0.25*pi - 0.5*lat + Szd2 = sin(zd2) + Syd2 = sin(yd2) + Czd2 = cos(zd2) + Cyd2 = cos(yd2) + q[0] = -Szd2*Syd2 + q[1] = Czd2*Syd2 + q[2] = Szd2*Cyd2 + q[3] = Czd2*Cyd2 + + return q + + def __div__(self, quat2): + """ + Divide one quaternion by another. + + Example usage:: + + >>> q1 = Quat((20,30,40)) + >>> q2 = Quat((30,40,50)) + >>> q = q1 / q2 + + Performs the operation as q1 * inverse q2 + + :returns: product q1 * inverse q2 + :rtype: Quat + + """ + return self * quat2.inv() + + + def __mul__(self, quat2): + """ + Multiply quaternion by another. + + Example usage:: + + >>> q1 = Quat((20,30,40)) + >>> q2 = Quat((30,40,50)) + >>> (q1 * q2).equatorial + array([ 349.73395729, 76.25393056, 127.61636727]) + + :returns: product q1 * q2 + :rtype: Quat + + """ + q1 = self.q + q2 = quat2.q + mult = np.zeros(4) + mult[0] = q1[3]*q2[0] - q1[2]*q2[1] + q1[1]*q2[2] + q1[0]*q2[3] + mult[1] = q1[2]*q2[0] + q1[3]*q2[1] - q1[0]*q2[2] + q1[1]*q2[3] + mult[2] = -q1[1]*q2[0] + q1[0]*q2[1] + q1[3]*q2[2] + q1[2]*q2[3] + mult[3] = -q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] + q1[3]*q2[3] + return Quat(mult) + + def inv(self): + """ + Invert the quaternion + + :returns: inverted quaternion + :rtype: Quat + """ + return Quat([self.q[0], self.q[1], self.q[2], -self.q[3]]) + + + + + +def normalize(array): + """ + Normalize a 4 element array/list/numpy.array for use as a quaternion + + :param quat_array: 4 element list/array + :returns: normalized array + :rtype: numpy array + + """ + quat = np.array(array) + return quat / np.sqrt(np.dot(quat, quat)) + diff --git a/python/__init__.py b/python/__init__.py index ae0cbb8..1ae0a13 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -56,6 +56,8 @@ from modes_sql import modes_output_sql from modes_sbs1 import modes_output_sbs1 from modes_kml import modes_kml from modes_raw_server import modes_raw_server +from modes_flightgear import modes_flightgear +from Quaternion import * # ---------------------------------------------------------------- # Tail of workaround diff --git a/python/cpr.py b/python/cpr.py index e20ba11..51ef86c 100644 --- a/python/cpr.py +++ b/python/cpr.py @@ -24,7 +24,9 @@ #from string import split, join #from math import pi, floor, cos, acos import math, time -#this implements CPR position decoding. +#this implements CPR position decoding and encoding. +#the decoder is implemented as a class, cpr_decoder, which keeps state for local decoding. +#the encoder is cpr_encode([lat, lon], type (even=0, odd=1), and surface (0 for surface, 1 for airborne)) latz = 15 @@ -103,9 +105,6 @@ def cpr_resolve_global(evenpos, oddpos, mostrecent, surface): #ok this is consid dlateven = dlat(0, surface); dlatodd = dlat(1, surface); -# print dlateven; -# print dlatodd; - evenpos = [float(evenpos[0]), float(evenpos[1])] oddpos = [float(oddpos[0]), float(oddpos[1])] @@ -113,13 +112,9 @@ def cpr_resolve_global(evenpos, oddpos, mostrecent, surface): #ok this is consid j = math.floor(((59*evenpos[0] - 60*oddpos[0])/2**nbits(surface)) + 0.5) #latitude index - #print "Latitude index: %i" % j #should be 6, getting 5? - rlateven = dlateven * (mod(j, 60)+evenpos[0]/2**nbits(surface)) rlatodd = dlatodd * (mod(j, 59)+ oddpos[0]/2**nbits(surface)) - #print "Rlateven: %f\nRlatodd: %f" % (rlateven, rlatodd,) - if nl(rlateven) != nl(rlatodd): #print "Boundary straddle!" return (None, None,) @@ -136,11 +131,7 @@ def cpr_resolve_global(evenpos, oddpos, mostrecent, surface): #ok this is consid nlthing = nl(rlat) ni = nlthing - mostrecent - #print "ni is %i" % ni - m = math.floor(((evenpos[1]*(nlthing-1)-oddpos[1]*(nlthing))/2**nbits(surface))+0.5) #longitude index, THIS LINE IS CORRECT - #print "m is %f" % m #should be -16 - if mostrecent == 0: enclon = evenpos[1] @@ -203,7 +194,6 @@ class cpr_decoder: del poslist[key] def decode(self, icao24, encoded_lat, encoded_lon, cpr_format, surface): -#def cpr_decode(my_location, icao24, encoded_lat, encoded_lon, cpr_format, evenlist, oddlist, lkplist, surface): #add the info to the position reports list for global decoding if cpr_format==1: self.oddlist[icao24] = [encoded_lat, encoded_lon, time.time()] @@ -226,7 +216,6 @@ class cpr_decoder: self.lkplist[icao24] = [decoded_lat, decoded_lon, time.time()] #update the local position for next time elif ((icao24 in self.evenlist) and (icao24 in self.oddlist) and abs(self.evenlist[icao24][2] - self.oddlist[icao24][2]) < 10): - # print "debug: valid even/odd positions, performing global decode." newer = (self.oddlist[icao24][2] - self.evenlist[icao24][2]) > 0 #figure out which report is newer [decoded_lat, decoded_lon] = cpr_resolve_global(self.evenlist[icao24][0:2], self.oddlist[icao24][0:2], newer, surface) #do a global decode if decoded_lat is not None: @@ -241,7 +230,7 @@ class cpr_decoder: #print "settled on position: %.6f, %.6f" % (decoded_lat, decoded_lon,) - if decoded_lat is not None: + if decoded_lat is not None and self.my_location is not None: [rnge, bearing] = range_bearing(self.my_location, [decoded_lat, decoded_lon]) else: rnge = None @@ -249,3 +238,21 @@ class cpr_decoder: return [decoded_lat, decoded_lon, rnge, bearing] +#encode CPR position +def cpr_encode(lat, lon, ctype, surface): + if surface is True: + scalar = float(2**19) + else: + scalar = float(2**17) + + dlati = float(dlat(ctype, False)) + yz = math.floor(scalar * (mod(lat, dlati)/dlati) + 0.5) + rlat = dlati * ((yz / scalar) + math.floor(lat / dlati)) + + dloni = 360.0 / nl_eo(rlat, ctype) + xz = math.floor(scalar * (mod(lon, dloni)/dloni) + 0.5) + + yz = int(mod(yz, scalar)) + xz = int(mod(xz, scalar)) + + return (yz, xz) #lat, lon diff --git a/python/modes_parse.py b/python/modes_parse.py index ec041e3..4350e67 100644 --- a/python/modes_parse.py +++ b/python/modes_parse.py @@ -1,5 +1,5 @@ # -# Copyright 2010 Nick Foster +# Copyright 2010, 2012 Nick Foster # # This file is part of gr-air-modes # @@ -23,15 +23,15 @@ import time, os, sys from string import split, join from altitude import decode_alt -from cpr import cpr_decoder +import cpr import math class modes_parse: def __init__(self, mypos): self.my_location = mypos - self.cpr = cpr_decoder(self.my_location) + self.cpr = cpr.cpr_decoder(self.my_location) - def parse0(self, shortdata, parity, ecc): + def parse0(self, shortdata): # shortdata = long(shortdata, 16) #parity = long(parity) @@ -45,7 +45,7 @@ class modes_parse: return [vs, cc, sl, ri, altitude] - def parse4(self, shortdata, parity, ecc): + def parse4(self, shortdata): # shortdata = long(shortdata, 16) fs = shortdata >> 24 & 0x07 #flight status: 0 is airborne normal, 1 is ground normal, 2 is airborne alert, 3 is ground alert, 4 is alert SPI, 5 is normal SPI dr = shortdata >> 19 & 0x1F #downlink request: 0 means no req, bit 0 is Comm-B msg rdy bit, bit 1 is TCAS info msg rdy, bit 2 is Comm-B bcast #1 msg rdy, bit2+bit0 is Comm-B bcast #2 msg rdy, @@ -58,7 +58,7 @@ class modes_parse: - def parse5(self, shortdata, parity, ecc): + def parse5(self, shortdata): # shortdata = long(shortdata, 16) fs = shortdata >> 24 & 0x07 #flight status: 0 is airborne normal, 1 is ground normal, 2 is airborne alert, 3 is ground alert, 4 is alert SPI, 5 is normal SPI dr = shortdata >> 19 & 0x1F #downlink request: 0 means no req, bit 0 is Comm-B msg rdy bit, bit 1 is TCAS info msg rdy, bit 2 is Comm-B bcast #1 msg rdy, bit2+bit0 is Comm-B bcast #2 msg rdy, @@ -67,7 +67,7 @@ class modes_parse: return [fs, dr, um] - def parse11(self, shortdata, parity, ecc): + def parse11(self, shortdata, ecc): # shortdata = long(shortdata, 16) interrogator = ecc & 0x0F @@ -76,15 +76,7 @@ class modes_parse: return [icao24, interrogator, ca] -#def parse17(self, shortdata, longdata, parity, ecc): -# shortdata = long(shortdata, 16) -# longdata = long(longdata, 16) -# parity = long(parity, 16) -# ecc = long(ecc, 16) - -# subtype = (longdata >> 51) & 0x1F; - - #the subtypes are: + #the Type 17 subtypes are: #0: No position information #1: Identification (Category set D) #2: Identification (Category set C) @@ -108,26 +100,16 @@ class modes_parse: #30: Aircraft operational coordination #31: Aircraft operational status + categories = [["NO INFO", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED"],\ + ["NO INFO", "SURFACE EMERGENCY VEHICLE", "SURFACE SERVICE VEHICLE", "FIXED OBSTRUCTION", "RESERVED", "RESERVED", "RESERVED"],\ + ["NO INFO", "GLIDER", "BALLOON/BLIMP", "PARACHUTE", "ULTRALIGHT", "RESERVED", "UAV", "SPACECRAFT"],\ + ["NO INFO", "LIGHT", "SMALL", "LARGE", "LARGE HIGH VORTEX", "HEAVY", "HIGH PERFORMANCE", "ROTORCRAFT"]] -# if subtype == 4: -# retstr = parseBDS08(shortdata, longdata, parity, ecc) -# elif subtype >= 9 and subtype <= 18: -# retstr = parseBDS05(shortdata, longdata, parity, ecc) -# elif subtype == 19: -# subsubtype = (longdata >> 48) & 0x07 -# if subsubtype == 0: -# retstr = parseBDS09_0(shortdata, longdata, parity, ecc) -# elif subsubtype == 1: -# retstr = parseBDS09_1(shortdata, longdata, parity, ecc) -# else: -# retstr = "BDS09 subtype " + str(subsubtype) + " not implemented" -# else: -# retstr = "Type 17, subtype " + str(subtype) + " not implemented" - -# return retstr - - def parseBDS08(self, shortdata, longdata, parity, ecc): + def parseBDS08(self, shortdata, longdata): icao24 = shortdata & 0xFFFFFF + subtype = (longdata >> 51) & 0x1F + category = (longdata >> 48) & 0x07 + catstring = self.categories[subtype-1][category] msg = "" for i in range(0, 8): @@ -135,7 +117,7 @@ class modes_parse: #retstr = "Type 17 subtype 04 (ident) from " + "%x" % icao24 + " with data " + msg - return msg + return (msg, catstring) def charmap(self, d): if d > 0 and d < 27: @@ -149,7 +131,7 @@ class modes_parse: return retval - def parseBDS05(self, shortdata, longdata, parity, ecc): + def parseBDS05(self, shortdata, longdata): icao24 = shortdata & 0xFFFFFF encoded_lon = longdata & 0x1FFFF @@ -166,7 +148,7 @@ class modes_parse: #welp turns out it looks like there's only 17 bits in the BDS0,6 ground packet after all. - def parseBDS06(self, shortdata, longdata, parity, ecc): + def parseBDS06(self, shortdata, longdata): icao24 = shortdata & 0xFFFFFF encoded_lon = longdata & 0x1FFFF @@ -181,7 +163,7 @@ class modes_parse: return [altitude, decoded_lat, decoded_lon, rnge, bearing] - def parseBDS09_0(self, shortdata, longdata, parity, ecc): + def parseBDS09_0(self, shortdata, longdata): icao24 = shortdata & 0xFFFFFF vert_spd = ((longdata >> 6) & 0x1FF) * 32 ud = bool((longdata >> 15) & 1) @@ -209,9 +191,9 @@ class modes_parse: #retstr = "Type 17 subtype 09-0 (track report) from " + "%x" % icao24 + " with velocity " + "%.0f" % velocity + "kt heading " + "%.0f" % heading + " VS " + "%.0f" % vert_spd - return [velocity, heading, vert_spd] + return [velocity, heading, vert_spd, turn_rate] - def parseBDS09_1(self, shortdata, longdata, parity, ecc): + def parseBDS09_1(self, shortdata, longdata): icao24 = shortdata & 0xFFFFFF alt_geo_diff = longdata & 0x7F - 1 above_below = bool((longdata >> 7) & 1) @@ -253,3 +235,4 @@ class modes_parse: #retstr = "Type 17 subtype 09-1 (track report) from " + "%x" % icao24 + " with velocity " + "%.0f" % velocity + "kt heading " + "%.0f" % heading + " VS " + "%.0f" % vert_spd return [velocity, heading, vert_spd] + diff --git a/python/modes_print.py b/python/modes_print.py index 1448c29..3b1698f 100644 --- a/python/modes_print.py +++ b/python/modes_print.py @@ -43,15 +43,15 @@ class modes_output_print(modes_parse.modes_parse): output = None; if msgtype == 0: - output = self.print0(shortdata, parity, ecc) + output = self.print0(shortdata, ecc) elif msgtype == 4: - output = self.print4(shortdata, parity, ecc) + output = self.print4(shortdata, ecc) elif msgtype == 5: - output = self.print5(shortdata, parity, ecc) + output = self.print5(shortdata, ecc) elif msgtype == 11: - output = self.print11(shortdata, parity, ecc) + output = self.print11(shortdata, ecc) elif msgtype == 17: - output = self.print17(shortdata, longdata, parity, ecc) + output = self.print17(shortdata, longdata) else: output = "No handler for message type " + str(msgtype) + " from %x" % ecc @@ -64,8 +64,8 @@ class modes_output_print(modes_parse.modes_parse): output = "(%.0f %.10f) " % (refdb, timestamp) + output print output - def print0(self, shortdata, parity, ecc): - [vs, cc, sl, ri, altitude] = self.parse0(shortdata, parity, ecc) + def print0(self, shortdata, ecc): + [vs, cc, sl, ri, altitude] = self.parse0(shortdata) retstr = "Type 0 (short A-A surveillance) from " + "%x" % ecc + " at " + str(altitude) + "ft" # the ri values below 9 are used for other things. might want to print those someday. @@ -79,9 +79,9 @@ class modes_output_print(modes_parse.modes_parse): return retstr - def print4(self, shortdata, parity, ecc): + def print4(self, shortdata, ecc): - [fs, dr, um, altitude] = self.parse4(shortdata, parity, ecc) + [fs, dr, um, altitude] = self.parse4(shortdata) retstr = "Type 4 (short surveillance altitude reply) from " + "%x" % ecc + " at " + str(altitude) + "ft" @@ -98,8 +98,8 @@ class modes_output_print(modes_parse.modes_parse): return retstr - def print5(self, shortdata, parity, ecc): - [fs, dr, um] = self.parse5(shortdata, parity, ecc) + def print5(self, shortdata, ecc): + [fs, dr, um] = self.parse5(shortdata) retstr = "Type 5 (short surveillance ident reply) from " + "%x" % ecc + " with ident " + str(shortdata & 0x1FFF) @@ -116,44 +116,45 @@ class modes_output_print(modes_parse.modes_parse): return retstr - def print11(self, shortdata, parity, ecc): - [icao24, interrogator, ca] = self.parse11(shortdata, parity, ecc) + def print11(self, shortdata, ecc): + [icao24, interrogator, ca] = self.parse11(shortdata, ecc) retstr = "Type 11 (all call reply) from " + "%x" % icao24 + " in reply to interrogator " + str(interrogator) return retstr - def print17(self, shortdata, longdata, parity, ecc): + def print17(self, shortdata, longdata): icao24 = shortdata & 0xFFFFFF subtype = (longdata >> 51) & 0x1F; retstr = None - if subtype == 4: - msg = self.parseBDS08(shortdata, longdata, parity, ecc) - retstr = "Type 17 subtype 04 (ident) from " + "%x" % icao24 + " with data " + msg + if 1 <= subtype <= 4: + (msg, typestring) = self.parseBDS08(shortdata, longdata) + retstr = "Type 17 subtype 04 (ident) from " + "%x" % icao24 + " of type " + typestring + " with ident " + msg elif subtype >= 5 and subtype <= 8: - [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(shortdata, longdata, parity, ecc) + [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(shortdata, longdata) if decoded_lat is not None: - retstr = "Type 17 subtype 06 (surface report) from " + "%x" % icao24 + " at (" + "%.6f" % decoded_lat + ", " + "%.6f" % decoded_lon + ") (" + "%.2f" % rnge + " @ " + "%.0f" % bearing + ")" + retstr = "Type 17 subtype 06 (surface report) from " + "%x" % icao24 + " at (" + "%.6f" % decoded_lat + ", " + "%.6f" % decoded_lon + ")" + if rnge is not None and bearing is not None: + retstr += " (" + "%.2f" % rnge + " @ " + "%.0f" % bearing + ")" elif subtype >= 9 and subtype <= 18: - [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(shortdata, longdata, parity, ecc) + [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(shortdata, longdata) if decoded_lat is not None: - retstr = "Type 17 subtype 05 (position report) from " + "%x" % icao24 + " at (" + "%.6f" % decoded_lat + ", " + "%.6f" % decoded_lon + ") (" + "%.2f" % rnge + " @ " + "%.0f" % bearing + ") at " + str(altitude) + "ft" - -# this is a trigger to capture the bizarre BDS0,5 squitters you keep seeing on the map with latitudes all over the place -# if icao24 == 0xa1ede9: -# print "Buggy squitter with shortdata %s longdata %s parity %s ecc %s" % (str(shortdata), str(longdata), str(parity), str(ecc),) + retstr = "Type 17 subtype 05 (position report) from " + "%x" % icao24 + " at (" + "%.6f" % decoded_lat + ", " + "%.6f" % decoded_lon + ")" + if rnge is not None and bearing is not None: + retstr += " (" + "%.2f" % rnge + " @ " + "%.0f" % bearing + ")" + retstr += " at " + str(altitude) + "ft" elif subtype == 19: subsubtype = (longdata >> 48) & 0x07 if subsubtype == 0: - [velocity, heading, vert_spd] = self.parseBDS09_0(shortdata, longdata, parity, ecc) + [velocity, heading, vert_spd] = self.parseBDS09_0(shortdata, longdata) retstr = "Type 17 subtype 09-0 (track report) from " + "%x" % icao24 + " with velocity " + "%.0f" % velocity + "kt heading " + "%.0f" % heading + " VS " + "%.0f" % vert_spd elif subsubtype == 1: - [velocity, heading, vert_spd] = self.parseBDS09_1(shortdata, longdata, parity, ecc) + [velocity, heading, vert_spd] = self.parseBDS09_1(shortdata, longdata) retstr = "Type 17 subtype 09-1 (track report) from " + "%x" % icao24 + " with velocity " + "%.0f" % velocity + "kt heading " + "%.0f" % heading + " VS " + "%.0f" % vert_spd else: