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

@@ -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:
+      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.lpfiltcoeffs = gr.firdes.low_pass(1, rate, 1.8e6, 100e3)
-    self.lpfilter = gr.fir_filter_ccf(1, self.lpfiltcoeffs)
+    #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 :
+      if not queue.empty_p() :
-        while queue.empty_p() == 0 :
+        while not queue.empty_p() :
           msg = queue.delete_head() #blocking read
 
           for out in outputs:

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;

include/modes_crc.h

@@ -20,10 +20,10 @@
 # 
 */
 
-#ifndef INCLUDED_MODES_PARITY_H
+#ifndef INCLUDED_MODES_CRC_H
-#define INCLUDED_MODES_PARITY_H
+#define INCLUDED_MODES_CRC_H
-extern const unsigned int modes_parity_table[112];
+extern const unsigned int modes_crc_table[112];
-int modes_check_parity(unsigned char data[], int length);
+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);
 

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")

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<gr_tag_t> 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;
+			double tstamp = tag_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
-
-			if(d_timestamp.offset != 0) {
-				tstamp = tag_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
-			}
 
 			//now tag the preamble
 			add_item_tag(0, //stream ID

lib/air_modes_slicer.cc

@@ -29,7 +29,7 @@
 #include <air_modes_types.h>
 #include <sstream>
 #include <iomanip>
-#include <modes_parity.h>
+#include <modes_crc.h>
 #include <iostream>
 #include <gr_tags.h>
 #include <air_modes_api.h>
@@ -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
+		//crc 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;}
+		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);

lib/modes_crc.cc

@@ -27,7 +27,7 @@
 
 #include <stdio.h>
 #include <air_modes_types.h>
-#include <modes_parity.h>
+#include <modes_crc.h>
 #include <math.h>
 #include <stdlib.h>
 
@@ -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;
+	int crc, i;
-	// Check both long and short
+	for(i = 0; i < length; i++)
-	short_crc = 0;
-	long_crc = 0;
-	for(i = 0; i < 56; i++)
 	{
 		if(data[i/8] & (1 << (7-(i%8))))
 		{
-			short_crc ^= modes_parity_table[i+56];
+			crc ^= modes_crc_table[i+(112-length)];
-			long_crc ^= modes_parity_table[i];
-
 		}
 	}
-	for( ; i < length; i++)
+    return crc;
-	{
-		if(data[i/8] & (1 << (7-(i%8))))
-		{
-			long_crc ^= modes_parity_table[i];
 }
-	}
-	if(length == 112) return long_crc;
-	else return short_crc;
-}
-

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
 )
 

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))
+

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

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

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):
+	#the Type 17 subtypes are:
-#	shortdata = long(shortdata, 16)
-#	longdata = long(longdata, 16)
-#	parity = long(parity, 16)
-#	ecc = long(ecc, 16)
-
-#	subtype = (longdata >> 51) & 0x1F;
-
-	#the 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:
+  def parseBDS08(self, shortdata, longdata):
-#		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):
     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]
+

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):
+  def print0(self, shortdata, ecc):
-    [vs, cc, sl, ri, altitude] = self.parse0(shortdata, parity, 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):
+  def print5(self, shortdata, ecc):
-    [fs, dr, um] = self.parse5(shortdata, parity, 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):
+  def print11(self, shortdata, ecc):
-    [icao24, interrogator, ca] = self.parse11(shortdata, parity, 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:
+    if 1 <= subtype <= 4:
-      msg = self.parseBDS08(shortdata, longdata, parity, ecc)
+      (msg, typestring) = self.parseBDS08(shortdata, longdata)
-      retstr = "Type 17 subtype 04 (ident) from " + "%x" % icao24 + " with data " + msg
+      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"
+        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:
-#   this is a trigger to capture the bizarre BDS0,5 squitters you keep seeing on the map with latitudes all over the place
+            retstr += " (" + "%.2f" % rnge + " @ " + "%.0f" % bearing + ")"
-#     if icao24 == 0xa1ede9:
+        retstr += " at " + str(altitude) + "ft"
-#       print "Buggy squitter with shortdata %s longdata %s parity %s ecc %s" % (str(shortdata), str(longdata), str(parity), str(ecc),)
 
     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: